/**************************************************************************** * * Copyright (c) 2013-2018 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * 1. Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * 2. Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in * the documentation and/or other materials provided with the * distribution. * 3. Neither the name PX4 nor the names of its contributors may be * used to endorse or promote products derived from this software * without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * ****************************************************************************/ /** * @file mc_att_control_main.cpp * Multicopter attitude controller. * * Publication documenting this type of Quaternion Attitude Control: * Nonlinear Quadrocopter Attitude Control (2013) * by Dario Brescianini, Markus Hehn and Raffaello D'Andrea * Institute for Dynamic Systems and Control (IDSC), ETH Zurich * * @author Lorenz Meier * @author Anton Babushkin * @author Sander Smeets * @author Matthias Grob * * The controller has two loops: P loop for angular error and PD loop for angular rate error. * Desired rotation calculated keeping in mind that yaw response is normally slower than roll/pitch. * For small deviations controller rotates copter to have shortest path of thrust vector and independently rotates around yaw, * so actual rotation axis is not constant. For large deviations controller rotates copter around fixed axis. * These two approaches fused seamlessly with weight depending on angular error. * When thrust vector directed near-horizontally (e.g. roll ~= PI/2) yaw setpoint ignored because of singularity. * Controller doesn't use Euler angles for work, they generated only for more human-friendly control and logging. * If rotation matrix setpoint is invalid it will be generated from Euler angles for compatibility with old position controllers. */ #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include /** * Multicopter attitude control app start / stop handling function * * @ingroup apps */ extern "C" __EXPORT int mc_att_control_main(int argc, char *argv[]); #define MIN_TAKEOFF_THRUST 0.2f #define TPA_RATE_LOWER_LIMIT 0.05f #define AXIS_INDEX_ROLL 0 #define AXIS_INDEX_PITCH 1 #define AXIS_INDEX_YAW 2 #define AXIS_COUNT 3 #define MAX_GYRO_COUNT 3 using namespace matrix; class MulticopterAttitudeControl { public: /** * Constructor */ MulticopterAttitudeControl(); /** * Destructor, also kills the main task */ ~MulticopterAttitudeControl(); /** * Start the multicopter attitude control task. * * @return OK on success. */ int start(); private: 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 */ int _v_rates_sp_sub; /**< vehicle rates setpoint subscription */ int _v_control_mode_sub; /**< vehicle control mode subscription */ int _params_sub; /**< parameter updates subscription */ int _manual_control_sp_sub; /**< manual control setpoint subscription */ int _vehicle_status_sub; /**< vehicle status subscription */ int _motor_limits_sub; /**< motor limits subscription */ int _battery_status_sub; /**< battery status subscription */ int _sensor_gyro_sub[MAX_GYRO_COUNT]; /**< gyro data subscription */ int _sensor_correction_sub; /**< sensor thermal correction subscription */ int _sensor_bias_sub; /**< sensor in-run bias correction subscription */ unsigned _gyro_count; int _selected_gyro; orb_advert_t _v_rates_sp_pub; /**< rate setpoint publication */ orb_advert_t _actuators_0_pub; /**< attitude actuator controls publication */ orb_advert_t _controller_status_pub; /**< controller status 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 */ struct vehicle_attitude_setpoint_s _v_att_sp; /**< vehicle attitude setpoint */ struct vehicle_rates_setpoint_s _v_rates_sp; /**< vehicle rates setpoint */ struct manual_control_setpoint_s _manual_control_sp; /**< manual control setpoint */ struct vehicle_control_mode_s _v_control_mode; /**< vehicle control mode */ struct actuator_controls_s _actuators; /**< actuator controls */ struct vehicle_status_s _vehicle_status; /**< vehicle status */ struct battery_status_s _battery_status; /**< battery status */ struct sensor_gyro_s _sensor_gyro; /**< gyro data before thermal correctons and ekf bias estimates are applied */ struct sensor_correction_s _sensor_correction; /**< sensor thermal corrections */ struct sensor_bias_s _sensor_bias; /**< sensor in-run bias corrections */ MultirotorMixer::saturation_status _saturation_status{}; perf_counter_t _loop_perf; /**< loop performance counter */ perf_counter_t _controller_latency_perf; math::LowPassFilter2p _lp_filters_d[3]; /**< low-pass filters for D-term (roll, pitch & yaw) */ static constexpr const float initial_update_rate_hz = 250.f; /**< loop update rate used for initialization */ float _loop_update_rate_hz; /**< current rate-controller loop update rate in [Hz] */ math::Vector<3> _rates_prev; /**< angular rates on previous step */ math::Vector<3> _rates_prev_filtered; /**< angular rates on previous step (low-pass filtered) */ math::Vector<3> _rates_sp; /**< angular rates setpoint */ math::Vector<3> _rates_int; /**< angular rates integral error */ float _thrust_sp; /**< thrust setpoint */ math::Vector<3> _att_control; /**< attitude control vector */ math::Matrix<3, 3> _board_rotation = {}; /**< rotation matrix for the orientation that the board is mounted */ struct { param_t roll_p; param_t roll_rate_p; param_t roll_rate_i; param_t roll_rate_integ_lim; param_t roll_rate_d; param_t roll_rate_ff; param_t pitch_p; param_t pitch_rate_p; param_t pitch_rate_i; param_t pitch_rate_integ_lim; param_t pitch_rate_d; param_t pitch_rate_ff; param_t d_term_cutoff_freq; param_t tpa_breakpoint_p; param_t tpa_breakpoint_i; param_t tpa_breakpoint_d; param_t tpa_rate_p; param_t tpa_rate_i; param_t tpa_rate_d; param_t yaw_p; param_t yaw_rate_p; param_t yaw_rate_i; param_t yaw_rate_integ_lim; param_t yaw_rate_d; param_t yaw_rate_ff; param_t yaw_ff; param_t roll_rate_max; param_t pitch_rate_max; param_t yaw_rate_max; param_t yaw_auto_max; param_t acro_roll_max; param_t acro_pitch_max; param_t acro_yaw_max; param_t acro_expo; param_t acro_superexpo; param_t rattitude_thres; param_t vtol_wv_yaw_rate_scale; param_t bat_scale_en; param_t board_rotation; param_t board_offset[3]; } _params_handles; /**< handles for interesting parameters */ struct { Vector3f attitude_p; /**< P gain for attitude control */ math::Vector<3> rate_p; /**< P gain for angular rate error */ math::Vector<3> rate_i; /**< I gain for angular rate error */ math::Vector<3> rate_int_lim; /**< integrator state limit for rate loop */ math::Vector<3> rate_d; /**< D gain for angular rate error */ math::Vector<3> rate_ff; /**< Feedforward gain for desired rates */ float yaw_ff; /**< yaw control feed-forward */ float d_term_cutoff_freq; /**< Cutoff frequency for the D-term filter */ float tpa_breakpoint_p; /**< Throttle PID Attenuation breakpoint */ float tpa_breakpoint_i; /**< Throttle PID Attenuation breakpoint */ float tpa_breakpoint_d; /**< Throttle PID Attenuation breakpoint */ float tpa_rate_p; /**< Throttle PID Attenuation slope */ float tpa_rate_i; /**< Throttle PID Attenuation slope */ float tpa_rate_d; /**< Throttle PID Attenuation slope */ float roll_rate_max; float pitch_rate_max; float yaw_rate_max; float yaw_auto_max; math::Vector<3> mc_rate_max; /**< attitude rate limits in stabilized modes */ math::Vector<3> auto_rate_max; /**< attitude rate limits in auto modes */ matrix::Vector3f acro_rate_max; /**< max attitude rates in acro mode */ float acro_expo; /**< function parameter for expo stick curve shape */ float acro_superexpo; /**< function parameter for superexpo stick curve shape */ float rattitude_thres; float vtol_wv_yaw_rate_scale; /**< Scale value [0, 1] for yaw rate setpoint */ int32_t bat_scale_en; int32_t board_rotation; float board_offset[3]; } _params; /** * Update our local parameter cache. */ void parameters_update(); /** * Check for parameter update and handle it. */ void battery_status_poll(); void parameter_update_poll(); void sensor_bias_poll(); void sensor_correction_poll(); void vehicle_attitude_poll(); void vehicle_attitude_setpoint_poll(); void vehicle_control_mode_poll(); void vehicle_manual_poll(); void vehicle_motor_limits_poll(); void vehicle_rates_setpoint_poll(); void vehicle_status_poll(); /** * Attitude controller. */ void control_attitude(float dt); /** * Attitude rates controller. */ void control_attitude_rates(float dt); /** * Throttle PID attenuation. */ math::Vector<3> pid_attenuations(float tpa_breakpoint, float tpa_rate); /** * Shim for calling task_main from task_create. */ static void task_main_trampoline(int argc, char *argv[]); /** * Main attitude control task. */ void task_main(); }; namespace mc_att_control { MulticopterAttitudeControl *g_control; } MulticopterAttitudeControl::MulticopterAttitudeControl() : _task_should_exit(false), _control_task(-1), /* subscriptions */ _v_att_sub(-1), _v_att_sp_sub(-1), _v_control_mode_sub(-1), _params_sub(-1), _manual_control_sp_sub(-1), _vehicle_status_sub(-1), _motor_limits_sub(-1), _battery_status_sub(-1), _sensor_correction_sub(-1), _sensor_bias_sub(-1), /* gyro selection */ _gyro_count(1), _selected_gyro(0), /* publications */ _v_rates_sp_pub(nullptr), _actuators_0_pub(nullptr), _controller_status_pub(nullptr), _rates_sp_id(nullptr), _actuators_id(nullptr), _actuators_0_circuit_breaker_enabled(false), _v_att{}, _v_att_sp{}, _v_rates_sp{}, _manual_control_sp{}, _v_control_mode{}, _actuators{}, _vehicle_status{}, _battery_status{}, _sensor_gyro{}, _sensor_correction{}, _sensor_bias{}, _saturation_status{}, /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control")), _controller_latency_perf(perf_alloc_once(PC_ELAPSED, "ctrl_latency")), _lp_filters_d{ {initial_update_rate_hz, 50.f}, {initial_update_rate_hz, 50.f}, {initial_update_rate_hz, 50.f}}, // will be initialized correctly when params are loaded _loop_update_rate_hz(initial_update_rate_hz) { for (uint8_t i = 0; i < MAX_GYRO_COUNT; i++) { _sensor_gyro_sub[i] = -1; } _vehicle_status.is_rotary_wing = true; /* initialize quaternions in messages to be valid */ _v_att.q[0] = 1.f; _v_att_sp.q_d[0] = 1.f; _params.rate_p.zero(); _params.rate_i.zero(); _params.rate_int_lim.zero(); _params.rate_d.zero(); _params.rate_ff.zero(); _params.yaw_ff = 0.0f; _params.roll_rate_max = 0.0f; _params.pitch_rate_max = 0.0f; _params.yaw_rate_max = 0.0f; _params.mc_rate_max.zero(); _params.auto_rate_max.zero(); _params.acro_rate_max.zero(); _params.rattitude_thres = 1.0f; _params.bat_scale_en = 0; _params.board_rotation = 0; _params.board_offset[0] = 0.0f; _params.board_offset[1] = 0.0f; _params.board_offset[2] = 0.0f; _rates_prev.zero(); _rates_prev_filtered.zero(); _rates_sp.zero(); _rates_int.zero(); _thrust_sp = 0.0f; _att_control.zero(); _board_rotation.identity(); _params_handles.roll_p = param_find("MC_ROLL_P"); _params_handles.roll_rate_p = param_find("MC_ROLLRATE_P"); _params_handles.roll_rate_i = param_find("MC_ROLLRATE_I"); _params_handles.roll_rate_integ_lim = param_find("MC_RR_INT_LIM"); _params_handles.roll_rate_d = param_find("MC_ROLLRATE_D"); _params_handles.roll_rate_ff = param_find("MC_ROLLRATE_FF"); _params_handles.pitch_p = param_find("MC_PITCH_P"); _params_handles.pitch_rate_p = param_find("MC_PITCHRATE_P"); _params_handles.pitch_rate_i = param_find("MC_PITCHRATE_I"); _params_handles.pitch_rate_integ_lim = param_find("MC_PR_INT_LIM"); _params_handles.pitch_rate_d = param_find("MC_PITCHRATE_D"); _params_handles.pitch_rate_ff = param_find("MC_PITCHRATE_FF"); _params_handles.d_term_cutoff_freq = param_find("MC_DTERM_CUTOFF"); _params_handles.tpa_breakpoint_p = param_find("MC_TPA_BREAK_P"); _params_handles.tpa_breakpoint_i = param_find("MC_TPA_BREAK_I"); _params_handles.tpa_breakpoint_d = param_find("MC_TPA_BREAK_D"); _params_handles.tpa_rate_p = param_find("MC_TPA_RATE_P"); _params_handles.tpa_rate_i = param_find("MC_TPA_RATE_I"); _params_handles.tpa_rate_d = param_find("MC_TPA_RATE_D"); _params_handles.yaw_p = param_find("MC_YAW_P"); _params_handles.yaw_rate_p = param_find("MC_YAWRATE_P"); _params_handles.yaw_rate_i = param_find("MC_YAWRATE_I"); _params_handles.yaw_rate_integ_lim = param_find("MC_YR_INT_LIM"); _params_handles.yaw_rate_d = param_find("MC_YAWRATE_D"); _params_handles.yaw_rate_ff = param_find("MC_YAWRATE_FF"); _params_handles.yaw_ff = param_find("MC_YAW_FF"); _params_handles.roll_rate_max = param_find("MC_ROLLRATE_MAX"); _params_handles.pitch_rate_max = param_find("MC_PITCHRATE_MAX"); _params_handles.yaw_rate_max = param_find("MC_YAWRATE_MAX"); _params_handles.yaw_auto_max = param_find("MC_YAWRAUTO_MAX"); _params_handles.acro_roll_max = param_find("MC_ACRO_R_MAX"); _params_handles.acro_pitch_max = param_find("MC_ACRO_P_MAX"); _params_handles.acro_yaw_max = param_find("MC_ACRO_Y_MAX"); _params_handles.acro_expo = param_find("MC_ACRO_EXPO"); _params_handles.acro_superexpo = param_find("MC_ACRO_SUPEXPO"); _params_handles.rattitude_thres = param_find("MC_RATT_TH"); _params_handles.bat_scale_en = param_find("MC_BAT_SCALE_EN"); /* rotations */ _params_handles.board_rotation = param_find("SENS_BOARD_ROT"); /* rotation offsets */ _params_handles.board_offset[0] = param_find("SENS_BOARD_X_OFF"); _params_handles.board_offset[1] = param_find("SENS_BOARD_Y_OFF"); _params_handles.board_offset[2] = param_find("SENS_BOARD_Z_OFF"); /* fetch initial parameter values */ parameters_update(); /* initialize thermal corrections as we might not immediately get a topic update (only non-zero values) */ for (unsigned i = 0; i < 3; i++) { // used scale factors to unity _sensor_correction.gyro_scale_0[i] = 1.0f; _sensor_correction.gyro_scale_1[i] = 1.0f; _sensor_correction.gyro_scale_2[i] = 1.0f; } } MulticopterAttitudeControl::~MulticopterAttitudeControl() { if (_control_task != -1) { /* task wakes up every 100ms or so at the longest */ _task_should_exit = true; /* wait for a second for the task to quit at our request */ unsigned i = 0; do { /* wait 20ms */ usleep(20000); /* if we have given up, kill it */ if (++i > 50) { px4_task_delete(_control_task); break; } } while (_control_task != -1); } mc_att_control::g_control = nullptr; } void MulticopterAttitudeControl::parameters_update() { float v; /* roll gains */ param_get(_params_handles.roll_p, &v); _params.attitude_p(0) = v; param_get(_params_handles.roll_rate_p, &v); _params.rate_p(0) = v; param_get(_params_handles.roll_rate_i, &v); _params.rate_i(0) = v; param_get(_params_handles.roll_rate_integ_lim, &v); _params.rate_int_lim(0) = v; param_get(_params_handles.roll_rate_d, &v); _params.rate_d(0) = v; param_get(_params_handles.roll_rate_ff, &v); _params.rate_ff(0) = v; /* pitch gains */ param_get(_params_handles.pitch_p, &v); _params.attitude_p(1) = v; param_get(_params_handles.pitch_rate_p, &v); _params.rate_p(1) = v; param_get(_params_handles.pitch_rate_i, &v); _params.rate_i(1) = v; param_get(_params_handles.pitch_rate_integ_lim, &v); _params.rate_int_lim(1) = v; param_get(_params_handles.pitch_rate_d, &v); _params.rate_d(1) = v; param_get(_params_handles.pitch_rate_ff, &v); _params.rate_ff(1) = v; param_get(_params_handles.d_term_cutoff_freq, &_params.d_term_cutoff_freq); if (fabsf(_lp_filters_d[0].get_cutoff_freq() - _params.d_term_cutoff_freq) > 0.01f) { _lp_filters_d[0].set_cutoff_frequency(_loop_update_rate_hz, _params.d_term_cutoff_freq); _lp_filters_d[1].set_cutoff_frequency(_loop_update_rate_hz, _params.d_term_cutoff_freq); _lp_filters_d[2].set_cutoff_frequency(_loop_update_rate_hz, _params.d_term_cutoff_freq); _lp_filters_d[0].reset(_rates_prev(0)); _lp_filters_d[1].reset(_rates_prev(1)); _lp_filters_d[2].reset(_rates_prev(2)); } param_get(_params_handles.tpa_breakpoint_p, &_params.tpa_breakpoint_p); param_get(_params_handles.tpa_breakpoint_i, &_params.tpa_breakpoint_i); param_get(_params_handles.tpa_breakpoint_d, &_params.tpa_breakpoint_d); param_get(_params_handles.tpa_rate_p, &_params.tpa_rate_p); param_get(_params_handles.tpa_rate_i, &_params.tpa_rate_i); param_get(_params_handles.tpa_rate_d, &_params.tpa_rate_d); /* yaw gains */ param_get(_params_handles.yaw_p, &v); _params.attitude_p(2) = v; param_get(_params_handles.yaw_rate_p, &v); _params.rate_p(2) = v; param_get(_params_handles.yaw_rate_i, &v); _params.rate_i(2) = v; param_get(_params_handles.yaw_rate_integ_lim, &v); _params.rate_int_lim(2) = v; param_get(_params_handles.yaw_rate_d, &v); _params.rate_d(2) = v; param_get(_params_handles.yaw_rate_ff, &v); _params.rate_ff(2) = v; param_get(_params_handles.yaw_ff, &_params.yaw_ff); /* angular rate limits */ param_get(_params_handles.roll_rate_max, &_params.roll_rate_max); _params.mc_rate_max(0) = math::radians(_params.roll_rate_max); param_get(_params_handles.pitch_rate_max, &_params.pitch_rate_max); _params.mc_rate_max(1) = math::radians(_params.pitch_rate_max); param_get(_params_handles.yaw_rate_max, &_params.yaw_rate_max); _params.mc_rate_max(2) = math::radians(_params.yaw_rate_max); /* auto angular rate limits */ param_get(_params_handles.roll_rate_max, &_params.roll_rate_max); _params.auto_rate_max(0) = math::radians(_params.roll_rate_max); param_get(_params_handles.pitch_rate_max, &_params.pitch_rate_max); _params.auto_rate_max(1) = math::radians(_params.pitch_rate_max); param_get(_params_handles.yaw_auto_max, &_params.yaw_auto_max); _params.auto_rate_max(2) = math::radians(_params.yaw_auto_max); /* manual rate control acro mode rate limits and expo */ param_get(_params_handles.acro_roll_max, &v); _params.acro_rate_max(0) = math::radians(v); param_get(_params_handles.acro_pitch_max, &v); _params.acro_rate_max(1) = math::radians(v); param_get(_params_handles.acro_yaw_max, &v); _params.acro_rate_max(2) = math::radians(v); param_get(_params_handles.acro_expo, &_params.acro_expo); param_get(_params_handles.acro_superexpo, &_params.acro_superexpo); /* stick deflection needed in rattitude mode to control rates not angles */ param_get(_params_handles.rattitude_thres, &_params.rattitude_thres); if (_vehicle_status.is_vtol) { param_get(_params_handles.vtol_wv_yaw_rate_scale, &_params.vtol_wv_yaw_rate_scale); } else { _params.vtol_wv_yaw_rate_scale = 0.f; } param_get(_params_handles.bat_scale_en, &_params.bat_scale_en); _actuators_0_circuit_breaker_enabled = circuit_breaker_enabled("CBRK_RATE_CTRL", CBRK_RATE_CTRL_KEY); /* rotation of the autopilot relative to the body */ param_get(_params_handles.board_rotation, &(_params.board_rotation)); /* fine adjustment of the rotation */ param_get(_params_handles.board_offset[0], &(_params.board_offset[0])); param_get(_params_handles.board_offset[1], &(_params.board_offset[1])); param_get(_params_handles.board_offset[2], &(_params.board_offset[2])); /* get transformation matrix from sensor/board to body frame */ get_rot_matrix((enum Rotation)_params.board_rotation, &_board_rotation); /* fine tune the rotation */ math::Matrix<3, 3> board_rotation_offset; board_rotation_offset.from_euler(M_DEG_TO_RAD_F * _params.board_offset[0], M_DEG_TO_RAD_F * _params.board_offset[1], M_DEG_TO_RAD_F * _params.board_offset[2]); _board_rotation = board_rotation_offset * _board_rotation; } void MulticopterAttitudeControl::parameter_update_poll() { bool updated; /* Check if parameters have changed */ orb_check(_params_sub, &updated); if (updated) { struct parameter_update_s param_update; orb_copy(ORB_ID(parameter_update), _params_sub, ¶m_update); parameters_update(); } } void MulticopterAttitudeControl::vehicle_control_mode_poll() { bool updated; /* Check if vehicle control mode has changed */ orb_check(_v_control_mode_sub, &updated); if (updated) { orb_copy(ORB_ID(vehicle_control_mode), _v_control_mode_sub, &_v_control_mode); } } void MulticopterAttitudeControl::vehicle_manual_poll() { bool updated; /* get pilots inputs */ orb_check(_manual_control_sp_sub, &updated); if (updated) { orb_copy(ORB_ID(manual_control_setpoint), _manual_control_sp_sub, &_manual_control_sp); } } void MulticopterAttitudeControl::vehicle_attitude_setpoint_poll() { /* check if there is a new setpoint */ bool updated; orb_check(_v_att_sp_sub, &updated); if (updated) { orb_copy(ORB_ID(vehicle_attitude_setpoint), _v_att_sp_sub, &_v_att_sp); } } void MulticopterAttitudeControl::vehicle_rates_setpoint_poll() { /* check if there is a new setpoint */ bool updated; orb_check(_v_rates_sp_sub, &updated); if (updated) { orb_copy(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_sub, &_v_rates_sp); } } 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); /* set correct uORB ID, depending on if vehicle is VTOL or not */ if (_rates_sp_id == nullptr) { if (_vehicle_status.is_vtol) { _rates_sp_id = ORB_ID(mc_virtual_rates_setpoint); _actuators_id = ORB_ID(actuator_controls_virtual_mc); _params_handles.vtol_wv_yaw_rate_scale = param_find("VT_WV_YAWR_SCL"); parameters_update(); } else { _rates_sp_id = ORB_ID(vehicle_rates_setpoint); _actuators_id = ORB_ID(actuator_controls_0); } } } } void MulticopterAttitudeControl::vehicle_motor_limits_poll() { /* check if there is a new message */ bool updated; orb_check(_motor_limits_sub, &updated); if (updated) { multirotor_motor_limits_s motor_limits = {}; orb_copy(ORB_ID(multirotor_motor_limits), _motor_limits_sub, &motor_limits); _saturation_status.value = motor_limits.saturation_status; } } void MulticopterAttitudeControl::battery_status_poll() { /* check if there is a new message */ bool updated; orb_check(_battery_status_sub, &updated); if (updated) { orb_copy(ORB_ID(battery_status), _battery_status_sub, &_battery_status); } } void MulticopterAttitudeControl::vehicle_attitude_poll() { /* check if there is a new message */ bool updated; orb_check(_v_att_sub, &updated); if (updated) { orb_copy(ORB_ID(vehicle_attitude), _v_att_sub, &_v_att); } } void MulticopterAttitudeControl::sensor_correction_poll() { /* check if there is a new message */ bool updated; orb_check(_sensor_correction_sub, &updated); if (updated) { orb_copy(ORB_ID(sensor_correction), _sensor_correction_sub, &_sensor_correction); } /* update the latest gyro selection */ if (_sensor_correction.selected_gyro_instance < _gyro_count) { _selected_gyro = _sensor_correction.selected_gyro_instance; } } void MulticopterAttitudeControl::sensor_bias_poll() { /* check if there is a new message */ bool updated; orb_check(_sensor_bias_sub, &updated); if (updated) { orb_copy(ORB_ID(sensor_bias), _sensor_bias_sub, &_sensor_bias); } } /** * Attitude controller. * Input: 'vehicle_attitude_setpoint' topics (depending on mode) * Output: '_rates_sp' vector, '_thrust_sp' */ void MulticopterAttitudeControl::control_attitude(float dt) { vehicle_attitude_setpoint_poll(); _thrust_sp = _v_att_sp.thrust; /* prepare yaw weight from the ratio between roll/pitch and yaw gains */ Vector3f attitude_gain = _params.attitude_p; const float roll_pitch_gain = (attitude_gain(0) + attitude_gain(1)) / 2.f; const float yaw_w = math::constrain(attitude_gain(2) / roll_pitch_gain, 0.f, 1.f); attitude_gain(2) = roll_pitch_gain; /* get estimated and desired vehicle attitude */ Quatf q(_v_att.q); Quatf qd(_v_att_sp.q_d); /* ensure input quaternions are exactly normalized because acosf(1.00001) == NaN */ q.normalize(); qd.normalize(); /* calculate reduced desired attitude neglecting vehicle's yaw to prioritize roll and pitch */ Vector3f e_z = q.dcm_z(); Vector3f e_z_d = qd.dcm_z(); Quatf qd_red(e_z, e_z_d); if (abs(qd_red(1)) > (1.f - 1e-5f) || abs(qd_red(2)) > (1.f - 1e-5f)) { /* In the infinitesimal corner case where the vehicle and thrust have the completely opposite direction, * full attitude control anyways generates no yaw input and directly takes the combination of * roll and pitch leading to the correct desired yaw. Ignoring this case would still be totally safe and stable. */ qd_red = qd; } else { /* transform rotation from current to desired thrust vector for into a reduced world frame attitude */ qd_red *= q; } /* mix full and reduced desired attitude */ Quatf q_mix = qd_red.inversed() * qd; q_mix *= math::signNoZero(q_mix(0)); /* catch numerical problems with the domain of acosf and asinf */ q_mix(0) = math::constrain(q_mix(0), -1.f, 1.f); q_mix(3) = math::constrain(q_mix(3), -1.f, 1.f); qd = qd_red * Quatf(cosf(yaw_w * acosf(q_mix(0))), 0, 0, sinf(yaw_w * asinf(q_mix(3)))); /* quaternion attitude control law, qe is rotation from q to qd */ Quatf qe = q.inversed() * qd; /* using sin(alpha/2) scaled rotation axis as attitude error (see quaternion definition by axis angle) * also taking care of the antipodal unit quaternion ambiguity */ Vector3f eq = 2.f * math::signNoZero(qe(0)) * qe.imag(); /* calculate angular rates setpoint */ eq = eq.emult(attitude_gain); _rates_sp = math::Vector<3>(eq.data()); /* Feed forward the yaw setpoint rate. We need to apply the yaw rate in the body frame. * We infer the body z axis by taking the last column of R.transposed (== q.inversed) * because it's the rotation axis for body yaw and multiply it by the rate and gain. */ Vector3f yaw_feedforward_rate = q.inversed().dcm_z(); yaw_feedforward_rate *= _v_att_sp.yaw_sp_move_rate * _params.yaw_ff; yaw_feedforward_rate(2) *= yaw_w; _rates_sp += math::Vector<3>(yaw_feedforward_rate.data()); /* limit rates */ for (int i = 0; i < 3; i++) { if ((_v_control_mode.flag_control_velocity_enabled || _v_control_mode.flag_control_auto_enabled) && !_v_control_mode.flag_control_manual_enabled) { _rates_sp(i) = math::constrain(_rates_sp(i), -_params.auto_rate_max(i), _params.auto_rate_max(i)); } else { _rates_sp(i) = math::constrain(_rates_sp(i), -_params.mc_rate_max(i), _params.mc_rate_max(i)); } } /* VTOL weather-vane mode, dampen yaw rate */ if (_vehicle_status.is_vtol && _v_att_sp.disable_mc_yaw_control) { if (_v_control_mode.flag_control_velocity_enabled || _v_control_mode.flag_control_auto_enabled) { const float wv_yaw_rate_max = _params.auto_rate_max(2) * _params.vtol_wv_yaw_rate_scale; _rates_sp(2) = math::constrain(_rates_sp(2), -wv_yaw_rate_max, wv_yaw_rate_max); // prevent integrator winding up in weathervane mode _rates_int(2) = 0.0f; } } } /* * Throttle PID attenuation * Function visualization available here https://www.desmos.com/calculator/gn4mfoddje * Input: 'tpa_breakpoint', 'tpa_rate', '_thrust_sp' * Output: 'pidAttenuationPerAxis' vector */ math::Vector<3> MulticopterAttitudeControl::pid_attenuations(float tpa_breakpoint, float tpa_rate) { /* throttle pid attenuation factor */ float tpa = 1.0f - tpa_rate * (fabsf(_v_rates_sp.thrust) - tpa_breakpoint) / (1.0f - tpa_breakpoint); tpa = fmaxf(TPA_RATE_LOWER_LIMIT, fminf(1.0f, tpa)); math::Vector<3> pidAttenuationPerAxis; pidAttenuationPerAxis(AXIS_INDEX_ROLL) = tpa; pidAttenuationPerAxis(AXIS_INDEX_PITCH) = tpa; pidAttenuationPerAxis(AXIS_INDEX_YAW) = 1.0; return pidAttenuationPerAxis; } /* * Attitude rates controller. * Input: '_rates_sp' vector, '_thrust_sp' * Output: '_att_control' vector */ void MulticopterAttitudeControl::control_attitude_rates(float dt) { /* reset integral if disarmed */ if (!_v_control_mode.flag_armed || !_vehicle_status.is_rotary_wing) { _rates_int.zero(); } // get the raw gyro data and correct for thermal errors math::Vector<3> rates; if (_selected_gyro == 0) { rates(0) = (_sensor_gyro.x - _sensor_correction.gyro_offset_0[0]) * _sensor_correction.gyro_scale_0[0]; rates(1) = (_sensor_gyro.y - _sensor_correction.gyro_offset_0[1]) * _sensor_correction.gyro_scale_0[1]; rates(2) = (_sensor_gyro.z - _sensor_correction.gyro_offset_0[2]) * _sensor_correction.gyro_scale_0[2]; } else if (_selected_gyro == 1) { rates(0) = (_sensor_gyro.x - _sensor_correction.gyro_offset_1[0]) * _sensor_correction.gyro_scale_1[0]; rates(1) = (_sensor_gyro.y - _sensor_correction.gyro_offset_1[1]) * _sensor_correction.gyro_scale_1[1]; rates(2) = (_sensor_gyro.z - _sensor_correction.gyro_offset_1[2]) * _sensor_correction.gyro_scale_1[2]; } else if (_selected_gyro == 2) { rates(0) = (_sensor_gyro.x - _sensor_correction.gyro_offset_2[0]) * _sensor_correction.gyro_scale_2[0]; rates(1) = (_sensor_gyro.y - _sensor_correction.gyro_offset_2[1]) * _sensor_correction.gyro_scale_2[1]; rates(2) = (_sensor_gyro.z - _sensor_correction.gyro_offset_2[2]) * _sensor_correction.gyro_scale_2[2]; } else { rates(0) = _sensor_gyro.x; rates(1) = _sensor_gyro.y; rates(2) = _sensor_gyro.z; } // rotate corrected measurements from sensor to body frame rates = _board_rotation * rates; // correct for in-run bias errors rates(0) -= _sensor_bias.gyro_x_bias; rates(1) -= _sensor_bias.gyro_y_bias; rates(2) -= _sensor_bias.gyro_z_bias; math::Vector<3> rates_p_scaled = _params.rate_p.emult(pid_attenuations(_params.tpa_breakpoint_p, _params.tpa_rate_p)); //math::Vector<3> rates_i_scaled = _params.rate_i.emult(pid_attenuations(_params.tpa_breakpoint_i, _params.tpa_rate_i)); math::Vector<3> rates_d_scaled = _params.rate_d.emult(pid_attenuations(_params.tpa_breakpoint_d, _params.tpa_rate_d)); /* angular rates error */ math::Vector<3> rates_err = _rates_sp - rates; /* apply low-pass filtering to the rates for D-term */ math::Vector<3> rates_filtered( _lp_filters_d[0].apply(rates(0)), _lp_filters_d[1].apply(rates(1)), _lp_filters_d[2].apply(rates(2))); _att_control = rates_p_scaled.emult(rates_err) + _rates_int - rates_d_scaled.emult(rates_filtered - _rates_prev_filtered) / dt + _params.rate_ff.emult(_rates_sp); _rates_prev = rates; _rates_prev_filtered = rates_filtered; /* update integral only if motors are providing enough thrust to be effective */ if (_thrust_sp > MIN_TAKEOFF_THRUST) { for (int i = AXIS_INDEX_ROLL; i < AXIS_COUNT; i++) { // Check for positive control saturation bool positive_saturation = ((i == AXIS_INDEX_ROLL) && _saturation_status.flags.roll_pos) || ((i == AXIS_INDEX_PITCH) && _saturation_status.flags.pitch_pos) || ((i == AXIS_INDEX_YAW) && _saturation_status.flags.yaw_pos); // Check for negative control saturation bool negative_saturation = ((i == AXIS_INDEX_ROLL) && _saturation_status.flags.roll_neg) || ((i == AXIS_INDEX_PITCH) && _saturation_status.flags.pitch_neg) || ((i == AXIS_INDEX_YAW) && _saturation_status.flags.yaw_neg); // prevent further positive control saturation if (positive_saturation) { rates_err(i) = math::min(rates_err(i), 0.0f); } // prevent further negative control saturation if (negative_saturation) { rates_err(i) = math::max(rates_err(i), 0.0f); } // Perform the integration using a first order method and do not propaate the result if out of range or invalid float rate_i = _rates_int(i) + _params.rate_i(i) * rates_err(i) * dt; if (PX4_ISFINITE(rate_i) && rate_i > -_params.rate_int_lim(i) && rate_i < _params.rate_int_lim(i)) { _rates_int(i) = rate_i; } } } /* explicitly limit the integrator state */ for (int i = AXIS_INDEX_ROLL; i < AXIS_COUNT; i++) { _rates_int(i) = math::constrain(_rates_int(i), -_params.rate_int_lim(i), _params.rate_int_lim(i)); } } void MulticopterAttitudeControl::task_main_trampoline(int argc, char *argv[]) { mc_att_control::g_control->task_main(); } void MulticopterAttitudeControl::task_main() { /* * do subscriptions */ _v_att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); _v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); _v_rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); _v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); _vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); _motor_limits_sub = orb_subscribe(ORB_ID(multirotor_motor_limits)); _battery_status_sub = orb_subscribe(ORB_ID(battery_status)); _gyro_count = math::min(orb_group_count(ORB_ID(sensor_gyro)), MAX_GYRO_COUNT); if (_gyro_count == 0) { _gyro_count = 1; } for (unsigned s = 0; s < _gyro_count; s++) { _sensor_gyro_sub[s] = orb_subscribe_multi(ORB_ID(sensor_gyro), s); } _sensor_correction_sub = orb_subscribe(ORB_ID(sensor_correction)); _sensor_bias_sub = orb_subscribe(ORB_ID(sensor_bias)); /* initialize parameters cache */ parameters_update(); /* wakeup source: gyro data from sensor selected by the sensor app */ px4_pollfd_struct_t poll_fds = {}; poll_fds.events = POLLIN; const hrt_abstime task_start = hrt_absolute_time(); hrt_abstime last_run = task_start; float dt_accumulator = 0.f; int loop_counter = 0; while (!_task_should_exit) { poll_fds.fd = _sensor_gyro_sub[_selected_gyro]; /* wait for up to 100ms for data */ int pret = px4_poll(&poll_fds, 1, 100); /* timed out - periodic check for _task_should_exit */ if (pret == 0) { continue; } /* this is undesirable but not much we can do - might want to flag unhappy status */ if (pret < 0) { warn("mc att ctrl: poll error %d, %d", pret, errno); /* sleep a bit before next try */ usleep(100000); continue; } perf_begin(_loop_perf); /* run controller on gyro changes */ if (poll_fds.revents & POLLIN) { const hrt_abstime now = hrt_absolute_time(); float dt = (now - last_run) / 1e6f; last_run = now; /* guard against too small (< 2ms) and too large (> 20ms) dt's */ if (dt < 0.002f) { dt = 0.002f; } else if (dt > 0.02f) { dt = 0.02f; } /* copy gyro data */ orb_copy(ORB_ID(sensor_gyro), _sensor_gyro_sub[_selected_gyro], &_sensor_gyro); /* check for updates in other topics */ parameter_update_poll(); vehicle_control_mode_poll(); vehicle_manual_poll(); vehicle_status_poll(); vehicle_motor_limits_poll(); battery_status_poll(); vehicle_attitude_poll(); sensor_correction_poll(); sensor_bias_poll(); /* Check if we are in rattitude mode and the pilot is above the threshold on pitch * or roll (yaw can rotate 360 in normal att control). If both are true don't * even bother running the attitude controllers */ if (_v_control_mode.flag_control_rattitude_enabled) { if (fabsf(_manual_control_sp.y) > _params.rattitude_thres || fabsf(_manual_control_sp.x) > _params.rattitude_thres) { _v_control_mode.flag_control_attitude_enabled = false; } } if (_v_control_mode.flag_control_attitude_enabled) { control_attitude(dt); /* publish attitude rates setpoint */ _v_rates_sp.roll = _rates_sp(0); _v_rates_sp.pitch = _rates_sp(1); _v_rates_sp.yaw = _rates_sp(2); _v_rates_sp.thrust = _thrust_sp; _v_rates_sp.timestamp = hrt_absolute_time(); if (_v_rates_sp_pub != nullptr) { orb_publish(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp); } else if (_rates_sp_id) { _v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp); } } else { /* attitude controller disabled, poll rates setpoint topic */ if (_v_control_mode.flag_control_manual_enabled) { /* manual rates control - ACRO mode */ matrix::Vector3f man_rate_sp; man_rate_sp(0) = math::superexpo(_manual_control_sp.y, _params.acro_expo, _params.acro_superexpo); man_rate_sp(1) = math::superexpo(-_manual_control_sp.x, _params.acro_expo, _params.acro_superexpo); man_rate_sp(2) = math::superexpo(_manual_control_sp.r, _params.acro_expo, _params.acro_superexpo); man_rate_sp = man_rate_sp.emult(_params.acro_rate_max); _rates_sp = math::Vector<3>(man_rate_sp.data()); _thrust_sp = _manual_control_sp.z; /* publish attitude rates setpoint */ _v_rates_sp.roll = _rates_sp(0); _v_rates_sp.pitch = _rates_sp(1); _v_rates_sp.yaw = _rates_sp(2); _v_rates_sp.thrust = _thrust_sp; _v_rates_sp.timestamp = hrt_absolute_time(); if (_v_rates_sp_pub != nullptr) { orb_publish(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp); } else if (_rates_sp_id) { _v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp); } } else { /* attitude controller disabled, poll rates setpoint topic */ vehicle_rates_setpoint_poll(); _rates_sp(0) = _v_rates_sp.roll; _rates_sp(1) = _v_rates_sp.pitch; _rates_sp(2) = _v_rates_sp.yaw; _thrust_sp = _v_rates_sp.thrust; } } if (_v_control_mode.flag_control_rates_enabled) { control_attitude_rates(dt); /* publish actuator controls */ _actuators.control[0] = (PX4_ISFINITE(_att_control(0))) ? _att_control(0) : 0.0f; _actuators.control[1] = (PX4_ISFINITE(_att_control(1))) ? _att_control(1) : 0.0f; _actuators.control[2] = (PX4_ISFINITE(_att_control(2))) ? _att_control(2) : 0.0f; _actuators.control[3] = (PX4_ISFINITE(_thrust_sp)) ? _thrust_sp : 0.0f; _actuators.control[7] = _v_att_sp.landing_gear; _actuators.timestamp = hrt_absolute_time(); _actuators.timestamp_sample = _sensor_gyro.timestamp; /* scale effort by battery status */ if (_params.bat_scale_en && _battery_status.scale > 0.0f) { for (int i = 0; i < 4; i++) { _actuators.control[i] *= _battery_status.scale; } } if (!_actuators_0_circuit_breaker_enabled) { if (_actuators_0_pub != nullptr) { orb_publish(_actuators_id, _actuators_0_pub, &_actuators); perf_end(_controller_latency_perf); } else if (_actuators_id) { _actuators_0_pub = orb_advertise(_actuators_id, &_actuators); } } /* publish controller status */ rate_ctrl_status_s rate_ctrl_status; rate_ctrl_status.timestamp = hrt_absolute_time(); rate_ctrl_status.rollspeed = _rates_prev(0); rate_ctrl_status.pitchspeed = _rates_prev(1); rate_ctrl_status.yawspeed = _rates_prev(2); rate_ctrl_status.rollspeed_integ = _rates_int(0); rate_ctrl_status.pitchspeed_integ = _rates_int(1); rate_ctrl_status.yawspeed_integ = _rates_int(2); int instance; orb_publish_auto(ORB_ID(rate_ctrl_status), &_controller_status_pub, &rate_ctrl_status, &instance, ORB_PRIO_DEFAULT); } if (_v_control_mode.flag_control_termination_enabled) { if (!_vehicle_status.is_vtol) { _rates_sp.zero(); _rates_int.zero(); _thrust_sp = 0.0f; _att_control.zero(); /* publish actuator controls */ _actuators.control[0] = 0.0f; _actuators.control[1] = 0.0f; _actuators.control[2] = 0.0f; _actuators.control[3] = 0.0f; _actuators.timestamp = hrt_absolute_time(); _actuators.timestamp_sample = _sensor_gyro.timestamp; if (!_actuators_0_circuit_breaker_enabled) { if (_actuators_0_pub != nullptr) { orb_publish(_actuators_id, _actuators_0_pub, &_actuators); perf_end(_controller_latency_perf); } else if (_actuators_id) { _actuators_0_pub = orb_advertise(_actuators_id, &_actuators); } } } } /* calculate loop update rate while disarmed or at least a few times (updating the filter is expensive) */ if (!_v_control_mode.flag_armed || (now - task_start) < 3300000) { dt_accumulator += dt; ++loop_counter; if (dt_accumulator > 1.f) { const float loop_update_rate = (float)loop_counter / dt_accumulator; _loop_update_rate_hz = _loop_update_rate_hz * 0.5f + loop_update_rate * 0.5f; dt_accumulator = 0; loop_counter = 0; _lp_filters_d[0].set_cutoff_frequency(_loop_update_rate_hz, _params.d_term_cutoff_freq); _lp_filters_d[1].set_cutoff_frequency(_loop_update_rate_hz, _params.d_term_cutoff_freq); _lp_filters_d[2].set_cutoff_frequency(_loop_update_rate_hz, _params.d_term_cutoff_freq); } } } perf_end(_loop_perf); } _control_task = -1; } int MulticopterAttitudeControl::start() { ASSERT(_control_task == -1); /* start the task */ _control_task = px4_task_spawn_cmd("mc_att_control", SCHED_DEFAULT, SCHED_PRIORITY_ATTITUDE_CONTROL, 1700, (px4_main_t)&MulticopterAttitudeControl::task_main_trampoline, nullptr); if (_control_task < 0) { warn("task start failed"); return -errno; } return OK; } int mc_att_control_main(int argc, char *argv[]) { if (argc < 2) { warnx("usage: mc_att_control {start|stop|status}"); return 1; } if (!strcmp(argv[1], "start")) { if (mc_att_control::g_control != nullptr) { warnx("already running"); return 1; } mc_att_control::g_control = new MulticopterAttitudeControl; if (mc_att_control::g_control == nullptr) { warnx("alloc failed"); return 1; } if (OK != mc_att_control::g_control->start()) { delete mc_att_control::g_control; mc_att_control::g_control = nullptr; warnx("start failed"); return 1; } return 0; } if (!strcmp(argv[1], "stop")) { if (mc_att_control::g_control == nullptr) { warnx("not running"); return 1; } delete mc_att_control::g_control; mc_att_control::g_control = nullptr; return 0; } if (!strcmp(argv[1], "status")) { if (mc_att_control::g_control) { warnx("running"); return 0; } else { warnx("not running"); return 1; } } warnx("unrecognized command"); return 1; }