void Standard::update_transition_state()
{
// copy virtual attitude setpoint to real attitude setpoint
memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s));
if (_vtol_schedule.flight_mode == TRANSITION_TO_FW) {
if (_params_standard.front_trans_dur <= 0.0f) {
// just set the final target throttle value
_pusher_throttle = _params_standard.pusher_trans;
} else if (_pusher_throttle <= _params_standard.pusher_trans) {
// ramp up throttle to the target throttle value
_pusher_throttle = _params_standard.pusher_trans *
(float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_standard.front_trans_dur * 1000000.0f);
}
// do blending of mc and fw controls if a blending airspeed has been provided
if (_airspeed_trans_blend_margin > 0.0f && _airspeed->true_airspeed_m_s >= _params_standard.airspeed_blend) {
float weight = 1.0f - fabsf(_airspeed->true_airspeed_m_s - _params_standard.airspeed_blend) /
_airspeed_trans_blend_margin;
_mc_roll_weight = weight;
_mc_pitch_weight = weight;
_mc_yaw_weight = weight;
} else {
// at low speeds give full weight to mc
_mc_roll_weight = 1.0f;
_mc_pitch_weight = 1.0f;
_mc_yaw_weight = 1.0f;
}
} else if (_vtol_schedule.flight_mode == TRANSITION_TO_MC) {
// continually increase mc attitude control as we transition back to mc mode
if (_params_standard.back_trans_dur > 0.0f) {
float weight = (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_standard.back_trans_dur *
1000000.0f);
_mc_roll_weight = weight;
_mc_pitch_weight = weight;
_mc_yaw_weight = weight;
} else {
_mc_roll_weight = 1.0f;
_mc_pitch_weight = 1.0f;
_mc_yaw_weight = 1.0f;
}
// in fw mode we need the multirotor motors to stop spinning, in backtransition mode we let them spin up again
if (_flag_enable_mc_motors) {
set_max_mc(2000);
set_idle_mc();
_flag_enable_mc_motors = false;
}
}
_mc_roll_weight = math::constrain(_mc_roll_weight, 0.0f, 1.0f);
_mc_pitch_weight = math::constrain(_mc_pitch_weight, 0.0f, 1.0f);
_mc_yaw_weight = math::constrain(_mc_yaw_weight, 0.0f, 1.0f);
}
void Standard::update_fw_state()
{
// in fw mode we need the multirotor motors to stop spinning, in backtransition mode we let them spin up again
if (!_flag_enable_mc_motors) {
set_max_mc(950);
set_idle_fw(); // force them to stop, not just idle
_flag_enable_mc_motors = true;
}
}
void Standard::update_fw_state()
{
// copy virtual attitude setpoint to real attitude setpoint
memcpy(_v_att_sp, _fw_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s));
// in fw mode we need the multirotor motors to stop spinning, in backtransition mode we let them spin up again
if (!_flag_enable_mc_motors) {
set_max_mc(950);
set_idle_fw(); // force them to stop, not just idle
_flag_enable_mc_motors = true;
}
}
void Standard::update_mc_state()
{
VtolType::update_mc_state();
// enable MC motors here in case we transitioned directly to MC mode
if (_flag_enable_mc_motors) {
set_max_mc(2000);
set_idle_mc();
_flag_enable_mc_motors = false;
}
// if the thrust scale param is zero or the drone is on manual mode,
// then the pusher-for-pitch strategy is disabled and we can return
if (_params_standard.forward_thrust_scale < FLT_EPSILON ||
!_v_control_mode->flag_control_position_enabled) {
return;
}
// Do not engage pusher assist during a failsafe event
// There could be a problem with the fixed wing drive
if (_attc->get_vtol_vehicle_status()->vtol_transition_failsafe) {
return;
}
// disable pusher assist during landing
if (_attc->get_pos_sp_triplet()->current.valid
&& _attc->get_pos_sp_triplet()->current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
return;
}
matrix::Dcmf R(matrix::Quatf(_v_att->q));
matrix::Dcmf R_sp(matrix::Quatf(_v_att_sp->q_d));
matrix::Eulerf euler(R);
matrix::Eulerf euler_sp(R_sp);
_pusher_throttle = 0.0f;
// direction of desired body z axis represented in earth frame
matrix::Vector3f body_z_sp(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2));
// rotate desired body z axis into new frame which is rotated in z by the current
// heading of the vehicle. we refer to this as the heading frame.
matrix::Dcmf R_yaw = matrix::Eulerf(0.0f, 0.0f, -euler(2));
body_z_sp = R_yaw * body_z_sp;
body_z_sp.normalize();
// calculate the desired pitch seen in the heading frame
// this value corresponds to the amount the vehicle would try to pitch forward
float pitch_forward = atan2f(body_z_sp(0), body_z_sp(2));
// only allow pitching forward up to threshold, the rest of the desired
// forward acceleration will be compensated by the pusher
if (pitch_forward < -_params_standard.down_pitch_max) {
// desired roll angle in heading frame stays the same
float roll_new = -asinf(body_z_sp(1));
_pusher_throttle = (sinf(-pitch_forward) - sinf(_params_standard.down_pitch_max))
* _params_standard.forward_thrust_scale;
// return the vehicle to level position
float pitch_new = 0.0f;
// create corrected desired body z axis in heading frame
matrix::Dcmf R_tmp = matrix::Eulerf(roll_new, pitch_new, 0.0f);
matrix::Vector3f tilt_new(R_tmp(0, 2), R_tmp(1, 2), R_tmp(2, 2));
// rotate the vector into a new frame which is rotated in z by the desired heading
// with respect to the earh frame.
float yaw_error = _wrap_pi(euler_sp(2) - euler(2));
matrix::Dcmf R_yaw_correction = matrix::Eulerf(0.0f, 0.0f, -yaw_error);
tilt_new = R_yaw_correction * tilt_new;
// now extract roll and pitch setpoints
_v_att_sp->pitch_body = atan2f(tilt_new(0), tilt_new(2));
_v_att_sp->roll_body = -asinf(tilt_new(1));
R_sp = matrix::Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, euler_sp(2));
matrix::Quatf q_sp(R_sp);
q_sp.copyTo(_v_att_sp->q_d);
}
_pusher_throttle = _pusher_throttle < 0.0f ? 0.0f : _pusher_throttle;
}
void Standard::update_transition_state()
{
float mc_weight = 1.0f;
float time_since_trans_start = (float)(hrt_absolute_time() - _vtol_schedule.transition_start) * 1e-6f;
VtolType::update_transition_state();
// copy virtual attitude setpoint to real attitude setpoint
memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s));
if (_vtol_schedule.flight_mode == TRANSITION_TO_FW) {
if (_params_standard.pusher_ramp_dt <= 0.0f) {
// just set the final target throttle value
_pusher_throttle = _params->front_trans_throttle;
} else if (_pusher_throttle <= _params->front_trans_throttle) {
// ramp up throttle to the target throttle value
_pusher_throttle = _params->front_trans_throttle * time_since_trans_start / _params_standard.pusher_ramp_dt;
}
// do blending of mc and fw controls if a blending airspeed has been provided and the minimum transition time has passed
if (_airspeed_trans_blend_margin > 0.0f &&
_airspeed->indicated_airspeed_m_s >= _params->airspeed_blend &&
time_since_trans_start > _params->front_trans_time_min) {
mc_weight = 1.0f - fabsf(_airspeed->indicated_airspeed_m_s - _params->airspeed_blend) /
_airspeed_trans_blend_margin;
// time based blending when no airspeed sensor is set
} else if (_params->airspeed_disabled) {
mc_weight = 1.0f - time_since_trans_start / _params->front_trans_time_min;
mc_weight = math::constrain(2.0f * mc_weight, 0.0f, 1.0f);
}
// ramp up FW_PSP_OFF
_v_att_sp->pitch_body = _params_standard.pitch_setpoint_offset * (1.0f - mc_weight);
matrix::Quatf q_sp(matrix::Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, _v_att_sp->yaw_body));
q_sp.copyTo(_v_att_sp->q_d);
_v_att_sp->q_d_valid = true;
// check front transition timeout
if (_params->front_trans_timeout > FLT_EPSILON) {
if (time_since_trans_start > _params->front_trans_timeout) {
// transition timeout occured, abort transition
_attc->abort_front_transition("Transition timeout");
}
}
} else if (_vtol_schedule.flight_mode == TRANSITION_TO_MC) {
// maintain FW_PSP_OFF
_v_att_sp->pitch_body = _params_standard.pitch_setpoint_offset;
matrix::Quatf q_sp(matrix::Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, _v_att_sp->yaw_body));
q_sp.copyTo(_v_att_sp->q_d);
_v_att_sp->q_d_valid = true;
_pusher_throttle = 0.0f;
if (time_since_trans_start >= _params_standard.reverse_delay) {
// Handle throttle reversal for active breaking
float thrscale = (time_since_trans_start - _params_standard.reverse_delay) / (_params_standard.pusher_ramp_dt);
thrscale = math::constrain(thrscale, 0.0f, 1.0f);
_pusher_throttle = thrscale * _params->back_trans_throttle;
}
// continually increase mc attitude control as we transition back to mc mode
if (_params_standard.back_trans_ramp > FLT_EPSILON) {
mc_weight = time_since_trans_start / _params_standard.back_trans_ramp;
}
// in fw mode we need the multirotor motors to stop spinning, in backtransition mode we let them spin up again
if (_flag_enable_mc_motors) {
set_max_mc(2000);
set_idle_mc();
_flag_enable_mc_motors = false;
}
}
mc_weight = math::constrain(mc_weight, 0.0f, 1.0f);
_mc_roll_weight = mc_weight;
_mc_pitch_weight = mc_weight;
_mc_yaw_weight = mc_weight;
_mc_throttle_weight = mc_weight;
}
void Standard::update_mc_state()
{
VtolType::update_mc_state();
// enable MC motors here in case we transitioned directly to MC mode
if (_flag_enable_mc_motors) {
set_max_mc(2000);
set_idle_mc();
_flag_enable_mc_motors = false;
}
// if the thrust scale param is zero then the pusher-for-pitch strategy is disabled and we can return
if (_params_standard.forward_thrust_scale < FLT_EPSILON) {
return;
}
matrix::Dcmf R(matrix::Quatf(_v_att->q));
matrix::Dcmf R_sp(&_v_att_sp->R_body[0]);
matrix::Eulerf euler(R);
matrix::Eulerf euler_sp(R_sp);
_pusher_throttle = 0.0f;
// direction of desired body z axis represented in earth frame
matrix::Vector3f body_z_sp(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2));
// rotate desired body z axis into new frame which is rotated in z by the current
// heading of the vehicle. we refer to this as the heading frame.
matrix::Dcmf R_yaw = matrix::Eulerf(0.0f, 0.0f, -euler(2));
body_z_sp = R_yaw * body_z_sp;
body_z_sp.normalize();
// calculate the desired pitch seen in the heading frame
// this value corresponds to the amount the vehicle would try to pitch forward
float pitch_forward = asinf(body_z_sp(0));
// only allow pitching forward up to threshold, the rest of the desired
// forward acceleration will be compensated by the pusher
if (pitch_forward < -_params_standard.down_pitch_max) {
// desired roll angle in heading frame stays the same
float roll_new = -atan2f(body_z_sp(1), body_z_sp(2));
_pusher_throttle = (sinf(-pitch_forward) - sinf(_params_standard.down_pitch_max))
* _v_att_sp->thrust * _params_standard.forward_thrust_scale;
// limit desired pitch
float pitch_new = -_params_standard.down_pitch_max;
// create corrected desired body z axis in heading frame
matrix::Dcmf R_tmp = matrix::Eulerf(roll_new, pitch_new, 0.0f);
matrix::Vector3f tilt_new(R_tmp(0, 2), R_tmp(1, 2), R_tmp(2, 2));
// rotate the vector into a new frame which is rotated in z by the desired heading
// with respect to the earh frame.
float yaw_error = _wrap_pi(euler_sp(2) - euler(2));
matrix::Dcmf R_yaw_correction = matrix::Eulerf(0.0f, 0.0f, -yaw_error);
tilt_new = R_yaw_correction * tilt_new;
// now extract roll and pitch setpoints
float pitch = asinf(tilt_new(0));
float roll = -atan2f(tilt_new(1), tilt_new(2));
R_sp = matrix::Eulerf(roll, pitch, euler_sp(2));
matrix::Quatf q_sp(R_sp);
memcpy(&_v_att_sp->R_body[0], &R_sp._data[0], sizeof(_v_att_sp->R_body));
memcpy(&_v_att_sp->q_d[0], &q_sp._data[0], sizeof(_v_att_sp->q_d));
}
_pusher_throttle = _pusher_throttle < 0.0f ? 0.0f : _pusher_throttle;
}
void Standard::update_transition_state()
{
VtolType::update_transition_state();
// copy virtual attitude setpoint to real attitude setpoint
memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s));
if (_vtol_schedule.flight_mode == TRANSITION_TO_FW) {
if (_params_standard.front_trans_dur <= 0.0f) {
// just set the final target throttle value
_pusher_throttle = _params_standard.pusher_trans;
} else if (_pusher_throttle <= _params_standard.pusher_trans) {
// ramp up throttle to the target throttle value
_pusher_throttle = _params_standard.pusher_trans *
(float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_standard.front_trans_dur * 1000000.0f);
}
// do blending of mc and fw controls if a blending airspeed has been provided and the minimum transition time has passed
if (_airspeed_trans_blend_margin > 0.0f &&
_airspeed->indicated_airspeed_m_s >= _params_standard.airspeed_blend &&
(float)hrt_elapsed_time(&_vtol_schedule.transition_start) > (_params_standard.front_trans_time_min * 1000000.0f)
) {
float weight = 1.0f - fabsf(_airspeed->indicated_airspeed_m_s - _params_standard.airspeed_blend) /
_airspeed_trans_blend_margin;
_mc_roll_weight = weight;
_mc_pitch_weight = weight;
_mc_yaw_weight = weight;
_mc_throttle_weight = weight;
// time based blending when no airspeed sensor is set
} else if (_params_standard.airspeed_mode == control_state_s::AIRSPD_MODE_DISABLED &&
(float)hrt_elapsed_time(&_vtol_schedule.transition_start) < (_params_standard.front_trans_time_min * 1000000.0f) &&
(float)hrt_elapsed_time(&_vtol_schedule.transition_start) > ((_params_standard.front_trans_time_min / 2.0f) *
1000000.0f)
) {
float weight = 1.0f - ((float)(hrt_elapsed_time(&_vtol_schedule.transition_start) - ((
_params_standard.front_trans_time_min / 2.0f) * 1000000.0f)) /
((_params_standard.front_trans_time_min / 2.0f) * 1000000.0f));
weight = math::constrain(weight, 0.0f, 1.0f);
_mc_roll_weight = weight;
_mc_pitch_weight = weight;
_mc_yaw_weight = weight;
_mc_throttle_weight = weight;
} else {
// at low speeds give full weight to mc
_mc_roll_weight = 1.0f;
_mc_pitch_weight = 1.0f;
_mc_yaw_weight = 1.0f;
_mc_throttle_weight = 1.0f;
}
// check front transition timeout
if (_params_standard.front_trans_timeout > FLT_EPSILON) {
if ((float)hrt_elapsed_time(&_vtol_schedule.transition_start) > (_params_standard.front_trans_timeout * 1000000.0f)) {
// transition timeout occured, abort transition
_attc->abort_front_transition("Transition timeout");
}
}
} else if (_vtol_schedule.flight_mode == TRANSITION_TO_MC) {
// continually increase mc attitude control as we transition back to mc mode
if (_params_standard.back_trans_dur > 0.0f) {
float weight = (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_standard.back_trans_dur *
1000000.0f);
_mc_roll_weight = weight;
_mc_pitch_weight = weight;
_mc_yaw_weight = weight;
_mc_throttle_weight = weight;
} else {
_mc_roll_weight = 1.0f;
_mc_pitch_weight = 1.0f;
_mc_yaw_weight = 1.0f;
_mc_throttle_weight = 1.0f;
}
// in fw mode we need the multirotor motors to stop spinning, in backtransition mode we let them spin up again
if (_flag_enable_mc_motors) {
set_max_mc(2000);
set_idle_mc();
_flag_enable_mc_motors = false;
}
}
_mc_roll_weight = math::constrain(_mc_roll_weight, 0.0f, 1.0f);
_mc_pitch_weight = math::constrain(_mc_pitch_weight, 0.0f, 1.0f);
_mc_yaw_weight = math::constrain(_mc_yaw_weight, 0.0f, 1.0f);
_mc_throttle_weight = math::constrain(_mc_throttle_weight, 0.0f, 1.0f);
}
