From 159029661e6681e017f8812b585fa305567565c1 Mon Sep 17 00:00:00 2001 From: RCmags Date: Fri, 17 Nov 2023 20:22:46 -0400 Subject: [PATCH] imu angle as integral --- controller/controller.ino | 7 ----- controller/imu.h | 6 ++--- controller/parameters.h | 8 ++---- controller/pid.h | 54 ++++++++++++++++++++++----------------- controller/servos.h | 9 +++---- 5 files changed, 39 insertions(+), 45 deletions(-) diff --git a/controller/controller.ino b/controller/controller.ino index 53fed9a..87666db 100755 --- a/controller/controller.ino +++ b/controller/controller.ino @@ -56,18 +56,11 @@ void loop() { // controller output imu.updateBias(); - #ifdef USING_AUTO_LEVEL - autoLevel(input); - #endif - float output[4]; PIDcontroller(input, output); #ifdef USING_TAIL_ROTOR output[2] += torqueBias( input[3] ); // anti-torque #endif - - output[3] = output[2] + input[3]; - output[2] -= input[3]; // move servos servoPosition(output); diff --git a/controller/imu.h b/controller/imu.h index 48dbee9..7874c1f 100755 --- a/controller/imu.h +++ b/controller/imu.h @@ -35,6 +35,6 @@ float gyroY() { return imu.gx(); } float gyroZ() { return imu.gz(); } // sign of accelerometer axes may differ from gyroscope -float accelX() { return -imu.ay(); } -float accelY() { return imu.ax(); } -float accelZ() { return imu.az(); } +float accelX() { return imu.ay(); } +float accelY() { return -imu.ax(); } +float accelZ() { return -imu.az(); } diff --git a/controller/parameters.h b/controller/parameters.h index e617d80..3a1a4a2 100755 --- a/controller/parameters.h +++ b/controller/parameters.h @@ -43,12 +43,8 @@ // 2B. Auto level //---------------------------------------------------------- /* NOTE: must enable auto level to use */ - // I. Gains -#define GAIN_ANG_PITCH 0 // Gain to set pitch input to self-level aircraft -#define GAIN_ANG_ROLL 0 // Gain to set roll input to self-level aircraft - // II. Trim #define TRIM_ANG_PITCH 0.0 // trim to self-level aircraft in pitch -#define TRIM_ANG_ROLL 0 // trim to self-level aircraft in roll +#define TRIM_ANG_ROLL -20.0 // trim to self-level aircraft in roll //---------------------------------------------------------- // 3. Servo trims @@ -84,4 +80,4 @@ //#define USING_WEIGHT_SHIFT // Uncomment for weight-shift control //#define USING_TAIL_ROTOR // Uncomment for tail rotor correction //#define USING_INTEGRAL_DECAY // Uncomment to enable integral decay -//#define USING_AUTO_LEVEL // Uncomment to enable self-leveling correction +#define USING_AUTO_LEVEL // Uncomment to enable self-leveling correction diff --git a/controller/pid.h b/controller/pid.h index 4ae8bef..415d8f4 100755 --- a/controller/pid.h +++ b/controller/pid.h @@ -28,7 +28,26 @@ void PIDcontroller( float* input, float* output ) { static float angvel_s[3] = {0}; // smoothed proportional float angvel[] = { gyroX(), gyroY(), gyroZ() }; - float angvel_t[] = { input[0], input[1], input[2] }; + float angvel_t[] = { input[0]*SCALE, + input[1]*SCALE, + input[2]*SCALE }; + + #ifdef USING_AUTO_LEVEL + // offsets + constexpr float ANG_ROLL = TRIM_ANG_ROLL * DEG_TO_RAD; + constexpr float ANG_PITCH = TRIM_ANG_PITCH * DEG_TO_RAD; + + // yaw change + float dyaw = -0.5 * angvel_t[2] * dt; + fusion.rotateHeading( dyaw, SMALL_ANGLE ); + + // heading + updateFusion(); + + angle[0] = GAIN_INT[0] * ( fusion.roll() - ANG_ROLL ); + angle[1] = GAIN_INT[1] * ( fusion.pitch() - ANG_PITCH ); + angle[2] = GAIN_INT[2] * fusion.yaw(); + #endif for( int i = 0; i < 3; i += 1 ) { // alpha-beta filter: @@ -37,16 +56,18 @@ void PIDcontroller( float* input, float* output ) { angacc[i] += BETA*dvel / dt; // derivative // target rate - angvel_t[i] *= SCALE; // target angular velocity float dangvel = angvel[i] - angvel_t[i]; // integrate angvel[i] += angacc[i] * 0.5 * dt; // trapezoidal rule. Use derivative to improve area slice. - angle[i] += GAIN_INT[i] * dangvel * dt; - - #ifdef USING_INTEGRAL_DECAY - angle[i] -= DECAY[i] * angle[i] * dt; - #endif + + #ifndef USING_AUTO_LEVEL + angle[i] += GAIN_INT[i] * dangvel * dt; + + #ifdef USING_INTEGRAL_DECAY + angle[i] -= DECAY[i] * angle[i] * dt; + #endif + #endif angle[i] = constrain(angle[i], -PWM_CHANGE, PWM_CHANGE); // prevent windup } @@ -101,25 +122,10 @@ void PIDcontroller( float* input, float* output ) { #endif } -/* bias controls to self upright */ -void autoLevel(float* output) { - constexpr float ANG_ROLL = TRIM_ANG_ROLL * DEG_TO_RAD; - constexpr float ANG_PITCH = TRIM_ANG_PITCH * DEG_TO_RAD; - - // horizontal angles - updateFusion(); - float pitch = fusion.roll() - ANG_ROLL; - float roll = fusion.pitch() - ANG_PITCH; - - // control rates - output[1] -= float(GAIN_ANG_ROLL) * roll; - output[0] -= float(GAIN_ANG_PITCH) * pitch; -} - -/* counter main-rotor torque via yaw bias +// counter main-rotor torque via yaw bias float torqueBias(float input) { constexpr float SCALE = 0.5 / PWM_CHANGE; input = input < 0 ? 0 : input; input *= SCALE; return pow(input, TORQUE_POWER) * TORQUE_GAIN; -} */ +} diff --git a/controller/servos.h b/controller/servos.h index 5ea87f6..6a947db 100755 --- a/controller/servos.h +++ b/controller/servos.h @@ -1,21 +1,20 @@ //=========== Servos / Tail ESC ============ #include -Servo servo[4]; +Servo servo[3]; //---------------------- Constants ------------------------- constexpr float PWM_MID_SERVO[] = { PWM_MID + TRIM_PITCH, PWM_MID + TRIM_ROLL, - PWM_MID + TRIM_YAW, PWM_MID + TRIM_YAW }; -constexpr int SERVO_PIN[] = {3,4,5,A0}; // Output pins: roll, pitch, yaw1, yaw2 +constexpr int SERVO_PIN[] = {3,4,5}; // Output pins: roll, pitch, yaw //--------------------------------------------------------- /* initialize and center servos */ void setupServos() { - for( int i=0; i < 4; i += 1 ) { + for( int i=0; i < 3; i += 1 ) { pinMode( SERVO_PIN[i], OUTPUT); servo[i].attach( SERVO_PIN[i] ); servo[i].writeMicroseconds( PWM_MID_SERVO[i] ); @@ -24,7 +23,7 @@ void setupServos() { /* set servo positions */ void servoPosition( float* output ) { - for( int i=0; i < 4; i += 1 ) { + for( int i=0; i < 3; i += 1 ) { output[i] = constrain( output[i], -PWM_CHANGE, PWM_CHANGE); servo[i].writeMicroseconds( PWM_MID_SERVO[i] + output[i] ); }