diff --git a/src/modules/rover_pos_control/CMakeLists.txt b/src/modules/rover_pos_control/CMakeLists.txt index 5e093862e7..22ab31d2e4 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 eb3e39c0e9..45a78469ce 100644 --- a/src/modules/rover_pos_control/RoverPositionControl.cpp +++ b/src/modules/rover_pos_control/RoverPositionControl.cpp @@ -102,6 +102,10 @@ void RoverPositionControl::parameters_update(bool force) _param_speed_d.get(), _param_speed_imax.get(), _param_gndspeed_max.get()); + _rate_control.setGains(matrix::Vector3f(0.0, 0.0, _param_rate_p.get()), matrix::Vector3f(0.0, 0.0, _param_rate_i.get()), + matrix::Vector3f(0.0, 0.0, _param_rate_d.get())); + _rate_control.setFeedForwardGain(matrix::Vector3f(0.0, 0.0, _param_rate_ff.get())); + _rate_control.setIntegratorLimit(matrix::Vector3f(0.0, 0.0, _param_rate_imax.get())); } } @@ -151,6 +155,17 @@ RoverPositionControl::manual_control_setpoint_poll() _attitude_sp_pub.publish(_att_sp); + } else if (_control_mode.flag_control_rates_enabled) { + // STABILIZED mode generate the attitude setpoint from manual user inputs + _rates_sp.roll = 0.0; + _rates_sp.pitch = 0.0; + _rates_sp.yaw = _manual_control_setpoint.y; + _rates_sp.thrust_body[0] = _manual_control_setpoint.z; + + _rates_sp.timestamp = hrt_absolute_time(); + + _rates_sp_pub.publish(_rates_sp); + } else { _act_controls.control[actuator_controls_s::INDEX_ROLL] = 0.0f; // Nominally roll: _manual_control_setpoint.y; _act_controls.control[actuator_controls_s::INDEX_PITCH] = 0.0f; // Nominally pitch: -_manual_control_setpoint.x; @@ -187,6 +202,14 @@ RoverPositionControl::attitude_setpoint_poll() } } +void +RoverPositionControl::rates_setpoint_poll() +{ + if (_rates_sp_sub.updated()) { + _rates_sp_sub.copy(&_rates_sp); + } +} + void RoverPositionControl::vehicle_attitude_poll() { @@ -195,6 +218,14 @@ RoverPositionControl::vehicle_attitude_poll() } } +void +RoverPositionControl::vehicle_angular_acceleration_poll() +{ + if (_vehicle_angular_acceleration_sub.updated()) { + _vehicle_angular_acceleration_sub.copy(&_vehicle_angular_acceleration); + } +} + bool RoverPositionControl::control_position(const matrix::Vector2d ¤t_position, const matrix::Vector3f &ground_speed, const position_setpoint_triplet_s &pos_sp_triplet) @@ -380,31 +411,59 @@ 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; + // PX4_INFO("Yaw error: %f", double(euler_sp(2))); - float control_effort = euler_sp(2) / _param_max_turn_angle.get(); - control_effort = math::constrain(control_effort, -1.0f, 1.0f); + _rates_sp.roll = 0.0; + _rates_sp.pitch = 0.0; + _rates_sp.yaw = euler_sp(2) / _param_max_turn_angle.get(); + _rates_sp.thrust_body[0] = math::constrain(att_sp.thrust_body[0], 0.0f, 1.0f); + _rates_sp.timestamp = hrt_absolute_time(); + _rates_sp_pub.publish(_rates_sp); + control_rates(_vehicle_rates, _vehicle_angular_acceleration, _local_pos, _rates_sp); +} - _act_controls.control[actuator_controls_s::INDEX_YAW] = control_effort; +void +RoverPositionControl::control_rates(const vehicle_angular_velocity_s &rates, const vehicle_angular_acceleration_s &acc, + const vehicle_local_position_s &local_pos, + const vehicle_rates_setpoint_s &rates_sp) +{ + float dt = 0.01; // Using non zero value to a avoid division by zero - const float control_throttle = att_sp.thrust_body[0]; + if (_control_rates_last_called > 0) { + dt = hrt_elapsed_time(&_control_rates_last_called) * 1e-6f; + } + + const matrix::Vector3f current_velocity(local_pos.vx, local_pos.vy, local_pos.vz); + const matrix::Vector3f vehicle_rates(rates.xyz[0], rates.xyz[1], rates.xyz[2]); + const matrix::Vector3f rates_setpoint(rates_sp.roll, rates_sp.pitch, rates_sp.yaw); + const matrix::Vector3f angular_acceleration{acc.xyz}; + bool lock_integrator = bool(current_velocity.norm() < _param_rate_i_minspeed.get()); + const matrix::Vector3f torque = _rate_control.update(vehicle_rates, rates_setpoint, angular_acceleration, dt, + lock_integrator); + + _steering_input = math::constrain(_steering_input + torque(2), -1.0f, 1.0f); + + _act_controls.control[actuator_controls_s::INDEX_YAW] = _steering_input; + + 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() { parameters_update(true); /* run controller on gyro changes */ - vehicle_angular_velocity_s angular_velocity; - - if (_vehicle_angular_velocity_sub.update(&angular_velocity)) { - + if (_vehicle_angular_velocity_sub.update(&_vehicle_rates)) { + // grab corresponding vehicle_angular_acceleration immediately after vehicle_angular_velocity copy + vehicle_angular_acceleration_poll(); /* check vehicle control mode for changes to publication state */ vehicle_control_mode_poll(); attitude_setpoint_poll(); + rates_setpoint_poll(); vehicle_attitude_poll(); manual_control_setpoint_poll(); @@ -413,6 +472,10 @@ RoverPositionControl::Run() /* update parameters from storage */ parameters_update(); + if (!is_finite(_steering_input)) { + _steering_input = 0.0; + } + /* only run controller if position changed */ if (_local_pos_sub.update(&_local_pos)) { @@ -492,6 +555,14 @@ RoverPositionControl::Run() } + if (_control_mode.flag_control_rates_enabled + && !_control_mode.flag_control_attitude_enabled + && !_control_mode.flag_control_position_enabled + && !_control_mode.flag_control_velocity_enabled) { + //Body Rate control + control_rates(_vehicle_rates, _vehicle_angular_acceleration, _local_pos, _rates_sp); + } + /* Only publish if any of the proper modes are enabled */ if (_control_mode.flag_control_velocity_enabled || _control_mode.flag_control_attitude_enabled || diff --git a/src/modules/rover_pos_control/RoverPositionControl.hpp b/src/modules/rover_pos_control/RoverPositionControl.hpp index 6d1e293d73..5df8338311 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 @@ -67,6 +68,9 @@ #include #include #include +#include +#include +#include #include #include #include @@ -106,6 +110,7 @@ private: uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + uORB::Publication _rates_sp_pub{ORB_ID(vehicle_rates_setpoint)}; uORB::Publication _attitude_sp_pub{ORB_ID(vehicle_attitude_setpoint)}; uORB::Publication _pos_ctrl_status_pub{ORB_ID(position_controller_status)}; /**< navigation capabilities publication */ uORB::Publication _actuator_controls_pub{ORB_ID(actuator_controls_0)}; /**< actuator controls publication */ @@ -118,10 +123,12 @@ private: uORB::Subscription _att_sub{ORB_ID(vehicle_attitude)}; uORB::Subscription _att_sp_sub{ORB_ID(vehicle_attitude_setpoint)}; uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)}; - + uORB::Subscription _rates_sp_sub{ORB_ID(vehicle_rates_setpoint)}; + uORB::Subscription _vehicle_angular_acceleration_sub{ORB_ID(vehicle_angular_acceleration)}; 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 */ @@ -136,6 +143,7 @@ private: perf_counter_t _loop_perf; /**< loop performance counter */ hrt_abstime _control_position_last_called{0}; /**) _param_speed_imax, (ParamFloat) _param_throttle_speed_scaler, + (ParamFloat) _param_rate_p, + (ParamFloat) _param_rate_i, + (ParamFloat) _param_rate_d, + (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, (ParamFloat) _param_wheel_base, (ParamFloat) _param_max_turn_angle, + (ParamFloat) _param_att_p, (ParamFloat) _param_gnd_man_y_max, (ParamFloat) _param_nav_loiter_rad /**< loiter radius for Rover */ ) @@ -204,8 +223,10 @@ private: 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_acceleration_poll(); void manual_control_setpoint_poll(); /** @@ -215,5 +236,8 @@ private: const position_setpoint_triplet_s &_pos_sp_triplet); void control_velocity(const matrix::Vector3f ¤t_velocity); 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 vehicle_angular_acceleration_s &acc, + const vehicle_local_position_s &local_pos, + 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 0000000000..9025c96a1c --- /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 RoverRateControl) 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 0000000000..df13da5dd6 --- /dev/null +++ b/src/modules/rover_pos_control/RoverRateControl/RoverRateControl.cpp @@ -0,0 +1,104 @@ +/**************************************************************************** + * + * 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; +} + +Vector3f RoverRateControl::update(const Vector3f &rate, const Vector3f &rate_sp, const Vector3f &angular_accel, + const float dt, const bool landed) +{ + // 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 0000000000..83428a1d13 --- /dev/null +++ b/src/modules/rover_pos_control/RoverRateControl/RoverRateControl.hpp @@ -0,0 +1,110 @@ +/**************************************************************************** + * + * Copyright (c) 2019-2021 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 + +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; }; + + /** + * 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 0000000000..9bae5a64a3 --- /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 6458fb1fea..d3e5f61e2e 100644 --- a/src/modules/rover_pos_control/rover_pos_control_params.c +++ b/src/modules/rover_pos_control/rover_pos_control_params.c @@ -275,6 +275,19 @@ PARAM_DEFINE_FLOAT(GND_SPEED_MAX, 10.0f); */ PARAM_DEFINE_FLOAT(GND_MAX_ANG, 0.7854f); +/** + * Attitude control P gain + * + * + * @unit rad + * @min 0.0 + * @max 5.0 + * @decimal 3 + * @increment 0.01 + * @group Rover Position Control + */ +PARAM_DEFINE_FLOAT(GND_ATT_P, 1.0f); + /** * Max manual yaw rate * @@ -285,3 +298,87 @@ PARAM_DEFINE_FLOAT(GND_MAX_ANG, 0.7854f); * @group Rover Position Control */ PARAM_DEFINE_FLOAT(GND_MAN_Y_MAX, 150.0f); + +/** + * 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 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_D, 0.0f); + +/** + * 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 + * + * @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);