diff options
Diffstat (limited to 'src/modules/mc_att_control/mc_att_control_main.cpp')
-rw-r--r-- | src/modules/mc_att_control/mc_att_control_main.cpp | 76 |
1 files changed, 62 insertions, 14 deletions
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 81a13edb8..0702e6378 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -66,6 +66,7 @@ #include <uORB/topics/vehicle_rates_setpoint.h> #include <uORB/topics/vehicle_attitude.h> #include <uORB/topics/vehicle_control_mode.h> +#include <uORB/topics/vehicle_status.h> #include <uORB/topics/actuator_armed.h> #include <uORB/topics/parameter_update.h> #include <systemlib/param/param.h> @@ -96,12 +97,12 @@ public: MulticopterAttitudeControl(); /** - * Destructor, also kills the sensors task. + * Destructor, also kills the main task */ ~MulticopterAttitudeControl(); /** - * Start the sensors task. + * Start the multicopter attitude control task. * * @return OK on success. */ @@ -109,8 +110,8 @@ public: private: - bool _task_should_exit; /**< if true, sensor task should exit */ - int _control_task; /**< task handle for sensor task */ + bool _task_should_exit; /**< if true, task_main() should exit */ + int _control_task; /**< task handle */ int _v_att_sub; /**< vehicle attitude subscription */ int _v_att_sp_sub; /**< vehicle attitude setpoint subscription */ @@ -119,11 +120,15 @@ private: int _params_sub; /**< parameter updates subscription */ int _manual_control_sp_sub; /**< manual control setpoint subscription */ int _armed_sub; /**< arming status subscription */ + int _vehicle_status_sub; /**< vehicle status subscription */ orb_advert_t _att_sp_pub; /**< attitude setpoint publication */ orb_advert_t _v_rates_sp_pub; /**< rate setpoint publication */ orb_advert_t _actuators_0_pub; /**< attitude actuator controls publication */ + orb_id_t _rates_sp_id; // pointer to correct rates setpoint uORB metadata structure + orb_id_t _actuators_id; // pointer to correct actuator controls0 uORB metadata structure + bool _actuators_0_circuit_breaker_enabled; /**< circuit breaker to suppress output */ struct vehicle_attitude_s _v_att; /**< vehicle attitude */ @@ -133,6 +138,7 @@ private: struct vehicle_control_mode_s _v_control_mode; /**< vehicle control mode */ struct actuator_controls_s _actuators; /**< actuator controls */ struct actuator_armed_s _armed; /**< actuator arming status */ + struct vehicle_status_s _vehicle_status; /**< vehicle status */ perf_counter_t _loop_perf; /**< loop performance counter */ @@ -168,6 +174,8 @@ private: param_t acro_roll_max; 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 { @@ -182,6 +190,8 @@ private: float man_pitch_max; float man_yaw_max; math::Vector<3> acro_rate_max; /**< max attitude rates in acro mode */ + + param_t autostart_id; } _params; /** @@ -230,6 +240,11 @@ private: void control_attitude_rates(float dt); /** + * Check for vehicle status updates. + */ + void vehicle_status_poll(); + + /** * Shim for calling task_main from task_create. */ static void task_main_trampoline(int argc, char *argv[]); @@ -264,6 +279,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _params_sub(-1), _manual_control_sp_sub(-1), _armed_sub(-1), + _vehicle_status_sub(-1), /* publications */ _att_sp_pub(-1), @@ -283,6 +299,8 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : memset(&_v_control_mode, 0, sizeof(_v_control_mode)); memset(&_actuators, 0, sizeof(_actuators)); memset(&_armed, 0, sizeof(_armed)); + memset(&_vehicle_status, 0, sizeof(_vehicle_status)); + _vehicle_status.is_rotary_wing = true; _params.att_p.zero(); _params.rate_p.zero(); @@ -295,6 +313,8 @@ 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(); @@ -324,8 +344,19 @@ 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(); + // 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() @@ -409,6 +440,8 @@ 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; } @@ -417,7 +450,7 @@ MulticopterAttitudeControl::parameter_update_poll() { bool updated; - /* Check HIL state if vehicle status has changed */ + /* Check if parameters have changed */ orb_check(_params_sub, &updated); if (updated) { @@ -432,7 +465,7 @@ MulticopterAttitudeControl::vehicle_control_mode_poll() { bool updated; - /* Check HIL state if vehicle status has changed */ + /* Check if vehicle control mode has changed */ orb_check(_v_control_mode_sub, &updated); if (updated) { @@ -489,6 +522,18 @@ MulticopterAttitudeControl::arming_status_poll() } } +void +MulticopterAttitudeControl::vehicle_status_poll() +{ + /* check if there is new status information */ + bool vehicle_status_updated; + orb_check(_vehicle_status_sub, &vehicle_status_updated); + + if (vehicle_status_updated) { + orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status); + } +} + /* * Attitude controller. * Input: 'manual_control_setpoint' and 'vehicle_attitude_setpoint' topics (depending on mode) @@ -585,7 +630,7 @@ MulticopterAttitudeControl::control_attitude(float dt) } /* publish the attitude setpoint if needed */ - if (publish_att_sp) { + if (publish_att_sp && _vehicle_status.is_rotary_wing) { _v_att_sp.timestamp = hrt_absolute_time(); if (_att_sp_pub > 0) { @@ -682,7 +727,7 @@ void MulticopterAttitudeControl::control_attitude_rates(float dt) { /* reset integral if disarmed */ - if (!_armed.armed) { + if (!_armed.armed || !_vehicle_status.is_rotary_wing) { _rates_int.zero(); } @@ -732,6 +777,7 @@ MulticopterAttitudeControl::task_main() _params_sub = orb_subscribe(ORB_ID(parameter_update)); _manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); + _vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); /* initialize parameters cache */ parameters_update(); @@ -783,6 +829,7 @@ MulticopterAttitudeControl::task_main() vehicle_control_mode_poll(); arming_status_poll(); vehicle_manual_poll(); + vehicle_status_poll(); if (_v_control_mode.flag_control_attitude_enabled) { control_attitude(dt); @@ -795,10 +842,10 @@ MulticopterAttitudeControl::task_main() _v_rates_sp.timestamp = hrt_absolute_time(); if (_v_rates_sp_pub > 0) { - orb_publish(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_pub, &_v_rates_sp); + orb_publish(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp); } else { - _v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_v_rates_sp); + _v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp); } } else { @@ -819,10 +866,10 @@ MulticopterAttitudeControl::task_main() _v_rates_sp.timestamp = hrt_absolute_time(); if (_v_rates_sp_pub > 0) { - orb_publish(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_pub, &_v_rates_sp); + orb_publish(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp); } else { - _v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_v_rates_sp); + _v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp); } } else { @@ -847,11 +894,12 @@ MulticopterAttitudeControl::task_main() if (!_actuators_0_circuit_breaker_enabled) { if (_actuators_0_pub > 0) { - orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators); + orb_publish(_actuators_id, _actuators_0_pub, &_actuators); } else { - _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators); + _actuators_0_pub = orb_advertise(_actuators_id, &_actuators); } + } } } |