dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

vtol_vehicle_status: replace several status bools by single vehicle_vtol_state 

Replace vtol_in_rw_mode, vtol_in_trans_mode, in_transition_to_fw by vehicle_vtol_state.

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer 2022-01-20 12:55:06 +01:00
parent 635f64a2e5
commit 2b7efeacca
6 changed files with 45 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
msg/vehicle_status.msg
View File

@ -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

5
msg/vtol_vehicle_status.msg
View File

@ -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

3
src/modules/airspeed_selector/airspeed_selector_main.cpp
View File

@ -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);

34
src/modules/commander/Commander.cpp
View File

@ -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;
}

4
src/modules/vtol_att_control/tailsitter.cpp
View File

@ -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;
}
}

35
src/modules/vtol_att_control/vtol_att_control_main.cpp
View File

@ -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();