From 5774244fdcb4dea69efacc6d056cc5d04b6768bf Mon Sep 17 00:00:00 2001 From: Eike Ahmels Date: Sun, 17 Mar 2024 16:18:41 +0100 Subject: [PATCH 1/2] alpha of IRC rotorSENSE --- Inc/eeprom.h | 12 ++++++- Inc/functions.h | 35 ++++++++++++++++++- Inc/sounds.h | 4 +++ Mcu/f421/Src/peripherals.c | 12 +++++++ Src/functions.c | 31 +++++++++++++++++ Src/main.c | 63 ++++++++++++++++++++++++++++++++++ Src/sounds.c | 69 ++++++++++++++++++++++++++++++++++++++ 7 files changed, 224 insertions(+), 2 deletions(-) diff --git a/Inc/eeprom.h b/Inc/eeprom.h index e412484b..3d9794ea 100644 --- a/Inc/eeprom.h +++ b/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); diff --git a/Inc/functions.h b/Inc/functions.h index 6334015a..5fffa6a3 100644 --- a/Inc/functions.h +++ b/Inc/functions.h @@ -9,8 +9,10 @@ #define FUNCTIONS_H_ #include "main.h" -#include "targets.h" +#include +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 +23,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 index c58eeee9..a230ab2e 100644 --- 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 setVolume(uint8_t volume); extern void delayMillis(uint32_t millis); diff --git a/Mcu/f421/Src/peripherals.c b/Mcu/f421/Src/peripherals.c index 7bd12e35..878eb03d 100644 --- a/Mcu/f421/Src/peripherals.c +++ b/Mcu/f421/Src/peripherals.c @@ -73,8 +73,20 @@ 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/functions.c b/Src/functions.c index 0b950761..750e24a7 100644 --- a/Src/functions.c +++ b/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 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 (eepromBuffer.telementry_on_interval) { diff --git a/Src/sounds.c b/Src/sounds.c index 14fde893..e5ee95dc 100644 --- a/Src/sounds.c +++ b/Src/sounds.c @@ -120,6 +120,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(); From 796f2e1db5bef44d236cf6a13a4f4a80db165195 Mon Sep 17 00:00:00 2001 From: Eike Ahmels Date: Sun, 24 Nov 2024 23:24:11 +0100 Subject: [PATCH 2/2] rotor sense, flag set/unset and dshot beacon override --- Inc/functions.h | 1 + Inc/version.h | 2 +- Mcu/f421/Src/peripherals.c | 12 --------- Src/main.c | 54 +++++++++++++++++++++++++++++++------- Src/sounds.c | 2 ++ 5 files changed, 49 insertions(+), 22 deletions(-) diff --git a/Inc/functions.h b/Inc/functions.h index 5fffa6a3..606720fe 100644 --- a/Inc/functions.h +++ b/Inc/functions.h @@ -9,6 +9,7 @@ #define FUNCTIONS_H_ #include "main.h" +#include "targets.h" #include int findIndex(uint8_t *array, uint8_t size, uint8_t target); diff --git a/Inc/version.h b/Inc/version.h index 67208d1d..7e3cba91 100644 --- 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 index 878eb03d..7bd12e35 100644 --- 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 index 44ec770e..f5f525b8 100644 --- 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,6 +1231,7 @@ if (!stepper_sine && armed) { #endif } +#ifndef DISABLE_ROTOR_SENSE uint8_t rotorSenseValue = 0; uint8_t rotorSenseValueLast = 0; uint8_t rotorSenseFalseDetected = 0; @@ -1215,6 +1239,7 @@ 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 index e5ee95dc..4caecb99 100644 --- 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() {