From d9754a9a9d3f8491f268510aeaee05b4e1bc498f Mon Sep 17 00:00:00 2001 From: Jaeyoung-Lim Date: Sun, 17 Jan 2021 21:52:49 +0100 Subject: [PATCH] Add support for rover bodyrate setpoints --- src/modules/rover_pos_control/CMakeLists.txt | 4 + .../RoverPositionControl.cpp | 91 +++++++++- .../RoverPositionControl.hpp | 21 ++- .../RoverRateControl/CMakeLists.txt | 42 +++++ .../RoverRateControl/RoverRateControl.cpp | 168 ++++++++++++++++++ .../RoverRateControl/RoverRateControl.hpp | 118 ++++++++++++ .../RoverRateControl/RoverRateControlTest.cpp | 44 +++++ .../rover_pos_control_params.c | 73 ++++++++ 8 files changed, 556 insertions(+), 5 deletions(-) create mode 100644 src/modules/rover_pos_control/RoverRateControl/CMakeLists.txt create mode 100644 src/modules/rover_pos_control/RoverRateControl/RoverRateControl.cpp create mode 100644 src/modules/rover_pos_control/RoverRateControl/RoverRateControl.hpp create mode 100644 src/modules/rover_pos_control/RoverRateControl/RoverRateControlTest.cpp diff --git a/src/modules/rover_pos_control/CMakeLists.txt b/src/modules/rover_pos_control/CMakeLists.txt index 5e093862e768..22ab31d2e4cd 100644 --- a/src/modules/rover_pos_control/CMakeLists.txt +++ b/src/modules/rover_pos_control/CMakeLists.txt @@ -30,6 +30,9 @@ # POSSIBILITY OF SUCH DAMAGE. # ############################################################################ + +add_subdirectory(RoverRateControl) + px4_add_module( MODULE modules__rover_pos_control MAIN rover_pos_control @@ -37,6 +40,7 @@ px4_add_module( RoverPositionControl.cpp RoverPositionControl.hpp DEPENDS + RoverRateControl l1 pid ) diff --git a/src/modules/rover_pos_control/RoverPositionControl.cpp b/src/modules/rover_pos_control/RoverPositionControl.cpp index a19d9b19895f..4b278fb1bfe2 100644 --- a/src/modules/rover_pos_control/RoverPositionControl.cpp +++ b/src/modules/rover_pos_control/RoverPositionControl.cpp @@ -90,6 +90,12 @@ void RoverPositionControl::parameters_update(bool force) _param_speed_d.get(), _param_speed_imax.get(), _param_gndspeed_max.get()); + + // _rate_control.set_k_p(_param_rate_p.get()); + // _rate_control.set_k_i(_param_rate_i.get()); + // _rate_control.set_k_ff(_param_rate_ff.get()); + // _rate_control.set_integrator_max(_param_rate_imax.get()); + // _rate_control.set_max_rate(_param_rate_max.get()); } } @@ -148,6 +154,28 @@ RoverPositionControl::vehicle_attitude_poll() } } +void +RoverPositionControl::rates_setpoint_poll() +{ + bool rates_sp_updated; + orb_check(_rates_sp_sub, &rates_sp_updated); + + if (rates_sp_updated) { + orb_copy(ORB_ID(vehicle_rates_setpoint), _rates_sp_sub, &_rates_sp); + } +} + +void +RoverPositionControl::vehicle_angular_velocity_poll() +{ + bool rates_updated; + orb_check(_vehicle_angular_velocity_sub, &rates_updated); + + if (rates_updated) { + orb_copy(ORB_ID(vehicle_angular_velocity), _vehicle_angular_velocity_sub, &_vehicle_rates); + } +} + bool RoverPositionControl::control_position(const matrix::Vector2f ¤t_position, const matrix::Vector3f &ground_speed, const position_setpoint_triplet_s &pos_sp_triplet) @@ -334,7 +362,12 @@ RoverPositionControl::control_attitude(const vehicle_attitude_s &att, const vehi // quaternion attitude control law, qe is rotation from q to qd const Quatf qe = Quatf(att.q).inversed() * Quatf(att_sp.q_d); const Eulerf euler_sp = qe; - + //TODO: Switch to rate controller + // struct ECL_ControlData control_input = {}; + // control_input.yaw = euler_sp(2); + // _rate_control.control_attitude(control_input); + // control_input.yaw_rate_setpoint = _rate_control.get_desired_rate(); + // float control_effort = att_control.control_bodyrate(control_input); float control_effort = euler_sp(2) / _param_max_turn_angle.get(); control_effort = math::constrain(control_effort, -1.0f, 1.0f); @@ -346,15 +379,44 @@ RoverPositionControl::control_attitude(const vehicle_attitude_s &att, const vehi } +void +RoverPositionControl::control_rates(const vehicle_angular_velocity_s &rates, const matrix::Vector3f ¤t_velocity, + const vehicle_rates_setpoint_s &rates_sp) +{ + // struct ECL_ControlData control_input = {}; + // const float current_speed = current_velocity.norm(); + + // Set scaling factor with local velocity + // control_input.groundspeed = current_speed; + // // Lock integrator when local velocity is small + // control_input.lock_integrator = (current_speed < _param_rate_i_minspeed.get()); + + // control_input.body_z_rate = rates.xyz[2]; + // _rate_control.set_bodyrate_setpoint(rates_sp.yaw); + + float control_effort = 0.0f; + // float control_effort = _rate_control.control_bodyrate(control_input); + control_effort = math::constrain(control_effort, -1.0f, 1.0f); + + _act_controls.control[actuator_controls_s::INDEX_YAW] = control_effort; + + const float control_throttle = rates_sp.thrust_body[0]; + + _act_controls.control[actuator_controls_s::INDEX_THROTTLE] = math::constrain(control_throttle, 0.0f, 1.0f); +} + + void RoverPositionControl::run() { _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); _local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); + _vehicle_angular_velocity_sub = orb_subscribe(ORB_ID(vehicle_angular_velocity)); _manual_control_setpoint_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); _pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); _att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); + _rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); _vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); _sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); @@ -369,7 +431,7 @@ RoverPositionControl::run() parameters_update(true); /* wakeup source(s) */ - px4_pollfd_struct_t fds[5]; + px4_pollfd_struct_t fds[6]; /* Setup of loop */ fds[0].fd = _global_pos_sub; @@ -382,6 +444,8 @@ RoverPositionControl::run() fds[3].events = POLLIN; fds[4].fd = _local_pos_sub; // Added local position as source of position fds[4].events = POLLIN; + fds[5].fd = _vehicle_angular_velocity_sub; // Added local position as source of position + fds[5].events = POLLIN; while (!should_exit()) { @@ -397,6 +461,8 @@ RoverPositionControl::run() /* check vehicle control mode for changes to publication state */ vehicle_control_mode_poll(); attitude_setpoint_poll(); + rates_setpoint_poll(); + vehicle_angular_velocity_poll(); //manual_control_setpoint_poll(); _vehicle_acceleration_sub.update(); @@ -480,14 +546,28 @@ RoverPositionControl::run() vehicle_attitude_poll(); - if (!manual_mode && _control_mode.flag_control_attitude_enabled + if (manual_mode && _control_mode.flag_control_attitude_enabled && !_control_mode.flag_control_position_enabled && !_control_mode.flag_control_velocity_enabled) { - control_attitude(_vehicle_att, _att_sp); + PX4_INFO("Control Attitude"); + + } + } + + if (fds[5].revents & POLLIN) { + vehicle_angular_velocity_poll(); + if (!manual_mode && _control_mode.flag_control_rates_enabled + && !_control_mode.flag_control_attitude_enabled + && !_control_mode.flag_control_position_enabled + && !_control_mode.flag_control_velocity_enabled) { + //Offboard rate control + matrix::Vector3f vehicle_velocity(_local_pos.vx, _local_pos.vy, _local_pos.vz); + control_rates(_vehicle_rates, vehicle_velocity, _rates_sp); } + //TODO: Add stabilized mode for rovers } if (fds[1].revents & POLLIN) { @@ -517,6 +597,7 @@ RoverPositionControl::run() if (_control_mode.flag_control_velocity_enabled || _control_mode.flag_control_attitude_enabled || _control_mode.flag_control_position_enabled || + _control_mode.flag_control_rates_enabled || manual_mode) { /* publish the actuator controls */ _actuator_controls_pub.publish(_act_controls); @@ -530,7 +611,9 @@ RoverPositionControl::run() orb_unsubscribe(_local_pos_sub); orb_unsubscribe(_manual_control_setpoint_sub); orb_unsubscribe(_pos_sp_triplet_sub); + orb_unsubscribe(_rates_sp_sub); orb_unsubscribe(_vehicle_attitude_sub); + orb_unsubscribe(_vehicle_angular_velocity_sub); orb_unsubscribe(_sensor_combined_sub); warnx("exiting.\n"); diff --git a/src/modules/rover_pos_control/RoverPositionControl.hpp b/src/modules/rover_pos_control/RoverPositionControl.hpp index 2795205beea3..9c50350942f0 100644 --- a/src/modules/rover_pos_control/RoverPositionControl.hpp +++ b/src/modules/rover_pos_control/RoverPositionControl.hpp @@ -41,8 +41,9 @@ * @author Marco Zorzi */ -#include +#include +#include #include #include #include @@ -65,6 +66,8 @@ #include #include #include +#include +#include #include #include #include @@ -108,7 +111,9 @@ class RoverPositionControl : public ModuleBase, public Mod int _manual_control_setpoint_sub{-1}; /**< notification of manual control updates */ int _pos_sp_triplet_sub{-1}; int _att_sp_sub{-1}; + int _rates_sp_sub{-1}; int _vehicle_attitude_sub{-1}; + int _vehicle_angular_velocity_sub{-1}; int _sensor_combined_sub{-1}; uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; @@ -116,11 +121,13 @@ class RoverPositionControl : public ModuleBase, public Mod manual_control_setpoint_s _manual_control_setpoint{}; /**< r/c channel data */ position_setpoint_triplet_s _pos_sp_triplet{}; /**< triplet of mission items */ vehicle_attitude_setpoint_s _att_sp{}; /**< attitude setpoint > */ + vehicle_rates_setpoint_s _rates_sp{}; /**< rate setpoint > */ vehicle_control_mode_s _control_mode{}; /**< control mode */ vehicle_global_position_s _global_pos{}; /**< global vehicle position */ vehicle_local_position_s _local_pos{}; /**< global vehicle position */ actuator_controls_s _act_controls{}; /**< direct control of actuators */ vehicle_attitude_s _vehicle_att{}; + vehicle_angular_velocity_s _vehicle_rates{}; sensor_combined_s _sensor_combined{}; uORB::SubscriptionData _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)}; @@ -137,6 +144,7 @@ class RoverPositionControl : public ModuleBase, public Mod uint8_t _pos_reset_counter{0}; // captures the number of times the estimator has reset the horizontal position ECL_L1_Pos_Controller _gnd_control; + RoverRateControl _rate_control; enum UGV_POSCTRL_MODE { UGV_POSCTRL_MODE_AUTO, @@ -167,6 +175,13 @@ class RoverPositionControl : public ModuleBase, public Mod (ParamFloat) _param_speed_imax, (ParamFloat) _param_throttle_speed_scaler, + (ParamFloat) _param_rate_p, + (ParamFloat) _param_rate_i, + (ParamFloat) _param_rate_ff, + (ParamFloat) _param_rate_imax, + (ParamFloat) _param_rate_max, + (ParamFloat) _param_rate_i_minspeed, + (ParamFloat) _param_throttle_min, (ParamFloat) _param_throttle_max, (ParamFloat) _param_throttle_cruise, @@ -184,8 +199,10 @@ class RoverPositionControl : public ModuleBase, public Mod void manual_control_setpoint_poll(); void position_setpoint_triplet_poll(); void attitude_setpoint_poll(); + void rates_setpoint_poll(); void vehicle_control_mode_poll(); void vehicle_attitude_poll(); + void vehicle_angular_velocity_poll(); /** * Control position. @@ -194,5 +211,7 @@ class RoverPositionControl : public ModuleBase, public Mod const position_setpoint_triplet_s &_pos_sp_triplet); void control_velocity(const matrix::Vector3f ¤t_velocity, const position_setpoint_triplet_s &pos_sp_triplet); void control_attitude(const vehicle_attitude_s &att, const vehicle_attitude_setpoint_s &att_sp); + void control_rates(const vehicle_angular_velocity_s &rates, const matrix::Vector3f ¤t_velocity, + const vehicle_rates_setpoint_s &rates_sp); }; diff --git a/src/modules/rover_pos_control/RoverRateControl/CMakeLists.txt b/src/modules/rover_pos_control/RoverRateControl/CMakeLists.txt new file mode 100644 index 000000000000..946f86fe0899 --- /dev/null +++ b/src/modules/rover_pos_control/RoverRateControl/CMakeLists.txt @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (c) 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. +# +############################################################################ + +px4_add_library(RoverRateControl + RoverRateControl.cpp + RoverRateControl.hpp +) +target_compile_options(RoverRateControl PRIVATE ${MAX_CUSTOM_OPT_LEVEL}) +target_include_directories(RoverRateControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) +target_link_libraries(RoverRateControl PRIVATE mathlib) + +px4_add_unit_gtest(SRC RoverRateControlTest.cpp LINKLIBS RateControl) diff --git a/src/modules/rover_pos_control/RoverRateControl/RoverRateControl.cpp b/src/modules/rover_pos_control/RoverRateControl/RoverRateControl.cpp new file mode 100644 index 000000000000..80ea7826cc80 --- /dev/null +++ b/src/modules/rover_pos_control/RoverRateControl/RoverRateControl.cpp @@ -0,0 +1,168 @@ +/**************************************************************************** + * + * Copyright (c) 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 RoverRateControl.cpp + */ + +#include +#include + +using namespace matrix; + +void RoverRateControl::setGains(const Vector3f &P, const Vector3f &I, const Vector3f &D) +{ + _gain_p = P; + _gain_i = I; + _gain_d = D; +} + +void RoverRateControl::setSaturationStatus(const MultirotorMixer::saturation_status &status) +{ + _mixer_saturation_positive[0] = status.flags.roll_pos; + _mixer_saturation_positive[1] = status.flags.pitch_pos; + _mixer_saturation_positive[2] = status.flags.yaw_pos; + _mixer_saturation_negative[0] = status.flags.roll_neg; + _mixer_saturation_negative[1] = status.flags.pitch_neg; + _mixer_saturation_negative[2] = status.flags.yaw_neg; +} + +Vector3f RoverRateControl::update(const Vector3f &rate, const Vector3f &rate_sp, const Vector3f &angular_accel, + const float dt, const bool landed) +{ + // /* Do not calculate control signal with bad inputs */ + // if (!(PX4_ISFINITE(ctl_data.body_z_rate))) { + + // return math::constrain(_last_output, -1.0f, 1.0f); + // } + + // /* get the usual dt estimate */ + // uint64_t dt_micros = hrt_elapsed_time(&_last_run); + // _last_run = hrt_absolute_time(); + // float dt = (float)dt_micros * 1e-6f; + + // /* lock integral for long intervals */ + // bool lock_integrator = ctl_data.lock_integrator; + + // if (dt_micros > 500000) { + // lock_integrator = true; + // } + + // //TODO: Handle differential rover and ackerman differently + // // A. Integrator should not integrate if there is no groundspeed for ackerman + // // B. speed scaling should not be done in differential rovers + + // //Only using vehicle kinematiccs, without considering tire slip + + // /* Calculate body angular rate error */ + // _rate_error = _bodyrate_setpoint - ctl_data.body_z_rate; // body angular rate error + + // float scaler = math::max(ctl_data.groundspeed, 0.1f); + + // if (!lock_integrator && _k_i > 0.0f) { + + // float id = _rate_error * dt; + + // /* + // * anti-windup: do not allow integrator to increase if actuator is at limit + // */ + // if (_last_output < -1.0f) { + // /* only allow motion to center: increase value */ + // id = math::max(id, 0.0f); + + // } else if (_last_output > 1.0f) { + // /* only allow motion to center: decrease value */ + // id = math::min(id, 0.0f); + // } + + // /* add and constrain */ + // _integrator = math::constrain(_integrator + id * _k_i, -_integrator_max, _integrator_max); + // } + + // /* Apply PI rate controller and store non-limited output */ + // _last_output = (_bodyrate_setpoint * _k_ff + _rate_error * _k_p) / scaler + _integrator; + // _last_output = math::constrain(_last_output, -1.0f, 1.0f); + // return _last_output; + + // angular rates error + Vector3f rate_error = rate_sp - rate; + + // PID control with feed forward + const Vector3f torque = _gain_p.emult(rate_error) + _rate_int - _gain_d.emult(angular_accel) + _gain_ff.emult(rate_sp); + + // update integral only if we are not landed + if (!landed) { + updateIntegral(rate_error, dt); + } + + return torque; +} + +void RoverRateControl::updateIntegral(Vector3f &rate_error, const float dt) +{ + for (int i = 0; i < 3; i++) { + // prevent further positive control saturation + if (_mixer_saturation_positive[i]) { + rate_error(i) = math::min(rate_error(i), 0.f); + } + + // prevent further negative control saturation + if (_mixer_saturation_negative[i]) { + rate_error(i) = math::max(rate_error(i), 0.f); + } + + // I term factor: reduce the I gain with increasing rate error. + // This counteracts a non-linear effect where the integral builds up quickly upon a large setpoint + // change (noticeable in a bounce-back effect after a flip). + // The formula leads to a gradual decrease w/o steps, while only affecting the cases where it should: + // with the parameter set to 400 degrees, up to 100 deg rate error, i_factor is almost 1 (having no effect), + // and up to 200 deg error leads to <25% reduction of I. + float i_factor = rate_error(i) / math::radians(400.f); + i_factor = math::max(0.0f, 1.f - i_factor * i_factor); + + // Perform the integration using a first order method + float rate_i = _rate_int(i) + i_factor * _gain_i(i) * rate_error(i) * dt; + + // do not propagate the result if out of range or invalid + if (PX4_ISFINITE(rate_i)) { + _rate_int(i) = math::constrain(rate_i, -_lim_int(i), _lim_int(i)); + } + } +} + +void RoverRateControl::getRoverRateControlStatus(rate_ctrl_status_s &rate_ctrl_status) +{ + rate_ctrl_status.rollspeed_integ = _rate_int(0); + rate_ctrl_status.pitchspeed_integ = _rate_int(1); + rate_ctrl_status.yawspeed_integ = _rate_int(2); +} diff --git a/src/modules/rover_pos_control/RoverRateControl/RoverRateControl.hpp b/src/modules/rover_pos_control/RoverRateControl/RoverRateControl.hpp new file mode 100644 index 000000000000..97637147c9ba --- /dev/null +++ b/src/modules/rover_pos_control/RoverRateControl/RoverRateControl.hpp @@ -0,0 +1,118 @@ +/**************************************************************************** + * + * Copyright (c) 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 RoverRateControl.hpp + * + * PID 3 axis angular rate / angular velocity control. + */ + +#pragma once + +#include +#include + +#include +#include + +class RoverRateControl +{ +public: + RoverRateControl() = default; + ~RoverRateControl() = default; + + /** + * Set the rate control gains + * @param P 3D vector of proportional gains for body x,y,z axis + * @param I 3D vector of integral gains + * @param D 3D vector of derivative gains + */ + void setGains(const matrix::Vector3f &P, const matrix::Vector3f &I, const matrix::Vector3f &D); + + /** + * Set the mximum absolute value of the integrator for all axes + * @param integrator_limit limit value for all axes x, y, z + */ + void setIntegratorLimit(const matrix::Vector3f &integrator_limit) { _lim_int = integrator_limit; }; + + /** + * Set direct rate to torque feed forward gain + * @see _gain_ff + * @param FF 3D vector of feed forward gains for body x,y,z axis + */ + void setFeedForwardGain(const matrix::Vector3f &FF) { _gain_ff = FF; }; + + /** + * Set saturation status + * @param status message from mixer reporting about saturation + */ + void setSaturationStatus(const MultirotorMixer::saturation_status &status); + + /** + * Run one control loop cycle calculation + * @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 matrix::Vector3f &angular_accel, const float dt, const bool landed); + + /** + * Set the integral term to 0 to prevent windup + * @see _rate_int + */ + void resetIntegral() { _rate_int.zero(); } + + /** + * Get status message of controller for logging/debugging + * @param rate_ctrl_status status message to fill with internal states + */ + void getRoverRateControlStatus(rate_ctrl_status_s &rate_ctrl_status); + +private: + void updateIntegral(matrix::Vector3f &rate_error, const float dt); + + // Gains + matrix::Vector3f _gain_p; ///< rate control proportional gain for all axes x, y, z + matrix::Vector3f _gain_i; ///< rate control integral gain + 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 + + // States + matrix::Vector3f _rate_int; ///< integral term of the rate controller + + bool _mixer_saturation_positive[3] {}; + bool _mixer_saturation_negative[3] {}; +}; diff --git a/src/modules/rover_pos_control/RoverRateControl/RoverRateControlTest.cpp b/src/modules/rover_pos_control/RoverRateControl/RoverRateControlTest.cpp new file mode 100644 index 000000000000..9bae5a64a378 --- /dev/null +++ b/src/modules/rover_pos_control/RoverRateControl/RoverRateControlTest.cpp @@ -0,0 +1,44 @@ +/**************************************************************************** + * + * Copyright (C) 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. + * + ****************************************************************************/ + +#include +#include + +using namespace matrix; + +TEST(RoverRateControlTest, AllZeroCase) +{ + RoverRateControl rate_control; + Vector3f torque = rate_control.update(Vector3f(), Vector3f(), Vector3f(), 0.f, false); + EXPECT_EQ(torque, Vector3f()); +} diff --git a/src/modules/rover_pos_control/rover_pos_control_params.c b/src/modules/rover_pos_control/rover_pos_control_params.c index cc3b639b12c8..b15fff3e9ec0 100644 --- a/src/modules/rover_pos_control/rover_pos_control_params.c +++ b/src/modules/rover_pos_control/rover_pos_control_params.c @@ -286,3 +286,76 @@ PARAM_DEFINE_FLOAT(GND_WHEEL_BASE, 2.0f); * @group Rover Position Control */ PARAM_DEFINE_FLOAT(GND_MAX_ANG, 0.7854f); + +/** + * Rover Rate Proportional Gain + * + * @unit rad/s + * @min 0.0 + * @max 10.0 + * @decimal 3 + * @increment 0.01 + * @group Rover Position Control + */ +PARAM_DEFINE_FLOAT(GND_RATE_P, 5.0f); + +/** + * Rover Rate Integral Gain + * + * @unit rad/s + * @min 0.0 + * @max 1.0 + * @decimal 3 + * @increment 0.005 + * @group Rover Position Control + */ +PARAM_DEFINE_FLOAT(GND_RATE_I, 0.5f); + +/** + * Rover Rate Proportional Gain + * + * @unit rad/s + * @min 0.0 + * @max 10.0 + * @decimal 3 + * @increment 0.01 + * @group Rover Position Control + */ +PARAM_DEFINE_FLOAT(GND_RATE_FF, 1.6f); + +/** + * Rover Rate Maximum Integral Gain + * + * @unit rad/s + * @min 0.0 + * @max 50.0 + * @decimal 3 + * @increment 0.005 + * @group Rover Position Control + */ +PARAM_DEFINE_FLOAT(GND_RATE_IMAX, 1.0f); + +/** + * Rover Rate Maximum Rate + * + * @unit rad/s + * @min 0.0 + * @max 50.0 + * @decimal 3 + * @increment 0.005 + * @group Rover Position Control + */ +PARAM_DEFINE_FLOAT(GND_RATE_MAX, 5.0f); + +/** + * Rover Rate Integral Minimum speed + * Rate integral is locked below this speed + * + * @unit m/s + * @min 0.0 + * @max 50.0 + * @decimal 3 + * @increment 0.005 + * @group Rover Position Control + */ +PARAM_DEFINE_FLOAT(GND_RATE_IMINSPD, 0.3f);