From 7aa3b1ae2757ae07e4a35587a74f6d6ce56d3f92 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Tue, 20 Dec 2022 13:07:52 +0100 Subject: [PATCH] Set max vel for PID controller --- src/modules/src/position_controller_pid.c | 30 +++++++++---------- .../interface/platform_defaults_bolt.h | 4 +-- .../interface/platform_defaults_cf2.h | 4 +-- .../interface/platform_defaults_flapper.h | 4 +-- .../interface/platform_defaults_tag.h | 4 +-- 5 files changed, 23 insertions(+), 23 deletions(-) diff --git a/src/modules/src/position_controller_pid.c b/src/modules/src/position_controller_pid.c index 173bb998be..a706e059fd 100644 --- a/src/modules/src/position_controller_pid.c +++ b/src/modules/src/position_controller_pid.c @@ -58,13 +58,13 @@ struct this_s { }; // Maximum roll/pitch angle permited -static float rLimit = PID_VEL_ROLL_MAX; -static float pLimit = PID_VEL_PITCH_MAX; +static float rLimit = PID_VEL_ROLL_MAX; +static float pLimit = PID_VEL_PITCH_MAX; static float rpLimitOverhead = 1.10f; // Velocity maximums static float xVelMax = PID_POS_VEL_X_MAX; static float yVelMax = PID_POS_VEL_Y_MAX; -static float zVelMax = PID_POS_VEL_Z_MAX; +static float zVelMax = PID_POS_VEL_Z_MAX; static float velMaxOverhead = 1.10f; static const float thrustScale = 1000.0f; @@ -207,7 +207,7 @@ void positionController(float* thrust, attitude_t *attitude, const setpoint_t *s state_body_x = state->position.x * cosyaw + state->position.y * sinyaw; state_body_y = -state->position.x * sinyaw + state->position.y * cosyaw; - + float globalvx = setpoint->velocity.x; float globalvy = setpoint->velocity.y; @@ -287,66 +287,66 @@ void positionControllerResetAllfilters() { /** * Log variables of the PID position controller - * + * * Note: rename to posCtrlPID ? */ LOG_GROUP_START(posCtl) /** * @brief PID controller target desired body-yaw-aligned velocity x [m/s] - * + * * Note: Same as stabilizer log */ LOG_ADD(LOG_FLOAT, targetVX, &this.pidVX.pid.desired) /** * @brief PID controller target desired body-yaw-aligned velocity y [m/s] - * + * * Note: Same as stabilizer log */ LOG_ADD(LOG_FLOAT, targetVY, &this.pidVY.pid.desired) /** * @brief PID controller target desired velocity z [m/s] - * + * * Note: Same as stabilizer log */ LOG_ADD(LOG_FLOAT, targetVZ, &this.pidVZ.pid.desired) /** * @brief PID controller target desired body-yaw-aligned position x [m] - * + * * Note: Same as stabilizer log */ LOG_ADD(LOG_FLOAT, targetX, &this.pidX.pid.desired) /** * @brief PID controller target desired body-yaw-aligned position y [m] - * + * * Note: Same as stabilizer log */ LOG_ADD(LOG_FLOAT, targetY, &this.pidY.pid.desired) /** * @brief PID controller target desired global position z [m] - * + * * Note: Same as stabilizer log */ LOG_ADD(LOG_FLOAT, targetZ, &this.pidZ.pid.desired) /** * @brief PID state body-yaw-aligned velocity x [m/s] - * + * */ LOG_ADD(LOG_FLOAT, bodyVX, &state_body_vx) /** * @brief PID state body-yaw-aligned velocity y [m/s] - * + * */ LOG_ADD(LOG_FLOAT, bodyVY, &state_body_vy) /** * @brief PID state body-yaw-aligned position x [m] - * + * */ LOG_ADD(LOG_FLOAT, bodyX, &state_body_x) /** * @brief PID state body-yaw-aligned position y [m] - * + * */ LOG_ADD(LOG_FLOAT, bodyY, &state_body_y) diff --git a/src/platform/interface/platform_defaults_bolt.h b/src/platform/interface/platform_defaults_bolt.h index aa2f84f41a..a25f4c0a72 100644 --- a/src/platform/interface/platform_defaults_bolt.h +++ b/src/platform/interface/platform_defaults_bolt.h @@ -121,5 +121,5 @@ #define PID_POS_Z_KFF 0.0f #define PID_POS_VEL_X_MAX 1.0f -#define PID_POS_VEL_Y_MAX 0.0f -#define PID_POS_VEL_Z_MAX 0.0f +#define PID_POS_VEL_Y_MAX 1.0f +#define PID_POS_VEL_Z_MAX 1.0f diff --git a/src/platform/interface/platform_defaults_cf2.h b/src/platform/interface/platform_defaults_cf2.h index 36357e0941..147ac41aa4 100644 --- a/src/platform/interface/platform_defaults_cf2.h +++ b/src/platform/interface/platform_defaults_cf2.h @@ -121,5 +121,5 @@ #define PID_POS_Z_KFF 0.0f #define PID_POS_VEL_X_MAX 1.0f -#define PID_POS_VEL_Y_MAX 0.0f -#define PID_POS_VEL_Z_MAX 0.0f +#define PID_POS_VEL_Y_MAX 1.0f +#define PID_POS_VEL_Z_MAX 1.0f diff --git a/src/platform/interface/platform_defaults_flapper.h b/src/platform/interface/platform_defaults_flapper.h index 1ebff09629..9c5548c70a 100644 --- a/src/platform/interface/platform_defaults_flapper.h +++ b/src/platform/interface/platform_defaults_flapper.h @@ -121,8 +121,8 @@ #define PID_POS_Z_KFF 0.0f #define PID_POS_VEL_X_MAX 1.0f -#define PID_POS_VEL_Y_MAX 0.0f -#define PID_POS_VEL_Z_MAX 0.0f +#define PID_POS_VEL_Y_MAX 1.0f +#define PID_POS_VEL_Z_MAX 1.0f // PID filter configuration #define ATTITUDE_ROLL_RATE_LPF_CUTOFF_FREQ 12.5f diff --git a/src/platform/interface/platform_defaults_tag.h b/src/platform/interface/platform_defaults_tag.h index a95fb620ee..a789633575 100644 --- a/src/platform/interface/platform_defaults_tag.h +++ b/src/platform/interface/platform_defaults_tag.h @@ -121,5 +121,5 @@ #define PID_POS_Z_KFF 0.0f #define PID_POS_VEL_X_MAX 1.0f -#define PID_POS_VEL_Y_MAX 0.0f -#define PID_POS_VEL_Z_MAX 0.0f +#define PID_POS_VEL_Y_MAX 1.0f +#define PID_POS_VEL_Z_MAX 1.0f