fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Replace old pid library with new one

Browse Source
This commit is contained in:
Matthias Grob 2024-11-12 18:05:11 +01:00
parent e047972cde
commit b89c53d28c
19 changed files with 102 additions and 423 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/lib/pid/CMakeLists.txt
View File

@ -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

185
src/lib/pid/pid.cpp
View File

@ -1,185 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
* Author: Laurens Mackay <mackayl@student.ethz.ch>
* Tobias Naegeli <naegelit@student.ethz.ch>
* Martin Rutschmann <rutmarti@student.ethz.ch>
* Anton Babushkin <anton.babushkin@me.com>
* Julian Oes <joes@student.ethz.ch>
*
* 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 <mackayl@student.ethz.ch>
* @author Tobias Naegeli <naegelit@student.ethz.ch>
* @author Martin Rutschmann <rutmarti@student.ethz.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
* @author Julian Oes <joes@student.ethz.ch>
*/
#include "pid.h"
#include <math.h>
#include <px4_platform_common/defines.h>
#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;
}

91
src/lib/pid/pid.h
View File

@ -1,91 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
* Author: Laurens Mackay <mackayl@student.ethz.ch>
* Tobias Naegeli <naegelit@student.ethz.ch>
* Martin Rutschmann <rutmarti@student.ethz.ch>
* Anton Babushkin <anton.babushkin@me.com>
* Julian Oes <joes@student.ethz.ch>
*
* 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 <mackayl@student.ethz.ch>
* @author Tobias Naegeli <naegelit@student.ethz.ch>
* @author Martin Rutschmann <rutmarti@student.ethz.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
* @author Julian Oes <joes@student.ethz.ch>
*/
#ifndef PID_H_
#define PID_H_
#include <stdint.h>
__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_ */

2
src/modules/rover_ackermann/RoverAckermannControl/CMakeLists.txt
View File

@ -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})

36
src/modules/rover_ackermann/RoverAckermannControl/RoverAckermannControl.cpp
View File

@ -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);

6
src/modules/rover_ackermann/RoverAckermannControl/RoverAckermannControl.hpp
View File

@ -46,7 +46,7 @@
#include <uORB/topics/actuator_servos.h>
// Standard libraries
#include <lib/pid/pid.h>
#include <lib/pid/PID.hpp>
#include <matrix/matrix/math.hpp>
#include <matrix/math.hpp>
#include <mathlib/mathlib.h>
@ -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<float> _steering_with_rate_limit{0.f};
SlewRate<float> _forward_speed_setpoint_with_accel_limit{0.f};

2
src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.hpp
View File

@ -57,7 +57,7 @@
#include <mathlib/mathlib.h>
#include <lib/geo/geo.h>
#include <math.h>
#include <lib/pid/pid.h>
#include <lib/pid/PID.hpp>
using namespace matrix;

2
src/modules/rover_differential/RoverDifferentialControl/CMakeLists.txt
View File

@ -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})

60
src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.cpp
View File

@ -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<float> &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<float> &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();
}

12
src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.hpp
View File

@ -47,7 +47,7 @@
#include <lib/slew_rate/SlewRateYaw.hpp>
// Standard libraries
#include <lib/pid/pid.h>
#include <lib/pid/PID.hpp>
#include <matrix/matrix/math.hpp>
#include <matrix/math.hpp>
#include <mathlib/mathlib.h>
@ -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<float> &yaw_rate_with_accel_limit, PID_t &pid_yaw_rate, bool normalized);
float wheel_track, float dt, SlewRate<float> &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<float> &forward_speed_setpoint_with_accel_limit, PID_t &pid_throttle, float max_accel, float max_decel,
SlewRate<float> &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<float> _forward_speed_setpoint_with_accel_limit{0.f};
SlewRate<float> _yaw_rate_with_accel_limit{0.f};
SlewRateYaw<float> _yaw_setpoint_with_yaw_rate_limit;

2
src/modules/rover_mecanum/RoverMecanum.hpp
View File

@ -53,7 +53,7 @@
#include <uORB/topics/rover_mecanum_setpoint.h>
// Standard libraries
#include <lib/pid/pid.h>
#include <lib/pid/PID.hpp>
#include <matrix/matrix/math.hpp>
// Local includes

2
src/modules/rover_mecanum/RoverMecanumControl/CMakeLists.txt
View File

@ -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})

78
src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.cpp
View File

@ -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();
}

10
src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.hpp
View File

@ -46,7 +46,7 @@
// Standard libraries
#include <lib/pid/pid.h>
#include <lib/pid/PID.hpp>
#include <matrix/matrix/math.hpp>
#include <matrix/math.hpp>
#include <mathlib/mathlib.h>
@ -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(

2
src/modules/rover_pos_control/CMakeLists.txt
View File

@ -38,5 +38,5 @@ px4_add_module(
RoverPositionControl.hpp
DEPENDS
l1
pid
PID
)

25
src/modules/rover_pos_control/RoverPositionControl.cpp
View File

@ -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 &current_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 &current_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();

6
src/modules/rover_pos_control/RoverPositionControl.hpp
View File

@ -48,7 +48,7 @@
#include <lib/l1/ECL_L1_Pos_Controller.hpp>
#include <lib/mathlib/mathlib.h>
#include <lib/perf/perf_counter.h>
#include <lib/pid/pid.h>
#include <lib/pid/PID.hpp>
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/defines.h>
#include <px4_platform_common/posix.h>
@ -128,8 +128,6 @@ private:
uORB::Publication<vehicle_thrust_setpoint_s> _vehicle_thrust_setpoint_pub{ORB_ID(vehicle_thrust_setpoint)};
uORB::Publication<vehicle_torque_setpoint_s> _vehicle_torque_setpoint_pub{ORB_ID(vehicle_torque_setpoint)};
uORB::SubscriptionData<vehicle_acceleration_s> _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)};
perf_counter_t _loop_perf; /**< loop performance counter */
hrt_abstime _control_position_last_called{0}; /**<last call of control_position */
@ -139,7 +137,7 @@ private:
/* Pid controller for the speed. Here we assume we can control airspeed but the control variable is actually on
the throttle. For now just assuming a proportional scaler between controlled airspeed and throttle output.*/
PID_t _speed_ctrl{};
PID _speed_ctrl{};
// estimator reset counters
uint8_t _pos_reset_counter{0}; // captures the number of times the estimator has reset the horizontal position

1
src/modules/uuv_att_control/uuv_att_control.hpp
View File

@ -49,7 +49,6 @@
#include <lib/geo/geo.h>
#include <lib/mathlib/mathlib.h>
#include <lib/perf/perf_counter.h>
#include <lib/pid/pid.h>
#include <matrix/math.hpp>
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/defines.h>

1
src/modules/uuv_pos_control/uuv_pos_control.hpp
View File

@ -48,7 +48,6 @@
#include <lib/geo/geo.h>
#include <lib/mathlib/mathlib.h>
#include <lib/perf/perf_counter.h>
#include <lib/pid/pid.h>
#include <matrix/math.hpp>
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/defines.h>