alpha of IRC rotorSENSE

am32-firmware · Mar 17, 2024 · 57bbf20 · 57bbf20
1 parent e9fc66b
commit 57bbf20
Show file tree

Hide file tree

Showing 5 changed files with 201 additions and 0 deletions.
diff --git a/Inc/functions.h b/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);
 int 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_ */
diff --git a/Inc/sounds.h b/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 saveEEpromSettings(void);
 void setVolume(uint8_t volume);
 

diff --git a/Src/functions.c b/Src/functions.c
@@ -37,6 +37,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;
+}
+
 int getAbsDif(int number1, int number2)
 {
     int result = number1 - number2;

diff --git a/Src/main.c b/Src/main.c
@@ -1346,6 +1346,14 @@ void setInput()
 #endif
 }
 
+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;
+
 void tenKhzRoutine()
 { // 20khz as of 2.00 to be renamed
     duty_cycle = duty_cycle_setpoint;
@@ -1393,6 +1401,61 @@ void tenKhzRoutine()
                 }
             }
         }
+    } else if (!running) {
+        step = 1;
+        changeCompInput();
+        delayMicros(1);
+        RELOAD_WATCHDOG_COUNTER();
+        rotorSenseValue = getCompOutputLevel();
+        step = 2;
+        changeCompInput();
+        delayMicros(1);
+        RELOAD_WATCHDOG_COUNTER();
+        rotorSenseValue = getCompOutputLevel() << 1 | rotorSenseValue;
+        step = 3;
+        changeCompInput();
+        delayMicros(1);
+        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)) {
+                    playNonReversedTune();
+                    if (dir_reversed == 1) {
+                        rotorSenseFalseDetected++;
+                    } else {
+                        rotorSenseFalseDetected = 0;
+                    }
+                } else {
+                    if (searchSequence(rotorSenseDetectedSequence, 6, rotorSenseReversedSequence, 6)) {
+                        playReversedTune();
+                        if (dir_reversed == 0) {
+                            rotorSenseFalseDetected++;
+                        } else {
+                            rotorSenseFalseDetected = 0;
+                        }
+                    }
+                }
+
+                if (rotorSenseFalseDetected == 2) {
+                    dir_reversed = (dir_reversed + 1) % 2;
+                    saveEEpromSettings();
+                    playRotorSenseSaveTune();
+                    rotorSenseFalseDetected = 0;
+                }
+
+
+                rotorSenseDetectedSequenceIndex = 0;
+
+                ZERO_A(rotorSenseDetectedSequence);
+
+            }
+
+            rotorSenseValueLast = rotorSenseValue;
+        }
     }
 
     if (TLM_ON_INTERVAL) {

diff --git a/Src/sounds.c b/Src/sounds.c
@@ -123,6 +123,75 @@ void playStartupTune()
     __enable_irq();
 }
 
+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();
+}
+
 void playBrushedStartupTune()
 {
     __disable_irq();