rotor sense, flag set/unset and dshot beacon override

am32-firmware · Nov 24, 2024 · 9bc5f57 · 9bc5f57
1 parent 7b3021c
commit 9bc5f57
Show file tree

Hide file tree

Showing 4 changed files with 50 additions and 10 deletions.
diff --git a/Inc/functions.h b/Inc/functions.h
@@ -9,6 +9,7 @@
 #define FUNCTIONS_H_
 
 #include "main.h"
+#include "targets.h"
 #include <stdbool.h>
 
 int findIndex(uint8_t *array, uint8_t size, uint8_t target);

diff --git a/Inc/version.h b/Inc/version.h
@@ -4,4 +4,4 @@
 #define VERSION_MAJOR 2
 #define VERSION_MINOR 17
 
-#define EEPROM_VERSION 2
+#define EEPROM_VERSION 3
diff --git a/Src/main.c b/Src/main.c
@@ -396,6 +396,9 @@ char fast_deccel = 0;
 uint16_t last_duty_cycle = 0;
 uint16_t duty_cycle_setpoint = 0;
 char play_tone_flag = 0;
+#ifndef DISABLE_ROTOR_SENSE
+uint8_t times_tone_played = 0;
+#endif
 
 typedef enum { GPIO_PIN_RESET = 0U,
     GPIO_PIN_SET } GPIO_PinState;
@@ -434,6 +437,7 @@ char prop_brake_active = 0;
 
 char dshot_telemetry = 0;
 
+uint32_t consecutive_same_command_time = 0;
 uint8_t last_dshot_command = 0;
 char old_routine = 1;
 uint16_t adjusted_input = 0;
@@ -579,6 +583,13 @@ float doPidCalculations(struct fastPID* pidnow, int actual, int target)
     return pidnow->pid_output;
 }
 
+void migrateEEpromSettings(void)
+{
+    if (eepromBuffer.eeprom_version == 2 && EEPROM_VERSION == 3) {
+        eepromBuffer.flags = (uint16_t)0x0;
+    }
+}
+
 void loadEEpromSettings()
 {
     read_flash_bin(eepromBuffer.buffer, eeprom_address, sizeof(eepromBuffer.buffer));
@@ -1095,6 +1106,9 @@ if (!stepper_sine && armed) {
 
         if (input < 47 + (80 * eepromBuffer.use_sine_start)) {
             if (play_tone_flag != 0) {
+                #ifndef DISABLE_ROTOR_SENSE
+                times_tone_played++;
+                #endif
                 switch (play_tone_flag) {
 
                 case 1:
@@ -1115,6 +1129,16 @@ if (!stepper_sine && armed) {
                 }
                 play_tone_flag = 0;
             }
+            #ifndef DISABLE_ROTOR_SENSE
+            else {
+                times_tone_played = 0;
+            }
+
+            if (times_tone_played > 9 && (eepromBuffer.flags & ROTOR_SENSE_DONE) == AM32_FLAG_SET) {
+                eepromBuffer.flags &= ~ROTOR_SENSE_DONE;
+                playRotorSenseSaveTune();
+            }
+            #endif
 
             if (!eepromBuffer.comp_pwm) {
                 duty_cycle_setpoint = 0;
@@ -1208,13 +1232,15 @@ if (!stepper_sine && armed) {
 #endif
 }
 
+#ifndef DISABLE_ROTOR_SENSE
 uint8_t rotorSenseValue = 0;
 uint8_t rotorSenseValueLast = 0;
 uint8_t rotorSenseFalseDetected = 0;
 uint8_t rotorSenseSequence[6] = {5, 4, 6, 2, 3, 1};
 uint8_t rotorSenseReversedSequence[6] = {1, 3, 2, 6, 4, 5};
 uint8_t rotorSenseDetectedSequence[6] = {0, 0, 0, 0, 0, 0};
 uint8_t rotorSenseDetectedSequenceIndex = 0;
+#endif
 
 void tenKhzRoutine()
 { // 20khz as of 2.00 to be renamed
@@ -1267,20 +1293,22 @@ void tenKhzRoutine()
                 }
             }
         }
-    } else if (!running && (eepromBuffer.flags & ROTOR_SENSE_DONE) == AM32_FLAG_UNSET) {
+    }
+#ifndef DISABLE_ROTOR_SENSE
+    if (!running && (eepromBuffer.flags & ROTOR_SENSE_DONE) == AM32_FLAG_UNSET) {
         step = 1;
         changeCompInput();
-        delayMicros(1);
+        delayMicros(5);
         RELOAD_WATCHDOG_COUNTER();
         rotorSenseValue = getCompOutputLevel();
         step = 2;
         changeCompInput();
-        delayMicros(1);
+        delayMicros(5);
         RELOAD_WATCHDOG_COUNTER();
         rotorSenseValue = getCompOutputLevel() << 1 | rotorSenseValue;
         step = 3;
         changeCompInput();
-        delayMicros(1);
+        delayMicros(5);
         RELOAD_WATCHDOG_COUNTER();
         rotorSenseValue = getCompOutputLevel() << 2 | rotorSenseValue;
 
@@ -1289,16 +1317,16 @@ void tenKhzRoutine()
 
             if (rotorSenseDetectedSequenceIndex > 5) {
                 if (searchSequence(rotorSenseDetectedSequence, 6, rotorSenseSequence, 6)) {
-                    playNonReversedTune();
-                    if (dir_reversed == 1) {
+                    if (eepromBuffer.dir_reversed == 1) {
+                        playNonReversedTune();
                         rotorSenseFalseDetected++;
                     } else {
                         rotorSenseFalseDetected = 0;
                     }
                 } else {
                     if (searchSequence(rotorSenseDetectedSequence, 6, rotorSenseReversedSequence, 6)) {
-                        playReversedTune();
-                        if (dir_reversed == 0) {
+                        if (eepromBuffer.dir_reversed == 0) {
+                            playReversedTune();
                             rotorSenseFalseDetected++;
                         } else {
                             rotorSenseFalseDetected = 0;
@@ -1307,7 +1335,8 @@ void tenKhzRoutine()
                 }
 
                 if (rotorSenseFalseDetected == 2) {
-                    dir_reversed = (dir_reversed + 1) % 2;
+                    eepromBuffer.dir_reversed = (eepromBuffer.dir_reversed + 1) % 2;
+                    eepromBuffer.flags |= ROTOR_SENSE_DONE;
                     saveEEpromSettings();
                     playRotorSenseSaveTune();
                     rotorSenseFalseDetected = 0;
@@ -1323,6 +1352,7 @@ void tenKhzRoutine()
             rotorSenseValueLast = rotorSenseValue;
         }
     }
+#endif
 
     if (eepromBuffer.telementry_on_interval) {
         telem_ms_count++;
@@ -1683,6 +1713,11 @@ int main(void)
 
     loadEEpromSettings();
 
+    if (eepromBuffer.eeprom_version < EEPROM_VERSION) {
+        migrateEEpromSettings();
+        saveEEpromSettings();
+    }
+
 #ifdef USE_MAKE
     if (
         firmware_info.version_major != eepromBuffer.version.major ||
@@ -1784,8 +1819,10 @@ int main(void)
 	#endif
 #endif
     zero_input_count = 0;
+#ifndef DISABLE_WATCHDOG
     MX_IWDG_Init();
     RELOAD_WATCHDOG_COUNTER();
+#endif
 #ifdef GIMBAL_MODE
     eepromBuffer.bi_direction = 1;
     eepromBuffer.use_sine_start = 1;

diff --git a/Src/sounds.c b/Src/sounds.c
@@ -120,6 +120,7 @@ void playStartupTune()
     __enable_irq();
 }
 
+#ifndef DISABLE_ROTOR_SENSE
 void playReversedTune()
 {
     __disable_irq();
@@ -188,6 +189,7 @@ void playNonReversedTune()
 
     __enable_irq();
 }
+#endif
 
 void playBrushedStartupTune()
 {