From 95824e3b756a7b9c85b189361f35c30ff03970aa Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Fri, 10 Jan 2025 06:00:06 +0300 Subject: [PATCH] Make max tilt and max angle rates MAVLink parameters Also decrease default max yaw rate to 300 degrees --- flix/control.ino | 18 ++++++++++-------- flix/parameters.ino | 4 ++++ 2 files changed, 14 insertions(+), 8 deletions(-) diff --git a/flix/control.ino b/flix/control.ino index 4633043..a330138 100644 --- a/flix/control.ino +++ b/flix/control.ino @@ -29,8 +29,8 @@ #define YAW_P 3 #define PITCHRATE_MAX radians(360) #define ROLLRATE_MAX radians(360) -#define YAWRATE_MAX radians(360) -#define MAX_TILT radians(30) +#define YAWRATE_MAX radians(300) +#define TILT_MAX radians(30) #define RATES_D_LPF_ALPHA 0.2 // cutoff frequency ~ 40 Hz @@ -44,6 +44,8 @@ PID yawRatePID(YAWRATE_P, YAWRATE_I, YAWRATE_D); PID rollPID(ROLL_P, ROLL_I, ROLL_D); PID pitchPID(PITCH_P, PITCH_I, PITCH_D); PID yawPID(YAW_P, 0, 0); +Vector maxRate(ROLLRATE_MAX, PITCHRATE_MAX, YAWRATE_MAX); +float tiltMax = TILT_MAX; Quaternion attitudeTarget; Vector ratesTarget; @@ -83,18 +85,18 @@ void interpretRC() { if (mode == ACRO) { yawMode = YAW_RATE; - ratesTarget.x = controls[RC_CHANNEL_ROLL] * ROLLRATE_MAX; - ratesTarget.y = controls[RC_CHANNEL_PITCH] * PITCHRATE_MAX; - ratesTarget.z = -controls[RC_CHANNEL_YAW] * YAWRATE_MAX; // positive yaw stick means clockwise rotation in FLU + ratesTarget.x = controls[RC_CHANNEL_ROLL] * maxRate.x; + ratesTarget.y = controls[RC_CHANNEL_PITCH] * maxRate.y; + ratesTarget.z = -controls[RC_CHANNEL_YAW] * maxRate.z; // positive yaw stick means clockwise rotation in FLU } else if (mode == STAB) { yawMode = controls[RC_CHANNEL_YAW] == 0 ? YAW : YAW_RATE; attitudeTarget = Quaternion::fromEulerZYX(Vector( - controls[RC_CHANNEL_ROLL] * MAX_TILT, - controls[RC_CHANNEL_PITCH] * MAX_TILT, + controls[RC_CHANNEL_ROLL] * tiltMax, + controls[RC_CHANNEL_PITCH] * tiltMax, attitudeTarget.getYaw())); - ratesTarget.z = -controls[RC_CHANNEL_YAW] * YAWRATE_MAX; // positive yaw stick means clockwise rotation in FLU + ratesTarget.z = -controls[RC_CHANNEL_YAW] * maxRate.z; // positive yaw stick means clockwise rotation in FLU } else if (mode == MANUAL) { // passthrough mode diff --git a/flix/parameters.ino b/flix/parameters.ino index 2fb34df..092ae20 100644 --- a/flix/parameters.ino +++ b/flix/parameters.ino @@ -36,6 +36,10 @@ Parameter parameters[] = { {"PITCH_I", &pitchPID.i}, {"PITCH_D", &pitchPID.d}, {"YAW_P", &yawPID.p}, + {"PITCHRATE_MAX", &maxRate.y}, + {"ROLLRATE_MAX", &maxRate.x}, + {"YAWRATE_MAX", &maxRate.z}, + {"TILT_MAX", &tiltMax}, // imu {"ACC_BIAS_X", &accBias.x}, {"ACC_BIAS_Y", &accBias.y},