diff options
author | Thomas Gubler <thomasgubler@gmail.com> | 2014-12-25 09:59:58 +0100 |
---|---|---|
committer | Thomas Gubler <thomasgubler@gmail.com> | 2014-12-25 09:59:58 +0100 |
commit | b376316fb19e497f76512a0353183936396e769d (patch) | |
tree | 428bcd6ee1926b57a64cc47d903273225fb56ba4 /src/modules | |
parent | 25af4b266ca48b183a1ad375856396f67d6ab30f (diff) | |
parent | d8eefa30538331fde7e5ce79fef4e04dc62664bc (diff) | |
download | px4-firmware-b376316fb19e497f76512a0353183936396e769d.tar.gz px4-firmware-b376316fb19e497f76512a0353183936396e769d.tar.bz2 px4-firmware-b376316fb19e497f76512a0353183936396e769d.zip |
Merge remote-tracking branch 'upstream/isvtol' into dev_ros
Conflicts:
src/modules/mc_att_control/mc_att_control_main.cpp
src/modules/uORB/topics/vehicle_status.h
Diffstat (limited to 'src/modules')
-rw-r--r-- | src/modules/commander/commander.cpp | 17 | ||||
-rw-r--r-- | src/modules/fw_att_control/fw_att_control_main.cpp | 37 | ||||
-rw-r--r-- | src/modules/mc_att_control/mc_att_control.cpp | 17 | ||||
-rw-r--r-- | src/modules/mc_att_control/mc_att_control.h | 2 |
4 files changed, 28 insertions, 45 deletions
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 9262f9e81..dca5c1a8e 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1098,6 +1098,10 @@ 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)); @@ -1119,6 +1123,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); @@ -1127,6 +1133,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); } @@ -2230,14 +2238,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 0025aae7f..a0f74517c 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -195,8 +195,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 { @@ -237,7 +235,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 */ @@ -339,6 +336,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")), @@ -397,19 +397,8 @@ 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(); - // 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() @@ -484,8 +473,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); @@ -601,6 +588,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); + } } } @@ -701,11 +696,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 diff --git a/src/modules/mc_att_control/mc_att_control.cpp b/src/modules/mc_att_control/mc_att_control.cpp index 90a6c90fe..5dfe18cf2 100644 --- a/src/modules/mc_att_control/mc_att_control.cpp +++ b/src/modules/mc_att_control/mc_att_control.cpp @@ -97,17 +97,6 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _params_handles.acro_roll_max = PX4_PARAM_INIT(MC_ACRO_R_MAX); _params_handles.acro_pitch_max = PX4_PARAM_INIT(MC_ACRO_P_MAX); _params_handles.acro_yaw_max = PX4_PARAM_INIT(MC_ACRO_Y_MAX); - _params_handles.autostart_id = PX4_PARAM_INIT(SYS_AUTOSTART); - - /* 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?*/ - _is_vtol = true; - } - else { - _is_vtol = false; - } /* * do subscriptions @@ -235,7 +224,7 @@ void MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehi if (_v_rates_sp_pub != nullptr) { _v_rates_sp_pub->publish(_v_rates_sp_mod); } else { - if (_is_vtol) { + if (_v_status->get()._is_vtol) { _v_rates_sp_pub = PX4_ADVERTISE(_n, mc_virtual_rates_setpoint); } else { _v_rates_sp_pub = PX4_ADVERTISE(_n, vehicle_rates_setpoint); @@ -264,7 +253,7 @@ void MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehi _v_rates_sp_pub->publish(_v_rates_sp_mod); } else { - if (_is_vtol) { + if (_v_status->get()._is_vtol) { _v_rates_sp_pub = PX4_ADVERTISE(_n, mc_virtual_rates_setpoint); } else { _v_rates_sp_pub = PX4_ADVERTISE(_n, vehicle_rates_setpoint); @@ -295,7 +284,7 @@ void MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehi _actuators_0_pub->publish(_actuators); } else { - if (_is_vtol) { + if (_v_status()->get()._is_vtol) { _actuators_0_pub = PX4_ADVERTISE(_n, actuator_controls_virtual_mc); } else { _actuators_0_pub = PX4_ADVERTISE(_n, actuator_controls_0); diff --git a/src/modules/mc_att_control/mc_att_control.h b/src/modules/mc_att_control/mc_att_control.h index 33552c269..bff5289fd 100644 --- a/src/modules/mc_att_control/mc_att_control.h +++ b/src/modules/mc_att_control/mc_att_control.h @@ -93,8 +93,6 @@ private: px4::Publisher * _v_rates_sp_pub; /**< rate setpoint publication */ px4::Publisher * _actuators_0_pub; /**< attitude actuator controls publication */ - bool _is_vtol; /**< true if vehicle is vtol, to be replaced with global API */ - px4::NodeHandle _n; struct { |