void UpdateHighStateController(int state) {
if (state != control_state) {
control_state = state;
switch (control_state) {
case STATE_CONTROL_HIGH_DISABLE:
set_motor_state(-1, STATE_CONTROL_DISABLE);
break;
default:
set_motor_state(-1, STATE_CONTROL_VELOCITY);
break;
}
}
}
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));
}
