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/fw_att_control/fw_att_control_main.cpp | 160 +++++++++++++++------ 1 file changed, 119 insertions(+), 41 deletions(-) (limited to 'src/modules/fw_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 e770c11a2..daf787863 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -35,8 +35,9 @@ * @file fw_att_control_main.c * Implementation of a generic attitude controller based on classic orthogonal PIDs. * - * @author Lorenz Meier - * @author Thomas Gubler + * @author Lorenz Meier + * @author Thomas Gubler + * @author Roman Bapst * */ @@ -76,6 +77,7 @@ #include #include + /** * Fixedwing attitude control app start / stop handling function * @@ -92,12 +94,12 @@ public: FixedwingAttitudeControl(); /** - * Destructor, also kills the sensors task. + * Destructor, also kills the main task. */ ~FixedwingAttitudeControl(); /** - * Start the sensors task. + * Start the main task. * * @return OK on success. */ @@ -112,9 +114,9 @@ public: private: - bool _task_should_exit; /**< if true, sensor task should exit */ + bool _task_should_exit; /**< if true, attitude control task should exit */ bool _task_running; /**< if true, task is running in its mainloop */ - int _control_task; /**< task handle for sensor task */ + int _control_task; /**< task handle */ int _att_sub; /**< vehicle attitude subscription */ int _accel_sub; /**< accelerometer subscription */ @@ -128,13 +130,16 @@ 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_1_pub; /**< actuator control group 1 setpoint (Airframe) */ + orb_advert_t _actuators_virtual_fw_pub; /**publisher for VTOL vehicle*/ + orb_advert_t _actuators_2_pub; /**< actuator control group 1 setpoint (Airframe) */ struct vehicle_attitude_s _att; /**< vehicle attitude */ struct accel_report _accel; /**< body frame accelerations */ struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ + struct vehicle_rates_setpoint_s _rates_sp; /* attitude rates setpoint */ struct manual_control_setpoint_s _manual; /**< r/c channel data */ struct airspeed_s _airspeed; /**< airspeed */ struct vehicle_control_mode_s _vcontrol_mode; /**< vehicle control mode */ @@ -189,6 +194,8 @@ 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 { @@ -228,6 +235,8 @@ private: param_t pitchsp_offset_deg; 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 */ @@ -289,7 +298,7 @@ private: static void task_main_trampoline(int argc, char *argv[]); /** - * Main sensor collection task. + * Main attitude controller collection task. */ void task_main(); @@ -325,9 +334,11 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : /* publications */ _rate_sp_pub(-1), + _rate_sp_virtual_pub(-1), _attitude_sp_pub(-1), _actuators_0_pub(-1), - _actuators_1_pub(-1), + _actuators_virtual_fw_pub(-1), + _actuators_2_pub(-1), /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "fw att control")), @@ -341,6 +352,7 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _att = {}; _accel = {}; _att_sp = {}; + _rates_sp = {}; _manual = {}; _airspeed = {}; _vcontrol_mode = {}; @@ -386,6 +398,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(); } @@ -462,6 +476,7 @@ 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); @@ -497,7 +512,7 @@ FixedwingAttitudeControl::vehicle_control_mode_poll() { bool vcontrol_mode_updated; - /* Check HIL state if vehicle status has changed */ + /* Check if vehicle control mode has changed */ orb_check(_vcontrol_mode_sub, &vcontrol_mode_updated); if (vcontrol_mode_updated) { @@ -529,7 +544,6 @@ FixedwingAttitudeControl::vehicle_airspeed_poll() if (airspeed_updated) { orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed); -// warnx("airspeed poll: ind: %.4f, true: %.4f", _airspeed.indicated_airspeed_m_s, _airspeed.true_airspeed_m_s); } } @@ -616,6 +630,18 @@ FixedwingAttitudeControl::task_main() 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(); @@ -679,6 +705,65 @@ 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 !!! + + * 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 + * the pitch axis compared to the neutral position of the vehicle in multicopter mode + * we need to swap the roll and the yaw axis (1st and 3rd column) in the rotation matrix. + * Additionally, in order to get the correct sign of the pitch, we need to multiply + * the new x axis of the rotation matrix with -1 + * + * original: modified: + * + * Rxx Ryx Rzx -Rzx Ryx Rxx + * Rxy Ryy Rzy -Rzy Ryy Rxy + * Rxz Ryz Rzz -Rzz Ryz Rxz + * */ + math::Matrix<3, 3> R; //original rotation matrix + math::Matrix<3, 3> R_adapted; //modified rotation matrix + R.set(_att.R); + R_adapted.set(_att.R); + + //move z to x + R_adapted(0, 0) = R(0, 2); + R_adapted(1, 0) = R(1, 2); + R_adapted(2, 0) = R(2, 2); + //move x to z + R_adapted(0, 2) = R(0, 0); + R_adapted(1, 2) = R(1, 0); + R_adapted(2, 2) = R(2, 0); + + //change direction of pitch (convert to right handed system) + R_adapted(0, 0) = -R_adapted(0, 0); + R_adapted(1, 0) = -R_adapted(1, 0); + R_adapted(2, 0) = -R_adapted(2, 0); + math::Vector<3> euler_angles; //adapted euler angles for fixed wing operation + euler_angles = R_adapted.to_euler(); + //fill in new attitude data + _att.roll = euler_angles(0); + _att.pitch = euler_angles(1); + _att.yaw = euler_angles(2); + _att.R[0][0] = R_adapted(0, 0); + _att.R[0][1] = R_adapted(0, 1); + _att.R[0][2] = R_adapted(0, 2); + _att.R[1][0] = R_adapted(1, 0); + _att.R[1][1] = R_adapted(1, 1); + _att.R[1][2] = R_adapted(1, 2); + _att.R[2][0] = R_adapted(2, 0); + _att.R[2][1] = R_adapted(2, 1); + _att.R[2][2] = R_adapted(2, 2); + + // lastly, roll- and yawspeed have to be swaped + float helper = _att.rollspeed; + _att.rollspeed = -_att.yawspeed; + _att.yawspeed = helper; + } + vehicle_airspeed_poll(); vehicle_setpoint_poll(); @@ -696,7 +781,7 @@ FixedwingAttitudeControl::task_main() /* lock integrator until control is started */ bool lock_integrator; - if (_vcontrol_mode.flag_control_attitude_enabled) { + if (_vcontrol_mode.flag_control_attitude_enabled && !_vehicle_status.is_rotary_wing) { lock_integrator = false; } else { @@ -705,10 +790,10 @@ FixedwingAttitudeControl::task_main() /* Simple handling of failsafe: deploy parachute if failsafe is on */ if (_vcontrol_mode.flag_control_termination_enabled) { - _actuators_airframe.control[1] = 1.0f; + _actuators_airframe.control[7] = 1.0f; // warnx("_actuators_airframe.control[1] = 1.0f;"); } else { - _actuators_airframe.control[1] = 0.0f; + _actuators_airframe.control[7] = 0.0f; // warnx("_actuators_airframe.control[1] = -1.0f;"); } @@ -802,18 +887,18 @@ FixedwingAttitudeControl::task_main() att_sp.thrust = throttle_sp; /* lazily publish the setpoint only once available */ - if (_attitude_sp_pub > 0) { + if (_attitude_sp_pub > 0 && !_vehicle_status.is_rotary_wing) { /* publish the attitude setpoint */ orb_publish(ORB_ID(vehicle_attitude_setpoint), _attitude_sp_pub, &att_sp); - } else { + } else if (_attitude_sp_pub < 0 && !_vehicle_status.is_rotary_wing) { /* advertise and publish */ _attitude_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); } } /* If the aircraft is on ground reset the integrators */ - if (_vehicle_status.condition_landed) { + if (_vehicle_status.condition_landed || _vehicle_status.is_rotary_wing) { _roll_ctrl.reset_integrator(); _pitch_ctrl.reset_integrator(); _yaw_ctrl.reset_integrator(); @@ -915,20 +1000,19 @@ FixedwingAttitudeControl::task_main() * Lazily publish the rate setpoint (for analysis, the actuators are published below) * only once available */ - vehicle_rates_setpoint_s rates_sp; - rates_sp.roll = _roll_ctrl.get_desired_rate(); - rates_sp.pitch = _pitch_ctrl.get_desired_rate(); - rates_sp.yaw = _yaw_ctrl.get_desired_rate(); + _rates_sp.roll = _roll_ctrl.get_desired_rate(); + _rates_sp.pitch = _pitch_ctrl.get_desired_rate(); + _rates_sp.yaw = _yaw_ctrl.get_desired_rate(); - rates_sp.timestamp = hrt_absolute_time(); + _rates_sp.timestamp = hrt_absolute_time(); if (_rate_sp_pub > 0) { - /* publish the attitude setpoint */ - orb_publish(ORB_ID(vehicle_rates_setpoint), _rate_sp_pub, &rates_sp); + /* publish the attitude rates setpoint */ + orb_publish(ORB_ID(vehicle_rates_setpoint), _rate_sp_pub, &_rates_sp); - } else { - /* advertise and publish */ - _rate_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &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); } } else { @@ -948,28 +1032,22 @@ FixedwingAttitudeControl::task_main() _actuators.timestamp = hrt_absolute_time(); _actuators_airframe.timestamp = hrt_absolute_time(); - if (_actuators_0_pub > 0) { - /* publish the attitude setpoint */ + /* 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 { - /* advertise and publish */ - _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_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_1_pub > 0) { - /* publish the attitude setpoint */ - orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_airframe); -// warnx("%.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f", -// (double)_actuators_airframe.control[0], (double)_actuators_airframe.control[1], (double)_actuators_airframe.control[2], -// (double)_actuators_airframe.control[3], (double)_actuators_airframe.control[4], (double)_actuators_airframe.control[5], -// (double)_actuators_airframe.control[6], (double)_actuators_airframe.control[7]); + if (_actuators_2_pub > 0) { + /* publish the actuator controls*/ + orb_publish(ORB_ID(actuator_controls_2), _actuators_2_pub, &_actuators_airframe); } else { /* advertise and publish */ - _actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_airframe); + _actuators_2_pub = orb_advertise(ORB_ID(actuator_controls_2), &_actuators_airframe); } - } loop_counter++; -- 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/fw_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/fw_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