diff options
-rw-r--r-- | src/modules/commander/commander.cpp | 18 | ||||
-rw-r--r-- | src/modules/fw_att_control/fw_att_control_main.cpp | 32 | ||||
-rw-r--r-- | src/modules/mc_att_control/mc_att_control_main.cpp | 19 | ||||
-rw-r--r-- | src/modules/uORB/topics/vehicle_status.h | 4 |
4 files changed, 40 insertions, 33 deletions
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 744323c01..8d2e62e32 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1092,6 +1092,11 @@ int commander_thread_main(int argc, char *argv[]) status.is_rotary_wing = false; } + /* set vehicle_status.is_vtol flag */ + status.is_vtol = (status.system_type == VEHICLE_TYPE_VTOL_DUOROTOR) || + (status.system_type == VEHICLE_TYPE_VTOL_QUADROTOR); + + /* check and update system / component ID */ param_get(_param_system_id, &(status.system_id)); param_get(_param_component_id, &(status.component_id)); @@ -1113,6 +1118,8 @@ int commander_thread_main(int argc, char *argv[]) /* navigation parameters */ param_get(_param_takeoff_alt, &takeoff_alt); + + /* Safety parameters */ param_get(_param_enable_parachute, ¶chute_enabled); param_get(_param_enable_datalink_loss, &datalink_loss_enabled); param_get(_param_datalink_loss_timeout, &datalink_loss_timeout); @@ -1121,6 +1128,8 @@ int commander_thread_main(int argc, char *argv[]) param_get(_param_ef_throttle_thres, &ef_throttle_thres); param_get(_param_ef_current2throttle_thres, &ef_current2throttle_thres); param_get(_param_ef_time_thres, &ef_time_thres); + + /* Autostart id */ param_get(_param_autostart_id, &autostart_id); } @@ -2217,14 +2226,7 @@ set_control_mode() { /* set vehicle_control_mode according to set_navigation_state */ control_mode.flag_armed = armed.armed; - /* TODO: check this */ - if (autostart_id < 13000 || autostart_id >= 14000) { - control_mode.flag_external_manual_override_ok = !status.is_rotary_wing; - - } else { - control_mode.flag_external_manual_override_ok = false; - } - + control_mode.flag_external_manual_override_ok = !status.is_rotary_wing && !status.is_vtol; control_mode.flag_system_hil_enabled = status.hil_state == HIL_STATE_ON; control_mode.flag_control_offboard_enabled = false; 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 8d18ae68c..72d9a5452 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -338,6 +338,9 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _actuators_0_pub(-1), _actuators_2_pub(-1), + _rates_sp_id(ORB_ID(vehicle_rates_setpoint)), + _actuators_id(ORB_ID(actuator_controls_0)), + /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "fw att control")), _nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control nonfinite input")), @@ -400,15 +403,6 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : /* fetch initial parameter values */ parameters_update(); - // set correct uORB ID, depending on if vehicle is VTOL or not - if (_parameters.autostart_id >= 13000 && _parameters.autostart_id <= 13999) { /* VTOL airframe?*/ - _rates_sp_id = ORB_ID(fw_virtual_rates_setpoint); - _actuators_id = ORB_ID(actuator_controls_virtual_fw); - } - else { - _rates_sp_id = ORB_ID(vehicle_rates_setpoint); - _actuators_id = ORB_ID(actuator_controls_0); - } } FixedwingAttitudeControl::~FixedwingAttitudeControl() @@ -600,6 +594,14 @@ FixedwingAttitudeControl::vehicle_status_poll() if (vehicle_status_updated) { orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status); + /* set correct uORB ID, depending on if vehicle is VTOL or not */ + if (_vehicle_status.is_vtol) { + _rates_sp_id = ORB_ID(fw_virtual_rates_setpoint); + _actuators_id = ORB_ID(actuator_controls_virtual_fw); + } else { + _rates_sp_id = ORB_ID(vehicle_rates_setpoint); + _actuators_id = ORB_ID(actuator_controls_0); + } } } @@ -700,11 +702,11 @@ FixedwingAttitudeControl::task_main() /* load local copies */ orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att); - if (_parameters.autostart_id >= 13000 - && _parameters.autostart_id <= 13999) { //vehicle type is VTOL, need to modify attitude! - /* The following modification to the attitude is vehicle specific and in this case applies - to tail-sitter models !!! - + if (_vehicle_status.is_vtol) { + /* vehicle type is VTOL, need to modify attitude! + * The following modification to the attitude is vehicle specific and in this case applies + * to tail-sitter models !!! + * * Since the VTOL airframe is initialized as a multicopter we need to * modify the estimated attitude for the fixed wing operation. * Since the neutral position of the vehicle in fixed wing mode is -90 degrees rotated around @@ -849,7 +851,7 @@ FixedwingAttitudeControl::task_main() _yaw_ctrl.reset_integrator(); } } else if (_vcontrol_mode.flag_control_velocity_enabled) { - /* + /* * Velocity should be controlled and manual is enabled. */ roll_sp = (_manual.y * _parameters.man_roll_max - _parameters.trim_roll) diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 0702e6378..d864feb25 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -285,6 +285,8 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _att_sp_pub(-1), _v_rates_sp_pub(-1), _actuators_0_pub(-1), + _rates_sp_id(ORB_ID(vehicle_rates_setpoint)), + _actuators_id(ORB_ID(actuator_controls_0)), _actuators_0_circuit_breaker_enabled(false), @@ -348,15 +350,6 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : /* fetch initial parameter values */ parameters_update(); - // set correct uORB ID, depending on if vehicle is VTOL or not - if (_params.autostart_id >= 13000 && _params.autostart_id <= 13999) { /* VTOL airframe?*/ - _rates_sp_id = ORB_ID(mc_virtual_rates_setpoint); - _actuators_id = ORB_ID(actuator_controls_virtual_mc); - } - else { - _rates_sp_id = ORB_ID(vehicle_rates_setpoint); - _actuators_id = ORB_ID(actuator_controls_0); - } } MulticopterAttitudeControl::~MulticopterAttitudeControl() @@ -531,6 +524,14 @@ MulticopterAttitudeControl::vehicle_status_poll() if (vehicle_status_updated) { orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status); + /* set correct uORB ID, depending on if vehicle is VTOL or not */ + if (_vehicle_status.is_vtol) { + _rates_sp_id = ORB_ID(mc_virtual_rates_setpoint); + _actuators_id = ORB_ID(actuator_controls_virtual_mc); + } else { + _rates_sp_id = ORB_ID(vehicle_rates_setpoint); + _actuators_id = ORB_ID(actuator_controls_0); + } } } diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 749d00d75..b4d5c7b88 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -185,7 +185,9 @@ struct vehicle_status_s { int32_t system_id; /**< system id, inspired by MAVLink's system ID field */ int32_t component_id; /**< subsystem / component id, inspired by MAVLink's component ID field */ - bool is_rotary_wing; + 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 condition_battery_voltage_valid; bool condition_system_in_air_restore; /**< true if we can restore in mid air */ |