fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-02 09:50:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

tailsitter: removed usage of hrt_elapsed and small cleanup

Browse Source 
- do not call hrt_elapsed_time since it's expensive
- remove P2 front transition phase (was not even used)

Signed-off-by: Roman <bapstroman@gmail.com>
This commit is contained in:
Roman
2018-02-27 14:25:46 +01:00
committed by Roman Bapst
parent 196d0e40b4
commit bf22f02567
2 changed files with 10 additions and 67 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/vtol_att_control/tailsitter.cpp
+10 -66
View File
@@ -45,7 +45,6 @@
#define ARSP_YAW_CTRL_DISABLE 4.0f // airspeed at which we stop controlling yaw during a front transition
#define THROTTLE_TRANSITION_MAX 0.25f // maximum added thrust above last value in transition
#define PITCH_TRANSITION_FRONT_P1 -1.1f // pitch angle to switch to TRANSITION_P2
#define PITCH_TRANSITION_FRONT_P2 -1.2f // pitch angle to switch to FW
#define PITCH_TRANSITION_BACK -0.25f // pitch angle to switch to MC
Tailsitter::Tailsitter(VtolAttitudeControl *attc) :
@@ -109,12 +108,6 @@ void Tailsitter::update_vtol_state()
_vtol_schedule.flight_mode = MC_MODE;
break;
case TRANSITION_FRONT_P2:
// NOT USED
// failsafe into multicopter mode
//_vtol_schedule.flight_mode = MC_MODE;
break;
case TRANSITION_BACK:
// check if we have reached pitch angle to switch to MC mode
@@ -143,20 +136,13 @@ void Tailsitter::update_vtol_state()
if ((_airspeed->indicated_airspeed_m_s >= _params->transition_airspeed
&& pitch <= PITCH_TRANSITION_FRONT_P1) || can_transition_on_ground()) {
_vtol_schedule.flight_mode = FW_MODE;
//_vtol_schedule.transition_start = hrt_absolute_time();
}
break;
case TRANSITION_FRONT_P2:
case TRANSITION_BACK:
// failsafe into fixed wing mode
_vtol_schedule.flight_mode = FW_MODE;
/* **LATER*** if pitch is closer to mc (-45>)
* go to transition P1
*/
break;
}
}
@@ -180,11 +166,6 @@ void Tailsitter::update_vtol_state()
_vtol_vehicle_status->vtol_in_trans_mode = true;
break;
case TRANSITION_FRONT_P2:
_vtol_mode = TRANSITION_TO_FW;
_vtol_vehicle_status->vtol_in_trans_mode = true;
break;
case TRANSITION_BACK:
_vtol_mode = TRANSITION_TO_MC;
_vtol_vehicle_status->vtol_in_trans_mode = true;
@@ -194,6 +175,8 @@ void Tailsitter::update_vtol_state()
void Tailsitter::update_transition_state()
{
float time_since_trans_start = (float)(hrt_absolute_time() - _vtol_schedule.transition_start);
if (!_flag_was_in_trans_mode) {
// save desired heading for transition and last thrust value
_yaw_transition = _v_att_sp->yaw_body;
@@ -206,9 +189,9 @@ void Tailsitter::update_transition_state()
if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P1) {
/** create time dependant pitch angle set point + 0.2 rad overlap over the switch value*/
// create time dependant pitch angle set point + 0.2 rad overlap over the switch value
_v_att_sp->pitch_body = _pitch_transition_start - (fabsf(PITCH_TRANSITION_FRONT_P1 - _pitch_transition_start) *
(float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params->front_trans_duration * 1000000.0f));
time_since_trans_start / (_params->front_trans_duration * 1e6f));
_v_att_sp->pitch_body = math::constrain(_v_att_sp->pitch_body, PITCH_TRANSITION_FRONT_P1 - 0.2f,
_pitch_transition_start);
@@ -225,43 +208,6 @@ void Tailsitter::update_transition_state()
_mc_roll_weight = 1.0f;
_mc_pitch_weight = 1.0f;
} else if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P2) {
// the plane is ready to go into fixed wing mode, smoothly switch the actuator controls, keep pitching down
/** no motor switching */
if (flag_idle_mc) {
set_idle_fw();
flag_idle_mc = false;
}
/** create time dependant pitch angle set point + 0.2 rad overlap over the switch value*/
if (_v_att_sp->pitch_body >= (PITCH_TRANSITION_FRONT_P2 - 0.2f)) {
_v_att_sp->pitch_body = PITCH_TRANSITION_FRONT_P1 -
(fabsf(PITCH_TRANSITION_FRONT_P2 - PITCH_TRANSITION_FRONT_P1) * (float)hrt_elapsed_time(
&_vtol_schedule.transition_start) / (_params_tailsitter.front_trans_dur_p2 * 1000000.0f));
if (_v_att_sp->pitch_body <= (PITCH_TRANSITION_FRONT_P2 - 0.2f)) {
_v_att_sp->pitch_body = PITCH_TRANSITION_FRONT_P2 - 0.2f;
}
}
_v_att_sp->thrust = _thrust_transition;
/** start blending MC and FW controls from pitch -45 to pitch -70 for smooth control takeover*/
//_mc_roll_weight = 1.0f - 1.0f * ((float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tailsitter.front_trans_dur_p2 * 1000000.0f));
//_mc_pitch_weight = 1.0f - 1.0f * ((float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tailsitter.front_trans_dur_p2 * 1000000.0f));
_mc_roll_weight = 0.0f;
_mc_pitch_weight = 0.0f;
/** keep yaw disabled */
_mc_yaw_weight = 0.0f;
} else if (_vtol_schedule.flight_mode == TRANSITION_BACK) {
if (!flag_idle_mc) {
@@ -269,22 +215,20 @@ void Tailsitter::update_transition_state()
flag_idle_mc = true;
}
/** create time dependant pitch angle set point stating at -pi/2 + 0.2 rad overlap over the switch value*/
// create time dependant pitch angle set point stating at -pi/2 + 0.2 rad overlap over the switch value
_v_att_sp->pitch_body = M_PI_2_F + _pitch_transition_start + fabsf(PITCH_TRANSITION_BACK + 1.57f) *
(float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params->back_trans_duration * 1000000.0f);
time_since_trans_start / (_params->back_trans_duration * 1e6f);
_v_att_sp->pitch_body = math::constrain(_v_att_sp->pitch_body, -2.0f, PITCH_TRANSITION_BACK + 0.2f);
_v_att_sp->thrust = _mc_virtual_att_sp->thrust;
/** keep yaw disabled */
// keep yaw disabled
_mc_yaw_weight = 0.0f;
/** smoothly move control weight to MC */
_mc_roll_weight = 1.0f * (float)hrt_elapsed_time(&_vtol_schedule.transition_start) /
(_params->back_trans_duration * 1000000.0f);
_mc_pitch_weight = 1.0f * (float)hrt_elapsed_time(&_vtol_schedule.transition_start) /
(_params->back_trans_duration * 1000000.0f);
// smoothly move control weight to MC
_mc_roll_weight = 1.0f * time_since_trans_start / (_params->back_trans_duration * 1e6f);
_mc_pitch_weight = 1.0f * time_since_trans_start / (_params->back_trans_duration * 1e6f);
}
src/modules/vtol_att_control/tailsitter.h
-1
View File
@@ -74,7 +74,6 @@ private:
enum vtol_mode {
MC_MODE = 0, /**< vtol is in multicopter mode */
TRANSITION_FRONT_P1, /**< vtol is in front transition part 1 mode */
TRANSITION_FRONT_P2, /**< vtol is in front transition part 2 mode */
TRANSITION_BACK, /**< vtol is in back transition mode */
FW_MODE /**< vtol is in fixed wing mode */
};