From 62de8a2c5b35c8837f5772f793b4049200c0e6ac Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Thu, 17 Oct 2019 12:28:12 +0200 Subject: [PATCH] mc_rate_control: add inertia matrix --- .../MulticopterRateControl.cpp | 143 +++++++++++++----- .../MulticopterRateControl.hpp | 22 ++- .../RateControl/RateControl.cpp | 36 ++++- .../RateControl/RateControl.hpp | 33 +++- .../RateControl/RateControlTest.cpp | 3 +- .../mc_rate_control/mc_rate_control_params.c | 1 - .../mc_rate_control/vehicle_model_params.c | 109 +++++++++++++ 7 files changed, 302 insertions(+), 45 deletions(-) create mode 100644 src/modules/mc_rate_control/vehicle_model_params.c diff --git a/src/modules/mc_rate_control/MulticopterRateControl.cpp b/src/modules/mc_rate_control/MulticopterRateControl.cpp index 1d1e1b5d08..4c45a0a52c 100644 --- a/src/modules/mc_rate_control/MulticopterRateControl.cpp +++ b/src/modules/mc_rate_control/MulticopterRateControl.cpp @@ -94,6 +94,14 @@ MulticopterRateControl::parameters_updated() _acro_rate_max = Vector3f(radians(_param_mc_acro_r_max.get()), radians(_param_mc_acro_p_max.get()), radians(_param_mc_acro_y_max.get())); + // inertia matrix + const float inertia[3][3] = { + {_param_vm_inertia_xx.get(), _param_vm_inertia_xy.get(), _param_vm_inertia_xz.get()}, + {_param_vm_inertia_xy.get(), _param_vm_inertia_yy.get(), _param_vm_inertia_yz.get()}, + {_param_vm_inertia_xz.get(), _param_vm_inertia_yz.get(), _param_vm_inertia_zz.get()} + }; + _rate_control.setInertiaMatrix(matrix::Matrix3f(inertia)); + _actuators_0_circuit_breaker_enabled = circuit_breaker_enabled_by_val(_param_cbrk_rate_ctrl.get(), CBRK_RATE_CTRL_KEY); } @@ -164,6 +172,7 @@ MulticopterRateControl::Run() if (_vehicle_angular_velocity_sub.update(&angular_velocity)) { const hrt_abstime now = hrt_absolute_time(); + _timestamp_sample = angular_velocity.timestamp_sample; // Guard against too small (< 0.2ms) and too large (> 20ms) dt's. const float dt = math::constrain(((now - _last_run) / 1e6f), 0.0002f, 0.02f); @@ -271,7 +280,7 @@ MulticopterRateControl::Run() } // run the rate controller - if (_v_control_mode.flag_control_rates_enabled && !_actuators_0_circuit_breaker_enabled) { + if (_v_control_mode.flag_control_rates_enabled) { // reset integral if disarmed if (!_v_control_mode.flag_armed || _vehicle_status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { @@ -291,7 +300,7 @@ MulticopterRateControl::Run() } // run rate controller - const Vector3f att_control = _rate_control.update(rates, _rates_sp, dt, _maybe_landed || _landed); + _rate_control.update(rates, _rates_sp, dt, _maybe_landed || _landed); // publish rate controller status rate_ctrl_status_s rate_ctrl_status{}; @@ -299,41 +308,22 @@ MulticopterRateControl::Run() rate_ctrl_status.timestamp = hrt_absolute_time(); _controller_status_pub.publish(rate_ctrl_status); - // publish actuator controls - actuator_controls_s actuators{}; - actuators.control[actuator_controls_s::INDEX_ROLL] = PX4_ISFINITE(att_control(0)) ? att_control(0) : 0.0f; - actuators.control[actuator_controls_s::INDEX_PITCH] = PX4_ISFINITE(att_control(1)) ? att_control(1) : 0.0f; - actuators.control[actuator_controls_s::INDEX_YAW] = PX4_ISFINITE(att_control(2)) ? att_control(2) : 0.0f; - actuators.control[actuator_controls_s::INDEX_THROTTLE] = PX4_ISFINITE(_thrust_sp) ? _thrust_sp : 0.0f; - actuators.control[actuator_controls_s::INDEX_LANDING_GEAR] = (float)_landing_gear.landing_gear; - actuators.timestamp_sample = angular_velocity.timestamp_sample; - - // scale effort by battery status if enabled - if (_param_mc_bat_scale_en.get()) { - if (_battery_status_sub.updated()) { - battery_status_s battery_status; - - if (_battery_status_sub.copy(&battery_status)) { - _battery_status_scale = battery_status.scale; - } - } - - if (_battery_status_scale > 0.0f) { - for (int i = 0; i < 4; i++) { - actuators.control[i] *= _battery_status_scale; - } - } - } - - actuators.timestamp = hrt_absolute_time(); - orb_publish_auto(_actuators_id, &_actuators_0_pub, &actuators, nullptr, ORB_PRIO_DEFAULT); + // publish controller output + publish_actuator_controls(); + publish_angular_acceleration_setpoint(); + publish_torque_setpoint(); + publish_thrust_setpoint(); } else if (_v_control_mode.flag_control_termination_enabled) { if (!_vehicle_status.is_vtol) { - // publish actuator controls - actuator_controls_s actuators{}; - actuators.timestamp = hrt_absolute_time(); - orb_publish_auto(_actuators_id, &_actuators_0_pub, &actuators, nullptr, ORB_PRIO_DEFAULT); + // reset controller + _rate_control.reset(); + + // publish controller output + publish_actuator_controls(); + publish_angular_acceleration_setpoint(); + publish_torque_setpoint(); + publish_thrust_setpoint(); } } } @@ -341,6 +331,91 @@ MulticopterRateControl::Run() perf_end(_loop_perf); } +void +MulticopterRateControl::publish_actuator_controls() +{ + Vector3f act_control = _rate_control.getTorqueSetpoint(); + + actuator_controls_s actuators{}; + actuators.timestamp = hrt_absolute_time(); + actuators.timestamp_sample = _timestamp_sample; + actuators.control[actuator_controls_s::INDEX_ROLL] = PX4_ISFINITE(act_control(0)) ? act_control(0) : 0.0f; + actuators.control[actuator_controls_s::INDEX_PITCH] = PX4_ISFINITE(act_control(1)) ? act_control(1) : 0.0f; + actuators.control[actuator_controls_s::INDEX_YAW] = PX4_ISFINITE(act_control(2)) ? act_control(2) : 0.0f; + actuators.control[actuator_controls_s::INDEX_THROTTLE] = PX4_ISFINITE(_thrust_sp) ? _thrust_sp : 0.0f; + actuators.control[actuator_controls_s::INDEX_LANDING_GEAR] = (float)_landing_gear.landing_gear; + + // scale effort by battery status if enabled + if (_param_mc_bat_scale_en.get()) { + if (_battery_status_sub.updated()) { + battery_status_s battery_status; + + if (_battery_status_sub.copy(&battery_status)) { + _battery_status_scale = battery_status.scale; + } + } + + if (_battery_status_scale > 0.0f) { + for (int i = 0; i < 4; i++) { + actuators.control[i] *= _battery_status_scale; + } + } + } + + if (!_actuators_0_circuit_breaker_enabled) { + orb_publish_auto(_actuators_id, &_actuators_0_pub, &actuators, nullptr, ORB_PRIO_DEFAULT); + } +} + +void +MulticopterRateControl::publish_angular_acceleration_setpoint() +{ + Vector3f angular_accel_sp = _rate_control.getAngularAccelerationSetpoint(); + + vehicle_angular_acceleration_setpoint_s v_angular_accel_sp = {}; + v_angular_accel_sp.timestamp = hrt_absolute_time(); + v_angular_accel_sp.timestamp_sample = _timestamp_sample; + v_angular_accel_sp.xyz[0] = (PX4_ISFINITE(angular_accel_sp(0))) ? angular_accel_sp(0) : 0.0f; + v_angular_accel_sp.xyz[1] = (PX4_ISFINITE(angular_accel_sp(1))) ? angular_accel_sp(1) : 0.0f; + v_angular_accel_sp.xyz[2] = (PX4_ISFINITE(angular_accel_sp(2))) ? angular_accel_sp(2) : 0.0f; + + if (!_vehicle_status.is_vtol) { + _vehicle_angular_acceleration_setpoint_pub.publish(v_angular_accel_sp); + } +} + +void +MulticopterRateControl::publish_torque_setpoint() +{ + Vector3f torque_sp = _rate_control.getTorqueSetpoint(); + + vehicle_torque_setpoint_s v_torque_sp = {}; + v_torque_sp.timestamp = hrt_absolute_time(); + v_torque_sp.timestamp_sample = _timestamp_sample; + v_torque_sp.xyz[0] = (PX4_ISFINITE(torque_sp(0))) ? torque_sp(0) : 0.0f; + v_torque_sp.xyz[1] = (PX4_ISFINITE(torque_sp(1))) ? torque_sp(1) : 0.0f; + v_torque_sp.xyz[2] = (PX4_ISFINITE(torque_sp(2))) ? torque_sp(2) : 0.0f; + + if (!_vehicle_status.is_vtol) { + _vehicle_torque_setpoint_pub.publish(v_torque_sp); + } +} + +void +MulticopterRateControl::publish_thrust_setpoint() +{ + vehicle_thrust_setpoint_s v_thrust_sp = {}; + v_thrust_sp.timestamp = hrt_absolute_time(); + v_thrust_sp.timestamp_sample = _timestamp_sample; + v_thrust_sp.xyz[0] = 0.0f; + v_thrust_sp.xyz[1] = 0.0f; + v_thrust_sp.xyz[2] = (PX4_ISFINITE(-_thrust_sp)) ? (-_thrust_sp) : 0.0f; + + if (!_vehicle_status.is_vtol) { + _vehicle_thrust_setpoint_pub.publish(v_thrust_sp); + } +} + int MulticopterRateControl::task_spawn(int argc, char *argv[]) { MulticopterRateControl *instance = new MulticopterRateControl(); diff --git a/src/modules/mc_rate_control/MulticopterRateControl.hpp b/src/modules/mc_rate_control/MulticopterRateControl.hpp index 7bf82c4ac9..c331b19110 100644 --- a/src/modules/mc_rate_control/MulticopterRateControl.hpp +++ b/src/modules/mc_rate_control/MulticopterRateControl.hpp @@ -54,11 +54,15 @@ #include #include #include +#include #include #include #include #include #include +#include +#include + class MulticopterRateControl : public ModuleBase, public ModuleParams, public px4::WorkItem { @@ -98,6 +102,11 @@ private: */ float get_landing_gear_state(); + void publish_angular_acceleration_setpoint(); + void publish_torque_setpoint(); + void publish_thrust_setpoint(); + void publish_actuator_controls(); + RateControl _rate_control; ///< class for rate control calculations uORB::Subscription _v_rates_sp_sub{ORB_ID(vehicle_rates_setpoint)}; /**< vehicle rates setpoint subscription */ @@ -115,6 +124,9 @@ private: uORB::PublicationMulti _controller_status_pub{ORB_ID(rate_ctrl_status), ORB_PRIO_DEFAULT}; /**< controller status publication */ uORB::Publication _landing_gear_pub{ORB_ID(landing_gear)}; uORB::Publication _v_rates_sp_pub{ORB_ID(vehicle_rates_setpoint)}; /**< rate setpoint publication */ + uORB::Publication _vehicle_angular_acceleration_setpoint_pub{ORB_ID(vehicle_angular_acceleration_setpoint)}; /**< angular acceleration setpoint publication */ + uORB::Publication _vehicle_thrust_setpoint_pub{ORB_ID(vehicle_thrust_setpoint)}; /**< thrust setpoint publication */ + uORB::Publication _vehicle_torque_setpoint_pub{ORB_ID(vehicle_torque_setpoint)}; /**< torque setpoint publication */ orb_advert_t _actuators_0_pub{nullptr}; /**< attitude actuator controls publication */ orb_id_t _actuators_id{nullptr}; /**< pointer to correct actuator controls0 uORB metadata structure */ @@ -143,6 +155,7 @@ private: hrt_abstime _task_start{hrt_absolute_time()}; hrt_abstime _last_run{0}; + hrt_abstime _timestamp_sample{0}; float _dt_accumulator{0.0f}; int _loop_counter{0}; @@ -184,7 +197,14 @@ private: (ParamBool) _param_mc_bat_scale_en, - (ParamInt) _param_cbrk_rate_ctrl + (ParamInt) _param_cbrk_rate_ctrl, + + (ParamFloat) _param_vm_inertia_xx, /**< vehicle inertia */ + (ParamFloat) _param_vm_inertia_yy, + (ParamFloat) _param_vm_inertia_zz, + (ParamFloat) _param_vm_inertia_xy, + (ParamFloat) _param_vm_inertia_xz, + (ParamFloat) _param_vm_inertia_yz ) matrix::Vector3f _acro_rate_max; /**< max attitude rates in acro mode */ diff --git a/src/modules/mc_rate_control/RateControl/RateControl.cpp b/src/modules/mc_rate_control/RateControl/RateControl.cpp index 830a5bf43a..d626ec8213 100644 --- a/src/modules/mc_rate_control/RateControl/RateControl.cpp +++ b/src/modules/mc_rate_control/RateControl/RateControl.cpp @@ -66,7 +66,7 @@ void RateControl::setSaturationStatus(const MultirotorMixer::saturation_status & _mixer_saturation_negative[2] = status.flags.yaw_neg; } -Vector3f RateControl::update(const Vector3f &rate, const Vector3f &rate_sp, const float dt, const bool landed) +void RateControl::update(const Vector3f &rate, const Vector3f &rate_sp, const float dt, const bool landed) { // angular rates error Vector3f rate_error = rate_sp - rate; @@ -79,9 +79,28 @@ Vector3f RateControl::update(const Vector3f &rate, const Vector3f &rate_sp, cons rate_d = (rate_filtered - _rate_prev_filtered) / dt; } - // PID control with feed forward - const Vector3f torque = _gain_p.emult(rate_error) + _rate_int - _gain_d.emult(rate_d) + _gain_ff.emult(rate_sp); + // P + D control + _angular_accel_sp = _gain_p.emult(rate_error) - _gain_d.emult(rate_d); + // I + FF control + Vector3f torque_feedforward = _rate_int + _gain_ff.emult(rate_sp); + + // compute torque setpoint + // Note: this may go into a separate module for general usage with FW and VTOLs + // if so, TODO: + // - [x] publish accel sp + // - [ ] publish torque ff sp + // - [ ] add dynamic model module + // - [ ] move params for inertia to that module + // - [ ] poll vehicle_angular_velocity & vehicle_angular_acceleration_setpoint & vehicle_torque_feedforward_setpoint => compute and publish vehicle_torque_setpoint + // - [ ] (eventually) add param for mass, poll vehicle_linear_acceleration_setpoint + vehicle_attitude => compute and publish vehicle_thrust_setpoint + _torque_sp = ( + _inertia * _angular_accel_sp + + torque_feedforward + + rate.cross(_inertia * rate) + ); + + // save states _rate_prev = rate; _rate_prev_filtered = rate_filtered; @@ -89,8 +108,6 @@ Vector3f RateControl::update(const Vector3f &rate, const Vector3f &rate_sp, cons if (!landed) { updateIntegral(rate_error, dt); } - - return torque; } void RateControl::updateIntegral(Vector3f &rate_error, const float dt) @@ -125,6 +142,15 @@ void RateControl::updateIntegral(Vector3f &rate_error, const float dt) } } +void RateControl::reset() +{ + _rate_int.zero(); + _rate_prev.zero(); + _rate_prev_filtered.zero(); + _torque_sp.zero(); + _angular_accel_sp.zero(); +} + void RateControl::getRateControlStatus(rate_ctrl_status_s &rate_ctrl_status) { rate_ctrl_status.rollspeed_integ = _rate_int(0); diff --git a/src/modules/mc_rate_control/RateControl/RateControl.hpp b/src/modules/mc_rate_control/RateControl/RateControl.hpp index ddc4fb57f5..a37e4a69f1 100644 --- a/src/modules/mc_rate_control/RateControl/RateControl.hpp +++ b/src/modules/mc_rate_control/RateControl/RateControl.hpp @@ -80,6 +80,13 @@ public: */ void setFeedForwardGain(const matrix::Vector3f &FF) { _gain_ff = FF; }; + /** + * Set inertia matrix + * @see _inertia + * @param inertia inertia matrix + */ + void setInertiaMatrix(const matrix::Matrix3f &inertia) { _inertia = inertia; }; + /** * Set saturation status * @param status message from mixer reporting about saturation @@ -91,10 +98,20 @@ public: * @param rate estimation of the current vehicle angular rate * @param rate_sp desired vehicle angular rate setpoint * @param dt desired vehicle angular rate setpoint - * @return [-1,1] normalized torque vector to apply to the vehicle */ - matrix::Vector3f update(const matrix::Vector3f &rate, const matrix::Vector3f &rate_sp, const float dt, - const bool landed); + void update(const matrix::Vector3f &rate, const matrix::Vector3f &rate_sp, const float dt, const bool landed); + + /** + * Get the desired angular acceleration + * @see _angular_accel_sp + */ + const matrix::Vector3f &getAngularAccelerationSetpoint() {return _angular_accel_sp;}; + + /** + * Get the torque vector to apply to the vehicle + * @see _torque_sp + */ + const matrix::Vector3f &getTorqueSetpoint() {return _torque_sp;}; /** * Set the integral term to 0 to prevent windup @@ -102,6 +119,11 @@ public: */ void resetIntegral() { _rate_int.zero(); } + /** + * ReSet the controller state + */ + void reset(); + /** * Get status message of controller for logging/debugging * @param rate_ctrl_status status message to fill with internal states @@ -117,6 +139,7 @@ private: matrix::Vector3f _gain_d; ///< rate control derivative gain matrix::Vector3f _lim_int; ///< integrator term maximum absolute value matrix::Vector3f _gain_ff; ///< direct rate to torque feed forward gain only useful for helicopters + matrix::Matrix3f _inertia{matrix::eye()}; ///< inertia matrix // States matrix::Vector3f _rate_prev; ///< angular rates of previous update @@ -125,4 +148,8 @@ private: math::LowPassFilter2pVector3f _lp_filters_d{0.f, 0.f}; ///< low-pass filters for D-term (roll, pitch & yaw) bool _mixer_saturation_positive[3] {}; bool _mixer_saturation_negative[3] {}; + + // Output + matrix::Vector3f _angular_accel_sp; //< Angular acceleration setpoint computed using P and D gains + matrix::Vector3f _torque_sp; //< Torque setpoint to apply to the vehicle }; diff --git a/src/modules/mc_rate_control/RateControl/RateControlTest.cpp b/src/modules/mc_rate_control/RateControl/RateControlTest.cpp index 414948e659..8e86787355 100644 --- a/src/modules/mc_rate_control/RateControl/RateControlTest.cpp +++ b/src/modules/mc_rate_control/RateControl/RateControlTest.cpp @@ -39,6 +39,7 @@ using namespace matrix; TEST(RateControlTest, AllZeroCase) { RateControl rate_control; - Vector3f torque = rate_control.update(Vector3f(), Vector3f(), 0.f, false); + rate_control.update(Vector3f(), Vector3f(), 0.f, false); + Vector3f torque = rate_control.getTorqueSetpoint(); EXPECT_EQ(torque, Vector3f()); } diff --git a/src/modules/mc_rate_control/mc_rate_control_params.c b/src/modules/mc_rate_control/mc_rate_control_params.c index 80bf264889..20235f961a 100644 --- a/src/modules/mc_rate_control/mc_rate_control_params.c +++ b/src/modules/mc_rate_control/mc_rate_control_params.c @@ -412,4 +412,3 @@ PARAM_DEFINE_INT32(MC_BAT_SCALE_EN, 0); * @group Multicopter Rate Control */ PARAM_DEFINE_FLOAT(MC_DTERM_CUTOFF, 0.f); - diff --git a/src/modules/mc_rate_control/vehicle_model_params.c b/src/modules/mc_rate_control/vehicle_model_params.c new file mode 100644 index 0000000000..cb1f08f00e --- /dev/null +++ b/src/modules/mc_rate_control/vehicle_model_params.c @@ -0,0 +1,109 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2019 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 vehicle_model_params.c + * Parameters for vehicle model. + * + * @author Julien Lecoeur + */ + +/** + * Mass + * + * @unit kg + * @decimal 5 + * @increment 0.00001 + * @group Vehicle Model + */ +PARAM_DEFINE_FLOAT(VM_MASS, 1.f); + +/** + * Inertia matrix, XX component + * + * @unit kg.m^2 + * @decimal 5 + * @increment 0.00001 + * @group Vehicle Model + */ +PARAM_DEFINE_FLOAT(VM_INERTIA_XX, 1.f); + +/** + * Inertia matrix, YY component + * + * @unit kg.m^2 + * @decimal 5 + * @increment 0.00001 + * @group Vehicle Model + */ +PARAM_DEFINE_FLOAT(VM_INERTIA_YY, 1.f); + +/** + * Inertia matrix, ZZ component + * + * @unit kg.m^2 + * @decimal 5 + * @increment 0.00001 + * @group Vehicle Model + */ +PARAM_DEFINE_FLOAT(VM_INERTIA_ZZ, 1.f); + +/** + * Inertia matrix, XY component + * + * @unit kg.m^2 + * @decimal 5 + * @increment 0.00001 + * @group Vehicle Model + */ +PARAM_DEFINE_FLOAT(VM_INERTIA_XY, 0.f); + +/** + * Inertia matrix, XZ component + * + * @unit kg.m^2 + * @decimal 5 + * @increment 0.00001 + * @group Vehicle Model + */ +PARAM_DEFINE_FLOAT(VM_INERTIA_XZ, 0.f); + +/** + * Inertia matrix, YZ component + * + * @unit kg.m^2 + * @decimal 5 + * @increment 0.00001 + * @group Vehicle Model + */ +PARAM_DEFINE_FLOAT(VM_INERTIA_YZ, 0.f);