diff --git a/src/lib/pid/CMakeLists.txt b/src/lib/pid/CMakeLists.txt index 044e1b39b4..8adba376b8 100644 --- a/src/lib/pid/CMakeLists.txt +++ b/src/lib/pid/CMakeLists.txt @@ -38,5 +38,3 @@ px4_add_library(PID target_include_directories(PID PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) px4_add_unit_gtest(SRC PIDTest.cpp LINKLIBS PID) - -px4_add_library(pid pid.cpp) # TODO: remove deprecated pid library diff --git a/src/lib/pid/pid.cpp b/src/lib/pid/pid.cpp deleted file mode 100644 index 076c1afeed..0000000000 --- a/src/lib/pid/pid.cpp +++ /dev/null @@ -1,185 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. - * Author: Laurens Mackay - * Tobias Naegeli - * Martin Rutschmann - * Anton Babushkin - * Julian Oes - * - * 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 pid.cpp - * - * Implementation of generic PID controller. - * - * @author Laurens Mackay - * @author Tobias Naegeli - * @author Martin Rutschmann - * @author Anton Babushkin - * @author Julian Oes - */ - -#include "pid.h" -#include -#include - -#define SIGMA 0.000001f - -__EXPORT void pid_init(PID_t *pid, pid_mode_t mode, float dt_min) -{ - pid->mode = mode; - pid->dt_min = dt_min; - pid->kp = 0.0f; - pid->ki = 0.0f; - pid->kd = 0.0f; - pid->integral = 0.0f; - pid->integral_limit = 0.0f; - pid->output_limit = 0.0f; - pid->error_previous = 0.0f; - pid->last_output = 0.0f; -} - -__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float integral_limit, float output_limit) -{ - int ret = 0; - - if (PX4_ISFINITE(kp)) { - pid->kp = kp; - - } else { - ret = 1; - } - - if (PX4_ISFINITE(ki)) { - pid->ki = ki; - - } else { - ret = 1; - } - - if (PX4_ISFINITE(kd)) { - pid->kd = kd; - - } else { - ret = 1; - } - - if (PX4_ISFINITE(integral_limit)) { - pid->integral_limit = integral_limit; - - } else { - ret = 1; - } - - if (PX4_ISFINITE(output_limit)) { - pid->output_limit = output_limit; - - } else { - ret = 1; - } - - return ret; -} - -__EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt) -{ - if (!PX4_ISFINITE(sp) || !PX4_ISFINITE(val) || !PX4_ISFINITE(val_dot) || !PX4_ISFINITE(dt)) { - return pid->last_output; - } - - float i, d; - - /* current error value */ - float error = sp - val; - - /* current error derivative */ - if (pid->mode == PID_MODE_DERIVATIV_CALC) { - d = (error - pid->error_previous) / fmaxf(dt, pid->dt_min); - pid->error_previous = error; - - } else if (pid->mode == PID_MODE_DERIVATIV_CALC_NO_SP) { - d = (-val - pid->error_previous) / fmaxf(dt, pid->dt_min); - pid->error_previous = -val; - - } else if (pid->mode == PID_MODE_DERIVATIV_SET) { - d = -val_dot; - - } else { - d = 0.0f; - } - - if (!PX4_ISFINITE(d)) { - d = 0.0f; - } - - /* calculate PD output */ - float output = (error * pid->kp) + (d * pid->kd); - - if (pid->ki > SIGMA) { - // Calculate the error integral and check for saturation - i = pid->integral + (error * dt); - - /* check for saturation */ - if (PX4_ISFINITE(i)) { - if ((pid->output_limit < SIGMA || (fabsf(output + (i * pid->ki)) <= pid->output_limit)) && - fabsf(i) <= pid->integral_limit) { - /* not saturated, use new integral value */ - pid->integral = i; - } - } - - /* add I component to output */ - output += pid->integral * pid->ki; - } - - /* limit output */ - if (PX4_ISFINITE(output)) { - if (pid->output_limit > SIGMA) { - if (output > pid->output_limit) { - output = pid->output_limit; - - } else if (output < -pid->output_limit) { - output = -pid->output_limit; - } - } - - pid->last_output = output; - } - - return pid->last_output; -} - - -__EXPORT void pid_reset_integral(PID_t *pid) -{ - pid->integral = 0.0f; -} diff --git a/src/lib/pid/pid.h b/src/lib/pid/pid.h deleted file mode 100644 index e8b1aac4fd..0000000000 --- a/src/lib/pid/pid.h +++ /dev/null @@ -1,91 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. - * Author: Laurens Mackay - * Tobias Naegeli - * Martin Rutschmann - * Anton Babushkin - * Julian Oes - * - * 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 pid.h - * - * Definition of generic PID controller. - * - * @author Laurens Mackay - * @author Tobias Naegeli - * @author Martin Rutschmann - * @author Anton Babushkin - * @author Julian Oes - */ - -#ifndef PID_H_ -#define PID_H_ - -#include - -__BEGIN_DECLS - -typedef enum PID_MODE { - /* Use PID_MODE_DERIVATIV_NONE for a PI controller (vs PID) */ - PID_MODE_DERIVATIV_NONE = 0, - /* PID_MODE_DERIVATIV_CALC calculates discrete derivative from previous error, - * val_dot in pid_calculate() will be ignored */ - PID_MODE_DERIVATIV_CALC, - /* PID_MODE_DERIVATIV_CALC_NO_SP calculates discrete derivative from previous value, - * setpoint derivative will be ignored, val_dot in pid_calculate() will be ignored */ - PID_MODE_DERIVATIV_CALC_NO_SP, - /* Use PID_MODE_DERIVATIV_SET if you have the derivative already (Gyros, Kalman) */ - PID_MODE_DERIVATIV_SET -} pid_mode_t; - -typedef struct { - pid_mode_t mode; - float dt_min; - float kp; - float ki; - float kd; - float integral; - float integral_limit; - float output_limit; - float error_previous; - float last_output; -} PID_t; - -__EXPORT void pid_init(PID_t *pid, pid_mode_t mode, float dt_min); -__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float integral_limit, float output_limit); -__EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt); -__EXPORT void pid_reset_integral(PID_t *pid); - -__END_DECLS - -#endif /* PID_H_ */ diff --git a/src/modules/rover_ackermann/RoverAckermannControl/CMakeLists.txt b/src/modules/rover_ackermann/RoverAckermannControl/CMakeLists.txt index 9645c7acf9..e0574682f1 100644 --- a/src/modules/rover_ackermann/RoverAckermannControl/CMakeLists.txt +++ b/src/modules/rover_ackermann/RoverAckermannControl/CMakeLists.txt @@ -35,5 +35,5 @@ px4_add_library(RoverAckermannControl RoverAckermannControl.cpp ) -target_link_libraries(RoverAckermannControl PUBLIC pid) +target_link_libraries(RoverAckermannControl PUBLIC PID) target_include_directories(RoverAckermannControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/rover_ackermann/RoverAckermannControl/RoverAckermannControl.cpp b/src/modules/rover_ackermann/RoverAckermannControl/RoverAckermannControl.cpp index 5da47d449c..ad02e79141 100644 --- a/src/modules/rover_ackermann/RoverAckermannControl/RoverAckermannControl.cpp +++ b/src/modules/rover_ackermann/RoverAckermannControl/RoverAckermannControl.cpp @@ -41,27 +41,19 @@ RoverAckermannControl::RoverAckermannControl(ModuleParams *parent) : ModuleParam { updateParams(); _rover_ackermann_status_pub.advertise(); - pid_init(&_pid_throttle, PID_MODE_DERIVATIV_NONE, 0.001f); - } void RoverAckermannControl::updateParams() { ModuleParams::updateParams(); - pid_set_parameters(&_pid_throttle, - _param_ra_speed_p.get(), // Proportional gain - _param_ra_speed_i.get(), // Integral gain - 0, // Derivative gain - _param_ra_speed_i.get() > FLT_EPSILON ? 1.f / _param_ra_speed_i.get() : 0.f, // Integral limit - 1); // Output limit + _pid_throttle.setGains(_param_ra_speed_p.get(), _param_ra_speed_i.get(), 0.f); + _pid_throttle.setIntegralLimit(1.f); + _pid_throttle.setOutputLimit(1.f); - pid_set_parameters(&_pid_lat_accel, - _param_ra_lat_accel_p.get(), // Proportional gain - _param_ra_lat_accel_i.get(), // Integral gain - 0, // Derivative gain - _param_ra_lat_accel_i.get() > FLT_EPSILON ? 1.f / _param_ra_lat_accel_i.get() : 0.f, // Integral limit - 1); // Output limit + _pid_lat_accel.setGains(_param_ra_lat_accel_p.get(), _param_ra_lat_accel_i.get(), 0.f); + _pid_lat_accel.setIntegralLimit(1.f); + _pid_lat_accel.setOutputLimit(1.f); // Update slew rates if (_param_ra_max_accel.get() > FLT_EPSILON && _param_ra_max_speed.get() > FLT_EPSILON) { @@ -117,8 +109,8 @@ void RoverAckermannControl::computeMotorCommands(const float vehicle_forward_spe if (sign(vehicle_forward_speed_temp) == 1) { // Only do closed loop control when driving forwards (backwards driving is non-minimum phase and can therefor introduce instability) - steering_setpoint += pid_calculate(&_pid_lat_accel, _rover_ackermann_setpoint.lateral_acceleration_setpoint, - vehicle_lateral_acceleration, 0, dt); + _pid_lat_accel.setSetpoint(_rover_ackermann_setpoint.lateral_acceleration_setpoint); + steering_setpoint += _pid_lat_accel.update(vehicle_lateral_acceleration, dt); } _rover_ackermann_setpoint.steering_setpoint = math::constrain(steering_setpoint, -_param_ra_max_steer_angle.get(), @@ -156,8 +148,8 @@ void RoverAckermannControl::computeMotorCommands(const float vehicle_forward_spe _rover_ackermann_status.steering_setpoint_normalized = steering_normalized; _rover_ackermann_status.adjusted_steering_setpoint_normalized = _steering_with_rate_limit.getState(); _rover_ackermann_status.measured_lateral_acceleration = vehicle_lateral_acceleration; - _rover_ackermann_status.pid_throttle_integral = _pid_throttle.integral * _param_ra_speed_i.get(); - _rover_ackermann_status.pid_lat_accel_integral = _pid_lat_accel.integral * _param_ra_lat_accel_i.get(); + _rover_ackermann_status.pid_throttle_integral = _pid_throttle.getIntegral(); + _rover_ackermann_status.pid_lat_accel_integral = _pid_lat_accel.getIntegral(); _rover_ackermann_status_pub.publish(_rover_ackermann_status); // Publish to motor @@ -218,8 +210,8 @@ float RoverAckermannControl::calcNormalizedSpeedSetpoint(const float forward_spe -1.f, 1.f); } - forward_speed_normalized += pid_calculate(&_pid_throttle, _forward_speed_setpoint_with_accel_limit.getState(), - vehicle_forward_speed, 0, dt); // Feedback + _pid_throttle.setSetpoint(_forward_speed_setpoint_with_accel_limit.getState()); + forward_speed_normalized += _pid_throttle.update(vehicle_forward_speed, dt); } return math::constrain(forward_speed_normalized, -1.f, 1.f); @@ -228,8 +220,8 @@ float RoverAckermannControl::calcNormalizedSpeedSetpoint(const float forward_spe void RoverAckermannControl::resetControllers() { - pid_reset_integral(&_pid_throttle); - pid_reset_integral(&_pid_lat_accel); + _pid_throttle.resetIntegral(); + _pid_lat_accel.resetIntegral(); _forward_speed_setpoint_with_accel_limit.setForcedValue(0.f); _steering_with_rate_limit.setForcedValue(0.f); diff --git a/src/modules/rover_ackermann/RoverAckermannControl/RoverAckermannControl.hpp b/src/modules/rover_ackermann/RoverAckermannControl/RoverAckermannControl.hpp index a781ea79ca..7f23caff4c 100644 --- a/src/modules/rover_ackermann/RoverAckermannControl/RoverAckermannControl.hpp +++ b/src/modules/rover_ackermann/RoverAckermannControl/RoverAckermannControl.hpp @@ -46,7 +46,7 @@ #include // Standard libraries -#include +#include #include #include #include @@ -114,8 +114,8 @@ private: hrt_abstime _timestamp{0}; // Controllers - PID_t _pid_throttle; // The PID controller for the closed loop speed control - PID_t _pid_lat_accel; // The PID controller for the closed loop speed control + PID _pid_throttle; // The PID controller for the closed loop speed control + PID _pid_lat_accel; // The PID controller for the closed loop speed control SlewRate _steering_with_rate_limit{0.f}; SlewRate _forward_speed_setpoint_with_accel_limit{0.f}; diff --git a/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.hpp b/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.hpp index 752b5b3417..cf6fa56fc1 100644 --- a/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.hpp +++ b/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.hpp @@ -57,7 +57,7 @@ #include #include #include -#include +#include using namespace matrix; diff --git a/src/modules/rover_differential/RoverDifferentialControl/CMakeLists.txt b/src/modules/rover_differential/RoverDifferentialControl/CMakeLists.txt index 58a5239f8c..7464de4f5b 100644 --- a/src/modules/rover_differential/RoverDifferentialControl/CMakeLists.txt +++ b/src/modules/rover_differential/RoverDifferentialControl/CMakeLists.txt @@ -35,5 +35,5 @@ px4_add_library(RoverDifferentialControl RoverDifferentialControl.cpp ) -target_link_libraries(RoverDifferentialControl PUBLIC pid) +target_link_libraries(RoverDifferentialControl PUBLIC PID) target_include_directories(RoverDifferentialControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.cpp b/src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.cpp index deb0b9e37b..e864e3e31f 100644 --- a/src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.cpp +++ b/src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.cpp @@ -42,9 +42,6 @@ RoverDifferentialControl::RoverDifferentialControl(ModuleParams *parent) : Modul { updateParams(); _rover_differential_status_pub.advertise(); - pid_init(&_pid_yaw_rate, PID_MODE_DERIVATIV_NONE, 0.001f); - pid_init(&_pid_throttle, PID_MODE_DERIVATIV_NONE, 0.001f); - pid_init(&_pid_yaw, PID_MODE_DERIVATIV_NONE, 0.001f); } void RoverDifferentialControl::updateParams() @@ -54,24 +51,15 @@ void RoverDifferentialControl::updateParams() _max_yaw_accel = _param_rd_max_yaw_accel.get() * M_DEG_TO_RAD_F; // Update PID - pid_set_parameters(&_pid_yaw_rate, - _param_rd_yaw_rate_p.get(), // Proportional gain - _param_rd_yaw_rate_i.get(), // Integral gain - 0.f, // Derivative gain - 1.f, // Integral limit - 1.f); // Output limit - pid_set_parameters(&_pid_throttle, - _param_rd_p_gain_speed.get(), // Proportional gain - _param_rd_i_gain_speed.get(), // Integral gain - 0.f, // Derivative gain - 1.f, // Integral limit - 1.f); // Output limit - pid_set_parameters(&_pid_yaw, - _param_rd_p_gain_yaw.get(), // Proportional gain - _param_rd_i_gain_yaw.get(), // Integral gain - 0.f, // Derivative gain - _max_yaw_rate, // Integral limit - _max_yaw_rate); // Output limit + _pid_yaw_rate.setGains(_param_rd_yaw_rate_p.get(), _param_rd_yaw_rate_i.get(), 0.f); + _pid_yaw_rate.setIntegralLimit(1.f); + _pid_yaw_rate.setOutputLimit(1.f); + _pid_throttle.setGains(_param_rd_p_gain_speed.get(), _param_rd_i_gain_speed.get(), 0.f); + _pid_throttle.setIntegralLimit(1.f); + _pid_throttle.setOutputLimit(1.f); + _pid_yaw.setGains(_param_rd_p_gain_yaw.get(), _param_rd_i_gain_yaw.get(), 0.f); + _pid_yaw.setIntegralLimit(_max_yaw_rate); + _pid_yaw.setOutputLimit(_max_yaw_rate); // Update slew rates if (_max_yaw_rate > FLT_EPSILON) { @@ -99,8 +87,10 @@ void RoverDifferentialControl::computeMotorCommands(const float vehicle_yaw, con if (PX4_ISFINITE(_rover_differential_setpoint.yaw_setpoint)) { _yaw_setpoint_with_yaw_rate_limit.update(matrix::wrap_pi(_rover_differential_setpoint.yaw_setpoint), dt); _rover_differential_status.adjusted_yaw_setpoint = matrix::wrap_pi(_yaw_setpoint_with_yaw_rate_limit.getState()); - const float heading_error = matrix::wrap_pi(_yaw_setpoint_with_yaw_rate_limit.getState() - vehicle_yaw); - _rover_differential_setpoint.yaw_rate_setpoint = pid_calculate(&_pid_yaw, heading_error, 0, 0, dt); + _pid_yaw.setSetpoint( + matrix::wrap_pi(_yaw_setpoint_with_yaw_rate_limit.getState() - + vehicle_yaw)); // error as setpoint to take care of wrapping + _rover_differential_setpoint.yaw_rate_setpoint = _pid_yaw.update(0.f, dt); _rover_differential_status.clyaw_yaw_rate_setpoint = _rover_differential_setpoint.yaw_rate_setpoint; } else { @@ -142,9 +132,9 @@ void RoverDifferentialControl::computeMotorCommands(const float vehicle_yaw, con _rover_differential_status.measured_forward_speed = vehicle_forward_speed; _rover_differential_status.measured_yaw = vehicle_yaw; _rover_differential_status.measured_yaw_rate = vehicle_yaw_rate; - _rover_differential_status.pid_yaw_rate_integral = _pid_yaw_rate.integral; - _rover_differential_status.pid_throttle_integral = _pid_throttle.integral; - _rover_differential_status.pid_yaw_integral = _pid_yaw.integral; + _rover_differential_status.pid_yaw_rate_integral = _pid_yaw_rate.getIntegral(); + _rover_differential_status.pid_throttle_integral = _pid_throttle.getIntegral(); + _rover_differential_status.pid_yaw_integral = _pid_yaw.getIntegral(); _rover_differential_status_pub.publish(_rover_differential_status); // Publish to motors @@ -158,7 +148,7 @@ void RoverDifferentialControl::computeMotorCommands(const float vehicle_yaw, con float RoverDifferentialControl::calcNormalizedSpeedDiff(const float yaw_rate_setpoint, const float vehicle_yaw_rate, const float max_thr_yaw_r, const float max_yaw_accel, const float wheel_track, const float dt, SlewRate &yaw_rate_with_accel_limit, - PID_t &pid_yaw_rate, const bool normalized) + PID &pid_yaw_rate, const bool normalized) { float slew_rate_normalization{1.f}; @@ -190,8 +180,8 @@ float RoverDifferentialControl::calcNormalizedSpeedDiff(const float yaw_rate_set max_thr_yaw_r, -1.f, 1.f); } - speed_diff_normalized += pid_calculate(&pid_yaw_rate, yaw_rate_with_accel_limit.getState(), vehicle_yaw_rate, 0, - dt); // Feedback + pid_yaw_rate.setSetpoint(yaw_rate_with_accel_limit.getState()); + speed_diff_normalized += pid_yaw_rate.update(vehicle_yaw_rate, dt); } return math::constrain(speed_diff_normalized, -1.f, 1.f); @@ -200,7 +190,7 @@ float RoverDifferentialControl::calcNormalizedSpeedDiff(const float yaw_rate_set float RoverDifferentialControl::calcNormalizedSpeedSetpoint(const float forward_speed_setpoint, const float vehicle_forward_speed, const float max_thr_spd, SlewRate &forward_speed_setpoint_with_accel_limit, - PID_t &pid_throttle, const float max_accel, const float max_decel, const float dt, const bool normalized) + PID &pid_throttle, const float max_accel, const float max_decel, const float dt, const bool normalized) { float slew_rate_normalization{1.f}; @@ -242,8 +232,8 @@ float RoverDifferentialControl::calcNormalizedSpeedSetpoint(const float forward_ -1.f, 1.f); } - forward_speed_normalized += pid_calculate(&pid_throttle, _forward_speed_setpoint_with_accel_limit.getState(), - vehicle_forward_speed, 0, dt); // Feedback + pid_throttle.setSetpoint(_forward_speed_setpoint_with_accel_limit.getState()); + forward_speed_normalized += pid_throttle.update(vehicle_forward_speed, dt); } return math::constrain(forward_speed_normalized, -1.f, 1.f); @@ -267,7 +257,7 @@ matrix::Vector2f RoverDifferentialControl::computeInverseKinematics(float forwar void RoverDifferentialControl::resetControllers() { - pid_reset_integral(&_pid_throttle); - pid_reset_integral(&_pid_yaw_rate); - pid_reset_integral(&_pid_yaw); + _pid_throttle.resetIntegral(); + _pid_yaw_rate.resetIntegral(); + _pid_yaw.resetIntegral(); } diff --git a/src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.hpp b/src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.hpp index 5a445e7882..e08153356b 100644 --- a/src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.hpp +++ b/src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.hpp @@ -47,7 +47,7 @@ #include // Standard libraries -#include +#include #include #include #include @@ -100,7 +100,7 @@ private: * @return Normalized speed differece setpoint with applied slew rates [-1, 1]. */ float calcNormalizedSpeedDiff(float yaw_rate_setpoint, float vehicle_yaw_rate, float max_thr_yaw_r, float max_yaw_accel, - float wheel_track, float dt, SlewRate &yaw_rate_with_accel_limit, PID_t &pid_yaw_rate, bool normalized); + float wheel_track, float dt, SlewRate &yaw_rate_with_accel_limit, PID &pid_yaw_rate, bool normalized); /** * @brief Compute normalized forward speed setpoint and apply slew rates. * to the forward speed setpoint and doing closed loop speed control if enabled. @@ -116,7 +116,7 @@ private: * @return Normalized forward speed setpoint with applied slew rates [-1, 1]. */ float calcNormalizedSpeedSetpoint(float forward_speed_setpoint, float vehicle_forward_speed, float max_thr_spd, - SlewRate &forward_speed_setpoint_with_accel_limit, PID_t &pid_throttle, float max_accel, float max_decel, + SlewRate &forward_speed_setpoint_with_accel_limit, PID &pid_throttle, float max_accel, float max_decel, float dt, bool normalized); /** @@ -142,9 +142,9 @@ private: float _max_yaw_accel{0.f}; // Controllers - PID_t _pid_throttle; // Closed loop speed control - PID_t _pid_yaw; // Closed loop yaw control - PID_t _pid_yaw_rate; // Closed loop yaw rate control + PID _pid_throttle; // Closed loop speed control + PID _pid_yaw; // Closed loop yaw control + PID _pid_yaw_rate; // Closed loop yaw rate control SlewRate _forward_speed_setpoint_with_accel_limit{0.f}; SlewRate _yaw_rate_with_accel_limit{0.f}; SlewRateYaw _yaw_setpoint_with_yaw_rate_limit; diff --git a/src/modules/rover_mecanum/RoverMecanum.hpp b/src/modules/rover_mecanum/RoverMecanum.hpp index e76124aca9..b2d89efbd3 100644 --- a/src/modules/rover_mecanum/RoverMecanum.hpp +++ b/src/modules/rover_mecanum/RoverMecanum.hpp @@ -53,7 +53,7 @@ #include // Standard libraries -#include +#include #include // Local includes diff --git a/src/modules/rover_mecanum/RoverMecanumControl/CMakeLists.txt b/src/modules/rover_mecanum/RoverMecanumControl/CMakeLists.txt index 9777ae5d18..61b64980f1 100644 --- a/src/modules/rover_mecanum/RoverMecanumControl/CMakeLists.txt +++ b/src/modules/rover_mecanum/RoverMecanumControl/CMakeLists.txt @@ -35,5 +35,5 @@ px4_add_library(RoverMecanumControl RoverMecanumControl.cpp ) -target_link_libraries(RoverMecanumControl PUBLIC pid) +target_link_libraries(RoverMecanumControl PUBLIC PID) target_include_directories(RoverMecanumControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.cpp b/src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.cpp index 9f52ad1756..87c9f86079 100644 --- a/src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.cpp +++ b/src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.cpp @@ -42,40 +42,28 @@ RoverMecanumControl::RoverMecanumControl(ModuleParams *parent) : ModuleParams(pa { updateParams(); _rover_mecanum_status_pub.advertise(); - pid_init(&_pid_yaw_rate, PID_MODE_DERIVATIV_NONE, 0.001f); - pid_init(&_pid_forward_throttle, PID_MODE_DERIVATIV_NONE, 0.001f); - pid_init(&_pid_lateral_throttle, PID_MODE_DERIVATIV_NONE, 0.001f); - pid_init(&_pid_yaw, PID_MODE_DERIVATIV_NONE, 0.001f); } void RoverMecanumControl::updateParams() { ModuleParams::updateParams(); + + _pid_yaw_rate.setGains(_param_rm_yaw_rate_p.get(), _param_rm_yaw_rate_i.get(), 0.f); + _pid_yaw_rate.setIntegralLimit(1.f); + _pid_yaw_rate.setOutputLimit(1.f); + + _pid_forward_throttle.setGains(_param_rm_p_gain_speed.get(), _param_rm_i_gain_speed.get(), 0.f); + _pid_forward_throttle.setIntegralLimit(1.f); + _pid_forward_throttle.setOutputLimit(1.f); + + _pid_lateral_throttle.setGains(_param_rm_p_gain_speed.get(), _param_rm_i_gain_speed.get(), 0.f); + _pid_lateral_throttle.setIntegralLimit(1.f); + _pid_lateral_throttle.setOutputLimit(1.f); + _max_yaw_rate = _param_rm_max_yaw_rate.get() * M_DEG_TO_RAD_F; - pid_set_parameters(&_pid_yaw_rate, - _param_rm_yaw_rate_p.get(), // Proportional gain - _param_rm_yaw_rate_i.get(), // Integral gain - 0.f, // Derivative gain - 1.f, // Integral limit - 1.f); // Output limit - pid_set_parameters(&_pid_forward_throttle, - _param_rm_p_gain_speed.get(), // Proportional gain - _param_rm_i_gain_speed.get(), // Integral gain - 0.f, // Derivative gain - 1.f, // Integral limit - 1.f); // Output limit - pid_set_parameters(&_pid_lateral_throttle, - _param_rm_p_gain_speed.get(), // Proportional gain - _param_rm_i_gain_speed.get(), // Integral gain - 0.f, // Derivative gain - 1.f, // Integral limit - 1.f); // Output limit - pid_set_parameters(&_pid_yaw, - _param_rm_p_gain_yaw.get(), // Proportional gain - _param_rm_i_gain_yaw.get(), // Integral gain - 0.f, // Derivative gain - _max_yaw_rate, // Integral limit - _max_yaw_rate); // Output limit + _pid_yaw.setGains(_param_rm_p_gain_yaw.get(), _param_rm_i_gain_yaw.get(), 0.f); + _pid_yaw.setIntegralLimit(_max_yaw_rate); + _pid_yaw.setOutputLimit(_max_yaw_rate); } void RoverMecanumControl::computeMotorCommands(const float vehicle_yaw, const float vehicle_yaw_rate, @@ -91,11 +79,12 @@ void RoverMecanumControl::computeMotorCommands(const float vehicle_yaw, const fl // Closed loop yaw control if (PX4_ISFINITE(_rover_mecanum_setpoint.yaw_setpoint)) { - const float heading_error = matrix::wrap_pi(_rover_mecanum_setpoint.yaw_setpoint - vehicle_yaw); - _rover_mecanum_setpoint.yaw_rate_setpoint = pid_calculate(&_pid_yaw, heading_error, 0.f, 0.f, dt); + _pid_yaw.setSetpoint( + matrix::wrap_pi(_rover_mecanum_setpoint.yaw_setpoint - vehicle_yaw)); // error as setpoint to take care of wrapping + _rover_mecanum_setpoint.yaw_rate_setpoint = _pid_yaw.update(0.f, dt); } else { - pid_reset_integral(&_pid_yaw); + _pid_yaw.resetIntegral(); } // Yaw rate control @@ -108,8 +97,9 @@ void RoverMecanumControl::computeMotorCommands(const float vehicle_yaw, const fl _param_rm_max_thr_yaw_r.get(), -1.f, 1.f); } + _pid_yaw_rate.setSetpoint(_rover_mecanum_setpoint.yaw_rate_setpoint); speed_diff_normalized = math::constrain(speed_diff_normalized + - pid_calculate(&_pid_yaw_rate, _rover_mecanum_setpoint.yaw_rate_setpoint, vehicle_yaw_rate, 0.f, dt), + _pid_yaw_rate.update(vehicle_yaw_rate, dt), -1.f, 1.f); // Feedback } else { // Use normalized setpoint @@ -129,8 +119,8 @@ void RoverMecanumControl::computeMotorCommands(const float vehicle_yaw, const fl -_param_rm_max_thr_spd.get(), _param_rm_max_thr_spd.get(), -1.f, 1.f); } - forward_throttle += pid_calculate(&_pid_forward_throttle, _rover_mecanum_setpoint.forward_speed_setpoint, - vehicle_forward_speed, 0, dt); // Feedback + _pid_forward_throttle.setSetpoint(_rover_mecanum_setpoint.forward_speed_setpoint); + forward_throttle += _pid_forward_throttle.update(vehicle_forward_speed, dt); // Closed loop lateral speed control if (_param_rm_max_thr_spd.get() > FLT_EPSILON) { // Feedforward @@ -138,8 +128,8 @@ void RoverMecanumControl::computeMotorCommands(const float vehicle_yaw, const fl -_param_rm_max_thr_spd.get(), _param_rm_max_thr_spd.get(), -1.f, 1.f); } - lateral_throttle += pid_calculate(&_pid_lateral_throttle, _rover_mecanum_setpoint.lateral_speed_setpoint, - vehicle_lateral_speed, 0, dt); // Feedback + _pid_lateral_throttle.setSetpoint(_rover_mecanum_setpoint.lateral_speed_setpoint); + lateral_throttle += _pid_lateral_throttle.update(vehicle_lateral_speed, dt); } else { // Use normalized setpoint forward_throttle = PX4_ISFINITE(_rover_mecanum_setpoint.forward_speed_setpoint_normalized) ? math::constrain( @@ -156,10 +146,10 @@ void RoverMecanumControl::computeMotorCommands(const float vehicle_yaw, const fl rover_mecanum_status.adjusted_yaw_rate_setpoint = _rover_mecanum_setpoint.yaw_rate_setpoint; rover_mecanum_status.measured_yaw_rate = vehicle_yaw_rate; rover_mecanum_status.measured_yaw = vehicle_yaw; - rover_mecanum_status.pid_yaw_rate_integral = _pid_yaw_rate.integral; - rover_mecanum_status.pid_yaw_integral = _pid_yaw.integral; - rover_mecanum_status.pid_forward_throttle_integral = _pid_forward_throttle.integral; - rover_mecanum_status.pid_lateral_throttle_integral = _pid_lateral_throttle.integral; + rover_mecanum_status.pid_yaw_rate_integral = _pid_yaw_rate.getIntegral(); + rover_mecanum_status.pid_yaw_integral = _pid_yaw.getIntegral(); + rover_mecanum_status.pid_forward_throttle_integral = _pid_forward_throttle.getIntegral(); + rover_mecanum_status.pid_lateral_throttle_integral = _pid_lateral_throttle.getIntegral(); _rover_mecanum_status_pub.publish(rover_mecanum_status); // Publish to motors @@ -213,8 +203,8 @@ matrix::Vector4f RoverMecanumControl::computeInverseKinematics(float forward_thr void RoverMecanumControl::resetControllers() { - pid_reset_integral(&_pid_forward_throttle); - pid_reset_integral(&_pid_lateral_throttle); - pid_reset_integral(&_pid_yaw_rate); - pid_reset_integral(&_pid_yaw); + _pid_forward_throttle.resetIntegral(); + _pid_lateral_throttle.resetIntegral(); + _pid_yaw_rate.resetIntegral(); + _pid_yaw.resetIntegral(); } diff --git a/src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.hpp b/src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.hpp index 27a084934a..42cec627a0 100644 --- a/src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.hpp +++ b/src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.hpp @@ -46,7 +46,7 @@ // Standard libraries -#include +#include #include #include #include @@ -112,10 +112,10 @@ private: float _max_yaw_rate{0.f}; // Controllers - PID_t _pid_forward_throttle; // PID for the closed loop forward speed control - PID_t _pid_lateral_throttle; // PID for the closed loop lateral speed control - PID_t _pid_yaw; // PID for the closed loop yaw control - PID_t _pid_yaw_rate; // PID for the closed loop yaw rate control + PID _pid_forward_throttle; // PID for the closed loop forward speed control + PID _pid_lateral_throttle; // PID for the closed loop lateral speed control + PID _pid_yaw; // PID for the closed loop yaw control + PID _pid_yaw_rate; // PID for the closed loop yaw rate control // Parameters DEFINE_PARAMETERS( diff --git a/src/modules/rover_pos_control/CMakeLists.txt b/src/modules/rover_pos_control/CMakeLists.txt index 5e093862e7..ee85e3cc39 100644 --- a/src/modules/rover_pos_control/CMakeLists.txt +++ b/src/modules/rover_pos_control/CMakeLists.txt @@ -38,5 +38,5 @@ px4_add_module( RoverPositionControl.hpp DEPENDS l1 - pid + PID ) diff --git a/src/modules/rover_pos_control/RoverPositionControl.cpp b/src/modules/rover_pos_control/RoverPositionControl.cpp index 928ce13c6e..7466077a93 100644 --- a/src/modules/rover_pos_control/RoverPositionControl.cpp +++ b/src/modules/rover_pos_control/RoverPositionControl.cpp @@ -92,13 +92,9 @@ void RoverPositionControl::parameters_update(bool force) _gnd_control.set_l1_damping(_param_l1_damping.get()); _gnd_control.set_l1_period(_param_l1_period.get()); - pid_init(&_speed_ctrl, PID_MODE_DERIVATIV_CALC, 0.01f); - pid_set_parameters(&_speed_ctrl, - _param_speed_p.get(), - _param_speed_i.get(), - _param_speed_d.get(), - _param_speed_imax.get(), - _param_gndspeed_max.get()); + _speed_ctrl.setGains(_param_speed_p.get(), _param_speed_i.get(), _param_speed_d.get()); + _speed_ctrl.setIntegralLimit(_param_speed_imax.get()); + _speed_ctrl.setOutputLimit(_param_gndspeed_max.get()); } } @@ -239,12 +235,9 @@ RoverPositionControl::control_position(const matrix::Vector2d ¤t_position, const Dcmf R_to_body(Quatf(_vehicle_att.q).inversed()); const Vector3f vel = R_to_body * Vector3f(ground_speed(0), ground_speed(1), ground_speed(2)); - const float x_vel = vel(0); - const float x_acc = _vehicle_acceleration_sub.get().xyz[0]; - // Compute airspeed control out and just scale it as a constant - mission_throttle = _param_throttle_speed_scaler.get() - * pid_calculate(&_speed_ctrl, mission_target_speed, x_vel, x_acc, dt); + _speed_ctrl.setSetpoint(mission_target_speed); + mission_throttle = _param_throttle_speed_scaler.get() * _speed_ctrl.update(vel(0), dt); // Constrain throttle between min and max mission_throttle = math::constrain(mission_throttle, _param_throttle_min.get(), _param_throttle_max.get()); @@ -327,10 +320,8 @@ RoverPositionControl::control_velocity(const matrix::Vector3f ¤t_velocity) const Dcmf R_to_body(Quatf(_vehicle_att.q).inversed()); const Vector3f vel = R_to_body * Vector3f(current_velocity(0), current_velocity(1), current_velocity(2)); - const float x_vel = vel(0); - const float x_acc = _vehicle_acceleration_sub.get().xyz[0]; - - const float control_throttle = pid_calculate(&_speed_ctrl, desired_speed, x_vel, x_acc, dt); + _speed_ctrl.setSetpoint(desired_speed); + const float control_throttle = _speed_ctrl.update(vel(0), dt); //Constrain maximum throttle to mission throttle _throttle_control = math::constrain(control_throttle, 0.0f, mission_throttle); @@ -392,8 +383,6 @@ RoverPositionControl::Run() vehicle_attitude_poll(); manual_control_setpoint_poll(); - _vehicle_acceleration_sub.update(); - /* update parameters from storage */ parameters_update(); diff --git a/src/modules/rover_pos_control/RoverPositionControl.hpp b/src/modules/rover_pos_control/RoverPositionControl.hpp index ee1c65efa0..d3a26438e2 100644 --- a/src/modules/rover_pos_control/RoverPositionControl.hpp +++ b/src/modules/rover_pos_control/RoverPositionControl.hpp @@ -48,7 +48,7 @@ #include #include #include -#include +#include #include #include #include @@ -128,8 +128,6 @@ private: uORB::Publication _vehicle_thrust_setpoint_pub{ORB_ID(vehicle_thrust_setpoint)}; uORB::Publication _vehicle_torque_setpoint_pub{ORB_ID(vehicle_torque_setpoint)}; - uORB::SubscriptionData _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)}; - perf_counter_t _loop_perf; /**< loop performance counter */ hrt_abstime _control_position_last_called{0}; /** #include #include -#include #include #include #include diff --git a/src/modules/uuv_pos_control/uuv_pos_control.hpp b/src/modules/uuv_pos_control/uuv_pos_control.hpp index 4d94ac9cd9..192365a98d 100644 --- a/src/modules/uuv_pos_control/uuv_pos_control.hpp +++ b/src/modules/uuv_pos_control/uuv_pos_control.hpp @@ -48,7 +48,6 @@ #include #include #include -#include #include #include #include