void VtolType::update_mc_state()
{
if (!flag_idle_mc) {
flag_idle_mc = set_idle_mc();
}
if (_motor_state != ENABLED) {
_motor_state = VtolType::set_motor_state(_motor_state, ENABLED);
}
// copy virtual attitude setpoint to real attitude setpoint
memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s));
_mc_roll_weight = 1.0f;
_mc_pitch_weight = 1.0f;
_mc_yaw_weight = 1.0f;
// VTOL weathervane
_v_att_sp->disable_mc_yaw_control = false;
if (_attc->get_pos_sp_triplet()->current.valid &&
!_v_control_mode->flag_control_manual_enabled) {
if (_params->wv_takeoff && _attc->get_pos_sp_triplet()->current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) {
_v_att_sp->disable_mc_yaw_control = true;
} else if (_params->wv_loiter
&& _attc->get_pos_sp_triplet()->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER) {
_v_att_sp->disable_mc_yaw_control = true;
} else if (_params->wv_land && _attc->get_pos_sp_triplet()->current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
_v_att_sp->disable_mc_yaw_control = true;
}
}
}
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 Tiltrotor::update_transition_state()
{
if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P1) {
// for the first part of the transition the rear rotors are enabled
if (_rear_motors != ENABLED) {
set_rear_motor_state(ENABLED);
}
// tilt rotors forward up to certain angle
if (_tilt_control <= _params_tiltrotor.tilt_transition ) {
_tilt_control = _params_tiltrotor.tilt_mc +
fabsf(_params_tiltrotor.tilt_transition - _params_tiltrotor.tilt_mc) * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tiltrotor.front_trans_dur * 1000000.0f);
}
// do blending of mc and fw controls
if (_airspeed->true_airspeed_m_s >= _params_tiltrotor.airspeed_blend_start) {
_mc_roll_weight = 0.0f;
} else {
// at low speeds give full weight to mc
_mc_roll_weight = 1.0f;
}
// disable mc yaw control once the plane has picked up speed
_mc_yaw_weight = 1.0f;
if (_airspeed->true_airspeed_m_s > ARSP_YAW_CTRL_DISABLE) {
_mc_yaw_weight = 0.0f;
}
} else if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P2) {
// the plane is ready to go into fixed wing mode, tilt the rotors forward completely
_tilt_control = _params_tiltrotor.tilt_transition +
fabsf(_params_tiltrotor.tilt_fw - _params_tiltrotor.tilt_transition) * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tiltrotor.front_trans_dur_p2 * 1000000.0f);
_mc_roll_weight = 0.0f;
} else if (_vtol_schedule.flight_mode == TRANSITION_BACK) {
if (_rear_motors != IDLE) {
set_rear_motor_state(IDLE);
}
if (!flag_idle_mc) {
set_idle_mc();
flag_idle_mc = true;
}
// tilt rotors back
if (_tilt_control > _params_tiltrotor.tilt_mc) {
_tilt_control = _params_tiltrotor.tilt_fw -
fabsf(_params_tiltrotor.tilt_fw - _params_tiltrotor.tilt_mc) * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tiltrotor.back_trans_dur * 1000000.0f);
}
// set zero throttle for backtransition otherwise unwanted moments will be created
_actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE] = 0.0f;
_mc_roll_weight = 0.0f;
}
_mc_roll_weight = math::constrain(_mc_roll_weight, 0.0f, 1.0f);
_mc_yaw_weight = math::constrain(_mc_yaw_weight, 0.0f, 1.0f);
}
void Tailsitter::update_mc_state()
{
VtolType::update_mc_state();
// set idle speed for rotary wing mode
if (!flag_idle_mc) {
set_idle_mc();
flag_idle_mc = true;
}
}
void Tailsitter::update_mc_state()
{
// set idle speed for rotary wing mode
if (!flag_idle_mc) {
set_idle_mc();
flag_idle_mc = true;
}
_mc_roll_weight = 1.0f;
_mc_pitch_weight = 1.0f;
_mc_yaw_weight = 1.0f;
// copy virtual attitude setpoint to real attitude setpoint
memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s));
}
void Tiltrotor::update_mc_state()
{
// make sure motors are not tilted
_tilt_control = _params_tiltrotor.tilt_mc;
// enable rear motors
if (_rear_motors != ENABLED) {
set_rear_motor_state(ENABLED);
}
// set idle speed for rotary wing mode
if (!flag_idle_mc) {
set_idle_mc();
flag_idle_mc = true;
}
_mc_roll_weight = 1.0f;
_mc_pitch_weight = 1.0f;
_mc_yaw_weight = 1.0f;
}
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);
}
void Tailsitter::update_transition_state()
{
if (!_flag_was_in_trans_mode) {
// save desired heading for transition and last thrust value
_yaw_transition = _v_att_sp->yaw_body;
_pitch_transition_start = _v_att_sp->pitch_body;
_throttle_transition = _v_att_sp->thrust;
_flag_was_in_trans_mode = true;
}
if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P1) {
/** 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_tailsitter.front_trans_dur * 1000000.0f));
_v_att_sp->pitch_body = math::constrain(_v_att_sp->pitch_body , PITCH_TRANSITION_FRONT_P1 - 0.2f ,
_pitch_transition_start);
/** create time dependant throttle signal higher than in MC and growing untill P2 switch speed reached */
if (_airspeed->true_airspeed_m_s <= _params_tailsitter.airspeed_trans) {
_v_att_sp->thrust = _throttle_transition + (fabsf(THROTTLE_TRANSITION_MAX * _throttle_transition) *
(float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tailsitter.front_trans_dur * 1000000.0f));
_v_att_sp->thrust = math::constrain(_v_att_sp->thrust , _throttle_transition ,
(1.0f + THROTTLE_TRANSITION_MAX) * _throttle_transition);
}
// disable mc yaw control once the plane has picked up speed
if (_airspeed->true_airspeed_m_s > ARSP_YAW_CTRL_DISABLE) {
_mc_yaw_weight = 0.0f;
} else {
_mc_yaw_weight = 1.0f;
}
_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;
}
}
/** 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) {
set_idle_mc();
flag_idle_mc = true;
}
/** 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_tailsitter.back_trans_dur * 1000000.0f);
_v_att_sp->pitch_body = math::constrain(_v_att_sp->pitch_body , -2.0f , PITCH_TRANSITION_BACK + 0.2f);
// throttle value is decreesed
_v_att_sp->thrust = _throttle_transition * 0.9f;
/** 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_tailsitter.back_trans_dur * 1000000.0f);
_mc_pitch_weight = 1.0f * (float)hrt_elapsed_time(&_vtol_schedule.transition_start) /
(_params_tailsitter.back_trans_dur * 1000000.0f);
}
_mc_roll_weight = math::constrain(_mc_roll_weight, 0.0f, 1.0f);
_mc_yaw_weight = math::constrain(_mc_yaw_weight, 0.0f, 1.0f);
_mc_pitch_weight = math::constrain(_mc_pitch_weight, 0.0f, 1.0f);
// compute desired attitude and thrust setpoint for the transition
_v_att_sp->timestamp = hrt_absolute_time();
_v_att_sp->roll_body = 0.0f;
_v_att_sp->yaw_body = _yaw_transition;
_v_att_sp->R_valid = true;
math::Matrix<3, 3> R_sp;
R_sp.from_euler(_v_att_sp->roll_body, _v_att_sp->pitch_body, _v_att_sp->yaw_body);
memcpy(&_v_att_sp->R_body[0], R_sp.data, sizeof(_v_att_sp->R_body));
math::Quaternion q_sp;
q_sp.from_dcm(R_sp);
memcpy(&_v_att_sp->q_d[0], &q_sp.data[0], sizeof(_v_att_sp->q_d));
}
void Tiltrotor::update_transition_state()
{
VtolType::update_transition_state();
float time_since_trans_start = (float)(hrt_absolute_time() - _vtol_schedule.transition_start) * 1e-6f;
if (!_flag_was_in_trans_mode) {
// save desired heading for transition and last thrust value
_flag_was_in_trans_mode = true;
}
if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P1) {
// for the first part of the transition the rear rotors are enabled
if (_motor_state != ENABLED) {
_motor_state = set_motor_state(_motor_state, ENABLED);
}
// tilt rotors forward up to certain angle
if (_tilt_control <= _params_tiltrotor.tilt_transition) {
_tilt_control = _params_tiltrotor.tilt_mc +
fabsf(_params_tiltrotor.tilt_transition - _params_tiltrotor.tilt_mc) * time_since_trans_start /
_params->front_trans_duration;
}
// at low speeds give full weight to MC
_mc_roll_weight = 1.0f;
_mc_yaw_weight = 1.0f;
// reduce MC controls once the plane has picked up speed
if (!_params->airspeed_disabled && _airspeed->indicated_airspeed_m_s > ARSP_YAW_CTRL_DISABLE) {
_mc_yaw_weight = 0.0f;
}
if (!_params->airspeed_disabled && _airspeed->indicated_airspeed_m_s >= _params->airspeed_blend) {
_mc_roll_weight = 1.0f - (_airspeed->indicated_airspeed_m_s - _params->airspeed_blend) /
(_params->transition_airspeed - _params->airspeed_blend);
}
// without airspeed do timed weight changes
if (_params->airspeed_disabled
&& time_since_trans_start > _params->front_trans_time_min) {
_mc_roll_weight = 1.0f - (time_since_trans_start - _params->front_trans_time_min) /
(_params->front_trans_time_openloop - _params->front_trans_time_min);
_mc_yaw_weight = _mc_roll_weight;
}
_thrust_transition = _mc_virtual_att_sp->thrust;
} else if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P2) {
// the plane is ready to go into fixed wing mode, tilt the rotors forward completely
_tilt_control = _params_tiltrotor.tilt_transition +
fabsf(_params_tiltrotor.tilt_fw - _params_tiltrotor.tilt_transition) * time_since_trans_start /
_params_tiltrotor.front_trans_dur_p2;
_mc_roll_weight = 0.0f;
_mc_yaw_weight = 0.0f;
// ramp down rear motors (setting MAX_PWM down scales the given output into the new range)
int rear_value = (1.0f - time_since_trans_start / _params_tiltrotor.front_trans_dur_p2) *
(PWM_DEFAULT_MAX - PWM_DEFAULT_MIN) + PWM_DEFAULT_MIN;
_motor_state = set_motor_state(_motor_state, VALUE, rear_value);
_thrust_transition = _mc_virtual_att_sp->thrust;
} else if (_vtol_schedule.flight_mode == TRANSITION_BACK) {
if (_motor_state != ENABLED) {
_motor_state = set_motor_state(_motor_state, ENABLED);
}
// set idle speed for rotary wing mode
if (!flag_idle_mc) {
flag_idle_mc = set_idle_mc();
}
// tilt rotors back
if (_tilt_control > _params_tiltrotor.tilt_mc) {
_tilt_control = _params_tiltrotor.tilt_fw -
fabsf(_params_tiltrotor.tilt_fw - _params_tiltrotor.tilt_mc) * time_since_trans_start / _params->back_trans_duration;
}
// set zero throttle for backtransition otherwise unwanted moments will be created
_actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE] = 0.0f;
_mc_roll_weight = 0.0f;
}
_mc_roll_weight = math::constrain(_mc_roll_weight, 0.0f, 1.0f);
_mc_yaw_weight = math::constrain(_mc_yaw_weight, 0.0f, 1.0f);
// copy virtual attitude setpoint to real attitude setpoint (we use multicopter att sp)
memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s));
}
void VtolAttitudeControl::task_main()
{
warnx("started");
fflush(stdout);
/* do subscriptions */
_v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
_mc_virtual_v_rates_sp_sub = orb_subscribe(ORB_ID(mc_virtual_rates_setpoint));
_fw_virtual_v_rates_sp_sub = orb_subscribe(ORB_ID(fw_virtual_rates_setpoint));
_v_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
_v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
_local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
_battery_status_sub = orb_subscribe(ORB_ID(battery_status));
_actuator_inputs_mc = orb_subscribe(ORB_ID(actuator_controls_virtual_mc));
_actuator_inputs_fw = orb_subscribe(ORB_ID(actuator_controls_virtual_fw));
parameters_update(); // initialize parameter cache
/* update vtol vehicle status*/
_vtol_vehicle_status.fw_permanent_stab = _params.vtol_fw_permanent_stab == 1 ? true : false;
// make sure we start with idle in mc mode
set_idle_mc();
flag_idle_mc = true;
/* wakeup source*/
struct pollfd fds[3]; /*input_mc, input_fw, parameters*/
fds[0].fd = _actuator_inputs_mc;
fds[0].events = POLLIN;
fds[1].fd = _actuator_inputs_fw;
fds[1].events = POLLIN;
fds[2].fd = _params_sub;
fds[2].events = POLLIN;
while (!_task_should_exit) {
/*Advertise/Publish vtol vehicle status*/
if (_vtol_vehicle_status_pub > 0) {
orb_publish(ORB_ID(vtol_vehicle_status), _vtol_vehicle_status_pub, &_vtol_vehicle_status);
} else {
_vtol_vehicle_status.timestamp = hrt_absolute_time();
_vtol_vehicle_status_pub = orb_advertise(ORB_ID(vtol_vehicle_status), &_vtol_vehicle_status);
}
/* wait for up to 100ms for data */
int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
/* timed out - periodic check for _task_should_exit */
if (pret == 0) {
continue;
}
/* this is undesirable but not much we can do - might want to flag unhappy status */
if (pret < 0) {
warn("poll error %d, %d", pret, errno);
/* sleep a bit before next try */
usleep(100000);
continue;
}
if (fds[2].revents & POLLIN) { //parameters were updated, read them now
/* read from param to clear updated flag */
struct parameter_update_s update;
orb_copy(ORB_ID(parameter_update), _params_sub, &update);
/* update parameters from storage */
parameters_update();
}
_vtol_vehicle_status.fw_permanent_stab = _params.vtol_fw_permanent_stab == 1 ? true : false;
vehicle_control_mode_poll(); //Check for changes in vehicle control mode.
vehicle_manual_poll(); //Check for changes in manual inputs.
arming_status_poll(); //Check for arming status updates.
actuator_controls_mc_poll(); //Check for changes in mc_attitude_control output
actuator_controls_fw_poll(); //Check for changes in fw_attitude_control output
vehicle_rates_sp_mc_poll();
vehicle_rates_sp_fw_poll();
parameters_update_poll();
vehicle_local_pos_poll(); // Check for new sensor values
vehicle_airspeed_poll();
vehicle_battery_poll();
if (_manual_control_sp.aux1 <= 0.0f) { /* vehicle is in mc mode */
_vtol_vehicle_status.vtol_in_rw_mode = true;
if (!flag_idle_mc) { /* we want to adjust idle speed for mc mode */
set_idle_mc();
flag_idle_mc = true;
}
/* got data from mc_att_controller */
if (fds[0].revents & POLLIN) {
vehicle_manual_poll(); /* update remote input */
orb_copy(ORB_ID(actuator_controls_virtual_mc), _actuator_inputs_mc, &_actuators_mc_in);
// scale pitch control with total airspeed
scale_mc_output();
fill_mc_att_control_output();
fill_mc_att_rates_sp();
if (_actuators_0_pub > 0) {
orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators_out_0);
} else {
_actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators_out_0);
}
if (_actuators_1_pub > 0) {
orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_out_1);
} else {
_actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_out_1);
}
}
}
if (_manual_control_sp.aux1 >= 0.0f) { /* vehicle is in fw mode */
_vtol_vehicle_status.vtol_in_rw_mode = false;
if (flag_idle_mc) { /* we want to adjust idle speed for fixed wing mode */
set_idle_fw();
flag_idle_mc = false;
}
if (fds[1].revents & POLLIN) { /* got data from fw_att_controller */
orb_copy(ORB_ID(actuator_controls_virtual_fw), _actuator_inputs_fw, &_actuators_fw_in);
vehicle_manual_poll(); //update remote input
fill_fw_att_control_output();
fill_fw_att_rates_sp();
if (_actuators_0_pub > 0) {
orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators_out_0);
} else {
_actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators_out_0);
}
if (_actuators_1_pub > 0) {
orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_out_1);
} else {
_actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_out_1);
}
}
}
// publish the attitude rates setpoint
if(_v_rates_sp_pub > 0) {
orb_publish(ORB_ID(vehicle_rates_setpoint),_v_rates_sp_pub,&_v_rates_sp);
}
else {
_v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint),&_v_rates_sp);
}
}
warnx("exit");
_control_task = -1;
_exit(0);
}
