Merge pull request #3412 from mikeller/added_arming_disable_beeps

Added beeps to indicate the reason for arming being disabled.

src/main/drivers/system.c

@@ -162,49 +162,45 @@ void delay(uint32_t ms)
 }
 
 #define SHORT_FLASH_DURATION 50
+#define SHORT_FLASH_COUNT 5
 #define CODE_FLASH_DURATION 250
 
-void failureLedCode(failureMode_e mode, int codeRepeatsRemaining)
+static void indicate(uint8_t count, uint16_t duration)
 {
-    int codeFlashesRemaining;
-    int shortFlashesRemaining;
-
-    while (codeRepeatsRemaining--) {
+    if (count) {
         LED1_ON;
         LED0_OFF;
-        shortFlashesRemaining = 5;
-        codeFlashesRemaining = mode + 1;
-        uint8_t flashDuration = SHORT_FLASH_DURATION;
 
-        while (shortFlashesRemaining || codeFlashesRemaining) {
+        while (count--) {
             LED1_TOGGLE;
             LED0_TOGGLE;
             BEEP_ON;
-            delay(flashDuration);
+            delay(duration);
 
             LED1_TOGGLE;
             LED0_TOGGLE;
             BEEP_OFF;
-            delay(flashDuration);
-
-            if (shortFlashesRemaining) {
-                shortFlashesRemaining--;
-                if (shortFlashesRemaining == 0) {
-                    delay(500);
-                    flashDuration = CODE_FLASH_DURATION;
-                }
-            } else {
-                codeFlashesRemaining--;
-            }
+            delay(duration);
         }
+    }
+}
+
+void indicateFailure(failureMode_e mode, int codeRepeatsRemaining)
+{
+    while (codeRepeatsRemaining--) {
+        indicate(SHORT_FLASH_COUNT, SHORT_FLASH_DURATION);
+
+        delay(500);
+
+        indicate(mode + 1, CODE_FLASH_DURATION);
+
         delay(1000);
     }
-
 }
 
 void failureMode(failureMode_e mode)
 {
-    failureLedCode(mode, 10);
+    indicateFailure(mode, 10);
 
 #ifdef DEBUG
     systemReset();

src/main/drivers/system.h

@@ -33,7 +33,7 @@ typedef enum {
 } failureMode_e;
 
 // failure
-void failureLedCode(failureMode_e mode, int repeatCount);
+void indicateFailure(failureMode_e mode, int repeatCount);
 void failureMode(failureMode_e mode);
 
 // bootloader/IAP

src/main/fc/fc_core.c

@@ -17,6 +17,7 @@
 
 #include <stdbool.h>
 #include <stdint.h>
+#include <string.h>
 
 #include "platform.h"
 
@@ -35,6 +36,7 @@
 
 #include "drivers/gyro_sync.h"
 #include "drivers/light_led.h"
+#include "drivers/system.h"
 #include "drivers/time.h"
 #include "drivers/transponder_ir.h"
 
@@ -111,7 +113,7 @@ static bool reverseMotors = false;
 static uint32_t disarmAt;     // Time of automatic disarm when "Don't spin the motors when armed" is enabled and auto_disarm_delay is nonzero
 
 bool isRXDataNew;
-static bool armingCalibrationWasInitialised;
+static int lastArmingDisabledReason = 0;
 
 PG_REGISTER_WITH_RESET_TEMPLATE(throttleCorrectionConfig_t, throttleCorrectionConfig, PG_THROTTLE_CORRECTION_CONFIG, 0);
 
@@ -141,6 +143,11 @@ static bool isCalibrating()
     return (!isAccelerationCalibrationComplete() && sensors(SENSOR_ACC)) || (!isGyroCalibrationComplete());
 }
 
+void resetArmingDisabled(void)
+{
+    lastArmingDisabledReason = 0;
+}
+
 void updateArmingStatus(void)
 {
     if (ARMING_FLAG(ARMED)) {
@@ -188,8 +195,6 @@ void updateArmingStatus(void)
 
 void disarm(void)
 {
-    armingCalibrationWasInitialised = false;
-
     if (ARMING_FLAG(ARMED)) {
         DISABLE_ARMING_FLAG(ARMED);
 
@@ -205,12 +210,8 @@ void disarm(void)
 
 void tryArm(void)
 {
-    static bool firstArmingCalibrationWasCompleted;
-
-    if (armingConfig()->gyro_cal_on_first_arm && !firstArmingCalibrationWasCompleted) {
-        gyroStartCalibration();
-        armingCalibrationWasInitialised = true;
-        firstArmingCalibrationWasCompleted = true;
+    if (armingConfig()->gyro_cal_on_first_arm) {
+        gyroStartCalibration(true);
     }
 
     updateArmingStatus();
@@ -233,7 +234,7 @@ void tryArm(void)
                     pwmWriteDshotCommand(index, DSHOT_CMD_SPIN_DIRECTION_REVERSED);
                 }
             }
-	}
+	    }
 #endif
 
         ENABLE_ARMING_FLAG(ARMED);
@@ -242,6 +243,8 @@ void tryArm(void)
 
         disarmAt = millis() + armingConfig()->auto_disarm_delay * 1000;   // start disarm timeout, will be extended when throttle is nonzero
 
+        lastArmingDisabledReason = 0;
+
         //beep to indicate arming
 #ifdef GPS
         if (feature(FEATURE_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 5) {
@@ -252,12 +255,15 @@ void tryArm(void)
 #else
         beeper(BEEPER_ARMING);
 #endif
+    } else {
+        if (!isFirstArmingGyroCalibrationRunning()) {
+            int armingDisabledReason = ffs(getArmingDisableFlags());
+            if (lastArmingDisabledReason != armingDisabledReason) {
+                lastArmingDisabledReason = armingDisabledReason;
 
-        return;
-    }
-
-    if (!ARMING_FLAG(ARMED)) {
-        beeperConfirmationBeeps(1);
+                beeperWarningBeeps(armingDisabledReason);
+            }
+        }
     }
 }
 

src/main/fc/fc_core.h

@@ -40,6 +40,8 @@ union rollAndPitchTrims_u;
 void applyAndSaveAccelerometerTrimsDelta(union rollAndPitchTrims_u *rollAndPitchTrimsDelta);
 void handleInflightCalibrationStickPosition();
 
+void resetArmingDisabled(void);
+
 void disarm(void);
 void tryArm(void);
 

src/main/fc/fc_init.c

@@ -481,7 +481,7 @@ void init(void)
 
     if (!sensorsAutodetect()) {
         // if gyro was not detected due to whatever reason, notify and don't arm.
-        failureLedCode(FAILURE_MISSING_ACC, 2);
+        indicateFailure(FAILURE_MISSING_ACC, 2);
         setArmingDisabled(ARMING_DISABLED_NO_GYRO);
     }
 
@@ -632,7 +632,7 @@ void init(void)
     if (mixerConfig()->mixerMode == MIXER_GIMBAL) {
         accSetCalibrationCycles(CALIBRATING_ACC_CYCLES);
     }
-    gyroStartCalibration();
+    gyroStartCalibration(false);
 #ifdef BARO
     baroSetCalibrationCycles(CALIBRATING_BARO_CYCLES);
 #endif

src/main/fc/rc_controls.c

@@ -147,6 +147,7 @@ void processRcStickPositions(throttleStatus_e throttleStatus)
             tryArm();
         } else {
             // Disarming via ARM BOX
+            resetArmingDisabled();
 
             if (ARMING_FLAG(ARMED) && rxIsReceivingSignal() && !failsafeIsActive()  ) {
                 rcDisarmTicks++;
@@ -187,7 +188,7 @@ void processRcStickPositions(throttleStatus_e throttleStatus)
 
     if (rcSticks == THR_LO + YAW_LO + PIT_LO + ROL_CE) {
         // GYRO calibration
-        gyroStartCalibration();
+        gyroStartCalibration(false);
 
 #ifdef GPS
         if (feature(FEATURE_GPS)) {
@@ -232,6 +233,8 @@ void processRcStickPositions(throttleStatus_e throttleStatus)
             tryArm();
 
             return;
+        } else {
+            resetArmingDisabled();
         }
     }
 

src/main/io/beeper.c

@@ -78,7 +78,7 @@ PG_RESET_TEMPLATE(beeperDevConfig_t, beeperDevConfig,
 #define BEEPER_NAMES
 #endif
 
-#define MAX_MULTI_BEEPS 20   //size limit for 'beep_multiBeeps[]'
+#define MAX_MULTI_BEEPS 64   //size limit for 'beep_multiBeeps[]'
 
 #define BEEPER_COMMAND_REPEAT 0xFE
 #define BEEPER_COMMAND_STOP   0xFF
@@ -151,11 +151,15 @@ static const uint8_t beep_gyroCalibrated[] = {
 };
 
 // array used for variable # of beeps (reporting GPS sat count, etc)
-static uint8_t beep_multiBeeps[MAX_MULTI_BEEPS + 2];
+static uint8_t beep_multiBeeps[MAX_MULTI_BEEPS + 1];
 
 #define BEEPER_CONFIRMATION_BEEP_DURATION 2
 #define BEEPER_CONFIRMATION_BEEP_GAP_DURATION 20
 
+#define BEEPER_WARNING_BEEP_1_DURATION 20
+#define BEEPER_WARNING_BEEP_2_DURATION 5
+#define BEEPER_WARNING_BEEP_GAP_DURATION 10
+
 // Beeper off = 0 Beeper on = 1
 static uint8_t beeperIsOn = 0;
 
@@ -270,25 +274,43 @@ void beeperSilence(void)
 
     currentBeeperEntry = NULL;
 }
+
 /*
  * Emits the given number of 20ms beeps (with 200ms spacing).
  * This function returns immediately (does not block).
  */
 void beeperConfirmationBeeps(uint8_t beepCount)
 {
-    int i;
-    int cLimit;
-
-    i = 0;
-    cLimit = beepCount * 2;
-    if(cLimit > MAX_MULTI_BEEPS)
-        cLimit = MAX_MULTI_BEEPS;  //stay within array size
+    uint32_t i = 0;
+    uint32_t cLimit = beepCount * 2;
+    if (cLimit > MAX_MULTI_BEEPS) {
+        cLimit = MAX_MULTI_BEEPS;
+    }
     do {
-        beep_multiBeeps[i++] = BEEPER_CONFIRMATION_BEEP_DURATION;       // 20ms beep
-        beep_multiBeeps[i++] = BEEPER_CONFIRMATION_BEEP_GAP_DURATION;   // 200ms pause
+        beep_multiBeeps[i++] = BEEPER_CONFIRMATION_BEEP_DURATION;
+        beep_multiBeeps[i++] = BEEPER_CONFIRMATION_BEEP_GAP_DURATION;
     } while (i < cLimit);
-    beep_multiBeeps[i] = BEEPER_COMMAND_STOP;     //sequence end
-    beeper(BEEPER_MULTI_BEEPS);    //initiate sequence
+    beep_multiBeeps[i] = BEEPER_COMMAND_STOP;
+
+    beeper(BEEPER_MULTI_BEEPS);
+}
+
+void beeperWarningBeeps(uint8_t beepCount)
+{
+    uint32_t i = 0;
+    uint32_t cLimit = beepCount * 4;
+    if (cLimit >= MAX_MULTI_BEEPS) {
+        cLimit = MAX_MULTI_BEEPS;
+    }
+    do {
+        beep_multiBeeps[i++] = BEEPER_WARNING_BEEP_1_DURATION;
+        beep_multiBeeps[i++] = BEEPER_WARNING_BEEP_GAP_DURATION;
+        beep_multiBeeps[i++] = BEEPER_WARNING_BEEP_2_DURATION;
+        beep_multiBeeps[i++] = BEEPER_WARNING_BEEP_GAP_DURATION;
+    } while (i < cLimit);
+    beep_multiBeeps[i] = BEEPER_COMMAND_STOP;
+
+    beeper(BEEPER_MULTI_BEEPS);
 }
 
 #ifdef GPS
@@ -302,7 +324,7 @@ void beeperGpsStatus(void)
             beep_multiBeeps[i++] = 10;
         } while (i < MAX_MULTI_BEEPS && gpsSol.numSat > i / 2);
 
-        beep_multiBeeps[i-1] = 50; // extend last pause
+        beep_multiBeeps[i - 1] = 50; // extend last pause
         beep_multiBeeps[i] = BEEPER_COMMAND_STOP;
 
         beeper(BEEPER_MULTI_BEEPS);    //initiate sequence
@@ -444,6 +466,7 @@ bool isBeeperOn(void)
 void beeper(beeperMode_e mode) {UNUSED(mode);}
 void beeperSilence(void) {}
 void beeperConfirmationBeeps(uint8_t beepCount) {UNUSED(beepCount);}
+void beeperWarningBeeps(uint8_t beepCount) {UNUSED(beepCount);}
 void beeperUpdate(timeUs_t currentTimeUs) {UNUSED(currentTimeUs);}
 uint32_t getArmingBeepTimeMicros(void) {return 0;}
 beeperMode_e beeperModeForTableIndex(int idx) {UNUSED(idx); return BEEPER_SILENCE;}

src/main/io/beeper.h

@@ -61,6 +61,7 @@ void beeper(beeperMode_e mode);
 void beeperSilence(void);
 void beeperUpdate(timeUs_t currentTimeUs);
 void beeperConfirmationBeeps(uint8_t beepCount);
+void beeperWarningBeeps(uint8_t beepCount);
 uint32_t getArmingBeepTimeMicros(void);
 beeperMode_e beeperModeForTableIndex(int idx);
 const char *beeperNameForTableIndex(int idx);

src/main/sensors/gyro.c

@@ -77,6 +77,8 @@ typedef struct gyroCalibration_s {
     uint16_t calibratingG;
 } gyroCalibration_t;
 
+bool firstArmingCalibrationWasStarted = false;
+
 typedef union gyroSoftFilter_u {
     biquadFilter_t gyroFilterLpfState[XYZ_AXIS_COUNT];
     pt1Filter_t gyroFilterPt1State[XYZ_AXIS_COUNT];
@@ -489,9 +491,20 @@ static void gyroSetCalibrationCycles(gyroSensor_t *gyroSensor)
     gyroSensor->calibration.calibratingG = gyroCalculateCalibratingCycles();
 }
 
-void gyroStartCalibration(void)
+void gyroStartCalibration(bool isFirstArmingCalibration)
 {
-    gyroSetCalibrationCycles(&gyroSensor0);
+    if (!(isFirstArmingCalibration && firstArmingCalibrationWasStarted)) {
+        gyroSetCalibrationCycles(&gyroSensor0);
+
+        if (isFirstArmingCalibration) {
+            firstArmingCalibrationWasStarted = true;
+        }
+    }
+}
+
+bool isFirstArmingGyroCalibrationRunning(void)
+{
+    return firstArmingCalibrationWasStarted && !isGyroCalibrationComplete();
 }
 
 STATIC_UNIT_TESTED void performGyroCalibration(gyroSensor_t *gyroSensor, uint8_t gyroMovementCalibrationThreshold)
@@ -525,7 +538,9 @@ STATIC_UNIT_TESTED void performGyroCalibration(gyroSensor_t *gyroSensor, uint8_t
 
     if (isOnFinalGyroCalibrationCycle(&gyroSensor->calibration)) {
         schedulerResetTaskStatistics(TASK_SELF); // so calibration cycles do not pollute tasks statistics
-        beeper(BEEPER_GYRO_CALIBRATED);
+        if (!firstArmingCalibrationWasStarted || !isArmingDisabled()) {
+            beeper(BEEPER_GYRO_CALIBRATED);
+        }
     }
     --gyroSensor->calibration.calibratingG;
 

src/main/sensors/gyro.h

@@ -74,7 +74,8 @@ struct mpuConfiguration_s;
 const struct mpuConfiguration_s *gyroMpuConfiguration(void);
 struct mpuDetectionResult_s;
 const struct mpuDetectionResult_s *gyroMpuDetectionResult(void);
-void gyroStartCalibration(void);
+void gyroStartCalibration(bool isFirstArmingCalibration);
+bool isFirstArmingGyroCalibrationRunning(void);
 bool isGyroCalibrationComplete(void);
 void gyroReadTemperature(void);
 int16_t gyroGetTemperature(void);

src/main/target/SITL/target.c

@@ -269,7 +269,7 @@ void failureMode(failureMode_e mode) {
     while(1);
 }
 
-void failureLedCode(failureMode_e mode, int repeatCount)
+void indicateFailure(failureMode_e mode, int repeatCount)
 {
     UNUSED(repeatCount);
     printf("Failure LED flash for: [failureMode]!!! %d\n", mode);	

src/test/unit/rc_controls_unittest.cc

@@ -701,4 +701,5 @@ uint8_t stateFlags = 0;
 int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];
 rxRuntimeConfig_t rxRuntimeConfig;
 PG_REGISTER(blackboxConfig_t, blackboxConfig, PG_BLACKBOX_CONFIG, 0);
+void resetArmingDisabled(void) {}
 }