Merge pull request #4898 from iNavFlight/development

Release 2.2.1

docs/Board - IFLIGHTF7_TWING.md

@@ -0,0 +1,19 @@
+### Hardware
+- MCU: STM32F722RET6
+- IMU1: ICM20689_A (SPI1)
+- IMU2: ICM20689_B (SPI1)
+- OSD: AT7456E (SPI2)
+- Barometer: BMP280 (IIC)
+- Blackbox: Micron M25P16VP (SPI3)
+
+### Feature
+- High-performance and DSP with FPU, ARM Cortex-M7 MCU with 512 Kbytes Flash. 
+- DUAL gyro ICM20689_A and ICM20689_B, they can work together to show you better performance.
+- The 16M byte SPI flash for data logging
+- USB VCP and boot select button on board(for DFU)
+- Serial LED interface(LED_STRIP)
+- VBAT/CURR/RSSI sensors input
+- Suppose IRC Tramp/smart audio/FPV Camera Control/FPORT/telemetry
+- Supports SBus, Spektrum1024/2048, PPM. No external inverters required (built-in).
+- Supports I2C device extend(baro/compass/OLED etc)(socket)
+- Supports GPS (socket)

docs/Cli.md

@@ -282,6 +282,8 @@ A shorter form is also supported to enable and disable functions using `serial <
 |  imu_dcm_ki  | 50 | Inertial Measurement Unit KI Gain for accelerometer measurements |
 |  imu_dcm_kp_mag  | 10000 | Inertial Measurement Unit KP Gain for compass measurements |
 |  imu_dcm_ki_mag  | 0 | Inertial Measurement Unit KI Gain for compass measurements |
+|  imu_acc_ignore_rate  | 0 | Total gyro rotation rate threshold [deg/s] to consider accelerometer trustworthy on airplanes |
+|  imu_acc_ignore_slope | 0 | Half-width of the interval to gradually reduce accelerometer weight. Centered at `imu_acc_ignore_rate` (exactly 50% weight) |
 |  pos_hold_deadband  | 20 | Stick deadband in [r/c points], applied after r/c deadband and expo |
 |  alt_hold_deadband  | 50 | Defines the deadband of throttle during alt_hold [r/c points] |
 |  yaw_motor_direction  | 1 | Use if you need to inverse yaw motor direction. |

docs/Controls.md

@@ -23,8 +23,6 @@ The stick positions are combined to activate different functions:
 
 | Function                      | Throttle | Yaw     | Pitch  | Roll   |
 | ----------------------------- | -------- | ------- | ------ | ------ |
-| ARM                           | LOW      | HIGH    | CENTER | CENTER |
-| DISARM                        | LOW      | LOW     | CENTER | CENTER |
 | Profile 1                     | LOW      | LOW     | CENTER | LOW    |
 | Profile 2                     | LOW      | LOW     | HIGH   | CENTER |
 | Profile 3                     | LOW      | LOW     | CENTER | HIGH   |

src/main/blackbox/blackbox.c

@@ -576,7 +576,8 @@ static bool testBlackboxConditionUncached(FlightLogFieldCondition condition)
     case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0:
     case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_1:
     case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_2:
-        return pidBank()->pid[condition - FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0].D != 0;
+        // D output can be set by either the D or the FF term
+        return pidBank()->pid[condition - FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0].D != 0 || pidBank()->pid[condition - FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0].FF != 0;
 
     case FLIGHT_LOG_FIELD_CONDITION_MAG:
 #ifdef USE_MAG

src/main/build/version.h

@@ -17,7 +17,7 @@
 
 #define FC_VERSION_MAJOR            2  // increment when a major release is made (big new feature, etc)
 #define FC_VERSION_MINOR            2  // increment when a minor release is made (small new feature, change etc)
-#define FC_VERSION_PATCH_LEVEL      0  // increment when a bug is fixed
+#define FC_VERSION_PATCH_LEVEL      1  // increment when a bug is fixed
 
 #define STR_HELPER(x) #x
 #define STR(x) STR_HELPER(x)

src/main/drivers/accgyro/accgyro_mpu6000.c

@@ -102,11 +102,11 @@ static void mpu6000AccAndGyroInit(gyroDev_t *gyro)
     busWrite(busDev, MPU_RA_SMPLRT_DIV, config->gyroConfigValues[1]);
     delayMicroseconds(15);
 
-    // Gyro +/- 1000 DPS Full Scale
+    // Gyro +/- 2000 DPS Full Scale
     busWrite(busDev, MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3);
     delayMicroseconds(15);
 
-    // Accel +/- 8 G Full Scale
+    // Accel +/- 16 G Full Scale
     busWrite(busDev, MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3);
     delayMicroseconds(15);
 

src/main/drivers/accgyro/accgyro_mpu6050.c

@@ -87,7 +87,7 @@ static void mpu6050AccAndGyroInit(gyroDev_t *gyro)
         busWrite(gyro->busDev, MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3);
         delayMicroseconds(15);
 
-        // Accel +/- 8 G Full Scale
+        // Accel +/- 16 G Full Scale
         busWrite(gyro->busDev, MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3);
         delayMicroseconds(15);
 

src/main/drivers/usb_io.c

@@ -69,11 +69,11 @@ void usbGenerateDisconnectPulse(void)
     IO_t usbPin = IOGetByTag(IO_TAG(PA12));
     IOConfigGPIO(usbPin, IOCFG_OUT_OD);
 
-    IOHi(usbPin);
+    IOLo(usbPin);
 
     delay(200);
 
-    IOLo(usbPin);
+    IOHi(usbPin);
 }
 
 #endif

src/main/fc/settings.yaml

@@ -801,6 +801,14 @@ groups:
       - name: small_angle
         min: 0
         max: 180
+      - name: imu_acc_ignore_rate
+        field: acc_ignore_rate
+        min: 0
+        max: 20
+      - name: imu_acc_ignore_slope
+        field: acc_ignore_slope
+        min: 0
+        max: 5
 
   - name: PG_ARMING_CONFIG
     type: armingConfig_t

src/main/flight/imu.c

@@ -76,6 +76,7 @@
 
 #define SPIN_RATE_LIMIT             20
 #define MAX_ACC_SQ_NEARNESS         25      // 25% or G^2, accepted acceleration of (0.87 - 1.12G)
+#define IMU_CENTRIFUGAL_LPF         1       // Hz
 
 FASTRAM fpVector3_t imuMeasuredAccelBF;
 FASTRAM fpVector3_t imuMeasuredRotationBF;
@@ -89,17 +90,20 @@ FASTRAM attitudeEulerAngles_t attitude;             // absolute angle inclinatio
 FASTRAM float rMat[3][3];
 
 STATIC_FASTRAM imuRuntimeConfig_t imuRuntimeConfig;
+STATIC_FASTRAM pt1Filter_t rotRateFilter;
 
 STATIC_FASTRAM bool gpsHeadingInitialized;
 
-PG_REGISTER_WITH_RESET_TEMPLATE(imuConfig_t, imuConfig, PG_IMU_CONFIG, 0);
+PG_REGISTER_WITH_RESET_TEMPLATE(imuConfig_t, imuConfig, PG_IMU_CONFIG, 2);
 
 PG_RESET_TEMPLATE(imuConfig_t, imuConfig,
     .dcm_kp_acc = 2500,             // 0.25 * 10000
     .dcm_ki_acc = 50,               // 0.005 * 10000
     .dcm_kp_mag = 10000,            // 1.00 * 10000
     .dcm_ki_mag = 0,                // 0.00 * 10000
-    .small_angle = 25
+    .small_angle = 25,
+    .acc_ignore_rate = 0,
+    .acc_ignore_slope = 0
 );
 
 STATIC_UNIT_TESTED void imuComputeRotationMatrix(void)
@@ -156,6 +160,9 @@ void imuInit(void)
 
     quaternionInitUnit(&orientation);
     imuComputeRotationMatrix();
+
+    // Initialize rotation rate filter
+    pt1FilterReset(&rotRateFilter, 0);
 }
 
 void imuSetMagneticDeclination(float declinationDeg)
@@ -282,7 +289,7 @@ static void imuCheckAndResetOrientationQuaternion(const fpQuaternion_t * quat, c
 #endif
 }
 
-static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVector3_t * accBF, const fpVector3_t * magBF, bool useCOG, float courseOverGround)
+static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVector3_t * accBF, const fpVector3_t * magBF, bool useCOG, float courseOverGround, float accWScaler, float magWScaler)
 {
     STATIC_FASTRAM fpVector3_t vGyroDriftEstimate = { 0 };
 
@@ -369,7 +376,7 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe
         }
 
         // Calculate kP gain and apply proportional feedback
-        vectorScale(&vErr, &vErr, imuRuntimeConfig.dcm_kp_mag * imuGetPGainScaleFactor());
+        vectorScale(&vErr, &vErr, imuRuntimeConfig.dcm_kp_mag * magWScaler);
         vectorAdd(&vRotation, &vRotation, &vErr);
     }
 
@@ -399,7 +406,7 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe
         }
 
         // Calculate kP gain and apply proportional feedback
-        vectorScale(&vErr, &vErr, imuRuntimeConfig.dcm_kp_acc * imuGetPGainScaleFactor());
+        vectorScale(&vErr, &vErr, imuRuntimeConfig.dcm_kp_acc * accWScaler);
         vectorAdd(&vRotation, &vRotation, &vErr);
     }
 
@@ -461,8 +468,9 @@ STATIC_UNIT_TESTED void imuUpdateEulerAngles(void)
     }
 }
 
-static bool imuCanUseAccelerometerForCorrection(void)
+static float imuCalculateAccelerometerWeight(const float dT)
 {
+    // If centrifugal test passed - do the usual "nearness" style check
     float accMagnitudeSq = 0;
 
     for (int axis = 0; axis < 3; axis++) {
@@ -471,8 +479,44 @@ static bool imuCanUseAccelerometerForCorrection(void)
 
     // Magnitude^2 in percent of G^2
     const float nearness = ABS(100 - (accMagnitudeSq * 100));
+    const float accWeight_Nearness = (nearness > MAX_ACC_SQ_NEARNESS) ? 0.0f : 1.0f;
+
+    // Experiment: if rotation rate on a FIXED_WING is higher than a threshold - centrifugal force messes up too much and we 
+    // should not use measured accel for AHRS comp
+    //      Centrifugal acceleration AccelC = Omega^2 * R = Speed^2 / R
+    //          Omega = Speed / R
+    //      For a banked turn R = Speed^2 / (G * tan(Roll))
+    //          Omega = G * tan(Roll) / Speed 
+    //      Knowing the typical airspeed is around ~20 m/s we can calculate roll angles that yield certain angular rate
+    //          1 deg   =>  0.49 deg/s
+    //          2 deg   =>  0.98 deg/s
+    //          5 deg   =>  2.45 deg/s
+    //         10 deg   =>  4.96 deg/s
+    //      Therefore for a typical plane a sustained angular rate of ~2.45 deg/s will yield a banking error of ~5 deg
+    //  Since we can't do proper centrifugal compensation at the moment we pass the magnitude of angular rate through an 
+    //  LPF with a low cutoff and if it's larger than our threshold - invalidate accelerometer
+
+    // Default - don't apply rate/ignore scaling
+    float accWeight_RateIgnore = 1.0f;
+
+    if (ARMING_FLAG(ARMED) && STATE(FIXED_WING) && imuConfig()->acc_ignore_rate) {
+        const float rotRateMagnitude = sqrtf(vectorNormSquared(&imuMeasuredRotationBF));
+        const float rotRateMagnitudeFiltered = pt1FilterApply4(&rotRateFilter, rotRateMagnitude, IMU_CENTRIFUGAL_LPF, dT);
+
+        if (imuConfig()->acc_ignore_slope) {
+            const float rateSlopeMin = DEGREES_TO_RADIANS((imuConfig()->acc_ignore_rate - imuConfig()->acc_ignore_slope));
+            const float rateSlopeMax = DEGREES_TO_RADIANS((imuConfig()->acc_ignore_rate + imuConfig()->acc_ignore_slope));
+
+            accWeight_RateIgnore = scaleRangef(constrainf(rotRateMagnitudeFiltered, rateSlopeMin, rateSlopeMax), rateSlopeMin, rateSlopeMax, 1.0f, 0.0f);
+        }
+        else {
+            if (rotRateMagnitudeFiltered > DEGREES_TO_RADIANS(imuConfig()->acc_ignore_rate)) {
+                accWeight_RateIgnore = 0.0f;
+            }
+        }
+    }
 
-    return (nearness > MAX_ACC_SQ_NEARNESS) ? false : true;
+    return accWeight_Nearness * accWeight_RateIgnore;
 }
 
 static void imuCalculateEstimatedAttitude(float dT)
@@ -483,8 +527,6 @@ static void imuCalculateEstimatedAttitude(float dT)
     const bool canUseMAG = false;
 #endif
 
-    const bool useAcc = imuCanUseAccelerometerForCorrection();
-
     float courseOverGround = 0;
     bool useMag = false;
     bool useCOG = false;
@@ -528,10 +570,16 @@ static void imuCalculateEstimatedAttitude(float dT)
 
     fpVector3_t measuredMagBF = { .v = { mag.magADC[X], mag.magADC[Y], mag.magADC[Z] } };
 
+    const float magWeight = imuGetPGainScaleFactor() * 1.0f;
+    const float accWeight = imuGetPGainScaleFactor() * imuCalculateAccelerometerWeight(dT);
+    const bool useAcc = (accWeight > 0.001f);
+
     imuMahonyAHRSupdate(dT, &imuMeasuredRotationBF,
                             useAcc ? &imuMeasuredAccelBF : NULL,
                             useMag ? &measuredMagBF : NULL,
-                            useCOG, courseOverGround);
+                            useCOG, courseOverGround,
+                            accWeight,
+                            magWeight);
 
     imuUpdateEulerAngles();
 }

src/main/flight/imu.h

@@ -47,6 +47,8 @@ typedef struct imuConfig_s {
     uint16_t dcm_kp_mag;                    // DCM filter proportional gain ( x 10000) for magnetometer and GPS heading
     uint16_t dcm_ki_mag;                    // DCM filter integral gain ( x 10000) for magnetometer and GPS heading
     uint8_t small_angle;
+    uint8_t acc_ignore_rate;
+    uint8_t acc_ignore_slope;
 } imuConfig_t;
 
 PG_DECLARE(imuConfig_t, imuConfig);

src/main/flight/pid.c

@@ -981,3 +981,21 @@ void FAST_CODE pidController(void)
         }
     }
 }
+
+pidType_e pidIndexGetType(pidIndex_e pidIndex)
+{
+    if (STATE(FIXED_WING)) {
+        // FW specific
+        if (pidIndex == PID_ROLL || pidIndex == PID_PITCH || pidIndex == PID_YAW) {
+            return PID_TYPE_PIFF;
+        }
+        if (pidIndex == PID_VEL_XY || pidIndex == PID_VEL_Z) {
+            return PID_TYPE_NONE;
+        }
+    }
+    // Common
+    if (pidIndex == PID_SURFACE) {
+        return PID_TYPE_NONE;
+    }
+    return PID_TYPE_PID;
+}

src/main/flight/pid.h

@@ -68,6 +68,13 @@ typedef enum {
     PID_ITEM_COUNT
 } pidIndex_e;
 
+// TODO(agh): PIDFF
+typedef enum {
+    PID_TYPE_NONE,  // Not used in the current platform/mixer/configuration
+    PID_TYPE_PID,   // Uses P, I and D terms
+    PID_TYPE_PIFF,  // Uses P, I and FF, ignoring D
+} pidType_e;
+
 typedef struct pid8_s {
     uint8_t P;
     uint8_t I;
@@ -182,3 +189,5 @@ int16_t getHeadingHoldTarget(void);
 
 void autotuneUpdateState(void);
 void autotuneFixedWingUpdate(const flight_dynamics_index_t axis, float desiredRateDps, float reachedRateDps, float pidOutput);
+
+pidType_e pidIndexGetType(pidIndex_e pidIndex);

src/main/flight/pid_autotune.c

@@ -74,7 +74,7 @@ typedef struct {
     bool    pidSaturated;
     float   gainP;
     float   gainI;
-    float   gainD;
+    float   gainFF;
 } pidAutotuneData_t;
 
 #define AUTOTUNE_SAVE_PERIOD        5000        // Save interval is 5 seconds - when we turn off autotune we'll restore values from previous update at most 5 sec ago
@@ -90,7 +90,8 @@ void autotuneUpdateGains(pidAutotuneData_t * data)
     for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
         pidBankMutable()->pid[axis].P = lrintf(data[axis].gainP);
         pidBankMutable()->pid[axis].I = lrintf(data[axis].gainI);
-        pidBankMutable()->pid[axis].D = lrintf(data[axis].gainD);
+        pidBankMutable()->pid[axis].D = 0;
+        pidBankMutable()->pid[axis].FF = lrintf(data[axis].gainFF);
     }
     schedulePidGainsUpdate();
 }
@@ -114,7 +115,7 @@ void autotuneStart(void)
     for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
         tuneCurrent[axis].gainP = pidBank()->pid[axis].P;
         tuneCurrent[axis].gainI = pidBank()->pid[axis].I;
-        tuneCurrent[axis].gainD = pidBank()->pid[axis].D;
+        tuneCurrent[axis].gainFF = pidBank()->pid[axis].FF;
         tuneCurrent[axis].pidSaturated = false;
         tuneCurrent[axis].stateEnterTime = millis();
         tuneCurrent[axis].state = DEMAND_TOO_LOW;
@@ -200,18 +201,18 @@ void autotuneFixedWingUpdate(const flight_dynamics_index_t axis, float desiredRa
                 break;
             case DEMAND_OVERSHOOT:
                 if (stateTimeMs >= pidAutotuneConfig()->fw_overshoot_time) {
-                    tuneCurrent[axis].gainD = tuneCurrent[axis].gainD * (100 - AUTOTUNE_FIXED_WING_DECREASE_STEP) / 100.0f;
-                    if (tuneCurrent[axis].gainD < AUTOTUNE_FIXED_WING_MIN_FF) {
-                        tuneCurrent[axis].gainD = AUTOTUNE_FIXED_WING_MIN_FF;
+                    tuneCurrent[axis].gainFF = tuneCurrent[axis].gainFF * (100 - AUTOTUNE_FIXED_WING_DECREASE_STEP) / 100.0f;
+                    if (tuneCurrent[axis].gainFF < AUTOTUNE_FIXED_WING_MIN_FF) {
+                        tuneCurrent[axis].gainFF = AUTOTUNE_FIXED_WING_MIN_FF;
                     }
                     gainsUpdated = true;
                 }
                 break;
             case DEMAND_UNDERSHOOT:
                 if (stateTimeMs >= pidAutotuneConfig()->fw_undershoot_time && !tuneCurrent[axis].pidSaturated) {
-                    tuneCurrent[axis].gainD = tuneCurrent[axis].gainD * (100 + AUTOTUNE_FIXED_WING_INCREASE_STEP) / 100.0f;
-                    if (tuneCurrent[axis].gainD > AUTOTUNE_FIXED_WING_MAX_FF) {
-                        tuneCurrent[axis].gainD = AUTOTUNE_FIXED_WING_MAX_FF;
+                    tuneCurrent[axis].gainFF = tuneCurrent[axis].gainFF * (100 + AUTOTUNE_FIXED_WING_INCREASE_STEP) / 100.0f;
+                    if (tuneCurrent[axis].gainFF > AUTOTUNE_FIXED_WING_MAX_FF) {
+                        tuneCurrent[axis].gainFF = AUTOTUNE_FIXED_WING_MAX_FF;
                     }
                     gainsUpdated = true;
                 }
@@ -220,24 +221,24 @@ void autotuneFixedWingUpdate(const flight_dynamics_index_t axis, float desiredRa
 
         if (gainsUpdated) {
             // Set P-gain to 10% of FF gain (quite agressive - FIXME)
-            tuneCurrent[axis].gainP = tuneCurrent[axis].gainD * (pidAutotuneConfig()->fw_ff_to_p_gain / 100.0f);
+            tuneCurrent[axis].gainP = tuneCurrent[axis].gainFF * (pidAutotuneConfig()->fw_ff_to_p_gain / 100.0f);
 
             // Set integrator gain to reach the same response as FF gain in 0.667 second
-            tuneCurrent[axis].gainI = (tuneCurrent[axis].gainD / FP_PID_RATE_FF_MULTIPLIER) * (1000.0f / pidAutotuneConfig()->fw_ff_to_i_time_constant) * FP_PID_RATE_I_MULTIPLIER;
+            tuneCurrent[axis].gainI = (tuneCurrent[axis].gainFF / FP_PID_RATE_FF_MULTIPLIER) * (1000.0f / pidAutotuneConfig()->fw_ff_to_i_time_constant) * FP_PID_RATE_I_MULTIPLIER;
             tuneCurrent[axis].gainI = constrainf(tuneCurrent[axis].gainI, 2.0f, 50.0f);
             autotuneUpdateGains(tuneCurrent);
 
             switch (axis) {
             case FD_ROLL:
-                blackboxLogAutotuneEvent(ADJUSTMENT_ROLL_D, tuneCurrent[axis].gainD);
+                blackboxLogAutotuneEvent(ADJUSTMENT_ROLL_D, tuneCurrent[axis].gainFF);
                 break;
 
             case FD_PITCH:
-                blackboxLogAutotuneEvent(ADJUSTMENT_PITCH_D, tuneCurrent[axis].gainD);
+                blackboxLogAutotuneEvent(ADJUSTMENT_PITCH_D, tuneCurrent[axis].gainFF);
                 break;
 
             case FD_YAW:
-                blackboxLogAutotuneEvent(ADJUSTMENT_YAW_D, tuneCurrent[axis].gainD);
+                blackboxLogAutotuneEvent(ADJUSTMENT_YAW_D, tuneCurrent[axis].gainFF);
                 break;
             }
         }