From ff55652f9a9cdb4cdad216f4c6166681d10f278c Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Tue, 2 Dec 2014 10:34:39 +0100 Subject: adapted attitude controllers to support VTOL --- src/modules/mc_att_control/mc_att_control_main.cpp | 89 +++++++++++++++++----- 1 file changed, 72 insertions(+), 17 deletions(-) (limited to 'src/modules/mc_att_control') 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..c5dccd8c3 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 #include #include +#include #include #include #include @@ -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,10 +120,13 @@ 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 _v_rates_sp_virtual_pub; /* virtual att rates sp publication */ orb_advert_t _actuators_0_pub; /**< attitude actuator controls publication */ + orb_advert_t _actuators_virtual_mc_pub; /**attitude actuator controls publication if vehicle is VTOL*/ bool _actuators_0_circuit_breaker_enabled; /**< circuit breaker to suppress output */ @@ -133,6 +137,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 +173,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 +189,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; /** @@ -229,6 +238,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. */ @@ -264,11 +278,14 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _params_sub(-1), _manual_control_sp_sub(-1), _armed_sub(-1), + _vehicle_status_sub(-1), /* publications */ _att_sp_pub(-1), _v_rates_sp_pub(-1), + _v_rates_sp_virtual_pub(-1), _actuators_0_pub(-1), + _actuators_virtual_mc_pub(-1), _actuators_0_circuit_breaker_enabled(false), @@ -283,6 +300,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 +314,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,6 +345,8 @@ 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(); } @@ -409,6 +432,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 +442,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 +457,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 +514,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 +622,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 +719,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(); } @@ -721,6 +758,8 @@ MulticopterAttitudeControl::task_main_trampoline(int argc, char *argv[]) void MulticopterAttitudeControl::task_main() { + warnx("started"); + fflush(stdout); /* * do subscriptions @@ -732,10 +771,24 @@ 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(); + /*Subscribe to correct actuator control topic, depending on what airframe we are using + * If airframe is of type VTOL then we want to publish the actuator controls on the virtual multicopter + * topic, from which the VTOL_att_control module is receiving data and processing it further)*/ + if (_params.autostart_id >= 13000 && _params.autostart_id <= 13999) { /* VTOL airframe?*/ + _actuators_virtual_mc_pub = orb_advertise(ORB_ID(actuator_controls_virtual_mc), &_actuators); + _v_rates_sp_virtual_pub = orb_advertise(ORB_ID(mc_virtual_rates_setpoint), &_v_rates_sp); + + } else { /*airframe is not of type VTOL, use standard topic for controls publication*/ + _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators); + _v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_v_rates_sp); + } + + /* wakeup source: vehicle attitude */ struct pollfd fds[1]; @@ -783,6 +836,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); @@ -797,8 +851,8 @@ MulticopterAttitudeControl::task_main() if (_v_rates_sp_pub > 0) { orb_publish(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_pub, &_v_rates_sp); - } else { - _v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_v_rates_sp); + } else if (_v_rates_sp_virtual_pub > 0) { + _v_rates_sp_pub = orb_publish(ORB_ID(mc_virtual_rates_setpoint), _v_rates_sp_virtual_pub, &_v_rates_sp); } } else { @@ -819,11 +873,11 @@ 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(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_pub, &_v_rates_sp); - } else { - _v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_v_rates_sp); - } + } else if (_v_rates_sp_virtual_pub > 0) { + _v_rates_sp_pub = orb_publish(ORB_ID(mc_virtual_rates_setpoint), _v_rates_sp_virtual_pub, &_v_rates_sp); + } } else { /* attitude controller disabled, poll rates setpoint topic */ @@ -846,12 +900,13 @@ MulticopterAttitudeControl::task_main() _actuators.timestamp = hrt_absolute_time(); if (!_actuators_0_circuit_breaker_enabled) { - if (_actuators_0_pub > 0) { + if (_actuators_0_pub > 0) { //normal mutlicopter airframe orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators); - } else { - _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators); + } else if (_actuators_0_pub < 0 && _vehicle_status.is_rotary_wing) { + orb_publish(ORB_ID(actuator_controls_virtual_mc), _actuators_virtual_mc_pub, &_actuators); } + } } } -- cgit v1.2.3 From 0e33e52d4ae2df06467f5b862bb0734c3ba23015 Mon Sep 17 00:00:00 2001 From: tumbili Date: Wed, 3 Dec 2014 09:44:34 +0100 Subject: use uORB ID to determine the correct rate_sp- and actuator topic to publish on --- src/modules/fw_att_control/fw_att_control_main.cpp | 48 +++++++++----------- src/modules/mc_att_control/mc_att_control_main.cpp | 51 ++++++++++------------ 2 files changed, 44 insertions(+), 55 deletions(-) (limited to 'src/modules/mc_att_control') 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 daf787863..679ef1d29 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -130,12 +130,13 @@ private: int _vehicle_status_sub; /**< vehicle status subscription */ orb_advert_t _rate_sp_pub; /**< rate setpoint publication */ - orb_advert_t _rate_sp_virtual_pub; /* virtual att rates setpoint topic (vtol) */ orb_advert_t _attitude_sp_pub; /**< attitude setpoint point */ orb_advert_t _actuators_0_pub; /**< actuator control group 0 setpoint */ - orb_advert_t _actuators_virtual_fw_pub; /**publisher for VTOL vehicle*/ orb_advert_t _actuators_2_pub; /**< actuator control group 1 setpoint (Airframe) */ + 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 + struct vehicle_attitude_s _att; /**< vehicle attitude */ struct accel_report _accel; /**< body frame accelerations */ struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ @@ -334,10 +335,8 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : /* publications */ _rate_sp_pub(-1), - _rate_sp_virtual_pub(-1), _attitude_sp_pub(-1), _actuators_0_pub(-1), - _actuators_virtual_fw_pub(-1), _actuators_2_pub(-1), /* performance counters */ @@ -402,6 +401,15 @@ 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() @@ -629,19 +637,7 @@ FixedwingAttitudeControl::task_main() orb_set_interval(_att_sub, 17); parameters_update(); - - /*Publish to correct actuator control topic, depending on what airframe we are using - * If airframe is of type VTOL then we want to publish the actuator controls on the virtual fixed wing - * topic, from which the vtol_att_control module is receiving data and processing it further)*/ - if (_parameters.autostart_id >= 13000 && _parameters.autostart_id <= 13999) { /* VTOL airframe?*/ - _actuators_virtual_fw_pub = orb_advertise(ORB_ID(actuator_controls_virtual_fw), &_actuators); - _rate_sp_virtual_pub = orb_advertise(ORB_ID(fw_virtual_rates_setpoint), &_rates_sp); - - } else { /*airframe is not of type VTOL, use standard topic for controls publication*/ - _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators); - _rate_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_rates_sp); - } - + /* get an initial update for all sensor and status data */ vehicle_airspeed_poll(); vehicle_setpoint_poll(); @@ -1008,11 +1004,10 @@ FixedwingAttitudeControl::task_main() if (_rate_sp_pub > 0) { /* publish the attitude rates setpoint */ - orb_publish(ORB_ID(vehicle_rates_setpoint), _rate_sp_pub, &_rates_sp); - - } else if (_rate_sp_virtual_pub > 0) { - /* publish the virtual attitude setpoint */ - orb_publish(ORB_ID(fw_virtual_rates_setpoint), _rate_sp_virtual_pub, &_rates_sp); + orb_publish(_rates_sp_id, _rate_sp_pub, &_rates_sp); + } else { + /* advertise the attitude rates setpoint */ + _rate_sp_pub = orb_advertise(_rates_sp_id, &_rates_sp); } } else { @@ -1033,11 +1028,10 @@ FixedwingAttitudeControl::task_main() _actuators_airframe.timestamp = hrt_absolute_time(); /* publish the actuator controls */ - if (_actuators_0_pub > 0) { //normal fixed wing airframe - orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators); - - } else if (_actuators_0_pub < 0 && !_vehicle_status.is_rotary_wing) { //VTOL airframe - orb_publish(ORB_ID(actuator_controls_virtual_fw), _actuators_virtual_fw_pub, &_actuators); + if (_actuators_0_pub > 0) { + orb_publish(_actuators_id, _actuators_0_pub, &_actuators); + } else { + _actuators_0_pub= orb_advertise(_actuators_id, &_actuators); } if (_actuators_2_pub > 0) { 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 c5dccd8c3..2431f07f4 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -124,9 +124,10 @@ private: orb_advert_t _att_sp_pub; /**< attitude setpoint publication */ orb_advert_t _v_rates_sp_pub; /**< rate setpoint publication */ - orb_advert_t _v_rates_sp_virtual_pub; /* virtual att rates sp publication */ orb_advert_t _actuators_0_pub; /**< attitude actuator controls publication */ - orb_advert_t _actuators_virtual_mc_pub; /**attitude actuator controls publication if vehicle is VTOL*/ + + 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 */ @@ -283,9 +284,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : /* publications */ _att_sp_pub(-1), _v_rates_sp_pub(-1), - _v_rates_sp_virtual_pub(-1), _actuators_0_pub(-1), - _actuators_virtual_mc_pub(-1), _actuators_0_circuit_breaker_enabled(false), @@ -349,6 +348,15 @@ 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() @@ -776,19 +784,6 @@ MulticopterAttitudeControl::task_main() /* initialize parameters cache */ parameters_update(); - /*Subscribe to correct actuator control topic, depending on what airframe we are using - * If airframe is of type VTOL then we want to publish the actuator controls on the virtual multicopter - * topic, from which the VTOL_att_control module is receiving data and processing it further)*/ - if (_params.autostart_id >= 13000 && _params.autostart_id <= 13999) { /* VTOL airframe?*/ - _actuators_virtual_mc_pub = orb_advertise(ORB_ID(actuator_controls_virtual_mc), &_actuators); - _v_rates_sp_virtual_pub = orb_advertise(ORB_ID(mc_virtual_rates_setpoint), &_v_rates_sp); - - } else { /*airframe is not of type VTOL, use standard topic for controls publication*/ - _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators); - _v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_v_rates_sp); - } - - /* wakeup source: vehicle attitude */ struct pollfd fds[1]; @@ -849,10 +844,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 if (_v_rates_sp_virtual_pub > 0) { - _v_rates_sp_pub = orb_publish(ORB_ID(mc_virtual_rates_setpoint), _v_rates_sp_virtual_pub, &_v_rates_sp); + } else { + _v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp); } } else { @@ -873,11 +868,11 @@ 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 if (_v_rates_sp_virtual_pub > 0) { - _v_rates_sp_pub = orb_publish(ORB_ID(mc_virtual_rates_setpoint), _v_rates_sp_virtual_pub, &_v_rates_sp); - } + } else { + _v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp); + } } else { /* attitude controller disabled, poll rates setpoint topic */ @@ -900,11 +895,11 @@ MulticopterAttitudeControl::task_main() _actuators.timestamp = hrt_absolute_time(); if (!_actuators_0_circuit_breaker_enabled) { - if (_actuators_0_pub > 0) { //normal mutlicopter airframe - orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators); + if (_actuators_0_pub > 0) { + orb_publish(_actuators_id, _actuators_0_pub, &_actuators); - } else if (_actuators_0_pub < 0 && _vehicle_status.is_rotary_wing) { - orb_publish(ORB_ID(actuator_controls_virtual_mc), _actuators_virtual_mc_pub, &_actuators); + } else { + _actuators_0_pub = orb_advertise(_actuators_id, &_actuators); } } -- cgit v1.2.3 From fbedd3ed206531704f24a0b76657c4583a49efe8 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Thu, 4 Dec 2014 14:23:19 +0100 Subject: removed whitespaces removed small unused code block --- src/modules/commander/commander.cpp | 2 +- src/modules/fw_att_control/fw_att_control_main.cpp | 3 +-- src/modules/mc_att_control/mc_att_control_main.cpp | 2 -- 3 files changed, 2 insertions(+), 5 deletions(-) (limited to 'src/modules/mc_att_control') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index bed467698..4c7664cd0 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -2240,7 +2240,7 @@ set_control_mode() control_mode.flag_control_velocity_enabled = false; control_mode.flag_control_termination_enabled = false; break; - + case NAVIGATION_STATE_ALTCTL: control_mode.flag_control_manual_enabled = true; control_mode.flag_control_auto_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 679ef1d29..956fc94de 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -77,7 +77,6 @@ #include #include - /** * Fixedwing attitude control app start / stop handling function * @@ -637,7 +636,7 @@ FixedwingAttitudeControl::task_main() orb_set_interval(_att_sub, 17); parameters_update(); - + /* get an initial update for all sensor and status data */ vehicle_airspeed_poll(); vehicle_setpoint_poll(); 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 2431f07f4..0702e6378 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -766,8 +766,6 @@ MulticopterAttitudeControl::task_main_trampoline(int argc, char *argv[]) void MulticopterAttitudeControl::task_main() { - warnx("started"); - fflush(stdout); /* * do subscriptions -- cgit v1.2.3