diff --git a/src/main/drivers/system.c b/src/main/drivers/system.c index b27a917d9..a3eee7939 100644 --- a/src/main/drivers/system.c +++ b/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(); diff --git a/src/main/drivers/system.h b/src/main/drivers/system.h index 4a5b83535..6e52268d6 100644 --- a/src/main/drivers/system.h +++ b/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 diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 3aa78b71d..2723f7c27 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -17,6 +17,7 @@ #include #include +#include #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); + } + } } } diff --git a/src/main/fc/fc_core.h b/src/main/fc/fc_core.h index 8bc8b4253..b7e0d6dcb 100644 --- a/src/main/fc/fc_core.h +++ b/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); diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index c76f936b2..2858710c3 100644 --- a/src/main/fc/fc_init.c +++ b/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 diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c index f90445703..4cc665034 100644 --- a/src/main/fc/rc_controls.c +++ b/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(); } } diff --git a/src/main/io/beeper.c b/src/main/io/beeper.c index 7b3a9d6e7..30ff2eb52 100644 --- a/src/main/io/beeper.c +++ b/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;} diff --git a/src/main/io/beeper.h b/src/main/io/beeper.h index 8a9c0430e..c48c8b4ea 100644 --- a/src/main/io/beeper.h +++ b/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); diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 977786da3..cb2cd46ef 100644 --- a/src/main/sensors/gyro.c +++ b/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; diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index ba7693e58..ce29ea471 100644 --- a/src/main/sensors/gyro.h +++ b/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); diff --git a/src/main/target/SITL/target.c b/src/main/target/SITL/target.c index 1e573f77f..cab463fec 100644 --- a/src/main/target/SITL/target.c +++ b/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); diff --git a/src/test/unit/rc_controls_unittest.cc b/src/test/unit/rc_controls_unittest.cc index 75f8e8358..edd846db6 100644 --- a/src/test/unit/rc_controls_unittest.cc +++ b/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) {} }