diff --git a/platformio.ini b/platformio.ini index d1321bcb8..4915abeb2 100644 --- a/platformio.ini +++ b/platformio.ini @@ -165,9 +165,9 @@ extends = stm32g473 [at32] extends = common -debug_tool = stlink +debug_tool = atlink upload_protocol = dfu -platform = https://github.com/ArteryTek/platform-arterytekat32.git#5729d36 +platform = https://github.com/ArteryTek/platform-arterytekat32.git#c8da386 platform_packages = toolchain-gccarmnoneeabi@~1.120301.0 framework = at32firmlib board_build.at32firmlib.custom_system_setup = yes diff --git a/script/device_gen.py b/script/device_gen.py index 16edee41d..069286654 100644 --- a/script/device_gen.py +++ b/script/device_gen.py @@ -1,7 +1,6 @@ import glob import hashlib -import yaml -import os +import re import modm_devices @@ -106,6 +105,66 @@ def __getitem__(self, item) -> modm_devices.device.Device: return value +def map_signal(s): + if s is None: + return None + + if s["driver"] == "tim" and s["name"].startswith("ch"): + return { + "func": "timer", + "af": int(s.get("af", "-1")), + "instance": int(s["instance"]), + "name": s["name"], + } + elif s["driver"] == "spi" and ( + s["name"] == "sck" + or s["name"] == "mosi" + or s["name"] == "miso" + or s["name"] == "tx" + or s["name"] == "rx" + ): + return { + "func": "spi", + "af": int(s.get("af", "-1")), + "instance": int(s["instance"]), + "name": {"tx": "mosi", "rx": "miso"}.get(s["name"], s["name"]), + } + elif (s["driver"] == "uart" or s["driver"] == "usart") and ( + s["name"] == "rx" or s["name"] == "tx" + ): + return { + "func": "serial", + "af": int(s.get("af", "-1")), + "instance": int(s["instance"]), + "name": s["name"], + } + elif s["driver"] == "adc" and re.match(r"in\d+", s.get("name", "in0")): + return { + "func": "adc", + "af": -1, + "instance": int(s["instance"]), + "name": s.get("name", "in0xff")[2:], + } + else: + return None + + +def map_tag(f): + if f is None: + return None + + if f["func"] == "timer": + return f"TIMER_TAG(TIMER{f['instance']}, TIMER_{f['name']})".upper() + elif f["func"] == "spi": + return f"SPI_TAG(SPI_PORT{f['instance']}, RES_SPI_{f['name']})".upper() + elif f["func"] == "serial": + return f"SERIAL_TAG(SERIAL_PORT{f['instance']}, RES_SERIAL_{f['name']})".upper() + elif f["func"] == "adc": + return f"ADC_TAG(ADC_DEVICE{f['instance']}, {f['name']})".upper() + else: + return None + + devices = [ "stm32f405rg", "stm32f411re", @@ -123,100 +182,58 @@ def __getitem__(self, item) -> modm_devices.device.Device: pins = {} key = next((x for x in caches.keys() if x.startswith(device)), None) - for driver in caches[key].get_all_drivers("gpio"): - for pin in driver["gpio"]: - funcs = [] + with open(f"src/system/{device[:9]}/gpio_pins.in", "w") as file: + for driver in caches[key].get_all_drivers("gpio"): + for pin in driver["gpio"]: + file.write(f"GPIO_PIN({pin['port']}, {pin['pin']})\n".upper()) + if "signal" not in pin: + continue - if "signal" in pin: for s in pin["signal"]: - if s["driver"] == "tim" and s["name"].startswith("ch"): - funcs.append( - { - "func": "timer", - "af": int(s["af"]), - "instance": int(s["instance"]), - "name": s["name"], - } - ) - - if s["driver"] == "spi" and ( - s["name"] == "sck" or s["name"] == "mosi" or s["name"] == "miso" - ): - funcs.append( - { - "func": "spi", - "af": int(s["af"]), - "instance": int(s["instance"]), - "name": s["name"], - } - ) - - if (s["driver"] == "uart" or s["driver"] == "usart") and ( - s["name"] == "rx" or s["name"] == "tx" - ): - funcs.append( - { - "func": "serial", - "af": int(s["af"]), - "instance": int(s["instance"]), - "name": s["name"], - } - ) - - if s["driver"] == "adc" and not ( - s["name"].startswith("inp") or s["name"].startswith("inn") - ): - funcs.append( - { - "func": "adc", - "af": -1, - "instance": int(s["instance"]), - "name": s["name"][2:], - } - ) - - pins[f"P{pin['port']}{pin['pin']}".upper()] = funcs - - with open(f"src/system/{device[:9]}/gpio_pins.yaml", "w") as file: - documents = yaml.dump(pins, file, sort_keys=False) - -for filename in glob.glob("src/system/*/gpio_pins.yaml"): - dir = os.path.dirname(filename) - - with open(filename, "r") as f: - pins = yaml.load(f, Loader=yaml.Loader) - - with open(f"{dir}/gpio_pins.in", "w") as file: - for k, funcs in pins.items(): - port = k[1] - pin = k[2:] - file.write(f"GPIO_PIN({port}, {pin})\n") - - for f in funcs: - line = f"GPIO_AF(PIN_{port}{pin}, {f['af']}, " - - if f["func"] == "timer": - line = ( - line - + f"TIMER_TAG(TIMER{f['instance']}, TIMER_{f['name'].upper()}))\n" - ) - - if f["func"] == "spi": - line = ( - line - + f"SPI_TAG(SPI_PORT{f['instance']}, RES_SPI_{f['name'].upper()}))\n" - ) - - if f["func"] == "serial": - line = ( - line - + f"SERIAL_TAG(SERIAL_PORT{f['instance']}, RES_SERIAL_{f['name'].upper()}))\n" - ) - - if f["func"] == "adc": - line = ( - line - + f"ADC_TAG(ADC_DEVICE{f['instance']}, {f['name'].upper()}))\n" + f = map_signal(s) + s = map_tag(f) + if s is None: + continue + file.write( + f"GPIO_AF(PIN_{pin['port']}{pin['pin']}, {f['af']}, {s})\n".upper() ) - file.write(line) + with open(f"src/system/{device[:9]}/dma.in", "w") as file: + for driver in caches[key].get_all_drivers("dma"): + if "streams" in driver: + for dma in driver["streams"]: + for stream in dma["stream"]: + for channel in stream["channel"]: + funcs = [ + r + for s in channel["signal"] + if (r := map_tag(map_signal(s))) is not None + ] + for func in funcs: + entry = ", ".join( + [ + f".tag = {func}", + f".port_index = {dma['instance']}", + f".stream_index = {stream['position']}", + f".channel = LL_DMA_CHANNEL_{channel['position']}", + ] + ) + file.write("{ " + entry + " },\n") + if "requests" in driver: + for dma in driver["requests"]: + for request in dma["request"]: + funcs = [ + r + for s in request["signal"] + if (r := map_tag(map_signal(s))) is not None + ] + for func in funcs: + entry = ", ".join( + [ + f".tag = {func}", + f".port_index = -1", + f".stream_index = -1", + f".request = {request['position']}", + ] + ) + file.write("{ " + entry + " },\n") diff --git a/src/config/config.h b/src/config/config.h index 16c940a3a..e4b79260f 100644 --- a/src/config/config.h +++ b/src/config/config.h @@ -160,8 +160,7 @@ // at eliminating propwash. Motor noise related to rpm is known to have a quadratic relationship with increasing throttle. While a quadratic curve // could have been selected for this feature, a faster moving parabolic one was selected in its place as the goal is not to follow motor noise, but // to get the filter out of the way as fast as possible in the interest of better performance and handling through reduced D filter latency when you need it most. -#define DTERM_DYNAMIC_LPF -#define DTERM_DYNAMIC_EXPO 1.0f +#define DTERM_DYNAMIC_TYPE FILTER_LP_PT1 #define DTERM_DYNAMIC_FREQ_MIN 70 #define DTERM_DYNAMIC_FREQ_MAX 260 diff --git a/src/config/feature.h b/src/config/feature.h index 1194684bc..577dd0475 100644 --- a/src/config/feature.h +++ b/src/config/feature.h @@ -12,6 +12,7 @@ #define USE_VTX #define USE_DIGITAL_VTX #define USE_MAX7456 +#define USE_RGB_LED #define USE_MOTOR_DSHOT #define USE_MOTOR_PWM diff --git a/src/core/failloop.h b/src/core/failloop.h index 19959c7dd..ff3e54f50 100644 --- a/src/core/failloop.h +++ b/src/core/failloop.h @@ -10,8 +10,8 @@ typedef enum { FAILLOOP_GYRO = 4, // gyro not found FAILLOOP_FAULT = 5, // clock, intterrupts, systick, gcc bad code, bad memory access (code issues like bad pointers) - this should not come up FAILLOOP_LOOPTIME = 6, // loop time issue - if loop time exceeds 20mS - FAILLOOP_DMA = 7, // dma error - triggered by dma pool - FAILLOOP_SPI = 8, // spi error - triggered by hardware spi driver only + FAILLOOP_DMA = 7, // dma error + FAILLOOP_SPI = 8, // spi error } __attribute__((__packed__)) failloop_t; void failloop(failloop_t val); \ No newline at end of file diff --git a/src/core/main.c b/src/core/main.c index de9f65b26..ac838825b 100644 --- a/src/core/main.c +++ b/src/core/main.c @@ -100,7 +100,7 @@ __attribute__((__used__)) int main() { motor_set_all(MOTOR_OFF); // wait for devices to wake up - time_delay_ms(300); + time_delay_ms(100); osd_init(); rx_spektrum_bind(); @@ -113,20 +113,17 @@ __attribute__((__used__)) int main() { sixaxis_init(); // give the gyro some time to settle - time_delay_ms(100); + time_delay_ms(50); // display bootlogo while calibrating sixaxis_gyro_cal(); - // wait for adc and vtx to wake up - time_delay_ms(100); - adc_init(); vbat_init(); rx_init(); vtx_init(); - rgb_init(); + rgb_led_init(); blackbox_init(); imu_init(); diff --git a/src/core/profile.c b/src/core/profile.c index 4724015b7..4b0d93ac7 100644 --- a/src/core/profile.c +++ b/src/core/profile.c @@ -203,17 +203,10 @@ const profile_t default_profile = { }, }, -#ifdef DTERM_DYNAMIC_LPF - .dterm_dynamic_enable = 1, -#else - .dterm_dynamic_enable = 0, -#endif -#ifdef DTERM_DYNAMIC_FREQ_MIN + .dterm_dynamic_type = DTERM_DYNAMIC_TYPE, .dterm_dynamic_min = DTERM_DYNAMIC_FREQ_MIN, -#endif -#ifdef DTERM_DYNAMIC_FREQ_MAX .dterm_dynamic_max = DTERM_DYNAMIC_FREQ_MAX, -#endif + #ifdef GYRO_DYNAMIC_NOTCH .gyro_dynamic_notch_enable = 1, #else diff --git a/src/core/profile.h b/src/core/profile.h index ea139788e..7ef5bc3f4 100644 --- a/src/core/profile.h +++ b/src/core/profile.h @@ -9,7 +9,7 @@ #define OSD_NUMBER_ELEMENTS 32 -#define PROFILE_VERSION MAKE_SEMVER(0, 2, 5) +#define PROFILE_VERSION MAKE_SEMVER(0, 2, 6) // Rates typedef enum { @@ -327,7 +327,7 @@ typedef struct { typedef struct { profile_filter_parameter_t gyro[FILTER_MAX_SLOTS]; profile_filter_parameter_t dterm[FILTER_MAX_SLOTS]; - uint8_t dterm_dynamic_enable; + filter_type_t dterm_dynamic_type; float dterm_dynamic_min; float dterm_dynamic_max; uint8_t gyro_dynamic_notch_enable; @@ -337,7 +337,7 @@ typedef struct { START_STRUCT(profile_filter_t) \ ARRAY_MEMBER(gyro, FILTER_MAX_SLOTS, profile_filter_parameter_t) \ ARRAY_MEMBER(dterm, FILTER_MAX_SLOTS, profile_filter_parameter_t) \ - MEMBER(dterm_dynamic_enable, uint8_t) \ + MEMBER(dterm_dynamic_type, uint8_t) \ MEMBER(dterm_dynamic_min, float) \ MEMBER(dterm_dynamic_max, float) \ MEMBER(gyro_dynamic_notch_enable, uint8_t) \ diff --git a/src/core/target.c b/src/core/target.c index f2025d352..2d714f4ad 100644 --- a/src/core/target.c +++ b/src/core/target.c @@ -41,6 +41,7 @@ target_info_t target_info = { #define INDEX_ARRAY_MEMBER CBOR_ENCODE_INDEX_ARRAY_MEMBER #define STR_ARRAY_MEMBER CBOR_ENCODE_STR_ARRAY_MEMBER +TARGET_DMA_MEMBERS TARGET_LED_MEMBERS TARGET_BUZZER_MEMBERS TARGET_SERIAL_MEMBERS @@ -69,6 +70,7 @@ TARGET_INFO_MEMBERS #define INDEX_ARRAY_MEMBER CBOR_DECODE_INDEX_ARRAY_MEMBER #define STR_ARRAY_MEMBER CBOR_DECODE_STR_ARRAY_MEMBER +TARGET_DMA_MEMBERS TARGET_LED_MEMBERS TARGET_BUZZER_MEMBERS TARGET_SERIAL_MEMBERS diff --git a/src/core/target.h b/src/core/target.h index a191d3c26..397128472 100644 --- a/src/core/target.h +++ b/src/core/target.h @@ -3,6 +3,7 @@ #include #include +#include "driver/dma.h" #include "rx/rx.h" #define LED_MAX 4 @@ -64,6 +65,32 @@ typedef enum { SERIAL_SOFT_COUNT = SERIAL_SOFT_MAX - SERIAL_SOFT_START, } __attribute__((__packed__)) serial_ports_t; +typedef struct { +#if defined(STM32H7) || defined(STM32G4) || defined(AT32) + uint32_t request; +#else + uint32_t channel; +#endif + resource_tag_t tag; + dma_stream_t dma; +} target_dma_t; + +#if defined(STM32H7) || defined(STM32G4) || defined(AT32) +#define TARGET_DMA_MEMBERS \ + START_STRUCT(target_dma_t) \ + MEMBER(request, uint32_t) \ + MEMBER(tag, resource_tag_t) \ + MEMBER(dma, dma_stream_t) \ + END_STRUCT() +#else +#define TARGET_DMA_MEMBERS \ + START_STRUCT(target_dma_t) \ + MEMBER(channel, uint32_t) \ + MEMBER(tag, resource_tag_t) \ + MEMBER(dma, dma_stream_t) \ + END_STRUCT() +#endif + typedef struct { gpio_pins_t pin; bool invert; @@ -186,6 +213,7 @@ typedef struct { gpio_pins_t fpv; gpio_pins_t vbat; gpio_pins_t ibat; + gpio_pins_t rgb_led; target_invert_pin_t sdcard_detect; target_invert_pin_t buzzer; @@ -193,6 +221,8 @@ typedef struct { uint16_t vbat_scale; uint16_t ibat_scale; + + target_dma_t dma[DMA_DEVICE_MAX]; } target_t; #define TARGET_MEMBERS \ @@ -213,11 +243,13 @@ typedef struct { MEMBER(fpv, gpio_pins_t) \ MEMBER(vbat, gpio_pins_t) \ MEMBER(ibat, gpio_pins_t) \ + MEMBER(rgb_led, gpio_pins_t) \ MEMBER(sdcard_detect, target_invert_pin_t) \ MEMBER(buzzer, target_invert_pin_t) \ ARRAY_MEMBER(motor_pins, MOTOR_PIN_MAX, gpio_pins_t) \ MEMBER(vbat_scale, uint16_t) \ MEMBER(ibat_scale, uint16_t) \ + MEMBER(dma, target_dma_array) \ END_STRUCT() typedef enum { @@ -268,4 +300,10 @@ cbor_result_t cbor_decode_gpio_pins_t(cbor_value_t *dec, gpio_pins_t *t); cbor_result_t cbor_encode_target_t(cbor_value_t *enc, const target_t *t); cbor_result_t cbor_decode_target_t(cbor_value_t *dec, target_t *t); -cbor_result_t cbor_encode_target_info_t(cbor_value_t *enc, const target_info_t *i); \ No newline at end of file +cbor_result_t cbor_encode_target_dma_t(cbor_value_t *enc, const target_dma_t *dma); +cbor_result_t cbor_decode_target_dma_t(cbor_value_t *dec, target_dma_t *dma); + +cbor_result_t cbor_encode_target_info_t(cbor_value_t *enc, const target_info_t *i); + +cbor_result_t cbor_encode_target_dma_array(cbor_value_t *dec, const target_dma_t (*dma)[DMA_DEVICE_MAX]); +cbor_result_t cbor_decode_target_dma_array(cbor_value_t *dec, target_dma_t (*dma)[DMA_DEVICE_MAX]); diff --git a/src/core/tasks.c b/src/core/tasks.c index 8e12b8c98..784db0c03 100644 --- a/src/core/tasks.c +++ b/src/core/tasks.c @@ -11,6 +11,7 @@ #include "io/blackbox.h" #include "io/buzzer.h" #include "io/led.h" +#include "io/rgb_led.h" #include "io/usb_configurator.h" #include "io/vbat.h" #include "io/vtx.h" @@ -22,14 +23,7 @@ void util_task() { // handle led commands led_update(); - -#if (RGB_LED_NUMBER > 0) - // RGB led control - rgb_led_lvc(); -#ifdef RGB_LED_DMA - rgb_dma_start(); -#endif -#endif + rgb_led_update(); buzzer_update(); } diff --git a/src/driver/dma.c b/src/driver/dma.c new file mode 100644 index 000000000..8191b552e --- /dev/null +++ b/src/driver/dma.c @@ -0,0 +1,109 @@ +#include "dma.h" + +#include "core/project.h" +#include "util/cbor_helper.h" +#include "util/util.h" + +dma_device_t dma_stream_map[DMA_STREAM_MAX]; + +cbor_result_t cbor_decode_dma_device_t(cbor_value_t *dec, dma_device_t *d) { + const uint8_t *name; + uint32_t name_len; + cbor_result_t res = cbor_decode_tstr(dec, &name, &name_len); + +#define DMA_DEVICE(member) \ + if (buf_equal_string(name, name_len, #member)) \ + *d = DMA_DEVICE_##member; \ + else + DMA_DEVICES +#undef DMA_DEVICE + *d = DMA_DEVICE_INVALID; + + return res; +} + +cbor_result_t cbor_encode_dma_device_t(cbor_value_t *enc, const dma_device_t *d) { + cbor_result_t res = CBOR_OK; + + switch (*d) { +#define DMA_DEVICE(_name) \ + case DMA_DEVICE_##_name: \ + CBOR_CHECK_ERROR(res = cbor_encode_str(enc, #_name)); \ + break; + DMA_DEVICES +#undef DMA_DEVICE + default: + break; + } + + return res; +} + +cbor_result_t cbor_decode_dma_stream_t(cbor_value_t *dec, dma_stream_t *d) { + const uint8_t *name; + uint32_t name_len; + cbor_result_t res = cbor_decode_tstr(dec, &name, &name_len); + +#define DMA_STREAM(_port, _stream) \ + if (buf_equal_string(name, name_len, "DMA" #_port "_STREAM" #_stream)) \ + *d = DMA##_port##_STREAM##_stream; \ + else + DMA_STREAMS +#undef DMA_STREAM + *d = DMA_STREAM_INVALID; + + return res; +} + +cbor_result_t cbor_encode_dma_stream_t(cbor_value_t *enc, const dma_stream_t *d) { + cbor_result_t res = CBOR_OK; + + switch (*d) { +#define DMA_STREAM(_port, _stream) \ + case DMA##_port##_STREAM##_stream: \ + CBOR_CHECK_ERROR(res = cbor_encode_str(enc, "DMA" #_port "_STREAM" #_stream)); \ + break; + DMA_STREAMS +#undef DMA_STREAM + default: + break; + } + + return res; +} + +cbor_result_t cbor_decode_target_dma_array(cbor_value_t *dec, target_dma_t (*dma)[DMA_DEVICE_MAX]) { + cbor_result_t res = CBOR_OK; + + cbor_container_t map; + CBOR_CHECK_ERROR(res = cbor_decode_map(dec, &map)); + for (uint32_t i = 0; i < cbor_decode_map_size(dec, &map); i++) { + dma_device_t dev = DMA_DEVICE_INVALID; + CBOR_CHECK_ERROR(res = cbor_decode_dma_device_t(dec, &dev)); + if (dev == DMA_DEVICE_INVALID) { + continue; + } + + target_dma_t *d = &(*dma)[dev]; + CBOR_CHECK_ERROR(res = cbor_decode_target_dma_t(dec, d)); + dma_stream_map[d->dma] = dev; + } + + return res; +} + +cbor_result_t cbor_encode_target_dma_array(cbor_value_t *enc, const target_dma_t (*dma)[DMA_DEVICE_MAX]) { + cbor_result_t res = CBOR_OK; + CBOR_CHECK_ERROR(res = cbor_encode_map_indefinite(enc)); + + for (uint32_t i = 0; i < DMA_DEVICE_MAX; i++) { + const target_dma_t *d = &(*dma)[i]; + if (d->dma == DMA_STREAM_INVALID) + continue; + + CBOR_CHECK_ERROR(res = cbor_encode_dma_device_t(enc, (const dma_device_t *)&i)); + CBOR_CHECK_ERROR(res = cbor_encode_target_dma_t(enc, d)); + } + + return cbor_encode_end_indefinite(enc); +} \ No newline at end of file diff --git a/src/driver/dma.h b/src/driver/dma.h index bb3b23c1c..ec43c7dff 100644 --- a/src/driver/dma.h +++ b/src/driver/dma.h @@ -1,31 +1,52 @@ #pragma once +#include #include -#include "core/project.h" +#include "driver/mcu/system.h" +#include "driver/resource.h" #define DMA_ALIGN_SIZE 32 #define DMA_ALIGN(offset) MEMORY_ALIGN(offset, DMA_ALIGN_SIZE) +#define DMA_DEVICES \ + DMA_DEVICE(DSHOT_CH1) \ + DMA_DEVICE(DSHOT_CH2) \ + DMA_DEVICE(DSHOT_CH3) \ + DMA_DEVICE(SPI1_RX) \ + DMA_DEVICE(SPI1_TX) \ + DMA_DEVICE(SPI2_RX) \ + DMA_DEVICE(SPI2_TX) \ + DMA_DEVICE(SPI3_RX) \ + DMA_DEVICE(SPI3_TX) \ + DMA_DEVICE(SPI4_RX) \ + DMA_DEVICE(SPI4_TX) \ + DMA_DEVICE(RGB) + typedef enum { - DMA_DEVICE_SPI1_RX, - DMA_DEVICE_SPI1_TX, - DMA_DEVICE_SPI2_RX, - DMA_DEVICE_SPI2_TX, - DMA_DEVICE_SPI3_RX, - DMA_DEVICE_SPI3_TX, - DMA_DEVICE_SPI4_RX, - DMA_DEVICE_SPI4_TX, - DMA_DEVICE_TIM1_CH1, - DMA_DEVICE_TIM1_CH3, - DMA_DEVICE_TIM1_CH4, - - DMA_DEVICE_MAX, + DMA_DEVICE_INVALID, +#define DMA_DEVICE(_name) DMA_DEVICE_##_name, + DMA_DEVICES DMA_DEVICE_MAX +#undef DMA_DEVICE } dma_device_t; -extern const dma_stream_def_t dma_stream_defs[DMA_DEVICE_MAX]; +typedef enum { + DMA_STREAM_INVALID, +#define DMA_STREAM(_port, _stream) DMA##_port##_STREAM##_stream, + DMA_STREAMS DMA_STREAM_MAX +#undef DMA_STREAM +} dma_stream_t; + +extern const dma_stream_def_t dma_stream_defs[DMA_STREAM_MAX]; +extern dma_device_t dma_stream_map[DMA_STREAM_MAX]; + +cbor_result_t cbor_decode_dma_device_t(cbor_value_t *dec, dma_device_t *d); +cbor_result_t cbor_encode_dma_device_t(cbor_value_t *enc, const dma_device_t *d); + +cbor_result_t cbor_decode_dma_stream_t(cbor_value_t *dec, dma_stream_t *d); +cbor_result_t cbor_encode_dma_stream_t(cbor_value_t *enc, const dma_stream_t *d); void dma_prepare_tx_memory(void *addr, uint32_t size); void dma_prepare_rx_memory(void *addr, uint32_t size); -void dma_enable_rcc(dma_device_t dev); +void dma_enable_rcc(const dma_stream_def_t *def); diff --git a/src/driver/mcu/at32/dma.c b/src/driver/mcu/at32/dma.c index 5c8f71808..7ad0c68df 100644 --- a/src/driver/mcu/at32/dma.c +++ b/src/driver/mcu/at32/dma.c @@ -1,50 +1,24 @@ #include "driver/dma.h" +#include "driver/adc.h" #include "driver/rcc.h" +#include "driver/resource.h" +#include "driver/timer.h" -#define DMAMUX_DMAREQ_ID_TIM1_CH1 DMAMUX_DMAREQ_ID_TMR1_CH1 -#define DMAMUX_DMAREQ_ID_TIM1_CH2 DMAMUX_DMAREQ_ID_TMR1_CH2 -#define DMAMUX_DMAREQ_ID_TIM1_CH3 DMAMUX_DMAREQ_ID_TMR1_CH3 -#define DMAMUX_DMAREQ_ID_TIM1_CH4 DMAMUX_DMAREQ_ID_TMR1_CH3 - -#define DMA_STREAMS \ - DMA_STREAM(1, 1, SPI1_RX) \ - DMA_STREAM(1, 2, SPI1_TX) \ - DMA_STREAM(1, 3, SPI2_RX) \ - DMA_STREAM(1, 4, SPI2_TX) \ - DMA_STREAM(1, 5, SPI3_RX) \ - DMA_STREAM(1, 6, SPI3_TX) \ - DMA_STREAM(2, 1, SPI4_RX) \ - DMA_STREAM(2, 2, SPI4_TX) \ - DMA_STREAM(2, 3, TIM1_CH1) \ - DMA_STREAM(2, 4, TIM1_CH3) \ - DMA_STREAM(2, 5, TIM1_CH4) - -#define DMA_STREAM(_port, _chan, _dev) \ - [DMA_DEVICE_##_dev] = { \ - .device = DMA_DEVICE_##_dev, \ +#define DMA_STREAM(_port, _chan) \ + [DMA##_port##_STREAM##_chan] = { \ .port = DMA##_port, \ .port_index = _port, \ - .channel = DMA##_port##_CHANNEL##_chan, \ - .channel_index = _chan, \ - .request = DMAMUX_DMAREQ_ID_##_dev, \ + .stream = DMA##_port##_CHANNEL##_chan, \ + .stream_index = _chan, \ .mux = DMA##_port##MUX_CHANNEL##_chan, \ .irq = DMA##_port##_Channel##_chan##_IRQn, \ }, - -const dma_stream_def_t dma_stream_defs[DMA_DEVICE_MAX] = {DMA_STREAMS}; - +const dma_stream_def_t dma_stream_defs[DMA_STREAM_MAX] = {DMA_STREAMS}; #undef DMA_STREAM -void dma_prepare_tx_memory(void *addr, uint32_t size) { -} - -void dma_prepare_rx_memory(void *addr, uint32_t size) { -} - -void dma_enable_rcc(dma_device_t dev) { - const dma_stream_def_t *dma = &dma_stream_defs[dev]; - switch (dma->port_index) { +void dma_enable_rcc(const dma_stream_def_t *def) { + switch (def->port_index) { case 1: rcc_enable(RCC_ENCODE(DMA1)); dmamux_enable(DMA1, TRUE); @@ -56,37 +30,45 @@ void dma_enable_rcc(dma_device_t dev) { } } -extern void dshot_dma_isr(dma_device_t dev); -extern void spi_dma_isr(dma_device_t dev); +void dma_prepare_tx_memory(void *addr, uint32_t size) {} +void dma_prepare_rx_memory(void *addr, uint32_t size) {} + +extern void dshot_dma_isr(const dma_device_t); +extern void spi_dma_isr(const dma_device_t); +extern void rgb_dma_isr(const dma_device_t); -static void handle_dma_stream_isr(dma_device_t dev) { +static void handle_dma_stream_isr(const dma_device_t dev) { switch (dev) { case DMA_DEVICE_SPI1_RX: - case DMA_DEVICE_SPI2_RX: - case DMA_DEVICE_SPI3_RX: - case DMA_DEVICE_SPI4_RX: case DMA_DEVICE_SPI1_TX: + case DMA_DEVICE_SPI2_RX: case DMA_DEVICE_SPI2_TX: + case DMA_DEVICE_SPI3_RX: case DMA_DEVICE_SPI3_TX: + case DMA_DEVICE_SPI4_RX: case DMA_DEVICE_SPI4_TX: spi_dma_isr(dev); break; - case DMA_DEVICE_TIM1_CH1: - case DMA_DEVICE_TIM1_CH3: - case DMA_DEVICE_TIM1_CH4: + case DMA_DEVICE_DSHOT_CH1: + case DMA_DEVICE_DSHOT_CH2: + case DMA_DEVICE_DSHOT_CH3: #ifdef USE_MOTOR_DSHOT dshot_dma_isr(dev); #endif break; + case DMA_DEVICE_RGB: +#ifdef USE_RGB_LED + rgb_dma_isr(dev); +#endif + break; + case DMA_DEVICE_INVALID: case DMA_DEVICE_MAX: break; } } -#define DMA_STREAM(_port, _chan, _dev) \ - void DMA##_port##_Channel##_chan##_IRQHandler() { \ - handle_dma_stream_isr(DMA_DEVICE_##_dev); \ - } +#define DMA_STREAM(_port, _stream) \ + void DMA##_port##_Channel##_stream##_IRQHandler() { handle_dma_stream_isr(dma_stream_map[DMA##_port##_STREAM##_stream]); } DMA_STREAMS diff --git a/src/driver/mcu/at32/dma.h b/src/driver/mcu/at32/dma.h index 94e6b3c1c..191269a5a 100644 --- a/src/driver/mcu/at32/dma.h +++ b/src/driver/mcu/at32/dma.h @@ -5,8 +5,24 @@ #define DMA_HDT_FLAG ((uint32_t)0x00000004) #define DMA_DTERR_FLAG ((uint32_t)0x00000008) -// 4bits per channel -#define dma_flag_for_channel(dev, flags) ((dev->port_index == 2 ? 0x10000000 : 0x0) | ((flags) << ((dev->channel_index - 1) * 4))) +// 4bits per stream +#define dma_flag_for_stream(dev, flags) ((dev->port_index == 2 ? 0x10000000 : 0x0) | ((flags) << ((dev->stream_index - 1) * 4))) -#define dma_is_flag_active_tc(dev) (dma_flag_get(dma_flag_for_channel(dev, DMA_FDT_FLAG))) -#define dma_clear_flag_tc(dev) dma_flag_clear(dma_flag_for_channel(dev, DMA_FDT_FLAG | DMA_HDT_FLAG | DMA_DTERR_FLAG)) +#define dma_is_flag_active_tc(dev) (dma_flag_get(dma_flag_for_stream(dev, DMA_FDT_FLAG))) +#define dma_clear_flag_tc(dev) dma_flag_clear(dma_flag_for_stream(dev, DMA_FDT_FLAG | DMA_HDT_FLAG | DMA_DTERR_FLAG)) + +#define DMA_STREAMS \ + DMA_STREAM(1, 1) \ + DMA_STREAM(1, 2) \ + DMA_STREAM(1, 3) \ + DMA_STREAM(1, 4) \ + DMA_STREAM(1, 5) \ + DMA_STREAM(1, 6) \ + DMA_STREAM(1, 7) \ + DMA_STREAM(2, 1) \ + DMA_STREAM(2, 2) \ + DMA_STREAM(2, 3) \ + DMA_STREAM(2, 4) \ + DMA_STREAM(2, 5) \ + DMA_STREAM(2, 6) \ + DMA_STREAM(2, 7) diff --git a/src/driver/mcu/at32/motor_dshot.c b/src/driver/mcu/at32/motor_dshot.c index 9f6e159d8..362c510b8 100644 --- a/src/driver/mcu/at32/motor_dshot.c +++ b/src/driver/mcu/at32/motor_dshot.c @@ -2,6 +2,7 @@ #include +#include "core/failloop.h" #include "core/profile.h" #include "core/project.h" #include "driver/dma.h" @@ -11,8 +12,16 @@ #ifdef USE_MOTOR_DSHOT -static void dshot_init_gpio_port(dshot_gpio_port_t *port) { - dma_enable_rcc(port->dma_device); +void dshot_init_gpio_port(dshot_gpio_port_t *port) { + const timer_def_t *tim = &timer_defs[TIMER_TAG_TIM(port->timer_tag)]; + + rcc_enable(tim->rcc); + + // setup timer to 1/3 of the full bit time + tmr_base_init(tim->instance, DSHOT_SYMBOL_TIME, 0); + tmr_clock_source_div_set(tim->instance, TMR_CLOCK_DIV1); + tmr_cnt_dir_set(tim->instance, TMR_COUNT_UP); + tmr_period_buffer_enable(tim->instance, TRUE); tmr_output_config_type tim_oc_init; tmr_output_default_para_init(&tim_oc_init); @@ -20,13 +29,16 @@ static void dshot_init_gpio_port(dshot_gpio_port_t *port) { tim_oc_init.oc_idle_state = TRUE; tim_oc_init.oc_polarity = TMR_OUTPUT_ACTIVE_LOW; tim_oc_init.oc_output_state = TRUE; - tmr_output_channel_config(TMR1, timer_channel_val(port->timer_channel), &tim_oc_init); - tmr_output_channel_buffer_enable(TMR1, timer_channel_val(port->timer_channel), TRUE); - const dma_stream_def_t *dma = &dma_stream_defs[port->dma_device]; + const uint32_t ch = timer_channel_val(TIMER_TAG_CH(port->timer_tag)); + tmr_output_channel_config(tim->instance, ch, &tim_oc_init); + tmr_output_channel_buffer_enable(tim->instance, ch, TRUE); + + const dma_stream_def_t *dma = &dma_stream_defs[target.dma[port->dma_device].dma]; + dma_enable_rcc(dma); - dma_reset(dma->channel); - dmamux_init(dma->mux, dma->request); + dma_reset(dma->stream); + dmamux_init(dma->mux, target.dma[port->dma_device].request); dma_init_type init; init.peripheral_base_addr = 0x0; // overwritten later @@ -39,75 +51,58 @@ static void dshot_init_gpio_port(dshot_gpio_port_t *port) { init.memory_data_width = DMA_MEMORY_DATA_WIDTH_WORD; init.loop_mode_enable = FALSE; init.priority = DMA_PRIORITY_VERY_HIGH; - dma_init(dma->channel, &init); - dma_interrupt_enable(dma->channel, DMA_FDT_INT, TRUE); + dma_init(dma->stream, &init); + dma_interrupt_enable(dma->stream, DMA_FDT_INT, TRUE); interrupt_enable(dma->irq, DMA_PRIORITY); -} - -void dshot_init_timer() { - rcc_enable(RCC_ENCODE(TMR1)); - // setup timer to 1/3 of the full bit time - tmr_base_init(TMR1, DSHOT_SYMBOL_TIME, 0); - tmr_clock_source_div_set(TMR1, TMR_CLOCK_DIV1); - tmr_cnt_dir_set(TMR1, TMR_COUNT_UP); - tmr_period_buffer_enable(TMR1, TRUE); - - for (uint32_t j = 0; j < dshot_gpio_port_count; j++) { - dshot_init_gpio_port(&dshot_gpio_ports[j]); - - for (uint8_t i = 0; i < DSHOT_DMA_SYMBOLS; i++) { - dshot_output_buffer[j][i * 3 + 0] = dshot_gpio_ports[j].set_mask; // start bit - dshot_output_buffer[j][i * 3 + 1] = 0; // actual bit, set below - dshot_output_buffer[j][i * 3 + 2] = dshot_gpio_ports[j].reset_mask; // return line to low - } - } - - tmr_counter_enable(TMR1, TRUE); + tmr_counter_enable(tim->instance, TRUE); } void dshot_dma_setup_output(uint32_t index) { - tmr_period_value_set(TMR1, DSHOT_SYMBOL_TIME); - const dshot_gpio_port_t *port = &dshot_gpio_ports[index]; - const dma_stream_def_t *dma = &dma_stream_defs[port->dma_device]; - dma->channel->ctrl_bit.dtd = 1; // DMA_DIR_MEMORY_TO_PERIPHERAL - dma->channel->ctrl_bit.mwidth = DMA_MEMORY_DATA_WIDTH_WORD; - dma->channel->ctrl_bit.pwidth = DMA_PERIPHERAL_DATA_WIDTH_WORD; - dma->channel->paddr = (uint32_t)(&port->gpio->scr); - dma->channel->maddr = (uint32_t)(&dshot_output_buffer[index][0]); - dma->channel->dtcnt_bit.cnt = DSHOT_DMA_BUFFER_SIZE; + const timer_def_t *tim = &timer_defs[TIMER_TAG_TIM(port->timer_tag)]; + tmr_period_value_set(tim->instance, DSHOT_SYMBOL_TIME); + + const dma_stream_def_t *dma = &dma_stream_defs[target.dma[port->dma_device].dma]; + dma->stream->ctrl_bit.dtd = 1; // DMA_DIR_MEMORY_TO_PERIPHERAL + dma->stream->ctrl_bit.mwidth = DMA_MEMORY_DATA_WIDTH_WORD; + dma->stream->ctrl_bit.pwidth = DMA_PERIPHERAL_DATA_WIDTH_WORD; + dma->stream->paddr = (uint32_t)(&port->gpio->scr); + dma->stream->maddr = (uint32_t)(&dshot_output_buffer[index][0]); + dma->stream->dtcnt_bit.cnt = DSHOT_DMA_BUFFER_SIZE; - dma_channel_enable(dma->channel, TRUE); - timer_enable_dma_request(TIMER1, port->timer_channel, true); + dma_channel_enable(dma->stream, TRUE); + timer_enable_dma_request(TIMER_TAG_TIM(port->timer_tag), TIMER_TAG_CH(port->timer_tag), true); } void dshot_dma_setup_input(uint32_t index) { - tmr_period_value_set(TMR1, GCR_SYMBOL_TIME); - const dshot_gpio_port_t *port = &dshot_gpio_ports[index]; - const dma_stream_def_t *dma = &dma_stream_defs[port->dma_device]; - dma->channel->ctrl_bit.dtd = 0; // DMA_DIR_PERIPHERAL_TO_MEMORY - dma->channel->ctrl_bit.mwidth = DMA_MEMORY_DATA_WIDTH_HALFWORD; - dma->channel->ctrl_bit.pwidth = DMA_PERIPHERAL_DATA_WIDTH_HALFWORD; - dma->channel->paddr = (uint32_t)(&port->gpio->idt); - dma->channel->maddr = (uint32_t)(&dshot_input_buffer[index][0]); - dma->channel->dtcnt_bit.cnt = GCR_DMA_BUFFER_SIZE; + const timer_def_t *tim = &timer_defs[TIMER_TAG_TIM(port->timer_tag)]; + tmr_period_value_set(tim->instance, GCR_SYMBOL_TIME); + + const dma_stream_def_t *dma = &dma_stream_defs[target.dma[port->dma_device].dma]; + dma->stream->ctrl_bit.dtd = 0; // DMA_DIR_PERIPHERAL_TO_MEMORY + dma->stream->ctrl_bit.mwidth = DMA_MEMORY_DATA_WIDTH_HALFWORD; + dma->stream->ctrl_bit.pwidth = DMA_PERIPHERAL_DATA_WIDTH_HALFWORD; + dma->stream->paddr = (uint32_t)(&port->gpio->idt); + dma->stream->maddr = (uint32_t)(&dshot_input_buffer[index][0]); + dma->stream->dtcnt_bit.cnt = GCR_DMA_BUFFER_SIZE; - dma_channel_enable(dma->channel, TRUE); - timer_enable_dma_request(TIMER1, port->timer_channel, true); + dma_channel_enable(dma->stream, TRUE); + timer_enable_dma_request(TIMER_TAG_TIM(port->timer_tag), TIMER_TAG_CH(port->timer_tag), true); } -void dshot_dma_isr(dma_device_t dev) { - const dma_stream_def_t *dma = &dma_stream_defs[dev]; +void dshot_dma_isr(const dma_device_t dev) { + const dma_stream_def_t *dma = &dma_stream_defs[target.dma[dev].dma]; + dma_clear_flag_tc(dma); - dma_channel_enable(dma->channel, FALSE); + dma_channel_enable(dma->stream, FALSE); const dshot_gpio_port_t *port = dshot_gpio_for_device(dev); - timer_enable_dma_request(TIMER1, port->timer_channel, false); + timer_enable_dma_request(TIMER_TAG_TIM(port->timer_tag), TIMER_TAG_CH(port->timer_tag), false); dshot_phase--; diff --git a/src/driver/mcu/at32/motor_pwm.c b/src/driver/mcu/at32/motor_pwm.c index 2f0c45f34..ba51edb6e 100644 --- a/src/driver/mcu/at32/motor_pwm.c +++ b/src/driver/mcu/at32/motor_pwm.c @@ -43,8 +43,8 @@ void motor_pwm_init() { continue; } - const uint8_t tim = TIMER_TAG_TIM(timer_tags[i]); - const uint8_t ch = TIMER_TAG_CH(timer_tags[i]); + const timer_index_t tim = TIMER_TAG_TIM(timer_tags[i]); + const timer_channel_t ch = TIMER_TAG_CH(timer_tags[i]); const timer_def_t *def = &timer_defs[tim]; tmr_counter_enable(def->instance, FALSE); @@ -54,6 +54,7 @@ void motor_pwm_init() { tmr_output_channel_config(def->instance, timer_channel_val(ch), &tim_oc_init); tmr_channel_value_set(def->instance, timer_channel_val(ch), 0); tmr_output_channel_buffer_enable(def->instance, timer_channel_val(ch), TRUE); + tmr_channel_enable(def->instance, timer_channel_val(ch), TRUE); tmr_output_enable(def->instance, TRUE); tmr_counter_enable(def->instance, TRUE); diff --git a/src/driver/mcu/at32/rgb_led.c b/src/driver/mcu/at32/rgb_led.c new file mode 100644 index 000000000..1f18d2aab --- /dev/null +++ b/src/driver/mcu/at32/rgb_led.c @@ -0,0 +1,162 @@ +#include "driver/rgb_led.h" + +#include +#include + +#include "core/failloop.h" +#include "core/project.h" +#include "driver/dma.h" +#include "driver/gpio.h" +#include "driver/interrupt.h" +#include "driver/timer.h" + +#if defined(USE_RGB_LED) + +#define TIMER_HZ PWM_CLOCK_FREQ_HZ +#define TIMER_DIV ((PWM_CLOCK_FREQ_HZ / TIMER_HZ) - 1) + +#define RGB_BIT_TIME ((TIMER_HZ / 800000) - 1) +#define RGB_T0H_TIME ((RGB_BIT_TIME / 3) + 1) +#define RGB_T1H_TIME ((RGB_BIT_TIME / 3) * 2 + 1) + +#define RGB_BITS_LED 24 +#define RGB_BUFFER_SIZE (RGB_BITS_LED * RGB_LED_MAX + 40) + +extern resource_tag_t rgb_timer_tag; +extern volatile bool rgb_dma_busy; + +static DMA_RAM uint32_t rgb_timer_buffer[RGB_BUFFER_SIZE]; +static uint32_t rgb_timer_buffer_count = 0; + +void rgb_led_init() { + const gpio_pins_t pin = target.rgb_led; + if (pin == PIN_NONE) { + return; + } + + rgb_timer_tag = target.dma[DMA_DEVICE_RGB].tag; + if (rgb_timer_tag == 0) { + return; + } + + gpio_config_t gpio_init; + gpio_init.mode = GPIO_ALTERNATE; + gpio_init.drive = GPIO_DRIVE_HIGH; + gpio_init.output = GPIO_PUSHPULL; + gpio_init.pull = GPIO_DOWN_PULL; + gpio_pin_init_tag(pin, gpio_init, rgb_timer_tag); + + const timer_channel_t ch = TIMER_TAG_CH(rgb_timer_tag); + const timer_index_t tim = TIMER_TAG_TIM(rgb_timer_tag); + const timer_def_t *def = &timer_defs[tim]; + + rcc_enable(def->rcc); + + tmr_counter_enable(def->instance, FALSE); + + tmr_base_init(def->instance, RGB_BIT_TIME, TIMER_DIV); + tmr_cnt_dir_set(def->instance, TMR_COUNT_UP); + tmr_clock_source_div_set(def->instance, TMR_CLOCK_DIV1); + + tmr_output_config_type tim_oc_init; + tmr_output_default_para_init(&tim_oc_init); + tim_oc_init.oc_mode = TMR_OUTPUT_CONTROL_PWM_MODE_A; + tim_oc_init.oc_idle_state = FALSE; + tim_oc_init.oc_polarity = TMR_OUTPUT_ACTIVE_HIGH; + tim_oc_init.oc_output_state = TRUE; + tmr_output_channel_config(def->instance, timer_channel_val(ch), &tim_oc_init); + + tmr_channel_value_set(def->instance, timer_channel_val(ch), 0); + tmr_output_channel_buffer_enable(def->instance, timer_channel_val(ch), TRUE); + tmr_channel_enable(def->instance, timer_channel_val(ch), TRUE); + + tmr_period_buffer_enable(def->instance, TRUE); + + const dma_stream_def_t *rgb_dma = &dma_stream_defs[target.dma[DMA_DEVICE_RGB].dma]; + dma_enable_rcc(rgb_dma); + dma_reset(rgb_dma->stream); + + dma_init_type init; + init.peripheral_base_addr = timer_channel_addr(def->instance, ch); + init.memory_base_addr = (uint32_t)rgb_timer_buffer; + init.direction = DMA_DIR_MEMORY_TO_PERIPHERAL; + init.buffer_size = RGB_BUFFER_SIZE; + init.peripheral_inc_enable = FALSE; + init.memory_inc_enable = TRUE; + init.peripheral_data_width = DMA_PERIPHERAL_DATA_WIDTH_WORD; + init.memory_data_width = DMA_MEMORY_DATA_WIDTH_WORD; + init.loop_mode_enable = FALSE; + init.priority = DMA_PRIORITY_MEDIUM; + dma_init(rgb_dma->stream, &init); + dmamux_init(rgb_dma->mux, target.dma[DMA_DEVICE_RGB].request); + + interrupt_enable(rgb_dma->irq, DMA_PRIORITY); + + tmr_output_enable(def->instance, TRUE); +} + +void rgb_led_set_value(uint32_t value, uint32_t count) { + if (rgb_led_busy()) { + return; + } + + uint32_t offset = 0; + for (uint32_t i = 0; i < 20; i++) { + rgb_timer_buffer[offset++] = 0; + } + for (uint32_t i = 0; i < count; i++) { + // rgb_led_value contains a (32bit) int that contains the RGB values in G R B format already + // Test each bit and assign the T1H or T0H depending on whether it is 1 or 0. + for (int32_t j = RGB_BITS_LED - 1; j >= 0; j--) { + rgb_timer_buffer[offset++] = ((value >> j) & 0x1) ? RGB_T1H_TIME : RGB_T0H_TIME; + } + } + for (uint32_t i = 0; i < 20; i++) { + rgb_timer_buffer[offset++] = 0; + } + rgb_timer_buffer_count = offset; +} + +void rgb_led_send() { + if (rgb_led_busy()) { + return; + } + + rgb_dma_busy = true; + + const timer_channel_t ch = TIMER_TAG_CH(rgb_timer_tag); + const timer_def_t *tim = &timer_defs[TIMER_TAG_TIM(rgb_timer_tag)]; + + const dma_stream_def_t *rgb_dma = &dma_stream_defs[target.dma[DMA_DEVICE_RGB].dma]; + + dma_clear_flag_tc(rgb_dma); + dma_prepare_tx_memory((void *)rgb_timer_buffer, sizeof(rgb_timer_buffer)); + + rgb_dma->stream->paddr = timer_channel_addr(tim->instance, ch); + rgb_dma->stream->maddr = (uint32_t)rgb_timer_buffer; + rgb_dma->stream->dtcnt = rgb_timer_buffer_count; + + dma_interrupt_enable(rgb_dma->stream, DMA_FDT_INT, TRUE); + + dma_channel_enable(rgb_dma->stream, TRUE); + timer_enable_dma_request(TIMER_TAG_TIM(rgb_timer_tag), ch, TRUE); + tmr_counter_enable(tim->instance, TRUE); +} + +void rgb_dma_isr(const dma_device_t dev) { + const dma_stream_def_t *dma = &dma_stream_defs[target.dma[dev].dma]; + + dma_clear_flag_tc(dma); + dma_channel_enable(dma->stream, FALSE); + + const timer_index_t tim = TIMER_TAG_TIM(rgb_timer_tag); + const timer_channel_t ch = TIMER_TAG_CH(rgb_timer_tag); + const timer_def_t *def = &timer_defs[tim]; + + tmr_counter_enable(def->instance, FALSE); + timer_enable_dma_request(tim, ch, FALSE); + + rgb_dma_busy = false; +} + +#endif \ No newline at end of file diff --git a/src/driver/mcu/at32/spi.c b/src/driver/mcu/at32/spi.c index 6c57fa63f..e829ddbd4 100644 --- a/src/driver/mcu/at32/spi.c +++ b/src/driver/mcu/at32/spi.c @@ -40,8 +40,6 @@ const spi_port_def_t spi_port_defs[SPI_PORT_MAX] = { extern FAST_RAM spi_txn_t txn_pool[SPI_TXN_MAX]; -#define PORT spi_port_defs[port] - static uint32_t spi_divider_to_ll(uint32_t divider) { switch (divider) { default: @@ -81,13 +79,13 @@ static uint32_t spi_find_divder(uint32_t clk_hz) { } static void spi_dma_init_rx(spi_ports_t port) { - const dma_stream_def_t *dma = &dma_stream_defs[PORT.dma_rx]; + const dma_stream_def_t *dma = &dma_stream_defs[target.dma[spi_port_defs[port].dma_rx].dma]; - dma_reset(dma->channel); - dmamux_init(dma->mux, dma->request); + dma_reset(dma->stream); + dmamux_init(dma->mux, target.dma[spi_port_defs[port].dma_rx].request); dma_init_type init; - init.peripheral_base_addr = (uint32_t)(&PORT.channel->dt); + init.peripheral_base_addr = (uint32_t)(&spi_port_defs[port].channel->dt); init.memory_base_addr = 0; init.direction = DMA_DIR_PERIPHERAL_TO_MEMORY; init.buffer_size = 0; @@ -97,25 +95,17 @@ static void spi_dma_init_rx(spi_ports_t port) { init.memory_data_width = DMA_MEMORY_DATA_WIDTH_BYTE; init.loop_mode_enable = FALSE; init.priority = DMA_PRIORITY_HIGH; - dma_init(dma->channel, &init); -} - -static void spi_dma_reset_rx(spi_ports_t port, uint8_t *rx_data, uint32_t rx_size) { - dma_prepare_rx_memory(rx_data, rx_size); - - const dma_stream_def_t *dma = &dma_stream_defs[PORT.dma_rx]; - dma->channel->maddr = (uint32_t)rx_data; - dma_data_number_set(dma->channel, rx_size); + dma_init(dma->stream, &init); } static void spi_dma_init_tx(spi_ports_t port) { - const dma_stream_def_t *dma = &dma_stream_defs[PORT.dma_tx]; + const dma_stream_def_t *dma = &dma_stream_defs[target.dma[spi_port_defs[port].dma_tx].dma]; - dma_reset(dma->channel); - dmamux_init(dma->mux, dma->request); + dma_reset(dma->stream); + dmamux_init(dma->mux, target.dma[spi_port_defs[port].dma_tx].request); dma_init_type init; - init.peripheral_base_addr = (uint32_t)(&PORT.channel->dt); + init.peripheral_base_addr = (uint32_t)(&spi_port_defs[port].channel->dt); init.memory_base_addr = 0; init.direction = DMA_DIR_MEMORY_TO_PERIPHERAL; init.buffer_size = 0; @@ -125,15 +115,7 @@ static void spi_dma_init_tx(spi_ports_t port) { init.memory_data_width = DMA_MEMORY_DATA_WIDTH_BYTE; init.loop_mode_enable = FALSE; init.priority = DMA_PRIORITY_HIGH; - dma_init(dma->channel, &init); -} - -static void spi_dma_reset_tx(spi_ports_t port, uint8_t *tx_data, uint32_t tx_size) { - dma_prepare_tx_memory(tx_data, tx_size); - - const dma_stream_def_t *dma = &dma_stream_defs[PORT.dma_tx]; - dma->channel->maddr = (uint32_t)tx_data; - dma_data_number_set(dma->channel, tx_size); + dma_init(dma->stream, &init); } static void spi_set_divider(spi_type *channel, spi_mclk_freq_div_type div) { @@ -186,32 +168,42 @@ void spi_reconfigure(spi_bus_device_t *bus) { } void spi_dma_transfer_begin(spi_ports_t port, uint8_t *buffer, uint32_t length) { - const dma_stream_def_t *dma_tx = &dma_stream_defs[PORT.dma_tx]; - const dma_stream_def_t *dma_rx = &dma_stream_defs[PORT.dma_rx]; + const dma_stream_def_t *dma_tx = &dma_stream_defs[target.dma[spi_port_defs[port].dma_tx].dma]; + const dma_stream_def_t *dma_rx = &dma_stream_defs[target.dma[spi_port_defs[port].dma_rx].dma]; dma_clear_flag_tc(dma_rx); dma_clear_flag_tc(dma_tx); - spi_dma_reset_rx(port, buffer, length); - spi_dma_reset_tx(port, buffer, length); + dma_prepare_rx_memory(buffer, length); + dma_prepare_tx_memory(buffer, length); + + dma_rx->stream->maddr = (uint32_t)buffer; + dma_data_number_set(dma_rx->stream, length); + + dma_tx->stream->maddr = (uint32_t)buffer; + dma_data_number_set(dma_tx->stream, length); - dma_interrupt_enable(dma_rx->channel, DMA_FDT_INT, TRUE); - dma_interrupt_enable(dma_rx->channel, DMA_DTERR_INT, TRUE); + dma_interrupt_enable(dma_rx->stream, DMA_FDT_INT, TRUE); + dma_interrupt_enable(dma_rx->stream, DMA_DTERR_INT, TRUE); - dma_channel_enable(dma_rx->channel, TRUE); - dma_channel_enable(dma_tx->channel, TRUE); + dma_channel_enable(dma_rx->stream, TRUE); + dma_channel_enable(dma_tx->stream, TRUE); - spi_i2s_dma_transmitter_enable(PORT.channel, TRUE); - spi_i2s_dma_receiver_enable(PORT.channel, TRUE); + spi_i2s_dma_transmitter_enable(spi_port_defs[port].channel, TRUE); + spi_i2s_dma_receiver_enable(spi_port_defs[port].channel, TRUE); - spi_enable(PORT.channel, TRUE); + spi_enable(spi_port_defs[port].channel, TRUE); } void spi_device_init(spi_ports_t port) { const spi_port_def_t *def = &spi_port_defs[port]; rcc_enable(def->rcc); - dma_enable_rcc(def->dma_rx); - dma_enable_rcc(def->dma_tx); + + const dma_stream_def_t *dma_rx = &dma_stream_defs[target.dma[spi_port_defs[port].dma_rx].dma]; + const dma_stream_def_t *dma_tx = &dma_stream_defs[target.dma[spi_port_defs[port].dma_tx].dma]; + + dma_enable_rcc(dma_rx); + dma_enable_rcc(dma_tx); spi_i2s_reset(def->channel); @@ -233,7 +225,6 @@ void spi_device_init(spi_ports_t port) { spi_dma_init_rx(port); spi_dma_init_tx(port); - const dma_stream_def_t *dma_rx = &dma_stream_defs[def->dma_rx]; interrupt_enable(dma_rx->irq, DMA_PRIORITY); } @@ -250,7 +241,7 @@ void spi_seg_submit_wait_ex(spi_bus_device_t *bus, const spi_txn_segment_t *segs spi_reconfigure(bus); spi_csn_enable(bus); - spi_enable(PORT.channel, TRUE); + spi_enable(spi_port_defs[port].channel, TRUE); for (uint32_t i = 0; i < count; i++) { const spi_txn_segment_t *seg = &segs[i]; @@ -267,15 +258,15 @@ void spi_seg_submit_wait_ex(spi_bus_device_t *bus, const spi_txn_segment_t *segs } for (uint32_t j = 0; j < size; j++) { - while (!spi_i2s_flag_get(PORT.channel, SPI_I2S_TDBE_FLAG)) + while (!spi_i2s_flag_get(spi_port_defs[port].channel, SPI_I2S_TDBE_FLAG)) ; - spi_i2s_data_transmit(PORT.channel, tx_data ? tx_data[j] : 0xFF); + spi_i2s_data_transmit(spi_port_defs[port].channel, tx_data ? tx_data[j] : 0xFF); - while (!spi_i2s_flag_get(PORT.channel, SPI_I2S_RDBF_FLAG)) + while (!spi_i2s_flag_get(spi_port_defs[port].channel, SPI_I2S_RDBF_FLAG)) ; - const uint8_t ret = spi_i2s_data_receive(PORT.channel); + const uint8_t ret = spi_i2s_data_receive(spi_port_defs[port].channel); if (rx_data != NULL) { rx_data[j] = ret; } @@ -283,14 +274,14 @@ void spi_seg_submit_wait_ex(spi_bus_device_t *bus, const spi_txn_segment_t *segs } spi_csn_disable(bus); - spi_enable(PORT.channel, FALSE); + spi_enable(spi_port_defs[port].channel, FALSE); spi_dev[port].dma_done = true; } static void handle_dma_rx_isr(spi_ports_t port) { - const dma_stream_def_t *dma_rx = &dma_stream_defs[PORT.dma_rx]; - const dma_stream_def_t *dma_tx = &dma_stream_defs[PORT.dma_tx]; + const dma_stream_def_t *dma_tx = &dma_stream_defs[target.dma[spi_port_defs[port].dma_tx].dma]; + const dma_stream_def_t *dma_rx = &dma_stream_defs[target.dma[spi_port_defs[port].dma_rx].dma]; if (!dma_is_flag_active_tc(dma_rx)) { return; @@ -299,21 +290,21 @@ static void handle_dma_rx_isr(spi_ports_t port) { dma_clear_flag_tc(dma_rx); dma_clear_flag_tc(dma_tx); - dma_interrupt_enable(dma_rx->channel, DMA_FDT_INT, FALSE); - dma_interrupt_enable(dma_rx->channel, DMA_DTERR_INT, FALSE); + dma_interrupt_enable(dma_rx->stream, DMA_FDT_INT, FALSE); + dma_interrupt_enable(dma_rx->stream, DMA_DTERR_INT, FALSE); - spi_i2s_dma_transmitter_enable(PORT.channel, FALSE); - spi_i2s_dma_receiver_enable(PORT.channel, FALSE); + spi_i2s_dma_transmitter_enable(spi_port_defs[port].channel, FALSE); + spi_i2s_dma_receiver_enable(spi_port_defs[port].channel, FALSE); - dma_channel_enable(dma_rx->channel, FALSE); - dma_channel_enable(dma_tx->channel, FALSE); + dma_channel_enable(dma_rx->stream, FALSE); + dma_channel_enable(dma_tx->stream, FALSE); - spi_enable(PORT.channel, FALSE); + spi_enable(spi_port_defs[port].channel, FALSE); spi_txn_finish(port); } -void spi_dma_isr(dma_device_t dev) { +void spi_dma_isr(const dma_device_t dev) { switch (dev) { case DMA_DEVICE_SPI1_RX: handle_dma_rx_isr(SPI_PORT1); diff --git a/src/driver/mcu/at32/system.h b/src/driver/mcu/at32/system.h index 8e9a65176..df760fa2a 100644 --- a/src/driver/mcu/at32/system.h +++ b/src/driver/mcu/at32/system.h @@ -20,17 +20,13 @@ typedef tmr_type timer_dev_t; typedef usart_type usart_dev_t; typedef struct { - uint32_t device; - dma_type *port; uint8_t port_index; - dma_channel_type *channel; - uint8_t channel_index; + dma_channel_type *stream; + uint8_t stream_index; - uint32_t request; dmamux_channel_type *mux; - IRQn_Type irq; } dma_stream_def_t; diff --git a/src/driver/mcu/at32/timer.c b/src/driver/mcu/at32/timer.c index fd7ab6004..4b06b6ba5 100644 --- a/src/driver/mcu/at32/timer.c +++ b/src/driver/mcu/at32/timer.c @@ -108,6 +108,25 @@ uint32_t timer_channel_val(timer_channel_t chan) { } } +uint32_t timer_channel_addr(timer_dev_t *timer, timer_channel_t chan) { + switch (chan) { + case TIMER_CH1: + case TIMER_CH1N: + return (uint32_t)(&timer->c1dt); + case TIMER_CH2: + case TIMER_CH2N: + return (uint32_t)(&timer->c2dt); + case TIMER_CH3: + case TIMER_CH3N: + return (uint32_t)(&timer->c3dt); + case TIMER_CH4: + case TIMER_CH4N: + return (uint32_t)(&timer->c4dt); + default: + return 0; + } +} + void timer_enable_dma_request(timer_index_t tim, timer_channel_t chan, bool state) { const timer_def_t *def = &timer_defs[tim]; diff --git a/src/driver/mcu/stm32/dma.c b/src/driver/mcu/stm32/dma.c index 13e7af8fe..04d880227 100644 --- a/src/driver/mcu/stm32/dma.c +++ b/src/driver/mcu/stm32/dma.c @@ -1,103 +1,38 @@ #include "driver/dma.h" +#include "core/failloop.h" +#include "driver/adc.h" #include "driver/rcc.h" +#include "driver/resource.h" +#include "driver/timer.h" -#if !defined(STM32G4) -// DMA1 Stream0 SPI3_RX -// DMA1 Stream1 -// DMA1 Stream2 -// DMA1 Stream3 SPI2_RX -// DMA1 Stream4 SPI2_TX -// DMA1 Stream5 -// DMA1 Stream6 -// DMA1 Stream7 SPI3_TX - -// DMA2 Stream0 SPI4_RX -// DMA2 Stream1 SPI4_TX -// DMA2 Stream2 SPI1_RX -// DMA2 Stream3 TIM1_CH1 -// DMA2 Stream4 TIM1_CH4 -// DMA2 Stream5 SPI1_TX -// DMA2 Stream6 TIM1_CH3 -// DMA2 Stream7 - -#define DMA_STREAMS \ - DMA_STREAM(2, 3, 2, SPI1_RX) \ - DMA_STREAM(2, 3, 5, SPI1_TX) \ - DMA_STREAM(1, 0, 3, SPI2_RX) \ - DMA_STREAM(1, 0, 4, SPI2_TX) \ - DMA_STREAM(1, 0, 0, SPI3_RX) \ - DMA_STREAM(1, 0, 7, SPI3_TX) \ - DMA_STREAM(2, 4, 0, SPI4_RX) \ - DMA_STREAM(2, 4, 1, SPI4_TX) \ - DMA_STREAM(2, 6, 3, TIM1_CH1) \ - DMA_STREAM(2, 6, 6, TIM1_CH3) \ - DMA_STREAM(2, 6, 4, TIM1_CH4) -#else -#define DMA_STREAMS \ - DMA_STREAM(1, 0, 1, SPI1_RX) \ - DMA_STREAM(1, 0, 2, SPI1_TX) \ - DMA_STREAM(1, 0, 3, SPI2_RX) \ - DMA_STREAM(1, 0, 4, SPI2_TX) \ - DMA_STREAM(1, 0, 5, SPI3_RX) \ - DMA_STREAM(1, 0, 6, SPI3_TX) \ - DMA_STREAM(1, 0, 7, SPI4_RX) \ - DMA_STREAM(1, 0, 8, SPI4_TX) \ - DMA_STREAM(2, 0, 1, TIM1_CH1) \ - DMA_STREAM(2, 0, 2, TIM1_CH3) \ - DMA_STREAM(2, 0, 3, TIM1_CH4) +#if defined(STM32F7) || defined(STM32H7) +#define CACHE_LINE_SIZE 32 +#define CACHE_LINE_MASK (CACHE_LINE_SIZE - 1) #endif #if defined(STM32G4) -#define DMA_STREAM(_port, _chan, _stream, _dev) \ - [DMA_DEVICE_##_dev] = { \ - .device = DMA_DEVICE_##_dev, \ +#define DMA_STREAM(_port, _stream) \ + [DMA##_port##_STREAM##_stream] = { \ .port = DMA##_port, \ .port_index = _port, \ - .channel = 0, \ - .channel_index = 0, \ - .request = LL_DMAMUX_REQ_##_dev, \ .stream = DMA##_port##_Channel##_stream, \ .stream_index = LL_DMA_CHANNEL_##_stream, \ .irq = DMA##_port##_Channel##_stream##_IRQn, \ }, -#elif defined(STM32H7) -#define DMA_STREAM(_port, _chan, _stream, _dev) \ - [DMA_DEVICE_##_dev] = { \ - .device = DMA_DEVICE_##_dev, \ - .port = DMA##_port, \ - .port_index = _port, \ - .channel = -1, \ - .channel_index = -1, \ - .request = LL_DMAMUX1_REQ_##_dev, \ - .stream = DMA##_port##_Stream##_stream, \ - .stream_index = LL_DMA_STREAM_##_stream, \ - .irq = DMA##_port##_Stream##_stream##_IRQn, \ - }, #else -#define DMA_STREAM(_port, _chan, _stream, _dev) \ - [DMA_DEVICE_##_dev] = { \ - .device = DMA_DEVICE_##_dev, \ +#define DMA_STREAM(_port, _stream) \ + [DMA##_port##_STREAM##_stream] = { \ .port = DMA##_port, \ .port_index = _port, \ - .channel = LL_DMA_CHANNEL_##_chan, \ - .channel_index = _chan, \ - .request = -1, \ .stream = DMA##_port##_Stream##_stream, \ .stream_index = LL_DMA_STREAM_##_stream, \ .irq = DMA##_port##_Stream##_stream##_IRQn, \ }, #endif - -const dma_stream_def_t dma_stream_defs[DMA_DEVICE_MAX] = {DMA_STREAMS}; - +const dma_stream_def_t dma_stream_defs[DMA_STREAM_MAX] = {DMA_STREAMS}; #undef DMA_STREAM -#if defined(STM32F7) || defined(STM32H7) -#define CACHE_LINE_SIZE 32 -#define CACHE_LINE_MASK (CACHE_LINE_SIZE - 1) -#endif - void dma_prepare_tx_memory(void *addr, uint32_t size) { #if defined(STM32F7) || defined(STM32H7) if (!WITHIN_DTCM_RAM(addr) && !WITHIN_DMA_RAM(addr)) { @@ -114,13 +49,12 @@ void dma_prepare_rx_memory(void *addr, uint32_t size) { #endif } -void dma_enable_rcc(dma_device_t dev) { +void dma_enable_rcc(const dma_stream_def_t *def) { #ifdef STM32G4 rcc_enable(RCC_AHB1_GRP1(DMAMUX1)); #endif - const dma_stream_def_t *dma = &dma_stream_defs[dev]; - switch (dma->port_index) { + switch (def->port_index) { case 1: rcc_enable(RCC_AHB1_GRP1(DMA1)); break; @@ -130,10 +64,37 @@ void dma_enable_rcc(dma_device_t dev) { } } -extern void dshot_dma_isr(dma_device_t dev); -extern void spi_dma_isr(dma_device_t dev); +#if !defined(STM32H7) && !defined(STM32G4) +uint32_t dma_map_channel(uint32_t channel) { + switch (channel) { + case 0: + return LL_DMA_CHANNEL_0; + case 1: + return LL_DMA_CHANNEL_1; + case 2: + return LL_DMA_CHANNEL_2; + case 3: + return LL_DMA_CHANNEL_3; + case 4: + return LL_DMA_CHANNEL_4; + case 5: + return LL_DMA_CHANNEL_5; + case 6: + return LL_DMA_CHANNEL_6; + case 7: + return LL_DMA_CHANNEL_7; + default: + failloop(FAILLOOP_DMA); + return 0; + } +} +#endif -static void handle_dma_stream_isr(dma_device_t dev) { +extern void dshot_dma_isr(const dma_device_t); +extern void spi_dma_isr(const dma_device_t); +extern void rgb_dma_isr(const dma_device_t); + +static void handle_dma_stream_isr(const dma_device_t dev) { switch (dev) { case DMA_DEVICE_SPI1_RX: case DMA_DEVICE_SPI2_RX: @@ -145,21 +106,27 @@ static void handle_dma_stream_isr(dma_device_t dev) { case DMA_DEVICE_SPI4_TX: spi_dma_isr(dev); break; - case DMA_DEVICE_TIM1_CH1: - case DMA_DEVICE_TIM1_CH3: - case DMA_DEVICE_TIM1_CH4: + case DMA_DEVICE_DSHOT_CH1: + case DMA_DEVICE_DSHOT_CH2: + case DMA_DEVICE_DSHOT_CH3: #ifdef USE_MOTOR_DSHOT dshot_dma_isr(dev); #endif break; + case DMA_DEVICE_RGB: +#ifdef USE_RGB_LED + rgb_dma_isr(dev); +#endif + break; + case DMA_DEVICE_INVALID: case DMA_DEVICE_MAX: break; } } -#define DMA_STREAM(_port, _chan, _stream, _dev) \ - void DMA##_port##_Stream##_stream##_IRQHandler() { handle_dma_stream_isr(DMA_DEVICE_##_dev); } \ - void DMA##_port##_Channel##_stream##_IRQHandler() { handle_dma_stream_isr(DMA_DEVICE_##_dev); } +#define DMA_STREAM(_port, _stream) \ + void DMA##_port##_Stream##_stream##_IRQHandler() { handle_dma_stream_isr(dma_stream_map[DMA##_port##_STREAM##_stream]); } \ + void DMA##_port##_Channel##_stream##_IRQHandler() { handle_dma_stream_isr(dma_stream_map[DMA##_port##_STREAM##_stream]); } DMA_STREAMS diff --git a/src/driver/mcu/stm32/dma.h b/src/driver/mcu/stm32/dma.h index 4f6269627..74f687bd6 100644 --- a/src/driver/mcu/stm32/dma.h +++ b/src/driver/mcu/stm32/dma.h @@ -1,5 +1,7 @@ #pragma once +#include + #ifdef STM32G4 #define DMA_FLAG_TE (0x1 << 3) #define DMA_FLAG_HT (0x1 << 2) @@ -14,6 +16,24 @@ #define LL_DMA_DisableStream LL_DMA_DisableChannel #define LL_DMA_IsEnabledStream LL_DMA_IsEnabledChannel +#define DMA_STREAMS \ + DMA_STREAM(1, 1) \ + DMA_STREAM(1, 2) \ + DMA_STREAM(1, 3) \ + DMA_STREAM(1, 4) \ + DMA_STREAM(1, 5) \ + DMA_STREAM(1, 6) \ + DMA_STREAM(1, 7) \ + DMA_STREAM(1, 8) \ + DMA_STREAM(2, 1) \ + DMA_STREAM(2, 2) \ + DMA_STREAM(2, 3) \ + DMA_STREAM(2, 4) \ + DMA_STREAM(2, 5) \ + DMA_STREAM(2, 6) \ + DMA_STREAM(2, 7) \ + DMA_STREAM(2, 8) + #else #define DMA_FLAG_TC (0x1 << 5) #define DMA_FLAG_HT (0x1 << 4) @@ -35,4 +55,24 @@ static const uint32_t _dma_flag_shift[] = {0, 6, 16, 22, 0, 6, 16, 22}; WRITE_REG(dev->port->HIFCR, dma_flag_for_channel(dev, DMA_FLAG_TC | DMA_FLAG_TE | DMA_FLAG_HT | DMA_FLAG_FE)); \ } -#endif \ No newline at end of file +#define DMA_STREAMS \ + DMA_STREAM(1, 0) \ + DMA_STREAM(1, 1) \ + DMA_STREAM(1, 2) \ + DMA_STREAM(1, 3) \ + DMA_STREAM(1, 4) \ + DMA_STREAM(1, 5) \ + DMA_STREAM(1, 6) \ + DMA_STREAM(1, 7) \ + DMA_STREAM(2, 0) \ + DMA_STREAM(2, 1) \ + DMA_STREAM(2, 2) \ + DMA_STREAM(2, 3) \ + DMA_STREAM(2, 4) \ + DMA_STREAM(2, 5) \ + DMA_STREAM(2, 6) \ + DMA_STREAM(2, 7) + +#endif + +uint32_t dma_map_channel(uint32_t channel); \ No newline at end of file diff --git a/src/driver/mcu/stm32/motor_dshot.c b/src/driver/mcu/stm32/motor_dshot.c index 90d8b4469..c4369a6b5 100644 --- a/src/driver/mcu/stm32/motor_dshot.c +++ b/src/driver/mcu/stm32/motor_dshot.c @@ -2,6 +2,7 @@ #include +#include "core/failloop.h" #include "core/profile.h" #include "core/project.h" #include "driver/dma.h" @@ -11,7 +12,22 @@ #ifdef USE_MOTOR_DSHOT -static void dshot_init_gpio_port(dshot_gpio_port_t *port) { +void dshot_init_gpio_port(dshot_gpio_port_t *port) { + const timer_def_t *tim = &timer_defs[TIMER_TAG_TIM(port->timer_tag)]; + + rcc_enable(tim->rcc); + + // setup timer to 1/3 of the full bit time + LL_TIM_InitTypeDef tim_init; + LL_TIM_StructInit(&tim_init); + tim_init.Autoreload = DSHOT_SYMBOL_TIME; + tim_init.Prescaler = 0; + tim_init.ClockDivision = 0; + tim_init.CounterMode = LL_TIM_COUNTERMODE_UP; + LL_TIM_Init(tim->instance, &tim_init); + LL_TIM_EnableARRPreload(tim->instance); + LL_TIM_DisableMasterSlaveMode(tim->instance); + LL_TIM_OC_InitTypeDef tim_oc_init; LL_TIM_OC_StructInit(&tim_oc_init); tim_oc_init.OCMode = LL_TIM_OCMODE_PWM1; @@ -19,20 +35,21 @@ static void dshot_init_gpio_port(dshot_gpio_port_t *port) { tim_oc_init.OCState = LL_TIM_OCSTATE_ENABLE; tim_oc_init.OCPolarity = LL_TIM_OCPOLARITY_LOW; tim_oc_init.CompareValue = 10; - LL_TIM_OC_Init(TIM1, timer_channel_val(port->timer_channel), &tim_oc_init); - LL_TIM_OC_EnablePreload(TIM1, timer_channel_val(port->timer_channel)); - const dma_stream_def_t *dma = &dma_stream_defs[port->dma_device]; + const uint32_t ch = timer_channel_val(TIMER_TAG_CH(port->timer_tag)); + LL_TIM_OC_Init(tim->instance, ch, &tim_oc_init); + LL_TIM_OC_EnablePreload(tim->instance, ch); - dma_enable_rcc(port->dma_device); + const dma_stream_def_t *dma = &dma_stream_defs[target.dma[port->dma_device].dma]; + dma_enable_rcc(dma); LL_DMA_DeInit(dma->port, dma->stream_index); LL_DMA_InitTypeDef DMA_InitStructure; LL_DMA_StructInit(&DMA_InitStructure); #if defined(STM32H7) || defined(STM32G4) - DMA_InitStructure.PeriphRequest = dma->request; + DMA_InitStructure.PeriphRequest = target.dma[port->dma_device].request; #else - DMA_InitStructure.Channel = dma->channel; + DMA_InitStructure.Channel = dma_map_channel(target.dma[port->dma_device].channel); #endif DMA_InitStructure.PeriphOrM2MSrcAddress = 0x0; // overwritten later DMA_InitStructure.MemoryOrM2MDstAddress = 0x0; // overwritten later @@ -50,44 +67,20 @@ static void dshot_init_gpio_port(dshot_gpio_port_t *port) { DMA_InitStructure.PeriphBurst = LL_DMA_PBURST_SINGLE; #endif LL_DMA_Init(dma->port, dma->stream_index, &DMA_InitStructure); - LL_DMA_EnableIT_TC(dma->port, dma->stream_index); interrupt_enable(dma->irq, DMA_PRIORITY); -} - -void dshot_init_timer() { - rcc_enable(RCC_APB2_GRP1(TIM1)); - - // setup timer to 1/3 of the full bit time - LL_TIM_InitTypeDef tim_init; - LL_TIM_StructInit(&tim_init); - tim_init.Autoreload = DSHOT_SYMBOL_TIME; - tim_init.Prescaler = 0; - tim_init.ClockDivision = 0; - tim_init.CounterMode = LL_TIM_COUNTERMODE_UP; - LL_TIM_Init(TIM1, &tim_init); - LL_TIM_EnableARRPreload(TIM1); - LL_TIM_DisableMasterSlaveMode(TIM1); - - for (uint32_t j = 0; j < dshot_gpio_port_count; j++) { - dshot_init_gpio_port(&dshot_gpio_ports[j]); - - for (uint8_t i = 0; i < DSHOT_DMA_SYMBOLS; i++) { - dshot_output_buffer[j][i * 3 + 0] = dshot_gpio_ports[j].set_mask; // start bit - dshot_output_buffer[j][i * 3 + 1] = 0; // actual bit, set below - dshot_output_buffer[j][i * 3 + 2] = dshot_gpio_ports[j].reset_mask; // return line to low - } - } + LL_DMA_EnableIT_TC(dma->port, dma->stream_index); - LL_TIM_EnableCounter(TIM1); + LL_TIM_EnableCounter(tim->instance); } void dshot_dma_setup_output(uint32_t index) { - LL_TIM_SetAutoReload(TIM1, DSHOT_SYMBOL_TIME); - const dshot_gpio_port_t *port = &dshot_gpio_ports[index]; - const dma_stream_def_t *dma = &dma_stream_defs[port->dma_device]; + const timer_def_t *tim = &timer_defs[TIMER_TAG_TIM(port->timer_tag)]; + LL_TIM_SetAutoReload(tim->instance, DSHOT_SYMBOL_TIME); + + const dma_stream_def_t *dma = &dma_stream_defs[target.dma[port->dma_device].dma]; LL_DMA_SetPeriphAddress(dma->port, dma->stream_index, (uint32_t)&port->gpio->BSRR); LL_DMA_SetMemoryAddress(dma->port, dma->stream_index, (uint32_t)&dshot_output_buffer[index][0]); LL_DMA_SetDataLength(dma->port, dma->stream_index, DSHOT_DMA_BUFFER_SIZE); @@ -96,15 +89,16 @@ void dshot_dma_setup_output(uint32_t index) { LL_DMA_SetMemorySize(dma->port, dma->stream_index, LL_DMA_MDATAALIGN_WORD); LL_DMA_EnableStream(dma->port, dma->stream_index); - timer_enable_dma_request(TIMER1, port->timer_channel, true); + timer_enable_dma_request(TIMER_TAG_TIM(port->timer_tag), TIMER_TAG_CH(port->timer_tag), true); } void dshot_dma_setup_input(uint32_t index) { - LL_TIM_SetAutoReload(TIM1, GCR_SYMBOL_TIME); - const dshot_gpio_port_t *port = &dshot_gpio_ports[index]; - const dma_stream_def_t *dma = &dma_stream_defs[port->dma_device]; + const timer_def_t *tim = &timer_defs[TIMER_TAG_TIM(port->timer_tag)]; + LL_TIM_SetAutoReload(tim->instance, GCR_SYMBOL_TIME); + + const dma_stream_def_t *dma = &dma_stream_defs[target.dma[port->dma_device].dma]; LL_DMA_SetPeriphAddress(dma->port, dma->stream_index, (uint32_t)&port->gpio->IDR); LL_DMA_SetMemoryAddress(dma->port, dma->stream_index, (uint32_t)&dshot_input_buffer[index][0]); LL_DMA_SetDataLength(dma->port, dma->stream_index, GCR_DMA_BUFFER_SIZE); @@ -113,16 +107,17 @@ void dshot_dma_setup_input(uint32_t index) { LL_DMA_SetMemorySize(dma->port, dma->stream_index, LL_DMA_MDATAALIGN_HALFWORD); LL_DMA_EnableStream(dma->port, dma->stream_index); - timer_enable_dma_request(TIMER1, port->timer_channel, true); + timer_enable_dma_request(TIMER_TAG_TIM(port->timer_tag), TIMER_TAG_CH(port->timer_tag), true); } -void dshot_dma_isr(dma_device_t dev) { - const dma_stream_def_t *dma = &dma_stream_defs[dev]; +void dshot_dma_isr(const dma_device_t dev) { + const dma_stream_def_t *dma = &dma_stream_defs[target.dma[dev].dma]; + dma_clear_flag_tc(dma); LL_DMA_DisableStream(dma->port, dma->stream_index); const dshot_gpio_port_t *port = dshot_gpio_for_device(dev); - timer_enable_dma_request(TIMER1, port->timer_channel, false); + timer_enable_dma_request(TIMER_TAG_TIM(port->timer_tag), TIMER_TAG_CH(port->timer_tag), false); dshot_phase--; diff --git a/src/driver/mcu/stm32/motor_pwm.c b/src/driver/mcu/stm32/motor_pwm.c index 1b26ea667..2fcc71a35 100644 --- a/src/driver/mcu/stm32/motor_pwm.c +++ b/src/driver/mcu/stm32/motor_pwm.c @@ -43,8 +43,8 @@ void motor_pwm_init() { continue; } - const uint8_t tim = TIMER_TAG_TIM(timer_tags[i]); - const uint8_t ch = TIMER_TAG_CH(timer_tags[i]); + const timer_index_t tim = TIMER_TAG_TIM(timer_tags[i]); + const timer_channel_t ch = TIMER_TAG_CH(timer_tags[i]); timer_up_init(tim, PWM_DIVIDER, PWM_TOP); LL_TIM_OC_Init(timer_defs[tim].instance, timer_channel_val(ch), &tim_oc_init); LL_TIM_EnableCounter(timer_defs[tim].instance); diff --git a/src/driver/mcu/stm32/reset.c b/src/driver/mcu/stm32/reset.c index 0753dc8e5..505d9ace8 100644 --- a/src/driver/mcu/stm32/reset.c +++ b/src/driver/mcu/stm32/reset.c @@ -46,11 +46,22 @@ __attribute__((__used__)) void system_check_for_bootloader() { case RESET_BOOTLOADER_MAGIC: { backup_register_write(0); -#ifdef STM32G4 + __disable_irq(); + + SysTick->CTRL = 0; + HAL_RCC_DeInit(); +#ifdef STM32G4 __HAL_SYSCFG_REMAPMEMORY_SYSTEMFLASH(); #endif + for (uint8_t i = 0; i < sizeof(NVIC->ICER) / sizeof(NVIC->ICER[0]); i++) + NVIC->ICER[i] = 0xFFFFFFFF; + for (uint8_t i = 0; i < sizeof(NVIC->ICPR) / sizeof(NVIC->ICPR[0]); i++) + NVIC->ICPR[i] = 0xFFFFFFFF; + + __enable_irq(); + void (*DfuBootJump)(void) = (void (*)(void))(*((uint32_t *)(BOOTLOADER_OFFSET + 4))); __set_MSP(*((uint32_t *)BOOTLOADER_OFFSET)); DfuBootJump(); diff --git a/src/driver/mcu/stm32/rgb_led.c b/src/driver/mcu/stm32/rgb_led.c new file mode 100644 index 000000000..0358c226e --- /dev/null +++ b/src/driver/mcu/stm32/rgb_led.c @@ -0,0 +1,172 @@ +#include "driver/rgb_led.h" + +#include +#include + +#include "core/failloop.h" +#include "core/project.h" +#include "driver/dma.h" +#include "driver/gpio.h" +#include "driver/interrupt.h" +#include "driver/timer.h" + +#if defined(USE_RGB_LED) + +#define TIMER_HZ PWM_CLOCK_FREQ_HZ +#define TIMER_DIV ((PWM_CLOCK_FREQ_HZ / TIMER_HZ) - 1) + +#define RGB_BIT_TIME ((TIMER_HZ / 800000) - 1) +#define RGB_T0H_TIME ((RGB_BIT_TIME / 3) + 1) +#define RGB_T1H_TIME ((RGB_BIT_TIME / 3) * 2 + 1) + +#define RGB_BITS_LED 24 +#define RGB_BUFFER_SIZE (RGB_BITS_LED * RGB_LED_MAX + 40) + +extern resource_tag_t rgb_timer_tag; +extern volatile bool rgb_dma_busy; + +static DMA_RAM uint32_t rgb_timer_buffer[RGB_BUFFER_SIZE]; +static uint32_t rgb_timer_buffer_count = 0; + +void rgb_led_init() { + const gpio_pins_t pin = target.rgb_led; + if (pin == PIN_NONE) { + return; + } + + rgb_timer_tag = target.dma[DMA_DEVICE_RGB].tag; + if (rgb_timer_tag == 0) { + return; + } + + gpio_config_t gpio_init; + gpio_init.mode = GPIO_ALTERNATE; + gpio_init.drive = GPIO_DRIVE_HIGH; + gpio_init.output = GPIO_PUSHPULL; + gpio_init.pull = GPIO_DOWN_PULL; + gpio_pin_init_tag(pin, gpio_init, rgb_timer_tag); + + const timer_channel_t ch = TIMER_TAG_CH(rgb_timer_tag); + const timer_index_t tim = TIMER_TAG_TIM(rgb_timer_tag); + const timer_def_t *def = &timer_defs[tim]; + + rcc_enable(def->rcc); + + LL_TIM_InitTypeDef tim_init; + LL_TIM_StructInit(&tim_init); + tim_init.Autoreload = RGB_BIT_TIME; + tim_init.Prescaler = TIMER_DIV; + tim_init.ClockDivision = 0; + tim_init.CounterMode = LL_TIM_COUNTERMODE_UP; + LL_TIM_Init(def->instance, &tim_init); + LL_TIM_EnableARRPreload(def->instance); + LL_TIM_DisableMasterSlaveMode(def->instance); + + LL_TIM_OC_InitTypeDef tim_oc_init; + LL_TIM_OC_StructInit(&tim_oc_init); + tim_oc_init.CompareValue = 0; + tim_oc_init.OCMode = LL_TIM_OCMODE_PWM1; + tim_oc_init.OCState = LL_TIM_OCSTATE_ENABLE; + tim_oc_init.OCPolarity = LL_TIM_OCPOLARITY_HIGH; + tim_oc_init.OCIdleState = LL_TIM_OCIDLESTATE_LOW; + tim_oc_init.OCNState = LL_TIM_OCSTATE_ENABLE; + tim_oc_init.OCNPolarity = LL_TIM_OCPOLARITY_LOW; + tim_oc_init.OCNIdleState = LL_TIM_OCIDLESTATE_LOW; + LL_TIM_OC_Init(def->instance, timer_channel_val(ch), &tim_oc_init); + LL_TIM_OC_EnablePreload(def->instance, timer_channel_val(ch)); + LL_TIM_EnableAllOutputs(def->instance); + + const dma_stream_def_t *rgb_dma = &dma_stream_defs[target.dma[DMA_DEVICE_RGB].dma]; + dma_enable_rcc(rgb_dma); + LL_DMA_DeInit(rgb_dma->port, rgb_dma->stream_index); + + LL_DMA_InitTypeDef DMA_InitStructure; + LL_DMA_StructInit(&DMA_InitStructure); +#if defined(STM32H7) || defined(STM32G4) + DMA_InitStructure.PeriphRequest = target.dma[DMA_DEVICE_RGB].request; +#else + DMA_InitStructure.Channel = dma_map_channel(target.dma[DMA_DEVICE_RGB].channel); +#endif + DMA_InitStructure.PeriphOrM2MSrcAddress = timer_channel_addr(def->instance, ch); + DMA_InitStructure.MemoryOrM2MDstAddress = (uint32_t)rgb_timer_buffer; + DMA_InitStructure.Direction = LL_DMA_DIRECTION_MEMORY_TO_PERIPH; + DMA_InitStructure.NbData = RGB_BUFFER_SIZE; + DMA_InitStructure.PeriphOrM2MSrcIncMode = LL_DMA_PERIPH_NOINCREMENT; + DMA_InitStructure.MemoryOrM2MDstIncMode = LL_DMA_MEMORY_INCREMENT; + DMA_InitStructure.PeriphOrM2MSrcDataSize = LL_DMA_PDATAALIGN_WORD; + DMA_InitStructure.MemoryOrM2MDstDataSize = LL_DMA_MDATAALIGN_WORD; + DMA_InitStructure.Mode = LL_DMA_MODE_NORMAL; + DMA_InitStructure.Priority = LL_DMA_PRIORITY_VERYHIGH; +#ifndef STM32G4 + DMA_InitStructure.FIFOMode = LL_DMA_FIFOMODE_DISABLE; + DMA_InitStructure.MemBurst = LL_DMA_MBURST_SINGLE; + DMA_InitStructure.PeriphBurst = LL_DMA_PBURST_SINGLE; +#endif + LL_DMA_Init(rgb_dma->port, rgb_dma->stream_index, &DMA_InitStructure); + + LL_DMA_EnableIT_TC(rgb_dma->port, rgb_dma->stream_index); + interrupt_enable(rgb_dma->irq, DMA_PRIORITY); +} + +void rgb_led_set_value(uint32_t value, uint32_t count) { + if (rgb_led_busy()) { + return; + } + + uint32_t offset = 0; + for (uint32_t i = 0; i < 20; i++) { + rgb_timer_buffer[offset++] = 0; + } + for (uint32_t i = 0; i < count; i++) { + // rgb_led_value contains a (32bit) int that contains the RGB values in G R B format already + // Test each bit and assign the T1H or T0H depending on whether it is 1 or 0. + for (int32_t j = RGB_BITS_LED - 1; j >= 0; j--) { + rgb_timer_buffer[offset++] = ((value >> j) & 0x1) ? RGB_T1H_TIME : RGB_T0H_TIME; + } + } + for (uint32_t i = 0; i < 20; i++) { + rgb_timer_buffer[offset++] = 0; + } + rgb_timer_buffer_count = offset; +} + +void rgb_led_send() { + if (rgb_led_busy()) { + return; + } + + rgb_dma_busy = true; + + const timer_channel_t ch = TIMER_TAG_CH(rgb_timer_tag); + const timer_def_t *tim = &timer_defs[TIMER_TAG_TIM(rgb_timer_tag)]; + + const dma_stream_def_t *rgb_dma = &dma_stream_defs[target.dma[DMA_DEVICE_RGB].dma]; + + dma_clear_flag_tc(rgb_dma); + dma_prepare_tx_memory((void *)rgb_timer_buffer, sizeof(rgb_timer_buffer)); + + LL_DMA_SetMemoryAddress(rgb_dma->port, rgb_dma->stream_index, (uint32_t)rgb_timer_buffer); + LL_DMA_SetDataLength(rgb_dma->port, rgb_dma->stream_index, rgb_timer_buffer_count); + + LL_DMA_EnableStream(rgb_dma->port, rgb_dma->stream_index); + timer_enable_dma_request(TIMER_TAG_TIM(rgb_timer_tag), ch, true); + LL_TIM_EnableCounter(tim->instance); +} + +void rgb_dma_isr(const dma_device_t dev) { + const dma_stream_def_t *dma = &dma_stream_defs[target.dma[dev].dma]; + + dma_clear_flag_tc(dma); + LL_DMA_DisableStream(dma->port, dma->stream_index); + + const timer_index_t tim = TIMER_TAG_TIM(rgb_timer_tag); + const timer_channel_t ch = TIMER_TAG_CH(rgb_timer_tag); + const timer_def_t *def = &timer_defs[tim]; + + LL_TIM_DisableCounter(def->instance); + timer_enable_dma_request(tim, ch, false); + + rgb_dma_busy = false; +} + +#endif \ No newline at end of file diff --git a/src/driver/mcu/stm32/spi.c b/src/driver/mcu/stm32/spi.c index 4aaf3f63b..2d195af02 100644 --- a/src/driver/mcu/stm32/spi.c +++ b/src/driver/mcu/stm32/spi.c @@ -42,8 +42,6 @@ const spi_port_def_t spi_port_defs[SPI_PORT_MAX] = { extern FAST_RAM spi_txn_t txn_pool[SPI_TXN_MAX]; -#define PORT spi_port_defs[port] - static uint32_t spi_divider_to_ll(uint32_t divider) { switch (divider) { default: @@ -79,20 +77,20 @@ static uint32_t spi_find_divder(uint32_t clk_hz) { } static void spi_dma_init_rx(spi_ports_t port) { - const dma_stream_def_t *dma = &dma_stream_defs[PORT.dma_rx]; + const dma_stream_def_t *dma = &dma_stream_defs[target.dma[spi_port_defs[port].dma_rx].dma]; LL_DMA_DeInit(dma->port, dma->stream_index); LL_DMA_InitTypeDef DMA_InitStructure; #if defined(STM32H7) || defined(STM32G4) - DMA_InitStructure.PeriphRequest = dma->request; + DMA_InitStructure.PeriphRequest = target.dma[spi_port_defs[port].dma_rx].request; #else - DMA_InitStructure.Channel = dma->channel; + DMA_InitStructure.Channel = dma_map_channel(target.dma[spi_port_defs[port].dma_rx].channel); #endif #if defined(STM32H7) - DMA_InitStructure.PeriphOrM2MSrcAddress = (uint32_t)&PORT.channel->RXDR; + DMA_InitStructure.PeriphOrM2MSrcAddress = (uint32_t)&spi_port_defs[port].channel->RXDR; #else - DMA_InitStructure.PeriphOrM2MSrcAddress = LL_SPI_DMA_GetRegAddr(PORT.channel); + DMA_InitStructure.PeriphOrM2MSrcAddress = LL_SPI_DMA_GetRegAddr(spi_port_defs[port].channel); #endif DMA_InitStructure.MemoryOrM2MDstAddress = 0; DMA_InitStructure.Direction = LL_DMA_DIRECTION_PERIPH_TO_MEMORY; @@ -109,23 +107,24 @@ static void spi_dma_init_rx(spi_ports_t port) { DMA_InitStructure.PeriphBurst = LL_DMA_PBURST_SINGLE; #endif LL_DMA_Init(dma->port, dma->stream_index, &DMA_InitStructure); + LL_DMA_DisableStream(dma->port, dma->stream_index); } static void spi_dma_init_tx(spi_ports_t port) { - const dma_stream_def_t *dma = &dma_stream_defs[PORT.dma_tx]; + const dma_stream_def_t *dma = &dma_stream_defs[target.dma[spi_port_defs[port].dma_tx].dma]; LL_DMA_DeInit(dma->port, dma->stream_index); LL_DMA_InitTypeDef DMA_InitStructure; #if defined(STM32H7) || defined(STM32G4) - DMA_InitStructure.PeriphRequest = dma->request; + DMA_InitStructure.PeriphRequest = target.dma[spi_port_defs[port].dma_tx].request; #else - DMA_InitStructure.Channel = dma->channel; + DMA_InitStructure.Channel = dma_map_channel(target.dma[spi_port_defs[port].dma_tx].channel); #endif #if defined(STM32H7) - DMA_InitStructure.PeriphOrM2MSrcAddress = (uint32_t)&PORT.channel->TXDR; + DMA_InitStructure.PeriphOrM2MSrcAddress = (uint32_t)&spi_port_defs[port].channel->TXDR; #else - DMA_InitStructure.PeriphOrM2MSrcAddress = LL_SPI_DMA_GetRegAddr(PORT.channel); + DMA_InitStructure.PeriphOrM2MSrcAddress = LL_SPI_DMA_GetRegAddr(spi_port_defs[port].channel); #endif DMA_InitStructure.MemoryOrM2MDstAddress = 0; DMA_InitStructure.Direction = LL_DMA_DIRECTION_MEMORY_TO_PERIPH; @@ -142,6 +141,7 @@ static void spi_dma_init_tx(spi_ports_t port) { DMA_InitStructure.PeriphBurst = LL_DMA_PBURST_SINGLE; #endif LL_DMA_Init(dma->port, dma->stream_index, &DMA_InitStructure); + LL_DMA_DisableStream(dma->port, dma->stream_index); } void spi_reconfigure(spi_bus_device_t *bus) { @@ -180,12 +180,12 @@ void spi_reconfigure(spi_bus_device_t *bus) { void spi_dma_transfer_begin(spi_ports_t port, uint8_t *buffer, uint32_t length) { #if !defined(STM32H7) // dummy read - while (LL_SPI_IsActiveFlag_RXNE(PORT.channel)) - LL_SPI_ReceiveData8(PORT.channel); + while (LL_SPI_IsActiveFlag_RXNE(spi_port_defs[port].channel)) + LL_SPI_ReceiveData8(spi_port_defs[port].channel); #endif - const dma_stream_def_t *dma_tx = &dma_stream_defs[PORT.dma_tx]; - const dma_stream_def_t *dma_rx = &dma_stream_defs[PORT.dma_rx]; + const dma_stream_def_t *dma_rx = &dma_stream_defs[target.dma[spi_port_defs[port].dma_rx].dma]; + const dma_stream_def_t *dma_tx = &dma_stream_defs[target.dma[spi_port_defs[port].dma_tx].dma]; dma_clear_flag_tc(dma_rx); dma_clear_flag_tc(dma_tx); @@ -208,23 +208,27 @@ void spi_dma_transfer_begin(spi_ports_t port, uint8_t *buffer, uint32_t length) LL_DMA_EnableStream(dma_rx->port, dma_rx->stream_index); LL_DMA_EnableStream(dma_tx->port, dma_tx->stream_index); - LL_SPI_EnableDMAReq_TX(PORT.channel); - LL_SPI_EnableDMAReq_RX(PORT.channel); + LL_SPI_EnableDMAReq_RX(spi_port_defs[port].channel); + LL_SPI_EnableDMAReq_TX(spi_port_defs[port].channel); #ifdef STM32H7 - LL_SPI_SetTransferSize(PORT.channel, length); + LL_SPI_SetTransferSize(spi_port_defs[port].channel, length); #endif - LL_SPI_Enable(PORT.channel); + LL_SPI_Enable(spi_port_defs[port].channel); #ifdef STM32H7 - LL_SPI_StartMasterTransfer(PORT.channel); + LL_SPI_StartMasterTransfer(spi_port_defs[port].channel); #endif } void spi_device_init(spi_ports_t port) { const spi_port_def_t *def = &spi_port_defs[port]; rcc_enable(def->rcc); - dma_enable_rcc(def->dma_rx); - dma_enable_rcc(def->dma_tx); + + const dma_stream_def_t *dma_rx = &dma_stream_defs[target.dma[spi_port_defs[port].dma_rx].dma]; + const dma_stream_def_t *dma_tx = &dma_stream_defs[target.dma[spi_port_defs[port].dma_tx].dma]; + + dma_enable_rcc(dma_rx); + dma_enable_rcc(dma_tx); LL_SPI_DeInit(def->channel); @@ -258,9 +262,11 @@ void spi_device_init(spi_ports_t port) { spi_dma_init_rx(port); spi_dma_init_tx(port); - const dma_stream_def_t *dma_rx = &dma_stream_defs[def->dma_rx]; interrupt_enable(dma_rx->irq, DMA_PRIORITY); + dma_clear_flag_tc(dma_rx); + dma_clear_flag_tc(dma_tx); + LL_DMA_EnableIT_TC(dma_rx->port, dma_rx->stream_index); LL_DMA_EnableIT_TE(dma_rx->port, dma_rx->stream_index); } @@ -283,11 +289,11 @@ void spi_seg_submit_wait_ex(spi_bus_device_t *bus, const spi_txn_segment_t *segs for (uint32_t i = 0; i < count; i++) { transfer_size += segs[i].size; } - LL_SPI_SetTransferSize(PORT.channel, transfer_size); - LL_SPI_Enable(PORT.channel); - LL_SPI_StartMasterTransfer(PORT.channel); + LL_SPI_SetTransferSize(spi_port_defs[port].channel, transfer_size); + LL_SPI_Enable(spi_port_defs[port].channel); + LL_SPI_StartMasterTransfer(spi_port_defs[port].channel); #else - LL_SPI_Enable(PORT.channel); + LL_SPI_Enable(spi_port_defs[port].channel); #endif for (uint32_t i = 0; i < count; i++) { @@ -306,21 +312,21 @@ void spi_seg_submit_wait_ex(spi_bus_device_t *bus, const spi_txn_segment_t *segs for (uint32_t j = 0; j < size; j++) { #if defined(STM32H7) - while (!LL_SPI_IsActiveFlag_TXP(PORT.channel)) + while (!LL_SPI_IsActiveFlag_TXP(spi_port_defs[port].channel)) ; #else - while (!LL_SPI_IsActiveFlag_TXE(PORT.channel)) + while (!LL_SPI_IsActiveFlag_TXE(spi_port_defs[port].channel)) ; #endif - LL_SPI_TransmitData8(PORT.channel, tx_data ? tx_data[j] : 0xFF); + LL_SPI_TransmitData8(spi_port_defs[port].channel, tx_data ? tx_data[j] : 0xFF); #if defined(STM32H7) - while (!LL_SPI_IsActiveFlag_RXP(PORT.channel)) + while (!LL_SPI_IsActiveFlag_RXP(spi_port_defs[port].channel)) ; #else - while (!LL_SPI_IsActiveFlag_RXNE(PORT.channel)) + while (!LL_SPI_IsActiveFlag_RXNE(spi_port_defs[port].channel)) ; #endif - const uint8_t ret = LL_SPI_ReceiveData8(PORT.channel); + const uint8_t ret = LL_SPI_ReceiveData8(spi_port_defs[port].channel); if (rx_data != NULL) { rx_data[j] = ret; } @@ -328,13 +334,13 @@ void spi_seg_submit_wait_ex(spi_bus_device_t *bus, const spi_txn_segment_t *segs } #if defined(STM32H7) - while (!LL_SPI_IsActiveFlag_EOT(PORT.channel)) + while (!LL_SPI_IsActiveFlag_EOT(spi_port_defs[port].channel)) ; - LL_SPI_ClearFlag_TXTF(PORT.channel); - LL_SPI_Disable(PORT.channel); + LL_SPI_ClearFlag_TXTF(spi_port_defs[port].channel); + LL_SPI_Disable(spi_port_defs[port].channel); #else - LL_SPI_Disable(PORT.channel); + LL_SPI_Disable(spi_port_defs[port].channel); #endif spi_csn_disable(bus); @@ -342,8 +348,8 @@ void spi_seg_submit_wait_ex(spi_bus_device_t *bus, const spi_txn_segment_t *segs } static void handle_dma_rx_isr(spi_ports_t port) { - const dma_stream_def_t *dma_rx = &dma_stream_defs[PORT.dma_rx]; - const dma_stream_def_t *dma_tx = &dma_stream_defs[PORT.dma_tx]; + const dma_stream_def_t *dma_tx = &dma_stream_defs[target.dma[spi_port_defs[port].dma_tx].dma]; + const dma_stream_def_t *dma_rx = &dma_stream_defs[target.dma[spi_port_defs[port].dma_rx].dma]; if (!dma_is_flag_active_tc(dma_rx)) { return; @@ -352,23 +358,23 @@ static void handle_dma_rx_isr(spi_ports_t port) { dma_clear_flag_tc(dma_rx); dma_clear_flag_tc(dma_tx); - LL_SPI_DisableDMAReq_TX(PORT.channel); - LL_SPI_DisableDMAReq_RX(PORT.channel); + LL_SPI_DisableDMAReq_TX(spi_port_defs[port].channel); + LL_SPI_DisableDMAReq_RX(spi_port_defs[port].channel); LL_DMA_DisableStream(dma_rx->port, dma_rx->stream_index); LL_DMA_DisableStream(dma_tx->port, dma_tx->stream_index); #if defined(STM32H7) // now we can disable the peripheral - LL_SPI_ClearFlag_TXTF(PORT.channel); + LL_SPI_ClearFlag_TXTF(spi_port_defs[port].channel); #endif - LL_SPI_Disable(PORT.channel); + LL_SPI_Disable(spi_port_defs[port].channel); spi_txn_finish(port); } -void spi_dma_isr(dma_device_t dev) { +void spi_dma_isr(const dma_device_t dev) { switch (dev) { case DMA_DEVICE_SPI1_RX: handle_dma_rx_isr(SPI_PORT1); diff --git a/src/driver/mcu/stm32/system.h b/src/driver/mcu/stm32/system.h index 7aa7da0d7..0f27cee97 100644 --- a/src/driver/mcu/stm32/system.h +++ b/src/driver/mcu/stm32/system.h @@ -117,16 +117,9 @@ typedef TIM_TypeDef timer_dev_t; typedef USART_TypeDef usart_dev_t; typedef struct { - uint32_t device; - DMA_TypeDef *port; uint8_t port_index; - uint32_t channel; - uint8_t channel_index; - - uint32_t request; - #ifdef STM32G4 DMA_Channel_TypeDef *stream; #else diff --git a/src/driver/mcu/stm32/timer.c b/src/driver/mcu/stm32/timer.c index 55596bbb1..80a87445c 100644 --- a/src/driver/mcu/stm32/timer.c +++ b/src/driver/mcu/stm32/timer.c @@ -81,6 +81,25 @@ uint32_t timer_channel_val(timer_channel_t chan) { } } +uint32_t timer_channel_addr(timer_dev_t *timer, timer_channel_t chan) { + switch (chan) { + case TIMER_CH1: + case TIMER_CH1N: + return (uint32_t)(&timer->CCR1); + case TIMER_CH2: + case TIMER_CH2N: + return (uint32_t)(&timer->CCR2); + case TIMER_CH3: + case TIMER_CH3N: + return (uint32_t)(&timer->CCR3); + case TIMER_CH4: + case TIMER_CH4N: + return (uint32_t)(&timer->CCR4); + default: + return 0; + } +} + void timer_enable_dma_request(timer_index_t tim, timer_channel_t chan, bool state) { const timer_def_t *def = &timer_defs[tim]; diff --git a/src/driver/motor_dshot.c b/src/driver/motor_dshot.c index 81a72a74c..274b14791 100644 --- a/src/driver/motor_dshot.c +++ b/src/driver/motor_dshot.c @@ -22,18 +22,9 @@ volatile uint32_t dshot_phase = 0; uint8_t dshot_gpio_port_count = 0; dshot_gpio_port_t dshot_gpio_ports[DSHOT_MAX_PORT_COUNT] = { - { - .timer_channel = TIMER_CH1, - .dma_device = DMA_DEVICE_TIM1_CH1, - }, - { - .timer_channel = TIMER_CH3, - .dma_device = DMA_DEVICE_TIM1_CH3, - }, - { - .timer_channel = TIMER_CH4, - .dma_device = DMA_DEVICE_TIM1_CH4, - }, + {.dma_device = DMA_DEVICE_DSHOT_CH1}, + {.dma_device = DMA_DEVICE_DSHOT_CH2}, + {.dma_device = DMA_DEVICE_DSHOT_CH3}, }; volatile DMA_RAM uint16_t dshot_input_buffer[DSHOT_MAX_PORT_COUNT][GCR_DMA_BUFFER_SIZE]; @@ -46,7 +37,7 @@ static motor_direction_t motor_dir = MOTOR_FORWARD; static bool dir_change_done = true; const dshot_gpio_port_t *dshot_gpio_for_device(const dma_device_t dev) { - return &dshot_gpio_ports[dev - DMA_DEVICE_TIM1_CH1]; + return &dshot_gpio_ports[dev - DMA_DEVICE_DSHOT_CH1]; } void dshot_gpio_init_output(gpio_pins_t pin) { @@ -219,7 +210,25 @@ void motor_dshot_init() { for (uint32_t i = 0; i < MOTOR_PIN_MAX; i++) { dshot_init_motor_pin(i); } - dshot_init_timer(); + + for (uint32_t j = 0; j < dshot_gpio_port_count; j++) { + dshot_gpio_port_t *port = &dshot_gpio_ports[j]; + + port->timer_tag = target.dma[port->dma_device].tag; + if (port->timer_tag == 0) { + failloop(FAILLOOP_DMA); + } + + dshot_init_gpio_port(port); + + for (uint8_t i = 0; i < DSHOT_DMA_SYMBOLS; i++) { + dshot_output_buffer[j][i * 3 + 0] = port->set_mask; // start bit + dshot_output_buffer[j][i * 3 + 1] = 0; // actual bit, set below + dshot_output_buffer[j][i * 3 + 2] = port->reset_mask; // return line to low + } + } + + motor_dir = MOTOR_FORWARD; } void motor_dshot_write(float *values) { diff --git a/src/driver/motor_dshot.h b/src/driver/motor_dshot.h index 78a0a129f..8a1cea61c 100644 --- a/src/driver/motor_dshot.h +++ b/src/driver/motor_dshot.h @@ -1,6 +1,7 @@ #pragma once #include "core/profile.h" +#include "core/project.h" #include "driver/dma.h" #include "driver/gpio.h" #include "driver/motor.h" @@ -19,7 +20,8 @@ typedef struct { uint32_t set_mask; uint32_t reset_mask; - timer_channel_t timer_channel; + resource_tag_t timer_tag; + dma_device_t dma_device; } dshot_gpio_port_t; @@ -60,7 +62,7 @@ extern volatile DMA_RAM uint32_t dshot_output_buffer[DSHOT_MAX_PORT_COUNT][DSHOT const dshot_gpio_port_t *dshot_gpio_for_device(const dma_device_t dev); -void dshot_init_timer(); +void dshot_init_gpio_port(dshot_gpio_port_t *port); void dshot_gpio_init_output(gpio_pins_t pin); void dshot_dma_setup_output(uint32_t index); diff --git a/src/driver/resource.c b/src/driver/resource.c new file mode 100644 index 000000000..e6445fcbc --- /dev/null +++ b/src/driver/resource.c @@ -0,0 +1,121 @@ +#include "resource.h" + +#include +#include + +#include "util/cbor_helper.h" +#include "util/util.h" + +#include "timer.h" + +#define check_str(str) \ + ({ \ + bool match = buf_equal_string((uint8_t *)end, sizeof(str) - 1, str); \ + if (match) \ + end += sizeof(str) - 1; \ + match; \ + }) + +cbor_result_t cbor_decode_resource_tag_t(cbor_value_t *dec, resource_tag_t *t) { + const uint8_t *name; + uint32_t name_len; + + CBOR_CHECK_ERROR(cbor_result_t res = cbor_decode_tstr(dec, &name, &name_len)); + + char *end = (char *)name; + if (check_str("SPI")) { + const int32_t index = strtol(end, &end, 10); + if (check_str("_MOSI")) { + *t = SPI_TAG(index, RES_SPI_MOSI); + } else if (check_str("_MISO")) { + *t = SPI_TAG(index, RES_SPI_MISO); + } else if (check_str("_SCK")) { + *t = SPI_TAG(index, RES_SPI_SCK); + } else { + return CBOR_ERR_INVALID_TYPE; + } + } else if (check_str("TIMER")) { + int32_t index = TIMER_INVALID; + switch (*end) { +#define TIMER(_num) \ + case '0' + _num: \ + index = TIMER##_num; \ + end++; \ + break; + TIMERS +#undef TIMER + } + if (!check_str("_CH")) { + return CBOR_ERR_INVALID_TYPE; + } + switch (*end) { + case '1': + *t = TIMER_TAG(index, TIMER_CH1); + break; + case '2': + *t = TIMER_TAG(index, TIMER_CH2); + break; + case '3': + *t = TIMER_TAG(index, TIMER_CH3); + break; + case '4': + *t = TIMER_TAG(index, TIMER_CH4); + break; + default: + return CBOR_ERR_INVALID_TYPE; + }; + } else { + return CBOR_ERR_INVALID_TYPE; + } + + return res; +} + +cbor_result_t cbor_encode_resource_tag_t(cbor_value_t *enc, const resource_tag_t *d) { + char tag[64]; + int len = 0; + + switch (RESOURCE_TAG_TYPE(*d)) { + case RESOURCE_SPI: + switch (SPI_TAG_PIN(*d)) { + case RES_SPI_MOSI: + len = snprintf(tag, 64, "SPI%d_MOSI", SPI_TAG_PORT(*d)); + break; + case RES_SPI_MISO: + len = snprintf(tag, 64, "SPI%d_MISO", SPI_TAG_PORT(*d)); + break; + case RES_SPI_SCK: + len = snprintf(tag, 64, "SPI%d_SCK", SPI_TAG_PORT(*d)); + break; + default: + break; + } + break; + case RESOURCE_TIM: + switch (TIMER_TAG_CH(*d)) { + case TIMER_CH1: + case TIMER_CH1N: + len = snprintf(tag, 64, "TIMER%d_CH1", TIMER_TAG_TIM(*d)); + break; + case TIMER_CH2: + case TIMER_CH2N: + len = snprintf(tag, 64, "TIMER%d_CH2", TIMER_TAG_TIM(*d)); + break; + case TIMER_CH3: + case TIMER_CH3N: + len = snprintf(tag, 64, "TIMER%d_CH3", TIMER_TAG_TIM(*d)); + break; + case TIMER_CH4: + case TIMER_CH4N: + len = snprintf(tag, 64, "TIMER%d_CH4", TIMER_TAG_TIM(*d)); + break; + default: + break; + } + break; + default: + break; + } + + return cbor_encode_tstr(enc, (uint8_t *)tag, len); +} \ No newline at end of file diff --git a/src/driver/resource.h b/src/driver/resource.h index b24594600..e982db0fe 100644 --- a/src/driver/resource.h +++ b/src/driver/resource.h @@ -2,6 +2,8 @@ #include +#include + typedef enum { RESOURCE_INVALID, RESOURCE_TIM, @@ -29,17 +31,20 @@ typedef uint32_t resource_tag_t; #define RESOURCE_TAG_TYPE(tag) (resource_type_t)(((tag) >> 24) & 0xFF) #define TIMER_TAG(tim, ch) RESOURCE_TAG(RESOURCE_TIM, (uint32_t)((tim) << 8) | (ch)) -#define TIMER_TAG_TIM(tag) (uint8_t)(((tag) >> 8) & 0xFF) -#define TIMER_TAG_CH(tag) (uint8_t)((tag)&0xFF) +#define TIMER_TAG_TIM(tag) (timer_index_t)(((tag) >> 8) & 0xFF) +#define TIMER_TAG_CH(tag) (timer_channel_t)((tag) & 0xFF) #define SPI_TAG(port, pin) RESOURCE_TAG(RESOURCE_SPI, (uint32_t)((port) << 8) | (pin)) #define SPI_TAG_PORT(tag) (uint8_t)(((tag) >> 8) & 0xFF) -#define SPI_TAG_PIN(tag) (uint8_t)((tag)&0xFF) +#define SPI_TAG_PIN(tag) (uint8_t)((tag) & 0xFF) #define SERIAL_TAG(serial, pin) RESOURCE_TAG(RESOURCE_SERIAL, (uint32_t)((serial) << 8) | (pin)) #define SERIAL_TAG_PORT(tag) (uint8_t)(((tag) >> 8) & 0xFF) -#define SERIAL_TAG_PIN(tag) (uint8_t)((tag)&0xFF) +#define SERIAL_TAG_PIN(tag) (uint8_t)((tag) & 0xFF) #define ADC_TAG(adc, ch) RESOURCE_TAG(RESOURCE_ADC, (uint32_t)((adc) << 8) | (ch)) -#define ADC_TAG_DEV(tag) (uint8_t)(((tag) >> 8) & 0xFF) -#define ADC_TAG_CH(tag) (uint8_t)((tag)&0xFF) \ No newline at end of file +#define ADC_TAG_DEV(tag) (adc_devices_t)(((tag) >> 8) & 0xFF) +#define ADC_TAG_CH(tag) (uint8_t)((tag) & 0xFF) + +cbor_result_t cbor_decode_resource_tag_t(cbor_value_t *dec, resource_tag_t *t); +cbor_result_t cbor_encode_resource_tag_t(cbor_value_t *enc, const resource_tag_t *d); diff --git a/src/driver/rgb_led.c b/src/driver/rgb_led.c index a7fa6027d..167bf0cf9 100644 --- a/src/driver/rgb_led.c +++ b/src/driver/rgb_led.c @@ -1,347 +1,12 @@ -//**********************************************WARNING - THIS FILE NOT YET PORTED FROM F0 SILVERWARE*********************************** - #include "driver/rgb_led.h" #include "core/project.h" -#include "driver/spi.h" -#include "driver/time.h" -#include "util/util.h" - -#if (RGB_LED_NUMBER > 0) -void rgb_send(int data); - -#ifdef RGB_LED_DMA - -#define RGB_BIT_TIME ((SYS_CLOCK_FREQ_HZ / 1000 / 800) - 1) -#define RGB_T0H_TIME (RGB_BIT_TIME * 0.30 + 0.05) -#define RGB_T1H_TIME (RGB_BIT_TIME * 0.60 + 0.05) - -extern int rgb_led_value[]; - -volatile int rgb_dma_phase = 0; // 3:rgb data ready - // 2:rgb dma buffer ready - -// wait to be cascaded after dshot, or fired at next frame if no dshot activity -// 1:rgb dma busy -// 0:idle - -const int offset = RGB_PIN > LL_GPIO_PIN_7; - -volatile uint32_t rgb_data_portA[RGB_LED_NUMBER * 24 / 4] = {0}; // DMA buffer: reset output when bit data=0 at TOH timing -volatile uint16_t rgb_portX[1] = {RGB_PIN}; // sum of all rgb pins at port -volatile uint32_t RGB_DATA16[16]; // 4-bit look-up table for dma buffer making - -void rgb_init() { - if ((RGB_PIN == LL_GPIO_PIN_13 || RGB_PIN == LL_GPIO_PIN_14) && RGB_PORT == GPIOA) { - // programming port used - // wait until 2 seconds from powerup passed - while (time_micros() < 2e6) - ; - } - - gpio_config_t GPIO_InitStructure; - - GPIO_InitStructure.mode = GPIO_OUTPUT; - GPIO_InitStructure.output = GPIO_PUSHPULL; - GPIO_InitStructure.pull = GPIO_NO_PULL; - GPIO_InitStructure.drive =GPIO_DRIVE_HIGH; - - GPIO_InitStructure.Pin = RGB_PIN; - LL_GPIO_Init(RGB_PORT, &GPIO_InitStructure); - -#ifndef USE_DSHOT_DMA_DRIVER - // RGB timer/DMA init - // TIM1_UP DMA_CH5: set all output to HIGH at TIM1 update - // TIM1_CH1 DMA_CH2: reset output if data=0 at T0H timing - // TIM1_CH4 DMA_CH4: reset all output at T1H timing - - TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; - TIM_OCInitTypeDef TIM_OCInitStructure; - - TIM_TimeBaseStructInit(&TIM_TimeBaseStructure); - TIM_OCStructInit(&TIM_OCInitStructure); - // TIM1 Periph clock enable - RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1, ENABLE); - - /* Time base configuration */ - TIM_TimeBaseStructure.TIM_Period = RGB_BIT_TIME; - TIM_TimeBaseStructure.TIM_Prescaler = 0; - TIM_TimeBaseStructure.TIM_ClockDivision = 0; - TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; - TIM_TimeBaseInit(TIM1, &TIM_TimeBaseStructure); - TIM_ARRPreloadConfig(TIM1, DISABLE); - - /* Timing Mode configuration: Channel 1 */ - TIM_OCInitStructure.OCMode = LL_TIM_OCMODE_FROZEN; - TIM_OCInitStructure.OCState = LL_TIM_OCSTATE_DISABLE; - TIM_OCInitStructure.CompareValue = RGB_T0H_TIME; - LL_TIM_OC_Init(TIM1, LL_TIM_CHANNEL_CH1, &TIM_OCInitStructure); - LL_TIM_OC_DisablePreload(TIM1, LL_TIM_CHANNEL_CH1); - - /* Timing Mode configuration: Channel 4 */ - TIM_OCInitStructure.OCMode = LL_TIM_OCMODE_FROZEN; - TIM_OCInitStructure.OCState = LL_TIM_OCSTATE_DISABLE; - TIM_OCInitStructure.CompareValue = RGB_T1H_TIME; - LL_TIM_OC_Init(TIM1, LL_TIM_CHANNEL_CH4, &TIM_OCInitStructure); - LL_TIM_OC_DisablePreload(TIM1, LL_TIM_CHANNEL_CH4); - - LL_LL_DMA_InitTypeDef DMA_InitStructure; - - LL_DMA_StructInit(&DMA_InitStructure); - RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1, ENABLE); - - /* DMA1 Channe5 configuration ----------------------------------------------*/ - LL_DMA_DeInit(DMA1_Channel3); - DMA_InitStructure.PeriphOrM2MSrcAddress = (uint32_t)&RGB_PORT->BSRR; - DMA_InitStructure.DMA_MemoryBaseAddr = (uint32_t)rgb_portX; - DMA_InitStructure.Direction = DMA_DIR_PeripheralDST; - DMA_InitStructure.NbData = RGB_LED_NUMBER * 24; - DMA_InitStructure.PeriphOrM2MSrcIncMode = LL_DMA_PERIPH_NOINCREMENT; - DMA_InitStructure.MemoryOrM2MDstIncMode = LL_DMA_MEMORY_NOINCREMENT; - DMA_InitStructure.PeriphOrM2MSrcDataSize = LL_DMA_PDATAALIGN_HALFWORD; - DMA_InitStructure.MemoryOrM2MDstDataSize = LL_DMA_MDATAALIGN_HALFWORD; - DMA_InitStructure.Mode = LL_DMA_MODE_NORMAL; - DMA_InitStructure.Priority = LL_DMA_PRIORITY_HIGH; - DMA_InitStructure.DMA_M2M = DMA_M2M_Disable; - DMA_Init(DMA1_Channel5, &DMA_InitStructure); - - /* DMA1 Channel2 configuration ----------------------------------------------*/ - LL_DMA_DeInit(DMA1_Channel2); - DMA_InitStructure.PeriphOrM2MSrcAddress = (uint32_t)&RGB_PORT->BRR + offset; - DMA_InitStructure.DMA_MemoryBaseAddr = (uint32_t)rgb_data_portA; - DMA_InitStructure.Direction = DMA_DIR_PeripheralDST; - DMA_InitStructure.NbData = RGB_LED_NUMBER * 24; - DMA_InitStructure.PeriphOrM2MSrcIncMode = LL_DMA_PERIPH_NOINCREMENT; - DMA_InitStructure.MemoryOrM2MDstIncMode = LL_DMA_MEMORY_INCREMENT; - DMA_InitStructure.PeriphOrM2MSrcDataSize = LL_DMA_PDATAALIGN_BYTE; - DMA_InitStructure.MemoryOrM2MDstDataSize = LL_DMA_MDATAALIGN_BYTE; - DMA_InitStructure.Mode = LL_DMA_MODE_NORMAL; - DMA_InitStructure.Priority = LL_DMA_PRIORITY_HIGH; - DMA_InitStructure.DMA_M2M = DMA_M2M_Disable; - DMA_Init(DMA1_Channel2, &DMA_InitStructure); - - /* DMA1 Channel4 configuration ----------------------------------------------*/ - LL_DMA_DeInit(DMA1_Channel4); - DMA_InitStructure.PeriphOrM2MSrcAddress = (uint32_t)&RGB_PORT->BRR; - DMA_InitStructure.DMA_MemoryBaseAddr = (uint32_t)rgb_portX; - DMA_InitStructure.Direction = DMA_DIR_PeripheralDST; - DMA_InitStructure.NbData = RGB_LED_NUMBER * 24; - DMA_InitStructure.PeriphOrM2MSrcIncMode = LL_DMA_PERIPH_NOINCREMENT; - DMA_InitStructure.MemoryOrM2MDstIncMode = LL_DMA_MEMORY_NOINCREMENT; - DMA_InitStructure.PeriphOrM2MSrcDataSize = LL_DMA_PDATAALIGN_HALFWORD; - DMA_InitStructure.MemoryOrM2MDstDataSize = LL_DMA_MDATAALIGN_HALFWORD; - DMA_InitStructure.Mode = LL_DMA_MODE_NORMAL; - DMA_InitStructure.Priority = LL_DMA_PRIORITY_HIGH; - DMA_InitStructure.DMA_M2M = DMA_M2M_Disable; - DMA_Init(DMA1_Channel4, &DMA_InitStructure); - - TIM_DMACmd(TIM1, TIM_DMA_Update | TIM_DMA_CC4 | TIM_DMA_CC1, ENABLE); - - NVIC_InitTypeDef NVIC_InitStructure; - /* configure DMA1 Channel4 interrupt */ - NVIC_InitStructure.NVIC_IRQChannel = DMA1_Channel4_5_IRQn; - NVIC_InitStructure.NVIC_IRQChannelPriority = (uint8_t)LL_DMA_PRIORITY_HIGH; - NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; - NVIC_Init(&NVIC_InitStructure); - /* enable DMA1 Channel4 transfer complete interrupt */ - DMA_ITConfig(DMA1_Channel4, DMA_IT_TC, ENABLE); -#endif - - for (int i = 0; i < RGB_LED_NUMBER; i++) { - rgb_led_value[i] = 0; - } - if (!rgb_dma_phase) - rgb_dma_phase = 3; - - int pin = rgb_portX[0]; - - if (offset) { - pin >>= 8; - } - - for (int i = 0; i < 16; i++) { - RGB_DATA16[i] = (i & 0x01) ? 0 : pin; - RGB_DATA16[i] <<= 8; - RGB_DATA16[i] |= (i & 0x02) ? 0 : pin; - RGB_DATA16[i] <<= 8; - RGB_DATA16[i] |= (i & 0x04) ? 0 : pin; - RGB_DATA16[i] <<= 8; - RGB_DATA16[i] |= (i & 0x08) ? 0 : pin; - } -} - -void rgb_dma_buffer_making() { - // generate rgb dma packet - int j = 0; - for (int n = 0; n < RGB_LED_NUMBER; n++) { - rgb_data_portA[j++] = RGB_DATA16[(rgb_led_value[n] >> 20) & 0x0f]; - rgb_data_portA[j++] = RGB_DATA16[(rgb_led_value[n] >> 16) & 0x0f]; - rgb_data_portA[j++] = RGB_DATA16[(rgb_led_value[n] >> 12) & 0x0f]; - rgb_data_portA[j++] = RGB_DATA16[(rgb_led_value[n] >> 8) & 0x0f]; - rgb_data_portA[j++] = RGB_DATA16[(rgb_led_value[n] >> 4) & 0x0f]; - rgb_data_portA[j++] = RGB_DATA16[(rgb_led_value[n]) & 0x0f]; - } -} - -void rgb_dma_trigger() { - TIM1->ARR = RGB_BIT_TIME; - TIM1->CCR1 = RGB_T0H_TIME; - TIM1->CCR4 = RGB_T1H_TIME; - - DMA1_Channel5->CPAR = (uint32_t)&RGB_PORT->BSRR; - DMA1_Channel5->CMAR = (uint32_t)rgb_portX; - DMA1_Channel2->CPAR = (uint32_t)&RGB_PORT->BRR + offset; - DMA1_Channel2->CMAR = (uint32_t)rgb_data_portA; - DMA1_Channel4->CPAR = (uint32_t)&RGB_PORT->BRR; - DMA1_Channel4->CMAR = (uint32_t)rgb_portX; - - DMA1_Channel2->CCR &= ~(DMA_CCR_MSIZE_0 | DMA_CCR_MSIZE_1 | DMA_CCR_PSIZE_0 | DMA_CCR_PSIZE_1); // switch from halfword to byte - - DMA_ClearFlag(DMA1_FLAG_GL2 | DMA1_FLAG_GL4 | DMA1_FLAG_GL5); - - DMA1_Channel5->CNDTR = RGB_LED_NUMBER * 24; - DMA1_Channel2->CNDTR = RGB_LED_NUMBER * 24; - DMA1_Channel4->CNDTR = RGB_LED_NUMBER * 24; - - TIM1->SR = 0; - - DMA_Cmd(DMA1_Channel2, ENABLE); - DMA_Cmd(DMA1_Channel4, ENABLE); - DMA_Cmd(DMA1_Channel5, ENABLE); - - TIM_DMACmd(TIM1, TIM_DMA_Update | TIM_DMA_CC4 | TIM_DMA_CC1, ENABLE); - - TIM_SetCounter(TIM1, RGB_BIT_TIME); - TIM_Cmd(TIM1, ENABLE); -} - -void rgb_dma_start() { - if (rgb_dma_phase <= 1) - return; - - if (rgb_dma_phase == 3) { - rgb_dma_buffer_making(); - rgb_dma_phase = 2; - return; - } - -#ifdef USE_DSHOT_DMA_DRIVER - extern int dshot_phase; - if (dshot_phase) - return; -#endif - - rgb_dma_phase = 1; - rgb_dma_trigger(); -} - -void rgb_send(int data) { - if (!rgb_dma_phase) - rgb_dma_phase = 3; -} - -// if dshot dma is used the routine is in that file -#if !defined(USE_DSHOT_DMA_DRIVER) && defined(RGB_LED_DMA) && (RGB_LED_NUMBER > 0) - -void DMA1_Channel4_5_IRQHandler() { - DMA_Cmd(DMA1_Channel5, DISABLE); - DMA_Cmd(DMA1_Channel2, DISABLE); - DMA_Cmd(DMA1_Channel4, DISABLE); - - TIM_DMACmd(TIM1, TIM_DMA_Update | TIM_DMA_CC4 | TIM_DMA_CC1, DISABLE); - DMA_ClearITPendingBit(DMA1_IT_TC4); - TIM_Cmd(TIM1, DISABLE); - - rgb_dma_phase = 0; -} -#endif - -#else - -// sets all leds to a brightness -void rgb_init() { - // spi port inits - - if ((RGB_PIN == LL_GPIO_PIN_13 || RGB_PIN == LL_GPIO_PIN_14) && RGB_PORT == GPIOA) { - // programming port used - - // wait until 2 seconds from powerup passed - while (time_micros() < 2e6) - ; - } - - gpio_config_t GPIO_InitStructure; - - GPIO_InitStructure.mode = GPIO_OUTPUT; - GPIO_InitStructure.output = GPIO_PUSHPULL; - GPIO_InitStructure.pull = GPIO_UP_PULL; - GPIO_InitStructure.drive =GPIO_DRIVE_HIGH; - - GPIO_InitStructure.Pin = RGB_PIN; - LL_GPIO_Init(RGB_PORT, &GPIO_InitStructure); - - for (int i = 0; i < RGB_LED_NUMBER; i++) { - rgb_send(0); - } -} - -#define RGBHIGH gpioset(RGB_PORT, RGB_PIN) -#define RGBLOW gpioreset(RGB_PORT, RGB_PIN); - -#pragma push - -#pragma Otime -#pragma O2 - -void delay1a() { - uint8_t count = 2; - while (count--) - ; -} - -void delay1b() { - uint8_t count = 2; - while (count--) - ; -} - -void delay2a() { - uint8_t count = 1; - while (count--) - ; -} - -void delay2b() { - uint8_t count = 3; - while (count--) - ; -} - -void rgb_send(int data) { - for (int i = 23; i >= 0; i--) { - if ((data >> i) & 1) { - RGBHIGH; - delay1a(); - RGBLOW; - delay1b(); - } else { - RGBHIGH; - delay2a(); - RGBLOW; - delay2b(); - } - } -} -#pragma pop - -#endif -#else -// rgb led not found -// some dummy headers just in case -void rgb_init() { -} +volatile bool rgb_dma_busy = false; +resource_tag_t rgb_timer_tag = 0; -void rgb_send(int data) { -} -#endif +bool rgb_led_busy() { + if (target.rgb_led == PIN_NONE) + return true; + return rgb_dma_busy; +} \ No newline at end of file diff --git a/src/driver/rgb_led.h b/src/driver/rgb_led.h index ded3d5ec5..a52dba850 100644 --- a/src/driver/rgb_led.h +++ b/src/driver/rgb_led.h @@ -1,6 +1,11 @@ #pragma once -void rgb_init(); -void rgb_send(int data); +#include +#include -void rgb_dma_start(); \ No newline at end of file +#define RGB_LED_MAX 32 + +void rgb_led_init(); +void rgb_led_set_value(uint32_t value, uint32_t count); +void rgb_led_send(); +bool rgb_led_busy(); \ No newline at end of file diff --git a/src/driver/rx/sx128x.c b/src/driver/rx/sx128x.c index fedaca4dc..7efd04661 100644 --- a/src/driver/rx/sx128x.c +++ b/src/driver/rx/sx128x.c @@ -23,6 +23,7 @@ volatile uint16_t irq_status = 0; static uint32_t busy_timeout = 1000; static uint8_t payload_len = 8; +static uint8_t buffer_status[2] = {0, 0}; extern volatile uint32_t packet_time_us; extern volatile uint8_t packet_status[2]; @@ -107,31 +108,29 @@ static void sx128x_set_dio0_active(void *arg) { dio0_active = 1; } +static void sx128x_read_buffer(void *arg) { + { + const spi_txn_segment_t segs[] = { + spi_make_seg_const(SX1280_RADIO_READ_BUFFER, buffer_status[1], 0x00), + spi_make_seg_buffer((uint8_t *)rx_spi_packet, NULL, payload_len), + }; + spi_seg_submit(&bus, segs); + } + { + const spi_txn_segment_t segs[] = { + read_command_txn(SX1280_RADIO_GET_PACKETSTATUS, (uint8_t *)packet_status, 2), + }; + spi_seg_submit(&bus, segs, .done_fn = sx128x_set_dio0_active); + } +} + static void sx128x_handle_irq_status(void *arg) { const uint16_t irq = ((irq_status & 0xFF) << 8 | ((irq_status >> 8) & 0xFF)); if ((irq & SX1280_IRQ_RX_DONE)) { - static uint8_t buffer_status[2] = {0, 0}; - { - static const spi_txn_segment_t segs[] = { - read_command_txn(SX1280_RADIO_GET_RXBUFFERSTATUS, buffer_status, 2), - }; - spi_seg_submit(&bus, segs); - } - { - const spi_txn_segment_t segs[] = { - spi_make_seg_const(SX1280_RADIO_READ_BUFFER), - spi_make_seg_delay(NULL, buffer_status + 1, 1), - spi_make_seg_const(0x00), - spi_make_seg_buffer((uint8_t *)rx_spi_packet, NULL, payload_len), - }; - spi_seg_submit(&bus, segs); - } - { - static const spi_txn_segment_t segs[] = { - read_command_txn(SX1280_RADIO_GET_PACKETSTATUS, (uint8_t *)packet_status, 2), - }; - spi_seg_submit(&bus, segs, .done_fn = sx128x_set_dio0_active); - } + const spi_txn_segment_t segs[] = { + read_command_txn(SX1280_RADIO_GET_RXBUFFERSTATUS, buffer_status, 2), + }; + spi_seg_submit(&bus, segs, .done_fn = sx128x_read_buffer); } else if ((irq & SX1280_IRQ_TX_DONE)) { sx128x_set_mode_async(SX1280_MODE_RX); } @@ -156,17 +155,17 @@ void sx128x_wait() { } void sx128x_handle_dio0_exti(bool level) { - if (!level) { + if (!level) return; - } + { - static const spi_txn_segment_t segs[] = { + const spi_txn_segment_t segs[] = { read_command_txn(SX1280_RADIO_GET_IRQSTATUS, (uint8_t *)&irq_status, 2), }; spi_seg_submit(&bus, segs); } { - static const spi_txn_segment_t segs[] = { + const spi_txn_segment_t segs[] = { spi_make_seg_const( SX1280_RADIO_CLR_IRQSTATUS, (uint8_t)(((uint16_t)SX1280_IRQ_RADIO_ALL >> 8) & 0x00FF), @@ -199,15 +198,11 @@ uint16_t sx128x_read_dio0() { } void sx128x_read_register_burst(const uint16_t reg, uint8_t *data, const uint8_t size) { - const uint8_t buf[4] = { - (SX1280_RADIO_READ_REGISTER), - ((reg & 0xFF00) >> 8), - (reg & 0x00FF), - 0x00, - }; - const spi_txn_segment_t segs[] = { - spi_make_seg_buffer(NULL, buf, 4), + spi_make_seg_const((SX1280_RADIO_READ_REGISTER), + ((reg & 0xFF00) >> 8), + (reg & 0x00FF), + 0x00), spi_make_seg_buffer(data, NULL, size), }; spi_seg_submit(&bus, segs); @@ -221,14 +216,10 @@ uint8_t sx128x_read_register(const uint16_t reg) { } void sx128x_write_register_burst(const uint16_t reg, const uint8_t *data, const uint8_t size) { - uint8_t buf[3] = { - (uint8_t)SX1280_RADIO_WRITE_REGISTER, - ((reg & 0xFF00) >> 8), - (reg & 0x00FF), - }; - const spi_txn_segment_t segs[] = { - spi_make_seg_buffer(NULL, buf, 3), + spi_make_seg_const((uint8_t)SX1280_RADIO_WRITE_REGISTER, + ((reg & 0xFF00) >> 8), + (reg & 0x00FF)), spi_make_seg_buffer(NULL, data, size), }; spi_seg_submit(&bus, segs); @@ -258,12 +249,8 @@ void sx128x_write_command(const sx128x_commands_t cmd, const uint8_t val) { } void sx128x_write_tx_buffer(const uint8_t offset, const volatile uint8_t *data, const uint8_t size) { - const uint8_t buf[2] = { - (uint8_t)SX1280_RADIO_WRITE_BUFFER, - offset, - }; const spi_txn_segment_t segs[] = { - spi_make_seg_buffer(NULL, buf, 2), + spi_make_seg_const(SX1280_RADIO_WRITE_BUFFER, offset), spi_make_seg_buffer(NULL, (uint8_t *)data, size), }; spi_seg_submit(&bus, segs); diff --git a/src/driver/spi.c b/src/driver/spi.c index 64914525b..655cae506 100644 --- a/src/driver/spi.c +++ b/src/driver/spi.c @@ -78,18 +78,6 @@ bool spi_txn_continue_port(spi_ports_t port) { } txn->status = TXN_IN_PROGRESS; - - if (txn->flags & TXN_DELAYED_TX) { - uint32_t txn_size = 0; - for (uint32_t i = 0; i < txn->segment_count; ++i) { - spi_txn_segment_t *seg = &txn->segments[i]; - if (seg->tx_data) { - memcpy((uint8_t *)txn->buffer + txn_size, seg->tx_data, seg->size); - } - txn_size += seg->size; - } - } - dev->dma_done = false; } @@ -134,12 +122,6 @@ void spi_seg_submit_ex(spi_bus_device_t *bus, const spi_txn_opts_t opts) { memset(txn->buffer + txn->size, 0xFF, seg->size); } break; - - case TXN_DELAY: - txn_seg->tx_data = seg->tx_data; - txn_seg->rx_data = seg->rx_data; - txn->flags |= TXN_DELAYED_TX; - break; } if (txn_seg->rx_data) { diff --git a/src/driver/spi.h b/src/driver/spi.h index e93a8f023..b35ea6a79 100644 --- a/src/driver/spi.h +++ b/src/driver/spi.h @@ -28,7 +28,6 @@ typedef struct { typedef enum { TXN_CONST, TXN_BUFFER, - TXN_DELAY, } spi_txn_segment_type_t; typedef struct { @@ -49,8 +48,6 @@ typedef struct { (spi_txn_segment_t) { .type = TXN_CONST, .bytes = {_bytes}, .size = sizeof((uint8_t[]){_bytes}) } #define spi_make_seg_buffer(_rx_data, _tx_data, _size) \ (spi_txn_segment_t) { .type = TXN_BUFFER, .rx_data = (_rx_data), .tx_data = (_tx_data), .size = (_size) } -#define spi_make_seg_delay(_rx_data, _tx_data, _size) \ - (spi_txn_segment_t) { .type = TXN_DELAY, .rx_data = (_rx_data), .tx_data = (_tx_data), .size = (_size) } struct spi_bus_device; @@ -62,7 +59,6 @@ typedef enum { } spi_txn_status_t; typedef enum { - TXN_DELAYED_TX = (1 << 0), TXN_DELAYED_RX = (1 << 1), } spi_txn_flags_t; diff --git a/src/driver/timer.c b/src/driver/timer.c index 6f39b16f6..c7d8316d0 100644 --- a/src/driver/timer.c +++ b/src/driver/timer.c @@ -4,11 +4,20 @@ timer_assigment_t timer_assigments[TIMER_ASSIGMENT_MAX] = {}; +static void timer_alloc_dma(timer_use_t use, const target_dma_t *entry) { + if (entry->dma == DMA_STREAM_INVALID || RESOURCE_TAG_TYPE(entry->tag) != RESOURCE_TIM) { + return; + } + + timer_alloc_tag(use, entry->tag); +} + void timer_alloc_init() { - if (target.brushless) { - timer_assigments[0].use = TIMER_USE_MOTOR_DSHOT; - timer_assigments[0].tag = TIMER_TAG(TIMER1, TIMER_CH1 | TIMER_CH3 | TIMER_CH4); + for (uint32_t i = DMA_DEVICE_DSHOT_CH1; i <= DMA_DEVICE_DSHOT_CH3; i++) { + timer_alloc_dma(TIMER_USE_MOTOR_DSHOT, &target.dma[i]); } + + timer_alloc_dma(TIMER_USE_RGB_LED, &target.dma[DMA_DEVICE_RGB]); } bool timer_alloc_tag(timer_use_t use, resource_tag_t tag) { diff --git a/src/driver/timer.h b/src/driver/timer.h index f5160ab4a..6d08c6aa2 100644 --- a/src/driver/timer.h +++ b/src/driver/timer.h @@ -4,50 +4,102 @@ #include "driver/rcc.h" #include "driver/resource.h" -typedef enum { - TIMER_INVALID, #if defined(STM32G4) - TIMER1, - TIMER2, - TIMER3, - TIMER4, - TIMER5, - TIMER6, - TIMER7, - TIMER8, - TIMER15, - TIMER16, - TIMER17, - TIMER20, -#else - TIMER1, - TIMER2, - TIMER3, - TIMER4, - TIMER5, -#ifndef STM32F411 - TIMER6, - TIMER7, - TIMER8, -#endif - TIMER9, - TIMER10, - TIMER11, -#if !defined(STM32F411) && !defined(STM32G473) - TIMER12, - TIMER13, - TIMER14, -#endif -#if defined(STM32H743) || defined(STM32G473) - TIMER15, - TIMER16, - TIMER17, -#endif -#if defined(AT32F4) || defined(STM32G473) - TIMER20, +#define TIMERS \ + TIMER(1) \ + TIMER(2) \ + TIMER(3) \ + TIMER(4) \ + TIMER(5) \ + TIMER(6) \ + TIMER(7) \ + TIMER(8) \ + TIMER(15) \ + TIMER(16) \ + TIMER(17) \ + TIMER(20) +#elif defined(STM32F411) +#define TIMERS \ + TIMER(1) \ + TIMER(2) \ + TIMER(3) \ + TIMER(4) \ + TIMER(5) \ + TIMER(9) \ + TIMER(10) \ + TIMER(11) +#elif defined(STM32F405) +#define TIMERS \ + TIMER(1) \ + TIMER(2) \ + TIMER(3) \ + TIMER(4) \ + TIMER(5) \ + TIMER(6) \ + TIMER(7) \ + TIMER(8) \ + TIMER(9) \ + TIMER(10) \ + TIMER(11) \ + TIMER(12) \ + TIMER(13) \ + TIMER(14) +#elif defined(STM32F7) +#define TIMERS \ + TIMER(1) \ + TIMER(2) \ + TIMER(3) \ + TIMER(4) \ + TIMER(5) \ + TIMER(6) \ + TIMER(7) \ + TIMER(8) \ + TIMER(9) \ + TIMER(10) \ + TIMER(11) \ + TIMER(12) \ + TIMER(13) \ + TIMER(14) +#elif defined(STM32H743) +#define TIMERS \ + TIMER(1) \ + TIMER(2) \ + TIMER(3) \ + TIMER(4) \ + TIMER(5) \ + TIMER(6) \ + TIMER(7) \ + TIMER(8) \ + TIMER(12) \ + TIMER(13) \ + TIMER(14) \ + TIMER(15) \ + TIMER(16) \ + TIMER(17) +#elif defined(AT32F4) +#define TIMERS \ + TIMER(1) \ + TIMER(2) \ + TIMER(3) \ + TIMER(4) \ + TIMER(5) \ + TIMER(6) \ + TIMER(7) \ + TIMER(8) \ + TIMER(9) \ + TIMER(10) \ + TIMER(11) \ + TIMER(12) \ + TIMER(13) \ + TIMER(14) \ + TIMER(20) #endif -#endif - TIMER_MAX, + +typedef enum { + TIMER_INVALID, +#define TIMER(_num) TIMER##_num, + TIMERS TIMER_MAX +#undef TIMER } timer_index_t; typedef enum { @@ -67,6 +119,7 @@ typedef enum { TIMER_USE_FREE, TIMER_USE_MOTOR_DSHOT, TIMER_USE_MOTOR_PWM, + TIMER_USE_RGB_LED, TIMER_USE_ELRS, TIMER_USE_SOFT_SERIAL, } timer_use_t; @@ -88,6 +141,7 @@ void timer_alloc_init(); bool timer_alloc_tag(timer_use_t use, resource_tag_t tag); resource_tag_t timer_alloc(timer_use_t use); uint32_t timer_channel_val(timer_channel_t chan); +uint32_t timer_channel_addr(timer_dev_t *timer, timer_channel_t chan); void timer_enable_dma_request(timer_index_t tim, timer_channel_t chan, bool state); void timer_up_init(timer_index_t tim, uint16_t divider, uint32_t period); \ No newline at end of file diff --git a/src/driver/vtx/vtx.c b/src/driver/vtx.c similarity index 93% rename from src/driver/vtx/vtx.c rename to src/driver/vtx.c index 99669784f..73936bf7b 100644 --- a/src/driver/vtx/vtx.c +++ b/src/driver/vtx.c @@ -1,4 +1,4 @@ -#include "driver/vtx/vtx.h" +#include "driver/vtx.h" #include "core/debug.h" #include "driver/serial.h" @@ -58,7 +58,9 @@ bool serial_vtx_send_data(uint8_t *data, uint32_t size) { return false; } - serial_write_bytes(&serial_vtx, data, size); + if (!serial_write_bytes(&serial_vtx, data, size)) { + return false; + } vtx_last_request = time_millis(); vtx_last_valid_read = time_millis(); diff --git a/src/driver/vtx/vtx.h b/src/driver/vtx.h similarity index 61% rename from src/driver/vtx/vtx.h rename to src/driver/vtx.h index 1d5194c86..c3c75a607 100644 --- a/src/driver/vtx/vtx.h +++ b/src/driver/vtx.h @@ -3,12 +3,11 @@ #include #include -typedef enum { - VTX_ERROR, - VTX_IDLE, - VTX_WAIT, - VTX_SUCCESS -} vtx_update_result_t; +extern uint32_t vtx_last_valid_read; +extern uint32_t vtx_last_request; + +extern uint8_t vtx_payload[32]; +extern uint8_t vtx_payload_offset; bool serial_vtx_is_ready(); bool serial_vtx_wait_for_ready(); diff --git a/src/driver/vtx/msp.c b/src/driver/vtx/msp.c deleted file mode 100644 index 266d06348..000000000 --- a/src/driver/vtx/msp.c +++ /dev/null @@ -1,158 +0,0 @@ - -#include "driver/vtx/msp.h" - -#include - -#include "core/debug.h" -#include "core/profile.h" -#include "driver/osd/displayport.h" -#include "driver/serial.h" -#include "io/msp.h" -#include "rx/unified_serial.h" -#include "util/crc.h" - -#ifdef USE_VTX - -extern msp_t *msp_vtx; - -extern uint32_t vtx_last_valid_read; -extern uint32_t vtx_last_request; - -static void serial_msp_send(msp_magic_t magic, uint8_t direction, uint16_t cmd, const uint8_t *data, uint16_t len) { - if (magic == MSP2_MAGIC) { - const uint32_t size = len + MSP2_HEADER_LEN + 1; - uint8_t vtx_frame[size]; - - vtx_frame[0] = '$'; - vtx_frame[1] = MSP2_MAGIC; - vtx_frame[2] = direction; - vtx_frame[3] = 0; // flag - vtx_frame[4] = (cmd >> 0) & 0xFF; - vtx_frame[5] = (cmd >> 8) & 0xFF; - vtx_frame[6] = (len >> 0) & 0xFF; - vtx_frame[7] = (len >> 8) & 0xFF; - - memcpy(vtx_frame + MSP2_HEADER_LEN, data, len); - vtx_frame[len + MSP2_HEADER_LEN] = crc8_dvb_s2_data(0, vtx_frame + 3, len + 5); - - serial_vtx_send_data(vtx_frame, size); - } else { - const uint32_t size = len + MSP_HEADER_LEN + 1; - uint8_t vtx_frame[size]; - - vtx_frame[0] = '$'; - vtx_frame[1] = MSP1_MAGIC; - vtx_frame[2] = direction; - vtx_frame[3] = len; - vtx_frame[4] = cmd; - - memcpy(vtx_frame + MSP_HEADER_LEN, data, len); - - uint8_t chksum = len; - for (uint8_t i = 4; i < (size - 1); i++) { - chksum ^= vtx_frame[i]; - } - vtx_frame[len + MSP_HEADER_LEN] = chksum; - - serial_vtx_send_data(vtx_frame, size); - } -} - -extern msp_t displayport_msp; -extern msp_t crsf_msp; - -static uint8_t msp_rx_buffer[128]; -static msp_t msp = { - .buffer = msp_rx_buffer, - .buffer_size = 128, - .buffer_offset = 0, - .send = serial_msp_send, - .device = MSP_DEVICE_VTX, -}; - -void serial_msp_vtx_init() { - if (serial_displayport.config.port != SERIAL_PORT_INVALID) { - // reuse existing msp for hdz - msp_vtx = &displayport_msp; - return; - } - - if (profile.serial.smart_audio == profile.serial.rx && - serial_rx_detected_protcol == RX_SERIAL_PROTOCOL_CRSF) { - msp_vtx = &crsf_msp; - return; - } - - const target_serial_port_t *dev = serial_get_dev(profile.serial.smart_audio); - if (!target_serial_port_valid(dev)) { - return; - } - - serial_port_config_t config; - config.port = profile.serial.smart_audio; - config.baudrate = 9600; - config.direction = SERIAL_DIR_TX_RX; - config.stop_bits = SERIAL_STOP_BITS_1; - config.invert = false; - config.half_duplex = true; - config.half_duplex_pp = false; - - serial_vtx_wait_for_ready(); - serial_init(&serial_vtx, config); - - msp_vtx = &msp; -} - -vtx_update_result_t serial_msp_vtx_update() { - if (serial_displayport.config.port != SERIAL_PORT_INVALID) { - if (!displayport_is_ready()) { - return VTX_WAIT; - } - return VTX_IDLE; - } - - if (profile.serial.smart_audio != SERIAL_PORT_INVALID && - profile.serial.smart_audio == profile.serial.rx && - serial_rx_detected_protcol == RX_SERIAL_PROTOCOL_CRSF) { - return VTX_IDLE; - } - - if (!serial_vtx_is_ready()) { - return VTX_WAIT; - } - - static bool in_progress = false; - static bool is_first_packet = true; - - uint8_t data = 0; - while (serial_vtx_read_byte(&data)) { - quic_debugf("MSP_VTX: read 0x%x %c", data, data); - - in_progress = true; - - msp_status_t status = msp_process_serial(msp_vtx, data); - switch (status) { - case MSP_ERROR: - case MSP_EOF: - break; - case MSP_SUCCESS: - in_progress = false; - is_first_packet = false; - return VTX_SUCCESS; - } - } - - if ((in_progress || is_first_packet) && (time_millis() - vtx_last_valid_read) > 500) { - quic_debugf("MSP_VTX: timeout waiting for packet"); - vtx_last_valid_read = time_millis(); - return VTX_ERROR; - } - - if (in_progress) { - return VTX_WAIT; - } - - return VTX_IDLE; -} - -#endif \ No newline at end of file diff --git a/src/driver/vtx/msp.h b/src/driver/vtx/msp.h deleted file mode 100644 index 332c71810..000000000 --- a/src/driver/vtx/msp.h +++ /dev/null @@ -1,8 +0,0 @@ -#pragma once - -#include - -#include "driver/vtx/vtx.h" - -void serial_msp_vtx_init(); -vtx_update_result_t serial_msp_vtx_update(); diff --git a/src/driver/vtx/sa.c b/src/driver/vtx/sa.c deleted file mode 100644 index 1a57f57c7..000000000 --- a/src/driver/vtx/sa.c +++ /dev/null @@ -1,291 +0,0 @@ -#include "driver/vtx/sa.h" - -#include - -#include "core/debug.h" -#include "core/profile.h" -#include "driver/serial.h" -#include "driver/time.h" -#include "driver/vtx/vtx.h" -#include "io/usb_configurator.h" -#include "io/vtx.h" -#include "util/crc.h" -#include "util/ring_buffer.h" -#include "util/util.h" - -#ifdef USE_VTX - -#define SMART_AUDIO_BAUDRATE_MIN 4650 -#define SMART_AUDIO_BAUDRATE_MAX 5050 - -#define SA_HEADER_SIZE 5 -#define SA_MAGIC_1 0xaa -#define SA_MAGIC_2 0x55 - -typedef enum { - PARSER_IDLE, - PARSER_READ_MAGIC_1, - PARSER_READ_MAGIC_2, - PARSER_READ_CMD, - PARSER_READ_LENGTH, - PARSER_READ_PAYLOAD, - PARSER_READ_CRC, -} smart_audio_parser_state_t; - -smart_audio_settings_t smart_audio_settings; - -static uint32_t baud_rate = 4800; -static uint32_t packets_sent = 0; -static uint32_t packets_recv = 0; - -static smart_audio_parser_state_t parser_state = PARSER_IDLE; - -extern uint32_t vtx_last_valid_read; -extern uint32_t vtx_last_request; - -extern uint8_t vtx_payload[32]; -extern uint8_t vtx_payload_offset; - -const uint8_t default_dac_power_levels[4] = { - 7, - 16, - 25, - 40, -}; - -static void serial_smart_audio_reconfigure() { - serial_vtx_wait_for_ready(); - - const target_serial_port_t *dev = serial_get_dev(profile.serial.smart_audio); - if (!target_serial_port_valid(dev)) { - return; - } - - serial_port_config_t config; - config.port = profile.serial.smart_audio; - config.baudrate = baud_rate; - config.direction = SERIAL_DIR_TX_RX; - config.stop_bits = SERIAL_STOP_BITS_2; - config.invert = false; - config.half_duplex = true; - config.half_duplex_pp = true; - - serial_init(&serial_vtx, config); -} - -static void smart_audio_auto_baud() { - static uint8_t last_percent = 0; - - // move quickly while we have not yet found a working baud rate - const uint8_t current_percent = ((packets_recv * 100) / packets_sent); - if (packets_sent < (last_percent == 0 ? 3 : 10)) { - last_percent = current_percent; - return; - } - - if (current_percent < 70) { - static int8_t direction = 1; - - // if the percentage degraded, switch it up - if (last_percent > current_percent) { - direction = direction == 1 ? -1 : 1; - } - - if ((direction == 1) && (baud_rate == SMART_AUDIO_BAUDRATE_MAX)) { - direction = -1; - } else if ((direction == -1 && baud_rate == SMART_AUDIO_BAUDRATE_MIN)) { - direction = 1; - } - - baud_rate += direction * 50; - quic_debugf("SMART_AUDIO: auto baud %d (%d) change %d vs %d", baud_rate, direction * 50, last_percent, current_percent); - serial_smart_audio_reconfigure(); - } - - last_percent = current_percent; - packets_sent = 0; - packets_recv = 0; -} - -static uint8_t serial_smart_audio_parse_packet(uint8_t cmd, uint8_t *payload, uint32_t length) { - switch (cmd) { - case SA_CMD_GET_SETTINGS: - case SA_CMD_GET_SETTINGS_V2: - case SA_CMD_GET_SETTINGS_V21: - smart_audio_settings.version = (cmd == SA_CMD_GET_SETTINGS ? 1 : (cmd == SA_CMD_GET_SETTINGS_V2 ? 2 : 3)); - smart_audio_settings.channel = payload[0]; - smart_audio_settings.power = payload[1]; - smart_audio_settings.mode = payload[2]; - smart_audio_settings.frequency = (uint16_t)(((uint16_t)payload[3] << 8) | payload[4]); - - if (cmd == SA_CMD_GET_SETTINGS_V21) { - smart_audio_settings.power = payload[5]; - - smart_audio_settings.level_count = min(payload[6], VTX_POWER_LEVEL_MAX); - - // SmartAudio seems to report buf[8] + 1 power levels, but one of them is zero. - // zero is indeed a valid power level to set the vtx to, but it activates pit mode. - // crucially, after sending 0 dbm, the vtx does NOT report its power level to be 0 dbm. - // instead, it reports whatever value was set previously and it reports to be in pit mode. - for (uint8_t i = 0; i < smart_audio_settings.level_count; i++) { - smart_audio_settings.dac_power_levels[i] = payload[7 + i + 1]; //+ 1 to skip the first power level, as mentioned above - } - } else { - smart_audio_settings.level_count = 4; - for (uint8_t i = 0; i < smart_audio_settings.level_count; i++) { - smart_audio_settings.dac_power_levels[i] = default_dac_power_levels[i]; - } - } - break; - - case SA_CMD_SET_FREQUENCY: - smart_audio_settings.frequency = (uint16_t)(((uint16_t)payload[0] << 8) | payload[1]); - break; - - case SA_CMD_SET_CHANNEL: - smart_audio_settings.channel = payload[0]; - break; - - case SA_CMD_SET_POWER: { - smart_audio_settings.power = payload[0]; - break; - } - case SA_CMD_SET_MODE: { - const uint8_t mode = payload[0]; - - // in-range pitmode - smart_audio_settings.mode |= ((mode >> 0) & 0x1) << 2; - - // out-range pitmode - smart_audio_settings.mode |= ((mode >> 1) & 0x1) << 3; - - // pit mode runnig - smart_audio_settings.mode |= ((mode >> 2) & 0x1) << 1; - - // locked bit - smart_audio_settings.mode |= ((mode >> 3) & 0x1) << 4; - break; - } - default: - quic_debugf("SMART_AUDIO: invalid cmd %d (%d)", cmd, length); - return 0; - } - - packets_recv++; - return 1; -} - -void serial_smart_audio_init() { - serial_smart_audio_reconfigure(); -} - -vtx_update_result_t serial_smart_audio_update() { - if (!serial_vtx_is_ready()) { - return VTX_WAIT; - } - if (parser_state == PARSER_IDLE) { - return VTX_IDLE; - } - if ((time_millis() - vtx_last_valid_read) > 500) { - return VTX_ERROR; - } - - static uint8_t length = 0; - - while (true) { - uint8_t data = 0; - if (serial_vtx_read_byte(&data) == 0) { - return VTX_WAIT; - } - quic_debugf("VTX parser_state: %d 0x%x", parser_state, data); - switch (parser_state) { - case PARSER_IDLE: - return VTX_IDLE; - - case PARSER_READ_MAGIC_1: - if (data == SA_MAGIC_1) { - parser_state = PARSER_READ_MAGIC_2; - } - break; - - case PARSER_READ_MAGIC_2: - if (data != SA_MAGIC_2) { - parser_state = PARSER_READ_MAGIC_1; - } else { - parser_state = PARSER_READ_CMD; - } - break; - - case PARSER_READ_CMD: - vtx_payload[0] = data; - parser_state = PARSER_READ_LENGTH; - break; - - case PARSER_READ_LENGTH: - length = vtx_payload[1] = data; - vtx_payload_offset = 0; - parser_state = length ? PARSER_READ_PAYLOAD : PARSER_READ_CRC; - break; - - case PARSER_READ_PAYLOAD: - vtx_payload[vtx_payload_offset + 2] = data; - if (++vtx_payload_offset == length) { - parser_state = PARSER_READ_CRC; - } - break; - - case PARSER_READ_CRC: { - parser_state = PARSER_IDLE; - const uint8_t theirs = crc8_dvb_s2_data(0, vtx_payload, length + 2); - if (data != theirs) { - quic_debugf("VTX crc error 0x%x vs 0x%x", data, theirs); - return VTX_ERROR; - } - if (!serial_smart_audio_parse_packet(vtx_payload[0], vtx_payload + 2, length - 2)) { - return VTX_ERROR; - } - return VTX_SUCCESS; - } - } - } - - return VTX_ERROR; -} - -void serial_smart_audio_send_payload(uint8_t cmd, const uint8_t *payload, const uint32_t size) { - if (!serial_vtx_is_ready()) { - return; - } - -#ifdef USE_AKK_SA_WORKAROUND -#define EXTRA_DUMMY_BYTES 1 -#else -#define EXTRA_DUMMY_BYTES 0 -#endif - - const uint32_t len = size + 1 + SA_HEADER_SIZE + EXTRA_DUMMY_BYTES; - uint8_t vtx_frame[len]; - - vtx_frame[0] = 0x00; - vtx_frame[1] = 0xAA; - vtx_frame[2] = 0x55; - vtx_frame[3] = (cmd << 1) | 0x1; - vtx_frame[4] = size; - for (uint8_t i = 0; i < size; i++) { - vtx_frame[i + SA_HEADER_SIZE] = payload[i]; - } - vtx_frame[size + SA_HEADER_SIZE] = crc8_dvb_s2_data(0, vtx_frame + 1, len - 2 - EXTRA_DUMMY_BYTES); -#ifdef USE_AKK_SA_WORKAROUND - vtx_frame[size + 1 + SA_HEADER_SIZE] = 0x00; -#endif - - smart_audio_auto_baud(); - - serial_vtx_send_data(vtx_frame, len); - if (parser_state == PARSER_IDLE) { - parser_state = PARSER_READ_MAGIC_1; - } - packets_sent++; -} - -#endif \ No newline at end of file diff --git a/src/driver/vtx/sa.h b/src/driver/vtx/sa.h deleted file mode 100644 index f597cf612..000000000 --- a/src/driver/vtx/sa.h +++ /dev/null @@ -1,37 +0,0 @@ -#pragma once - -#include - -#include "driver/vtx/vtx.h" - -typedef enum { - SA_CMD_GET_SETTINGS = 0x01, - SA_CMD_SET_POWER = 0x02, - SA_CMD_SET_CHANNEL = 0x03, - SA_CMD_SET_FREQUENCY = 0x04, - SA_CMD_SET_MODE = 0x05, - SA_CMD_GET_SETTINGS_V2 = 0x9, - SA_CMD_GET_SETTINGS_V21 = 0x11, -} smart_audio_cmd_t; - -typedef enum { - SA_MODE_FREQUENCY = 1 << 0, - SA_MODE_PIT = 1 << 1, - SA_MODE_IN_RANGE_PIT = 1 << 2, - SA_MODE_OUT_RANGE_PIT = 1 << 3, - SA_MODE_UNLOCKED = 1 << 4, -} smart_mode_t; - -typedef struct { - uint8_t version; - uint8_t channel; - uint8_t power; - uint8_t mode; - uint16_t frequency; - uint8_t level_count; - uint16_t dac_power_levels[8]; -} smart_audio_settings_t; - -void serial_smart_audio_init(); -vtx_update_result_t serial_smart_audio_update(); -void serial_smart_audio_send_payload(uint8_t cmd, const uint8_t *payload, const uint32_t size); \ No newline at end of file diff --git a/src/driver/vtx/tramp.c b/src/driver/vtx/tramp.c deleted file mode 100644 index 35a214e2c..000000000 --- a/src/driver/vtx/tramp.c +++ /dev/null @@ -1,176 +0,0 @@ -#include "driver/vtx/tramp.h" - -#include - -#include "core/debug.h" -#include "core/profile.h" -#include "driver/serial.h" -#include "driver/time.h" -#include "driver/vtx/vtx.h" -#include "util/ring_buffer.h" - -#ifdef USE_VTX - -typedef enum { - PARSER_IDLE, - PARSER_READ_MAGIC, - PARSER_READ_PAYLOAD, - PARSER_READ_CRC, - PRASER_WAIT_FOR_READY -} tramp_parser_state_t; - -tramp_settings_t tramp_settings; - -static tramp_parser_state_t parser_state = PARSER_IDLE; - -extern uint32_t vtx_last_valid_read; -extern uint32_t vtx_last_request; - -extern uint8_t vtx_payload[32]; -extern uint8_t vtx_payload_offset; - -static uint8_t crc8_data(const uint8_t *data) { - uint8_t crc = 0; - for (int i = 0; i < 13; i++) { - crc += data[i]; - } - return crc; -} - -void serial_tramp_init() { - serial_vtx_wait_for_ready(); - - const target_serial_port_t *dev = serial_get_dev(profile.serial.smart_audio); - if (!target_serial_port_valid(dev)) { - return; - } - - serial_port_config_t config; - config.port = profile.serial.smart_audio; - config.baudrate = 9600; - config.direction = SERIAL_DIR_TX_RX; - config.stop_bits = SERIAL_STOP_BITS_1; - config.invert = false; - config.half_duplex = true; - config.half_duplex_pp = false; - - serial_init(&serial_vtx, config); -} - -static bool tramp_is_query(uint8_t cmd) { - switch (cmd) { - case 'r': - case 'v': - case 's': - return true; - } - return false; -} - -static uint8_t tramp_parse_packet(uint8_t *payload) { - switch (payload[0]) { - case 'r': - tramp_settings.freq_min = payload[1] | (payload[2] << 8); - tramp_settings.freq_max = payload[3] | (payload[4] << 8); - tramp_settings.power_max = payload[5] | (payload[6] << 8); - break; - - case 'v': - tramp_settings.frequency = payload[1] | (payload[2] << 8); - tramp_settings.power = payload[3] | (payload[4] << 8); - tramp_settings.control_mode = payload[5]; - tramp_settings.pit_mode = payload[6]; - tramp_settings.current_power = payload[7] | (payload[8] << 8); - break; - - case 's': - tramp_settings.temp = payload[5] | (payload[6] << 8); - break; - } - - return 1; -} - -vtx_update_result_t serial_tramp_update() { - if (!serial_vtx_is_ready()) { - return VTX_WAIT; - } - if (parser_state > PARSER_IDLE && (time_millis() - vtx_last_valid_read) > 500) { - return VTX_ERROR; - } - -tramp_do_more: - switch (parser_state) { - case PARSER_IDLE: - return VTX_IDLE; - - case PARSER_READ_MAGIC: { - if (serial_vtx_read_byte(&vtx_payload[0]) == 0) { - return VTX_WAIT; - } - - if (vtx_payload[0] == 0x0F) { - parser_state = PARSER_READ_PAYLOAD; - vtx_payload_offset = 1; - } - goto tramp_do_more; - } - case PARSER_READ_PAYLOAD: { - if (serial_vtx_read_byte(&vtx_payload[vtx_payload_offset]) == 0) { - return VTX_WAIT; - } - - vtx_payload_offset++; - if (vtx_payload_offset >= 16) { - parser_state = PARSER_READ_CRC; - } - goto tramp_do_more; - } - case PARSER_READ_CRC: { - parser_state = PARSER_IDLE; - - const uint8_t crc = crc8_data(vtx_payload + 1); - if (vtx_payload[14] != crc || vtx_payload[15] != 0) { - return VTX_ERROR; - } - - if (!tramp_parse_packet(vtx_payload + 1)) { - return VTX_ERROR; - } - - return VTX_SUCCESS; - } - case PRASER_WAIT_FOR_READY: { - // dummy state for non querry packets - parser_state = PARSER_IDLE; - return VTX_SUCCESS; - } - } - - return VTX_ERROR; -} - -void serial_tramp_send_payload(uint8_t cmd, const uint16_t payload) { - if (!serial_vtx_is_ready()) { - return; - } - - uint8_t vtx_frame[16]; - memset(vtx_frame, 0, 16); - - vtx_frame[0] = 0x0F; - vtx_frame[1] = cmd; - vtx_frame[2] = payload & 0xff; - vtx_frame[3] = (payload >> 8) & 0xff; - vtx_frame[14] = crc8_data(vtx_frame + 1); - - serial_vtx_send_data(vtx_frame, 16); - - if (tramp_is_query(vtx_frame[1])) { - parser_state = PARSER_READ_MAGIC; - } else { - parser_state = PRASER_WAIT_FOR_READY; - } -} - -#endif \ No newline at end of file diff --git a/src/driver/vtx/tramp.h b/src/driver/vtx/tramp.h deleted file mode 100644 index 4eb7b3945..000000000 --- a/src/driver/vtx/tramp.h +++ /dev/null @@ -1,24 +0,0 @@ -#pragma once - -#include - -#include "driver/vtx/vtx.h" - -typedef struct { - uint16_t freq_min; - uint16_t freq_max; - - uint16_t power_max; - uint16_t current_power; - - uint16_t temp; - - uint16_t frequency; - uint16_t power; - uint8_t pit_mode; - uint8_t control_mode; -} tramp_settings_t; - -void serial_tramp_init(); -vtx_update_result_t serial_tramp_update(); -void serial_tramp_send_payload(uint8_t cmd, const uint16_t payload); \ No newline at end of file diff --git a/src/flight/filter.c b/src/flight/filter.c index 50aa13a3d..928861d8e 100644 --- a/src/flight/filter.c +++ b/src/flight/filter.c @@ -90,6 +90,92 @@ float filter_lp_pt3_step(filter_lp_pt3 *filter, filter_state_t *state, float in) return state->delay_element[0]; } +void filter_lp_lulu_coeff(filter_lp_lulu *filter, float hz) { + if (filter->hz == hz && filter->sample_period_us == state.looptime_autodetect) { + return; + } + filter->hz = hz; + filter->sample_period_us = state.looptime_autodetect; + + // The window value is half the wavelength of the wave that it filters. So if the wavelength of the cutoff frequency is 2 samples, the N value should be 1. If the wavelength is 4, N should be 2. Etc. + float cutoff_wave_length = 1.0f / hz / 4.0f; + float loop_wave_length = state.looptime_autodetect * 1e-6f; + uint8_t window_half_length = cutoff_wave_length / loop_wave_length; + + filter->num_samples = constrain(window_half_length, 1, 12); + filter->window_size = filter->num_samples * 2 + 1; +} + +void filter_lp_lulu_init(filter_lp_lulu *filter, filter_state_t *state, uint8_t count, float hz) { + filter_lp_lulu_coeff(filter, hz); + filter_init_state(state, count); +} + +static float fix_road(float *series, float *series_b, uint8_t index, uint8_t filter_n, uint8_t window_size) { + for (uint32_t N = 1; N <= filter_n; N++) { + const uint32_t index_neg = (index + window_size - 2 * N) % window_size; + + float prev_val = series[index_neg]; + float prev_val_b = series_b[index_neg]; + + uint32_t cur_index = (index_neg + 1) % window_size; + uint32_t index_pos = (cur_index + N) % window_size; + for (uint32_t i = window_size - 2 * N; i < window_size - N; i++) { + const float cur_val = series[cur_index]; + const float next_val = series[index_pos]; + if (prev_val < cur_val && cur_val > next_val) { + series[cur_index] = max(prev_val, next_val); + } + prev_val = cur_val; + + const float cur_val_b = series_b[cur_index]; + const float next_val_b = series_b[index_pos]; + if (prev_val_b < cur_val_b && cur_val_b > next_val_b) { + series_b[cur_index] = max(prev_val_b, next_val_b); + } + prev_val_b = cur_val_b; + + cur_index = (cur_index + 1) % window_size; + index_pos = (index_pos + 1) % window_size; + } + + prev_val = series[index_neg]; + prev_val_b = series_b[index_neg]; + + cur_index = (index_neg + 1) % window_size; + index_pos = (cur_index + N) % window_size; + for (uint32_t i = window_size - 2 * N; i < window_size - N; i++) { + const float cur_val = series[cur_index]; + const float next_val = series[index_pos]; + if (prev_val > cur_val && cur_val < next_val) { + series[cur_index] = min(prev_val, next_val); + } + prev_val = cur_val; + + const float cur_val_b = series_b[cur_index]; + const float next_val_b = series_b[index_pos]; + if (prev_val_b > cur_val_b && cur_val_b < next_val_b) { + series_b[cur_index] = min(prev_val_b, next_val_b); + } + prev_val_b = cur_val_b; + + cur_index = (cur_index + 1) % window_size; + index_pos = (index_pos + 1) % window_size; + } + } + + const uint8_t final_index = (index + window_size - filter_n) % window_size; + return (series[final_index] - series_b[final_index]) / 2; +} + +float filter_lp_lulu_step(filter_lp_lulu *filter, filter_state_t *state, float in) { + const uint8_t window_index = state->window_buf_index; + state->window_buf_index = (window_index + 1) % filter->window_size; + state->interim[window_index] = in; + state->interim_b[window_index] = -in; + return fix_road(state->interim, state->interim_b, window_index, filter->num_samples, filter->window_size); +} + void filter_biquad_notch_init(filter_biquad_notch_t *filter, filter_biquad_state_t *state, uint8_t count, float hz) { memset(filter, 0, sizeof(filter_biquad_notch_t)); filter_biquad_notch_coeff(filter, hz); @@ -190,6 +276,8 @@ void filter_init(filter_type_t type, filter_t *filter, filter_state_t *state, ui case FILTER_LP_PT3: filter_lp_pt3_init(&filter->lp_pt3, state, count, hz); break; + case FILTER_LP_LULU: + filter_lp_lulu_init(&filter->lp_lulu, state, count, hz); default: // no filter, do nothing break; @@ -207,6 +295,9 @@ void filter_coeff(filter_type_t type, filter_t *filter, float hz) { case FILTER_LP_PT3: filter_lp_pt3_coeff(&filter->lp_pt3, hz); break; + case FILTER_LP_LULU: + filter_lp_lulu_coeff(&filter->lp_lulu, hz); + break; default: // no filter, do nothing break; @@ -221,6 +312,8 @@ float filter_step(filter_type_t type, filter_t *filter, filter_state_t *state, f return filter_lp_pt2_step(&filter->lp_pt2, state, in); case FILTER_LP_PT3: return filter_lp_pt3_step(&filter->lp_pt3, state, in); + case FILTER_LP_LULU: + return filter_lp_lulu_step(&filter->lp_lulu, state, in); default: // no filter at all return in; diff --git a/src/flight/filter.h b/src/flight/filter.h index 23e28333a..6e1ac4bdb 100644 --- a/src/flight/filter.h +++ b/src/flight/filter.h @@ -9,10 +9,20 @@ typedef enum { FILTER_LP_PT1, FILTER_LP_PT2, FILTER_LP_PT3, + FILTER_LP_LULU, + + FILTER_MAX } __attribute__((__packed__)) filter_type_t; -typedef struct { - float delay_element[3]; +typedef union { + struct { + float delay_element[3]; + }; + struct { + float interim[32]; + float interim_b[32]; + uint8_t window_buf_index; + }; } filter_state_t; typedef struct { @@ -43,6 +53,15 @@ typedef struct { float alpha; } filter_lp_pt3; +// Max N = 15 +typedef struct { + float hz; + uint32_t sample_period_us; + + uint8_t window_size; + uint8_t num_samples; +} filter_lp_lulu; + typedef struct { float hz; uint32_t sample_period_us; @@ -58,6 +77,7 @@ typedef union { filter_lp_pt1 lp_pt1; filter_lp_pt2 lp_pt2; filter_lp_pt3 lp_pt3; + filter_lp_lulu lp_lulu; } filter_t; typedef struct { @@ -97,6 +117,10 @@ void filter_lp_pt3_init(filter_lp_pt3 *filter, filter_state_t *state, uint8_t co void filter_lp_pt3_coeff(filter_lp_pt3 *filter, float hz); float filter_lp_pt3_step(filter_lp_pt3 *filter, filter_state_t *state, float in); +void filter_lp_lulu_init(filter_lp_lulu *filter, filter_state_t *state, uint8_t count, float hz); +void filter_lp_lulu_coeff(filter_lp_lulu *filter, float hz); +float filter_lp_lulu_step(filter_lp_lulu *filter, filter_state_t *state, float in); + void filter_biquad_notch_init(filter_biquad_notch_t *filter, filter_biquad_state_t *state, uint8_t count, float hz); void filter_biquad_notch_coeff(filter_biquad_notch_t *filter, float hz); float filter_biquad_notch_step(filter_biquad_notch_t *filter, filter_biquad_state_t *state, float in); diff --git a/src/flight/input.c b/src/flight/input.c index d5bbda4dc..1cdfbe6fa 100644 --- a/src/flight/input.c +++ b/src/flight/input.c @@ -156,8 +156,18 @@ vec3_t input_rates_calc() { } float input_throttle_calc(float throttle) { - const float n = (throttle * 2.f - 1.f); const float expo = profile.rate.throttle_expo; const float mid = profile.rate.throttle_mid; - return constrain((n * n * n * expo + n * (1.f - expo) + 1.f) * mid, 0.0f, 1.0f); + const float throttle_minus_mid = throttle - mid; + + float divisor = 1; + if (throttle_minus_mid > 0.0f) { + divisor = 1 - mid; + } + if (throttle_minus_mid < 0.0f) { + divisor = mid; + } + + // betaflight's throttle curve: https://github.com/betaflight/betaflight/blob/c513b102b6f2af9aa0390cfa7ef908ec652552fc/src/main/fc/rc.c#L838 + return constrain(mid + throttle_minus_mid * (1 - expo + expo * (throttle_minus_mid * throttle_minus_mid) / (divisor * divisor)), 0.0f, 1.0f); } \ No newline at end of file diff --git a/src/flight/pid.c b/src/flight/pid.c index 9c9d1cd8d..a8268a0c9 100644 --- a/src/flight/pid.c +++ b/src/flight/pid.c @@ -42,7 +42,7 @@ static vec3_t last_error2; static filter_t filter[FILTER_MAX_SLOTS]; static filter_state_t filter_state[FILTER_MAX_SLOTS][3]; -static filter_lp_pt1 dynamic_filter; +static filter_t dynamic_filter; static filter_state_t dynamic_filter_state[3]; static filter_lp_pt1 rx_filter; @@ -50,15 +50,10 @@ static filter_state_t rx_filter_state[3]; void pid_init() { filter_lp_pt1_init(&rx_filter, rx_filter_state, 3, state.rx_filter_hz); - for (uint8_t i = 0; i < FILTER_MAX_SLOTS; i++) { filter_init(profile.filter.dterm[i].type, &filter[i], filter_state[i], 3, profile.filter.dterm[i].cutoff_freq); } - - if (profile.filter.dterm_dynamic_enable) { - // zero out filter, freq will be updated later on - filter_lp_pt1_init(&dynamic_filter, dynamic_filter_state, 3, DTERM_DYNAMIC_FREQ_MAX); - } + filter_init(profile.filter.dterm_dynamic_type, &dynamic_filter, dynamic_filter_state, 3, DTERM_DYNAMIC_FREQ_MAX); } // (iwindup = 0 windup is not allowed) (iwindup = 1 windup is allowed) @@ -92,11 +87,7 @@ static inline float pid_compute_iterm_windup(uint8_t x, float pid_output) { static inline float pid_filter_dterm(uint8_t x, float dterm) { dterm = filter_step(profile.filter.dterm[0].type, &filter[0], &filter_state[0][x], dterm); dterm = filter_step(profile.filter.dterm[1].type, &filter[1], &filter_state[1][x], dterm); - - if (profile.filter.dterm_dynamic_enable) { - dterm = filter_lp_pt1_step(&dynamic_filter, &dynamic_filter_state[x], dterm); - } - + dterm = filter_step(profile.filter.dterm_dynamic_type, &dynamic_filter, &dynamic_filter_state[x], dterm); return dterm; } @@ -147,9 +138,15 @@ static inline float pid_tda_compensation() { // output: state.pidoutput.axis[] = change required from motors void pid_calc() { filter_lp_pt1_coeff(&rx_filter, state.rx_filter_hz); + filter_coeff(profile.filter.dterm[0].type, &filter[0], profile.filter.dterm[0].cutoff_freq); filter_coeff(profile.filter.dterm[1].type, &filter[1], profile.filter.dterm[1].cutoff_freq); + const float dynamic_throttle = state.throttle + state.throttle * (1.0f - state.throttle); + const float dterm_dynamic_raw_freq = mapf(dynamic_throttle, 0.0f, 1.0f, profile.filter.dterm_dynamic_min, profile.filter.dterm_dynamic_max); + const float dterm_dynamic_freq = constrain(dterm_dynamic_raw_freq, profile.filter.dterm_dynamic_min, profile.filter.dterm_dynamic_max); + filter_coeff(profile.filter.dterm_dynamic_type, &dynamic_filter, dterm_dynamic_freq); + static vec3_t pid_output = {.roll = 0, .pitch = 0, .yaw = 0}; const float v_compensation = pid_voltage_compensation(); const float tda_compensation = pid_tda_compensation(); @@ -158,14 +155,6 @@ void pid_calc() { const float *stick_accelerator = profile.pid.stick_rates[stick_boost_profile].accelerator.axis; const float *stick_transition = profile.pid.stick_rates[stick_boost_profile].transition.axis; - if (profile.filter.dterm_dynamic_enable) { - float dynamic_throttle = state.throttle + state.throttle * (1 - state.throttle) * DTERM_DYNAMIC_EXPO; - float d_term_dynamic_freq = mapf(dynamic_throttle, 0.0f, 1.0f, profile.filter.dterm_dynamic_min, profile.filter.dterm_dynamic_max); - d_term_dynamic_freq = constrain(d_term_dynamic_freq, profile.filter.dterm_dynamic_min, profile.filter.dterm_dynamic_max); - - filter_lp_pt1_coeff(&dynamic_filter, d_term_dynamic_freq); - } - // rotates errors ierror = vec3_rotate(ierror, state.gyro_delta_angle); diff --git a/src/flight/sixaxis.c b/src/flight/sixaxis.c index a4f87c428..5f11109f9 100644 --- a/src/flight/sixaxis.c +++ b/src/flight/sixaxis.c @@ -20,9 +20,9 @@ #include "io/led.h" #include "util/util.h" -#define CAL_TIME 2e6 -#define WAIT_TIME 15e6 -#define GLOW_TIME 62500 +#define CAL_INTERVAL 2000 // time between measurements in us +#define CAL_COUNT 500 // count of samples +#define CAL_TRIES (CAL_COUNT * 100) // number of attempts #define GYRO_BIAS_LIMIT 800 #define ACCEL_BIAS_LIMIT 800 @@ -226,7 +226,7 @@ static bool sixaxis_wait_for_still(uint32_t timeout) { move_counter--; } - led_pwm(move_counter, 1000); + led_pwm(move_counter / 15.f, 1000); while ((time_micros() - now) < 1000) ; @@ -240,45 +240,31 @@ static bool sixaxis_wait_for_still(uint32_t timeout) { void sixaxis_gyro_cal() { for (uint8_t retry = 0; retry < 15; ++retry) { - if (sixaxis_wait_for_still(WAIT_TIME / 15)) { + if (sixaxis_wait_for_still(CAL_INTERVAL)) { // break only if it's already still, otherwise, wait and try again break; } - time_delay_ms(200); + time_delay_ms(100); } gyro_calibrate(); - uint8_t brightness = 0; - led_pwm(brightness, 1000); - gyro_data_t last_data = gyro_read(); - - uint32_t start = time_micros(); - uint32_t now = start; - int32_t cal_counter = CAL_TIME / 1000; - for (int32_t timeout = WAIT_TIME / 1000; timeout > 0; --timeout) { + for (uint32_t tries = 0, cal_counter = 0; tries < CAL_TRIES; tries++) { const gyro_data_t data = gyro_read(); - led_pwm(brightness, 1000); - if ((brightness & 1) ^ ((now - start) % GLOW_TIME > (GLOW_TIME >> 1))) { - brightness++; - brightness &= 0xF; - } + led_pwm(((float)(cal_counter) / (float)(CAL_COUNT)), CAL_INTERVAL); bool did_move = test_gyro_move(&last_data, &data); if (!did_move) { // only cali gyro when it's still for (uint8_t i = 0; i < 3; i++) { - lpf(&gyrocal[i], data.gyro.axis[i], lpfcalc(1000, 0.5 * 1e6)); + lpf(&gyrocal[i], data.gyro.axis[i], lpfcalc(CAL_INTERVAL, 0.5 * 1e6)); } - if (--cal_counter <= 0) { + if (cal_counter++ == CAL_COUNT) { break; } } - while ((time_micros() - now) < 1000) - ; - - now = time_micros(); + time_delay_us(CAL_INTERVAL); last_data = data; } } diff --git a/src/io/led.c b/src/io/led.c index cf63f6de7..969bc30e0 100644 --- a/src/io/led.c +++ b/src/io/led.c @@ -71,7 +71,7 @@ void led_off(uint8_t val) { } // delta- sigma first order modulator. -void led_pwm(uint8_t pwmval, float looptime) { +void led_pwm(float desired_brightness, float looptime) { static uint32_t last_time = 0; const uint32_t time = time_micros(); const uint32_t ledtime = time - last_time; @@ -81,8 +81,6 @@ void led_pwm(uint8_t pwmval, float looptime) { ds_integrator = constrain(ds_integrator, -2, 2); static float last_brightness = 0; - - const float desired_brightness = pwmval * (1.0f / 15.0f); ds_integrator += (desired_brightness - last_brightness) * ledtime * (1.0f / looptime); if (ds_integrator > 0.49f) { diff --git a/src/io/led.h b/src/io/led.h index cbcfe1828..60921404c 100644 --- a/src/io/led.h +++ b/src/io/led.h @@ -7,7 +7,7 @@ void led_init(); void led_on(uint8_t val); void led_off(uint8_t val); -void led_pwm(uint8_t pwmval, float looptime); +void led_pwm(float brightness, float looptime); void led_flash(); void led_blink(uint8_t count); diff --git a/src/io/msp.c b/src/io/msp.c index 5146fc614..926a1e8ed 100644 --- a/src/io/msp.c +++ b/src/io/msp.c @@ -27,7 +27,7 @@ enum { MSP_REBOOT_COUNT, }; -extern uint8_t msp_vtx_detected; +extern bool msp_vtx_detected; extern vtx_settings_t vtx_actual; extern char msp_vtx_band_letters[VTX_BAND_MAX]; extern uint8_t msp_vtx_band_is_factory[VTX_BAND_MAX]; @@ -65,6 +65,16 @@ static void msp_write_uint32_t(uint8_t *data, uint32_t val) { data[3] = val >> 24; } +static void msp_check_vtx_detected(msp_t *msp) { + if (msp_vtx_detected || msp->device != MSP_DEVICE_VTX) + return; + + if (vtx_actual.power_table.levels == 0) + return; + + msp_vtx_detected = true; +} + static void msp_process_serial_cmd(msp_t *msp, msp_magic_t magic, uint16_t cmd, uint8_t *payload, uint16_t size) { switch (cmd) { case MSP_API_VERSION: { @@ -333,6 +343,7 @@ static void msp_process_serial_cmd(msp_t *msp, msp_magic_t magic, uint16_t cmd, #endif #ifdef USE_VTX case MSP_VTX_CONFIG: { + msp_check_vtx_detected(msp); msp_vtx_send_config_reply(msp, magic); break; } @@ -404,6 +415,7 @@ static void msp_process_serial_cmd(msp_t *msp, msp_magic_t magic, uint16_t cmd, vtx_set(settings); } + msp_check_vtx_detected(msp); msp_send_reply(msp, magic, cmd, NULL, 0); break; } @@ -505,7 +517,7 @@ static void msp_process_serial_cmd(msp_t *msp, msp_magic_t magic, uint16_t cmd, case MSP_EEPROM_WRITE: { #ifdef USE_VTX if (msp->device == MSP_DEVICE_VTX) { - msp_vtx_detected = 1; + msp_check_vtx_detected(msp); } else #endif if (!flags.arm_state && msp->device != MSP_DEVICE_SPI_RX) { diff --git a/src/io/rgb_led.c b/src/io/rgb_led.c index f02b19202..df85fdfba 100644 --- a/src/io/rgb_led.c +++ b/src/io/rgb_led.c @@ -1,168 +1,68 @@ -#include +#include "io/rgb_led.h" #include "core/project.h" #include "driver/rgb_led.h" -#include "driver/time.h" #include "flight/control.h" +#include "flight/filter.h" #include "util/util.h" -// normal flight rgb colour - LED switch ON -#define RGB_VALUE_INFLIGHT_ON RGB(255, 255, 255) +#define RGB_LED_COUNT 0 +#define RGB_FILTER_TIME FILTERCALC(100, 1000000) -// normal flight rgb colour - LED switch OFF -#define RGB_VALUE_INFLIGHT_OFF RGB(0, 0, 0) +static uint32_t rgb_led_fade_color(uint32_t color, float fade_coeff) { + static float g_filt = 0; + const uint32_t g = (color >> 16) & 0xff; + lpf(&g_filt, g, fade_coeff); -// colour before bind -#define RGB_VALUE_BEFORE_BIND RGB(0, 128, 128) + static float r_filt = 0; + const uint32_t r = (color >> 8) & 0xff; + lpf(&r_filt, r, fade_coeff); -// fade from one color to another when changed -#define RGB_FILTER_ENABLE -#define RGB_FILTER_TIME_MICROSECONDS 50e3 + static float b_filt = 0; + const uint32_t b = (color >> 0) & 0xff; + lpf(&b_filt, b, fade_coeff); -// runs the update once every 16 loop times ( 16 mS ) -#define DOWNSAMPLE 16 - -#define RGB_FILTER_TIME lpfcalc(1000 * DOWNSAMPLE, RGB_FILTER_TIME_MICROSECONDS) -#define RGB(r, g, b) ((((int)g & 0xff) << 16) | (((int)r & 0xff) << 8) | ((int)b & 0xff)) - -#if (RGB_LED_NUMBER > 0) - -// array with individual led brightnesses -int rgb_led_value[RGB_LED_NUMBER]; -// loop count for downsampling -int rgb_loopcount = 0; -// rgb low pass filter variables -float r_filt, g_filt, b_filt; - -// sets all leds to a brightness -void rgb_led_set_all(int rgb) { -#ifdef RGB_FILTER_ENABLE - // deconstruct the colour into components - int g = rgb >> 16; - int r = (rgb & 0x0000FF00) >> 8; - int b = rgb & 0xff; - - // filter individual colors - lpf(&r_filt, r, RGB_FILTER_TIME); - lpf(&g_filt, g, RGB_FILTER_TIME); - lpf(&b_filt, b, RGB_FILTER_TIME); - - int temp = RGB(r_filt, g_filt, b_filt); - - for (int i = 0; i < RGB_LED_NUMBER; i++) - rgb_led_value[i] = temp; - -#else - for (int i = 0; i < RGB_LED_NUMBER; i++) - rgb_led_value[i] = rgb; -#endif -} - -// set an individual led brightness -void rgb_led_set_one(int led_number, int rgb) { - rgb_led_value[led_number] = rgb; + return RGB(r_filt, g_filt, b_filt); } -// flashes between 2 colours, duty cycle 1 - 15 -void rgb_ledflash(int color1, int color2, uint32_t period, int duty) { - if (time_micros() % period > (period * duty) >> 4) { - rgb_led_set_all(color1); +// flashes between 2 colours, duty cycle 1 - 16 +static uint32_t rgb_ledflash(uint32_t color1, uint32_t color2, uint32_t period_ms, uint8_t duty) { + const uint32_t period = period_ms * 1000; + const uint32_t divider = (period * duty) >> 5; + if (time_micros() % period > divider) { + return rgb_led_fade_color(color1, lpfcalc(divider, period)); } else { - rgb_led_set_all(color2); + return rgb_led_fade_color(color2, lpfcalc(divider, period)); } } -// speed of movement -float KR_SPEED = 0.005f * DOWNSAMPLE; - -float kr_position = 0; -int kr_dir = 0; - -// knight rider style led movement -void rgb_knight_rider() { - if (kr_dir) { - kr_position += KR_SPEED; - if (kr_position > RGB_LED_NUMBER - 1) - kr_dir = !kr_dir; - } else { - kr_position -= KR_SPEED; - if (kr_position < 0) - kr_dir = !kr_dir; +void rgb_led_update() { +#if defined(USE_RGB_LED) + if (RGB_LED_COUNT == 0 || rgb_led_busy()) { + return; } - // calculate led value - for (int i = 0; i < RGB_LED_NUMBER; i++) { - float led_bright = fabsf((float)i - kr_position); - if (led_bright > 1.0f) - led_bright = 1.0f; - led_bright = 1.0f - led_bright; + uint32_t new_value = 0; - // set a green background as well, 32 brightness - rgb_led_set_one(i, RGB((led_bright * 255.0f), (32.0f - led_bright * 32.0f), 0)); + if (flags.lowbatt) { + new_value = rgb_ledflash(RGB(255, 0, 0), RGB(0, 0, 255), 500, 16); + } + if (flags.rx_mode == RXMODE_BIND) { + new_value = rgb_ledflash(RGB(255, 0, 0), RGB(0, 0, 255), 100, 24); + } + if (flags.failsafe) { + new_value = rgb_ledflash(RGB(255, 0, 0), RGB(0, 0, 255), 500, 30); + } + if (flags.arm_switch && (flags.throttle_safety == 1 || flags.arm_safety == 1)) { + new_value = rgb_ledflash(RGB(255, 0, 0), RGB(0, 0, 255), 100, 8); } -} -// 2 led flasher -void rgb_ledflash_twin(int color1, int color2, uint32_t period) { - if (time_micros() % period > (period / 2)) { - for (int i = 0; i < RGB_LED_NUMBER; i++) { - if ((i / 2) * 2 == i) - rgb_led_set_one(i, color1); - else - rgb_led_set_one(i, color2); - } + static uint32_t current_value = 0; + if (current_value != new_value) { + rgb_led_set_value(new_value, RGB_LED_COUNT); + current_value = new_value; } else { - for (int i = 0; i < RGB_LED_NUMBER; i++) { - if ((i / 2) * 2 == i) - rgb_led_set_one(i, color2); - else - rgb_led_set_one(i, color1); - } + rgb_led_send(); } -} - -// main function -void rgb_led_lvc() { - rgb_loopcount++; - if (rgb_loopcount > DOWNSAMPLE) { - rgb_loopcount = 0; - // led flash logic - if (flags.lowbatt) { - // rgb_led_set_all( RGB( 255 , 0 , 0 ) ); - rgb_ledflash(RGB(255, 0, 0), RGB(255, 32, 0), 500000, 8); - } else { - if (flags.rx_mode == RXMODE_BIND) { - // bind mode - // rgb_ledflash ( RGB( 0 , 0 , 255 ), RGB( 0 , 128 , 0 ), 1000000, 12); - // rgb_ledflash_twin( RGB( 0 , 0 , 255 ), RGB( 0 , 128 , 0 ), 1000000); - rgb_led_set_all(RGB_VALUE_BEFORE_BIND); - // rgb_knight_rider(); - } else { // non bind - if (flags.failsafe) { - // failsafe flash - rgb_ledflash(RGB(0, 128, 0), RGB(0, 0, 128), 500000, 8); - // rgb_led_set_all( RGB( 0 , 128 , 128 ) ); - } else { - - if (rx_aux_on(AUX_LEDS_ON)) - - rgb_led_set_all(RGB_VALUE_INFLIGHT_ON); - else - rgb_led_set_all(RGB_VALUE_INFLIGHT_OFF); - } - } - } - -#ifdef RGB_LED_DMA - // send dma start signal - rgb_send(0); -#else - // send data to leds - for (int i = 0; i < RGB_LED_NUMBER; i++) { - rgb_send(rgb_led_value[i]); - } #endif - } } - -#endif diff --git a/src/io/rgb_led.h b/src/io/rgb_led.h index 686b389ac..921d3fee2 100644 --- a/src/io/rgb_led.h +++ b/src/io/rgb_led.h @@ -1,3 +1,5 @@ #pragma once -void rgb_led_lvc(); \ No newline at end of file +#define RGB(r, g, b) ((((uint32_t)g & 0xff) << 16) | (((uint32_t)r & 0xff) << 8) | ((uint32_t)b & 0xff)) + +void rgb_led_update(); \ No newline at end of file diff --git a/src/io/vtx.c b/src/io/vtx.c index 90fa8535d..798307aa8 100644 --- a/src/io/vtx.c +++ b/src/io/vtx.c @@ -9,9 +9,6 @@ #include "driver/gpio.h" #include "driver/serial.h" #include "driver/time.h" -#include "driver/vtx/msp.h" -#include "driver/vtx/sa.h" -#include "driver/vtx/tramp.h" #include "flight/control.h" #include "rx/rx.h" #include "rx/unified_serial.h" @@ -31,30 +28,12 @@ vtx_settings_t vtx_actual; static uint8_t apply_tries = 0; static uint32_t vtx_delay_start = 0; -static uint32_t vtx_delay_ms = 1000; +static uint32_t vtx_delay_ms = 100; -extern uint8_t smart_audio_detected; -extern smart_audio_settings_t smart_audio_settings; - -vtx_detect_status_t vtx_smart_audio_update(vtx_settings_t *actual); -void smart_audio_set_frequency(vtx_band_t band, vtx_channel_t channel); -void smart_audio_set_power_level(vtx_power_level_t power); -void smart_audio_set_pit_mode(vtx_pit_mode_t pit_mode); - -extern uint8_t tramp_detected; -extern tramp_settings_t tramp_settings; - -vtx_detect_status_t vtx_tramp_update(vtx_settings_t *actual); -void tramp_set_frequency(vtx_band_t band, vtx_channel_t channel); -void tramp_set_power_level(vtx_power_level_t power); -void tramp_set_pit_mode(vtx_pit_mode_t pit_mode); - -extern uint8_t msp_vtx_detected; - -vtx_detect_status_t vtx_msp_update(vtx_settings_t *actual); -void msp_vtx_set_frequency(vtx_band_t band, vtx_channel_t channel); -void msp_vtx_set_power_level(vtx_power_level_t power); -void msp_vtx_set_pit_mode(vtx_pit_mode_t pit_mode); +static const vtx_device_t *vtx_device = NULL; +extern const vtx_device_t msp_vtx_device; +extern const vtx_device_t smart_audio_vtx_device; +extern const vtx_device_t tramp_vtx_device; const uint16_t frequency_table[VTX_BAND_MAX][VTX_CHANNEL_MAX] = { {5865, 5845, 5825, 5805, 5785, 5765, 5745, 5725}, // VTX_BAND_A @@ -89,25 +68,6 @@ vtx_power_level_t vtx_power_level_index(vtx_power_table_t *power_table, uint16_t return VTX_POWER_LEVEL_1; } -static vtx_detect_status_t vtx_update_protocol(vtx_protocol_t proto, vtx_settings_t *actual) { - switch (proto) { - case VTX_PROTOCOL_TRAMP: - return vtx_tramp_update(actual); - - case VTX_PROTOCOL_SMART_AUDIO: - return vtx_smart_audio_update(actual); - - case VTX_PROTOCOL_MSP_VTX: - return vtx_msp_update(actual); - - case VTX_PROTOCOL_INVALID: - case VTX_PROTOCOL_MAX: - return VTX_DETECT_ERROR; - } - - return VTX_DETECT_ERROR; -} - void vtx_init() { vtx_settings.detected = VTX_PROTOCOL_INVALID; vtx_delay_start = time_millis(); @@ -126,7 +86,7 @@ static void vtx_update_fpv_pin() { // fpv switch on if (!fpv_init && flags.rx_mode == RXMODE_NORMAL && flags.on_ground == 1) { fpv_init = gpio_init_fpv(flags.rx_mode); - vtx_delay_ms = 1000; + vtx_delay_ms = 100; vtx_connect_tries = 0; } if (fpv_init) { @@ -140,7 +100,7 @@ static void vtx_update_fpv_pin() { } static bool vtx_detect_protocol() { - static vtx_protocol_t protocol_to_check = VTX_PROTOCOL_MSP_VTX; + static vtx_protocol_t protocol_to_check = VTX_PROTOCOL_SMART_AUDIO; static uint8_t protocol_is_init = 0; if (vtx_settings.detected) { @@ -164,34 +124,33 @@ static bool vtx_detect_protocol() { if (!protocol_is_init) { switch (protocol_to_check) { case VTX_PROTOCOL_TRAMP: - serial_tramp_init(); + vtx_device = &tramp_vtx_device; break; case VTX_PROTOCOL_SMART_AUDIO: - serial_smart_audio_init(); + vtx_device = &smart_audio_vtx_device; break; case VTX_PROTOCOL_MSP_VTX: - serial_msp_vtx_init(); + vtx_device = &msp_vtx_device; break; - case VTX_PROTOCOL_INVALID: - case VTX_PROTOCOL_MAX: - break; + default: + vtx_device = NULL; + return false; } + vtx_device->init(); protocol_is_init = 1; - return false; } - const vtx_detect_status_t status = vtx_update_protocol(protocol_to_check, &vtx_actual); - + const vtx_detect_status_t status = vtx_device->update(&vtx_actual); if (status == VTX_DETECT_SUCCESS) { // detect success, save detected proto vtx_settings.protocol = protocol_to_check; } else if (status == VTX_DETECT_ERROR) { vtx_connect_tries = 0; protocol_is_init = 0; - vtx_delay_ms = 500; + vtx_delay_ms = 100; if (vtx_settings.protocol == VTX_PROTOCOL_INVALID) { // only switch protocol if we are not fixed to one @@ -202,7 +161,6 @@ static bool vtx_detect_protocol() { } } } - return false; } @@ -220,26 +178,10 @@ static bool vtx_update_frequency() { return true; } - switch (vtx_settings.detected) { - case VTX_PROTOCOL_TRAMP: - tramp_set_frequency(vtx_settings.band, vtx_settings.channel); - break; - - case VTX_PROTOCOL_SMART_AUDIO: - smart_audio_set_frequency(vtx_settings.band, vtx_settings.channel); - break; - - case VTX_PROTOCOL_MSP_VTX: - msp_vtx_set_frequency(vtx_settings.band, vtx_settings.channel); - break; - - case VTX_PROTOCOL_INVALID: - case VTX_PROTOCOL_MAX: - break; + if (vtx_device->set_frequency(vtx_settings.band, vtx_settings.channel)) { + vtx_delay_ms = 10; + apply_tries++; } - - apply_tries++; - vtx_delay_ms = 10; return false; } @@ -256,27 +198,10 @@ static bool vtx_update_powerlevel() { return true; } - switch (vtx_settings.detected) { - case VTX_PROTOCOL_TRAMP: - tramp_set_power_level(vtx_settings.power_level); - break; - - case VTX_PROTOCOL_SMART_AUDIO: - smart_audio_set_power_level(vtx_settings.power_level); - break; - - case VTX_PROTOCOL_MSP_VTX: - msp_vtx_set_power_level(vtx_settings.power_level); - break; - - case VTX_PROTOCOL_INVALID: - case VTX_PROTOCOL_MAX: - break; + if (vtx_device->set_power_level(vtx_settings.power_level)) { + vtx_delay_ms = 10; + apply_tries++; } - - apply_tries++; - vtx_delay_ms = 10; - return false; } @@ -293,27 +218,10 @@ static bool vtx_update_pitmode() { return true; } - switch (vtx_settings.detected) { - case VTX_PROTOCOL_TRAMP: - tramp_set_pit_mode(vtx_settings.pit_mode); - break; - - case VTX_PROTOCOL_SMART_AUDIO: - smart_audio_set_pit_mode(vtx_settings.pit_mode); - break; - - case VTX_PROTOCOL_MSP_VTX: - msp_vtx_set_pit_mode(vtx_settings.pit_mode); - break; - - case VTX_PROTOCOL_INVALID: - case VTX_PROTOCOL_MAX: - break; + if (vtx_device->set_pit_mode(vtx_settings.pit_mode)) { + vtx_delay_ms = 10; + apply_tries++; } - - apply_tries++; - vtx_delay_ms = 10; - return false; } @@ -348,8 +256,11 @@ void vtx_update() { if (!vtx_detect_protocol()) { return; } + if (vtx_device == NULL) { + return; + } - const vtx_detect_status_t status = vtx_update_protocol(vtx_settings.detected, &vtx_actual); + const vtx_detect_status_t status = vtx_device->update(&vtx_actual); if (status < VTX_DETECT_SUCCESS) { // we are in wait or error state, do nothing return; @@ -385,12 +296,6 @@ void vtx_set(vtx_settings_t *vtx) { vtx_settings.protocol = vtx->protocol; vtx_settings.magic = 0xFFFF; vtx_settings.power_table.levels = 0; - - smart_audio_settings.version = 0; - smart_audio_detected = 0; - tramp_settings.freq_min = 0; - tramp_detected = 0; - msp_vtx_detected = 0; } else { vtx_settings.magic = VTX_SETTINGS_MAGIC; memcpy(&vtx_settings.power_table, &vtx->power_table, sizeof(vtx_power_table_t)); diff --git a/src/io/vtx.h b/src/io/vtx.h index 84263a583..7f38a4b26 100644 --- a/src/io/vtx.h +++ b/src/io/vtx.h @@ -10,7 +10,6 @@ typedef enum { VTX_DETECT_WAIT, VTX_DETECT_ERROR, VTX_DETECT_SUCCESS, - VTX_DETECT_UPDATE } vtx_detect_status_t; typedef enum { @@ -103,6 +102,14 @@ typedef struct { MEMBER(power_level, uint8_t) \ MEMBER(power_table, vtx_power_table_t) +typedef struct { + void (*init)(void); + vtx_detect_status_t (*update)(vtx_settings_t *); + bool (*set_frequency)(vtx_band_t, vtx_channel_t); + bool (*set_power_level)(vtx_power_level_t); + bool (*set_pit_mode)(vtx_pit_mode_t); +} vtx_device_t; + extern vtx_settings_t vtx_settings; void vtx_init(); diff --git a/src/io/vtx_msp.c b/src/io/vtx_msp.c index 35c41b053..242c13592 100644 --- a/src/io/vtx_msp.c +++ b/src/io/vtx_msp.c @@ -4,8 +4,10 @@ #include "core/profile.h" #include "driver/serial.h" -#include "driver/vtx/msp.h" +#include "driver/vtx.h" #include "io/msp.h" +#include "rx/unified_serial.h" +#include "util/crc.h" #ifdef USE_VTX @@ -32,7 +34,10 @@ typedef struct { extern uint8_t vtx_connect_tries; extern vtx_settings_t vtx_actual; -uint8_t msp_vtx_detected = 0; +extern msp_t displayport_msp; +extern msp_t crsf_msp; + +bool msp_vtx_detected = false; msp_t *msp_vtx; char msp_vtx_band_letters[VTX_BAND_MAX] = {'A', 'B', 'E', 'F', 'R', 'L'}; @@ -79,53 +84,162 @@ void msp_vtx_send_config_reply(msp_t *msp, msp_magic_t magic) { msp_send_reply(msp, magic, MSP_VTX_CONFIG, (uint8_t *)&config, sizeof(msp_vtx_config_t)); } -vtx_detect_status_t vtx_msp_update(vtx_settings_t *actual) { +static void serial_msp_send(msp_magic_t magic, uint8_t direction, uint16_t cmd, const uint8_t *data, uint16_t len) { + if (magic == MSP2_MAGIC) { + const uint32_t size = len + MSP2_HEADER_LEN + 1; + uint8_t vtx_frame[size]; + + vtx_frame[0] = '$'; + vtx_frame[1] = MSP2_MAGIC; + vtx_frame[2] = direction; + vtx_frame[3] = 0; // flag + vtx_frame[4] = (cmd >> 0) & 0xFF; + vtx_frame[5] = (cmd >> 8) & 0xFF; + vtx_frame[6] = (len >> 0) & 0xFF; + vtx_frame[7] = (len >> 8) & 0xFF; + + memcpy(vtx_frame + MSP2_HEADER_LEN, data, len); + vtx_frame[len + MSP2_HEADER_LEN] = crc8_dvb_s2_data(0, vtx_frame + 3, len + 5); + + serial_vtx_send_data(vtx_frame, size); + } else { + const uint32_t size = len + MSP_HEADER_LEN + 1; + uint8_t vtx_frame[size]; + + vtx_frame[0] = '$'; + vtx_frame[1] = MSP1_MAGIC; + vtx_frame[2] = direction; + vtx_frame[3] = len; + vtx_frame[4] = cmd; + + memcpy(vtx_frame + MSP_HEADER_LEN, data, len); + + uint8_t chksum = len; + for (uint8_t i = 4; i < (size - 1); i++) { + chksum ^= vtx_frame[i]; + } + vtx_frame[len + MSP_HEADER_LEN] = chksum; + + serial_vtx_send_data(vtx_frame, size); + } +} + +static uint8_t msp_rx_buffer[128]; +static msp_t msp = { + .buffer = msp_rx_buffer, + .buffer_size = 128, + .buffer_offset = 0, + .send = serial_msp_send, + .device = MSP_DEVICE_VTX, +}; + +static void msp_vtx_init() { + msp_vtx_detected = false; + + if (serial_displayport.config.port != SERIAL_PORT_INVALID) { + // reuse existing msp for hdz + msp_vtx = &displayport_msp; + return; + } + + if (profile.serial.smart_audio == profile.serial.rx && + serial_rx_detected_protcol == RX_SERIAL_PROTOCOL_CRSF) { + msp_vtx = &crsf_msp; + return; + } + + const target_serial_port_t *dev = serial_get_dev(profile.serial.smart_audio); + if (!target_serial_port_valid(dev)) { + return; + } + + serial_port_config_t config; + config.port = profile.serial.smart_audio; + config.baudrate = 9600; + config.direction = SERIAL_DIR_TX_RX; + config.stop_bits = SERIAL_STOP_BITS_1; + config.invert = false; + config.half_duplex = true; + config.half_duplex_pp = false; + + serial_vtx_wait_for_ready(); + serial_init(&serial_vtx, config); + + msp_vtx = &msp; +} + +static bool msp_vtx_feed_parser() { + if (serial_displayport.config.port != SERIAL_PORT_INVALID) { + // handled by digital vtx + return false; + } + if (profile.serial.smart_audio != SERIAL_PORT_INVALID && + profile.serial.smart_audio == profile.serial.rx && + serial_rx_detected_protcol == RX_SERIAL_PROTOCOL_CRSF) { + // handled by telemetry + return false; + } + if (!serial_vtx_is_ready()) { + return false; + } + + uint8_t data = 0; + while (serial_vtx_read_byte(&data)) { + msp_process_serial(msp_vtx, data); + } + + // did we time out? + return msp_vtx->buffer_offset && (time_millis() - vtx_last_valid_read) > 500; +} + +static vtx_detect_status_t msp_vtx_update(vtx_settings_t *actual) { if (vtx_connect_tries > MSP_VTX_DETECT_TRIES) { return VTX_DETECT_ERROR; } - - const vtx_update_result_t result = serial_msp_vtx_update(); - switch (result) { - case VTX_ERROR: + if (msp_vtx_feed_parser()) { vtx_connect_tries++; - case VTX_WAIT: return VTX_DETECT_WAIT; + } + if (!msp_vtx_detected) { + return VTX_DETECT_WAIT; + } - case VTX_SUCCESS: - case VTX_IDLE: - if (!msp_vtx_detected) { - return VTX_DETECT_WAIT; - } - + if (vtx_settings.detected != VTX_PROTOCOL_MSP_VTX) { if (vtx_settings.magic != VTX_SETTINGS_MAGIC) { vtx_set(actual); } - memcpy(&vtx_settings.power_table, &actual->power_table, sizeof(vtx_power_table_t)); - vtx_settings.detected = VTX_PROTOCOL_MSP_VTX; vtx_connect_tries = 0; - return VTX_DETECT_SUCCESS; } - - // wait otherwise - return VTX_DETECT_WAIT; + return VTX_DETECT_SUCCESS; } -void msp_vtx_set_frequency(vtx_band_t band, vtx_channel_t channel) { +static bool msp_vtx_set_frequency(vtx_band_t band, vtx_channel_t channel) { vtx_actual.band = band; vtx_actual.channel = channel; msp_vtx_send_config_reply(msp_vtx, MSP2_MAGIC); + return true; } -void msp_vtx_set_power_level(vtx_power_level_t power) { +static bool msp_vtx_set_power_level(vtx_power_level_t power) { vtx_actual.power_level = power; msp_vtx_send_config_reply(msp_vtx, MSP2_MAGIC); + return true; } -void msp_vtx_set_pit_mode(vtx_pit_mode_t pit_mode) { +static bool msp_vtx_set_pit_mode(vtx_pit_mode_t pit_mode) { vtx_actual.pit_mode = pit_mode; msp_vtx_send_config_reply(msp_vtx, MSP2_MAGIC); + return true; } +const vtx_device_t msp_vtx_device = { + .init = msp_vtx_init, + .update = msp_vtx_update, + .set_frequency = msp_vtx_set_frequency, + .set_power_level = msp_vtx_set_power_level, + .set_pit_mode = msp_vtx_set_pit_mode, +}; + #endif \ No newline at end of file diff --git a/src/io/vtx_smartaudio.c b/src/io/vtx_smartaudio.c index 1d591c840..a1e0333d9 100644 --- a/src/io/vtx_smartaudio.c +++ b/src/io/vtx_smartaudio.c @@ -3,27 +3,79 @@ #include #include +#include "core/debug.h" +#include "core/profile.h" #include "driver/serial.h" -#include "driver/vtx/sa.h" +#include "driver/vtx.h" +#include "util/crc.h" #include "util/util.h" #ifdef USE_VTX +typedef enum { + SA_CMD_GET_SETTINGS = 0x01, + SA_CMD_SET_POWER = 0x02, + SA_CMD_SET_CHANNEL = 0x03, + SA_CMD_SET_FREQUENCY = 0x04, + SA_CMD_SET_MODE = 0x05, + SA_CMD_GET_SETTINGS_V2 = 0x9, + SA_CMD_GET_SETTINGS_V21 = 0x11, +} smart_audio_cmd_t; + +typedef enum { + SA_MODE_FREQUENCY = 1 << 0, + SA_MODE_PIT = 1 << 1, + SA_MODE_IN_RANGE_PIT = 1 << 2, + SA_MODE_OUT_RANGE_PIT = 1 << 3, + SA_MODE_UNLOCKED = 1 << 4, +} smart_mode_t; + +typedef enum { + PARSER_IDLE, + PARSER_READ_MAGIC_1, + PARSER_READ_MAGIC_2, + PARSER_READ_CMD, + PARSER_READ_LENGTH, + PARSER_READ_PAYLOAD, + PARSER_READ_CRC, +} smart_audio_parser_state_t; + #define SMART_AUDIO_DETECT_TRIES 30 -extern uint8_t vtx_connect_tries; +#define SMART_AUDIO_BAUDRATE_MIN 4650 +#define SMART_AUDIO_BAUDRATE_MAX 5050 -uint8_t smart_audio_detected = 0; -extern smart_audio_settings_t smart_audio_settings; +#define SA_HEADER_SIZE 5 +#define SA_MAGIC_1 0xaa +#define SA_MAGIC_2 0x55 -static bool smart_audio_needs_update = false; +static uint8_t smart_audio_version = 0; +static uint32_t baud_rate = 4800; +static uint32_t packets_sent = 0; +static uint32_t packets_recv = 0; + +// for reasons unknown to mankind, betafpv decided to make the length field of the get settings response +// and _only_ the get settings response one longer than everything else, throwing off the parser. +// now we account for that stipidity with this variable. +static bool betafpv_workaround = false; + +extern vtx_settings_t vtx_actual; +extern uint8_t vtx_connect_tries; -static const char smart_audio_power_level_labels[4][VTX_POWER_LABEL_LEN] = { +static bool smart_audio_needs_update = false; +static smart_audio_parser_state_t parser_state = PARSER_IDLE; +static const char default_power_level_labels[4][VTX_POWER_LABEL_LEN] = { "25 ", "200 ", "500 ", "800 ", }; +static const uint8_t default_dac_power_levels[4] = { + 7, + 16, + 25, + 40, +}; static uint32_t smart_audio_dbi_to_mw(uint16_t dbi) { uint16_t mw = (uint16_t)powf(10.0f, dbi / 10.0f); @@ -48,58 +100,150 @@ static void smart_audio_write_mw(char *buf, uint32_t val) { *ptr++ = ' '; } -vtx_detect_status_t vtx_smart_audio_update(vtx_settings_t *actual) { - if (smart_audio_settings.version == 0 && vtx_connect_tries > SMART_AUDIO_DETECT_TRIES) { - return VTX_DETECT_ERROR; +static void smart_audio_init() { + const target_serial_port_t *dev = serial_get_dev(profile.serial.smart_audio); + if (!target_serial_port_valid(dev)) { + return; } - const vtx_update_result_t result = serial_smart_audio_update(); + serial_port_config_t config; + config.port = profile.serial.smart_audio; + config.baudrate = baud_rate; + config.direction = SERIAL_DIR_TX_RX; + config.stop_bits = SERIAL_STOP_BITS_2; + config.invert = false; + config.half_duplex = true; + config.half_duplex_pp = true; + + serial_vtx_wait_for_ready(); + serial_init(&serial_vtx, config); +} - if ((result == VTX_IDLE || result == VTX_ERROR) && smart_audio_settings.version == 0 && vtx_connect_tries <= SMART_AUDIO_DETECT_TRIES) { - // no smart audio detected, try again - serial_smart_audio_send_payload(SA_CMD_GET_SETTINGS, NULL, 0); - vtx_connect_tries++; - return VTX_DETECT_WAIT; +static void smart_audio_auto_baud() { + static uint8_t last_percent = 0; + + // move quickly while we have not yet found a working baud rate + const uint8_t current_percent = ((packets_recv * 100) / packets_sent); + if (packets_sent < (last_percent == 0 ? 3 : 10)) { + last_percent = current_percent; + return; } - if (result == VTX_SUCCESS) { - int8_t channel_index = -1; - if (smart_audio_settings.mode & SA_MODE_FREQUENCY) { - channel_index = vtx_find_frequency_index(smart_audio_settings.frequency); - } else { - channel_index = smart_audio_settings.channel; + if (current_percent < 70) { + static int8_t direction = 1; + + // if the percentage degraded, switch it up + if (last_percent > current_percent) { + direction = direction == 1 ? -1 : 1; } - if (channel_index >= 0) { - actual->band = channel_index / VTX_CHANNEL_MAX; - actual->channel = channel_index % VTX_CHANNEL_MAX; + if ((direction == 1) && (baud_rate == SMART_AUDIO_BAUDRATE_MAX)) { + direction = -1; + } else if ((direction == -1 && baud_rate == SMART_AUDIO_BAUDRATE_MIN)) { + direction = 1; } - actual->power_table.levels = smart_audio_settings.level_count; - memcpy(actual->power_table.values, smart_audio_settings.dac_power_levels, sizeof(smart_audio_settings.dac_power_levels)); - if (smart_audio_settings.version == 3) { - for (uint32_t i = 0; i < smart_audio_settings.level_count; i++) { - smart_audio_write_mw(actual->power_table.labels[i], smart_audio_dbi_to_mw(smart_audio_settings.dac_power_levels[i])); + betafpv_workaround = !betafpv_workaround; + baud_rate += direction * 50; + quic_debugf("SMART_AUDIO: auto baud %d (%d) change %d vs %d", baud_rate, direction * 50, last_percent, current_percent); + smart_audio_init(); + } + + last_percent = current_percent; + packets_sent = 0; + packets_recv = 0; +} + +static bool smart_audio_send_payload(uint8_t cmd, const uint8_t *payload, const uint32_t size) { + if (!serial_vtx_is_ready()) { + return false; + } + +#ifdef USE_AKK_SA_WORKAROUND +#define EXTRA_DUMMY_BYTES 1 +#else +#define EXTRA_DUMMY_BYTES 0 +#endif + + const uint32_t len = size + 1 + SA_HEADER_SIZE + EXTRA_DUMMY_BYTES; + uint8_t vtx_frame[len]; + + vtx_frame[0] = 0x00; + vtx_frame[1] = 0xAA; + vtx_frame[2] = 0x55; + vtx_frame[3] = (cmd << 1) | 0x1; + vtx_frame[4] = size; + for (uint8_t i = 0; i < size; i++) { + vtx_frame[i + SA_HEADER_SIZE] = payload[i]; + } + vtx_frame[size + SA_HEADER_SIZE] = crc8_dvb_s2_data(0, vtx_frame + 1, len - 2 - EXTRA_DUMMY_BYTES); +#ifdef USE_AKK_SA_WORKAROUND + vtx_frame[size + 1 + SA_HEADER_SIZE] = 0x00; +#endif + + smart_audio_auto_baud(); + + if (!serial_vtx_send_data(vtx_frame, len)) { + return false; + } + + parser_state = PARSER_READ_MAGIC_1; + packets_sent++; + + return true; +} + +static void smart_audio_parse_packet(vtx_settings_t *actual, uint8_t cmd, uint8_t *payload, uint32_t length) { + switch (cmd) { + case SA_CMD_GET_SETTINGS: + case SA_CMD_GET_SETTINGS_V2: + case SA_CMD_GET_SETTINGS_V21: { + smart_audio_version = (cmd == SA_CMD_GET_SETTINGS ? 1 : (cmd == SA_CMD_GET_SETTINGS_V2 ? 2 : 3)); + + const uint8_t channel = payload[0]; + uint8_t power = payload[1]; + const uint8_t mode = payload[2]; + const uint16_t frequency = (uint16_t)(((uint16_t)payload[3] << 8) | payload[4]); + + const uint8_t channel_index = mode & SA_MODE_FREQUENCY ? vtx_find_frequency_index(frequency) : channel; + actual->band = channel_index / VTX_CHANNEL_MAX; + actual->channel = channel_index % VTX_CHANNEL_MAX; + + if (cmd == SA_CMD_GET_SETTINGS_V21) { + power = payload[5]; + + // SmartAudio seems to report buf[8] + 1 power levels, but one of them is zero. + // zero is indeed a valid power level to set the vtx to, but it activates pit mode. + // crucially, after sending 0 dbm, the vtx does NOT report its power level to be 0 dbm. + // instead, it reports whatever value was set previously and it reports to be in pit mode. + actual->power_table.levels = min(payload[6], VTX_POWER_LEVEL_MAX); + for (uint8_t i = 0; i < actual->power_table.levels; i++) { + actual->power_table.values[i] = payload[7 + i + 1]; //+ 1 to skip the first power level, as mentioned above } } else { - memcpy(actual->power_table.labels, smart_audio_power_level_labels, sizeof(smart_audio_power_level_labels)); + actual->power_table.levels = 4; + for (uint8_t i = 0; i < actual->power_table.levels; i++) { + actual->power_table.values[i] = default_dac_power_levels[i]; + } } - if (smart_audio_settings.version == 2) { - actual->power_level = min(smart_audio_settings.power, VTX_POWER_LEVEL_MAX - 1); + if (smart_audio_version >= 3) { + for (uint32_t i = 0; i < actual->power_table.levels; i++) { + smart_audio_write_mw(actual->power_table.labels[i], smart_audio_dbi_to_mw(actual->power_table.values[i])); + } + actual->pit_mode = (mode & SA_MODE_PIT) ? VTX_PIT_MODE_ON : VTX_PIT_MODE_OFF; } else { - actual->power_level = vtx_power_level_index(&actual->power_table, smart_audio_settings.power); + memcpy(actual->power_table.labels, default_power_level_labels, sizeof(default_power_level_labels)); + actual->pit_mode = VTX_PIT_MODE_NO_SUPPORT; } - if (smart_audio_settings.version >= 3) { - actual->pit_mode = smart_audio_settings.mode & SA_MODE_PIT ? 1 : 0; + if (smart_audio_version == 2) { + actual->power_level = min(power, VTX_POWER_LEVEL_MAX - 1); } else { - actual->pit_mode = VTX_PIT_MODE_NO_SUPPORT; + actual->power_level = vtx_power_level_index(&actual->power_table, power); } - if (smart_audio_settings.version != 0 && smart_audio_detected == 0) { - smart_audio_detected = 1; - + if (vtx_settings.detected != VTX_PROTOCOL_SMART_AUDIO) { if (vtx_settings.magic != VTX_SETTINGS_MAGIC) { vtx_set(actual); } @@ -107,26 +251,133 @@ vtx_detect_status_t vtx_smart_audio_update(vtx_settings_t *actual) { vtx_settings.detected = VTX_PROTOCOL_SMART_AUDIO; vtx_connect_tries = 0; } + break; + } + case SA_CMD_SET_FREQUENCY: { + const uint8_t channel_index = vtx_find_frequency_index((uint16_t)(((uint16_t)payload[0] << 8) | payload[1])); + actual->band = channel_index / VTX_CHANNEL_MAX; + actual->channel = channel_index % VTX_CHANNEL_MAX; + break; + } - if (smart_audio_needs_update) { - serial_smart_audio_send_payload(SA_CMD_GET_SETTINGS, NULL, 0); - smart_audio_needs_update = false; + case SA_CMD_SET_CHANNEL: { + const uint8_t channel_index = payload[0]; + actual->band = channel_index / VTX_CHANNEL_MAX; + actual->channel = channel_index % VTX_CHANNEL_MAX; + break; + } + + case SA_CMD_SET_POWER: { + const uint8_t power = payload[0]; + if (smart_audio_version == 2) { + actual->power_level = min(power, VTX_POWER_LEVEL_MAX - 1); + } else { + actual->power_level = vtx_power_level_index(&actual->power_table, power); + } + break; + } + case SA_CMD_SET_MODE: { + // const uint8_t mode = payload[0]; + break; + } + default: + break; + } + + packets_recv++; +} + +static vtx_detect_status_t smart_audio_update(vtx_settings_t *actual) { + if (!serial_vtx_is_ready()) { + return VTX_DETECT_WAIT; + } + if (vtx_connect_tries > SMART_AUDIO_DETECT_TRIES) { + return VTX_DETECT_ERROR; + } + + static uint8_t length = 0; + + while (parser_state > PARSER_IDLE) { + if ((time_millis() - vtx_last_valid_read) > 500) { + parser_state = PARSER_IDLE; + vtx_connect_tries++; return VTX_DETECT_WAIT; } - return VTX_DETECT_SUCCESS; + uint8_t data = 0; + if (serial_vtx_read_byte(&data) == 0) { + return VTX_DETECT_WAIT; + } + + quic_debugf("SMART_AUDIO: state %d byte %x", parser_state, data); + + switch (parser_state) { + case PARSER_IDLE: + break; + + case PARSER_READ_MAGIC_1: + if (data == SA_MAGIC_1) { + parser_state = PARSER_READ_MAGIC_2; + } + break; + + case PARSER_READ_MAGIC_2: + if (data != SA_MAGIC_2) { + parser_state = PARSER_READ_MAGIC_1; + } else { + parser_state = PARSER_READ_CMD; + } + break; + + case PARSER_READ_CMD: + vtx_payload[0] = data; + parser_state = PARSER_READ_LENGTH; + break; + + case PARSER_READ_LENGTH: + length = vtx_payload[1] = data; + vtx_payload_offset = 0; + parser_state = length ? PARSER_READ_PAYLOAD : PARSER_READ_CRC; + if (length && betafpv_workaround) { + length--; + } + break; + + case PARSER_READ_PAYLOAD: + vtx_payload[vtx_payload_offset + 2] = data; + if (++vtx_payload_offset == length) { + parser_state = PARSER_READ_CRC; + } + break; + + case PARSER_READ_CRC: { + parser_state = PARSER_IDLE; + const uint8_t theirs = crc8_dvb_s2_data(0, vtx_payload, length + 2); + if (data == theirs) { + smart_audio_parse_packet(actual, vtx_payload[0], vtx_payload + 2, length - 2); + } + return VTX_DETECT_WAIT; + } + } } - if ((result == VTX_IDLE || result == VTX_ERROR) && smart_audio_detected) { - // we are detected and vtx is in idle, we can update stuff - return VTX_DETECT_UPDATE; + if (vtx_settings.detected != VTX_PROTOCOL_SMART_AUDIO && vtx_connect_tries <= SMART_AUDIO_DETECT_TRIES) { + // no smart audio detected, try again + smart_audio_send_payload(SA_CMD_GET_SETTINGS, NULL, 0); + vtx_connect_tries++; + return VTX_DETECT_WAIT; } - // wait otherwise - return VTX_DETECT_WAIT; + if (smart_audio_needs_update) { + smart_audio_send_payload(SA_CMD_GET_SETTINGS, NULL, 0); + smart_audio_needs_update = false; + return VTX_DETECT_WAIT; + } + + return VTX_DETECT_SUCCESS; } -void smart_audio_set_frequency(vtx_band_t band, vtx_channel_t channel) { +static bool smart_audio_set_frequency(vtx_band_t band, vtx_channel_t channel) { #ifdef SA_USE_SET_FREQUENCY if (smart_audio_settings.mode & SA_MODE_FREQUENCY) { const uint16_t frequency = frequency_table[band][channel]; @@ -134,51 +385,70 @@ void smart_audio_set_frequency(vtx_band_t band, vtx_channel_t channel) { (frequency >> 8) & 0xFF, frequency & 0xFF, }; - serial_smart_audio_send_payload(SA_CMD_SET_FREQUENCY, payload, 2); - } else + if (smart_audio_send_payload(SA_CMD_SET_FREQUENCY, payload, 2)) { + smart_audio_needs_update = true; + quic_debugf("SMART_AUDIO: smart_audio_set_frequency update requested"); + return true; + } + return false; + } #endif - { - smart_audio_settings.mode &= ~SA_MODE_FREQUENCY; + // smart_audio_settings.mode &= ~SA_MODE_FREQUENCY; - const uint8_t index = band * VTX_CHANNEL_MAX + channel; - const uint8_t payload[1] = {index}; - serial_smart_audio_send_payload(SA_CMD_SET_CHANNEL, payload, 1); + const uint8_t index = band * VTX_CHANNEL_MAX + channel; + if (smart_audio_send_payload(SA_CMD_SET_CHANNEL, &index, 1)) { smart_audio_needs_update = true; + quic_debugf("SMART_AUDIO: smart_audio_set_frequency update requested"); + return true; } + return false; } -void smart_audio_set_power_level(vtx_power_level_t power) { +static bool smart_audio_set_power_level(vtx_power_level_t power) { uint8_t level = power; - if (smart_audio_settings.version != 2) { - level = smart_audio_settings.dac_power_levels[power]; + if (smart_audio_version != 2) { + level = vtx_actual.power_table.values[power]; } - if (smart_audio_settings.version == 3) { + if (smart_audio_version == 3) { level |= 0x80; } - serial_smart_audio_send_payload(SA_CMD_SET_POWER, &level, 1); - smart_audio_needs_update = true; + if (smart_audio_send_payload(SA_CMD_SET_POWER, &level, 1)) { + smart_audio_needs_update = true; + quic_debugf("SMART_AUDIO: smart_audio_set_power_level update requested"); + return true; + } + return false; } -void smart_audio_set_pit_mode(vtx_pit_mode_t pit_mode) { - if (smart_audio_settings.version >= 3) { - uint8_t mode = 0; +static bool smart_audio_set_pit_mode(vtx_pit_mode_t pit_mode) { + if (smart_audio_version < 3) { + vtx_settings.pit_mode = VTX_PIT_MODE_NO_SUPPORT; + return true; + } - if ((smart_audio_settings.mode & SA_MODE_UNLOCKED) != 0) { - mode |= (1 << 3); // unlock - } + uint8_t mode = (1 << 3); // unlock - if (pit_mode == VTX_PIT_MODE_OFF) { - mode |= (1 << 2); - } else if (pit_mode == VTX_PIT_MODE_ON) { - // out-range was dropped for VTXes with SA >= v2.1 - mode |= (1 << 0); - } + if (pit_mode == VTX_PIT_MODE_OFF) { + mode |= (1 << 2); + } else if (pit_mode == VTX_PIT_MODE_ON) { + // out-range was dropped for VTXes with SA >= v2.1 + mode |= (1 << 0); + } - serial_smart_audio_send_payload(SA_CMD_SET_MODE, &mode, 1); + if (smart_audio_send_payload(SA_CMD_SET_MODE, &mode, 1)) { smart_audio_needs_update = true; - } else { - vtx_settings.pit_mode = VTX_PIT_MODE_NO_SUPPORT; + quic_debugf("SMART_AUDIO: smart_audio_set_pit_mode update requested"); + return true; } + return false; } +const vtx_device_t smart_audio_vtx_device = { + .init = smart_audio_init, + .update = smart_audio_update, + .set_frequency = smart_audio_set_frequency, + .set_power_level = smart_audio_set_power_level, + .set_pit_mode = smart_audio_set_pit_mode, +}; + #endif \ No newline at end of file diff --git a/src/io/vtx_tramp.c b/src/io/vtx_tramp.c index 640e2b531..d1f076b40 100644 --- a/src/io/vtx_tramp.c +++ b/src/io/vtx_tramp.c @@ -2,19 +2,26 @@ #include +#include "core/profile.h" #include "driver/serial.h" -#include "driver/vtx/tramp.h" +#include "driver/vtx.h" #ifdef USE_VTX +typedef enum { + PARSER_IDLE, + PARSER_READ_MAGIC, + PARSER_READ_PAYLOAD, + PARSER_READ_CRC, + PRASER_WAIT_FOR_READY +} tramp_parser_state_t; + #define TRAMP_DETECT_TRIES 5 extern uint8_t vtx_connect_tries; extern const uint16_t frequency_table[VTX_BAND_MAX][VTX_CHANNEL_MAX]; -uint8_t tramp_detected = 0; -extern tramp_settings_t tramp_settings; - +static tramp_parser_state_t parser_state = PARSER_IDLE; static const uint16_t tramp_power_level_values[VTX_POWER_LEVEL_MAX] = { 25, 100, @@ -25,7 +32,6 @@ static const uint16_t tramp_power_level_values[VTX_POWER_LEVEL_MAX] = { 0, 0, }; - static const char tramp_power_level_labels[VTX_POWER_LEVEL_MAX][VTX_POWER_LABEL_LEN] = { "25 ", "100 ", @@ -37,32 +43,59 @@ static const char tramp_power_level_labels[VTX_POWER_LEVEL_MAX][VTX_POWER_LABEL_ " ", }; -vtx_detect_status_t vtx_tramp_update(vtx_settings_t *actual) { - if (tramp_settings.freq_min == 0 && vtx_connect_tries > TRAMP_DETECT_TRIES) { - return VTX_DETECT_ERROR; +static uint8_t crc8_data(const uint8_t *data) { + uint8_t crc = 0; + for (int i = 0; i < 13; i++) { + crc += data[i]; } + return crc; +} - vtx_update_result_t result = serial_tramp_update(); - - if ((result == VTX_IDLE || result == VTX_ERROR) && tramp_settings.freq_min == 0 && vtx_connect_tries <= TRAMP_DETECT_TRIES) { - // no tramp detected, try again - serial_tramp_send_payload('r', 0); - vtx_connect_tries++; - return VTX_DETECT_WAIT; +static void tramp_init() { + const target_serial_port_t *dev = serial_get_dev(profile.serial.smart_audio); + if (!target_serial_port_valid(dev)) { + return; } - if (result == VTX_SUCCESS) { - if (tramp_settings.freq_min == 0) { - // tramp reset was successful but returned empty data - return VTX_DETECT_WAIT; - } + serial_port_config_t config; + config.port = profile.serial.smart_audio; + config.baudrate = 9600; + config.direction = SERIAL_DIR_TX_RX; + config.stop_bits = SERIAL_STOP_BITS_1; + config.invert = false; + config.half_duplex = true; + config.half_duplex_pp = false; + + serial_vtx_wait_for_ready(); + serial_init(&serial_vtx, config); +} - if (tramp_settings.frequency == 0) { - serial_tramp_send_payload('v', 0); - return VTX_DETECT_WAIT; - } +static bool tramp_is_query(uint8_t cmd) { + switch (cmd) { + case 'r': + case 'v': + case 's': + return true; + } + return false; +} - int8_t channel_index = vtx_find_frequency_index(tramp_settings.frequency); +static void tramp_parse_packet(vtx_settings_t *actual, uint8_t *payload) { + switch (payload[0]) { + case 'r': + // freq_min = payload[1] | (payload[2] << 8); + // freq_max = payload[3] | (payload[4] << 8); + // power_max = payload[5] | (payload[6] << 8); + break; + + case 'v': { + const uint16_t frequency = payload[1] | (payload[2] << 8); + const uint16_t power = payload[3] | (payload[4] << 8); + // const uint8_t control_mode = payload[5]; + const uint8_t pit_mode = payload[6]; + // const uint16_t current_power = payload[7] | (payload[8] << 8); + + int8_t channel_index = vtx_find_frequency_index(frequency); if (channel_index >= 0) { actual->band = channel_index / VTX_CHANNEL_MAX; actual->channel = channel_index % VTX_CHANNEL_MAX; @@ -72,19 +105,10 @@ vtx_detect_status_t vtx_tramp_update(vtx_settings_t *actual) { memcpy(actual->power_table.values, tramp_power_level_values, sizeof(tramp_power_level_values)); memcpy(actual->power_table.labels, tramp_power_level_labels, sizeof(tramp_power_level_labels)); - actual->power_level = vtx_power_level_index(&actual->power_table, tramp_settings.power); - actual->pit_mode = tramp_settings.pit_mode; - - // not all vtxes seem to return a non-zero value. :( - // as its unused lets just drop it. - // if (tramp_settings.temp == 0) { - // serial_tramp_send_payload('s', 0); - // return VTX_DETECT_WAIT; - // } - - if (tramp_settings.freq_min != 0 && tramp_settings.frequency != 0 && tramp_detected == 0) { - tramp_detected = 1; + actual->power_level = vtx_power_level_index(&actual->power_table, power); + actual->pit_mode = pit_mode; + if (vtx_settings.detected != VTX_PROTOCOL_TRAMP) { if (vtx_settings.magic != VTX_SETTINGS_MAGIC) { vtx_set(actual); } @@ -92,32 +116,129 @@ vtx_detect_status_t vtx_tramp_update(vtx_settings_t *actual) { vtx_settings.detected = VTX_PROTOCOL_TRAMP; vtx_connect_tries = 0; } - return VTX_DETECT_SUCCESS; + break; + } + case 's': + // temp = payload[5] | (payload[6] << 8); + break; } +} - if ((result == VTX_IDLE || result == VTX_ERROR) && tramp_detected) { - // we are detected and vtx is in idle, we can update stuff - return VTX_DETECT_UPDATE; +static bool tramp_send_payload(uint8_t cmd, const uint16_t payload) { + if (!serial_vtx_is_ready()) { + return false; } - // wait otherwise - return VTX_DETECT_WAIT; + uint8_t vtx_frame[16]; + memset(vtx_frame, 0, 16); + + vtx_frame[0] = 0x0F; + vtx_frame[1] = cmd; + vtx_frame[2] = payload & 0xff; + vtx_frame[3] = (payload >> 8) & 0xff; + vtx_frame[14] = crc8_data(vtx_frame + 1); + + if (!serial_vtx_send_data(vtx_frame, 16)) { + return false; + } + + if (tramp_is_query(vtx_frame[1])) { + parser_state = PARSER_READ_MAGIC; + } else { + parser_state = PRASER_WAIT_FOR_READY; + } + return true; } -void tramp_set_frequency(vtx_band_t band, vtx_channel_t channel) { +static vtx_detect_status_t tramp_update(vtx_settings_t *actual) { + if (!serial_vtx_is_ready()) { + return VTX_DETECT_WAIT; + } + if (vtx_connect_tries > TRAMP_DETECT_TRIES) { + return VTX_DETECT_ERROR; + } + + while (parser_state > PARSER_IDLE) { + if ((time_millis() - vtx_last_valid_read) > 500) { + parser_state = PARSER_IDLE; + if (vtx_settings.detected != VTX_PROTOCOL_TRAMP) { + vtx_connect_tries++; + } + return VTX_DETECT_WAIT; + } + + switch (parser_state) { + case PARSER_IDLE: + break; + + case PARSER_READ_MAGIC: { + if (serial_vtx_read_byte(&vtx_payload[0]) == 0) { + return VTX_DETECT_WAIT; + } + + if (vtx_payload[0] == 0x0F) { + parser_state = PARSER_READ_PAYLOAD; + vtx_payload_offset = 1; + } + break; + } + case PARSER_READ_PAYLOAD: { + if (serial_vtx_read_byte(&vtx_payload[vtx_payload_offset]) == 0) { + return VTX_DETECT_WAIT; + } + + vtx_payload_offset++; + if (vtx_payload_offset >= 16) { + parser_state = PARSER_READ_CRC; + } + break; + } + case PARSER_READ_CRC: { + parser_state = PARSER_IDLE; + + const uint8_t crc = crc8_data(vtx_payload + 1); + if (vtx_payload[14] == crc && vtx_payload[15] == 0) { + tramp_parse_packet(actual, vtx_payload + 1); + } + break; + } + case PRASER_WAIT_FOR_READY: { + // dummy state for non querry packets + parser_state = PARSER_IDLE; + break; + } + } + } + + if (vtx_settings.detected != VTX_PROTOCOL_TRAMP && vtx_connect_tries <= TRAMP_DETECT_TRIES) { + // no tramp detected, try again + tramp_send_payload('v', 0); + vtx_connect_tries++; + return VTX_DETECT_WAIT; + } + + return VTX_DETECT_SUCCESS; +} + +static bool tramp_set_frequency(vtx_band_t band, vtx_channel_t channel) { const uint16_t frequency = frequency_table[band][channel]; - serial_tramp_send_payload('F', frequency); - tramp_settings.frequency = 0; + return tramp_send_payload('F', frequency); } -void tramp_set_power_level(vtx_power_level_t power) { - serial_tramp_send_payload('P', tramp_power_level_values[power]); - tramp_settings.frequency = 0; +static bool tramp_set_power_level(vtx_power_level_t power) { + return tramp_send_payload('P', tramp_power_level_values[power]); } -void tramp_set_pit_mode(vtx_pit_mode_t pit_mode) { - serial_tramp_send_payload('I', pit_mode == VTX_PIT_MODE_ON ? 0 : 1); - tramp_settings.frequency = 0; +static bool tramp_set_pit_mode(vtx_pit_mode_t pit_mode) { + return tramp_send_payload('I', pit_mode == VTX_PIT_MODE_ON ? 0 : 1); } +const vtx_device_t tramp_vtx_device = { + .init = tramp_init, + .update = tramp_update, + .set_frequency = tramp_set_frequency, + .set_power_level = tramp_set_power_level, + .set_pit_mode = tramp_set_pit_mode, +}; + #endif \ No newline at end of file diff --git a/src/osd/render.c b/src/osd/render.c index 3fbadac01..0450e13d8 100644 --- a/src/osd/render.c +++ b/src/osd/render.c @@ -96,6 +96,14 @@ static const char *on_off_labels[] = { " ON", }; +static const char *filter_type_labels[] = { + "NONE", + " PT1", + " PT2", + " PT3", + " LULU", +}; + #pragma GCC diagnostic ignored "-Wmissing-braces" static const vec3_t rate_defaults[RATE_MODE_ACTUAL + 1][3] = { @@ -823,16 +831,9 @@ void osd_display() { osd_menu_start(); osd_menu_header("GYRO FILTERS"); - const char *filter_type_labels[] = { - "NONE", - " PT1", - " PT2", - " PT3", - }; - osd_menu_select(4, 4, "PASS 1 TYPE"); if (osd_menu_select_enum(18, 4, profile.filter.gyro[0].type, filter_type_labels)) { - profile.filter.gyro[0].type = osd_menu_adjust_int(profile.filter.gyro[0].type, 1, 0, FILTER_LP_PT3); + profile.filter.gyro[0].type = osd_menu_adjust_int(profile.filter.gyro[0].type, 1, 0, FILTER_MAX - 1); osd_state.reboot_fc_requested = 1; } @@ -843,7 +844,7 @@ void osd_display() { osd_menu_select(4, 6, "PASS 2 TYPE"); if (osd_menu_select_enum(18, 6, profile.filter.gyro[1].type, filter_type_labels)) { - profile.filter.gyro[1].type = osd_menu_adjust_int(profile.filter.gyro[1].type, 1, 0, FILTER_LP_PT3); + profile.filter.gyro[1].type = osd_menu_adjust_int(profile.filter.gyro[1].type, 1, 0, FILTER_MAX - 1); osd_state.reboot_fc_requested = 1; } @@ -867,16 +868,9 @@ void osd_display() { osd_menu_start(); osd_menu_header("D-TERM FILTERS"); - const char *filter_type_labels[] = { - "NONE", - " PT1", - " PT2", - " PT3", - }; - osd_menu_select(4, 3, "PASS 1 TYPE"); if (osd_menu_select_enum(18, 3, profile.filter.dterm[0].type, filter_type_labels)) { - profile.filter.dterm[0].type = osd_menu_adjust_int(profile.filter.dterm[0].type, 1, 0, FILTER_LP_PT2); + profile.filter.dterm[0].type = osd_menu_adjust_int(profile.filter.dterm[0].type, 1, 0, FILTER_MAX - 1); osd_state.reboot_fc_requested = 1; } @@ -887,7 +881,7 @@ void osd_display() { osd_menu_select(4, 5, "PASS 2 TYPE"); if (osd_menu_select_enum(18, 5, profile.filter.dterm[1].type, filter_type_labels)) { - profile.filter.dterm[1].type = osd_menu_adjust_int(profile.filter.dterm[1].type, 1, 0, FILTER_LP_PT2); + profile.filter.dterm[1].type = osd_menu_adjust_int(profile.filter.dterm[1].type, 1, 0, FILTER_MAX - 1); osd_state.reboot_fc_requested = 1; } @@ -896,9 +890,9 @@ void osd_display() { profile.filter.dterm[1].cutoff_freq = osd_menu_adjust_float(profile.filter.dterm[1].cutoff_freq, 10, 50, 500); } - osd_menu_select(4, 7, "DYNAMIC"); - if (osd_menu_select_enum(18, 7, profile.filter.dterm_dynamic_enable, on_off_labels)) { - profile.filter.dterm_dynamic_enable = osd_menu_adjust_int(profile.filter.dterm_dynamic_enable, 1, 0, 1); + osd_menu_select(4, 7, "DYNAMIC TYPE"); + if (osd_menu_select_enum(18, 7, profile.filter.dterm_dynamic_type, filter_type_labels)) { + profile.filter.dterm_dynamic_type = osd_menu_adjust_int(profile.filter.dterm_dynamic_type, 1, 0, FILTER_MAX - 1); osd_state.reboot_fc_requested = 1; }