rotor sense, flag set/unset and dshot beacon override

am32-firmware · Nov 25, 2024 · 796f2e1 · 796f2e1
1 parent 5774244
commit 796f2e1
Show file tree

Hide file tree

Showing 5 changed files with 49 additions and 22 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/Mcu/f421/Src/peripherals.c b/Mcu/f421/Src/peripherals.c
@@ -73,20 +73,8 @@ void AT_COMP_Init(void)
     crm_periph_clock_enable(CRM_CMP_PERIPH_CLOCK, TRUE);
     gpio_mode_QUICK(GPIOA, GPIO_MODE_ANALOG, GPIO_PULL_NONE, GPIO_PINS_1);
     gpio_mode_QUICK(GPIOA, GPIO_MODE_ANALOG, GPIO_PULL_NONE, GPIO_PINS_5);
-
-    cmp_init_type cmp_init_struct;
-    cmp_default_para_init(&cmp_init_struct);
-    cmp_init_struct.cmp_inverting = CMP_INVERTING_1_4VREFINT;
-    cmp_init_struct.cmp_output = CMP_OUTPUT_NONE;
-    cmp_init_struct.cmp_polarity = CMP_POL_NON_INVERTING;
-    cmp_init_struct.cmp_speed = CMP_SPEED_MEDIUM;
-    cmp_init_struct.cmp_hysteresis = CMP_HYSTERESIS_NONE;
-
-    cmp_init(CMP1_SELECTION, &cmp_init_struct);
-
     NVIC_SetPriority(ADC1_CMP_IRQn, 0);
     NVIC_EnableIRQ(ADC1_CMP_IRQn);
-
     cmp_enable(CMP1_SELECTION, TRUE);
 }
 

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;
@@ -579,6 +582,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 +1105,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 +1128,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 +1231,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 +1292,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 +1316,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 +1334,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 +1351,7 @@ void tenKhzRoutine()
             rotorSenseValueLast = rotorSenseValue;
         }
     }
+#endif
 
     if (eepromBuffer.telementry_on_interval) {
         telem_ms_count++;
@@ -1683,6 +1712,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 +1818,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()
 {