IRC rotor sense

Inc/eeprom.h

@@ -47,7 +47,8 @@ typedef union EEprom_u {
         uint8_t sine_mode_power; // 45
         uint8_t input_type; // 46
         uint8_t auto_advance; // 47
-        uint8_t reserved_2[4]; //48-51
+        uint16_t flags; // 48-49
+        uint8_t reserved_2[2]; //50-51
         uint8_t tune[124]; // 52-175
         struct {
             uint8_t can_node; // 176
@@ -64,6 +65,15 @@ typedef union EEprom_u {
     uint8_t buffer[192];
 } EEprom_t;
 
+typedef enum am32_flags_e {
+    ROTOR_SENSE_DONE = 1 << 0,
+    // all flags must be lower than FLAGS_COUNT (uint16_t flags)
+    AM32_FLAGS_COUNT = 1 << 16
+} am32_flags_t;
+
+#define AM32_FLAG_SET 0x1
+#define AM32_FLAG_UNSET 0x0
+
 extern EEprom_t eepromBuffer;
 
 // void save_to_flash(uint8_t *data);

Inc/functions.h

@@ -10,7 +10,10 @@
 
 #include "main.h"
 #include "targets.h"
+#include <stdbool.h>
 
+int findIndex(uint8_t *array, uint8_t size, uint8_t target);
+bool searchSequence(uint8_t array[], uint8_t size, uint8_t sequence[], uint8_t sequenceSize);
 uint32_t getAbsDif(int number1, int number2);
 void delayMicros(uint32_t micros);
 void delayMillis(uint32_t millis);
@@ -21,4 +24,35 @@ void gpio_mode_QUICK(gpio_type* gpio_periph, uint32_t mode,
 void gpio_mode_set(gpio_type* gpio_periph, uint32_t mode, uint32_t pull_up_down,
     uint32_t pin);
 #endif
+
+#define REVERSE_ARRAY(arr, size) do{\
+    int start = 0;\
+    int end = size - 1;\
+    while (start < end) {\
+        int temp = arr[start];\
+        arr[start] = arr[end];\
+        arr[end] = temp;\
+        start++;\
+        end--;\
+    }\
+} while(0)
+
+#define ZERO_ANY(T, a, n) do{\
+   T *a_ = (a);\
+   uint8_t n_ = (n);\
+   for (; n_ > 0; --n_, ++a_)\
+     *a_ = (T) { 0 };\
+} while (0)
+
+#define ARRAY_SIZE(a) (sizeof (a) / sizeof *(a))
+#define ZERO_ANY_A(T, a) ZERO_ANY(T, (a), ARRAY_SIZE(a))
+
+#define ZERO(a, n) do{\
+   uint8_t i_ = 0, n_ = (n);\
+   for (; i_ < n_; ++i_)\
+     (a)[i_] = 0;\
+} while (0)
+
+#define ZERO_A(a) ZERO((a), ARRAY_SIZE(a))
+
 #endif /* FUNCTIONS_H_ */

Inc/sounds.h

@@ -19,6 +19,10 @@ void playDuskingTune(void);
 void playDefaultTone(void);
 void playChangedTone(void);
 
+void playNonReversedTune(void);
+void playReversedTune(void);
+void playRotorSenseSaveTune(void);
+
 void setVolume(uint8_t volume);
 
 extern void delayMillis(uint32_t millis);

Inc/version.h

@@ -4,4 +4,4 @@
 #define VERSION_MAJOR 2
 #define VERSION_MINOR 17
 
-#define EEPROM_VERSION 2
+#define EEPROM_VERSION 3

Src/functions.c

@@ -39,6 +39,37 @@ long map(long x, long in_min, long in_max, long out_min, long out_max)
         return map(x, in_mid + 1, in_max, out_mid, out_max);
 }
 
+int findIndex(uint8_t *array, uint8_t size, uint8_t target) 
+{
+    int i=0;
+    while((i<size) && (array[i] != target)) i++;
+
+    return (i<size) ? (i) : (-1);
+}
+
+bool searchSequence(uint8_t array[], uint8_t size, uint8_t sequence[], uint8_t sequenceSize) {
+    // Loop through the array
+    for (int i = 0; i < size; i++) {
+        // Check if the current position matches the first element of the sequence
+        if (array[i] == sequence[0]) {
+            // Check if the sequence matches starting from the current position
+            bool found = true;
+            for (int j = 0; j < sequenceSize; j++) {
+                if (array[(i + j) % size] != sequence[j]) {
+                    found = false;
+                    break;
+                }
+            }
+            if (found) {
+                // Sequence found
+                return true;
+            }
+        }
+    }
+    // Sequence not found
+    return false;
+}
+
 uint32_t getAbsDif(int number1, int number2)
 {
     int result = number1 - number2;

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,6 +1231,16 @@ 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
     duty_cycle = duty_cycle_setpoint;
@@ -1260,6 +1293,65 @@ void tenKhzRoutine()
             }
         }
     }
+#ifndef DISABLE_ROTOR_SENSE
+    if (!running && (eepromBuffer.flags & ROTOR_SENSE_DONE) == AM32_FLAG_UNSET) {
+        step = 1;
+        changeCompInput();
+        delayMicros(5);
+        RELOAD_WATCHDOG_COUNTER();
+        rotorSenseValue = getCompOutputLevel();
+        step = 2;
+        changeCompInput();
+        delayMicros(5);
+        RELOAD_WATCHDOG_COUNTER();
+        rotorSenseValue = getCompOutputLevel() << 1 | rotorSenseValue;
+        step = 3;
+        changeCompInput();
+        delayMicros(5);
+        RELOAD_WATCHDOG_COUNTER();
+        rotorSenseValue = getCompOutputLevel() << 2 | rotorSenseValue;
+
+        if (rotorSenseValue != rotorSenseValueLast && rotorSenseValue > 0 && rotorSenseValue < 7) {
+            rotorSenseDetectedSequence[rotorSenseDetectedSequenceIndex++] = rotorSenseValue;
+
+            if (rotorSenseDetectedSequenceIndex > 5) {
+                if (searchSequence(rotorSenseDetectedSequence, 6, rotorSenseSequence, 6)) {
+                    if (eepromBuffer.dir_reversed == 1) {
+                        playNonReversedTune();
+                        rotorSenseFalseDetected++;
+                    } else {
+                        rotorSenseFalseDetected = 0;
+                    }
+                } else {
+                    if (searchSequence(rotorSenseDetectedSequence, 6, rotorSenseReversedSequence, 6)) {
+                        if (eepromBuffer.dir_reversed == 0) {
+                            playReversedTune();
+                            rotorSenseFalseDetected++;
+                        } else {
+                            rotorSenseFalseDetected = 0;
+                        }
+                    }
+                }
+
+                if (rotorSenseFalseDetected == 2) {
+                    eepromBuffer.dir_reversed = (eepromBuffer.dir_reversed + 1) % 2;
+                    eepromBuffer.flags |= ROTOR_SENSE_DONE;
+                    saveEEpromSettings();
+                    playRotorSenseSaveTune();
+                    rotorSenseFalseDetected = 0;
+                }
+
+
+                rotorSenseDetectedSequenceIndex = 0;
+
+                ZERO_A(rotorSenseDetectedSequence);
+
+            }
+
+            rotorSenseValueLast = rotorSenseValue;
+        }
+    }
+#endif
 
     if (eepromBuffer.telementry_on_interval) {
         telem_ms_count++;
@@ -1620,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 ||
@@ -1721,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;

Src/sounds.c

@@ -120,6 +120,77 @@ void playStartupTune()
     __enable_irq();
 }
 
+#ifndef DISABLE_ROTOR_SENSE
+void playReversedTune()
+{
+    __disable_irq();
+
+    comStep(3);
+    RELOAD_WATCHDOG_COUNTER();
+
+    for(uint8_t i = 0; i < 8; ++i) {
+        RELOAD_WATCHDOG_COUNTER();
+        playBJNote(500, 50);
+        SET_DUTY_CYCLE_ALL(0);
+        delayMillis(20);
+    }
+
+    allOff(); // turn all channels low again
+    SET_PRESCALER_PWM(0); // set prescaler back to 0.
+    SET_AUTO_RELOAD_PWM(TIMER1_MAX_ARR);
+    signaltimeout = 0;
+    RELOAD_WATCHDOG_COUNTER();
+
+    __enable_irq();
+}
+
+void playRotorSenseSaveTune()
+{
+    __disable_irq();
+
+    comStep(3);
+    RELOAD_WATCHDOG_COUNTER();
+
+    for(uint8_t i = 0; i < 3; ++i) {
+        RELOAD_WATCHDOG_COUNTER();
+        playBJNote(300, 100);
+        SET_DUTY_CYCLE_ALL(0);
+        delayMillis(20);
+    }
+
+    allOff(); // turn all channels low again
+    SET_PRESCALER_PWM(0); // set prescaler back to 0.
+    SET_AUTO_RELOAD_PWM(TIMER1_MAX_ARR);
+    signaltimeout = 0;
+    RELOAD_WATCHDOG_COUNTER();
+
+    __enable_irq();
+}
+
+void playNonReversedTune()
+{
+    __disable_irq();
+
+    comStep(3);
+    RELOAD_WATCHDOG_COUNTER();
+
+    for(uint8_t i = 0; i < 8; ++i) {
+        RELOAD_WATCHDOG_COUNTER();
+        playBJNote(400, 50);
+        SET_DUTY_CYCLE_ALL(0);
+        delayMillis(20);
+    }
+
+    allOff(); // turn all channels low again
+    SET_PRESCALER_PWM(0); // set prescaler back to 0.
+    SET_AUTO_RELOAD_PWM(TIMER1_MAX_ARR);
+    signaltimeout = 0;
+    RELOAD_WATCHDOG_COUNTER();
+
+    __enable_irq();
+}
+#endif
+
 void playBrushedStartupTune()
 {
     __disable_irq();