diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg index f3f481159e..d3653ef54d 100644 --- a/msg/vehicle_status.msg +++ b/msg/vehicle_status.msg @@ -69,7 +69,7 @@ uint8 system_type # system type, contains mavlink MAV_TYPE uint8 system_id # system id, contains MAVLink's system ID field uint8 component_id # subsystem / component id, contains MAVLink's component ID field -uint8 vehicle_type # Type of vehicle (fixed-wing, rotary wing, ground) +uint8 vehicle_type # Type of vehicle (rotary wing/fixed-wing/rover/airship, see above) # If the vehicle is a VTOL, then this value will be VEHICLE_TYPE_ROTARY_WING while flying as a multicopter, # and VEHICLE_TYPE_FIXED_WING when flying as a fixed-wing diff --git a/msg/vtol_vehicle_status.msg b/msg/vtol_vehicle_status.msg index 05591fd49b..2971a25ab2 100644 --- a/msg/vtol_vehicle_status.msg +++ b/msg/vtol_vehicle_status.msg @@ -7,7 +7,6 @@ uint8 VEHICLE_VTOL_STATE_FW = 4 uint64 timestamp # time since system start (microseconds) -bool vtol_in_rw_mode # true: vtol vehicle is in rotating wing mode -bool vtol_in_trans_mode -bool in_transition_to_fw # True if VTOL is doing a transition from MC to FW +uint8 vehicle_vtol_state # current state of the vtol, see VEHICLE_VTOL_STATE + bool vtol_transition_failsafe # vtol in transition failsafe mode diff --git a/src/modules/airspeed_selector/airspeed_selector_main.cpp b/src/modules/airspeed_selector/airspeed_selector_main.cpp index a240083955..d641e8011c 100644 --- a/src/modules/airspeed_selector/airspeed_selector_main.cpp +++ b/src/modules/airspeed_selector/airspeed_selector_main.cpp @@ -513,7 +513,8 @@ void AirspeedModule::update_wind_estimator_sideslip() // update wind and airspeed estimator _wind_estimator_sideslip.update(_time_now_usec); - if (_vehicle_local_position_valid && !_vtol_vehicle_status.vtol_in_rw_mode) { + if (_vehicle_local_position_valid + && _vtol_vehicle_status.vehicle_vtol_state == vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW) { Vector3f vI(_vehicle_local_position.vx, _vehicle_local_position.vy, _vehicle_local_position.vz); Quatf q(_vehicle_attitude.q); diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index d1e1a327b9..2ef3f756e8 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -827,7 +827,7 @@ Commander::Commander() : _status_flags.rc_calibration_valid = true; // default for vtol is rotary wing - _vtol_status.vtol_in_rw_mode = true; + _vtol_status.vehicle_vtol_state = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC; _vehicle_gps_position_valid.set_hysteresis_time_from(false, GPS_VALID_TIME); _vehicle_gps_position_valid.set_hysteresis_time_from(true, GPS_VALID_TIME); @@ -2083,8 +2083,10 @@ void Commander::updateParameters() _auto_disarm_killed.set_hysteresis_time_from(false, _param_com_kill_disarm.get() * 1_s); _offboard_available.set_hysteresis_time_from(true, _param_com_of_loss_t.get() * 1_s); - const bool is_rotary = is_rotary_wing(_status) || (is_vtol(_status) && _vtol_status.vtol_in_rw_mode); - const bool is_fixed = is_fixed_wing(_status) || (is_vtol(_status) && !_vtol_status.vtol_in_rw_mode); + const bool is_rotary = is_rotary_wing(_status) || (is_vtol(_status) + && _vtol_status.vehicle_vtol_state != vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW); + const bool is_fixed = is_fixed_wing(_status) || (is_vtol(_status) + && _vtol_status.vehicle_vtol_state == vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW); const bool is_ground = is_ground_rover(_status); /* disable manual override for all systems that rely on electronic stabilization */ @@ -2304,25 +2306,29 @@ Commander::run() /* Make sure that this is only adjusted if vehicle really is of type vtol */ if (is_vtol(_status)) { - // Check if there has been any change while updating the flags - const auto new_vehicle_type = _vtol_status.vtol_in_rw_mode ? - vehicle_status_s::VEHICLE_TYPE_ROTARY_WING : - vehicle_status_s::VEHICLE_TYPE_FIXED_WING; + // Check if there has been any change while updating the flags (transition = rotary wing status) + const auto new_vehicle_type = _vtol_status.vehicle_vtol_state == vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW ? + vehicle_status_s::VEHICLE_TYPE_FIXED_WING : + vehicle_status_s::VEHICLE_TYPE_ROTARY_WING; if (new_vehicle_type != _status.vehicle_type) { - _status.vehicle_type = _vtol_status.vtol_in_rw_mode ? - vehicle_status_s::VEHICLE_TYPE_ROTARY_WING : - vehicle_status_s::VEHICLE_TYPE_FIXED_WING; + _status.vehicle_type = new_vehicle_type; _status_changed = true; } - if (_status.in_transition_mode != _vtol_status.vtol_in_trans_mode) { - _status.in_transition_mode = _vtol_status.vtol_in_trans_mode; + const bool new_in_transition = _vtol_status.vehicle_vtol_state == + vtol_vehicle_status_s::VEHICLE_VTOL_STATE_TRANSITION_TO_FW + || _vtol_status.vehicle_vtol_state == vtol_vehicle_status_s::VEHICLE_VTOL_STATE_TRANSITION_TO_MC; + + if (_status.in_transition_mode != new_in_transition) { + _status.in_transition_mode = new_in_transition; _status_changed = true; } - if (_status.in_transition_to_fw != _vtol_status.in_transition_to_fw) { - _status.in_transition_to_fw = _vtol_status.in_transition_to_fw; + if (_status.in_transition_to_fw != (_vtol_status.vehicle_vtol_state == + vtol_vehicle_status_s::VEHICLE_VTOL_STATE_TRANSITION_TO_FW)) { + _status.in_transition_to_fw = (_vtol_status.vehicle_vtol_state == + vtol_vehicle_status_s::VEHICLE_VTOL_STATE_TRANSITION_TO_FW); _status_changed = true; } diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp index 17013a35ec..c04f37502d 100644 --- a/src/modules/vtol_att_control/tailsitter.cpp +++ b/src/modules/vtol_att_control/tailsitter.cpp @@ -174,24 +174,20 @@ void Tailsitter::update_vtol_state() switch (_vtol_schedule.flight_mode) { case vtol_mode::MC_MODE: _vtol_mode = mode::ROTARY_WING; - _vtol_vehicle_status->vtol_in_trans_mode = false; _flag_was_in_trans_mode = false; break; case vtol_mode::FW_MODE: _vtol_mode = mode::FIXED_WING; - _vtol_vehicle_status->vtol_in_trans_mode = false; _flag_was_in_trans_mode = false; break; case vtol_mode::TRANSITION_FRONT_P1: _vtol_mode = mode::TRANSITION_TO_FW; - _vtol_vehicle_status->vtol_in_trans_mode = true; break; case vtol_mode::TRANSITION_BACK: _vtol_mode = mode::TRANSITION_TO_MC; - _vtol_vehicle_status->vtol_in_trans_mode = true; break; } } diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index d3847a2542..ea85555c60 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -59,7 +59,8 @@ VtolAttitudeControl::VtolAttitudeControl() : WorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl), _loop_perf(perf_alloc(PC_ELAPSED, "vtol_att_control: cycle")) { - _vtol_vehicle_status.vtol_in_rw_mode = true; /* start vtol in rotary wing mode*/ + // start vtol in rotary wing mode + _vtol_vehicle_status.vehicle_vtol_state = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC; parameters_update(); @@ -327,13 +328,19 @@ VtolAttitudeControl::Run() // check in which mode we are in and call mode specific functions switch (_vtol_type->get_mode()) { case mode::TRANSITION_TO_FW: - case mode::TRANSITION_TO_MC: - // vehicle is doing a transition - _vtol_vehicle_status.vtol_in_trans_mode = true; - _vtol_vehicle_status.vtol_in_rw_mode = true; // making mc attitude controller work during transition - _vtol_vehicle_status.in_transition_to_fw = (_vtol_type->get_mode() == mode::TRANSITION_TO_FW); + // vehicle is doing a transition to FW + _vtol_vehicle_status.vehicle_vtol_state = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_TRANSITION_TO_FW; - _fw_virtual_att_sp_sub.update(&_fw_virtual_att_sp); + if (!_vtol_type->was_in_trans_mode() || mc_att_sp_updated || fw_att_sp_updated) { + _vtol_type->update_transition_state(); + _v_att_sp_pub.publish(_v_att_sp); + } + + break; + + case mode::TRANSITION_TO_MC: + // vehicle is doing a transition to MC + _vtol_vehicle_status.vehicle_vtol_state = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_TRANSITION_TO_MC; if (!_vtol_type->was_in_trans_mode() || mc_att_sp_updated || fw_att_sp_updated) { _vtol_type->update_transition_state(); @@ -344,20 +351,18 @@ VtolAttitudeControl::Run() case mode::ROTARY_WING: // vehicle is in rotary wing mode - _vtol_vehicle_status.vtol_in_rw_mode = true; - _vtol_vehicle_status.vtol_in_trans_mode = false; - _vtol_vehicle_status.in_transition_to_fw = false; + _vtol_vehicle_status.vehicle_vtol_state = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC; - _vtol_type->update_mc_state(); - _v_att_sp_pub.publish(_v_att_sp); + if (mc_att_sp_updated) { + _vtol_type->update_mc_state(); + _v_att_sp_pub.publish(_v_att_sp); + } break; case mode::FIXED_WING: // vehicle is in fw mode - _vtol_vehicle_status.vtol_in_rw_mode = false; - _vtol_vehicle_status.vtol_in_trans_mode = false; - _vtol_vehicle_status.in_transition_to_fw = false; + _vtol_vehicle_status.vehicle_vtol_state = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW; if (fw_att_sp_updated) { _vtol_type->update_fw_state();