From e18065081af0dbbbf50aaba30cc0b14621c7b29b Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 18 Dec 2014 14:46:31 +0100 Subject: is_vtol flag in vehicle_status Getting rid of the autostart based checks if the system is a vtol Fixes #1503 --- src/modules/commander/commander.cpp | 18 ++++++------ src/modules/fw_att_control/fw_att_control_main.cpp | 32 ++++++++++++---------- src/modules/mc_att_control/mc_att_control_main.cpp | 19 +++++++------ src/modules/uORB/topics/vehicle_status.h | 4 ++- 4 files changed, 40 insertions(+), 33 deletions(-) (limited to 'src') 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 */ -- cgit v1.2.3 From 32675ff1c150529bbb662fde87c0940ee90acd6e Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 18 Dec 2014 14:59:52 +0100 Subject: fix flag_external_manual_override_ok The flag should not depend on the vtol state anymore. The intended functionality of this is by now handled via the is_rotary_wing flag --- src/modules/commander/commander.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) (limited to 'src') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 8d2e62e32..5779183ab 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1096,7 +1096,6 @@ int commander_thread_main(int argc, char *argv[]) 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)); @@ -2226,7 +2225,7 @@ set_control_mode() { /* set vehicle_control_mode according to set_navigation_state */ control_mode.flag_armed = armed.armed; - control_mode.flag_external_manual_override_ok = !status.is_rotary_wing && !status.is_vtol; + control_mode.flag_external_manual_override_ok = !status.is_rotary_wing; control_mode.flag_system_hil_enabled = status.hil_state == HIL_STATE_ON; control_mode.flag_control_offboard_enabled = false; -- cgit v1.2.3 From 6f83f77bf422c4dd66f63aa2ac83e82dc2283b94 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 22 Dec 2014 15:18:09 +0100 Subject: fw att ctl: remove autostart id param --- src/modules/fw_att_control/fw_att_control_main.cpp | 7 ------- 1 file changed, 7 deletions(-) (limited to 'src') 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 72d9a5452..598408c9f 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -194,8 +194,6 @@ private: float man_roll_max; /**< Max Roll in rad */ float man_pitch_max; /**< Max Pitch in rad */ - param_t autostart_id; /* indicates which airframe is used */ - } _parameters; /**< local copies of interesting parameters */ struct { @@ -236,7 +234,6 @@ private: param_t man_roll_max; param_t man_pitch_max; - param_t autostart_id; /* indicates which airframe is used */ } _parameter_handles; /**< handles for interesting parameters */ @@ -399,8 +396,6 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _parameter_handles.man_roll_max = param_find("FW_MAN_R_MAX"); _parameter_handles.man_pitch_max = param_find("FW_MAN_P_MAX"); - _parameter_handles.autostart_id = param_find("SYS_AUTOSTART"); - /* fetch initial parameter values */ parameters_update(); } @@ -477,8 +472,6 @@ FixedwingAttitudeControl::parameters_update() _parameters.man_roll_max = math::radians(_parameters.man_roll_max); _parameters.man_pitch_max = math::radians(_parameters.man_pitch_max); - param_get(_parameter_handles.autostart_id, &_parameters.autostart_id); - /* pitch control parameters */ _pitch_ctrl.set_time_constant(_parameters.tconst); _pitch_ctrl.set_k_p(_parameters.p_p); -- cgit v1.2.3 From 6dd98cb3118d7d08bbf01b58737dcdc53b429e1b Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 22 Dec 2014 15:18:20 +0100 Subject: mc att ctl: remove autostart id param --- src/modules/mc_att_control/mc_att_control_main.cpp | 8 -------- 1 file changed, 8 deletions(-) (limited to 'src') 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 d864feb25..25b085b7b 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -175,7 +175,6 @@ private: param_t acro_pitch_max; param_t acro_yaw_max; - param_t autostart_id; //what frame are we using? } _params_handles; /**< handles for interesting parameters */ struct { @@ -191,7 +190,6 @@ private: float man_yaw_max; math::Vector<3> acro_rate_max; /**< max attitude rates in acro mode */ - param_t autostart_id; } _params; /** @@ -315,8 +313,6 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _params.man_yaw_max = 0.0f; _params.acro_rate_max.zero(); - _params.autostart_id = 0; //default - _rates_prev.zero(); _rates_sp.zero(); _rates_int.zero(); @@ -346,8 +342,6 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _params_handles.acro_pitch_max = param_find("MC_ACRO_P_MAX"); _params_handles.acro_yaw_max = param_find("MC_ACRO_Y_MAX"); - _params_handles.autostart_id = param_find("SYS_AUTOSTART"); - /* fetch initial parameter values */ parameters_update(); } @@ -433,8 +427,6 @@ MulticopterAttitudeControl::parameters_update() _actuators_0_circuit_breaker_enabled = circuit_breaker_enabled("CBRK_RATE_CTRL", CBRK_RATE_CTRL_KEY); - param_get(_params_handles.autostart_id, &_params.autostart_id); - return OK; } -- cgit v1.2.3 From d8eefa30538331fde7e5ce79fef4e04dc62664bc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 23 Dec 2014 21:50:16 +0100 Subject: VTOL: Do not allow manual override --- src/modules/commander/commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 5779183ab..de3e69373 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -2225,7 +2225,7 @@ set_control_mode() { /* set vehicle_control_mode according to set_navigation_state */ control_mode.flag_armed = armed.armed; - control_mode.flag_external_manual_override_ok = !status.is_rotary_wing; + 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; -- cgit v1.2.3