diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg index 68e4aa9dc6..244b8c1d92 100644 --- a/msg/vehicle_status.msg +++ b/msg/vehicle_status.msg @@ -106,6 +106,7 @@ uint32 component_id # subsystem / component id, inspired by MAVLink's componen bool is_rotary_wing # True if system is in rotary wing configuration, so for a VTOL this is only true while flying as a multicopter bool is_vtol # True if the system is VTOL capable bool vtol_fw_permanent_stab # True if vtol should stabilize attitude for fw in manual mode +bool in_transition_mode bool condition_battery_voltage_valid bool condition_system_in_air_restore # true if we can restore in mid air diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 5629ec43de..e103de3f61 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1498,6 +1498,7 @@ int commander_thread_main(int argc, char *argv[]) /* Make sure that this is only adjusted if vehicle really is of type vtol*/ if (is_vtol(&status)) { status.is_rotary_wing = vtol_status.vtol_in_rw_mode; + status.in_transition_mode = vtol_status.vtol_in_trans_mode; } } diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index ee78ef3110..c3f83e21ba 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -941,7 +941,7 @@ FixedwingAttitudeControl::task_main() att_sp.thrust = throttle_sp; /* lazily publish the setpoint only once available */ - if (!_vehicle_status.is_rotary_wing) { + if (!_vehicle_status.is_rotary_wing && !_vehicle_status.in_transition_mode) { if (_attitude_sp_pub != nullptr) { /* publish the attitude setpoint */ orb_publish(ORB_ID(vehicle_attitude_setpoint), _attitude_sp_pub, &att_sp); diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 8d2ebbb6af..0b18c9785d 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -1459,7 +1459,7 @@ MulticopterPositionControl::task_main() if (!(_control_mode.flag_control_offboard_enabled && !(_control_mode.flag_control_position_enabled || _control_mode.flag_control_velocity_enabled))) { - if (_att_sp_pub != nullptr && _vehicle_status.is_rotary_wing) { + if (_att_sp_pub != nullptr && (_vehicle_status.is_rotary_wing || _vehicle_status.in_transition_mode)) { orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp); } else if (_att_sp_pub == nullptr && _vehicle_status.is_rotary_wing){ _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp); diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp index 7bd58a4cc9..06cde02a70 100644 --- a/src/modules/vtol_att_control/tiltrotor.cpp +++ b/src/modules/vtol_att_control/tiltrotor.cpp @@ -281,12 +281,12 @@ void Tiltrotor::update_transition_state() // 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); + 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 = 1.0f - (_airspeed->true_airspeed_m_s - _params_tiltrotor.airspeed_blend_start) / (_params_tiltrotor.airspeed_trans - _params_tiltrotor.airspeed_blend_start); + _mc_roll_weight = 0.0f; } else { // at low speeds give full weight to mc _mc_roll_weight = 1.0f; @@ -301,18 +301,26 @@ void Tiltrotor::update_transition_state() } 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); + 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); + 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; }