void Standard::update_vtol_state()
{
/* After flipping the switch the vehicle will start the pusher (or tractor) motor, picking up
* forward speed. After the vehicle has picked up enough speed the rotors shutdown.
* For the back transition the pusher motor is immediately stopped and rotors reactivated.
*/
float mc_weight = _mc_roll_weight;
float time_since_trans_start = (float)(hrt_absolute_time() - _vtol_schedule.transition_start) * 1e-6f;
if (!_attc->is_fixed_wing_requested()) {
// the transition to fw mode switch is off
if (_vtol_schedule.flight_mode == MC_MODE) {
// in mc mode
_vtol_schedule.flight_mode = MC_MODE;
mc_weight = 1.0f;
_pusher_throttle = 0.0f;
_reverse_output = 0.0f;
} else if (_vtol_schedule.flight_mode == FW_MODE) {
// transition to mc mode
if (_vtol_vehicle_status->vtol_transition_failsafe == true) {
// Failsafe event, engage mc motors immediately
_vtol_schedule.flight_mode = MC_MODE;
_flag_enable_mc_motors = true;
_pusher_throttle = 0.0f;
_reverse_output = 0.0f;
} else {
// Regular backtransition
_vtol_schedule.flight_mode = TRANSITION_TO_MC;
_flag_enable_mc_motors = true;
_vtol_schedule.transition_start = hrt_absolute_time();
_reverse_output = _params_standard.reverse_output;
}
} else if (_vtol_schedule.flight_mode == TRANSITION_TO_FW) {
// failsafe back to mc mode
_vtol_schedule.flight_mode = MC_MODE;
mc_weight = 1.0f;
_pusher_throttle = 0.0f;
_reverse_output = 0.0f;
} else if (_vtol_schedule.flight_mode == TRANSITION_TO_MC) {
// transition to MC mode if transition time has passed or forward velocity drops below MPC cruise speed
const matrix::Dcmf R_to_body(matrix::Quatf(_v_att->q).inversed());
const matrix::Vector3f vel = R_to_body * matrix::Vector3f(_local_pos->vx, _local_pos->vy, _local_pos->vz);
float x_vel = vel(0);
if (time_since_trans_start > _params->back_trans_duration ||
(_local_pos->v_xy_valid && x_vel <= _params->mpc_xy_cruise)) {
_vtol_schedule.flight_mode = MC_MODE;
}
}
} else {
// the transition to fw mode switch is on
if (_vtol_schedule.flight_mode == MC_MODE || _vtol_schedule.flight_mode == TRANSITION_TO_MC) {
// start transition to fw mode
/* NOTE: The failsafe transition to fixed-wing was removed because it can result in an
* unsafe flying state. */
_vtol_schedule.flight_mode = TRANSITION_TO_FW;
_vtol_schedule.transition_start = hrt_absolute_time();
} else if (_vtol_schedule.flight_mode == FW_MODE) {
// in fw mode
_vtol_schedule.flight_mode = FW_MODE;
mc_weight = 0.0f;
} else if (_vtol_schedule.flight_mode == TRANSITION_TO_FW) {
// continue the transition to fw mode while monitoring airspeed for a final switch to fw mode
if (((_params->airspeed_disabled ||
_airspeed->indicated_airspeed_m_s >= _params->transition_airspeed) &&
time_since_trans_start > _params->front_trans_time_min) ||
can_transition_on_ground()) {
_vtol_schedule.flight_mode = FW_MODE;
// we can turn off the multirotor motors now
_flag_enable_mc_motors = false;
// don't set pusher throttle here as it's being ramped up elsewhere
_trans_finished_ts = hrt_absolute_time();
}
}
}
_mc_roll_weight = mc_weight;
_mc_pitch_weight = mc_weight;
_mc_yaw_weight = mc_weight;
_mc_throttle_weight = mc_weight;
// map specific control phases to simple control modes
switch (_vtol_schedule.flight_mode) {
case MC_MODE:
_vtol_mode = mode::ROTARY_WING;
break;
case FW_MODE:
_vtol_mode = mode::FIXED_WING;
break;
case TRANSITION_TO_FW:
_vtol_mode = mode::TRANSITION_TO_FW;
break;
case TRANSITION_TO_MC:
_vtol_mode = mode::TRANSITION_TO_MC;
break;
}
}
void Tiltrotor::update_vtol_state()
{
/* simple logic using a two way switch to perform transitions.
* after flipping the switch the vehicle will start tilting rotors, picking up
* forward speed. After the vehicle has picked up enough speed the rotors are tilted
* forward completely. For the backtransition the motors simply rotate back.
*/
if (!_attc->is_fixed_wing_requested()) {
// plane is in multicopter mode
switch (_vtol_schedule.flight_mode) {
case MC_MODE:
break;
case FW_MODE:
_vtol_schedule.flight_mode = TRANSITION_BACK;
_vtol_schedule.transition_start = hrt_absolute_time();
break;
case TRANSITION_FRONT_P1:
// failsafe into multicopter mode
_vtol_schedule.flight_mode = MC_MODE;
break;
case TRANSITION_FRONT_P2:
// failsafe into multicopter mode
_vtol_schedule.flight_mode = MC_MODE;
break;
case TRANSITION_BACK:
if (_tilt_control <= _params_tiltrotor.tilt_mc) {
_vtol_schedule.flight_mode = MC_MODE;
}
break;
}
} else {
switch (_vtol_schedule.flight_mode) {
case MC_MODE:
// initialise a front transition
_vtol_schedule.flight_mode = TRANSITION_FRONT_P1;
_vtol_schedule.transition_start = hrt_absolute_time();
break;
case FW_MODE:
break;
case TRANSITION_FRONT_P1: {
// allow switch if we are not armed for the sake of bench testing
bool transition_to_p2 = can_transition_on_ground();
float time_since_trans_start = (float)(hrt_absolute_time() - _vtol_schedule.transition_start) * 1e-6f;
// check if we have reached airspeed to switch to fw mode
transition_to_p2 |= !_params->airspeed_disabled &&
_airspeed->indicated_airspeed_m_s >= _params->transition_airspeed &&
time_since_trans_start > _params->front_trans_time_min;
// check if airspeed is invalid and transition by time
transition_to_p2 |= _params->airspeed_disabled &&
_tilt_control > _params_tiltrotor.tilt_transition &&
time_since_trans_start > _params->front_trans_time_openloop;
if (transition_to_p2) {
_vtol_schedule.flight_mode = TRANSITION_FRONT_P2;
_vtol_schedule.transition_start = hrt_absolute_time();
}
break;
}
case TRANSITION_FRONT_P2:
// if the rotors have been tilted completely we switch to fw mode
if (_tilt_control >= _params_tiltrotor.tilt_fw) {
_vtol_schedule.flight_mode = FW_MODE;
_tilt_control = _params_tiltrotor.tilt_fw;
}
break;
case TRANSITION_BACK:
// failsafe into fixed wing mode
_vtol_schedule.flight_mode = FW_MODE;
break;
}
}
// map tiltrotor specific control phases to simple control modes
switch (_vtol_schedule.flight_mode) {
case MC_MODE:
_vtol_mode = ROTARY_WING;
break;
case FW_MODE:
_vtol_mode = FIXED_WING;
break;
case TRANSITION_FRONT_P1:
case TRANSITION_FRONT_P2:
_vtol_mode = TRANSITION_TO_FW;
break;
case TRANSITION_BACK:
_vtol_mode = TRANSITION_TO_MC;
break;
}
}
void Standard::update_vtol_state()
{
parameters_update();
/* After flipping the switch the vehicle will start the pusher (or tractor) motor, picking up
* forward speed. After the vehicle has picked up enough speed the rotors shutdown.
* For the back transition the pusher motor is immediately stopped and rotors reactivated.
*/
if (!_attc->is_fixed_wing_requested()) {
// the transition to fw mode switch is off
if (_vtol_schedule.flight_mode == MC_MODE) {
// in mc mode
_vtol_schedule.flight_mode = MC_MODE;
_mc_roll_weight = 1.0f;
_mc_pitch_weight = 1.0f;
_mc_yaw_weight = 1.0f;
_mc_throttle_weight = 1.0f;
} else if (_vtol_schedule.flight_mode == FW_MODE) {
// transition to mc mode
if (_vtol_vehicle_status->vtol_transition_failsafe == true) {
// Failsafe event, engage mc motors immediately
_vtol_schedule.flight_mode = MC_MODE;
_flag_enable_mc_motors = true;
} else {
// Regular backtransition
_vtol_schedule.flight_mode = TRANSITION_TO_MC;
_flag_enable_mc_motors = true;
_vtol_schedule.transition_start = hrt_absolute_time();
}
} else if (_vtol_schedule.flight_mode == TRANSITION_TO_FW) {
// failsafe back to mc mode
_vtol_schedule.flight_mode = MC_MODE;
_mc_roll_weight = 1.0f;
_mc_pitch_weight = 1.0f;
_mc_yaw_weight = 1.0f;
_mc_throttle_weight = 1.0f;
} else if (_vtol_schedule.flight_mode == TRANSITION_TO_MC) {
// transition to MC mode if transition time has passed
// XXX: base this on XY hold velocity of MC
if (hrt_elapsed_time(&_vtol_schedule.transition_start) >
(_params_standard.back_trans_dur * 1000000.0f)) {
_vtol_schedule.flight_mode = MC_MODE;
}
}
// the pusher motor should never be powered when in or transitioning to mc mode
_pusher_throttle = 0.0f;
} else {
// the transition to fw mode switch is on
if (_vtol_schedule.flight_mode == MC_MODE || _vtol_schedule.flight_mode == TRANSITION_TO_MC) {
// start transition to fw mode
/* NOTE: The failsafe transition to fixed-wing was removed because it can result in an
* unsafe flying state. */
_vtol_schedule.flight_mode = TRANSITION_TO_FW;
_vtol_schedule.transition_start = hrt_absolute_time();
} else if (_vtol_schedule.flight_mode == FW_MODE) {
// in fw mode
_vtol_schedule.flight_mode = FW_MODE;
_mc_roll_weight = 0.0f;
_mc_pitch_weight = 0.0f;
_mc_yaw_weight = 0.0f;
_mc_throttle_weight = 0.0f;
} else if (_vtol_schedule.flight_mode == TRANSITION_TO_FW) {
// continue the transition to fw mode while monitoring airspeed for a final switch to fw mode
if (((_params_standard.airspeed_mode == control_state_s::AIRSPD_MODE_DISABLED ||
_airspeed->indicated_airspeed_m_s >= _params_standard.airspeed_trans) &&
(float)hrt_elapsed_time(&_vtol_schedule.transition_start)
> (_params_standard.front_trans_time_min * 1000000.0f)) ||
can_transition_on_ground()) {
_vtol_schedule.flight_mode = FW_MODE;
// we can turn off the multirotor motors now
_flag_enable_mc_motors = false;
// don't set pusher throttle here as it's being ramped up elsewhere
_trans_finished_ts = hrt_absolute_time();
}
}
}
// map specific control phases to simple control modes
switch (_vtol_schedule.flight_mode) {
case MC_MODE:
_vtol_mode = mode::ROTARY_WING;
break;
case FW_MODE:
_vtol_mode = mode::FIXED_WING;
break;
case TRANSITION_TO_FW:
_vtol_mode = mode::TRANSITION_TO_MC;
break;
case TRANSITION_TO_MC:
_vtol_mode = mode::TRANSITION_TO_FW;
break;
}
}
void Tailsitter::update_vtol_state()
{
/* simple logic using a two way switch to perform transitions.
* after flipping the switch the vehicle will start tilting in MC control mode, picking up
* forward speed. After the vehicle has picked up enough and sufficient pitch angle the uav will go into FW mode.
* For the backtransition the pitch is controlled in MC mode again and switches to full MC control reaching the sufficient pitch angle.
*/
matrix::Eulerf euler = matrix::Quatf(_v_att->q);
float pitch = euler.theta();
if (!_attc->is_fixed_wing_requested()) {
switch (_vtol_schedule.flight_mode) { // user switchig to MC mode
case MC_MODE:
break;
case FW_MODE:
_vtol_schedule.flight_mode = TRANSITION_BACK;
_vtol_schedule.transition_start = hrt_absolute_time();
break;
case TRANSITION_FRONT_P1:
// failsafe into multicopter mode
_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
if (pitch >= PITCH_TRANSITION_BACK) {
_vtol_schedule.flight_mode = MC_MODE;
}
break;
}
} else { // user switchig to FW mode
switch (_vtol_schedule.flight_mode) {
case MC_MODE:
// initialise a front transition
_vtol_schedule.flight_mode = TRANSITION_FRONT_P1;
_vtol_schedule.transition_start = hrt_absolute_time();
break;
case FW_MODE:
break;
case TRANSITION_FRONT_P1:
// check if we have reached airspeed and pitch angle to switch to TRANSITION P2 mode
if ((_airspeed->indicated_airspeed_m_s >= _params_tailsitter.airspeed_trans
&& 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;
}
}
// map tailsitter specific control phases to simple control modes
switch (_vtol_schedule.flight_mode) {
case MC_MODE:
_vtol_mode = ROTARY_WING;
_vtol_vehicle_status->vtol_in_trans_mode = false;
_flag_was_in_trans_mode = false;
break;
case FW_MODE:
_vtol_mode = FIXED_WING;
_vtol_vehicle_status->vtol_in_trans_mode = false;
_flag_was_in_trans_mode = false;
break;
case TRANSITION_FRONT_P1:
_vtol_mode = TRANSITION_TO_FW;
_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;
break;
}
}
