diff --git a/.github/ISSUE_TEMPLATE/Question.md b/.github/ISSUE_TEMPLATE/Question.md index 1e2fa8c825f..0a938d3b86e 100644 --- a/.github/ISSUE_TEMPLATE/Question.md +++ b/.github/ISSUE_TEMPLATE/Question.md @@ -9,8 +9,6 @@ For immediate help, just ask your question on one of the following platforms: * [Telegram channel](https://t.me/INAVFlight) * [Facebook group](https://www.facebook.com/groups/INAVOfficial) * [RC Groups thread](https://www.rcgroups.com/forums/showthread.php?2495732-Cleanflight-iNav-%28navigation-rewrite%29-project) -* [IRC channel](irc://irc.freenode.net/#inavflight) -* [Slack channel](https://publicslack.com/slacks/inavflight/invites/new) You can also read public documentations or watch video tutorials: diff --git a/Makefile b/Makefile index ab74b50e35a..d67ec9676a7 100644 --- a/Makefile +++ b/Makefile @@ -118,10 +118,10 @@ else $(error Unknown target MCU specified.) endif -GROUP_1_TARGETS := ALIENFLIGHTF3 ALIENFLIGHTF4 AIRHEROF3 AIRHEROF3_QUAD COLIBRI_RACE LUX_RACE SPARKY REVO SPARKY2 COLIBRI FALCORE FF_F35_LIGHTNING FF_FORTINIF4 FF_PIKOF4 FF_PIKOF4OSD -GROUP_2_TARGETS := SPRACINGF3 SPRACINGF3EVO SPRACINGF3EVO_1SS SPRACINGF3MINI SPRACINGF3NEO SPRACINGF4EVO CLRACINGF4AIR CLRACINGF4AIRV2 BEEROTORF4 BETAFLIGHTF3 BETAFLIGHTF4 PIKOBLX +GROUP_1_TARGETS := ALIENFLIGHTF3 ALIENFLIGHTF4 AIRHEROF3 AIRHEROF3_QUAD COLIBRI_RACE LUX_RACE SPARKY REVO SPARKY2 COLIBRI KISSFC FALCORE FF_F35_LIGHTNING FF_FORTINIF4 FF_PIKOF4 FF_PIKOF4OSD +GROUP_2_TARGETS := SPRACINGF3 SPRACINGF3EVO SPRACINGF3EVO_1SS SPRACINGF3MINI SPRACINGF4EVO CLRACINGF4AIR CLRACINGF4AIRV2 BEEROTORF4 BETAFLIGHTF3 BETAFLIGHTF4 PIKOBLX SPRACINGF3NEO GROUP_3_TARGETS := OMNIBUS AIRBOTF4 BLUEJAYF4 OMNIBUSF4 OMNIBUSF4PRO OMNIBUSF4V3 FIREWORKSV2 SPARKY2 MATEKF405 OMNIBUSF7 DYSF4PRO OMNIBUSF4PRO_LEDSTRIPM5 OMNIBUSF7NXT OMNIBUSF7V2 ASGARD32F4 -GROUP_4_TARGETS := ANYFC ANYFCF7 ANYFCF7_EXTERNAL_BARO ANYFCM7 ALIENFLIGHTNGF7 PIXRACER YUPIF4 YUPIF4MINI YUPIF4R2 YUPIF7 MATEKF405SE MATEKF411 MATEKF722 MATEKF405OSD MATEKF405_SERVOS6 +GROUP_4_TARGETS := ANYFC ANYFCF7 ANYFCF7_EXTERNAL_BARO ANYFCM7 ALIENFLIGHTNGF7 PIXRACER YUPIF4 YUPIF4MINI YUPIF4R2 YUPIF7 MATEKF405SE MATEKF411 MATEKF722 MATEKF405OSD MATEKF405_SERVOS6 NOX GROUP_OTHER_TARGETS := $(filter-out $(GROUP_1_TARGETS) $(GROUP_2_TARGETS) $(GROUP_3_TARGETS) $(GROUP_4_TARGETS), $(VALID_TARGETS)) REVISION = $(shell git rev-parse --short HEAD) @@ -224,6 +224,7 @@ CFLAGS += $(ARCH_FLAGS) \ -Werror=switch \ -ffunction-sections \ -fdata-sections \ + -fno-common \ $(DEVICE_FLAGS) \ -DUSE_STDPERIPH_DRIVER \ $(TARGET_FLAGS) \ @@ -322,6 +323,12 @@ settings-json: clean-settings: $(V1) $(RM) $(GENERATED_SETTINGS) +# CFLAGS used for ASM generation. These can't include the LTO related options +# since they prevent proper ASM generation. Since $(LTO_FLAGS) includes the +# optization level, we have to add it back. -g is required to make interleaved +# source/ASM work. +ASM_CFLAGS=-g $(OPTIMZE) $(filter-out $(LTO_FLAGS) -save-temps=obj, $(CFLAGS)) + # List of buildable ELF files and their object dependencies. # It would be nice to compute these lists, but that seems to be just beyond make. @@ -341,6 +348,10 @@ $(TARGET_OBJ_DIR)/%.o: %.c $(V1) mkdir -p $(dir $@) $(V1) echo %% $(notdir $<) "$(STDOUT)" $(V1) $(CROSS_CC) -c -o $@ $(CFLAGS) $< +ifeq ($(GENERATE_ASM), 1) + $(V1) $(CROSS_CC) -S -fverbose-asm -Wa,-aslh -o $(patsubst %.o,%.txt.S,$@) -g $(ASM_CFLAGS) $< +endif + # Assemble $(TARGET_OBJ_DIR)/%.o: %.s diff --git a/README.md b/README.md index 9b9457f891b..53e69df53ee 100644 --- a/README.md +++ b/README.md @@ -26,7 +26,7 @@ * PIDs from CF/BF can be used in INAV, no need to retune for INAV * And many more! -For a list of features, changes and some discussion please review the thread on RCGroups forums and consult the documentation. +For a list of features, changes and some discussion please review consult the releases [page](https://github.com/iNavFlight/inav/releases) and the documentation. ## Tools @@ -52,8 +52,6 @@ See: https://github.com/iNavFlight/inav/blob/master/docs/Installation.md * [Official Wiki](https://github.com/iNavFlight/inav/wiki) * [INAV Official on Telegram](https://t.me/INAVFlight) * [INAV Official on Facebook](https://www.facebook.com/groups/INAVOfficial) -* [INAV Official on Slack](https://publicslack.com/slacks/inavflight/invites/new) -* [INAV IRC channel] (irc://irc.freenode.net/#inavflight) * [RC Groups Support](https://www.rcgroups.com/forums/showthread.php?2495732-Cleanflight-iNav-(navigation-rewrite)-project) * [Video series by Painless360](https://www.youtube.com/playlist?list=PLYsWjANuAm4qdXEGFSeUhOZ10-H8YTSnH) * [Video series by Paweł Spychalski](https://www.youtube.com/playlist?list=PLOUQ8o2_nCLloACrA6f1_daCjhqY2x0fB) @@ -67,8 +65,9 @@ Contributions are welcome and encouraged. You can contribute in many ways: * Bug fixes. * New features. * Telling us your ideas and suggestions. +* Buying your hardware from this [link](https://inavflight.com/shop/u/bg/) -A good place to start is Telegram channel, Slack, Facebook goop or IRC channel on freenode (#inavflight), drop in, say hi. +A good place to start is Telegram channel or Facebook group. Drop in, say hi. Github issue tracker is a good place to search for existing issues or report a new bug/feature request: diff --git a/docs/Battery.md b/docs/Battery.md index 6889175d390..1b5e23ebd31 100644 --- a/docs/Battery.md +++ b/docs/Battery.md @@ -295,7 +295,7 @@ set battery_capacity_critical = 150 ## Remaining flight time and flight distance estimation -The estimated remaining flight time and flight distance estimations can be displayed on the OSD (for fixed wing only for the moment). They are calculated from the GPS distance from home, remaining battery capacity and average power draw. They are taking into account the requested altitude change and heading to home change after altitude change following the switch to RTH. They are also taking into account the estimated wind if `osd_use_wind_compensation` is set to `ON`. When the timer and distance indicator reach 0 they will blink and you need to go home in a straight line manually or by engaging RTH. You should be left with at least `rth_energy_margin`% of battery left when arriving home if the cruise speed and power are set correctly (see bellow). +The estimated remaining flight time and flight distance estimations can be displayed on the OSD (for fixed wing only for the moment). They are calculated from the GPS distance from home, remaining battery capacity and average power draw. They are taking into account the requested altitude change and heading to home change after altitude change following the switch to RTH. They are also taking into account the estimated wind if `osd_estimations_wind_compensation` is set to `ON`. When the timer and distance indicator reach 0 they will blink and you need to go home in a straight line manually or by engaging RTH. You should be left with at least `rth_energy_margin`% of battery left when arriving home if the cruise speed and power are set correctly (see bellow). To use this feature the following conditions need to be met: - The `VBAT`, `CURRENT_METER` and `GPS` features need to be enabled @@ -315,7 +315,7 @@ This features aims to compensate the throttle to get constant thrust with the sa It is working like this: `used_throttle = requested_throttle * (1 + (battery_full_voltage / sag_compensated_voltage - 1) * thr_comp_weight)`. -The default `thr_comp_weight` of 1 should be close to idal but if you want to tune this feature you need to find the difference in throttle value to achieve the same thrust (same power) when your battery is full and when your battery is almost empty then set `thr_comp_weight` to `(empty_battery_throttle / full_battery_throttle - 1) / (battery_full_voltage / battery_empty_sag_compensated_voltage - 1)` +The default `thr_comp_weight` of 1 should be close to ideal but if you want to tune this feature you need to find the difference in throttle value to achieve the same thrust (same power) when your battery is full and when your battery is almost empty then set `thr_comp_weight` to `(empty_battery_throttle / full_battery_throttle - 1) / (battery_full_voltage / battery_empty_sag_compensated_voltage - 1)` Example: If the drawn power is 100W when the battery is full (12.6V) with 53% throttle and the drawn power is 100W with 58% throttle when the battery is almost empty with the sag compensated voltage being 11.0V `thr_comp_weight` needs to be set to this value to compensate the throttle automatically: diff --git a/docs/Board - DALRCF405.md b/docs/Board - DALRCF405.md new file mode 100644 index 00000000000..08e65492e76 --- /dev/null +++ b/docs/Board - DALRCF405.md @@ -0,0 +1,106 @@ +# Board - DALRCF405 + +The DALRCF405 described here: +http://www.dalrcmodel.com/DALRC/plus/view.php?aid=184 + +This board use the STM32F405RGT6 microcontroller and have the following features: +* 1024K bytes of flash memory,192K bytes RAM,168 MHz CPU/210 DMIPS + +* The 16M byte SPI flash for data logging +* USB VCP and boot select button on board(for DFU) +* Stable voltage regulation,9V/2A DCDC BEC for VTX/camera etc.And could select 5v/9v +* Serial LED interface(LED_STRIP) +* VBAT/CURR/RSSI sensors input +* Suppose IRC Tramp/smart audio/FPV Camera Control/FPORT/telemetry +* Supports SBus, Spektrum1024/2048, PPM. No external inverters required (built-in). +* Supports I2C device extend(baro/compass/OLED etc) +* Supports GPS + +### Uarts +| Value | Identifier | RX | TX | Notes | +| ----- | ------------ | -----| -----| ------------------------------------------------------------------------------------------- | +| 1 | USART1 | PB7 | PA9 | PB7 FOR SBUS IN(inverter build in) | +| 2 | USART2 | PA3 | PA2 | PAD USE FOR TRAMP/smart audio | +| 3 | USART3 | PB11 | PB10| USE FOR GPS | +| 4 | USART4 | PA1 | PA0 | PA0 FOR RSSI/FPORT/TEL etc | +| 5 | USART5 | PD2 | PC12| PAD | + + +### I2C with GPS port together.Use for BARO or compass etc +| Value | Identifier | function | pin | Notes | +| ----- | ------------ | ---------| -------| ------------------------------------------------------------------------------------- | +| 1 | I2C1 | SDA | PB9 | with GPS outlet +| 2 | I2C1 | SCL | PB8 | with GPS outlet + + +### Buzzer/LED output +| Value | Identifier | function | pin | Notes | +| ----- | ------------ | ---------| -------| ------------------------------------------------------------------------------------- | +| 1 | LED0 | LED | PC14 | +| 2 | BEEPER | BEE | PC13 | + + +### VBAT input with 1/10 divider ratio,Current signal input,Analog/digit RSSI input +| Value | Identifier | function | pin | Notes | +| ----- | ------------ | ----------| ------| ------------------------------------------------------------------------------------- | +| 1 | ADC1 | VBAT | PC2 | DMA2_Stream0 +| 2 | ADC1 | CURR | PC1 | DMA2_Stream0 +| 3 | ADC1 | RSSI | PA0 | DMA2_Stream0 +| 4 | ADC1 | extend | PC0 | DMA2_Stream0 extend for other senser(PAD) + + +### 8 Outputs, 1 PPM input +| Value | Identifier | function | pin | Notes | +| ----- | ------------ | ----------| ------| ------------------------------------------------------------------------------------- | +| 1 | TIM12_CH2 | PPM | PB15 | PPM +| 2 | TIM3_CH3 | OUPUT1 | PB0 | DMA1_Stream7 +| 3 | TIM8_CH1 | OUPUT2 | PC6 | DMA2_Stream2 +| 4 | TIM1_CH3 | OUPUT3 | PA10 | DMA2_Stream6 +| 5 | TIM1_CH1 | OUPUT4 | PA8 | DMA2_Stream1 +| 6 | TIM8_CH3 | OUPUT5 | PC8 | DMA2_Stream4 +| 7 | TIM3_CH4 | OUPUT6 | PB1 | DMA1_Stream2 +| 8 | TIM3_CH2 | OUPUT7 | PC7 | DMA1_Stream5 +| 9 | TIM8_CH4 | OUPUT8 | PC9 | DMA2_Stream7 +| 10 | TIM4_CH1 | PWM | PB6 | DMA1_Stream0 LED_STRIP +| 11 | TIM2_CH1 | PWM | PA5 | FPV Camera Control(FCAM) + + +### Gyro & ACC ,suppose ICM20689/MPU6000 +| Value | Identifier | function | pin | Notes | +| ----- | ------------ | ---------| -------| ------------------------------------------------------------------------------------- | +| 1 | SPI1 | SCK | PB3 | +| 2 | SPI1 | MISO | PA6 | +| 3 | SPI1 | MOSI | PA7 | +| 4 | SPI1 | CS | PC4 | + +### OSD MAX7456 +| Value | Identifier | function | pin | Notes | +| ----- | ------------ | ---------| -------| ------------------------------------------------------------------------------------- | +| 1 | SPI3 | SCK | PC10 | +| 2 | SPI3 | MISO | PC11 | +| 3 | SPI3 | MOSI | PB5 | +| 4 | SPI3 | CS | PA15 | + +### 16Mbyte flash +| Value | Identifier | function | pin | Notes | +| ----- | ------------ | ---------| -------| ------------------------------------------------------------------------------------- | +| 1 | SPI2 | SCK | PB13 | +| 2 | SPI2 | MISO | PB14 | +| 3 | SPI2 | MOSI | PC3 | +| 4 | SPI2 | CS | PB12 | + +### SWD +| Pin | Function | Notes | +| --- | -------------- | -------------------------------------------- | +| 1 | SWCLK | PAD | +| 2 | Ground | PAD | +| 3 | SWDIO | PAD | +| 4 | 3V3 | PAD | + +###Designers +* ZhengNyway(nyway@vip.qq.com) FROM DALRC + + + + + diff --git a/docs/Board - DALRCF722DUAL.md b/docs/Board - DALRCF722DUAL.md new file mode 100644 index 00000000000..20287ee5210 --- /dev/null +++ b/docs/Board - DALRCF722DUAL.md @@ -0,0 +1,111 @@ +# Board - DALRCF722DUAL + +The DALRCF722DUAL described here: +http://www.dalrcmodel.com/DALRC/plus/view.php?aid=188 + +This board use the STM32F722RET6 microcontroller and have the following features: +* High-performance and DSP with FPU, ARM Cortex-M7 MCU with 512 Kbytes Flash +* 216 MHz CPU,462 DMIPS/2.14 DMIPS/MHz (Dhrystone 2.1) and DSP instructions, Art Accelerator, L1 cache, SDRAM +* DUAL gyro MPU6000 and ICM20602,could choose mpu6000,more stable and smooth.Or choose ICM20602,higher rate(32K/32K).Choose on CLI mode,experience different feature gyro on same board +* OSD on board +* The 16M byte SPI flash on board for data logging +* USB VCP and boot select button on board(for DFU) +* Stable voltage regulation,DUAL BEC 5v/2.5A and 10v/2A for VTX/camera etc.And could select 5v/10v with pad +* Serial LED interface(LED_STRIP) +* VBAT/CURR/RSSI sensors input +* Suppose IRC Tramp/smart audio/FPV Camera Control/FPORT/telemetry +* Supports SBus(built-in inverters), Spektrum1024/2048, PPM +* Supports I2C device extend(baro/compass/OLED etc)(socket) +* Supports GPS (socket) + +### All uarts have pad on board +| Value | Identifier | RX | TX | Notes | +| ----- | ------------ | -----| -----| ------------------------------------------------------------------------------------------- | +| 1 | USART1 | PB7 | PB6 | PB7 FOR SBUS IN(inverter build in)/PPM | +| 2 | USART2 | PA3 | PA2 | PAD USE FOR TRAMP/smart audio | +| 3 | USART3 | PC11 | PC10| USE FOR GPS | +| 4 | USART4 | PA1 | PA0 | PA0 FOR RSSI/FPORT/TEL etc | +| 5 | USART5 | PD2 | PC12| PAD | + + +### I2C with GPS port together.Use for BARO or compass etc +| Value | Identifier | function | pin | Notes | +| ----- | ------------ | ---------| -------| ------------------------------------------------------------------------------------- | +| 1 | I2C1 | SDA | PB9 | +| 2 | I2C1 | SCL | PB8 | + + +### Buzzer/LED output +| Value | Identifier | function | pin | Notes | +| ----- | ------------ | ---------| -------| ------------------------------------------------------------------------------------- | +| 1 | LED0 | LED | PC14 |On board +| 2 | BEEPER | BEE | PC13 |Pad + + +### VBAT input with 1/10 divider ratio,Current signal input,Analog/digit RSSI input +| Value | Identifier | function | pin | Notes | +| ----- | ------------ | ----------| ------| ------------------------------------------------------------------------------------- | +| 1 | ADC1 | VBAT | PC1 | +| 2 | ADC1 | CURR | PC0 | +| 3 | ADC1 | RSSI | PA0 | + + +### 8 Outputs +| Value | Identifier | function | pin | Notes | +| ----- | ------------ | ----------| ------| ------------------------------------------------------------------------------------- | +| 1 | TIM4_CH2 | PPM | PB7 | PPM +| 2 | TIM8_CH1 | OUPUT1 | PC6 | DMA +| 3 | TIM8_CH2 | OUPUT2 | PC7 | DMA +| 4 | TIM8_CH3 | OUPUT3 | PC8 | DMA +| 5 | TIM8_CH4 | OUPUT4 | PC9 | DMA +| 6 | TIM1_CH1 | OUPUT5 | PA8 | DMA +| 7 | TIM1_CH2 | OUPUT6 | PA9 | DMA +| 8 | TIM1_CH3 | OUPUT7 | PA10 | DMA NO PAD +| 9 | TIM2_CH4 | OUPUT8 | PB11 | NO PAD +| 10 | TIM2_CH1 | PWM | PA15 | DMA LED_STRIP +| 11 | TIM3_CH4 | PWM | PB1 | FPV Camera Control(FCAM) + + +### Gyro & ACC ,support ICM20602 and MPU6000 +| Value | Identifier | function | pin | Notes | +| ----- | ------------ | ---------| -------| ------------------------------------------------------------------------------------- | +| 1 | SPI1 | SCK | PA5 | MPU6000 & ICM20602 +| 2 | SPI1 | MISO | PA6 | MPU6000 & ICM20602 +| 3 | SPI1 | MOSI | PA7 | MPU6000 & ICM20602 +| 4 | SPI1 | CS1 | PB0 | MPU6000 +| 5 | SPI1 | CS2 | PA4 | ICM20602 +| 6 | SPI1 | INT1 | PB10 | MPU6000 +| 7 | SPI1 | INT2 | PC4 | ICM20602 + +### OSD MAX7456 +| Value | Identifier | function | pin | Notes | +| ----- | ------------ | ---------| -------| ------------------------------------------------------------------------------------- | +| 1 | SPI2 | SCK | PB13 | +| 2 | SPI2 | MISO | PB14 | +| 3 | SPI2 | MOSI | PB15 | +| 4 | SPI2 | CS | PB12 | + +### 16Mbyte flash +| Value | Identifier | function | pin | Notes | +| ----- | ------------ | ---------| -------| ------------------------------------------------------------------------------------- | +| 1 | SPI3 | SCK | PB3 | +| 2 | SPI3 | MISO | PB4 | +| 3 | SPI3 | MOSI | PB4 | +| 4 | SPI3 | CS | PB2 | + +### SWD +| Pin | Function | Notes | +| --- | -------------- | -------------------------------------------- | +| 1 | SWCLK | PAD | +| 2 | Ground | PAD | +| 3 | SWDIO | PAD | +| 4 | 3V3 | PAD | + + +###Designers +* ZhengNyway(nyway@vip.qq.com) FROM DALRC + + + + + diff --git a/docs/Board - FOXEERF405.md b/docs/Board - FOXEERF405.md new file mode 100644 index 00000000000..cdbecbf13b6 --- /dev/null +++ b/docs/Board - FOXEERF405.md @@ -0,0 +1,16 @@ +# Board - FOXEERF405 + +The FOXEERF405 described here: + +This board use the STM32F405RGT6 microcontroller and have the following features: +* 1024K bytes of flash memory,192K bytes RAM,168 MHz CPU/210 DMIPS +* The 16M byte SPI flash for data logging +* USB VCP and boot select button on board(for DFU) +* Stable voltage regulation,9V/2A DCDC BEC for VTX/camera etc.And could select 5v/9v with pad +* Serial LED interface(LED_STRIP) +* VBAT/CURR/RSSI sensors input +* Suppose IRC Tramp/smart audio/FPV Camera Control/FPORT/telemetry +* Supports SBus, Spektrum1024/2048, PPM. No external inverters required (built-in). +* Supports I2C device extend(baro/compass/OLED etc) +* Supports GPS +* Supports MPU6000 or ICM20689 diff --git a/docs/Board - FOXEERF722DUAL.md b/docs/Board - FOXEERF722DUAL.md new file mode 100644 index 00000000000..6e168aaac49 --- /dev/null +++ b/docs/Board - FOXEERF722DUAL.md @@ -0,0 +1,103 @@ +# Board - FOXEERF722DUAL + +The FOXEERF722DUAL described here: + +This board use the STM32F722RET6 microcontroller and have the following features: +* High-performance and DSP with FPU, ARM Cortex-M7 MCU with 512 Kbytes Flash +* 216 MHz CPU,462 DMIPS/2.14 DMIPS/MHz (Dhrystone 2.1) and DSP instructions, Art Accelerator, L1 cache, SDRAM +* DUAL gyro MPU6000 and ICM20602,could choose mpu6000,more stable and smooth.Or choose ICM20602,higher rate(32K/32K).Choose on CLI mode,experience different feature gyro on same board +* OSD on board +* The 16M byte SPI flash on board for data logging +* USB VCP and boot select button on board(for DFU) +* Stable voltage regulation,DUAL BEC 5v/2.5A and 9v/2A for VTX/camera etc.And could select 5v/9v with pad +* Serial LED interface(LED_STRIP) +* VBAT/CURR/RSSI sensors input +* Suppose IRC Tramp/smart audio/FPV Camera Control/FPORT/telemetry +* Supports SBus, Spektrum1024/2048, PPM +* Supports I2C device extend(baro/compass/OLED etc) +* Supports GPS + +### All uarts have pad on board +| Value | Identifier | RX | TX | Notes | +| ----- | ------------ | -----| -----| ------------------------------------------------------------------------------------------- | +| 1 | USART1 | PB7 | PB6 | PB7 FOR SBUS IN(inverter build in)/PPM | +| 2 | USART2 | PA3 | PA2 | PAD USE FOR TRAMP/smart audio | +| 3 | USART3 | PB11 | PB10| USE FOR GPS | +| 4 | USART4 | PA1 | PA0 | PA0 FOR RSSI/FPORT/TEL etc | +| 5 | USART5 | PD2 | PC12| PAD | + + +### I2C with GPS port together.Use for BARO or compass etc +| Value | Identifier | function | pin | Notes | +| ----- | ------------ | ---------| -------| ------------------------------------------------------------------------------------- | +| 1 | I2C1 | SDA | PB9 | outlet +| 2 | I2C1 | SCL | PB8 | outlet + + +### Buzzer/LED output +| Value | Identifier | function | pin | Notes | +| ----- | ------------ | ---------| -------| ------------------------------------------------------------------------------------- | +| 1 | LED0 | LED | PC15 |On board +| 2 | BEEPER | BEE | PA4 |Pad + + +### VBAT input with 1/10 divider ratio,Current signal input,Analog/digit RSSI input +| Value | Identifier | function | pin | Notes | +| ----- | ------------ | ----------| ------| ------------------------------------------------------------------------------------- | +| 1 | ADC1 | VBAT | PC0 | On board +| 2 | ADC1 | CURR | PC2 | +| 3 | ADC1 | RSSI | PA0 | + + +### 8 Outputs +| Value | Identifier | function | pin | Notes | +| ----- | ------------ | ----------| ------| ------------------------------------------------------------------------------------- | +| 1 | TIM4_CH2 | PPM | PB7 | PPM +| 2 | TIM1_CH2 | OUPUT1 | PA9 | DMA +| 3 | TIM1_CH1 | OUPUT2 | PA8 | DMA +| 4 | TIM8_CH4 | OUPUT3 | PC9 | DMA +| 5 | TIM8_CH3 | OUPUT4 | PC8 | DMA +| 6 | TIM8_CH1 | OUPUT5 | PC6 | DMA +| 7 | TIM8_CH2 | OUPUT6 | PC7 | DMA +| 8 | TIM2_CH1 | PWM | PA15 | DMA LED_STRIP +| 9 | TIM2_CH2 | PWM | PB3 | FPV Camera Control(FCAM) + + +### Gyro & ACC ,support ICM20602 and MPU6000 +| Value | Identifier | function | pin | Notes | +| ----- | ------------ | ---------| -------| ------------------------------------------------------------------------------------- | +| 1 | SPI1 | SCK | PA5 | MPU6000 & ICM20602 +| 2 | SPI1 | MISO | PA6 | MPU6000 & ICM20602 +| 3 | SPI1 | MOSI | PA7 | MPU6000 & ICM20602 +| 4 | SPI1 | CS1 | PB2 | MPU6000 +| 5 | SPI1 | CS2 | PB1 | ICM20602 +| 6 | SPI1 | INT1 | PC4 | MPU6000 +| 7 | SPI1 | INT2 | PB0 | ICM20602 + +### OSD MAX7456 +| Value | Identifier | function | pin | Notes | +| ----- | ------------ | ---------| -------| ------------------------------------------------------------------------------------- | +| 1 | SPI3 | SCK | PC10 | +| 2 | SPI3 | MISO | PC11 | +| 3 | SPI3 | MOSI | PB5 | +| 4 | SPI3 | CS | PC3 | + +### 16Mbyte flash +| Value | Identifier | function | pin | Notes | +| ----- | ------------ | ---------| -------| ------------------------------------------------------------------------------------- | +| 1 | SPI2 | SCK | PB13 | +| 2 | SPI2 | MISO | PB14 | +| 3 | SPI2 | MOSI | PB15 | +| 4 | SPI2 | CS | PB12 | + +### SWD +| Pin | Function | Notes | +| --- | -------------- | -------------------------------------------- | +| 1 | SWCLK | PAD | +| 2 | Ground | PAD | +| 3 | SWDIO | PAD | +| 4 | 3V3 | PAD | + + +###Designers +* NywayZheng(nyway@vip.qq.com) \ No newline at end of file diff --git a/docs/Board - Matek F411 Wing.md b/docs/Board - Matek F411 Wing.md index 7545d571e9c..de1d4bae74d 100644 --- a/docs/Board - Matek F411 Wing.md +++ b/docs/Board - Matek F411 Wing.md @@ -16,9 +16,14 @@ ## Details * [Full specification](http://www.mateksys.com/?portfolio=f411-wing) -* For Matek F411-WING use `MATEKF411` firmware. -* SBS pad has a built-in inverter and is connected to UART1 RX +* SBUS pad has a built-in inverter and is connected to UART1 RX + +## Available TARGETS + +* `MATEKF411` if you want to control LEDs and have SS1 TX on ST1 pad. +* `MATEKF411_SFTSRL2`if you want to use two softserials (TX only) at the same time. Eg. Smart Audio + S. Port, Smart Audio + LTM +* `MATEKF411_RSSI` if you want to have analog RSSI input on ST1 pad. SS1 TX will be available on the LED pad. ## Where to buy: -* [Banggood](https://inavflight.com/shop/p/MATEKF411WING) \ No newline at end of file +* [Banggood](https://inavflight.com/shop/p/MATEKF411WING) diff --git a/docs/Board - MatekF722-SE.md b/docs/Board - MatekF722-SE.md new file mode 100644 index 00000000000..6a14d973fa8 --- /dev/null +++ b/docs/Board - MatekF722-SE.md @@ -0,0 +1,92 @@ +# Board - Matek F722-SE + +Full details on the MATEKSYS F722-SE can be found on the Matek Website: [mateksys.com/?portfolio=f722-se](http://www.mateksys.com/?portfolio=f722-se). INAV Target: `MATEKF722SE` + +## Hardware Specs + +* *Mass:* ~10g +* *PCB Size:* 36x46mm + * 30x30mm Hole pattern (M4 size, M3 size with rubber isolators) + +### FC Specs + +* Processors and Sensors + * *MCU:* STM32F722RET6 + * *IMU:* MPU6000(Gyro1) & ICM20602(Gyro2) connected via SPI1 + * *Baro:* BMP280 (connected via I2C1) + * *OSD:* BetaFlight OSD (AT7456E connected via SPI2) +* *Blackbox:* MicroSD card slot (connected via SPI3) +* 5 UARTs +* 8 PWM/Dshot outputs +* 2 PINIO definition (VTX power switcher/user1 and 2 camera switcher/user2) + +### Integrated PDB Specs + +* *Input:* 6-36v (3-8S LiPo) w/TVS protection +* *ESC Pads:* Rated 4x30A per ESC pad set (4x46A burst) +* Voltage Regulators: + * *5v BEC:* 2A continuous load (3A burst) + * *3.3v LDO:* max load: 200mA +* Power Sense: + * *Current Sensor:* Rated for 184A (*Suggested scale value `179`*) + * *Voltage Sensor:* 1:10 signal output ratio (*Suggested scale value `110`*) + +## Status LEDs + +| LED | Color | Color Codes | +|---------------:|-------|:----------------------------------------------------------------------------------------------------------| +| FC Status | Blue | **Unlit:** USB disconnected and disarmed,
**Lit:** USB disconnected and armed,
**Flashing:** USB connected and disarmed,
**5x Rapid Flash then Flashing:** USB connected and arming command prevented | +| Accelerometer | Green | Accelerometer status (Lit: active, Unlit: inactive) | +| 3v3 Status | Red | Red: active, Unlit: inactive | + +## Pinout + +Pads are organised into two large banks of pads on left and right sides of board with a couple sets of pads on the underside of the board and ESC related connections near the board corners. + +``` + __________ + / U U \ +/-----------------------------\ +|oE Eo| +|SC SC| +| | +| P P | +| A A | +| D D | +| S S | +| | +|ES ES| +|oC Co| +\------------[USB]------------/ +``` + + +| Pad Silkscreen Label | Function | Notes | +|---------------------:|---------------|:-----------------------------------------------------------------------------------------------| +| `+ / -` | Battery In | 6-30vDC LiPo Power input (*Battery +/- and 4-in-1 ESC +/- pads*) | +| `S1-S8` | ESC Out | (*1-4 near ESC power connections, 5-8 on right side*) Supports PWM, Oneshot, Multishot, DSHOT | +| `GND, S1-S4` | ESC Out | (*Rear of board*) 4-in-1 ESC Out | +| `VBT, GND` | VBT Out | VBAT power pad (*marked for VTX*), power ON/OFF can be switched via PINIO1(PC8) | +| `CURR` | Current Sense | Current Sensor I/O pin (*output from onboard sensor or input from external sensor*) | +| `5V` | | Out from internal 5v BEC (*rated 2A continuous, 3A burst*) | +| `3V3` | | Out from 3v3 regulator (*rated 200mA*) | +| `4V5` | | Out from 4v4~4v8, 0.5A (*power is also supplied when connected via USB*) | +| `G` | GND | | +| `LED` | WS2812 Signal | | +| `Bz-, 5V` | Buzzer | | +| `CL, DA` | I2C | I2C connection marked for a magnetometer but could be used for whatever | +| `VTX` | VTX | VTX: Video out | +| `C1/C2` | Camera | C1: camera-1 IN, C1: camera-2 IN, 2 camera image can be switched via PINIO2(PC9) | +| `RX1, TX1` | UART1 | | +| `TX2` | UART2-TX | | +| `RX2` | UART2-RX | RX connection for Spektrum DSMX or DSM2, FlySky iBUS, or PPM (*Disable `UART2` for PPM*) | +| `RX3, TX3` | UART3 | TX3 can be used for VTX control | +| `RX4, TX4` | UART4 | RX4 pin `PA1` can be used for camera control(PWM) | +| `TX6` | UART6-TX | | +| `RX6` | UART6-RX | (*One per board corner*) Duplicates of RX6 pad for ESC Telemetry | +| `Rssi` | RSSI | FrSky RSSI input from RX (*Rear of board*) | +| `PA4 ` | ADC/DAC | Airspeed_ADC_PIN (*Rear of board*) | +| `D+, D-. VBus` | USB pins | (*Rear of board*) | + + + diff --git a/docs/Board - MatekF722.md b/docs/Board - MatekF722.md new file mode 100644 index 00000000000..063102b9944 --- /dev/null +++ b/docs/Board - MatekF722.md @@ -0,0 +1,14 @@ +# Board - MATEKSYS F722 + +## Vendor Information / specification +http://www.mateksys.com/?portfolio=f722-std + +## Firmware + +Two firmware variants are available. + +* `inav_x.y.z_MATEKF722.hex` +* `inav_x.y.z_MATEKF722_HEXSERVO.hex` + +The only difference is that the `HEXSERVO` variant provides a multi-rotor servo *only* on S7 (vice S6 and S7 in the standard firmware); this facilitates the use of a single servo on a hexcopter. + diff --git a/docs/Board - NOX.md b/docs/Board - NOX.md new file mode 100755 index 00000000000..a9553eabfa6 --- /dev/null +++ b/docs/Board - NOX.md @@ -0,0 +1,46 @@ +# Board - [NOX](https://inavflight.com/shop/p/NOX) + +![Airbot]https://youtu.be/j3zIYFYZ_SQ +![Banggood](https://img.staticbg.com/thumb/view/oaupload/banggood/images/A5/01/79d28a2c-ef4b-4e4f-bab6-14edf66bbb23.jpg) +![Banggood]https://img.staticbg.com/images/oaupload/banggood/images/2E/C5/c14a1e86-4e58-4bc8-85de-8e344cb382b9.jpg) +![Banggood]https://img.staticbg.com/images/oaupload/banggood/images/42/F7/45a68ade-9be1-4fff-afec-bbdd45f0331d.jpg) + +## Airbot Specification: +* Betaflight OSD +* STM32F411 MCU +* MPU6000 Gyro - It's also replaceable +* Barometer +* 5V BEC with LC filter (500ma) +* 4000uf capacitors onboard - No Need for bulky caps +* 4in1 ESC + +* BLHeli32 - The 32-bit architecture that we've come to expect +* DSHOT 1200 +* 35A Per motor +* Telemetry Output functionality +* ESC upgradeable + +## Banggood Specification: +* Model: F4 Nox4 +* Version: Acro Version / Deluxe Version +* Acro Version: Without Barometer and Blackbox +* Deluxe Version: With Barometer and Blackbox +* CPU: STM32F411C +* MPU: MPU6000 +* Input Voltage: Support 2-4S Lipo Input +* Built-In Betaflight OSD +* Built-in 5V @ 3A BEC +* 3.3V +* Built-in LC Filter +* DShot, Proshot ESC +* Support Spektrum 1024 /2048 , SBUS, IBUS, PPM +* Size: 27x27mm +* Mounting Hole: 20x20mm , M2.5 +* Weight: 3.3g +* DSM / IBUS/SBUS Receiver, choose UART RX2 +* PPM Receiver, don't need choose UART Port + + +## Where to buy: +* [Airbot](https://store.myairbot.com/noxv2.html) +* [Banggood](https://www.banggood.com/nl/20x20mm-Betaflight-F4-Noxe-MPU6000-Flight-Controller-AIO-OSD-5V-BEC-Built-in-LC-Filter-for-RC-Drone-p-1310419.html) diff --git a/docs/Board - WingFC.md b/docs/Board - WingFC.md new file mode 100644 index 00000000000..d5280c8c45f --- /dev/null +++ b/docs/Board - WingFC.md @@ -0,0 +1,12 @@ +# Board - Wing FC + +This FC can be bought here: https://inavflight.com/shop/s/bg/1318626 + +This board use the STM32F405RGT6 microcontroller and have the following features: +* 1024K bytes of flash memory,192K bytes RAM,168 MHz CPU/210 DMIPS + +* The 16M byte SPI flash for data logging +* USB VCP and boot select button on board(for DFU) +* Current sensor +* Supports I2C device (shared with UART1) +* PPM input (UART3 RX) diff --git a/docs/Cli.md b/docs/Cli.md index 41c19cdb382..547e1adea5a 100644 --- a/docs/Cli.md +++ b/docs/Cli.md @@ -86,6 +86,7 @@ After restoring it's always a good idea to `dump` or `diff` the settings once ag | `serialpassthrough `| where `id` is the zero based port index, `baud` is a standard baud rate, and mode is `rx`, `tx`, or both (`rxtx`) | | `set` | name=value or blank or * for list | | `status` | show system status | +| `temp_sensor` | list or configure temperature sensor(s). See docs/Temperature sensors.md | | `version` | | ## CLI Variable Reference @@ -96,15 +97,12 @@ After restoring it's always a good idea to `dump` or `diff` the settings once ag | i2c_speed | 400KHZ | This setting controls the clock speed of I2C bus. 400KHZ is the default that most setups are able to use. Some noise-free setups may be overclocked to 800KHZ. Some sensor chips or setups with long wires may work unreliably at 400KHZ - user can try lowering the clock speed to 200KHZ or even 100KHZ. User need to bear in mind that lower clock speeds might require higher looptimes (lower looptime rate) | | cpu_underclock | OFF | This option is only available on certain architectures (F3 CPUs at the moment). It makes CPU clock lower to reduce interference to long-range RC systems working at 433MHz | | gyro_sync | OFF | This option enables gyro_sync feature. In this case the loop will be synced to gyro refresh rate. Loop will always wait for the newest gyro measurement. Maximum gyro refresh rate is determined by gyro_hardware_lpf | -| acc_task_frequency | 500 | Determines accelerometer task frequency in async_mode = ALL. Depending on UAV type this frequency can be lowered to conserve CPU resources as long as vibrations are not a problem. | -| attitude_task_frequency | 250 | Determines attitude task frequency when async_mode = ALL | -| async_mode | NONE | Enables asynchronous mode processing for gyro/accelerometer and attitude computations. Allowed modes: NONE -> default behavior, all calculations are executed in main PID loop. GYRO -> gyro sampling and filtering is detached from main PID loop. PID loop runs based on looptime while gyro sampling rate is determined by the gyro driver using also the gyro_hardware_lpf setting. ALL -> in this mode, gyro, accelerometer and attitude are running as separate tasks. Accelerometer task frequency is determined by acc_task_frequency, attitude task frequency by attitude_task_frequency. In this mode ANGLE and HORIZON, as well GPS assisted flight modes (including PosHold) performance might be lowered if improper settings are used. | | min_check | 1100 | These are min/max values (in us) which, when a channel is smaller (min) or larger (max) than the value will activate various RC commands, such as arming, or stick configuration. Normally, every RC channel should be set so that min = 1000us, max = 2000us. On most transmitters this usually means 125% endpoints. Default check values are 100us above/below this value. | | max_check | 1900 | These are min/max values (in us) which, when a channel is smaller (min) or larger (max) than the value will activate various RC commands, such as arming, or stick configuration. Normally, every RC channel should be set so that min = 1000us, max = 2000us. On most transmitters this usually means 125% endpoints. Default check values are 100us above/below this value. | | rssi_channel | 0 | RX channel containing the RSSI signal | | rssi_min | 0 | The minimum RSSI value sent by the receiver, in %. For example, if your receiver's minimum RSSI value shows as 42% in the configurator/OSD set this parameter to 42. See also rssi_max. Note that rssi_min can be set to a value bigger than rssi_max to invert the RSSI calculation (i.e. bigger values mean lower RSSI). | | rssi_max | 100 | The maximum RSSI value sent by the receiver, in %. For example, if your receiver's maximum RSSI value shows as 83% in the configurator/OSD set this parameter to 83. See also rssi_min. | -| rc_smoothing | ON | Interpolation of Rc data during looptimes when there are no new updates. This gives smoother RC input to PID controller and cleaner PIDsum | +| rc_filter_frequency | 50 | RC data biquad filter cutoff frequency. Lower cutoff frequencies result in smoother response at expense of command control delay. Practical values are 20-50. Set to zero to disable entirely and use unsmoothed RC stick values | | input_filtering_mode | OFF | Filter out noise from OpenLRS Telemetry RX | | min_throttle | 1150 | These are min/max values (in us) that are sent to esc when armed. Defaults of 1150/1850 are OK for everyone, for use with AfroESC, they could be set to 1064/1864. | | max_throttle | 1850 | These are min/max values (in us) that are sent to esc when armed. Defaults of 1150/1850 are OK for everyone, for use with AfroESC, they could be set to 1064/1864. If you have brushed motors, the value should be set to 2000. | @@ -114,7 +112,7 @@ After restoring it's always a good idea to `dump` or `diff` the settings once ag | 3d_neutral | 1460 | Neutral (stop) throttle value for 3D mode | | 3d_deadband_throttle | 50 | Throttle signal will be held to a fixed value when throttle is centered with an error margin defined in this parameter. | | motor_pwm_rate | 400 | Output frequency (in Hz) for motor pins. Default is 400Hz for motor with motor_pwm_protocol set to STANDARD. For *SHOT (e.g. ONESHOT125) values of 1000 and 2000 have been tested by the development team and are supported. It may be possible to use higher values. For BRUSHED values of 8000 and above should be used. Setting to 8000 will use brushed mode at 8kHz switching frequency. Up to 32kHz is supported for brushed. Default is 16000 for boards with brushed motors. Note, that in brushed mode, minthrottle is offset to zero. For brushed mode, set max_throttle to 2000. | -| motor_pwm_protocol | STANDARD | Protocol that is used to send motor updates to ESCs. Possible values - STANDARD, ONESHOT125, ONESHOT42, MULTISHOT, BRUSHED | +| motor_pwm_protocol | STANDARD | Protocol that is used to send motor updates to ESCs. Possible values - STANDARD, ONESHOT125, ONESHOT42, MULTISHOT, DSHOT150, DSHOT300, DSHOT600, DSHOT1200, BRUSHED | | fixed_wing_auto_arm | OFF | Auto-arm fixed wing aircraft on throttle above min_throttle, and disarming with stick commands are disabled, so power cycle is required to disarm. Requires enabled motorstop and no arm switch configured. | | disarm_kill_switch | ON | Disarms the motors independently of throttle value. Setting to OFF reverts to the old behaviour of disarming only when the throttle is low. Only applies when arming and disarming with an AUX channel. | | auto_disarm_delay | 5 | Delay before automatic disarming when using stick arming and MOTOR_STOP. This does not apply when using FIXED_WING | @@ -301,6 +299,7 @@ After restoring it's always a good idea to `dump` or `diff` the settings once ag | osd_alt_alarm | 100 | Value above which to make the OSD relative altitude indicator blink (meters) | | osd_neg_alt_alarm | 5 | Value bellow which (negative altitude) to make the OSD relative altitude indicator blink (meters) | | osd_estimations_wind_compensation | ON | Use wind estimation for remaining flight time/distance estimation | +| osd_failsafe_switch_layout | OFF | If enabled the OSD automatically switches to the first layout during failsafe | | display_force_sw_blink | OFF | OFF = OSD hardware blink / ON = OSD software blink. If OSD warning text/values are invisible, try setting this to ON | | magzero_x | 0 | Magnetometer calibration X offset. If its 0 none offset has been applied and calibration is failed. | | magzero_y | 0 | Magnetometer calibration Y offset. If its 0 none offset has been applied and calibration is failed. | @@ -338,6 +337,7 @@ After restoring it's always a good idea to `dump` or `diff` the settings once ag | fw_iterm_throw_limit | 165 | Limits max/min I-term value in stabilization PID controller in case of Fixed Wing. It solves the problem of servo saturation before take-off/throwing the airplane into the air. By default, error accumulated in I-term can not exceed 1/3 of servo throw (around 165us). Set 0 to disable completely. | | fw_reference_airspeed | 1000 | Reference airspeed. Set this to airspeed at which PIDs were tuned. Usually should be set to cruise airspeed. Also used for coordinated turn calculation if airspeed sensor is not present. | | fw_turn_assist_yaw_gain | 1 | Gain required to keep the yaw rate consistent with the turn rate for a coordinated turn (in TURN_ASSIST mode). Value significantly different from 1.0 indicates a problem with the airspeed calibration (if present) or value of `fw_reference_airspeed` parameter | +| fw_loiter_direction | RIGHT | Direction of loitering: center point on right wing (clockwise - default), or center point on left wing (counterclockwise). If equal YAW then can be changed in flight using a yaw stick. | | mode_range_logic_operator | OR | Control how Mode selection works in flight modes. If you example have Angle mode configured on two different Aux channels, this controls if you need both activated ( AND ) or if you only need one activated ( OR ) to active angle mode. | | default_rate_profile | 0 | Default = profile number | | mag_declination | 0 | Current location magnetic declination in format. For example, -6deg 37min = -637 for Japan. Leading zero in ddd not required. Get your local magnetic declination here: http://magnetic-declination.com/ . Not in use if inav_auto_mag_decl is turned on and you acquire valid GPS fix. | @@ -369,7 +369,8 @@ After restoring it's always a good idea to `dump` or `diff` the settings once ag | fw_d_level | 75 | Fixed-wing attitude stabilisation HORIZON transition point | | max_angle_inclination_rll | 300 | Maximum inclination in level (angle) mode (ROLL axis). 100=10° | | max_angle_inclination_pit | 300 | Maximum inclination in level (angle) mode (PITCH axis). 100=10° | -| fw_min_throttle_down_pitch | 0 | Automatic pitch down angle when throttle is at 0 in angle mode. Progressively applied between cruise throttle and zero throttle | +| fw_iterm_limit_stick_position | 0.5 | Iterm is not allowed to grow when stick position is above threshold. This solves the problem of bounceback or followthrough when full stick deflection is applied on poorely tuned fixed wings. In other words, stabilization is partialy disabled when pilot is actively controlling the aircraft and active when sticks are not touched. `0` mean stick is in center position, `1` means it is fully deflected to either side | +| fw_min_throttle_down_pitch | 0 | Automatic pitch down angle when throttle is at 0 in angle mode. Progressively applied between cruise throttle and zero throttle (decidegrees) | | gyro_lpf_hz | 60 | Software-based filter to remove mechanical vibrations from the gyro signal. Value is cutoff frequency (Hz). For larger frames with bigger props set to lower value. | | acc_lpf_hz | 15 | Software-based filter to remove mechanical vibrations from the accelerometer measurements. Value is cutoff frequency (Hz). For larger frames with bigger props set to lower value. | | dterm_lpf_hz | 40 | | @@ -422,4 +423,13 @@ After restoring it's always a good idea to `dump` or `diff` the settings once ag | motor_accel_time | 0 | Minimum time for the motor(s) to accelerate from 0 to 100% throttle (ms) [0-1000] | | motor_decel_time | 0 | Minimum time for the motor(s) to deccelerate from 100 to 0% throttle (ms) [0-1000] | | thr_comp_weight | 0.692 | Weight used for the throttle compensation based on battery voltage. See the [battery documentation](Battery.md#automatic-throttle-compensation-based-on-battery-voltage) | - +| nav_mc_braking_speed_threshold | 100 | min speed in cm/s above which braking can happen | +| nav_mc_braking_disengage_speed | 75 | braking is disabled when speed goes below this value | +| nav_mc_braking_timeout | 2000 | timeout in ms for braking | +| nav_mc_braking_boost_factor | 100 | acceleration factor for BOOST phase | +| nav_mc_braking_boost_timeout | 750 | how long in ms BOOST phase can happen | +| nav_mc_braking_boost_speed_threshold | 150 | BOOST can be enabled when speed is above this value | +| nav_mc_braking_boost_disengage_speed | 100 | BOOST will be disabled when speed goes below this value | +| nav_mc_braking_bank_angle | 40 | max angle that MR is allowed to bank in BOOST mode | +| osd_artificial_horizon_max_pitch | 20 | Max pitch, in degrees, for OSD artificial horizon | +| baro_cal_tolerance | 150 | Baro calibration tolerance in cm. The default should allow the noisiest baro to complete calibration [cm]. | diff --git a/docs/Controls.md b/docs/Controls.md index a1a1a8b3ccb..37e3cb48401 100644 --- a/docs/Controls.md +++ b/docs/Controls.md @@ -41,6 +41,7 @@ The stick positions are combined to activate different functions: | Save current waypoint mission | LOW | CENTER | HIGH | LOW | | Load current waypoint mission | LOW | CENTER | HIGH | HIGH | | Save setting | LOW | LOW | LOW | HIGH | +| Enter OSD Menu (CMS) | CENTER | LOW | HIGH | CENTER | ![Stick Positions](assets/images/StickPositions.png) diff --git a/docs/Display.md b/docs/Display.md index ec2451cda1f..3e8e1bd7a27 100755 --- a/docs/Display.md +++ b/docs/Display.md @@ -19,10 +19,10 @@ UG-2864HSWEG01 ## Configuration -From the CLI enable the `DISPLAY` feature +From the CLI enable the `DASHBOARD` feature ``` -feature DISPLAY +feature DASHBOARD ``` @@ -72,4 +72,4 @@ More can be read about this procedure here: http://www.multiwii.com/forum/viewto ## Connections -Connect +5v, Ground, I2C SDA and I2C SCL from the flight controller to the display. \ No newline at end of file +Connect +5v, Ground, I2C SDA and I2C SCL from the flight controller to the display. diff --git a/docs/Looptime.md b/docs/Looptime.md deleted file mode 100644 index 8b2132e277c..00000000000 --- a/docs/Looptime.md +++ /dev/null @@ -1,33 +0,0 @@ -# Looptime / Control loop frequency - -Looptime / Control loop frequency (how often PID controller is executed and data is passed to motors) in INAV depends on multiple settings. - -More, with asynchronous gyro processing, not all tasks are executed in the same time. Depending on `async_mode` setting, there are following cases: - -## `async_mode = NONE` - -Gyroscope sampling and filtering, Accelerometer sampling and filtering, Attitude computation and PID control loop are executed as single tasks and processed one after another. - -| **gyro_sync** | **gyro_lpf** | **Control Loop Rate** (actual looptime) [us] | -| ---- | ---- | ----- | -| OFF | any | `looptime` | -| ON | != 256HZ | 1000 * `gyro_sync_denom` | -| ON | = 256HZ | 125 * `gyro_sync_denom` | - -## `async_mode = GYRO` - -In this mode, gyro sampling and filtering is decoupled from PID main loop and executed separately. In this mode, `gyro_sync` is forced and is always **ON** - -| **gyro_lpf** | **Control Loop Rate** [us] | Gyro looptime [us] | -| ---- | ----- | ---- | -| != 256HZ | `looptime` | 1000 * `gyro_sync_denom` | -| = 256HZ | `looptime` | 125 * `gyro_sync_denom` | - -## `async_mode = ALL` - -In this mode, Gyroscope sampling and filtering, Accelerometer sampling and filtering, Attitude computation and PID control loop are decoupled and run as separate tasks. - -| **gyro_lpf** | **Control Loop Rate** [us] | Gyro looptime [us] | Accelerometer looptime [us] | Attitude looptime [us] | -| ---- | ----- | ---- | ---- | ---- | -| != 256HZ | `looptime` | 1000 * `gyro_sync_denom` | 1000000 / `acc_task_frequency` | 1000000 / `attitude_task_frequency` | -| = 256HZ | `looptime` | 125 * `gyro_sync_denom` | 1000000 / `acc_task_frequency` | 1000000 / `attitude_task_frequency` | diff --git a/docs/Mixer.md b/docs/Mixer.md index 4c482cf6a15..604fafe5749 100644 --- a/docs/Mixer.md +++ b/docs/Mixer.md @@ -1,16 +1,18 @@ # Mixer and platform type -INAV supports a number of mixing configurations as well as custom mixing. Mixer configurations determine how the servos and motors work together to control the aircraft. +Mixing rules determine how servos and motors react to user and FC inputs. INAV supports various preset mixer configurations as well as custom mixing rules. ## Configuration -INAV Configurator provides graphical user interface for mixer configuration. All supported vehicle types are configurable with _mixer presets_ using Configurator. `mmix` and `smix` manual configuration in CLI should be used only for backup/restore purposes. +The mixer can be configured through the `Mixer` tab of the graphical user interface or using the CLI commands `mmix` and `smix`. `mmix` to define motor mixing rules and `smix` to define servo mixing rules. -User interface is described in [this video](https://www.youtube.com/watch?v=0cLFu-5syi0) +To use a mixer preset first select the platform type then the mixer preset matching your aircraft and either press the `Load and apply` or `Load mixer` buttons. The `Load and apply` button will load the mixer, save it and ask to reboot the flight controller. The `Load mixer` button only loads the preset mixing rules, you can then edit them to suit your needs and when you are done you need to press the `Save and Reboot` button to save the rules. + +Watch [this video](https://www.youtube.com/watch?v=0cLFu-5syi0) for a detailed description of the GUI and the documentation bellow for more details. ## Platform type -INAV can be used on a variety of vehicle types configured via Configurator or `platform_type` CLI property. Certain settings applies only when specific platform type is selected. For example, _flaps_ can be configured only if **AIRPLANE** platform type is used. The same goes for flight modes, output mappings, stabilization algorithms, etc. +The platform type determines what features will be available to match the type of aircraft: available flight modes, flight modes behaviour, availability of flaps and displayed types of mixer presets. It can be set through the GUI's `Mixer tab` or through the CLI's `platform_type` setting. Currently, following platform types are supported: @@ -18,129 +20,66 @@ Currently, following platform types are supported: * AIRPLANE * TRICOPTER -## Motor Mixing - -Custom motor mixing allows for completely customized motor configurations. Each motor must be defined with a custom mixing table for that motor. The mix must reflect how close each motor is with reference to the CG (Center of Gravity) of the flight controller. A motor closer to the CG of the flight controller will need to travel less distance than a motor further away. +## Writing custom mixing rules -Steps to configure custom mixer in the CLI: - -1. Use `mmix reset` to erase any existing custom mixing. -1. Issue a `mmix` statement for each motor. +## Motor Mixing -The mmix statement has the following syntax: `mmix n THROTTLE ROLL PITCH YAW` +A motor mixing rule is needed for each motor. Each rule defines weights that determine how the motor it applies to will change its speed relative to the requested throttle and flight dynamics: roll rate, pitch rate and yaw rate. The heigher a weight the more the input will have an impact on the speed of the motor. Refer to the following table for the meaning of each weight. -| Mixing table parameter | Definition | +| Weight | Definition | | ---------------------- | ---------- | -| n | Motor ordering number | -| THROTTLE | All motors that are used in this configuration are set to 1.0. Unused set to 0.0. | -| ROLL | Indicates how much roll authority this motor imparts to the roll of the flight controller. Accepts values nominally from 1.0 to -1.0. | -| PITCH | Indicates the pitch authority this motor has over the flight controller. Also accepts values nominally from 1.0 to -1.0. | -| YAW | Indicates the direction of the motor rotation in a relationship with the flight controller. 1.0 = CCW -1.0 = CW. | - -Note: the `mmix` command may show a motor mix that is not active, custom motor mixes are only active for models that use custom mixers. - -## Servo Mixing - -Custom servo mixing rules can be applied to each servo. Rules are applied in the CLI using `smix`. Rules link flight controller stabilization and receiver signals to physical PWM output pins on the FC board. Currently, pin id's 0 and 1 can only be used for motor outputs. Other pins may or may not work depending on the board you are using. - -The smix statement has the following syntax: `smix n SERVO_ID SIGNAL_SOURCE RATE SPEED` -For example, `smix 0 2 0 100 0` will create rule number 0 assigning Stabilised Roll to the third PWM pin on the FC board will full rate and no speed limit. - -| id | Flight Controller Output signal sources | -|----|-----------------| -| 0 | Stabilised ROLL | -| 1 | Stabilised PITCH | -| 2 | Stabilised YAW | -| 3 | Stabilised THROTTLE | -| 4 | RC ROLL | -| 5 | RC PITCH | -| 6 | RC YAW | -| 7 | RC THROTTLE | -| 8 | RC AUX 1 | -| 9 | RC AUX 2 | -| 10 | RC AUX 3 | -| 11 | RC AUX 4 | -| 12 | GIMBAL PITCH | -| 13 | GIMBAL ROLL | -| 14 | FEATURE FLAPS | - -| id | Servo Slot Optional Setup | -|----|--------------| -| 0 | GIMBAL PITCH | -| 1 | GIMBAL ROLL | -| 2 | ELEVATOR / SINGLECOPTER_4 | -| 3 | FLAPPERON 1 (LEFT) / SINGLECOPTER_1 | -| 4 | FLAPPERON 2 (RIGHT) / BICOPTER_LEFT / DUALCOPTER_LEFT / SINGLECOPTER_2 | -| 5 | RUDDER / BICOPTER_RIGHT / DUALCOPTER_RIGHT / SINGLECOPTER_3 | -| 6 | THROTTLE (Based ONLY on the first motor output) | -| 7 | FLAPS | - -### Servo rule rate - -Servo rule rate should be understood as a weight of a rule. To obtain full servo throw without clipping sum of all `smix` rates for a servo should equal `100`. For example, is servo #2 should be driven by sources 0 and 1 (Stabilized Roll and Stabilized Pitch) with equal strength, correct rules would be: - -``` -smix 0 2 0 50 0 -smix 1 2 1 50 0 -``` - -To obtain the stronger input of one source, increase the rate of this source while decreasing the others. For example, to drive servo #2 in 75% from source 0 and in 25% from source 1, correct rules would be: - -``` -smix 0 2 0 75 0 -smix 1 2 1 25 0 -``` - -If a sum of weights would be bigger than `100`, clipping to servo min and max values might appear. +| THROTTLE | Speed of the motor relative to throttle. Range [0.0, 1.0]. A motor with a weight of 0.5 will receive a command that will half of a motor with a 1.0 weight | +| ROLL | Indicates how much roll authority this motor imparts to the roll rate of the aircraft. Range [-1.0, 1.0]. For fixed wing models this is usually set to 0. A positive value means that the motor needs to accelerate for a positive roll rate request (rolling right). A negative value means that the motor needs to decelerate. | +| PITCH | Indicates how much pitch authority this motor imparts to the pitch rate of the aircraft. Range [-1.0, 1.0]. For fixed wing models this is usually set to 0. A positive value means that the motor needs to accelerate for a positive pitch rate request (pitching down). A negative value means that the motor needs to decelerate. | +| YAW | Indicates how much yaw authority this motor imparts to the yaw rate of the aircraft. Range [-1.0, 1.0]. For fixed wing models with more than one motor this weight can be used to setup differential thrust. For fixed wing models with only one motor this is usually set to 0. A positive value means that the motor needs to accelerate for a positive yaw rate request (clockwise yaw seen from the top of the model). A negative value means that the motor needs to decelerate | -> Note: the `smix` command may show a servo mix that is not active, custom servo mixes are only active for models that use custom mixers. +CLI commands to configure motor mixing rules: -### Servo speed +The `mmix reset` command removes all the existing motor mixing rules. -Custom servo mixer allows defining the speed of change for given servo rule. By default, all speeds are set to `0`, that means limiting is _NOT_ applied and rules source is directly written to a servo. That mean, if, for example, source (AUX) changes from 1000 to 2000 in one cycle, servo output will also change from 1000 to 2000 in one cycle. In this case, speed is limited only by the servo itself. +The `mmix` command is used to list, create or modify rules. To list the currently defined rules run the `mmix` command without parameters. -If value different than `0` is set as rule speed, the speed of change will be lowered accordingly. +To create or modify rules use the `mmix` command with the following syntax: `mmix `. `` is representing the index of the motor output pin (integer). The other parameters are decimal weights for each of the inputs. To disable a mixing rule set the `throttle` weight to 0. -`1 speed = 10 us/s` - -**Example speed values** -* 0 = no limiting -* 1 = 10us/s -> full servo sweep (from 1000 to 2000) is performed in 100s -* 10 = 100us/s -> full sweep (from 1000 to 2000) is performed in 10s -* 100 = 1000us/s -> full sweep in 1s -* 200 = 2000us/s -> full sweep in 0.5s - -Servo speed might be useful for functions like flaps, landing gear retraction and other where full speed provided for hardware is too much. - -## Servo Reversing - -Servos can be reversed using Configurator _Servo_ tab and _Reverse_ checkbox. - -`smix reverse` command is still working but it is advised not to use it anymore and it might be removed in next releases of firmware. - -## Servo configuration - -The cli `servo` command defines the settings for the servo outputs. -The cli mixer `smix` command controllers how the mixer maps internal FC data (RC input, PID stabilization output, channel forwarding, etc) to servo outputs. - -## Servo filtering - -A low-pass filter can be enabled for the servos. It may be useful for avoiding structural modes in the airframe, for example. - -### Configuration - -Currently, it can only be configured via the CLI: - -Use `set servo_lpf_hz=20` to enable filtering. This will set servo low pass filter to 20Hz. - -### Tuning - -One method for tuning the filter cutoff is as follows: - -1. Ensure your vehicle can move at least somewhat freely in the troublesome axis. For example, if you are having yaw oscillations on a tricopter, ensure that the copter is supported in a way that allows it to rotate left and right to at least some degree. Suspension near the CG is ideal. Alternatively, you can just fly the vehicle and trigger the problematic condition you are trying to eliminate, although tuning will be more tedious. - -2. Tap the vehicle at its end in the axis under evaluation. Directly commanding the servo in question to move may also be used. In the tricopter example, tap the end of the tail boom from the side, or command a yaw using your transmitter. - -3. If your vehicle oscillates for several seconds or even continues oscillating indefinitely, then the filter cutoff frequency should be reduced. Reduce the value of `servo_lowpass_freq` by half its current value and repeat the previous step. +## Servo Mixing -4. If the oscillations are dampened within roughly a second or are no longer present, then you are done. Be sure to run `save`. +At least one servo mixing rule is needed for each servo. Each rule defines how a servo will move relative to a specific input like a RC channel, or a requested flight dynamics rate or position from the flight controller. + +Each servo mixing rule has the following parameters: +* Servo index: defines which servo the rule will apply to. The absolute value of the index is not important, what matters is only the relative difference between the used indexes. The rule with the smaller servo index will apply to the first servo, the next higher servo index to the second servo, etc. More than one rule can use the same servo index. The output of the rules with the same servo index are added together to give the final output for the specified servo. +* Input: the input for the mixing rule, see a summary of the input types table bellow. +* Weight: percentage of the input to forward to the servo. Range [-1000, 1000]. Mixing rule output = input * weight. If the output of a set of mixing rules is lower/higher than the defined servo min/max the output is clipped (the servo will never travel farther than the set min/max). +* Speed: maximum rate of change of the mixing rule output. Used to limit the servo speed. 1 corresponds to maximum 10µs/s output rate of change. Set to 0 for no speed limit. For example: 10 = full sweep (1000 to 2000) in 10s, 100 = full sweep in 1s. + +| CLI input ID | Mixer input | Description | +|----|--------------------------|------------------------------------------------------------------------------| +| 0 | Stabilised ROLL | Roll command from the flight controller. Depends on the selected flight mode(s) | +| 1 | Stabilised PITCH | Pitch command from the flight controller. Depends on the selected flight mode(s) | +| 2 | Stabilised YAW | Yaw command from the flight controller. Depends on the selected flight mode(s) | +| 3 | Stabilised THROTTLE | Throttle command from the flight controller. Depends on the selected flight mode(s) | +| 4 | RC ROLL | Raw roll RC channel | +| 5 | RC PITCH | Raw pitch RC channel | +| 6 | RC YAW | Raw yaw RC channel | +| 7 | RC THROTTLE | Raw throttle RC channel | +| 8 | RC channel 5 | Raw RC channel 5 | +| 9 | RC channel 6 | Raw RC channel 6 | +| 10 | RC channel 7 | Raw RC channel 7 | +| 11 | RC channel 8 | Raw RC channel 8 | +| 12 | GIMBAL PITCH | Scaled pitch attitude of the aircraft [-90°, 90°] => [-500, 500] | +| 13 | GIMBAL ROLL | Scaled roll attitude of the aircraft [-180°, 180°] => [-500, 500] | +| 14 | FEATURE FLAPS | This input value is equal to the `flaperon_throw_offset` setting when the `FLAPERON` flight mode is enabled, 0 otherwise | +| 15 | RC channel 9 | Raw RC channel 9 | +| 16 | RC channel 10 | Raw RC channel 10 | +| 17 | RC channel 11 | Raw RC channel 11 | +| 18 | RC channel 12 | Raw RC channel 12 | +| 19 | RC channel 13 | Raw RC channel 13 | +| 20 | RC channel 14 | Raw RC channel 14 | +| 21 | RC channel 15 | Raw RC channel 15 | +| 22 | RC channel 16 | Raw RC channel 16 | + + +The `smix reset` command removes all the existing motor mixing rules. + +The `smix` command is used to list, create or modify rules. To list the currently defined rules run the `smix` command without parameters. + +To create or modify rules use the `smix` command with the following syntax: `smix `. `` is representing the index of the servo mixing rule to create or modify (integer). To disable a mixing rule set the weight to 0. diff --git a/docs/Rx.md b/docs/Rx.md index b90a1c782d4..3436037b45f 100644 --- a/docs/Rx.md +++ b/docs/Rx.md @@ -168,7 +168,7 @@ Only one receiver feature can be enabled at a time. The software has signal loss detection which is always enabled. Signal loss detection is used for safety and failsafe reasons. -The `rx_min_usec` and `rx_max_usec` settings helps detect when your RX stops sending any data, enters failsafe mode or when the RX looses signal. +The `rx_min_usec` and `rx_max_usec` settings helps detect when your RX stops sending any data, enters failsafe mode or when the RX loses signal. By default, when the signal loss is detected the FC will set pitch/roll/yaw to the value configured for `mid_rc`. The throttle will be set to the value configured for `rx_min_usec` or `mid_rc` if using 3D feature. @@ -178,49 +178,6 @@ Signal loss can be detected when: 2. using Serial RX and receiver indicates failsafe condition. 3. using any of the first 4 stick channels do not have a value in the range specified by `rx_min_usec` and `rx_max_usec`. -### RX loss configuration - -The `rxfail` cli command is used to configure per-channel rx-loss behaviour. -You can use the `rxfail` command to change this behaviour. -A flight channel can either be AUTOMATIC or HOLD, an AUX channel can either be SET or HOLD. - -* AUTOMATIC - Flight channels are set to safe values (low throttle, mid position for yaw/pitch/roll). -* HOLD - Channel holds the last value. -* SET - Channel is set to a specific configured value. - -The default mode is AUTOMATIC for flight channels and HOLD for AUX channels. - -The rxfail command can be used in conjunction with mode ranges to trigger various actions. - -The `rxfail` command takes 2 or 3 arguments. -* Index of channel (See below) -* Mode ('a' = AUTOMATIC, 'h' = HOLD, 's' = SET) -* A value to use when in SET mode. - -Channels are always specified in the same order, regardless of your channel mapping. - -* Roll is 0 -* Pitch is 1 -* Yaw is 2 -* Throttle is 3. -* Aux channels are 4 onwards. - -Examples: - -To make Throttle channel have an automatic value when RX loss is detected: - -`rxfail 3 a` - -To make AUX4 have a value of 2000 when RX loss is detected: - -`rxfail 7 s 2000` - -To make AUX8 hold it's value when RX loss is detected: - -`rxfail 11 h` - -WARNING: Always make sure you test the behavior is as expected after configuring rxfail settings! - #### `rx_min_usec` The lowest channel value considered valid. e.g. PWM/PPM pulse length diff --git a/docs/Servo.md b/docs/Servo.md new file mode 100644 index 00000000000..42acfaa3c9b --- /dev/null +++ b/docs/Servo.md @@ -0,0 +1,37 @@ +# Servo configuration + +Servos can be configured from the graphical user interface's `Servos` tab. + +* MID: middle/neutral point of the servo +* MIN: the minimum value that can be sent to the servo is MIN * Rate +* MAX: the maximum value that can be sent to the servo is MAX * Rate +* Rate: servo command = servo rate * mixer output +* Reverse: if enabled the servo output is reversed + +CLI commands to configure servos: + +The `servo` command is used to list or modify servo's configuration. To list the current servo's configuration run the `servo` command without parameters. + +To change the configuration of a servo use the `servo` command with the following syntax: `servo `. `` is representing the index of the servo output defined by a servo mixer (See (mixer documentation)[https://github.com/iNavFlight/inav/blob/master/docs/Mixer.md]). The other parameters must be positive integers apart from the rate wich valid range is [-125, 125]. + +## Servo filtering + +A low-pass filter can be enabled for the servos. It may be useful for avoiding structural modes in the airframe, for example. + +### Configuration + +Currently, it can only be configured via the CLI: + +Use `set servo_lpf_hz=20` to enable filtering. This will set servo low pass filter to 20Hz. + +### Tuning + +One method for tuning the filter cutoff is as follows: + +1. Ensure your vehicle can move at least somewhat freely in the troublesome axis. For example, if you are having yaw oscillations on a tricopter, ensure that the copter is supported in a way that allows it to rotate left and right to at least some degree. Suspension near the CG is ideal. Alternatively, you can just fly the vehicle and trigger the problematic condition you are trying to eliminate, although tuning will be more tedious. + +2. Tap the vehicle at its end in the axis under evaluation. Directly commanding the servo in question to move may also be used. In the tricopter example, tap the end of the tail boom from the side, or command a yaw using your transmitter. + +3. If your vehicle oscillates for several seconds or even continues oscillating indefinitely, then the filter cutoff frequency should be reduced. Reduce the value of `servo_lowpass_freq` by half its current value and repeat the previous step. + +4. If the oscillations are dampened within roughly a second or are no longer present, then you are done. Be sure to run `save`. diff --git a/docs/Temperature sensors.md b/docs/Temperature sensors.md new file mode 100644 index 00000000000..b63a980a9ca --- /dev/null +++ b/docs/Temperature sensors.md @@ -0,0 +1,104 @@ +# Temperature sensors + +It is now possible to measure temperatures with the help of the I²C LM75 and 1-Wire DS18B20 chips. The temperature values can be displayed on the OSD and they are also logged. A total maximum of 8 temperature sensors can be connected. The support is enabled by default on F4 and F7 FCs. To use with F3 FCs you need to build a custom firmware. + +## LM75 + +Up to 8 can be connected to the flight controller. + +* Package: SOP-8, breakout boards can be found easily +* Interface: I²C +* Supply: 2.7 to 5.5V +* Temperature range: -55 to +125°C + +## DS18B20 + +* Package: TO-92, SO-8, µSOP-8 +* Interface: 1-Wire +* Supply: 3.0 to 5.5V (parasitic power not supported) +* Temperature range: -55 to +125°C + +None of the flight controllers on the market at the time this documentation is written supports 1-Wire directly. To use these sensors a I²C to 1-Wire interface chip needs to be used, the DS2482. Connect the DS2482 SCL and SDA lines to your FC, add a 4.7kohm pull-up resistor between VCC and the DQ pin then connect all the sensors DQ pin to the DS2482 DQ pin. + +## Configuring temperature sensors + +The `temp_sensor` CLI command can be used to display and change the temperature sensors configuration. When a new temperature sensor is connected it is automatically detected and will appear in the output of the `temp_sensor` command. + +* `temp_sensor` without any argument displays all the sensors configuration +* `temp_sensor reset` deletes all the sensors +* `temp_sensor index type address alarm_min alarm_max osd_symbol label` to configure a new sensor or modify the configuration of an existing one. + +### Parameters description + +* `index` is the index of the configuration slot you want to change +* `type` can be `1` for LM75 or `2` for DS18B20 +* `address` is the address of the device on the bus. 0 to 7 for a LM75 or the full 64bit ROM in hex format for a 18B20 +* `alarm_min` is the temperature under which the corresponding OSD element will start blinking (decidegrees centigrade) +* `alarm_max` is the temperature above which the corresponding OSD element will start blinking (decidegrees centigrade) +* `osd_symbol` is the ID of a symbol to display on the OSD to the left of the temperature value. Use 0 to display a label instea (see next parameter). See the table bellow for the available IDs +* `label` is a 4 characters maximum label that is displayed on the OSD next to the temperature value + +| Symbol ID | Description | +|-----------|-----------------------------| +| 1 | Generic temperature symbol | +| 2 | ESC temperature symbol | +| 3 | VTX temperature symbol | +| 4 | Motor temperature symbol | +| 5 | Battery temperature symbol | +| 6 | Exterior temperature symbol | + +### Example output + +Example output of the `temp_sensor` command on a system with two LM75 and four DS18B20 sensors connected + +``` +temp_sensor 0 1 0 -200 600 0 +temp_sensor 1 1 1 -200 600 0 +temp_sensor 2 2 7c0118681e1cff28 -200 600 0 +temp_sensor 3 2 7d01186838f2ff28 -200 600 0 +temp_sensor 4 2 210118684001ff28 -200 600 0 +temp_sensor 5 2 f801186750c7ff28 -200 600 0 +temp_sensor 6 0 0 0 0 +temp_sensor 7 0 0 0 0 +``` + +To set for example the OSD symbol of the first temperature sensor to the ESC symbol: + +`temp_sensor 0 1 0 -200 600 2` + +To change for example the configuration of the fourth sensor to label `BATT`, minimum value alarm 0.5°C and maximum value alarm 45°C + +`temp_sensor 3 2 7d01186838f2ff28 5 450 0 BATT` + +## Building a custom firmware with temperature sensor support (F3 only) + +This needs to be added in the `target.h` file: + +``` +#define USE_TEMPERATURE_SENSOR +#define TEMPERATURE_I2C_BUS BUS_I2Cx // replace x with the index of the I²C bus the temperature sensors will be connected to + +// for LM75 sensors support +#define USE_TEMPERATURE_LM75 + +// for DS18B20 sensors +#define USE_1WIRE +#define USE_1WIRE_DS2482 +#define USE_TEMPERATURE_DS18B20 +``` + +## Configuring the way OSD temperature labels are displayed + +You can use the `osd_temp_label_align` setting to chose how the labels for the temperature sensor's values are displayed. Possible alignment values are `LEFT` and `RIGHT`. + +### Example + +``` +LEFT alignment: +T1 xxx°C +ESC xxx°C + +RIGHT alignment: + T1 xxx°C +ESC xxx°C +``` diff --git a/docs/assets/images/StickPositions.png b/docs/assets/images/StickPositions.png index 658423f5954..32006c1c122 100644 Binary files a/docs/assets/images/StickPositions.png and b/docs/assets/images/StickPositions.png differ diff --git a/docs/assets/images/StickPositions.svg b/docs/assets/images/StickPositions.svg index d6a98e7af35..17bb86ba255 100644 --- a/docs/assets/images/StickPositions.svg +++ b/docs/assets/images/StickPositions.svg @@ -14,7 +14,7 @@ viewBox="0 0 8782.4284 4964.1146" id="svg2" version="1.1" - inkscape:version="0.92.2 (5c3e80d, 2017-08-06)" + inkscape:version="0.92.4 (5da689c313, 2019-01-14)" sodipodi:docname="StickPositions.svg" inkscape:export-filename="C:\Users\stuphi\Dropbox\projects\quad\StickPositions.png" inkscape:export-xdpi="74.996788" @@ -778,6 +778,17 @@ y="1227.7418" id="tspan6015" style="font-size:286.5987854px;line-height:1.25;text-align:end;text-anchor:end">Save Setting + Enter OSD Menu (CMS) + + + + diff --git a/lib/main/STM32F7/Drivers/STM32F7xx_HAL_Driver/Src/stm32f7xx_ll_usb.c b/lib/main/STM32F7/Drivers/STM32F7xx_HAL_Driver/Src/stm32f7xx_ll_usb.c index c3ee209cc09..ffc4f247fe7 100644 --- a/lib/main/STM32F7/Drivers/STM32F7xx_HAL_Driver/Src/stm32f7xx_ll_usb.c +++ b/lib/main/STM32F7/Drivers/STM32F7xx_HAL_Driver/Src/stm32f7xx_ll_usb.c @@ -5,21 +5,21 @@ * @version V1.2.2 * @date 14-April-2017 * @brief USB Low Layer HAL module driver. - * - * This file provides firmware functions to manage the following + * + * This file provides firmware functions to manage the following * functionalities of the USB Peripheral Controller: * + Initialization/de-initialization functions * + I/O operation functions - * + Peripheral Control functions + * + Peripheral Control functions * + Peripheral State functions - * + * @verbatim ============================================================================== ##### How to use this driver ##### ============================================================================== [..] (#) Fill parameters of Init structure in USB_OTG_CfgTypeDef structure. - + (#) Call USB_CoreInit() API to initialize the USB Core peripheral. (#) The upper HAL HCD/PCD driver will call the right routines for its internal processes. @@ -53,7 +53,7 @@ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * ****************************************************************************** - */ + */ /* Includes ------------------------------------------------------------------*/ #include "stm32f7xx_hal.h" @@ -72,7 +72,7 @@ /* Private functions ---------------------------------------------------------*/ static HAL_StatusTypeDef USB_CoreReset(USB_OTG_GlobalTypeDef *USBx); -#ifdef USB_HS_PHYC +#ifdef USB_HS_PHYC static HAL_StatusTypeDef USB_HS_PHYCInit(USB_OTG_GlobalTypeDef *USBx); #endif @@ -81,15 +81,15 @@ static HAL_StatusTypeDef USB_HS_PHYCInit(USB_OTG_GlobalTypeDef *USBx); * @{ */ -/** @defgroup LL_USB_Group1 Initialization/de-initialization functions - * @brief Initialization and Configuration functions +/** @defgroup LL_USB_Group1 Initialization/de-initialization functions + * @brief Initialization and Configuration functions * -@verbatim +@verbatim =============================================================================== ##### Initialization/de-initialization functions ##### =============================================================================== [..] This section provides functions allowing to: - + @endverbatim * @{ */ @@ -105,12 +105,12 @@ HAL_StatusTypeDef USB_CoreInit(USB_OTG_GlobalTypeDef *USBx, USB_OTG_CfgTypeDef c { if (cfg.phy_itface == USB_OTG_ULPI_PHY) { - + USBx->GCCFG &= ~(USB_OTG_GCCFG_PWRDWN); /* Init The ULPI Interface */ USBx->GUSBCFG &= ~(USB_OTG_GUSBCFG_TSDPS | USB_OTG_GUSBCFG_ULPIFSLS | USB_OTG_GUSBCFG_PHYSEL); - + /* Select vbus source */ USBx->GUSBCFG &= ~(USB_OTG_GUSBCFG_ULPIEVBUSD | USB_OTG_GUSBCFG_ULPIEVBUSI); if(cfg.use_external_vbus == 1) @@ -118,53 +118,53 @@ HAL_StatusTypeDef USB_CoreInit(USB_OTG_GlobalTypeDef *USBx, USB_OTG_CfgTypeDef c USBx->GUSBCFG |= USB_OTG_GUSBCFG_ULPIEVBUSD; } /* Reset after a PHY select */ - USB_CoreReset(USBx); + USB_CoreReset(USBx); } -#ifdef USB_HS_PHYC - +#ifdef USB_HS_PHYC + else if (cfg.phy_itface == USB_OTG_HS_EMBEDDED_PHY) { USBx->GCCFG &= ~(USB_OTG_GCCFG_PWRDWN); - + /* Init The UTMI Interface */ USBx->GUSBCFG &= ~(USB_OTG_GUSBCFG_TSDPS | USB_OTG_GUSBCFG_ULPIFSLS | USB_OTG_GUSBCFG_PHYSEL); - + /* Select vbus source */ USBx->GUSBCFG &= ~(USB_OTG_GUSBCFG_ULPIEVBUSD | USB_OTG_GUSBCFG_ULPIEVBUSI); - + /* Select UTMI Interace */ USBx->GUSBCFG &= ~ USB_OTG_GUSBCFG_ULPI_UTMI_SEL; USBx->GCCFG |= USB_OTG_GCCFG_PHYHSEN; - + /* Enables control of a High Speed USB PHY */ USB_HS_PHYCInit(USBx); - + if(cfg.use_external_vbus == 1) { USBx->GUSBCFG |= USB_OTG_GUSBCFG_ULPIEVBUSD; } /* Reset after a PHY select */ - USB_CoreReset(USBx); - + USB_CoreReset(USBx); + } #endif else /* FS interface (embedded Phy) */ { /* Select FS Embedded PHY */ USBx->GUSBCFG |= USB_OTG_GUSBCFG_PHYSEL; - + /* Reset after a PHY select and set Host mode */ USB_CoreReset(USBx); - + /* Deactivate the power down*/ USBx->GCCFG = USB_OTG_GCCFG_PWRDWN; } - + if(cfg.dma_enable == ENABLE) { USBx->GAHBCFG |= USB_OTG_GAHBCFG_HBSTLEN_2; USBx->GAHBCFG |= USB_OTG_GAHBCFG_DMAEN; - } + } return HAL_OK; } @@ -193,7 +193,7 @@ HAL_StatusTypeDef USB_DisableGlobalInt(USB_OTG_GlobalTypeDef *USBx) USBx->GAHBCFG &= ~USB_OTG_GAHBCFG_GINT; return HAL_OK; } - + /** * @brief USB_SetCurrentMode : Set functional mode * @param USBx : Selected device @@ -201,28 +201,28 @@ HAL_StatusTypeDef USB_DisableGlobalInt(USB_OTG_GlobalTypeDef *USBx) * This parameter can be one of these values: * @arg USB_OTG_DEVICE_MODE: Peripheral mode * @arg USB_OTG_HOST_MODE: Host mode - * @arg USB_OTG_DRD_MODE: Dual Role Device mode + * @arg USB_OTG_DRD_MODE: Dual Role Device mode * @retval HAL status */ HAL_StatusTypeDef USB_SetCurrentMode(USB_OTG_GlobalTypeDef *USBx , USB_OTG_ModeTypeDef mode) { - USBx->GUSBCFG &= ~(USB_OTG_GUSBCFG_FHMOD | USB_OTG_GUSBCFG_FDMOD); - + USBx->GUSBCFG &= ~(USB_OTG_GUSBCFG_FHMOD | USB_OTG_GUSBCFG_FDMOD); + if ( mode == USB_OTG_HOST_MODE) { - USBx->GUSBCFG |= USB_OTG_GUSBCFG_FHMOD; + USBx->GUSBCFG |= USB_OTG_GUSBCFG_FHMOD; } else if ( mode == USB_OTG_DEVICE_MODE) { - USBx->GUSBCFG |= USB_OTG_GUSBCFG_FDMOD; + USBx->GUSBCFG |= USB_OTG_GUSBCFG_FDMOD; } HAL_Delay(50); - + return HAL_OK; } /** - * @brief USB_DevInit : Initializes the USB_OTG controller registers + * @brief USB_DevInit : Initializes the USB_OTG controller registers * for device mode * @param USBx : Selected device * @param cfg : pointer to a USB_OTG_CfgTypeDef structure that contains @@ -235,51 +235,51 @@ HAL_StatusTypeDef USB_DevInit (USB_OTG_GlobalTypeDef *USBx, USB_OTG_CfgTypeDef c /*Activate VBUS Sensing B */ USBx->GCCFG |= USB_OTG_GCCFG_VBDEN; - + if (cfg.vbus_sensing_enable == 0) { /* Deactivate VBUS Sensing B */ USBx->GCCFG &= ~ USB_OTG_GCCFG_VBDEN; - - /* B-peripheral session valid override enable*/ + + /* B-peripheral session valid override enable*/ USBx->GOTGCTL |= USB_OTG_GOTGCTL_BVALOEN; USBx->GOTGCTL |= USB_OTG_GOTGCTL_BVALOVAL; } - + /* Restart the Phy Clock */ USBx_PCGCCTL = 0; /* Device mode configuration */ USBx_DEVICE->DCFG |= DCFG_FRAME_INTERVAL_80; - + if(cfg.phy_itface == USB_OTG_ULPI_PHY) { if(cfg.speed == USB_OTG_SPEED_HIGH) - { + { /* Set High speed phy */ USB_SetDevSpeed (USBx , USB_OTG_SPEED_HIGH); } - else + else { /* set High speed phy in Full speed mode */ USB_SetDevSpeed (USBx , USB_OTG_SPEED_HIGH_IN_FULL); } } - + else if(cfg.phy_itface == USB_OTG_HS_EMBEDDED_PHY) { if(cfg.speed == USB_OTG_SPEED_HIGH) - { + { /* Set High speed phy */ USB_SetDevSpeed (USBx , USB_OTG_SPEED_HIGH); } - else + else { /* set High speed phy in Full speed mode */ USB_SetDevSpeed (USBx , USB_OTG_SPEED_HIGH_IN_FULL); } } - + else { /* Set Full speed phy */ @@ -289,13 +289,13 @@ HAL_StatusTypeDef USB_DevInit (USB_OTG_GlobalTypeDef *USBx, USB_OTG_CfgTypeDef c /* Flush the FIFOs */ USB_FlushTxFifo(USBx , 0x10); /* all Tx FIFOs */ USB_FlushRxFifo(USBx); - + /* Clear all pending Device Interrupts */ USBx_DEVICE->DIEPMSK = 0; USBx_DEVICE->DOEPMSK = 0; USBx_DEVICE->DAINT = 0xFFFFFFFF; USBx_DEVICE->DAINTMSK = 0; - + for (i = 0; i < cfg.dev_endpoints; i++) { if ((USBx_INEP(i)->DIEPCTL & USB_OTG_DIEPCTL_EPENA) == USB_OTG_DIEPCTL_EPENA) @@ -306,11 +306,11 @@ HAL_StatusTypeDef USB_DevInit (USB_OTG_GlobalTypeDef *USBx, USB_OTG_CfgTypeDef c { USBx_INEP(i)->DIEPCTL = 0; } - + USBx_INEP(i)->DIEPTSIZ = 0; USBx_INEP(i)->DIEPINT = 0xFF; } - + for (i = 0; i < cfg.dev_endpoints; i++) { if ((USBx_OUTEP(i)->DOEPCTL & USB_OTG_DOEPCTL_EPENA) == USB_OTG_DOEPCTL_EPENA) @@ -321,40 +321,40 @@ HAL_StatusTypeDef USB_DevInit (USB_OTG_GlobalTypeDef *USBx, USB_OTG_CfgTypeDef c { USBx_OUTEP(i)->DOEPCTL = 0; } - + USBx_OUTEP(i)->DOEPTSIZ = 0; USBx_OUTEP(i)->DOEPINT = 0xFF; } - + USBx_DEVICE->DIEPMSK &= ~(USB_OTG_DIEPMSK_TXFURM); - + if (cfg.dma_enable == 1) { /*Set threshold parameters */ USBx_DEVICE->DTHRCTL = (USB_OTG_DTHRCTL_TXTHRLEN_6 | USB_OTG_DTHRCTL_RXTHRLEN_6); USBx_DEVICE->DTHRCTL |= (USB_OTG_DTHRCTL_RXTHREN | USB_OTG_DTHRCTL_ISOTHREN | USB_OTG_DTHRCTL_NONISOTHREN); - + i= USBx_DEVICE->DTHRCTL; } - + /* Disable all interrupts. */ USBx->GINTMSK = 0; - + /* Clear any pending interrupts */ USBx->GINTSTS = 0xBFFFFFFF; /* Enable the common interrupts */ if (cfg.dma_enable == DISABLE) { - USBx->GINTMSK |= USB_OTG_GINTMSK_RXFLVLM; + USBx->GINTMSK |= USB_OTG_GINTMSK_RXFLVLM; } - + /* Enable interrupts matching to the Device mode ONLY */ USBx->GINTMSK |= (USB_OTG_GINTMSK_USBSUSPM | USB_OTG_GINTMSK_USBRST |\ USB_OTG_GINTMSK_ENUMDNEM | USB_OTG_GINTMSK_IEPINT |\ USB_OTG_GINTMSK_OEPINT | USB_OTG_GINTMSK_IISOIXFRM|\ USB_OTG_GINTMSK_PXFRM_IISOOXFRM | USB_OTG_GINTMSK_WUIM); - + if(cfg.Sof_enable) { USBx->GINTMSK |= USB_OTG_GINTMSK_SOFM; @@ -362,9 +362,9 @@ HAL_StatusTypeDef USB_DevInit (USB_OTG_GlobalTypeDef *USBx, USB_OTG_CfgTypeDef c if (cfg.vbus_sensing_enable == ENABLE) { - USBx->GINTMSK |= (USB_OTG_GINTMSK_SRQIM | USB_OTG_GINTMSK_OTGINT); + USBx->GINTMSK |= (USB_OTG_GINTMSK_SRQIM | USB_OTG_GINTMSK_OTGINT); } - + return HAL_OK; } @@ -380,9 +380,9 @@ HAL_StatusTypeDef USB_DevInit (USB_OTG_GlobalTypeDef *USBx, USB_OTG_CfgTypeDef c HAL_StatusTypeDef USB_FlushTxFifo (USB_OTG_GlobalTypeDef *USBx, uint32_t num ) { uint32_t count = 0; - - USBx->GRSTCTL = ( USB_OTG_GRSTCTL_TXFFLSH |(uint32_t)( num << 6)); - + + USBx->GRSTCTL = ( USB_OTG_GRSTCTL_TXFFLSH |(uint32_t)( num << 6)); + do { if (++count > 200000) @@ -391,7 +391,7 @@ HAL_StatusTypeDef USB_FlushTxFifo (USB_OTG_GlobalTypeDef *USBx, uint32_t num ) } } while ((USBx->GRSTCTL & USB_OTG_GRSTCTL_TXFFLSH) == USB_OTG_GRSTCTL_TXFFLSH); - + return HAL_OK; } @@ -404,9 +404,9 @@ HAL_StatusTypeDef USB_FlushTxFifo (USB_OTG_GlobalTypeDef *USBx, uint32_t num ) HAL_StatusTypeDef USB_FlushRxFifo(USB_OTG_GlobalTypeDef *USBx) { uint32_t count = 0; - + USBx->GRSTCTL = USB_OTG_GRSTCTL_RXFFLSH; - + do { if (++count > 200000) @@ -415,12 +415,12 @@ HAL_StatusTypeDef USB_FlushRxFifo(USB_OTG_GlobalTypeDef *USBx) } } while ((USBx->GRSTCTL & USB_OTG_GRSTCTL_RXFFLSH) == USB_OTG_GRSTCTL_RXFFLSH); - + return HAL_OK; } /** - * @brief USB_SetDevSpeed :Initializes the DevSpd field of DCFG register + * @brief USB_SetDevSpeed :Initializes the DevSpd field of DCFG register * depending the PHY type and the enumeration speed of the device. * @param USBx : Selected device * @param speed : device speed @@ -438,7 +438,7 @@ HAL_StatusTypeDef USB_SetDevSpeed(USB_OTG_GlobalTypeDef *USBx , uint8_t speed) } /** - * @brief USB_GetDevSpeed :Return the Dev Speed + * @brief USB_GetDevSpeed :Return the Dev Speed * @param USBx : Selected device * @retval speed : device speed * This parameter can be one of these values: @@ -449,7 +449,7 @@ HAL_StatusTypeDef USB_SetDevSpeed(USB_OTG_GlobalTypeDef *USBx , uint8_t speed) uint8_t USB_GetDevSpeed(USB_OTG_GlobalTypeDef *USBx) { uint8_t speed = 0; - + if((USBx_DEVICE->DSTS & USB_OTG_DSTS_ENUMSPD) == DSTS_ENUMSPD_HS_PHY_30MHZ_OR_60MHZ) { speed = USB_OTG_SPEED_HIGH; @@ -463,7 +463,7 @@ uint8_t USB_GetDevSpeed(USB_OTG_GlobalTypeDef *USBx) { speed = USB_OTG_SPEED_LOW; } - + return speed; } @@ -478,23 +478,23 @@ HAL_StatusTypeDef USB_ActivateEndpoint(USB_OTG_GlobalTypeDef *USBx, USB_OTG_EPTy if (ep->is_in == 1) { USBx_DEVICE->DAINTMSK |= USB_OTG_DAINTMSK_IEPM & ((1 << (ep->num))); - + if (((USBx_INEP(ep->num)->DIEPCTL) & USB_OTG_DIEPCTL_USBAEP) == 0) { USBx_INEP(ep->num)->DIEPCTL |= ((ep->maxpacket & USB_OTG_DIEPCTL_MPSIZ ) | (ep->type << 18 ) |\ - ((ep->num) << 22 ) | (USB_OTG_DIEPCTL_SD0PID_SEVNFRM) | (USB_OTG_DIEPCTL_USBAEP)); - } + ((ep->num) << 22 ) | (USB_OTG_DIEPCTL_SD0PID_SEVNFRM) | (USB_OTG_DIEPCTL_USBAEP)); + } } else { USBx_DEVICE->DAINTMSK |= USB_OTG_DAINTMSK_OEPM & ((1 << (ep->num)) << 16); - + if (((USBx_OUTEP(ep->num)->DOEPCTL) & USB_OTG_DOEPCTL_USBAEP) == 0) { USBx_OUTEP(ep->num)->DOEPCTL |= ((ep->maxpacket & USB_OTG_DOEPCTL_MPSIZ ) | (ep->type << 18 ) |\ (USB_OTG_DIEPCTL_SD0PID_SEVNFRM)| (USB_OTG_DOEPCTL_USBAEP)); - } + } } return HAL_OK; } @@ -507,20 +507,20 @@ HAL_StatusTypeDef USB_ActivateEndpoint(USB_OTG_GlobalTypeDef *USBx, USB_OTG_EPTy HAL_StatusTypeDef USB_ActivateDedicatedEndpoint(USB_OTG_GlobalTypeDef *USBx, USB_OTG_EPTypeDef *ep) { static __IO uint32_t debug = 0; - + /* Read DEPCTLn register */ if (ep->is_in == 1) { if (((USBx_INEP(ep->num)->DIEPCTL) & USB_OTG_DIEPCTL_USBAEP) == 0) { USBx_INEP(ep->num)->DIEPCTL |= ((ep->maxpacket & USB_OTG_DIEPCTL_MPSIZ ) | (ep->type << 18 ) |\ - ((ep->num) << 22 ) | (USB_OTG_DIEPCTL_SD0PID_SEVNFRM) | (USB_OTG_DIEPCTL_USBAEP)); - } - - + ((ep->num) << 22 ) | (USB_OTG_DIEPCTL_SD0PID_SEVNFRM) | (USB_OTG_DIEPCTL_USBAEP)); + } + + debug |= ((ep->maxpacket & USB_OTG_DIEPCTL_MPSIZ ) | (ep->type << 18 ) |\ - ((ep->num) << 22 ) | (USB_OTG_DIEPCTL_SD0PID_SEVNFRM) | (USB_OTG_DIEPCTL_USBAEP)); - + ((ep->num) << 22 ) | (USB_OTG_DIEPCTL_SD0PID_SEVNFRM) | (USB_OTG_DIEPCTL_USBAEP)); + USBx_DEVICE->DEACHMSK |= USB_OTG_DAINTMSK_IEPM & ((1 << (ep->num))); } else @@ -529,13 +529,13 @@ HAL_StatusTypeDef USB_ActivateDedicatedEndpoint(USB_OTG_GlobalTypeDef *USBx, USB { USBx_OUTEP(ep->num)->DOEPCTL |= ((ep->maxpacket & USB_OTG_DOEPCTL_MPSIZ ) | (ep->type << 18 ) |\ ((ep->num) << 22 ) | (USB_OTG_DOEPCTL_USBAEP)); - + debug = (uint32_t)(((uint32_t )USBx) + USB_OTG_OUT_ENDPOINT_BASE + (0)*USB_OTG_EP_REG_SIZE); debug = (uint32_t )&USBx_OUTEP(ep->num)->DOEPCTL; debug |= ((ep->maxpacket & USB_OTG_DOEPCTL_MPSIZ ) | (ep->type << 18 ) |\ - ((ep->num) << 22 ) | (USB_OTG_DOEPCTL_USBAEP)); - } - + ((ep->num) << 22 ) | (USB_OTG_DOEPCTL_USBAEP)); + } + USBx_DEVICE->DEACHMSK |= USB_OTG_DAINTMSK_OEPM & ((1 << (ep->num)) << 16); } @@ -553,14 +553,14 @@ HAL_StatusTypeDef USB_DeactivateEndpoint(USB_OTG_GlobalTypeDef *USBx, USB_OTG_EP if (ep->is_in == 1) { USBx_DEVICE->DEACHMSK &= ~(USB_OTG_DAINTMSK_IEPM & ((1 << (ep->num)))); - USBx_DEVICE->DAINTMSK &= ~(USB_OTG_DAINTMSK_IEPM & ((1 << (ep->num)))); - USBx_INEP(ep->num)->DIEPCTL &= ~ USB_OTG_DIEPCTL_USBAEP; + USBx_DEVICE->DAINTMSK &= ~(USB_OTG_DAINTMSK_IEPM & ((1 << (ep->num)))); + USBx_INEP(ep->num)->DIEPCTL &= ~ USB_OTG_DIEPCTL_USBAEP; } else { USBx_DEVICE->DEACHMSK &= ~(USB_OTG_DAINTMSK_OEPM & ((1 << (ep->num)) << 16)); - USBx_DEVICE->DAINTMSK &= ~(USB_OTG_DAINTMSK_OEPM & ((1 << (ep->num)) << 16)); - USBx_OUTEP(ep->num)->DOEPCTL &= ~USB_OTG_DOEPCTL_USBAEP; + USBx_DEVICE->DAINTMSK &= ~(USB_OTG_DAINTMSK_OEPM & ((1 << (ep->num)) << 16)); + USBx_OUTEP(ep->num)->DOEPCTL &= ~USB_OTG_DOEPCTL_USBAEP; } return HAL_OK; } @@ -581,7 +581,7 @@ HAL_StatusTypeDef USB_DeactivateDedicatedEndpoint(USB_OTG_GlobalTypeDef *USBx, U } else { - USBx_OUTEP(ep->num)->DOEPCTL &= ~USB_OTG_DOEPCTL_USBAEP; + USBx_OUTEP(ep->num)->DOEPCTL &= ~USB_OTG_DOEPCTL_USBAEP; USBx_DEVICE->DAINTMSK &= ~(USB_OTG_DAINTMSK_OEPM & ((1 << (ep->num)) << 16)); } return HAL_OK; @@ -591,25 +591,25 @@ HAL_StatusTypeDef USB_DeactivateDedicatedEndpoint(USB_OTG_GlobalTypeDef *USBx, U * @brief USB_EPStartXfer : setup and starts a transfer over an EP * @param USBx : Selected device * @param ep: pointer to endpoint structure - * @param dma: USB dma enabled or disabled + * @param dma: USB dma enabled or disabled * This parameter can be one of these values: - * 0 : DMA feature not used - * 1 : DMA feature used + * 0 : DMA feature not used + * 1 : DMA feature used * @retval HAL status */ HAL_StatusTypeDef USB_EPStartXfer(USB_OTG_GlobalTypeDef *USBx , USB_OTG_EPTypeDef *ep, uint8_t dma) { uint16_t pktcnt = 0; - + /* IN endpoint */ if (ep->is_in == 1) { /* Zero Length Packet? */ if (ep->xfer_len == 0) { - USBx_INEP(ep->num)->DIEPTSIZ &= ~(USB_OTG_DIEPTSIZ_PKTCNT); + USBx_INEP(ep->num)->DIEPTSIZ &= ~(USB_OTG_DIEPTSIZ_PKTCNT); USBx_INEP(ep->num)->DIEPTSIZ |= (USB_OTG_DIEPTSIZ_PKTCNT & (1 << 19)) ; - USBx_INEP(ep->num)->DIEPTSIZ &= ~(USB_OTG_DIEPTSIZ_XFRSIZ); + USBx_INEP(ep->num)->DIEPTSIZ &= ~(USB_OTG_DIEPTSIZ_XFRSIZ); } else { @@ -619,15 +619,15 @@ HAL_StatusTypeDef USB_EPStartXfer(USB_OTG_GlobalTypeDef *USBx , USB_OTG_EPTypeDe * exist ? 1 : 0) */ USBx_INEP(ep->num)->DIEPTSIZ &= ~(USB_OTG_DIEPTSIZ_XFRSIZ); - USBx_INEP(ep->num)->DIEPTSIZ &= ~(USB_OTG_DIEPTSIZ_PKTCNT); + USBx_INEP(ep->num)->DIEPTSIZ &= ~(USB_OTG_DIEPTSIZ_PKTCNT); USBx_INEP(ep->num)->DIEPTSIZ |= (USB_OTG_DIEPTSIZ_PKTCNT & (((ep->xfer_len + ep->maxpacket -1)/ ep->maxpacket) << 19)) ; - USBx_INEP(ep->num)->DIEPTSIZ |= (USB_OTG_DIEPTSIZ_XFRSIZ & ep->xfer_len); - + USBx_INEP(ep->num)->DIEPTSIZ |= (USB_OTG_DIEPTSIZ_XFRSIZ & ep->xfer_len); + if (ep->type == EP_TYPE_ISOC) { - USBx_INEP(ep->num)->DIEPTSIZ &= ~(USB_OTG_DIEPTSIZ_MULCNT); - USBx_INEP(ep->num)->DIEPTSIZ |= (USB_OTG_DIEPTSIZ_MULCNT & (1 << 29)); - } + USBx_INEP(ep->num)->DIEPTSIZ &= ~(USB_OTG_DIEPTSIZ_MULCNT); + USBx_INEP(ep->num)->DIEPTSIZ |= (USB_OTG_DIEPTSIZ_MULCNT & (1 << 29)); + } } if (dma == 1) @@ -656,42 +656,42 @@ HAL_StatusTypeDef USB_EPStartXfer(USB_OTG_GlobalTypeDef *USBx , USB_OTG_EPTypeDe { USBx_INEP(ep->num)->DIEPCTL |= USB_OTG_DIEPCTL_SD0PID_SEVNFRM; } - } - + } + /* EP enable, IN data in FIFO */ USBx_INEP(ep->num)->DIEPCTL |= (USB_OTG_DIEPCTL_CNAK | USB_OTG_DIEPCTL_EPENA); - + if (ep->type == EP_TYPE_ISOC) { - USB_WritePacket(USBx, ep->xfer_buff, ep->num, ep->xfer_len, dma); - } + USB_WritePacket(USBx, ep->xfer_buff, ep->num, ep->xfer_len, dma); + } } else /* OUT endpoint */ { /* Program the transfer size and packet count as follows: * pktcnt = N * xfersize = N * maxpacket - */ - USBx_OUTEP(ep->num)->DOEPTSIZ &= ~(USB_OTG_DOEPTSIZ_XFRSIZ); - USBx_OUTEP(ep->num)->DOEPTSIZ &= ~(USB_OTG_DOEPTSIZ_PKTCNT); - + */ + USBx_OUTEP(ep->num)->DOEPTSIZ &= ~(USB_OTG_DOEPTSIZ_XFRSIZ); + USBx_OUTEP(ep->num)->DOEPTSIZ &= ~(USB_OTG_DOEPTSIZ_PKTCNT); + if (ep->xfer_len == 0) { USBx_OUTEP(ep->num)->DOEPTSIZ |= (USB_OTG_DOEPTSIZ_XFRSIZ & ep->maxpacket); - USBx_OUTEP(ep->num)->DOEPTSIZ |= (USB_OTG_DOEPTSIZ_PKTCNT & (1 << 19)) ; + USBx_OUTEP(ep->num)->DOEPTSIZ |= (USB_OTG_DOEPTSIZ_PKTCNT & (1 << 19)) ; } else { - pktcnt = (ep->xfer_len + ep->maxpacket -1)/ ep->maxpacket; + pktcnt = (ep->xfer_len + ep->maxpacket -1)/ ep->maxpacket; USBx_OUTEP(ep->num)->DOEPTSIZ |= (USB_OTG_DOEPTSIZ_PKTCNT & (pktcnt << 19)); - USBx_OUTEP(ep->num)->DOEPTSIZ |= (USB_OTG_DOEPTSIZ_XFRSIZ & (ep->maxpacket * pktcnt)); + USBx_OUTEP(ep->num)->DOEPTSIZ |= (USB_OTG_DOEPTSIZ_XFRSIZ & (ep->maxpacket * pktcnt)); } if (dma == 1) { USBx_OUTEP(ep->num)->DOEPDMA = (uint32_t)ep->xfer_buff; } - + if (ep->type == EP_TYPE_ISOC) { if ((USBx_DEVICE->DSTS & ( 1 << 8 )) == 0) @@ -713,10 +713,10 @@ HAL_StatusTypeDef USB_EPStartXfer(USB_OTG_GlobalTypeDef *USBx , USB_OTG_EPTypeDe * @brief USB_EP0StartXfer : setup and starts a transfer over the EP 0 * @param USBx : Selected device * @param ep: pointer to endpoint structure - * @param dma: USB dma enabled or disabled + * @param dma: USB dma enabled or disabled * This parameter can be one of these values: - * 0 : DMA feature not used - * 1 : DMA feature used + * 0 : DMA feature not used + * 1 : DMA feature used * @retval HAL status */ HAL_StatusTypeDef USB_EP0StartXfer(USB_OTG_GlobalTypeDef *USBx , USB_OTG_EPTypeDef *ep, uint8_t dma) @@ -727,9 +727,9 @@ HAL_StatusTypeDef USB_EP0StartXfer(USB_OTG_GlobalTypeDef *USBx , USB_OTG_EPTypeD /* Zero Length Packet? */ if (ep->xfer_len == 0) { - USBx_INEP(ep->num)->DIEPTSIZ &= ~(USB_OTG_DIEPTSIZ_PKTCNT); + USBx_INEP(ep->num)->DIEPTSIZ &= ~(USB_OTG_DIEPTSIZ_PKTCNT); USBx_INEP(ep->num)->DIEPTSIZ |= (USB_OTG_DIEPTSIZ_PKTCNT & (1 << 19)) ; - USBx_INEP(ep->num)->DIEPTSIZ &= ~(USB_OTG_DIEPTSIZ_XFRSIZ); + USBx_INEP(ep->num)->DIEPTSIZ &= ~(USB_OTG_DIEPTSIZ_XFRSIZ); } else { @@ -739,20 +739,20 @@ HAL_StatusTypeDef USB_EP0StartXfer(USB_OTG_GlobalTypeDef *USBx , USB_OTG_EPTypeD * exist ? 1 : 0) */ USBx_INEP(ep->num)->DIEPTSIZ &= ~(USB_OTG_DIEPTSIZ_XFRSIZ); - USBx_INEP(ep->num)->DIEPTSIZ &= ~(USB_OTG_DIEPTSIZ_PKTCNT); - + USBx_INEP(ep->num)->DIEPTSIZ &= ~(USB_OTG_DIEPTSIZ_PKTCNT); + if(ep->xfer_len > ep->maxpacket) { ep->xfer_len = ep->maxpacket; } USBx_INEP(ep->num)->DIEPTSIZ |= (USB_OTG_DIEPTSIZ_PKTCNT & (1 << 19)) ; - USBx_INEP(ep->num)->DIEPTSIZ |= (USB_OTG_DIEPTSIZ_XFRSIZ & ep->xfer_len); - + USBx_INEP(ep->num)->DIEPTSIZ |= (USB_OTG_DIEPTSIZ_XFRSIZ & ep->xfer_len); + } - + /* EP enable, IN data in FIFO */ USBx_INEP(ep->num)->DIEPCTL |= (USB_OTG_DIEPCTL_CNAK | USB_OTG_DIEPCTL_EPENA); - + if (dma == 1) { USBx_INEP(ep->num)->DIEPDMA = (uint32_t)(ep->dma_addr); @@ -764,7 +764,7 @@ HAL_StatusTypeDef USB_EP0StartXfer(USB_OTG_GlobalTypeDef *USBx , USB_OTG_EPTypeD { USBx_DEVICE->DIEPEMPMSK |= 1U << (ep->num); } - } + } } else /* OUT endpoint */ { @@ -772,79 +772,79 @@ HAL_StatusTypeDef USB_EP0StartXfer(USB_OTG_GlobalTypeDef *USBx , USB_OTG_EPTypeD * pktcnt = N * xfersize = N * maxpacket */ - USBx_OUTEP(ep->num)->DOEPTSIZ &= ~(USB_OTG_DOEPTSIZ_XFRSIZ); - USBx_OUTEP(ep->num)->DOEPTSIZ &= ~(USB_OTG_DOEPTSIZ_PKTCNT); - + USBx_OUTEP(ep->num)->DOEPTSIZ &= ~(USB_OTG_DOEPTSIZ_XFRSIZ); + USBx_OUTEP(ep->num)->DOEPTSIZ &= ~(USB_OTG_DOEPTSIZ_PKTCNT); + if (ep->xfer_len > 0) { ep->xfer_len = ep->maxpacket; } - + USBx_OUTEP(ep->num)->DOEPTSIZ |= (USB_OTG_DOEPTSIZ_PKTCNT & (1 << 19)); - USBx_OUTEP(ep->num)->DOEPTSIZ |= (USB_OTG_DOEPTSIZ_XFRSIZ & (ep->maxpacket)); - + USBx_OUTEP(ep->num)->DOEPTSIZ |= (USB_OTG_DOEPTSIZ_XFRSIZ & (ep->maxpacket)); + if (dma == 1) { USBx_OUTEP(ep->num)->DOEPDMA = (uint32_t)(ep->xfer_buff); } - + /* EP enable */ - USBx_OUTEP(ep->num)->DOEPCTL |= (USB_OTG_DOEPCTL_CNAK | USB_OTG_DOEPCTL_EPENA); + USBx_OUTEP(ep->num)->DOEPCTL |= (USB_OTG_DOEPCTL_CNAK | USB_OTG_DOEPCTL_EPENA); } return HAL_OK; } /** - * @brief USB_WritePacket : Writes a packet into the Tx FIFO associated + * @brief USB_WritePacket : Writes a packet into the Tx FIFO associated * with the EP/channel - * @param USBx : Selected device + * @param USBx : Selected device * @param src : pointer to source buffer * @param ch_ep_num : endpoint or host channel number * @param len : Number of bytes to write - * @param dma: USB dma enabled or disabled + * @param dma: USB dma enabled or disabled * This parameter can be one of these values: - * 0 : DMA feature not used - * 1 : DMA feature used + * 0 : DMA feature not used + * 1 : DMA feature used * @retval HAL status */ HAL_StatusTypeDef USB_WritePacket(USB_OTG_GlobalTypeDef *USBx, uint8_t *src, uint8_t ch_ep_num, uint16_t len, uint8_t dma) { uint32_t count32b= 0 , i= 0; - + if (dma == 0) { count32b = (len + 3) / 4; for (i = 0; i < count32b; i++, src += 4) { - USBx_DFIFO(ch_ep_num) = *((__packed uint32_t *)src); + USBx_DFIFO(ch_ep_num) = *((uint32_t *)src); } } return HAL_OK; } /** - * @brief USB_ReadPacket : read a packet from the Tx FIFO associated + * @brief USB_ReadPacket : read a packet from the Tx FIFO associated * with the EP/channel - * @param USBx : Selected device + * @param USBx : Selected device * @param src : source pointer * @param ch_ep_num : endpoint or host channel number * @param len : Number of bytes to read - * @param dma: USB dma enabled or disabled + * @param dma: USB dma enabled or disabled * This parameter can be one of these values: - * 0 : DMA feature not used - * 1 : DMA feature used + * 0 : DMA feature not used + * 1 : DMA feature used * @retval pointer to destination buffer */ void *USB_ReadPacket(USB_OTG_GlobalTypeDef *USBx, uint8_t *dest, uint16_t len) { uint32_t i=0; uint32_t count32b = (len + 3) / 4; - + for ( i = 0; i < count32b; i++, dest += 4 ) { - *(__packed uint32_t *)dest = USBx_DFIFO(0); - + *(uint32_t *)dest = USBx_DFIFO(0); + } return ((void *)dest); } @@ -852,7 +852,7 @@ void *USB_ReadPacket(USB_OTG_GlobalTypeDef *USBx, uint8_t *dest, uint16_t len) /** * @brief USB_EPSetStall : set a stall condition over an EP * @param USBx : Selected device - * @param ep: pointer to endpoint structure + * @param ep: pointer to endpoint structure * @retval HAL status */ HAL_StatusTypeDef USB_EPSetStall(USB_OTG_GlobalTypeDef *USBx , USB_OTG_EPTypeDef *ep) @@ -861,16 +861,16 @@ HAL_StatusTypeDef USB_EPSetStall(USB_OTG_GlobalTypeDef *USBx , USB_OTG_EPTypeDef { if (((USBx_INEP(ep->num)->DIEPCTL) & USB_OTG_DIEPCTL_EPENA) == 0) { - USBx_INEP(ep->num)->DIEPCTL &= ~(USB_OTG_DIEPCTL_EPDIS); - } + USBx_INEP(ep->num)->DIEPCTL &= ~(USB_OTG_DIEPCTL_EPDIS); + } USBx_INEP(ep->num)->DIEPCTL |= USB_OTG_DIEPCTL_STALL; } else { if (((USBx_OUTEP(ep->num)->DOEPCTL) & USB_OTG_DOEPCTL_EPENA) == 0) { - USBx_OUTEP(ep->num)->DOEPCTL &= ~(USB_OTG_DOEPCTL_EPDIS); - } + USBx_OUTEP(ep->num)->DOEPCTL &= ~(USB_OTG_DOEPCTL_EPDIS); + } USBx_OUTEP(ep->num)->DOEPCTL |= USB_OTG_DOEPCTL_STALL; } return HAL_OK; @@ -880,7 +880,7 @@ HAL_StatusTypeDef USB_EPSetStall(USB_OTG_GlobalTypeDef *USBx , USB_OTG_EPTypeDef /** * @brief USB_EPClearStall : Clear a stall condition over an EP * @param USBx : Selected device - * @param ep: pointer to endpoint structure + * @param ep: pointer to endpoint structure * @retval HAL status */ HAL_StatusTypeDef USB_EPClearStall(USB_OTG_GlobalTypeDef *USBx, USB_OTG_EPTypeDef *ep) @@ -891,7 +891,7 @@ HAL_StatusTypeDef USB_EPClearStall(USB_OTG_GlobalTypeDef *USBx, USB_OTG_EPTypeDe if (ep->type == EP_TYPE_INTR || ep->type == EP_TYPE_BULK) { USBx_INEP(ep->num)->DIEPCTL |= USB_OTG_DIEPCTL_SD0PID_SEVNFRM; /* DATA0 */ - } + } } else { @@ -899,7 +899,7 @@ HAL_StatusTypeDef USB_EPClearStall(USB_OTG_GlobalTypeDef *USBx, USB_OTG_EPTypeDe if (ep->type == EP_TYPE_INTR || ep->type == EP_TYPE_BULK) { USBx_OUTEP(ep->num)->DOEPCTL |= USB_OTG_DOEPCTL_SD0PID_SEVNFRM; /* DATA0 */ - } + } } return HAL_OK; } @@ -912,7 +912,7 @@ HAL_StatusTypeDef USB_EPClearStall(USB_OTG_GlobalTypeDef *USBx, USB_OTG_EPTypeDe HAL_StatusTypeDef USB_StopDevice(USB_OTG_GlobalTypeDef *USBx) { uint32_t i; - + /* Clear Pending interrupt */ for (i = 0; i < 15 ; i++) { @@ -920,16 +920,16 @@ HAL_StatusTypeDef USB_StopDevice(USB_OTG_GlobalTypeDef *USBx) USBx_OUTEP(i)->DOEPINT = 0xFF; } USBx_DEVICE->DAINT = 0xFFFFFFFF; - + /* Clear interrupt masks */ USBx_DEVICE->DIEPMSK = 0; USBx_DEVICE->DOEPMSK = 0; USBx_DEVICE->DAINTMSK = 0; - + /* Flush the FIFO */ USB_FlushRxFifo(USBx); - USB_FlushTxFifo(USBx , 0x10 ); - + USB_FlushTxFifo(USBx , 0x10 ); + return HAL_OK; } @@ -944,8 +944,8 @@ HAL_StatusTypeDef USB_SetDevAddress (USB_OTG_GlobalTypeDef *USBx, uint8_t addre { USBx_DEVICE->DCFG &= ~ (USB_OTG_DCFG_DAD); USBx_DEVICE->DCFG |= (address << 4) & USB_OTG_DCFG_DAD ; - - return HAL_OK; + + return HAL_OK; } /** @@ -957,8 +957,8 @@ HAL_StatusTypeDef USB_DevConnect (USB_OTG_GlobalTypeDef *USBx) { USBx_DEVICE->DCTL &= ~USB_OTG_DCTL_SDIS ; HAL_Delay(3); - - return HAL_OK; + + return HAL_OK; } /** @@ -970,8 +970,8 @@ HAL_StatusTypeDef USB_DevDisconnect (USB_OTG_GlobalTypeDef *USBx) { USBx_DEVICE->DCTL |= USB_OTG_DCTL_SDIS ; HAL_Delay(3); - - return HAL_OK; + + return HAL_OK; } /** @@ -982,10 +982,10 @@ HAL_StatusTypeDef USB_DevDisconnect (USB_OTG_GlobalTypeDef *USBx) uint32_t USB_ReadInterrupts (USB_OTG_GlobalTypeDef *USBx) { uint32_t v = 0; - + v = USBx->GINTSTS; v &= USBx->GINTMSK; - return v; + return v; } /** @@ -1039,7 +1039,7 @@ uint32_t USB_ReadDevOutEPInterrupt (USB_OTG_GlobalTypeDef *USBx , uint8_t epnum) uint32_t USB_ReadDevInEPInterrupt (USB_OTG_GlobalTypeDef *USBx , uint8_t epnum) { uint32_t v, msk, emp; - + msk = USBx_DEVICE->DIEPMSK; emp = USBx_DEVICE->DIEPEMPMSK; msk |= ((emp >> epnum) & 0x1) << 7; @@ -1055,7 +1055,7 @@ uint32_t USB_ReadDevInEPInterrupt (USB_OTG_GlobalTypeDef *USBx , uint8_t epnum) */ void USB_ClearInterrupts (USB_OTG_GlobalTypeDef *USBx, uint32_t interrupt) { - USBx->GINTSTS |= interrupt; + USBx->GINTSTS |= interrupt; } /** @@ -1063,7 +1063,7 @@ void USB_ClearInterrupts (USB_OTG_GlobalTypeDef *USBx, uint32_t interrupt) * @param USBx : Selected device * @retval return core mode : Host or Device * This parameter can be one of these values: - * 0 : Host + * 0 : Host * 1 : Device */ uint32_t USB_GetMode(USB_OTG_GlobalTypeDef *USBx) @@ -1081,7 +1081,7 @@ HAL_StatusTypeDef USB_ActivateSetup (USB_OTG_GlobalTypeDef *USBx) { /* Set the MPS of the IN EP based on the enumeration speed */ USBx_INEP(0)->DIEPCTL &= ~USB_OTG_DIEPCTL_MPSIZ; - + if((USBx_DEVICE->DSTS & USB_OTG_DSTS_ENUMSPD) == DSTS_ENUMSPD_LS_PHY_6MHZ) { USBx_INEP(0)->DIEPCTL |= 3; @@ -1095,10 +1095,10 @@ HAL_StatusTypeDef USB_ActivateSetup (USB_OTG_GlobalTypeDef *USBx) /** * @brief Prepare the EP0 to start the first control setup * @param USBx : Selected device - * @param dma: USB dma enabled or disabled + * @param dma: USB dma enabled or disabled * This parameter can be one of these values: - * 0 : DMA feature not used - * 1 : DMA feature used + * 0 : DMA feature not used + * 1 : DMA feature used * @param psetup : pointer to setup packet * @retval HAL status */ @@ -1107,16 +1107,16 @@ HAL_StatusTypeDef USB_EP0_OutStart(USB_OTG_GlobalTypeDef *USBx, uint8_t dma, uin USBx_OUTEP(0)->DOEPTSIZ = 0; USBx_OUTEP(0)->DOEPTSIZ |= (USB_OTG_DOEPTSIZ_PKTCNT & (1 << 19)) ; USBx_OUTEP(0)->DOEPTSIZ |= (3 * 8); - USBx_OUTEP(0)->DOEPTSIZ |= USB_OTG_DOEPTSIZ_STUPCNT; - + USBx_OUTEP(0)->DOEPTSIZ |= USB_OTG_DOEPTSIZ_STUPCNT; + if (dma == 1) { USBx_OUTEP(0)->DOEPDMA = (uint32_t)psetup; /* EP enable */ USBx_OUTEP(0)->DOEPCTL = 0x80008000; } - - return HAL_OK; + + return HAL_OK; } @@ -1138,7 +1138,7 @@ static HAL_StatusTypeDef USB_CoreReset(USB_OTG_GlobalTypeDef *USBx) } } while ((USBx->GRSTCTL & USB_OTG_GRSTCTL_AHBIDL) == 0); - + /* Core Soft Reset */ count = 0; USBx->GRSTCTL |= USB_OTG_GRSTCTL_CSRST; @@ -1151,24 +1151,24 @@ static HAL_StatusTypeDef USB_CoreReset(USB_OTG_GlobalTypeDef *USBx) } } while ((USBx->GRSTCTL & USB_OTG_GRSTCTL_CSRST) == USB_OTG_GRSTCTL_CSRST); - + return HAL_OK; } -#ifdef USB_HS_PHYC +#ifdef USB_HS_PHYC /** * @brief Enables control of a High Speed USB PHY’s - * Init the low level hardware : GPIO, CLOCK, NVIC... + * Init the low level hardware : GPIO, CLOCK, NVIC... * @param USBx : Selected device * @retval HAL status */ static HAL_StatusTypeDef USB_HS_PHYCInit(USB_OTG_GlobalTypeDef *USBx) { uint32_t count = 0; - + /* Enable LDO */ USB_HS_PHYC->USB_HS_PHYC_LDO |= USB_HS_PHYC_LDO_ENABLE; - + /* wait for LDO Ready */ while((USB_HS_PHYC->USB_HS_PHYC_LDO & USB_HS_PHYC_LDO_STATUS) == RESET) { @@ -1191,7 +1191,7 @@ static HAL_StatusTypeDef USB_HS_PHYCInit(USB_OTG_GlobalTypeDef *USBx) { USB_HS_PHYC->USB_HS_PHYC_PLL = (uint32_t)(0x3 << 1); } - + else if (HSE_VALUE == 24000000) /* HSE = 24MHz */ { USB_HS_PHYC->USB_HS_PHYC_PLL = (uint32_t)(0x4 << 1); @@ -1204,23 +1204,23 @@ static HAL_StatusTypeDef USB_HS_PHYCInit(USB_OTG_GlobalTypeDef *USBx) { USB_HS_PHYC->USB_HS_PHYC_PLL = (uint32_t)(0x7 << 1); } - + /* Control the tuning interface of the High Speed PHY */ USB_HS_PHYC->USB_HS_PHYC_TUNE |= USB_HS_PHYC_TUNE_VALUE; - + /* Enable PLL internal PHY */ USB_HS_PHYC->USB_HS_PHYC_PLL |= USB_HS_PHYC_PLL_PLLEN; /* 2ms Delay required to get internal phy clock stable */ HAL_Delay(2); - + return HAL_OK; } #endif /* USB_HS_PHYC */ /** - * @brief USB_HostInit : Initializes the USB OTG controller registers - * for Host mode + * @brief USB_HostInit : Initializes the USB OTG controller registers + * for Host mode * @param USBx : Selected device * @param cfg : pointer to a USB_OTG_CfgTypeDef structure that contains * the configuration information for the specified USBx peripheral. @@ -1229,22 +1229,22 @@ static HAL_StatusTypeDef USB_HS_PHYCInit(USB_OTG_GlobalTypeDef *USBx) HAL_StatusTypeDef USB_HostInit (USB_OTG_GlobalTypeDef *USBx, USB_OTG_CfgTypeDef cfg) { uint32_t i; - + /* Restart the Phy Clock */ USBx_PCGCCTL = 0; - + /*Activate VBUS Sensing B */ USBx->GCCFG |= USB_OTG_GCCFG_VBDEN; - + /* Disable the FS/LS support mode only */ if((cfg.speed == USB_OTG_SPEED_FULL)&& (USBx != USB_OTG_FS)) { - USBx_HOST->HCFG |= USB_OTG_HCFG_FSLSS; + USBx_HOST->HCFG |= USB_OTG_HCFG_FSLSS; } else { - USBx_HOST->HCFG &= ~(USB_OTG_HCFG_FSLSS); + USBx_HOST->HCFG &= ~(USB_OTG_HCFG_FSLSS); } /* Make sure the FIFOs are flushed. */ @@ -1257,39 +1257,39 @@ HAL_StatusTypeDef USB_HostInit (USB_OTG_GlobalTypeDef *USBx, USB_OTG_CfgTypeDef USBx_HC(i)->HCINT = 0xFFFFFFFF; USBx_HC(i)->HCINTMSK = 0; } - + /* Enable VBUS driving */ USB_DriveVbus(USBx, 1); - + HAL_Delay(200); - + /* Disable all interrupts. */ USBx->GINTMSK = 0; - + /* Clear any pending interrupts */ USBx->GINTSTS = 0xFFFFFFFF; - + if(USBx == USB_OTG_FS) { /* set Rx FIFO size */ - USBx->GRXFSIZ = (uint32_t )0x80; + USBx->GRXFSIZ = (uint32_t )0x80; USBx->DIEPTXF0_HNPTXFSIZ = (uint32_t )(((0x60 << 16)& USB_OTG_NPTXFD) | 0x80); USBx->HPTXFSIZ = (uint32_t )(((0x40 << 16)& USB_OTG_HPTXFSIZ_PTXFD) | 0xE0); } else { /* set Rx FIFO size */ - USBx->GRXFSIZ = (uint32_t )0x200; + USBx->GRXFSIZ = (uint32_t )0x200; USBx->DIEPTXF0_HNPTXFSIZ = (uint32_t )(((0x100 << 16)& USB_OTG_NPTXFD) | 0x200); USBx->HPTXFSIZ = (uint32_t )(((0xE0 << 16)& USB_OTG_HPTXFSIZ_PTXFD) | 0x300); } - + /* Enable the common interrupts */ if (cfg.dma_enable == DISABLE) { - USBx->GINTMSK |= USB_OTG_GINTMSK_RXFLVLM; + USBx->GINTMSK |= USB_OTG_GINTMSK_RXFLVLM; } - + /* Enable interrupts matching to the Host mode ONLY */ USBx->GINTMSK |= (USB_OTG_GINTMSK_PRTIM | USB_OTG_GINTMSK_HCIM |\ USB_OTG_GINTMSK_SOFM |USB_OTG_GINTSTS_DISCINT|\ @@ -1299,20 +1299,20 @@ HAL_StatusTypeDef USB_HostInit (USB_OTG_GlobalTypeDef *USBx, USB_OTG_CfgTypeDef } /** - * @brief USB_InitFSLSPClkSel : Initializes the FSLSPClkSel field of the + * @brief USB_InitFSLSPClkSel : Initializes the FSLSPClkSel field of the * HCFG register on the PHY type and set the right frame interval * @param USBx : Selected device * @param freq : clock frequency * This parameter can be one of these values: - * HCFG_48_MHZ : Full Speed 48 MHz Clock - * HCFG_6_MHZ : Low Speed 6 MHz Clock + * HCFG_48_MHZ : Full Speed 48 MHz Clock + * HCFG_6_MHZ : Low Speed 6 MHz Clock * @retval HAL status */ HAL_StatusTypeDef USB_InitFSLSPClkSel(USB_OTG_GlobalTypeDef *USBx , uint8_t freq) { USBx_HOST->HCFG &= ~(USB_OTG_HCFG_FSLSPCS); USBx_HOST->HCFG |= (freq & USB_OTG_HCFG_FSLSPCS); - + if (freq == HCFG_48_MHZ) { USBx_HOST->HFIR = (uint32_t)48000; @@ -1320,8 +1320,8 @@ HAL_StatusTypeDef USB_InitFSLSPClkSel(USB_OTG_GlobalTypeDef *USBx , uint8_t freq else if (freq == HCFG_6_MHZ) { USBx_HOST->HFIR = (uint32_t)6000; - } - return HAL_OK; + } + return HAL_OK; } /** @@ -1340,9 +1340,9 @@ HAL_StatusTypeDef USB_ResetPort(USB_OTG_GlobalTypeDef *USBx) hprt0 &= ~(USB_OTG_HPRT_PENA | USB_OTG_HPRT_PCDET | USB_OTG_HPRT_PENCHNG | USB_OTG_HPRT_POCCHNG); - USBx_HPRT0 = (USB_OTG_HPRT_PRST | hprt0); + USBx_HPRT0 = (USB_OTG_HPRT_PRST | hprt0); HAL_Delay (100); /* See Note #1 */ - USBx_HPRT0 = ((~USB_OTG_HPRT_PRST) & hprt0); + USBx_HPRT0 = ((~USB_OTG_HPRT_PRST) & hprt0); HAL_Delay (10); return HAL_OK; @@ -1352,7 +1352,7 @@ HAL_StatusTypeDef USB_ResetPort(USB_OTG_GlobalTypeDef *USBx) * @brief USB_DriveVbus : activate or de-activate vbus * @param state : VBUS state * This parameter can be one of these values: - * 0 : VBUS Active + * 0 : VBUS Active * 1 : VBUS Inactive * @retval HAL status */ @@ -1367,13 +1367,13 @@ HAL_StatusTypeDef USB_DriveVbus (USB_OTG_GlobalTypeDef *USBx, uint8_t state) if (((hprt0 & USB_OTG_HPRT_PPWR) == 0 ) && (state == 1 )) { - USBx_HPRT0 = (USB_OTG_HPRT_PPWR | hprt0); + USBx_HPRT0 = (USB_OTG_HPRT_PPWR | hprt0); } if (((hprt0 & USB_OTG_HPRT_PPWR) == USB_OTG_HPRT_PPWR) && (state == 0 )) { - USBx_HPRT0 = ((~USB_OTG_HPRT_PPWR) & hprt0); + USBx_HPRT0 = ((~USB_OTG_HPRT_PPWR) & hprt0); } - return HAL_OK; + return HAL_OK; } /** @@ -1388,7 +1388,7 @@ HAL_StatusTypeDef USB_DriveVbus (USB_OTG_GlobalTypeDef *USBx, uint8_t state) uint32_t USB_GetHostSpeed (USB_OTG_GlobalTypeDef *USBx) { __IO uint32_t hprt0; - + hprt0 = USBx_HPRT0; return ((hprt0 & USB_OTG_HPRT_PSPD) >> 17); } @@ -1427,7 +1427,7 @@ uint32_t USB_GetCurrentFrame (USB_OTG_GlobalTypeDef *USBx) * This parameter can be a value from 0 to32K * @retval HAL state */ -HAL_StatusTypeDef USB_HC_Init(USB_OTG_GlobalTypeDef *USBx, +HAL_StatusTypeDef USB_HC_Init(USB_OTG_GlobalTypeDef *USBx, uint8_t ch_num, uint8_t epnum, uint8_t dev_address, @@ -1435,28 +1435,28 @@ HAL_StatusTypeDef USB_HC_Init(USB_OTG_GlobalTypeDef *USBx, uint8_t ep_type, uint16_t mps) { - + /* Clear old interrupt conditions for this host channel. */ USBx_HC(ch_num)->HCINT = 0xFFFFFFFF; - + /* Enable channel interrupts required for this transfer. */ - switch (ep_type) + switch (ep_type) { case EP_TYPE_CTRL: case EP_TYPE_BULK: - + USBx_HC(ch_num)->HCINTMSK = USB_OTG_HCINTMSK_XFRCM |\ USB_OTG_HCINTMSK_STALLM |\ USB_OTG_HCINTMSK_TXERRM |\ USB_OTG_HCINTMSK_DTERRM |\ USB_OTG_HCINTMSK_AHBERR |\ USB_OTG_HCINTMSK_NAKM ; - - if (epnum & 0x80) + + if (epnum & 0x80) { USBx_HC(ch_num)->HCINTMSK |= USB_OTG_HCINTMSK_BBERRM; - } - else + } + else { if(USBx != USB_OTG_FS) { @@ -1464,43 +1464,43 @@ HAL_StatusTypeDef USB_HC_Init(USB_OTG_GlobalTypeDef *USBx, } } break; - + case EP_TYPE_INTR: - + USBx_HC(ch_num)->HCINTMSK = USB_OTG_HCINTMSK_XFRCM |\ USB_OTG_HCINTMSK_STALLM |\ USB_OTG_HCINTMSK_TXERRM |\ USB_OTG_HCINTMSK_DTERRM |\ USB_OTG_HCINTMSK_NAKM |\ USB_OTG_HCINTMSK_AHBERR |\ - USB_OTG_HCINTMSK_FRMORM ; - - if (epnum & 0x80) + USB_OTG_HCINTMSK_FRMORM ; + + if (epnum & 0x80) { USBx_HC(ch_num)->HCINTMSK |= USB_OTG_HCINTMSK_BBERRM; } - + break; case EP_TYPE_ISOC: - + USBx_HC(ch_num)->HCINTMSK = USB_OTG_HCINTMSK_XFRCM |\ USB_OTG_HCINTMSK_ACKM |\ USB_OTG_HCINTMSK_AHBERR |\ - USB_OTG_HCINTMSK_FRMORM ; - - if (epnum & 0x80) + USB_OTG_HCINTMSK_FRMORM ; + + if (epnum & 0x80) { - USBx_HC(ch_num)->HCINTMSK |= (USB_OTG_HCINTMSK_TXERRM | USB_OTG_HCINTMSK_BBERRM); + USBx_HC(ch_num)->HCINTMSK |= (USB_OTG_HCINTMSK_TXERRM | USB_OTG_HCINTMSK_BBERRM); } break; } - + /* Enable the top level host channel interrupt. */ USBx_HOST->HAINTMSK |= (1 << ch_num); - + /* Make sure host channel interrupts are enabled. */ USBx->GINTMSK |= USB_OTG_GINTMSK_HCIM; - + /* Program the HCCHAR register */ USBx_HC(ch_num)->HCCHAR = (((dev_address << 22) & USB_OTG_HCCHAR_DAD) |\ (((epnum & 0x7F)<< 11) & USB_OTG_HCCHAR_EPNUM)|\ @@ -1508,33 +1508,33 @@ HAL_StatusTypeDef USB_HC_Init(USB_OTG_GlobalTypeDef *USBx, (((speed == HPRT0_PRTSPD_LOW_SPEED)<< 17) & USB_OTG_HCCHAR_LSDEV)|\ ((ep_type << 18) & USB_OTG_HCCHAR_EPTYP)|\ (mps & USB_OTG_HCCHAR_MPSIZ)); - + if (ep_type == EP_TYPE_INTR) { USBx_HC(ch_num)->HCCHAR |= USB_OTG_HCCHAR_ODDFRM ; } - return HAL_OK; + return HAL_OK; } /** * @brief Start a transfer over a host channel * @param USBx : Selected device * @param hc : pointer to host channel structure - * @param dma: USB dma enabled or disabled + * @param dma: USB dma enabled or disabled * This parameter can be one of these values: - * 0 : DMA feature not used - * 1 : DMA feature used + * 0 : DMA feature not used + * 1 : DMA feature used * @retval HAL state */ HAL_StatusTypeDef USB_HC_StartXfer(USB_OTG_GlobalTypeDef *USBx, USB_OTG_HCTypeDef *hc, uint8_t dma) { static __IO uint32_t tmpreg = 0; - uint8_t is_oddframe = 0; - uint16_t len_words = 0; + uint8_t is_oddframe = 0; + uint16_t len_words = 0; uint16_t num_packets = 0; uint16_t max_hc_pkt_count = 256; - + if((USBx != USB_OTG_FS) && (hc->speed == USB_OTG_SPEED_HIGH)) { if((dma == 0) && (hc->do_ping == 1)) @@ -1548,12 +1548,12 @@ HAL_StatusTypeDef USB_HC_StartXfer(USB_OTG_GlobalTypeDef *USBx, USB_OTG_HCTypeDe hc->do_ping = 0; } } - + /* Compute the expected number of packets associated to the transfer */ if (hc->xfer_len > 0) { num_packets = (hc->xfer_len + hc->max_packet - 1) / hc->max_packet; - + if (num_packets > max_hc_pkt_count) { num_packets = max_hc_pkt_count; @@ -1568,40 +1568,40 @@ HAL_StatusTypeDef USB_HC_StartXfer(USB_OTG_GlobalTypeDef *USBx, USB_OTG_HCTypeDe { hc->xfer_len = num_packets * hc->max_packet; } - + /* Initialize the HCTSIZn register */ USBx_HC(hc->ch_num)->HCTSIZ = (((hc->xfer_len) & USB_OTG_HCTSIZ_XFRSIZ)) |\ ((num_packets << 19) & USB_OTG_HCTSIZ_PKTCNT) |\ (((hc->data_pid) << 29) & USB_OTG_HCTSIZ_DPID); - + if (dma) { /* xfer_buff MUST be 32-bits aligned */ USBx_HC(hc->ch_num)->HCDMA = (uint32_t)hc->xfer_buff; } - + is_oddframe = (USBx_HOST->HFNUM & 0x01) ? 0 : 1; USBx_HC(hc->ch_num)->HCCHAR &= ~USB_OTG_HCCHAR_ODDFRM; USBx_HC(hc->ch_num)->HCCHAR |= (is_oddframe << 29); - + /* Set host channel enable */ tmpreg = USBx_HC(hc->ch_num)->HCCHAR; tmpreg &= ~USB_OTG_HCCHAR_CHDIS; tmpreg |= USB_OTG_HCCHAR_CHENA; USBx_HC(hc->ch_num)->HCCHAR = tmpreg; - + if (dma == 0) /* Slave mode */ - { + { if((hc->ep_is_in == 0) && (hc->xfer_len > 0)) { - switch(hc->ep_type) + switch(hc->ep_type) { /* Non periodic transfer */ case EP_TYPE_CTRL: case EP_TYPE_BULK: - + len_words = (hc->xfer_len + 3) / 4; - + /* check if there is enough space in FIFO space */ if(len_words > (USBx->HNPTXSTS & 0xFFFF)) { @@ -1617,19 +1617,19 @@ HAL_StatusTypeDef USB_HC_StartXfer(USB_OTG_GlobalTypeDef *USBx, USB_OTG_HCTypeDe if(len_words > (USBx_HOST->HPTXSTS & 0xFFFF)) /* split the transfer */ { /* need to process data in ptxfempty interrupt */ - USBx->GINTMSK |= USB_OTG_GINTMSK_PTXFEM; + USBx->GINTMSK |= USB_OTG_GINTMSK_PTXFEM; } break; - + default: break; } - + /* Write packet into the Tx FIFO. */ USB_WritePacket(USBx, hc->xfer_buff, hc->ch_num, hc->xfer_len, 0); } } - + return HAL_OK; } @@ -1653,56 +1653,56 @@ uint32_t USB_HC_ReadInterrupt (USB_OTG_GlobalTypeDef *USBx) HAL_StatusTypeDef USB_HC_Halt(USB_OTG_GlobalTypeDef *USBx , uint8_t hc_num) { uint32_t count = 0; - + /* Check for space in the request queue to issue the halt. */ if (((((USBx_HC(hc_num)->HCCHAR) & USB_OTG_HCCHAR_EPTYP) >> 18) == HCCHAR_CTRL) || (((((USBx_HC(hc_num)->HCCHAR) & USB_OTG_HCCHAR_EPTYP) >> 18) == HCCHAR_BULK))) { USBx_HC(hc_num)->HCCHAR |= USB_OTG_HCCHAR_CHDIS; - + if ((USBx->HNPTXSTS & 0xFFFF) == 0) { USBx_HC(hc_num)->HCCHAR &= ~USB_OTG_HCCHAR_CHENA; - USBx_HC(hc_num)->HCCHAR |= USB_OTG_HCCHAR_CHENA; + USBx_HC(hc_num)->HCCHAR |= USB_OTG_HCCHAR_CHENA; USBx_HC(hc_num)->HCCHAR &= ~USB_OTG_HCCHAR_EPDIR; - do + do { - if (++count > 1000) + if (++count > 1000) { break; } - } - while ((USBx_HC(hc_num)->HCCHAR & USB_OTG_HCCHAR_CHENA) == USB_OTG_HCCHAR_CHENA); + } + while ((USBx_HC(hc_num)->HCCHAR & USB_OTG_HCCHAR_CHENA) == USB_OTG_HCCHAR_CHENA); } else { - USBx_HC(hc_num)->HCCHAR |= USB_OTG_HCCHAR_CHENA; + USBx_HC(hc_num)->HCCHAR |= USB_OTG_HCCHAR_CHENA; } } else { USBx_HC(hc_num)->HCCHAR |= USB_OTG_HCCHAR_CHDIS; - + if ((USBx_HOST->HPTXSTS & 0xFFFF) == 0) { USBx_HC(hc_num)->HCCHAR &= ~USB_OTG_HCCHAR_CHENA; - USBx_HC(hc_num)->HCCHAR |= USB_OTG_HCCHAR_CHENA; + USBx_HC(hc_num)->HCCHAR |= USB_OTG_HCCHAR_CHENA; USBx_HC(hc_num)->HCCHAR &= ~USB_OTG_HCCHAR_EPDIR; - do + do { - if (++count > 1000) + if (++count > 1000) { break; } - } - while ((USBx_HC(hc_num)->HCCHAR & USB_OTG_HCCHAR_CHENA) == USB_OTG_HCCHAR_CHENA); + } + while ((USBx_HC(hc_num)->HCCHAR & USB_OTG_HCCHAR_CHENA) == USB_OTG_HCCHAR_CHENA); } else { - USBx_HC(hc_num)->HCCHAR |= USB_OTG_HCCHAR_CHENA; + USBx_HC(hc_num)->HCCHAR |= USB_OTG_HCCHAR_CHENA; } } - + return HAL_OK; } @@ -1720,14 +1720,14 @@ HAL_StatusTypeDef USB_DoPing(USB_OTG_GlobalTypeDef *USBx , uint8_t ch_num) USBx_HC(ch_num)->HCTSIZ = ((num_packets << 19) & USB_OTG_HCTSIZ_PKTCNT) |\ USB_OTG_HCTSIZ_DOPING; - + /* Set host channel enable */ tmpreg = USBx_HC(ch_num)->HCCHAR; tmpreg &= ~USB_OTG_HCCHAR_CHDIS; tmpreg |= USB_OTG_HCCHAR_CHENA; USBx_HC(ch_num)->HCCHAR = tmpreg; - - return HAL_OK; + + return HAL_OK; } /** @@ -1740,41 +1740,41 @@ HAL_StatusTypeDef USB_StopHost(USB_OTG_GlobalTypeDef *USBx) uint8_t i; uint32_t count = 0; uint32_t value; - + USB_DisableGlobalInt(USBx); - + /* Flush FIFO */ USB_FlushTxFifo(USBx, 0x10); USB_FlushRxFifo(USBx); - + /* Flush out any leftover queued requests. */ for (i = 0; i <= 15; i++) - { + { value = USBx_HC(i)->HCCHAR ; value |= USB_OTG_HCCHAR_CHDIS; - value &= ~USB_OTG_HCCHAR_CHENA; + value &= ~USB_OTG_HCCHAR_CHENA; value &= ~USB_OTG_HCCHAR_EPDIR; USBx_HC(i)->HCCHAR = value; } - - /* Halt all channels to put them into a known state. */ + + /* Halt all channels to put them into a known state. */ for (i = 0; i <= 15; i++) { value = USBx_HC(i)->HCCHAR ; - + value |= USB_OTG_HCCHAR_CHDIS; - value |= USB_OTG_HCCHAR_CHENA; + value |= USB_OTG_HCCHAR_CHENA; value &= ~USB_OTG_HCCHAR_EPDIR; - + USBx_HC(i)->HCCHAR = value; - do + do { - if (++count > 1000) + if (++count > 1000) { break; } - } + } while ((USBx_HC(i)->HCCHAR & USB_OTG_HCCHAR_CHENA) == USB_OTG_HCCHAR_CHENA); } @@ -1782,7 +1782,7 @@ HAL_StatusTypeDef USB_StopHost(USB_OTG_GlobalTypeDef *USBx) USBx_HOST->HAINT = 0xFFFFFFFF; USBx->GINTSTS = 0xFFFFFFFF; USB_EnableGlobalInt(USBx); - return HAL_OK; + return HAL_OK; } /** * @} diff --git a/make/mcu/STM32F3.mk b/make/mcu/STM32F3.mk index 06cfa44e473..232ce7fe042 100644 --- a/make/mcu/STM32F3.mk +++ b/make/mcu/STM32F3.mk @@ -70,7 +70,7 @@ MCU_COMMON_SRC = \ drivers/accgyro/accgyro.c \ drivers/adc_stm32f30x.c \ drivers/bus_i2c_stm32f30x.c \ - drivers/dma.c \ + drivers/dma_stm32f3xx.c \ drivers/serial_uart_stm32f30x.c \ drivers/system_stm32f30x.c \ drivers/timer_impl_stdperiph.c \ diff --git a/make/release.mk b/make/release.mk index 5d3c033cefa..19e6cad3355 100644 --- a/make/release.mk +++ b/make/release.mk @@ -1,19 +1,25 @@ RELEASE_TARGETS = AIRHEROF3 AIRHEROF3_QUAD -RELEASE_TARGETS += COLIBRI_RACE LUX_RACE FURYF3 FURYF3_SPIFLASH RCEXPLORERF3 RMDO SPARKY KFC32F3_INAV FALCORE MOTOLAB ANYFC BLUEJAYF4 COLIBRI F4BY +RELEASE_TARGETS += COLIBRI_RACE LUX_RACE FURYF3 FURYF3_SPIFLASH RCEXPLORERF3 RMDO SPARKY KFC32F3_INAV FALCORE MOTOLAB ANYFC BLUEJAYF4 COLIBRI F4BY DALRCF405 RELEASE_TARGETS += QUANTON REVO SPARKY2 YUPIF4 YUPIF4R2 YUPIF4MINI KROOZX PIKOBLX CLRACINGF4AIR CLRACINGF4AIRV2 PIXRACER BEEROTORF4 ANYFCF7 ANYFCF7_EXTERNAL_BARO ANYFCM7 RELEASE_TARGETS += ALIENFLIGHTNGF7 RELEASE_TARGETS += BETAFLIGHTF3 BETAFLIGHTF4 RELEASE_TARGETS += FF_F35_LIGHTNING FF_FORTINIF4 -RELEASE_TARGETS += KAKUTEF4 KAKUTEF4V2 +RELEASE_TARGETS += KAKUTEF4 KAKUTEF4V2 KAKUTEF7 RELEASE_TARGETS += SPRACINGF3 SPRACINGF3EVO SPRACINGF3EVO_1SS SPRACINGF3MINI SPRACINGF3NEO SPRACINGF4EVO -RELEASE_TARGETS += OMNIBUS AIRBOTF4 ASGARD32F4 FIREWORKSV2 -RELEASE_TARGETS += OMNIBUSF4 OMNIBUSF4PRO OMNIBUSF4V3 OMNIBUSF4PRO_LEDSTRIPM5 DYSF4PRO +RELEASE_TARGETS += OMNIBUS AIRBOTF4 ASGARD32F4 ASGARD32F7 FIREWORKSV2 +RELEASE_TARGETS += OMNIBUSF4 OMNIBUSF4PRO OMNIBUSF4V3 OMNIBUSF4PRO_LEDSTRIPM5 DYSF4PRO DYSF4PROV2 RELEASE_TARGETS += OMNIBUSF7 OMNIBUSF7V2 OMNIBUSF7NXT YUPIF7 -RELEASE_TARGETS += MATEKF405 MATEKF405_SERVOS6 MATEKF405OSD MATEKF722 MATEKF405SE MATEKF411 +RELEASE_TARGETS += MATEKF405 MATEKF405_SERVOS6 MATEKF405OSD MATEKF722 MATEKF722_HEXSERVO MATEKF722SE MATEKF722MINI MATEKF405SE MATEKF411 MATEKF411_SFTSRL2 MATEKF411_RSSI -RELEASE_TARGETS += SPEEDYBEEF4 +RELEASE_TARGETS += FOXEERF405 FOXEERF722DUAL + +RELEASE_TARGETS += SPEEDYBEEF4 FRSKYF3 FRSKYF4 + +RELEASE_TARGETS += NOX WINGFC + +RELEASE_TARGETS += OMNIBUSF4V6 diff --git a/make/source.mk b/make/source.mk index ee24740464b..5756257f2bc 100644 --- a/make/source.mk +++ b/make/source.mk @@ -11,7 +11,9 @@ COMMON_SRC = \ common/encoding.c \ common/filter.c \ common/maths.c \ + common/calibration.c \ common/memory.c \ + common/olc.c \ common/printf.c \ common/streambuf.c \ common/time.c \ @@ -42,6 +44,7 @@ COMMON_SRC = \ drivers/pwm_esc_detect.c \ drivers/pwm_mapping.c \ drivers/pwm_output.c \ + drivers/pinio.c \ drivers/rcc.c \ drivers/rx_pwm.c \ drivers/serial.c \ @@ -51,6 +54,12 @@ COMMON_SRC = \ drivers/system.c \ drivers/timer.c \ drivers/lights_io.c \ + drivers/1-wire.c \ + drivers/1-wire/ds_crc.c \ + drivers/1-wire/ds2482.c \ + drivers/temperature/ds18b20.c \ + drivers/temperature/lm75.c \ + drivers/pitotmeter_ms4525.c \ fc/cli.c \ fc/config.c \ fc/controlrate_profile.c \ @@ -60,6 +69,7 @@ COMMON_SRC = \ fc/fc_hardfaults.c \ fc/fc_msp.c \ fc/fc_msp_box.c \ + fc/rc_smoothing.c \ fc/rc_adjustments.c \ fc/rc_controls.c \ fc/rc_curves.c \ @@ -79,6 +89,7 @@ COMMON_SRC = \ io/beeper.c \ io/lights.c \ io/pwmdriver_i2c.c \ + io/piniobox.c \ io/serial.c \ io/serial_4way.c \ io/serial_4way_avrootloader.c \ @@ -133,6 +144,7 @@ COMMON_SRC = \ cms/cms_menu_osd.c \ cms/cms_menu_vtx_smartaudio.c \ cms/cms_menu_vtx_tramp.c \ + cms/cms_menu_vtx_ffpv.c \ common/colorconversion.c \ common/gps_conversion.c \ drivers/display_ug2864hsweg01.c \ @@ -145,6 +157,7 @@ COMMON_SRC = \ drivers/opflow/opflow_virtual.c \ drivers/vtx_common.c \ io/rangefinder_msp.c \ + io/rangefinder_benewake.c \ io/opflow_cxof.c \ io/opflow_msp.c \ io/dashboard.c \ @@ -184,6 +197,7 @@ COMMON_SRC = \ io/vtx_string.c \ io/vtx_smartaudio.c \ io/vtx_tramp.c \ + io/vtx_ffpv24g.c \ io/vtx_control.c COMMON_DEVICE_SRC = \ @@ -204,6 +218,7 @@ endif ifneq ($(filter SDCARD,$(FEATURES)),) TARGET_SRC += \ drivers/sdcard.c \ + drivers/sdcard_spi.c \ drivers/sdcard_standard.c \ io/asyncfatfs/asyncfatfs.c \ io/asyncfatfs/fat_standard.c diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index d80b03b1840..f60f48a64d8 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -76,6 +76,7 @@ #include "sensors/rangefinder.h" #include "sensors/sensors.h" #include "flight/wind_estimator.h" +#include "sensors/temperature.h" #if defined(ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT) @@ -92,7 +93,7 @@ #define BLACKBOX_INTERVED_CARD_DETECTION 0 #endif -PG_REGISTER_WITH_RESET_TEMPLATE(blackboxConfig_t, blackboxConfig, PG_BLACKBOX_CONFIG, 0); +PG_REGISTER_WITH_RESET_TEMPLATE(blackboxConfig_t, blackboxConfig, PG_BLACKBOX_CONFIG, 1); PG_RESET_TEMPLATE(blackboxConfig_t, blackboxConfig, .device = DEFAULT_BLACKBOX_DEVICE, @@ -269,6 +270,10 @@ static const blackboxDeltaFieldDefinition_t blackboxMainFields[] = { {"debug", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_DEBUG}, {"debug", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_DEBUG}, {"debug", 3, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_DEBUG}, + {"debug", 4, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_DEBUG}, + {"debug", 5, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_DEBUG}, + {"debug", 6, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_DEBUG}, + {"debug", 7, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_DEBUG}, /* Motors only rarely drops under minthrottle (when stick falls below mincommand), so predict minthrottle for it and use *unsigned* encoding (which is large for negative numbers but more compact for positive ones): */ {"motor", 0, UNSIGNED, .Ipredict = PREDICT(MINTHROTTLE), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_MOTORS_1)}, /* Subsequent motors base their I-frame values on the first one, P-frame values on the average of last two frames: */ @@ -320,7 +325,10 @@ static const blackboxConditionalFieldDefinition_t blackboxGpsGFields[] = { {"GPS_ground_course", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB), CONDITION(ALWAYS)}, {"GPS_hdop", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB), CONDITION(ALWAYS)}, {"GPS_eph", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB), CONDITION(ALWAYS)}, - {"GPS_epv", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB), CONDITION(ALWAYS)} + {"GPS_epv", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB), CONDITION(ALWAYS)}, + {"GPS_velned", 0, SIGNED, PREDICT(0), ENCODING(SIGNED_VB), CONDITION(ALWAYS)}, + {"GPS_velned", 1, SIGNED, PREDICT(0), ENCODING(SIGNED_VB), CONDITION(ALWAYS)}, + {"GPS_velned", 2, SIGNED, PREDICT(0), ENCODING(SIGNED_VB), CONDITION(ALWAYS)} }; // GPS home frame @@ -345,6 +353,20 @@ static const blackboxSimpleFieldDefinition_t blackboxSlowFields[] = { {"wind", 0, SIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)}, {"wind", 1, SIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)}, {"wind", 2, SIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)}, + {"IMUTemperature", -1, SIGNED, PREDICT(0), ENCODING(SIGNED_VB)}, +#ifdef USE_BARO + {"baroTemperature", -1, SIGNED, PREDICT(0), ENCODING(SIGNED_VB)}, +#endif +#ifdef USE_TEMPERATURE_SENSOR + {"sens0Temp", -1, SIGNED, PREDICT(0), ENCODING(SIGNED_VB)}, + {"sens1Temp", -1, SIGNED, PREDICT(0), ENCODING(SIGNED_VB)}, + {"sens2Temp", -1, SIGNED, PREDICT(0), ENCODING(SIGNED_VB)}, + {"sens3Temp", -1, SIGNED, PREDICT(0), ENCODING(SIGNED_VB)}, + {"sens4Temp", -1, SIGNED, PREDICT(0), ENCODING(SIGNED_VB)}, + {"sens5Temp", -1, SIGNED, PREDICT(0), ENCODING(SIGNED_VB)}, + {"sens6Temp", -1, SIGNED, PREDICT(0), ENCODING(SIGNED_VB)}, + {"sens7Temp", -1, SIGNED, PREDICT(0), ENCODING(SIGNED_VB)}, +#endif }; typedef enum BlackboxState { @@ -390,7 +412,7 @@ typedef struct blackboxMainState_s { int16_t gyroADC[XYZ_AXIS_COUNT]; int16_t accADC[XYZ_AXIS_COUNT]; int16_t attitude[XYZ_AXIS_COUNT]; - int16_t debug[DEBUG16_VALUE_COUNT]; + int32_t debug[DEBUG32_VALUE_COUNT]; int16_t motor[MAX_SUPPORTED_MOTORS]; int16_t servo[MAX_SUPPORTED_SERVOS]; @@ -435,7 +457,7 @@ typedef struct blackboxGpsState_s { // This data is updated really infrequently: typedef struct blackboxSlowState_s { uint32_t flightModeFlags; // extend this data size (from uint16_t) - uint8_t stateFlags; + uint32_t stateFlags; uint8_t failsafePhase; bool rxSignalReceived; bool rxFlightChannelsValid; @@ -443,6 +465,13 @@ typedef struct blackboxSlowState_s { uint16_t powerSupplyImpedance; uint16_t sagCompensatedVBat; int16_t wind[XYZ_AXIS_COUNT]; + int16_t imuTemperature; +#ifdef USE_BARO + int16_t baroTemperature; +#endif +#ifdef USE_TEMPERATURE_SENSOR + int16_t tempSensorTemperature[MAX_TEMP_SENSORS]; +#endif } __attribute__((__packed__)) blackboxSlowState_t; // We pack this struct so that padding doesn't interfere with memcmp() //From rc_controls.c @@ -747,7 +776,7 @@ static void writeIntraframe(void) blackboxWriteSigned16VBArray(blackboxCurrent->attitude, XYZ_AXIS_COUNT); if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_DEBUG)) { - blackboxWriteSigned16VBArray(blackboxCurrent->debug, DEBUG16_VALUE_COUNT); + blackboxWriteSignedVBArray(blackboxCurrent->debug, DEBUG32_VALUE_COUNT); } //Motors can be below minthrottle when disarmed, but that doesn't happen much @@ -958,7 +987,7 @@ static void writeInterframe(void) blackboxWriteMainStateArrayUsingAveragePredictor(offsetof(blackboxMainState_t, accADC), XYZ_AXIS_COUNT); blackboxWriteMainStateArrayUsingAveragePredictor(offsetof(blackboxMainState_t, attitude), XYZ_AXIS_COUNT); if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_DEBUG)) { - blackboxWriteMainStateArrayUsingAveragePredictor(offsetof(blackboxMainState_t, debug), DEBUG16_VALUE_COUNT); + blackboxWriteMainStateArrayUsingAveragePredictor(offsetof(blackboxMainState_t, debug), DEBUG32_VALUE_COUNT); } blackboxWriteMainStateArrayUsingAveragePredictor(offsetof(blackboxMainState_t, motor), getMotorCount()); @@ -1030,6 +1059,16 @@ static void writeSlowFrame(void) blackboxWriteSigned16VBArray(slowHistory.wind, XYZ_AXIS_COUNT); + blackboxWriteSignedVB(slowHistory.imuTemperature); + +#ifdef USE_BARO + blackboxWriteSignedVB(slowHistory.baroTemperature); +#endif + +#ifdef USE_TEMPERATURE_SENSOR + blackboxWriteSigned16VBArray(slowHistory.tempSensorTemperature, MAX_TEMP_SENSORS); +#endif + blackboxSlowFrameIterationTimer = 0; } @@ -1062,6 +1101,22 @@ static void loadSlowState(blackboxSlowState_t *slow) #endif } + bool valid_temp; + valid_temp = getIMUTemperature(&slow->imuTemperature); + if (!valid_temp) slow->imuTemperature = TEMPERATURE_INVALID_VALUE; + +#ifdef USE_BARO + valid_temp = getBaroTemperature(&slow->baroTemperature); + if (!valid_temp) slow->baroTemperature = TEMPERATURE_INVALID_VALUE; +#endif + +#ifdef USE_TEMPERATURE_SENSOR + for (uint8_t index; index < MAX_TEMP_SENSORS; ++index) { + valid_temp = getSensorTemperature(index, slow->tempSensorTemperature + index); + if (!valid_temp) slow->tempSensorTemperature[index] = TEMPERATURE_INVALID_VALUE; + } +#endif + } /** @@ -1243,6 +1298,7 @@ static void writeGPSFrame(timeUs_t currentTimeUs) blackboxWriteUnsignedVB(gpsSol.hdop); blackboxWriteUnsignedVB(gpsSol.eph); blackboxWriteUnsignedVB(gpsSol.epv); + blackboxWriteSigned16VBArray(gpsSol.velNED, XYZ_AXIS_COUNT); gpsHistory.GPS_numSat = gpsSol.numSat; gpsHistory.GPS_coord[0] = gpsSol.llh.lat; @@ -1275,7 +1331,6 @@ static void loadMainState(timeUs_t currentTimeUs) #endif #ifdef USE_NAV if (!STATE(FIXED_WING)) { - // log requested velocity in cm/s blackboxCurrent->mcPosAxisP[i] = lrintf(nav_pids->pos[i].output_constrained); @@ -1283,7 +1338,7 @@ static void loadMainState(timeUs_t currentTimeUs) blackboxCurrent->mcVelAxisPID[0][i] = lrintf(nav_pids->vel[i].proportional); blackboxCurrent->mcVelAxisPID[1][i] = lrintf(nav_pids->vel[i].integral); blackboxCurrent->mcVelAxisPID[2][i] = lrintf(nav_pids->vel[i].derivative); - + blackboxCurrent->mcVelAxisOutput[i] = lrintf(nav_pids->vel[i].output_constrained); } #endif } @@ -1320,7 +1375,7 @@ static void loadMainState(timeUs_t currentTimeUs) blackboxCurrent->attitude[1] = attitude.values.pitch; blackboxCurrent->attitude[2] = attitude.values.yaw; - for (int i = 0; i < DEBUG16_VALUE_COUNT; i++) { + for (int i = 0; i < DEBUG32_VALUE_COUNT; i++) { blackboxCurrent->debug[i] = debug[i]; } @@ -1521,7 +1576,7 @@ static bool blackboxWriteSysinfo(void) BLACKBOX_PRINT_HEADER_LINE("Firmware date", "%s %s", buildDate, buildTime); BLACKBOX_PRINT_HEADER_LINE("Log start datetime", "%s", blackboxGetStartDateTime(buf)); BLACKBOX_PRINT_HEADER_LINE("Craft name", "%s", systemConfig()->name); - BLACKBOX_PRINT_HEADER_LINE("P interval", "%d/%d", blackboxConfig()->rate_num, blackboxConfig()->rate_denom); + BLACKBOX_PRINT_HEADER_LINE("P interval", "%u/%u", blackboxConfig()->rate_num, blackboxConfig()->rate_denom); BLACKBOX_PRINT_HEADER_LINE("minthrottle", "%d", motorConfig()->minthrottle); BLACKBOX_PRINT_HEADER_LINE("maxthrottle", "%d", motorConfig()->maxthrottle); BLACKBOX_PRINT_HEADER_LINE("gyro_scale", "0x%x", castFloatBytesToInt(1.0f)); @@ -1548,7 +1603,7 @@ static bool blackboxWriteSysinfo(void) } ); - BLACKBOX_PRINT_HEADER_LINE("looptime", "%d", getPidUpdateRate()); + BLACKBOX_PRINT_HEADER_LINE("looptime", "%d", getLooptime()); BLACKBOX_PRINT_HEADER_LINE("rc_rate", "%d", 100); //For compatibility reasons write rc_rate 100 BLACKBOX_PRINT_HEADER_LINE("rc_expo", "%d", currentControlRateProfile->stabilized.rcExpo8); BLACKBOX_PRINT_HEADER_LINE("rc_yaw_expo", "%d", currentControlRateProfile->stabilized.rcYawExpo8); @@ -1597,7 +1652,7 @@ static bool blackboxWriteSysinfo(void) gyroConfig()->gyro_soft_notch_hz_2); BLACKBOX_PRINT_HEADER_LINE("gyro_notch_cutoff", "%d,%d", gyroConfig()->gyro_soft_notch_cutoff_1, gyroConfig()->gyro_soft_notch_cutoff_2); - BLACKBOX_PRINT_HEADER_LINE("acc_lpf_hz", "%d", pidProfile()->acc_soft_lpf_hz); + BLACKBOX_PRINT_HEADER_LINE("acc_lpf_hz", "%d", accelerometerConfig()->acc_lpf_hz); BLACKBOX_PRINT_HEADER_LINE("acc_hardware", "%d", accelerometerConfig()->acc_hardware); BLACKBOX_PRINT_HEADER_LINE("baro_hardware", "%d", barometerConfig()->baro_hardware); BLACKBOX_PRINT_HEADER_LINE("mag_hardware", "%d", compassConfig()->mag_hardware); @@ -1661,7 +1716,7 @@ void blackboxLogEvent(FlightLogEvent event, flightLogEventData_t *data) blackboxWriteUnsignedVB(data->loggingResume.currentTimeUs); break; case FLIGHT_LOG_EVENT_IMU_FAILURE: - blackboxWrite(0); + blackboxWriteUnsignedVB(data->imuError.errorCode); break; case FLIGHT_LOG_EVENT_LOG_END: blackboxPrintf("End of log (disarm reason:%d)", getDisarmReason()); @@ -1930,18 +1985,18 @@ void blackboxInit(void) blackboxSetState(BLACKBOX_STATE_DISABLED); } + /* FIXME is this really necessary ? Why? */ + int max_denom = 4096*1000 / gyroConfig()->looptime; + if (blackboxConfig()->rate_denom > max_denom) { + blackboxConfigMutable()->rate_denom = max_denom; + } /* Decide on how ofter are we going to log I-frames*/ if (blackboxConfig()->rate_denom <= 32) { blackboxIFrameInterval = 32; } - else if (blackboxConfig()->rate_denom <= 64) { - blackboxIFrameInterval = 64; - } - else if (blackboxConfig()->rate_denom <= 128) { - blackboxIFrameInterval = 128; - } else { - blackboxIFrameInterval = 256; + // Use next higher power of two via GCC builtin + blackboxIFrameInterval = 1 << (32 - __builtin_clz (blackboxConfig()->rate_denom - 1)); } } #endif diff --git a/src/main/blackbox/blackbox.h b/src/main/blackbox/blackbox.h index 0ce907e36cc..0b180e9f7c5 100644 --- a/src/main/blackbox/blackbox.h +++ b/src/main/blackbox/blackbox.h @@ -22,8 +22,8 @@ #include "config/parameter_group.h" typedef struct blackboxConfig_s { - uint8_t rate_num; - uint8_t rate_denom; + uint16_t rate_num; + uint16_t rate_denom; uint8_t device; uint8_t invertedCardDetection; } blackboxConfig_t; diff --git a/src/main/blackbox/blackbox_fielddefs.h b/src/main/blackbox/blackbox_fielddefs.h index aa15507a470..ea07135956f 100644 --- a/src/main/blackbox/blackbox_fielddefs.h +++ b/src/main/blackbox/blackbox_fielddefs.h @@ -139,6 +139,10 @@ typedef struct flightLogEvent_loggingResume_s { timeUs_t currentTimeUs; } flightLogEvent_loggingResume_t; +typedef struct flightLogEvent_IMUError_s { + uint32_t errorCode; +} flightLogEvent_IMUError_t; + #define FLIGHT_LOG_EVENT_INFLIGHT_ADJUSTMENT_FUNCTION_FLOAT_VALUE_FLAG 128 typedef union flightLogEventData_u { @@ -146,6 +150,7 @@ typedef union flightLogEventData_u { flightLogEvent_flightMode_t flightMode; // New event data flightLogEvent_inflightAdjustment_t inflightAdjustment; flightLogEvent_loggingResume_t loggingResume; + flightLogEvent_IMUError_t imuError; } flightLogEventData_t; typedef struct flightLogEvent_s { diff --git a/src/main/build/debug.c b/src/main/build/debug.c index 9aaa4693667..7852afd98ca 100644 --- a/src/main/build/debug.c +++ b/src/main/build/debug.c @@ -43,7 +43,7 @@ timeUs_t sectionTimes[2][4]; #endif -int16_t debug[DEBUG16_VALUE_COUNT]; +int32_t debug[DEBUG32_VALUE_COUNT]; uint8_t debugMode; #if defined(USE_DEBUG_TRACE) diff --git a/src/main/build/debug.h b/src/main/build/debug.h index 47cdd5557d4..5b66b08a673 100644 --- a/src/main/build/debug.h +++ b/src/main/build/debug.h @@ -19,8 +19,8 @@ #include #include -#define DEBUG16_VALUE_COUNT 4 -extern int16_t debug[DEBUG16_VALUE_COUNT]; +#define DEBUG32_VALUE_COUNT 8 +extern int32_t debug[DEBUG32_VALUE_COUNT]; extern uint8_t debugMode; #define DEBUG_SET(mode, index, value) {if (debugMode == (mode)) {debug[(index)] = (value);}} @@ -67,6 +67,8 @@ typedef enum { DEBUG_CRUISE, DEBUG_REM_FLIGHT_TIME, DEBUG_SMARTAUDIO, + DEBUG_ACC, + DEBUG_GENERIC, DEBUG_COUNT } debugType_e; diff --git a/src/main/build/version.c b/src/main/build/version.c index b4a9ea928f9..566f5b6c6f5 100644 --- a/src/main/build/version.c +++ b/src/main/build/version.c @@ -18,6 +18,7 @@ #include "version.h" const char * const targetName = __TARGET__; +const char * const compilerVersion = __VERSION__; const char * const shortGitRevision = __REVISION__; const char * const buildDate = __DATE__; -const char * const buildTime = __TIME__; +const char * const buildTime = __TIME__; \ No newline at end of file diff --git a/src/main/build/version.h b/src/main/build/version.h index fb83d427e07..a6fc172ff55 100644 --- a/src/main/build/version.h +++ b/src/main/build/version.h @@ -16,8 +16,8 @@ */ #define FC_VERSION_MAJOR 2 // increment when a major release is made (big new feature, etc) -#define FC_VERSION_MINOR 0 // increment when a minor release is made (small new feature, change etc) -#define FC_VERSION_PATCH_LEVEL 1 // increment when a bug is fixed +#define FC_VERSION_MINOR 1 // increment when a minor release is made (small new feature, change etc) +#define FC_VERSION_PATCH_LEVEL 0 // increment when a bug is fixed #define STR_HELPER(x) #x #define STR(x) STR_HELPER(x) @@ -26,6 +26,7 @@ #define MW_VERSION 231 +extern const char* const compilerVersion; extern const char* const targetName; #define GIT_SHORT_REVISION_LENGTH 8 // lower case hexadecimal digits. diff --git a/src/main/cms/cms_menu_builtin.c b/src/main/cms/cms_menu_builtin.c index 32336c4788b..df9feb7acf7 100644 --- a/src/main/cms/cms_menu_builtin.c +++ b/src/main/cms/cms_menu_builtin.c @@ -53,6 +53,7 @@ #include "cms/cms_menu_vtx_smartaudio.h" #include "cms/cms_menu_vtx_tramp.h" +#include "cms/cms_menu_vtx_ffpv.h" // Info @@ -119,6 +120,9 @@ static const OSD_Entry menuFeaturesEntries[] = #if defined(USE_VTX_TRAMP) OSD_SUBMENU_ENTRY("VTX TR", &cmsx_menuVtxTramp), #endif +#if defined(USE_VTX_FFPV) + OSD_SUBMENU_ENTRY("VTX FFPV", &cmsx_menuVtxFFPV), +#endif #endif // VTX_CONTROL #ifdef USE_LED_STRIP OSD_SUBMENU_ENTRY("LED STRIP", &cmsx_menuLedstrip), @@ -151,10 +155,10 @@ static const OSD_Entry menuMainEntries[] = OSD_SUBMENU_ENTRY("OSD", &cmsx_menuOsd), #endif OSD_SUBMENU_ENTRY("BATTERY", &cmsx_menuBattery), - OSD_SUBMENU_ENTRY("FC&FW INFO", &menuInfo), + OSD_SUBMENU_ENTRY("FC+FW INFO", &menuInfo), OSD_SUBMENU_ENTRY("MISC", &cmsx_menuMisc), - {"SAVE&REBOOT", OME_OSD_Exit, {.func = cmsMenuExit}, (void*)CMS_EXIT_SAVEREBOOT, 0}, + {"SAVE+REBOOT", OME_OSD_Exit, {.func = cmsMenuExit}, (void*)CMS_EXIT_SAVEREBOOT, 0}, {"EXIT", OME_OSD_Exit, {.func = cmsMenuExit}, (void*)CMS_EXIT, 0}, #ifdef CMS_MENU_DEBUG OSD_SUBMENU_ENTRY("ERR SAMPLE", &menuInfoEntries[0]), diff --git a/src/main/cms/cms_menu_osd.c b/src/main/cms/cms_menu_osd.c index cd28fd9e555..7690b5aec32 100755 --- a/src/main/cms/cms_menu_osd.c +++ b/src/main/cms/cms_menu_osd.c @@ -166,9 +166,7 @@ static const OSD_Entry menuOsdElemsEntries[] = OSD_ELEMENT_ENTRY("THR. (MANU)", OSD_THROTTLE_POS), OSD_ELEMENT_ENTRY("THR. (MANU/AUTO)", OSD_THROTTLE_POS_AUTO_THR), OSD_ELEMENT_ENTRY("SYS MESSAGES", OSD_MESSAGES), -#ifdef USE_VTX_COMMON OSD_ELEMENT_ENTRY("VTX CHAN", OSD_VTX_CHANNEL), -#endif // VTX OSD_ELEMENT_ENTRY("CURRENT (A)", OSD_CURRENT_DRAW), OSD_ELEMENT_ENTRY("POWER", OSD_POWER), OSD_ELEMENT_ENTRY("USED MAH", OSD_MAH_DRAWN), @@ -187,6 +185,8 @@ static const OSD_Entry menuOsdElemsEntries[] = OSD_ELEMENT_ENTRY("GPS LAT", OSD_GPS_LAT), OSD_ELEMENT_ENTRY("GPS LON", OSD_GPS_LON), OSD_ELEMENT_ENTRY("GPS HDOP", OSD_GPS_HDOP), + OSD_ELEMENT_ENTRY("3D SPEED", OSD_3D_SPEED), + OSD_ELEMENT_ENTRY("PLUS CODE", OSD_PLUS_CODE), #endif // GPS OSD_ELEMENT_ENTRY("HEADING", OSD_HEADING), OSD_ELEMENT_ENTRY("HEADING GR.", OSD_HEADING_GRAPH), @@ -197,6 +197,7 @@ static const OSD_Entry menuOsdElemsEntries[] = OSD_ELEMENT_ENTRY("VARIO NUM", OSD_VARIO_NUM), #endif // defined OSD_ELEMENT_ENTRY("ALTITUDE", OSD_ALTITUDE), + OSD_ELEMENT_ENTRY("ALTITUDE MSL", OSD_ALTITUDE_MSL), #if defined(USE_PITOT) OSD_ELEMENT_ENTRY("AIR SPEED", OSD_AIR_SPEED), #endif @@ -244,11 +245,27 @@ static const OSD_Entry menuOsdElemsEntries[] = OSD_ELEMENT_ENTRY("WIND HOR", OSD_WIND_SPEED_HORIZONTAL), OSD_ELEMENT_ENTRY("WIND VERT", OSD_WIND_SPEED_VERTICAL), + OSD_ELEMENT_ENTRY("IMU TEMP", OSD_IMU_TEMPERATURE), +#ifdef USE_BARO + OSD_ELEMENT_ENTRY("BARO TEMP", OSD_BARO_TEMPERATURE), +#endif + +#ifdef USE_TEMPERATURE_SENSOR + OSD_ELEMENT_ENTRY("SENSOR 0 TEMP", OSD_TEMP_SENSOR_0_TEMPERATURE), + OSD_ELEMENT_ENTRY("SENSOR 1 TEMP", OSD_TEMP_SENSOR_1_TEMPERATURE), + OSD_ELEMENT_ENTRY("SENSOR 2 TEMP", OSD_TEMP_SENSOR_2_TEMPERATURE), + OSD_ELEMENT_ENTRY("SENSOR 3 TEMP", OSD_TEMP_SENSOR_3_TEMPERATURE), + OSD_ELEMENT_ENTRY("SENSOR 4 TEMP", OSD_TEMP_SENSOR_4_TEMPERATURE), + OSD_ELEMENT_ENTRY("SENSOR 5 TEMP", OSD_TEMP_SENSOR_5_TEMPERATURE), + OSD_ELEMENT_ENTRY("SENSOR 6 TEMP", OSD_TEMP_SENSOR_6_TEMPERATURE), + OSD_ELEMENT_ENTRY("SENSOR 7 TEMP", OSD_TEMP_SENSOR_7_TEMPERATURE), +#endif + OSD_BACK_ENTRY, OSD_END_ENTRY, }; -#if defined(USE_VTX_COMMON) && defined(USE_GPS) && defined(USE_BARO) && defined(USE_PITOT) +#if defined(USE_GPS) && defined(USE_BARO) && defined(USE_PITOT) && defined(USE_TEMPERATURE_SENSOR) // All CMS OSD elements should be enabled in this case. The menu has 3 extra // elements (label, back and end), but there's an OSD element that we intentionally // don't show here (OSD_DEBUG). @@ -304,7 +321,7 @@ static long osdElementsOnEnter(const OSD_Entry *from) // First entry is the label. Store the current layout // and override it on the OSD so previews so this layout. osdCurrentLayout = from - cmsx_menuOsdLayoutEntries - 1; - osdOverrideLayout(osdCurrentLayout); + osdOverrideLayout(osdCurrentLayout, 0); return 0; } @@ -313,7 +330,7 @@ static long osdElementsOnExit(const OSD_Entry *from) UNUSED(from); // Stop overriding OSD layout - osdOverrideLayout(-1); + osdOverrideLayout(-1, 0); return 0; } diff --git a/src/main/cms/cms_menu_vtx_ffpv.c b/src/main/cms/cms_menu_vtx_ffpv.c new file mode 100644 index 00000000000..f0c17d335d1 --- /dev/null +++ b/src/main/cms/cms_menu_vtx_ffpv.c @@ -0,0 +1,216 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software under the terms of the + * GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) + * any later version. + * + * Cleanflight and Betaflight are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +#include +#include +#include +#include + +#include "platform.h" + +#if defined(USE_CMS) && defined(USE_VTX_FFPV) + +#include "common/printf.h" +#include "common/utils.h" + +#include "cms/cms.h" +#include "cms/cms_types.h" + +#include "drivers/vtx_common.h" + +#include "fc/config.h" + +#include "io/vtx_string.h" +#include "io/vtx_ffpv24g.h" +#include "io/vtx.h" + +static bool ffpvCmsDrawStatusString(char *buf, unsigned bufsize) +{ + const char *defaultString = "- -- ---- ---"; +// m bc ffff ppp +// 01234567890123 + + if (bufsize < strlen(defaultString) + 1) { + return false; + } + + strcpy(buf, defaultString); + + vtxDevice_t *vtxDevice = vtxCommonDevice(); + if (!vtxDevice || vtxCommonGetDeviceType(vtxDevice) != VTXDEV_FFPV || !vtxCommonDeviceIsReady(vtxDevice)) { + return true; + } + + buf[0] = '*'; + buf[1] = ' '; + buf[2] = ffpvBandLetters[ffpvGetRuntimeState()->band]; + buf[3] = ffpvChannelNames[ffpvGetRuntimeState()->channel][0]; + buf[4] = ' '; + + tfp_sprintf(&buf[5], "%4d", ffpvGetRuntimeState()->frequency); + tfp_sprintf(&buf[9], " %3d", ffpvGetRuntimeState()->powerMilliwatt); + + return true; +} + +uint8_t ffpvCmsBand = 1; +uint8_t ffpvCmsChan = 1; +uint16_t ffpvCmsFreqRef; +static uint8_t ffpvCmsPower = 1; + +static const OSD_TAB_t ffpvCmsEntBand = { &ffpvCmsBand, VTX_FFPV_BAND_COUNT, ffpvBandNames }; +static const OSD_TAB_t ffpvCmsEntChan = { &ffpvCmsChan, VTX_FFPV_CHANNEL_COUNT, ffpvChannelNames }; +static const OSD_TAB_t ffpvCmsEntPower = { &ffpvCmsPower, VTX_FFPV_POWER_COUNT, ffpvPowerNames }; + +static void ffpvCmsUpdateFreqRef(void) +{ + if (ffpvCmsBand > 0 && ffpvCmsChan > 0) { + ffpvCmsFreqRef = ffpvFrequencyTable[ffpvCmsBand - 1][ffpvCmsChan - 1]; + } +} + +static long ffpvCmsConfigBand(displayPort_t *pDisp, const void *self) +{ + UNUSED(pDisp); + UNUSED(self); + + if (ffpvCmsBand == 0) { + // Bounce back + ffpvCmsBand = 1; + } + else { + ffpvCmsUpdateFreqRef(); + } + + return 0; +} + +static long ffpvCmsConfigChan(displayPort_t *pDisp, const void *self) +{ + UNUSED(pDisp); + UNUSED(self); + + if (ffpvCmsChan == 0) { + // Bounce back + ffpvCmsChan = 1; + } + else { + ffpvCmsUpdateFreqRef(); + } + + return 0; +} + +static long ffpvCmsConfigPower(displayPort_t *pDisp, const void *self) +{ + UNUSED(pDisp); + UNUSED(self); + + if (ffpvCmsPower == 0) { + // Bounce back + ffpvCmsPower = 1; + } + + return 0; +} + +static long ffpvCmsCommence(displayPort_t *pDisp, const void *self) +{ + UNUSED(pDisp); + UNUSED(self); + + // call driver directly + ffpvSetBandAndChannel(ffpvCmsBand, ffpvCmsChan); + ffpvSetRFPowerByIndex(ffpvCmsPower); + + // update'vtx_' settings + vtxSettingsConfigMutable()->band = ffpvCmsBand; + vtxSettingsConfigMutable()->channel = ffpvCmsChan; + vtxSettingsConfigMutable()->power = ffpvCmsPower; + vtxSettingsConfigMutable()->freq = ffpvFrequencyTable[ffpvCmsBand - 1][ffpvCmsChan - 1]; + + saveConfigAndNotify(); + + return MENU_CHAIN_BACK; +} + +static void ffpvCmsInitSettings(void) +{ + ffpvCmsBand = ffpvGetRuntimeState()->band; + ffpvCmsChan = ffpvGetRuntimeState()->channel; + ffpvCmsPower = ffpvGetRuntimeState()->powerIndex; + + ffpvCmsUpdateFreqRef(); +} + +static long ffpvCmsOnEnter(const OSD_Entry *from) +{ + UNUSED(from); + + ffpvCmsInitSettings(); + return 0; +} + +static const OSD_Entry ffpvCmsMenuCommenceEntries[] = +{ + OSD_LABEL_ENTRY("CONFIRM"), + OSD_FUNC_CALL_ENTRY("YES", ffpvCmsCommence), + + OSD_BACK_ENTRY, + OSD_END_ENTRY, +}; + +static const CMS_Menu ffpvCmsMenuCommence = { +#ifdef CMS_MENU_DEBUG + .GUARD_text = "XVTXTRC", + .GUARD_type = OME_MENU, +#endif + .onEnter = NULL, + .onExit = NULL, + .onGlobalExit = NULL, + .entries = ffpvCmsMenuCommenceEntries, +}; + +static const OSD_Entry ffpvMenuEntries[] = +{ + OSD_LABEL_ENTRY("- TRAMP -"), + + OSD_LABEL_FUNC_DYN_ENTRY("", ffpvCmsDrawStatusString), + OSD_TAB_CALLBACK_ENTRY("BAND", ffpvCmsConfigBand, &ffpvCmsEntBand), + OSD_TAB_CALLBACK_ENTRY("CHAN", ffpvCmsConfigChan, &ffpvCmsEntChan), + OSD_UINT16_RO_ENTRY("(FREQ)", &ffpvCmsFreqRef), + OSD_TAB_CALLBACK_ENTRY("POWER", ffpvCmsConfigPower, &ffpvCmsEntPower), + OSD_SUBMENU_ENTRY("SET", &ffpvCmsMenuCommence), + + OSD_BACK_ENTRY, + OSD_END_ENTRY, +}; + +const CMS_Menu cmsx_menuVtxFFPV = { +#ifdef CMS_MENU_DEBUG + .GUARD_text = "XVTXTR", + .GUARD_type = OME_MENU, +#endif + .onEnter = ffpvCmsOnEnter, + .onExit = NULL, + .onGlobalExit = NULL, + .entries = ffpvMenuEntries, +}; +#endif diff --git a/src/main/cms/cms_menu_vtx_ffpv.h b/src/main/cms/cms_menu_vtx_ffpv.h new file mode 100644 index 00000000000..6faf7cb59ab --- /dev/null +++ b/src/main/cms/cms_menu_vtx_ffpv.h @@ -0,0 +1,23 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once + +#include "cms/cms.h" +#include "cms/cms_types.h" + +extern const CMS_Menu cmsx_menuVtxFFPV; diff --git a/src/main/common/bitarray.c b/src/main/common/bitarray.c index ac8e1dc46b2..92332b9e332 100755 --- a/src/main/common/bitarray.c +++ b/src/main/common/bitarray.c @@ -17,6 +17,7 @@ #include #include +#include #include "bitarray.h" @@ -37,6 +38,16 @@ void bitArrayClr(bitarrayElement_t *array, unsigned bit) BITARRAY_BIT_OP((uint32_t*)array, bit, &=~); } +void bitArraySetAll(bitarrayElement_t *array, size_t size) +{ + memset(array, 0xFF, size); +} + +void bitArrayClrAll(bitarrayElement_t *array, size_t size) +{ + memset(array, 0, size); +} + __attribute__((always_inline)) static inline uint8_t __CTZ(uint32_t val) { // __builtin_ctz is not defined for zero, since it's arch @@ -67,7 +78,7 @@ int bitArrayFindFirstSet(const bitarrayElement_t *array, unsigned start, size_t // First iteration might need to mask some bits uint32_t mask = 0xFFFFFFFF << (start % (8 * 4)); if ((ret = __CTZ(*p & mask)) != 32) { - return ret; + return (((char *)p) - ((char *)ptr)) * 8 + ret; } p++; while (p < end) { diff --git a/src/main/common/bitarray.h b/src/main/common/bitarray.h index b84f8e366a6..2fa9423f0a1 100755 --- a/src/main/common/bitarray.h +++ b/src/main/common/bitarray.h @@ -34,6 +34,12 @@ bool bitArrayGet(const bitarrayElement_t *array, unsigned bit); void bitArraySet(bitarrayElement_t *array, unsigned bit); void bitArrayClr(bitarrayElement_t *array, unsigned bit); +void bitArraySetAll(bitarrayElement_t *array, size_t size); +void bitArrayClrAll(bitarrayElement_t *array, size_t size); + +#define BITARRAY_SET_ALL(array) bitArraySetAll(array, sizeof(array)) +#define BITARRAY_CLR_ALL(array) bitArrayClrAll(array, sizeof(array)) + // Returns the first set bit with pos >= start_bit, or -1 if all bits // are zero. Note that size must indicate the size of array in bytes. // In most cases, you should use the BITARRAY_FIND_FIRST_SET() macro diff --git a/src/main/common/calibration.c b/src/main/common/calibration.c new file mode 100644 index 00000000000..4c5efba6846 --- /dev/null +++ b/src/main/common/calibration.c @@ -0,0 +1,189 @@ +/* + * This file is part of INAV Project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute and/or modify + * it under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or (at your + * option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see http://www.gnu.org/licenses/. + */ + +#include +#include +#include + +#include "build/debug.h" +#include "drivers/time.h" +#include "common/calibration.h" + +void zeroCalibrationStartS(zeroCalibrationScalar_t * s, timeMs_t window, float threshold, bool allowFailure) +{ + // Reset parameters and state + s->params.state = ZERO_CALIBRATION_IN_PROGRESS; + s->params.startTimeMs = millis(); + s->params.windowSizeMs = window; + s->params.stdDevThreshold = threshold; + s->params.allowFailure = allowFailure; + + s->params.sampleCount = 0; + s->val.accumulatedValue = 0; + devClear(&s->val.stdDev); +} + +bool zeroCalibrationIsCompleteS(zeroCalibrationScalar_t * s) +{ + return !(s->params.state == ZERO_CALIBRATION_IN_PROGRESS); +} + +bool zeroCalibrationIsSuccessfulS(zeroCalibrationScalar_t * s) +{ + return (s->params.state == ZERO_CALIBRATION_DONE); +} + +void zeroCalibrationAddValueS(zeroCalibrationScalar_t * s, const float v) +{ + if (s->params.state != ZERO_CALIBRATION_IN_PROGRESS) { + return; + } + + // Add value + s->val.accumulatedValue += v; + s->params.sampleCount++; + devPush(&s->val.stdDev, v); + + // Check if calibration is complete + if ((millis() - s->params.startTimeMs) > s->params.windowSizeMs) { + const float stddev = devStandardDeviation(&s->val.stdDev); + if (stddev > s->params.stdDevThreshold) { + if (!s->params.allowFailure) { + // If deviation is too big - restart calibration + s->params.startTimeMs = millis(); + s->params.sampleCount = 0; + s->val.accumulatedValue = 0; + devClear(&s->val.stdDev); + } + else { + // We are allowed to fail + s->params.state = ZERO_CALIBRATION_FAIL; + } + } + else { + // All seems ok - calculate average value + s->val.accumulatedValue = s->val.accumulatedValue / s->params.sampleCount; + s->params.state = ZERO_CALIBRATION_DONE; + } + } +} + +void zeroCalibrationGetZeroS(zeroCalibrationScalar_t * s, float * v) +{ + if (s->params.state != ZERO_CALIBRATION_DONE) { + *v = 0.0f; + } + else { + *v = s->val.accumulatedValue; + } +} + +void zeroCalibrationStartV(zeroCalibrationVector_t * s, timeMs_t window, float threshold, bool allowFailure) +{ + // Reset parameters and state + s->params.state = ZERO_CALIBRATION_IN_PROGRESS; + s->params.startTimeMs = millis(); + s->params.windowSizeMs = window; + s->params.stdDevThreshold = threshold; + s->params.allowFailure = allowFailure; + + s->params.sampleCount = 0; + for (int i = 0; i < 3; i++) { + s->val[i].accumulatedValue = 0; + devClear(&s->val[i].stdDev); + } +} + +bool zeroCalibrationIsCompleteV(zeroCalibrationVector_t * s) +{ + return !(s->params.state == ZERO_CALIBRATION_IN_PROGRESS); +} + +bool zeroCalibrationIsSuccessfulV(zeroCalibrationVector_t * s) +{ + return (s->params.state == ZERO_CALIBRATION_DONE); +} + +void zeroCalibrationAddValueV(zeroCalibrationVector_t * s, const fpVector3_t * v) +{ + if (s->params.state != ZERO_CALIBRATION_IN_PROGRESS) { + return; + } + + // Add value + for (int i = 0; i < 3; i++) { + s->val[i].accumulatedValue += v->v[i]; + devPush(&s->val[i].stdDev, v->v[i]); + } + + s->params.sampleCount++; + + // Check if calibration is complete + if ((millis() - s->params.startTimeMs) > s->params.windowSizeMs) { + bool needRecalibration = false; + + for (int i = 0; i < 3 && !needRecalibration; i++) { + const float stddev = devStandardDeviation(&s->val[i].stdDev); + //debug[i] = stddev; + if (stddev > s->params.stdDevThreshold) { + needRecalibration = true; + } + } + + if (needRecalibration) { + if (!s->params.allowFailure) { + // If deviation is too big - restart calibration + s->params.startTimeMs = millis(); + s->params.sampleCount = 0; + for (int i = 0; i < 3; i++) { + s->val[i].accumulatedValue = 0; + devClear(&s->val[i].stdDev); + } + } + else { + // We are allowed to fail + s->params.state = ZERO_CALIBRATION_FAIL; + } + } + else { + // All seems ok - calculate average value + s->val[0].accumulatedValue = s->val[0].accumulatedValue / s->params.sampleCount; + s->val[1].accumulatedValue = s->val[1].accumulatedValue / s->params.sampleCount; + s->val[2].accumulatedValue = s->val[2].accumulatedValue / s->params.sampleCount; + s->params.state = ZERO_CALIBRATION_DONE; + } + } +} + +void zeroCalibrationGetZeroV(zeroCalibrationVector_t * s, fpVector3_t * v) +{ + if (s->params.state != ZERO_CALIBRATION_DONE) { + vectorZero(v); + } + else { + v->v[0] = s->val[0].accumulatedValue; + v->v[1] = s->val[1].accumulatedValue; + v->v[2] = s->val[2].accumulatedValue; + } +} diff --git a/src/main/common/calibration.h b/src/main/common/calibration.h new file mode 100644 index 00000000000..a6443aae485 --- /dev/null +++ b/src/main/common/calibration.h @@ -0,0 +1,76 @@ +/* + * This file is part of INAV Project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute and/or modify + * it under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or (at your + * option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see http://www.gnu.org/licenses/. + */ + +#pragma once + +#include +#include +#include + +#include "common/maths.h" +#include "common/time.h" +#include "common/vector.h" + +typedef enum { + ZERO_CALIBRATION_NONE, + ZERO_CALIBRATION_IN_PROGRESS, + ZERO_CALIBRATION_DONE, + ZERO_CALIBRATION_FAIL, +} zeroCalibrationState_e; + +typedef struct { + zeroCalibrationState_e state; + timeMs_t startTimeMs; + timeMs_t windowSizeMs; + bool allowFailure; + unsigned sampleCount; + float stdDevThreshold; +} zeroCalibrationParams_t; + +typedef struct { + float accumulatedValue; + stdev_t stdDev; +} zeroCalibrationValue_t; + +typedef struct { + zeroCalibrationParams_t params; + zeroCalibrationValue_t val; +} zeroCalibrationScalar_t; + +typedef struct { + zeroCalibrationParams_t params; + zeroCalibrationValue_t val[3]; +} zeroCalibrationVector_t; + +void zeroCalibrationStartS(zeroCalibrationScalar_t * s, timeMs_t window, float threshold, bool allowFailure); +bool zeroCalibrationIsCompleteS(zeroCalibrationScalar_t * s); +bool zeroCalibrationIsSuccessfulS(zeroCalibrationScalar_t * s); +void zeroCalibrationAddValueS(zeroCalibrationScalar_t * s, const float v); +void zeroCalibrationGetZeroS(zeroCalibrationScalar_t * s, float * v); + +void zeroCalibrationStartV(zeroCalibrationVector_t * s, timeMs_t window, float threshold, bool allowFailure); +bool zeroCalibrationIsCompleteV(zeroCalibrationVector_t * s); +bool zeroCalibrationIsSuccessfulV(zeroCalibrationVector_t * s); +void zeroCalibrationAddValueV(zeroCalibrationVector_t * s, const fpVector3_t * v); +void zeroCalibrationGetZeroV(zeroCalibrationVector_t * s, fpVector3_t * v); diff --git a/src/main/common/filter.c b/src/main/common/filter.c index 924383bef88..a2a7551cdb9 100644 --- a/src/main/common/filter.c +++ b/src/main/common/filter.c @@ -40,11 +40,12 @@ float nullFilterApply(void *filter, float input) // f_cut = cutoff frequency void pt1FilterInitRC(pt1Filter_t *filter, float tau, float dT) { + filter->state = 0.0f; filter->RC = tau; filter->dT = dT; } -void pt1FilterInit(pt1Filter_t *filter, uint8_t f_cut, float dT) +void pt1FilterInit(pt1Filter_t *filter, float f_cut, float dT) { pt1FilterInitRC(filter, 1.0f / (2.0f * M_PIf * f_cut), dT); } @@ -70,7 +71,7 @@ float pt1FilterApply3(pt1Filter_t *filter, float input, float dT) return filter->state; } -float pt1FilterApply4(pt1Filter_t *filter, float input, uint16_t f_cut, float dT) +float pt1FilterApply4(pt1Filter_t *filter, float input, float f_cut, float dT) { // Pre calculate and store RC if (!filter->RC) { diff --git a/src/main/common/filter.h b/src/main/common/filter.h index 2811095ce17..ae417364379 100644 --- a/src/main/common/filter.h +++ b/src/main/common/filter.h @@ -55,13 +55,13 @@ typedef float (*filterApplyFnPtr)(void *filter, float input); float nullFilterApply(void *filter, float input); -void pt1FilterInit(pt1Filter_t *filter, uint8_t f_cut, float dT); +void pt1FilterInit(pt1Filter_t *filter, float f_cut, float dT); void pt1FilterInitRC(pt1Filter_t *filter, float tau, float dT); void pt1FilterSetTimeConstant(pt1Filter_t *filter, float tau); float pt1FilterGetLastOutput(pt1Filter_t *filter); float pt1FilterApply(pt1Filter_t *filter, float input); float pt1FilterApply3(pt1Filter_t *filter, float input, float dT); -float pt1FilterApply4(pt1Filter_t *filter, float input, uint16_t f_cut, float dt); +float pt1FilterApply4(pt1Filter_t *filter, float input, float f_cut, float dt); void pt1FilterReset(pt1Filter_t *filter, float input); void rateLimitFilterInit(rateLimitFilter_t *filter); diff --git a/src/main/common/memory.c b/src/main/common/memory.c index 977c6aad5a4..f6aea5c057b 100644 --- a/src/main/common/memory.c +++ b/src/main/common/memory.c @@ -33,7 +33,7 @@ #include "fc/runtime_config.h" #if !defined(DYNAMIC_HEAP_SIZE) -#define DYNAMIC_HEAP_SIZE (1024) +#define DYNAMIC_HEAP_SIZE (2048) #endif static uint32_t dynHeap[DYNAMIC_HEAP_SIZE / sizeof(uint32_t)]; diff --git a/src/main/common/olc.c b/src/main/common/olc.c new file mode 100644 index 00000000000..7ae5ae58217 --- /dev/null +++ b/src/main/common/olc.c @@ -0,0 +1,221 @@ +#include + +#include "common/maths.h" +#include "common/olc.h" + +// This is a port of https://github.com/google/open-location-code/blob/master/c/olc.c +// to avoid double floating point math and use integer math as much as possible. + +#define SEPARATOR_CHAR '+' +#define SEPARATOR_POS 8 +#define PADDING_CHAR '0' + +#define ENCODING_BASE 20 +#define PAIR_CODE_LEN 10u +#define CODE_LEN_MAX 15u + +#define GRID_COLS 4 +#define GRID_ROWS (ENCODING_BASE / GRID_COLS) + + +#define LAT_MAX (90 * OLC_DEG_MULTIPLIER) +#define LON_MAX (180 * OLC_DEG_MULTIPLIER) + +static const char alphabet[] = "23456789CFGHJMPQRVWX"; + +// Initialized via init_constants() +static olc_coord_t grid_size; +static olc_coord_t initial_resolution; + +static void init_constants(void) +{ + static int inited = 0; + if (inited) { + return; + } + inited = 1; + + // Work out the encoding base exponent necessary to represent 360 degrees. + olc_coord_t initial_exponent = floorf(logf(2 * (LON_MAX / OLC_DEG_MULTIPLIER)) / logf(ENCODING_BASE)); + + // Work out the enclosing resolution (in degrees) for the grid algorithm. + grid_size = (1 / powf(ENCODING_BASE, PAIR_CODE_LEN / 2 - (initial_exponent + 1))) * OLC_DEG_MULTIPLIER; + + // Work out the initial resolution + initial_resolution = powf(ENCODING_BASE, initial_exponent) * OLC_DEG_MULTIPLIER; +} + +// Raises a number to an exponent, handling negative exponents. +static float pow_negf(float base, float exponent) +{ + if (exponent == 0) { + return 1; + } + + if (exponent > 0) { + return powf(base, exponent); + } + + return 1 / powf(base, -exponent); +} + +// Compute the latitude precision value for a given code length. Lengths <= 10 +// have the same precision for latitude and longitude, but lengths > 10 have +// different precisions due to the grid method having fewer columns than rows. +static float compute_precision_for_length(int length) +{ + // Magic numbers! + if (length <= (int)PAIR_CODE_LEN) { + return pow_negf(ENCODING_BASE, floorf((length / -2) + 2)); + } + + return pow_negf(ENCODING_BASE, -3) / powf(5, length - (int)PAIR_CODE_LEN); +} + +static olc_coord_t adjust_latitude(olc_coord_t lat, size_t code_len) +{ + if (lat < -LAT_MAX) { + lat = -LAT_MAX; + } + if (lat > LAT_MAX) { + lat = LAT_MAX; + } + if (lat >= LAT_MAX) { + // Subtract half the code precision to get the latitude into the code area. + olc_coord_t precision = compute_precision_for_length(code_len) * OLC_DEG_MULTIPLIER; + lat -= precision / 2; + } + return lat; +} + +static olc_coord_t normalize_longitude(olc_coord_t lon) +{ + while (lon < -LON_MAX) { + lon += LON_MAX; + lon += LON_MAX; + } + while (lon >= LON_MAX) { + lon -= LON_MAX; + lon -= LON_MAX; + } + return lon; +} + +// Encodes positive range lat,lon into a sequence of OLC lat/lon pairs. This +// uses pairs of characters (latitude and longitude in that order) to represent +// each step in a 20x20 grid. Each code, therefore, has 1/400th the area of +// the previous code. +static unsigned encode_pairs(uolc_coord_t lat, uolc_coord_t lon, size_t length, char *buf, size_t bufsize) +{ + if ((length + 1) >= bufsize) { + buf[0] = '\0'; + return 0; + } + + unsigned pos = 0; + olc_coord_t resolution = initial_resolution; + // Add two digits on each pass. + for (size_t digit_count = 0; + digit_count < length; + digit_count += 2, resolution /= ENCODING_BASE) { + size_t digit_value; + + // Do the latitude - gets the digit for this place and subtracts that + // for the next digit. + digit_value = lat / resolution; + lat -= digit_value * resolution; + buf[pos++] = alphabet[digit_value]; + + // Do the longitude - gets the digit for this place and subtracts that + // for the next digit. + digit_value = lon / resolution; + lon -= digit_value * resolution; + buf[pos++] = alphabet[digit_value]; + + // Should we add a separator here? + if (pos == SEPARATOR_POS && pos < length) { + buf[pos++] = SEPARATOR_CHAR; + } + } + while (pos < SEPARATOR_POS) { + buf[pos++] = PADDING_CHAR; + } + if (pos == SEPARATOR_POS) { + buf[pos++] = SEPARATOR_CHAR; + } + buf[pos] = '\0'; + return pos; +} + +// Encodes a location using the grid refinement method into an OLC string. The +// grid refinement method divides the area into a grid of 4x5, and uses a +// single character to refine the area. The grid squares use the OLC +// characters in order to number the squares as follows: +// +// R V W X +// J M P Q +// C F G H +// 6 7 8 9 +// 2 3 4 5 +// +// This allows default accuracy OLC codes to be refined with just a single +// character. +static int encode_grid(uolc_coord_t lat, uolc_coord_t lon, size_t length, + char *buf, size_t bufsize) +{ + if ((length + 1) >= bufsize) { + buf[0] = '\0'; + return 0; + } + + int pos = 0; + + olc_coord_t lat_grid_size = grid_size; + olc_coord_t lon_grid_size = grid_size; + + lat %= lat_grid_size; + lon %= lon_grid_size; + + for (size_t i = 0; i < length; i++) { + olc_coord_t lat_div = lat_grid_size / GRID_ROWS; + olc_coord_t lon_div = lon_grid_size / GRID_COLS; + + if (lat_div == 0 || lon_div == 0) { + // This case happens when OLC_DEG_MULTIPLIER doesn't have enough + // precision for the requested length. + break; + } + + // Work out the row and column. + size_t row = lat / lat_div; + size_t col = lon / lon_div; + lat_grid_size /= GRID_ROWS; + lon_grid_size /= GRID_COLS; + lat -= row * lat_grid_size; + lon -= col * lon_grid_size; + buf[pos++] = alphabet[row * GRID_COLS + col]; + } + buf[pos] = '\0'; + return pos; +} + +int olc_encode(olc_coord_t lat, olc_coord_t lon, size_t length, char *buf, size_t bufsize) +{ + int pos = 0; + + length = MIN(length, CODE_LEN_MAX); + + // Adjust latitude and longitude so they fall into positive ranges. + uolc_coord_t alat = adjust_latitude(lat, length) + LAT_MAX; + uolc_coord_t alon = normalize_longitude(lon) + LON_MAX; + + init_constants(); + + pos += encode_pairs(alat, alon, MIN(length, PAIR_CODE_LEN), buf + pos, bufsize - pos); + // If the requested length indicates we want grid refined codes. + if (length > PAIR_CODE_LEN) { + pos += encode_grid(alat, alon, length - PAIR_CODE_LEN, buf + pos, bufsize - pos); + } + buf[pos] = '\0'; + return pos; +} \ No newline at end of file diff --git a/src/main/common/olc.h b/src/main/common/olc.h new file mode 100644 index 00000000000..ce243eb8768 --- /dev/null +++ b/src/main/common/olc.h @@ -0,0 +1,14 @@ +#pragma once + +#include +#include + +#define OLC_DEG_MULTIPLIER ((olc_coord_t)10000000LL) // 1e7 + +typedef int32_t olc_coord_t; +typedef uint32_t uolc_coord_t; + +// olc_encodes the given coordinates in lat and lon (deg * OLC_DEG_MULTIPLIER) +// as an OLC code of the given length. It returns the number of characters +// written to buf. +int olc_encode(olc_coord_t lat, olc_coord_t lon, size_t length, char *buf, size_t bufsize); \ No newline at end of file diff --git a/src/main/common/utils.h b/src/main/common/utils.h index e3c41e9238b..a17f77d8195 100644 --- a/src/main/common/utils.h +++ b/src/main/common/utils.h @@ -28,12 +28,21 @@ #define CONCAT_HELPER(x,y) x ## y #define CONCAT(x,y) CONCAT_HELPER(x, y) +#define CONCAT4_HELPER(x, y, z, w) x ## y ## z ## w +#define CONCAT4(x, y, z, w) CONCAT4_HELPER(x, y, z, w) + #define STR_HELPER(x) #x #define STR(x) STR_HELPER(x) #define EXPAND_I(x) x #define EXPAND(x) EXPAND_I(x) +// Expand all argumens and call macro with them. When expansion of some argument contains ',', it will be passed as multiple arguments +// #define TAKE3(_1,_2,_3) CONCAT3(_1,_2,_3) +// #define MULTI2 A,B +// PP_CALL(TAKE3, MULTI2, C) expands to ABC +#define PP_CALL(macro, ...) macro(__VA_ARGS__) + #if !defined(UNUSED) #define UNUSED(x) (void)(x) #endif diff --git a/src/main/config/parameter_group_ids.h b/src/main/config/parameter_group_ids.h index dd37d7466d3..3d522e336a9 100644 --- a/src/main/config/parameter_group_ids.h +++ b/src/main/config/parameter_group_ids.h @@ -75,7 +75,8 @@ //#define PG_IBUS_TELEMETRY_CONFIG 53 #define PG_VTX_CONFIG 54 #define PG_ELERES_CONFIG 55 -#define PG_CF_END 56 +#define PG_TEMP_SENSOR_CONFIG 56 +#define PG_CF_END 57 // Driver configuration //#define PG_DRIVER_PWM_RX_CONFIG 100 @@ -101,7 +102,8 @@ #define PG_OPFLOW_CONFIG 1012 #define PG_DISPLAY_CONFIG 1013 #define PG_LIGHTS_CONFIG 1014 -#define PG_INAV_END 1014 +#define PG_PINIOBOX_CONFIG 1015 +#define PG_INAV_END 1015 // OSD configuration (subject to change) //#define PG_OSD_FONT_CONFIG 2047 diff --git a/src/main/drivers/1-wire.c b/src/main/drivers/1-wire.c new file mode 100644 index 00000000000..a0c04e634d0 --- /dev/null +++ b/src/main/drivers/1-wire.c @@ -0,0 +1,31 @@ + +#include + +#include "drivers/1-wire.h" +#include "drivers/1-wire/ds2482.h" + +#include "drivers/logging.h" + + +#ifdef USE_1WIRE + +#ifdef USE_1WIRE_DS2482 +static bool ds2482Detected = false; +static owDev_t ds2482Dev; +#endif + +void owInit(void) +{ + memset(&ds2482Dev, 0, sizeof(ds2482Dev)); + addBootlogEvent2(BOOT_EVENT_TEMP_SENSOR_DETECTION, BOOT_EVENT_FLAGS_NONE); +#ifdef USE_1WIRE_DS2482 + if (ds2482Detect(&ds2482Dev)) ds2482Detected = true; +#endif +} + +owDev_t *getOwDev(void) +{ + return ds2482Detected ? &ds2482Dev : NULL; +} + +#endif /* USE_1WIRE */ diff --git a/src/main/drivers/1-wire.h b/src/main/drivers/1-wire.h new file mode 100644 index 00000000000..5a3fc2d4fd9 --- /dev/null +++ b/src/main/drivers/1-wire.h @@ -0,0 +1,75 @@ + +#pragma once + +#include "drivers/bus.h" + +#ifdef USE_1WIRE + +#define OW_STATUS_1WB_POS 0 // 1-Wire busy +#define OW_STATUS_PPD_POS 1 // Presense-pulse detect +#define OW_STATUS_SD_POS 2 // Short detected +#define OW_STATUS_LL_POS 3 // Logic level +#define OW_STATUS_RST_POS 4 // Device reset +#define OW_STATUS_SBR_POS 5 // Single bit result +#define OW_STATUS_TSB_POS 6 // Triplet second bit +#define OW_STATUS_DIR_POS 7 // Branch direction taken + +#define OW_BUS_BUSY(status) (status & (1 << OW_STATUS_1WB_POS)) +#define OW_DEVICE_PRESENT(status) (status & (1 << OW_STATUS_PPD_POS)) // True if a device have been detected on the bus after a bus reset +#define OW_RESET(status) (status & (1 << OW_STATUS_RST_POS)) +#define OW_LOGIC_LEVEL(status) (status & (1 << OW_STATUS_LL_POS)) +#define OW_SHORT_DETECTED(status) (status & (1 << OW_STATUS_SD_POS)) +#define OW_SBR_VALUE(status) ((status >> OW_STATUS_SBR_POS) & 1) // Extract single bit read value or triplet first bit from status register value +#define OW_TSB_VALUE(status) ((status >> OW_STATUS_TSB_POS) & 1) // Extract triplet second bit value from status register value +#define OW_DIR_VALUE(status) ((status >> OW_STATUS_DIR_POS) & 1) // Extract triplet chosen direction bit value from status register value + +#define OW_TRIPLET_FIRST_BIT(tripletResult) (tripletResult & (1 << 0)) +#define OW_TRIPLET_SECOND_BIT(tripletResult) (tripletResult & (1 << 1)) +#define OW_TRIPLET_DIRECTION_BIT(tripletResult) (tripletResult & (1 << 2)) + +#define OW_SINGLE_BIT_WRITE0 0 +#define OW_SINGLE_BIT_WRITE1_READ (1<<7) + +typedef struct owDev_s { + busDevice_t *busDev; + + uint8_t status; + + bool (*reset)(struct owDev_s *owDev); + bool (*owResetCommand)(struct owDev_s *owDev); + bool (*owReset)(struct owDev_s *owDev); + bool (*waitForBus)(struct owDev_s *owDev); + bool (*readConfig)(struct owDev_s *owDev, uint8_t *config); + bool (*writeConfig)(struct owDev_s *owDev, uint8_t config); + bool (*readStatus)(struct owDev_s *owDev, uint8_t *status); + uint8_t (*getStatus)(struct owDev_s *owDev); + bool (*poll)(struct owDev_s *owDev, bool waitForBus, uint8_t *status); + bool (*owBusReady)(struct owDev_s *owDev); + + // 1-Wire ROM + bool (*owSearchRom)(struct owDev_s *owDev, uint8_t familyCode, uint64_t *romTable, uint8_t *romTableLen); + bool (*owMatchRomCommand)(struct owDev_s *owDev); + bool (*owMatchRom)(struct owDev_s *owDev, uint64_t rom); + bool (*owSkipRomCommand)(struct owDev_s *owDev); + bool (*owSkipRom)(struct owDev_s *owDev); + + // 1-Wire read/write + bool (*owWriteByteCommand)(struct owDev_s *owDev, uint8_t byte); + bool (*owWriteByte)(struct owDev_s *owDev, uint8_t byte); + bool (*owWriteBuf)(struct owDev_s *owDev, const uint8_t *buf, uint8_t len); + bool (*owReadByteCommand)(struct owDev_s *owDev); + bool (*owReadByteResult)(struct owDev_s *owDev, uint8_t *result); + bool (*owReadByte)(struct owDev_s *owDev, uint8_t *result); + bool (*owReadBuf)(struct owDev_s *owDev, uint8_t *buf, uint8_t len); + bool (*owSingleBitCommand)(struct owDev_s *owDev, uint8_t type); + bool (*owSingleBitResult)(struct owDev_s *owDev); + bool (*owSingleBit)(struct owDev_s *owDev, uint8_t type, bool *result); + bool (*owTripletCommand)(struct owDev_s *owDev, uint8_t direction); + uint8_t (*owTripletResult)(struct owDev_s *owDev); + bool (*owTriplet)(struct owDev_s *owDev, uint8_t direction, uint8_t *result); +} owDev_t; + +void owInit(void); +owDev_t *getOwDev(void); + +#endif /* defined(USE_1WIRE) && defined(USE_1WIRE_DS2482) */ diff --git a/src/main/drivers/1-wire/ds2482.c b/src/main/drivers/1-wire/ds2482.c new file mode 100644 index 00000000000..cfe10b4d5cc --- /dev/null +++ b/src/main/drivers/1-wire/ds2482.c @@ -0,0 +1,433 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include +#include + +#include "platform.h" + +#include "build/debug.h" + +#include "drivers/1-wire.h" +#include "drivers/1-wire/ds_crc.h" +#include "drivers/1-wire/ds2482.h" +#include "drivers/time.h" + + +#define DS2482_STATUS_REG_ADDR 0xF0 +#define DS2482_READ_DATA_REG_ADDR 0xE1 +#define DS2482_CONFIG_REG_ADDR 0xC3 + +#define DS2482_CONFIG_REG_APU (1<<0) // Active pull-up +#define DS2482_CONFIG_REG_SPU (1<<2) // Strong pull-up +#define DS2482_CONFIG_REG_WS (1<<3) // 1-Wire speed + +#define DS2482_STATUS_REG_1WB_POS 0 // 1-Wire busy +#define DS2482_STATUS_REG_PPD_POS 1 // Presense-pulse detect +#define DS2482_STATUS_REG_SD_POS 2 // Short detected +#define DS2482_STATUS_REG_LL_POS 3 // Logic level +#define DS2482_STATUS_REG_RST_POS 4 // Device reset +#define DS2482_STATUS_REG_SBR_POS 5 // Single bit result +#define DS2482_STATUS_REG_TSB_POS 6 // Triplet second bit +#define DS2482_STATUS_REG_DIR_POS 7 // Branch direction taken + +#define DS2482_1WIRE_BUSY(status) (status & (1 << DS2482_STATUS_REG_1WB_POS)) +#define DS2482_DEVICE_PRESENT(status) (status & (1 << DS2482_STATUS_REG_PPD_POS)) // True if a device have been detected on the bus after a bus reset +#define DS2482_RESET(status) (status & (1 << DS2482_STATUS_REG_RST_POS)) +#define DS2482_LOGIC_LEVEL(status) (status & (1 << DS2482_STATUS_REG_LL_POS)) +#define DS2482_SHORT_DETECTED(status) (status & (1 << DS2482_STATUS_REG_SD_POS)) +#define DS2482_SBR_VALUE(status) ((status >> DS2482_STATUS_REG_SBR_POS) & 1) // Extract single bit read value or triplet first bit from status register value +#define DS2482_TSB_VALUE(status) ((status >> DS2482_STATUS_REG_TSB_POS) & 1) // Extract triplet second bit value from status register value +#define DS2482_DIR_VALUE(status) ((status >> DS2482_STATUS_REG_DIR_POS) & 1) // Extract triplet chosen direction bit value from status register value + +#define DS2482_1WIRE_SINGLE_BIT_WRITE0 0 +#define DS2482_1WIRE_SINGLE_BIT_WRITE1_READ (1<<7) + +#define DS2482_CONFIG_WRITE_BYTE(config) (config | ((~config & 0xF) << 4)) // Config's upper nibble should be the one's complement of lower nibble when writing + +#define DS2482_RESET_CMD 0xF0 +#define DS2482_SET_READ_PTR_CMD 0xE1 +#define DS2482_WRITE_CONFIG_CMD 0xD2 +#define DS2482_1WIRE_RESET_CMD 0xB4 +#define DS2482_1WIRE_SINGLE_BIT_CMD 0x87 +#define DS2482_1WIRE_WRITE_BYTE_CMD 0xA5 +#define DS2482_1WIRE_READ_BYTE_CMD 0x96 +#define DS2482_1WIRE_TRIPLET_CMD 0x78 + +#define _1WIRE_SEARCH_ROM_CMD 0xF0 +#define _1WIRE_READ_ROM_CMD 0x33 +#define _1WIRE_MATCH_ROM_CMD 0x55 +#define _1WIRE_SKIP_ROM_CMD 0xCC + + +#if defined(USE_1WIRE) && defined(USE_1WIRE_DS2482) + + +static bool ds2482Reset(owDev_t *owDev) +{ + return busWrite(owDev->busDev, 0xFF, DS2482_RESET_CMD); +} + +static bool ds2482SetReadPtr(owDev_t *owDev, uint8_t reg) +{ + return busWrite(owDev->busDev, DS2482_SET_READ_PTR_CMD, reg); +} + +static bool ds2482ReadReg(owDev_t *owDev, uint8_t reg, uint8_t *byte) +{ + bool ack = ds2482SetReadPtr(owDev, reg); + if (!ack) return false; + return busRead(owDev->busDev, 0xFF, byte); +} + +static bool ds2482ReadByte(owDev_t *owDev, uint8_t *byte) +{ + return ds2482ReadReg(owDev, DS2482_READ_DATA_REG_ADDR, byte); +} + +static bool ds2482ReadConfig(owDev_t *owDev, uint8_t *config) +{ + return ds2482ReadReg(owDev, DS2482_CONFIG_REG_ADDR, config); +} + +static bool ds2482WriteConfig(owDev_t *owDev, uint8_t config) +{ + return busWrite(owDev->busDev, DS2482_WRITE_CONFIG_CMD, DS2482_CONFIG_WRITE_BYTE(config)); +} + +static bool ds2482ReadStatus(owDev_t *owDev, uint8_t *status) +{ + bool ack = ds2482ReadReg(owDev, DS2482_STATUS_REG_ADDR, &owDev->status); + if (!ack) return false; + *status = owDev->status; + return true; +} + +static uint8_t ds2482GetStatus(owDev_t *owDev) +{ + return owDev->status; +} + +static bool ds2482Poll(owDev_t *owDev, bool waitForBus, uint8_t *status) +{ + do { + bool ack = busRead(owDev->busDev, 0xFF, &owDev->status); + if (!ack) return false; + } while (waitForBus && DS2482_1WIRE_BUSY(owDev->status)); + if (status) *status = owDev->status; + return true; +} + +static bool ds2482WaitForBus(owDev_t *owDev) +{ + return ds2482Poll(owDev, true, NULL); +} + +static bool ds2482OwBusReady(owDev_t *owDev) +{ + bool ack = busRead(owDev->busDev, 0xFF, &owDev->status); + if (!ack) return false; + return !DS2482_1WIRE_BUSY(owDev->status); +} + +static bool ds2482OwResetCommand(owDev_t *owDev) +{ + return busWrite(owDev->busDev, 0xFF, DS2482_1WIRE_RESET_CMD); +} + +static bool ds2482OwReset(owDev_t *owDev) +{ + bool ack = ds2482OwResetCommand(owDev); + if (!ack) return false; + return ds2482WaitForBus(owDev); +} + +static bool ds2482OwWriteByteCommand(owDev_t *owDev, uint8_t byte) +{ + return busWrite(owDev->busDev, DS2482_1WIRE_WRITE_BYTE_CMD, byte); +} + +static bool ds2482OwWriteByte(owDev_t *owDev, uint8_t byte) +{ + bool ack = ds2482OwWriteByteCommand(owDev, byte); + if (!ack) return false; + return ds2482WaitForBus(owDev); +} + +static bool ds2482OwWriteBuf(owDev_t *owDev, const uint8_t *buf, uint8_t len) +{ + for (uint8_t index = 0; index < len; ++index) { + bool ack = ds2482OwWriteByte(owDev, buf[index]); + if (!ack) return false; + } + return true; +} + +static bool ds2482OwReadByteCommand(owDev_t *owDev) +{ + return busWrite(owDev->busDev, 0xFF, DS2482_1WIRE_READ_BYTE_CMD); +} + +static bool ds2482OwReadByte(owDev_t *owDev, uint8_t *result) +{ + bool ack = ds2482OwReadByteCommand(owDev); + if (!ack) return false; + + ack = ds2482WaitForBus(owDev); + if (!ack) return false; + + return ds2482ReadByte(owDev, result); +} + +static bool ds2482OwReadBuf(owDev_t *owDev, uint8_t *buf, uint8_t len) +{ + for (uint8_t index = 0; index < len; ++index) { + bool ack = ds2482OwReadByte(owDev, buf + index); + if (!ack) return false; + } + return true; +} + +static bool ds2482OwSingleBitCommand(owDev_t *owDev, uint8_t type) +{ + return busWrite(owDev->busDev, DS2482_1WIRE_SINGLE_BIT_CMD, type); +} + +static bool ds2482OwSingleBitResult(owDev_t *owDev) +{ + return DS2482_SBR_VALUE(owDev->status); +} + +static bool ds2482OwSingleBit(owDev_t *owDev, uint8_t type, bool *result) +{ + bool ack = ds2482OwSingleBitCommand(owDev, type); + + ack = ds2482WaitForBus(owDev); + if (!ack) return false; + + if (result) *result = ds2482OwSingleBitResult(owDev); + return true; +} + +static bool ds2482OwTripletCommand(owDev_t *owDev, uint8_t direction) +{ + return busWrite(owDev->busDev, DS2482_1WIRE_TRIPLET_CMD, direction << 7); +} + +static uint8_t ds2482OwTripletResult(owDev_t *owDev) +{ + return owDev->status >> 5; +} + +static bool ds2482OwTriplet(owDev_t *owDev, uint8_t direction, uint8_t *result) +{ + bool ack = ds2482OwTripletCommand(owDev, direction << 7); + if (!ack) return false; + ack = ds2482Poll(owDev, true, NULL); + if (!ack) return false; + *result = ds2482OwTripletResult(owDev); + return true; +} + +static bool ds2482OwSkipRomCommand(owDev_t *owDev) +{ + return ds2482OwWriteByte(owDev, _1WIRE_SKIP_ROM_CMD); +} + +static bool ds2482OwSkipRom(owDev_t *owDev) +{ + bool ack = ds2482OwReset(owDev); + if (!ack) return false; + + ack = ds2482OwSkipRomCommand(owDev); + if (!ack) return false; + + return ds2482WaitForBus(owDev); +} + +static bool ds2482OwMatchRomCommand(owDev_t *owDev) +{ + return ds2482OwWriteByteCommand(owDev, _1WIRE_MATCH_ROM_CMD); +} + +static bool ds2482OwMatchRom(owDev_t *owDev, uint64_t rom) +{ + bool ack = ds2482OwReset(owDev); + if (!ack) return false; + + ack = ds2482OwMatchRomCommand(owDev); + if (!ack) return false; + + ack = ds2482WaitForBus(owDev); + if (!ack) return false; + + for (uint8_t romByteIndex = 0; romByteIndex < 8; ++romByteIndex) { + ack = ds2482OwWriteByte(owDev, ((uint8_t *)&rom)[romByteIndex]); + if (!ack) return false; + } + + return true; +} + +static bool ds2482OwSearchRom(owDev_t *owDev, uint8_t family_code, uint64_t *rom_table, uint8_t *rom_table_len) +{ + bool ack; + uint8_t last_collision_index = 0, rom_index = 0; + + do { + + uint8_t rom_byte_index = 0, rom_bit_index = 1, rom_byte_mask = 1; + uint8_t dir_zero_last_index = 0; // Bit index where the 0 direction has been chosen after collision + uint8_t *rom = (uint8_t *)&rom_table[rom_index]; + + ack = ds2482OwReset(owDev); + if (!ack) goto ds2482SearchRomReturn; + + ack = ds2482Poll(owDev, true, NULL); + if (!ack) goto ds2482SearchRomReturn; + + if (!DS2482_DEVICE_PRESENT(owDev->status)) + goto ds2482SearchRomReturn; + + ack = ds2482OwWriteByte(owDev, _1WIRE_SEARCH_ROM_CMD); + if (!ack) goto ds2482SearchRomReturn; + + do { + + uint8_t direction; + if (family_code && (rom_bit_index < 9)) { + direction = (family_code >> (rom_bit_index - 1)) & 1; + } else if ((rom_index > 0) && (rom_bit_index < last_collision_index)) { + const uint8_t *previous_rom = (uint8_t *)&rom_table[rom_index-1]; + direction = (previous_rom[rom_byte_index] & rom_byte_mask) > 0; + } else { + direction = rom_bit_index == last_collision_index; + } + + ack = ds2482OwTripletCommand(owDev, direction); + if (!ack) goto ds2482SearchRomReturn; + + ack = ds2482Poll(owDev, true, NULL); + if (!ack) goto ds2482SearchRomReturn; + + uint8_t triplet_sbr = DS2482_SBR_VALUE(owDev->status); + uint8_t triplet_tsb = DS2482_TSB_VALUE(owDev->status); + uint8_t triplet_dir = DS2482_DIR_VALUE(owDev->status); + + if (triplet_sbr && triplet_tsb) break; // Error, the device have been disconnected during the search, restart + + if (family_code && (rom_bit_index < 9) && (triplet_dir != direction)) + goto ds2482SearchRomReturn; + + if (triplet_dir) + rom[rom_byte_index] |= rom_byte_mask; + else { + if (!(triplet_sbr || triplet_tsb)) dir_zero_last_index = rom_bit_index; // Collision + rom[rom_byte_index] &= ~rom_byte_mask; + } + + rom_bit_index += 1; + + if (!(rom_byte_mask <<= 1)) { + rom_byte_index += 1; + rom_byte_mask = 1; + } + + } while (rom_byte_index < 8); + + if ((rom_bit_index > 64) && (rom[7] == ds_crc8(rom, 7))) { + rom_index += 1; + last_collision_index = dir_zero_last_index; + if (!last_collision_index) break; // All the devices have been found + } + + } while (rom_index < *rom_table_len); + +ds2482SearchRomReturn: + + *rom_table_len = rom_index; + + return ack; + +} + + +static bool ds2482Init(owDev_t *owDev) +{ + return ds2482WriteConfig(owDev, DS2482_CONFIG_REG_APU); +} + +#define DETECTION_MAX_RETRY_COUNT 5 +static bool deviceDetect(owDev_t *owDev) +{ + for (int retryCount = 0; retryCount < DETECTION_MAX_RETRY_COUNT; retryCount++) { + delay(10); + if (ds2482Reset(owDev)) return true; + } + + return false; +} + +bool ds2482Detect(owDev_t *owDev) +{ + owDev->busDev = busDeviceInit(BUSTYPE_I2C, DEVHW_DS2482, 0, OWNER_1WIRE); + if (owDev->busDev == NULL) { + return false; + } + + if (!deviceDetect(owDev)) { + busDeviceDeInit(owDev->busDev); + return false; + } + + ds2482Init(owDev); + + owDev->reset = ds2482Reset; + owDev->owResetCommand = ds2482OwResetCommand; + owDev->owReset = ds2482OwReset; + owDev->waitForBus = ds2482WaitForBus; + owDev->readConfig = ds2482ReadConfig; + owDev->writeConfig = ds2482WriteConfig; + owDev->readStatus = ds2482ReadStatus; + owDev->getStatus = ds2482GetStatus; + owDev->poll = ds2482Poll; + owDev->owBusReady = ds2482OwBusReady; + + owDev->owSearchRom = ds2482OwSearchRom; + owDev->owMatchRomCommand = ds2482OwMatchRomCommand; + owDev->owMatchRom = ds2482OwMatchRom; + owDev->owSkipRomCommand = ds2482OwSkipRomCommand; + owDev->owSkipRom = ds2482OwSkipRom; + + owDev->owWriteByteCommand = ds2482OwWriteByteCommand; + owDev->owWriteByte = ds2482OwWriteByte; + owDev->owWriteBuf = ds2482OwWriteBuf; + owDev->owReadByteCommand = ds2482OwReadByteCommand; + owDev->owReadByteResult = ds2482ReadByte; + owDev->owReadByte = ds2482OwReadByte; + owDev->owReadBuf = ds2482OwReadBuf; + owDev->owSingleBitCommand = ds2482OwSingleBitCommand; + owDev->owSingleBitResult = ds2482OwSingleBitResult; + owDev->owSingleBit = ds2482OwSingleBit; + owDev->owTripletCommand = ds2482OwTripletCommand; + owDev->owTripletResult = ds2482OwTripletResult; + owDev->owTriplet = ds2482OwTriplet; + + return true; +} + +#endif /* defined(USE_1WIRE) && defined(USE_1WIRE_DS2482) */ diff --git a/src/main/drivers/1-wire/ds2482.h b/src/main/drivers/1-wire/ds2482.h new file mode 100644 index 00000000000..ad4d6177b2a --- /dev/null +++ b/src/main/drivers/1-wire/ds2482.h @@ -0,0 +1,28 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once + +//#include "drivers/io_types.h" +#include +#include "drivers/1-wire.h" + +#if defined(USE_1WIRE) && defined(USE_1WIRE_DS2482) + +bool ds2482Detect(owDev_t *owDev); + +#endif /* defined(USE_1WIRE) && defined(USE_1WIRE_DS2482) */ diff --git a/src/main/drivers/1-wire/ds_crc.c b/src/main/drivers/1-wire/ds_crc.c new file mode 100644 index 00000000000..8332ab0a5fa --- /dev/null +++ b/src/main/drivers/1-wire/ds_crc.c @@ -0,0 +1,28 @@ + +#include + +uint8_t ds_crc8_bit_update(uint8_t crc8, uint8_t bit) +{ + uint8_t crc8_base = (crc8 >> 1) & 0x73; + uint8_t xor1 = (crc8 & 1) ^ bit; + return crc8_base | ((((crc8 >> 3) & 1) ^ xor1) << 2) | ((((crc8 >> 4) & 1) ^ xor1) << 3) | (xor1 << 7); +} + +uint8_t ds_crc8_byte_update(uint8_t crc8, uint8_t byte) +{ + for (uint8_t i = 0; i < 8; ++i) { + uint8_t mix = (crc8 ^ byte) & 0x01; + crc8 >>= 1; + if (mix) crc8 ^= 0x8C; + byte >>= 1; + } + return crc8; +} + +uint8_t ds_crc8(const uint8_t *addr, uint8_t len) +{ + uint8_t crc8 = 0; + for (uint8_t i=0; i < len; ++i) crc8 = ds_crc8_byte_update(crc8, addr[i]); + return crc8; +} + diff --git a/src/main/drivers/1-wire/ds_crc.h b/src/main/drivers/1-wire/ds_crc.h new file mode 100644 index 00000000000..839a3e1e107 --- /dev/null +++ b/src/main/drivers/1-wire/ds_crc.h @@ -0,0 +1,8 @@ + +#pragma once + +#include + +uint8_t ds_crc8_bit_update(uint8_t crc, uint8_t bit); +uint8_t ds_crc8_byte_update(uint8_t crc, uint8_t byte); +uint8_t ds_crc8(const uint8_t *addr, uint8_t len); diff --git a/src/main/drivers/accgyro/accgyro_icm20689.c b/src/main/drivers/accgyro/accgyro_icm20689.c new file mode 100644 index 00000000000..4766e5aa946 --- /dev/null +++ b/src/main/drivers/accgyro/accgyro_icm20689.c @@ -0,0 +1,149 @@ +/* + * This file is part of iNavFlight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software under the terms of the + * GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) + * any later version. + * + * Cleanflight and Betaflight are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +#include +#include +#include + +#include "platform.h" +#include "build/debug.h" + +#include "common/axis.h" +#include "common/maths.h" + +#include "drivers/system.h" +#include "drivers/time.h" +#include "drivers/io.h" +#include "drivers/exti.h" +#include "drivers/bus.h" + +#include "drivers/accgyro/accgyro.h" +#include "drivers/accgyro/accgyro_mpu.h" +#include "drivers/accgyro/accgyro_icm20689.h" + +#if (defined(USE_GYRO_ICM20689) || defined(USE_ACC_ICM20689)) + +static uint8_t icm20689DeviceDetect(const busDevice_t *busDev) +{ + busSetSpeed(busDev, BUS_SPEED_INITIALIZATION); + + busWrite(busDev, MPU_RA_PWR_MGMT_1, ICM20689_BIT_RESET); + + uint8_t attemptsRemaining = 20; + uint8_t in; + do { + delay(150); + busRead(busDev, MPU_RA_WHO_AM_I, &in); + switch (in) { + case ICM20601_WHO_AM_I_CONST: + case ICM20602_WHO_AM_I_CONST: + case ICM20608G_WHO_AM_I_CONST: + case ICM20689_WHO_AM_I_CONST: + return true; + } + } while (attemptsRemaining--); + + return false; +} + +static void icm20689AccInit(accDev_t *acc) +{ + acc->acc_1G = 512 * 4; +} + +bool icm20689AccDetect(accDev_t *acc) +{ + acc->busDev = busDeviceOpen(BUSTYPE_ANY, DEVHW_ICM20689, acc->imuSensorToUse); + if (acc->busDev == NULL) { + return false; + } + + mpuContextData_t * ctx = busDeviceGetScratchpadMemory(acc->busDev); + if (ctx->chipMagicNumber != 0x50D1) { + return false; + } + + acc->initFn = icm20689AccInit; + acc->readFn = mpuAccReadScratchpad; + + return true; +} + +static void icm20689AccAndGyroInit(gyroDev_t *gyro) +{ + busDevice_t * busDev = gyro->busDev; + const gyroFilterAndRateConfig_t * config = mpuChooseGyroConfig(gyro->lpf, 1000000 / gyro->requestedSampleIntervalUs); + gyro->sampleRateIntervalUs = 1000000 / config->gyroRateHz; + + gyroIntExtiInit(gyro); + + busSetSpeed(busDev, BUS_SPEED_INITIALIZATION); + + busWrite(busDev, MPU_RA_PWR_MGMT_1, ICM20689_BIT_RESET); + delay(100); + busWrite(busDev, MPU_RA_SIGNAL_PATH_RESET, 0x03); + delay(100); + busWrite(busDev, MPU_RA_PWR_MGMT_1, INV_CLK_PLL); + delay(15); + busWrite(busDev, MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3); // XXX + delay(15); + busWrite(busDev, MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3); + delay(15); + busWrite(busDev, MPU_RA_CONFIG, config->gyroConfigValues[0]); + delay(15); + busWrite(busDev, MPU_RA_SMPLRT_DIV, config->gyroConfigValues[1]); // Get Divider Drops + delay(100); + + // Data ready interrupt configuration + busWrite(busDev, MPU_RA_INT_PIN_CFG, 0x10); // INT_ANYRD_2CLEAR, BYPASS_EN + + delay(15); + +#ifdef USE_MPU_DATA_READY_SIGNAL + busWrite(busDev, MPU_RA_INT_ENABLE, 0x01); // RAW_RDY_EN interrupt enable +#endif +} + +bool icm20689GyroDetect(gyroDev_t *gyro) +{ + gyro->busDev = busDeviceInit(BUSTYPE_ANY, DEVHW_ICM20689, gyro->imuSensorToUse, OWNER_MPU); + if (gyro->busDev == NULL) { + return false; + } + + if (!icm20689DeviceDetect(gyro->busDev)) { + busDeviceDeInit(gyro->busDev); + return false; + } + + // Magic number for ACC detection to indicate that we have detected MPU6000 gyro + mpuContextData_t * ctx = busDeviceGetScratchpadMemory(gyro->busDev); + ctx->chipMagicNumber = 0x50D1; + + gyro->initFn = icm20689AccAndGyroInit; + gyro->readFn = mpuGyroReadScratchpad; + gyro->intStatusFn = gyroCheckDataReady; + gyro->temperatureFn = mpuTemperatureReadScratchpad; + gyro->scale = 1.0f / 16.4f; // 16.4 dps/lsb scalefactor + + return true; +} + +#endif diff --git a/src/main/drivers/accgyro/accgyro_icm20689.h b/src/main/drivers/accgyro/accgyro_icm20689.h new file mode 100644 index 00000000000..bf37b1d4ad2 --- /dev/null +++ b/src/main/drivers/accgyro/accgyro_icm20689.h @@ -0,0 +1,32 @@ +/* + * This file is part of iNavFlight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software under the terms of the + * GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) + * any later version. + * + * Cleanflight and Betaflight are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +#pragma once + +#include "drivers/bus.h" + +#define ICM20689_BIT_RESET (0x80) + +#if (defined(USE_GYRO_ICM20689) || defined(USE_ACC_ICM20689)) + +bool icm20689AccDetect(accDev_t *acc); +bool icm20689GyroDetect(gyroDev_t *gyro); + +#endif diff --git a/src/main/drivers/adc.h b/src/main/drivers/adc.h index 90d78258438..5ba4b999da0 100644 --- a/src/main/drivers/adc.h +++ b/src/main/drivers/adc.h @@ -46,6 +46,10 @@ uint16_t adcGetChannel(uint8_t channel); bool adcIsFunctionAssigned(uint8_t function); int adcGetFunctionChannelAllocation(uint8_t function); -#if !defined(USE_ADC_AVERAGING) -#define ADC_AVERAGE_N_SAMPLES 1 +#if defined(USE_ADC_AVERAGING) +#if !defined(ADC_AVERAGE_N_SAMPLES) +#define ADC_AVERAGE_N_SAMPLES 20 +#endif +#else +#define ADC_AVERAGE_N_SAMPLES 1 #endif diff --git a/src/main/drivers/adc_stm32f4xx.c b/src/main/drivers/adc_stm32f4xx.c index 04d045d7d84..f980d16cd9b 100644 --- a/src/main/drivers/adc_stm32f4xx.c +++ b/src/main/drivers/adc_stm32f4xx.c @@ -33,7 +33,7 @@ #include "adc_impl.h" #if !defined(ADC1_DMA_STREAM) -#define ADC1_DMA_STREAM DMA2_Stream4 +#define ADC1_DMA_STREAM DMA2_Stream0 #endif static adcDevice_t adcHardware[ADCDEV_COUNT] = { diff --git a/src/main/drivers/adc_stm32f7xx.c b/src/main/drivers/adc_stm32f7xx.c index 0b9be5758a8..4b5aae88384 100644 --- a/src/main/drivers/adc_stm32f7xx.c +++ b/src/main/drivers/adc_stm32f7xx.c @@ -34,7 +34,7 @@ #include "adc_impl.h" static adcDevice_t adcHardware[ADCDEV_COUNT] = { - { .ADCx = ADC1, .rccADC = RCC_APB2(ADC1), .rccDMA = RCC_AHB1(DMA2), .DMAy_Streamx = DMA2_Stream4, .channel = DMA_CHANNEL_0, .enabled = false, .usedChannelCount = 0 }, + { .ADCx = ADC1, .rccADC = RCC_APB2(ADC1), .rccDMA = RCC_AHB1(DMA2), .DMAy_Streamx = DMA2_Stream0, .channel = DMA_CHANNEL_0, .enabled = false, .usedChannelCount = 0 }, //{ .ADCx = ADC2, .rccADC = RCC_APB2(ADC2), .rccDMA = RCC_AHB1(DMA2), .DMAy_Streamx = DMA2_Stream1, .channel = DMA_Channel_0, .enabled = false, .usedChannelCount = 0 } }; @@ -110,7 +110,7 @@ static void adcInstanceInit(ADCDevice adcDevice) adc->DmaHandle.Init.Channel = adc->channel; adc->DmaHandle.Init.Direction = DMA_PERIPH_TO_MEMORY; adc->DmaHandle.Init.PeriphInc = DMA_PINC_DISABLE; - adc->DmaHandle.Init.MemInc = adc->usedChannelCount > 1 ? DMA_MINC_ENABLE : DMA_MINC_DISABLE; + adc->DmaHandle.Init.MemInc = ((adc->usedChannelCount > 1) || (ADC_AVERAGE_N_SAMPLES > 1)) ? DMA_MINC_ENABLE : DMA_MINC_DISABLE; adc->DmaHandle.Init.PeriphDataAlignment = DMA_PDATAALIGN_HALFWORD; adc->DmaHandle.Init.MemDataAlignment = DMA_MDATAALIGN_HALFWORD; adc->DmaHandle.Init.Mode = DMA_CIRCULAR; @@ -152,7 +152,7 @@ static void adcInstanceInit(ADCDevice adcDevice) //HAL_CLEANINVALIDATECACHE((uint32_t*)&adcValues[adcDevice], configuredAdcChannels); /*##-4- Start the conversion process #######################################*/ - if (HAL_ADC_Start_DMA(&adc->ADCHandle, (uint32_t*)&adcValues[adcDevice], adc->usedChannelCount) != HAL_OK) + if (HAL_ADC_Start_DMA(&adc->ADCHandle, (uint32_t*)&adcValues[adcDevice], adc->usedChannelCount * ADC_AVERAGE_N_SAMPLES) != HAL_OK) { /* Start Conversation Error */ } diff --git a/src/main/drivers/bus.c b/src/main/drivers/bus.c index b4eac1544ae..d1ec5de353d 100755 --- a/src/main/drivers/bus.c +++ b/src/main/drivers/bus.c @@ -47,6 +47,7 @@ static void busDevPreInit_SPI(const busDeviceDescriptor_t * descriptor) static void busDevPreInit(const busDeviceDescriptor_t * descriptor) { switch (descriptor->busType) { + default: case BUSTYPE_NONE: break; @@ -121,6 +122,7 @@ busDevice_t * busDeviceInit(busType_e bus, devHardwareType_e hw, uint8_t tag, re if (dev) { switch (descriptor->busType) { + default: case BUSTYPE_NONE: return NULL; @@ -180,6 +182,7 @@ void busSetSpeed(const busDevice_t * dev, busSpeed_e speed) UNUSED(speed); switch (dev->busType) { + default: case BUSTYPE_NONE: // Not available break; @@ -212,16 +215,7 @@ void * busDeviceGetScratchpadMemory(const busDevice_t * dev) bool busTransfer(const busDevice_t * dev, uint8_t * rxBuf, const uint8_t * txBuf, int length) { #ifdef USE_SPI - // busTransfer function is only supported on SPI bus - if (dev->busType == BUSTYPE_SPI) { - busTransferDescriptor_t dsc = { - .rxBuf = rxBuf, - .txBuf = txBuf, - .length = length - }; - - return spiBusTransferMultiple(dev, &dsc, 1); - } + return spiBusTransfer(dev, rxBuf, txBuf, length); #else UNUSED(dev); UNUSED(rxBuf); @@ -355,3 +349,45 @@ bool busRead(const busDevice_t * dev, uint8_t reg, uint8_t * data) return false; } } + +void busSelectDevice(const busDevice_t * dev) +{ +#ifdef USE_SPI + if (dev->busType == BUSTYPE_SPI && (dev->flags & DEVFLAGS_USE_MANUAL_DEVICE_SELECT)) { + spiBusSelectDevice(dev); + } +#else + UNUSED(dev); +#endif +} + +void busDeselectDevice(const busDevice_t * dev) +{ +#ifdef USE_SPI + if (dev->busType == BUSTYPE_SPI && (dev->flags & DEVFLAGS_USE_MANUAL_DEVICE_SELECT)) { + spiBusDeselectDevice(dev); + } +#else + UNUSED(dev); +#endif +} + +bool busIsBusy(const busDevice_t * dev) +{ + switch (dev->busType) { + case BUSTYPE_SPI: +#ifdef USE_SPI + return spiBusIsBusy(dev); +#else + UNUSED(dev); + return false; +#endif + + case BUSTYPE_I2C: + // Not implemented for I2C, respond as always free + return false; + + default: + return false; + } +} diff --git a/src/main/drivers/bus.h b/src/main/drivers/bus.h index c171323bc25..a2f40d0822f 100755 --- a/src/main/drivers/bus.h +++ b/src/main/drivers/bus.h @@ -59,9 +59,17 @@ typedef enum { BUSTYPE_ANY = 0, BUSTYPE_NONE = 0, BUSTYPE_I2C = 1, - BUSTYPE_SPI = 2 + BUSTYPE_SPI = 2, + BUSTYPE_SDIO = 3, } busType_e; +typedef enum { + BUSINDEX_1 = 0, + BUSINDEX_2 = 1, + BUSINDEX_3 = 2, + BUSINDEX_4 = 3 +} busIndex_e; + /* Ultimately all hardware descriptors will go to target definition files. * Driver code will merely query for it's HW descriptor and initialize it */ typedef enum { @@ -83,6 +91,7 @@ typedef enum { DEVHW_MPU6050, DEVHW_MPU6500, DEVHW_BMI160, + DEVHW_ICM20689, /* Combined ACC/GYRO/MAG chips */ DEVHW_MPU9250, @@ -104,6 +113,19 @@ typedef enum { DEVHW_MAG3110, DEVHW_LIS3MDL, + /* Temp sensor chips */ + DEVHW_LM75_0, + DEVHW_LM75_1, + DEVHW_LM75_2, + DEVHW_LM75_3, + DEVHW_LM75_4, + DEVHW_LM75_5, + DEVHW_LM75_6, + DEVHW_LM75_7, + + /* 1-wire interface chips */ + DEVHW_DS2482, + /* OSD chips */ DEVHW_MAX7456, @@ -117,11 +139,14 @@ typedef enum { DEVHW_PCA9685, // PWM output device DEVHW_M25P16, // SPI NOR flash DEVHW_UG2864, // I2C OLED display + DEVHW_SDCARD, // Generic SD-Card } devHardwareType_e; typedef enum { DEVFLAGS_NONE = 0, - DEVFLAGS_USE_RAW_REGISTERS = (1 << 0), // Don't manipulate MSB for R/W selection + DEVFLAGS_USE_RAW_REGISTERS = (1 << 0), // Don't manipulate MSB for R/W selection (SPI), allow using 0xFF register to raw i2c reads/writes + DEVFLAGS_USE_MANUAL_DEVICE_SELECT = (1 << 1), // (SPI only) Don't automatically select/deselect device + DEVFLAGS_SPI_MODE_0 = (1 << 2), // (SPI only) Use CPOL=0/CPHA=0 (if unset MODE3 is used - CPOL=1/CPHA=1) } deviceFlags_e; typedef struct busDeviceDescriptor_s { @@ -243,13 +268,16 @@ bool i2cBusWriteRegister(const busDevice_t * dev, uint8_t reg, uint8_t data); bool i2cBusReadBuffer(const busDevice_t * dev, uint8_t reg, uint8_t * data, uint8_t length); bool i2cBusReadRegister(const busDevice_t * dev, uint8_t reg, uint8_t * data); - +bool spiBusIsBusy(const busDevice_t * dev); void spiBusSetSpeed(const busDevice_t * dev, busSpeed_e speed); +bool spiBusTransfer(const busDevice_t * dev, uint8_t * rxBuf, const uint8_t * txBuf, int length); bool spiBusTransferMultiple(const busDevice_t * dev, busTransferDescriptor_t * dsc, int count); bool spiBusWriteBuffer(const busDevice_t * dev, uint8_t reg, const uint8_t * data, uint8_t length); bool spiBusWriteRegister(const busDevice_t * dev, uint8_t reg, uint8_t data); bool spiBusReadBuffer(const busDevice_t * dev, uint8_t reg, uint8_t * data, uint8_t length); bool spiBusReadRegister(const busDevice_t * dev, uint8_t reg, uint8_t * data); +void spiBusSelectDevice(const busDevice_t * dev); +void spiBusDeselectDevice(const busDevice_t * dev); /* Pre-initialize all known device descriptors to make sure hardware state is consistent and known * Initialize bus hardware */ @@ -266,6 +294,10 @@ void * busDeviceGetScratchpadMemory(const busDevice_t * dev); void busSetSpeed(const busDevice_t * dev, busSpeed_e speed); +/* Select/Deselect device will allow code to do something during device transfer or do transfer in chunks over some time */ +void busSelectDevice(const busDevice_t * dev); +void busDeselectDevice(const busDevice_t * dev); + bool busWriteBuf(const busDevice_t * busdev, uint8_t reg, const uint8_t * data, uint8_t length); bool busReadBuf(const busDevice_t * busdev, uint8_t reg, uint8_t * data, uint8_t length); bool busRead(const busDevice_t * busdev, uint8_t reg, uint8_t * data); @@ -273,3 +305,5 @@ bool busWrite(const busDevice_t * busdev, uint8_t reg, uint8_t data); bool busTransfer(const busDevice_t * dev, uint8_t * rxBuf, const uint8_t * txBuf, int length); bool busTransferMultiple(const busDevice_t * dev, busTransferDescriptor_t * buffers, int count); + +bool busIsBusy(const busDevice_t * dev); diff --git a/src/main/drivers/bus_busdev_i2c.c b/src/main/drivers/bus_busdev_i2c.c index 3547158050c..96c2cd3bd08 100755 --- a/src/main/drivers/bus_busdev_i2c.c +++ b/src/main/drivers/bus_busdev_i2c.c @@ -28,21 +28,25 @@ bool i2cBusWriteBuffer(const busDevice_t * dev, uint8_t reg, const uint8_t * data, uint8_t length) { - return i2cWriteBuffer(dev->busdev.i2c.i2cBus, dev->busdev.i2c.address, reg, length, data); + const bool allowRawAccess = (dev->flags & DEVFLAGS_USE_RAW_REGISTERS); + return i2cWriteBuffer(dev->busdev.i2c.i2cBus, dev->busdev.i2c.address, reg, length, data, allowRawAccess); } bool i2cBusWriteRegister(const busDevice_t * dev, uint8_t reg, uint8_t data) { - return i2cWrite(dev->busdev.i2c.i2cBus, dev->busdev.i2c.address, reg, data); + const bool allowRawAccess = (dev->flags & DEVFLAGS_USE_RAW_REGISTERS); + return i2cWrite(dev->busdev.i2c.i2cBus, dev->busdev.i2c.address, reg, data, allowRawAccess); } bool i2cBusReadBuffer(const busDevice_t * dev, uint8_t reg, uint8_t * data, uint8_t length) { - return i2cRead(dev->busdev.i2c.i2cBus, dev->busdev.i2c.address, reg, length, data); + const bool allowRawAccess = (dev->flags & DEVFLAGS_USE_RAW_REGISTERS); + return i2cRead(dev->busdev.i2c.i2cBus, dev->busdev.i2c.address, reg, length, data, allowRawAccess); } bool i2cBusReadRegister(const busDevice_t * dev, uint8_t reg, uint8_t * data) { - return i2cRead(dev->busdev.i2c.i2cBus, dev->busdev.i2c.address, reg, 1, data); + const bool allowRawAccess = (dev->flags & DEVFLAGS_USE_RAW_REGISTERS); + return i2cRead(dev->busdev.i2c.i2cBus, dev->busdev.i2c.address, reg, 1, data, allowRawAccess); } #endif diff --git a/src/main/drivers/bus_busdev_spi.c b/src/main/drivers/bus_busdev_spi.c index 6e9670e8a56..b5616bdd998 100755 --- a/src/main/drivers/bus_busdev_spi.c +++ b/src/main/drivers/bus_busdev_spi.c @@ -29,33 +29,68 @@ #include "drivers/bus_spi.h" #include "drivers/time.h" +void spiBusSelectDevice(const busDevice_t * dev) +{ + IOLo(dev->busdev.spi.csnPin); + __NOP(); +} + +void spiBusDeselectDevice(const busDevice_t * dev) +{ + __NOP(); + IOHi(dev->busdev.spi.csnPin); +} + void spiBusSetSpeed(const busDevice_t * dev, busSpeed_e speed) { const SPIClockSpeed_e spiClock[] = { SPI_CLOCK_INITIALIZATON, SPI_CLOCK_SLOW, SPI_CLOCK_STANDARD, SPI_CLOCK_FAST, SPI_CLOCK_ULTRAFAST }; SPI_TypeDef * instance = spiInstanceByDevice(dev->busdev.spi.spiBus); #ifdef BUS_SPI_SPEED_MAX - if (speed > BUS_SPI_SPEED_MAX) - speed = BUS_SPI_SPEED_MAX; + if (speed > BUS_SPI_SPEED_MAX) + speed = BUS_SPI_SPEED_MAX; #endif spiSetSpeed(instance, spiClock[speed]); } +bool spiBusTransfer(const busDevice_t * dev, uint8_t * rxBuf, const uint8_t * txBuf, int length) +{ + SPI_TypeDef * instance = spiInstanceByDevice(dev->busdev.spi.spiBus); + + if (!(dev->flags & DEVFLAGS_USE_MANUAL_DEVICE_SELECT)) { + IOLo(dev->busdev.spi.csnPin); + __NOP(); + } + + spiTransfer(instance, rxBuf, txBuf, length); + + if (!(dev->flags & DEVFLAGS_USE_MANUAL_DEVICE_SELECT)) { + __NOP(); + IOHi(dev->busdev.spi.csnPin); + } + + return true; +} + bool spiBusTransferMultiple(const busDevice_t * dev, busTransferDescriptor_t * dsc, int count) { SPI_TypeDef * instance = spiInstanceByDevice(dev->busdev.spi.spiBus); - IOLo(dev->busdev.spi.csnPin); - __NOP(); + if (!(dev->flags & DEVFLAGS_USE_MANUAL_DEVICE_SELECT)) { + IOLo(dev->busdev.spi.csnPin); + __NOP(); + } for (int n = 0; n < count; n++) { spiTransfer(instance, dsc[n].rxBuf, dsc[n].txBuf, dsc[n].length); } - __NOP(); - IOHi(dev->busdev.spi.csnPin); + if (!(dev->flags & DEVFLAGS_USE_MANUAL_DEVICE_SELECT)) { + __NOP(); + IOHi(dev->busdev.spi.csnPin); + } return true; } @@ -64,12 +99,18 @@ bool spiBusWriteRegister(const busDevice_t * dev, uint8_t reg, uint8_t data) { SPI_TypeDef * instance = spiInstanceByDevice(dev->busdev.spi.spiBus); - IOLo(dev->busdev.spi.csnPin); - delayMicroseconds(1); + if (!(dev->flags & DEVFLAGS_USE_MANUAL_DEVICE_SELECT)) { + IOLo(dev->busdev.spi.csnPin); + delayMicroseconds(1); + } + spiTransferByte(instance, reg); spiTransferByte(instance, data); - IOHi(dev->busdev.spi.csnPin); - delayMicroseconds(1); + + if (!(dev->flags & DEVFLAGS_USE_MANUAL_DEVICE_SELECT)) { + delayMicroseconds(1); + IOHi(dev->busdev.spi.csnPin); + } return true; } @@ -78,10 +119,16 @@ bool spiBusWriteBuffer(const busDevice_t * dev, uint8_t reg, const uint8_t * dat { SPI_TypeDef * instance = spiInstanceByDevice(dev->busdev.spi.spiBus); - IOLo(dev->busdev.spi.csnPin); + if (!(dev->flags & DEVFLAGS_USE_MANUAL_DEVICE_SELECT)) { + IOLo(dev->busdev.spi.csnPin); + } + spiTransferByte(instance, reg); spiTransfer(instance, NULL, data, length); - IOHi(dev->busdev.spi.csnPin); + + if (!(dev->flags & DEVFLAGS_USE_MANUAL_DEVICE_SELECT)) { + IOHi(dev->busdev.spi.csnPin); + } return true; } @@ -90,10 +137,16 @@ bool spiBusReadBuffer(const busDevice_t * dev, uint8_t reg, uint8_t * data, uint { SPI_TypeDef * instance = spiInstanceByDevice(dev->busdev.spi.spiBus); - IOLo(dev->busdev.spi.csnPin); + if (!(dev->flags & DEVFLAGS_USE_MANUAL_DEVICE_SELECT)) { + IOLo(dev->busdev.spi.csnPin); + } + spiTransferByte(instance, reg); spiTransfer(instance, data, NULL, length); - IOHi(dev->busdev.spi.csnPin); + + if (!(dev->flags & DEVFLAGS_USE_MANUAL_DEVICE_SELECT)) { + IOHi(dev->busdev.spi.csnPin); + } return true; } @@ -102,11 +155,23 @@ bool spiBusReadRegister(const busDevice_t * dev, uint8_t reg, uint8_t * data) { SPI_TypeDef * instance = spiInstanceByDevice(dev->busdev.spi.spiBus); - IOLo(dev->busdev.spi.csnPin); + if (!(dev->flags & DEVFLAGS_USE_MANUAL_DEVICE_SELECT)) { + IOLo(dev->busdev.spi.csnPin); + } + spiTransferByte(instance, reg); spiTransfer(instance, data, NULL, 1); - IOHi(dev->busdev.spi.csnPin); + + if (!(dev->flags & DEVFLAGS_USE_MANUAL_DEVICE_SELECT)) { + IOHi(dev->busdev.spi.csnPin); + } return true; } + +bool spiBusIsBusy(const busDevice_t * dev) +{ + SPI_TypeDef * instance = spiInstanceByDevice(dev->busdev.spi.spiBus); + return spiIsBusBusy(instance); +} #endif diff --git a/src/main/drivers/bus_i2c.h b/src/main/drivers/bus_i2c.h index a8b0212b954..55b07b8e7f0 100644 --- a/src/main/drivers/bus_i2c.h +++ b/src/main/drivers/bus_i2c.h @@ -61,8 +61,8 @@ typedef struct i2cDevice_s { void i2cSetSpeed(uint8_t speed); void i2cInit(I2CDevice device); -bool i2cWriteBuffer(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len_, const uint8_t *data); -bool i2cWrite(I2CDevice device, uint8_t addr_, uint8_t reg, uint8_t data); -bool i2cRead(I2CDevice device, uint8_t addr_, uint8_t reg, uint8_t len, uint8_t* buf); +bool i2cWriteBuffer(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len_, const uint8_t *data, bool allowRawAccess); +bool i2cWrite(I2CDevice device, uint8_t addr_, uint8_t reg, uint8_t data, bool allowRawAccess); +bool i2cRead(I2CDevice device, uint8_t addr_, uint8_t reg, uint8_t len, uint8_t* buf, bool allowRawAccess); uint16_t i2cGetErrorCounter(void); diff --git a/src/main/drivers/bus_i2c_hal.c b/src/main/drivers/bus_i2c_hal.c index f361496aac9..d64e0cf51e8 100644 --- a/src/main/drivers/bus_i2c_hal.c +++ b/src/main/drivers/bus_i2c_hal.c @@ -170,7 +170,7 @@ static bool i2cHandleHardwareFailure(I2CDevice device) return false; } -bool i2cWriteBuffer(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len_, const uint8_t *data) +bool i2cWriteBuffer(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len_, const uint8_t *data, bool allowRawAccess) { if (device == I2CINVALID) return false; @@ -183,10 +183,12 @@ bool i2cWriteBuffer(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len_, HAL_StatusTypeDef status; - if (reg_ == 0xFF) + if (reg_ == 0xFF && allowRawAccess) { status = HAL_I2C_Master_Transmit(&i2cHandle[device].Handle, addr_ << 1, (uint8_t *)data, len_, I2C_DEFAULT_TIMEOUT); - else + } + else { status = HAL_I2C_Mem_Write(&i2cHandle[device].Handle,addr_ << 1, reg_, I2C_MEMADD_SIZE_8BIT, (uint8_t *)data, len_, I2C_DEFAULT_TIMEOUT); + } if (status != HAL_OK) return i2cHandleHardwareFailure(device); @@ -194,12 +196,12 @@ bool i2cWriteBuffer(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len_, return true; } -bool i2cWrite(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t data) +bool i2cWrite(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t data, bool allowRawAccess) { - return i2cWriteBuffer(device, addr_, reg_, 1, &data); + return i2cWriteBuffer(device, addr_, reg_, 1, &data, allowRawAccess); } -bool i2cRead(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len, uint8_t* buf) +bool i2cRead(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len, uint8_t* buf, bool allowRawAccess) { if (device == I2CINVALID) return false; @@ -212,10 +214,12 @@ bool i2cRead(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len, uint8_t HAL_StatusTypeDef status; - if (reg_ == 0xFF) + if (reg_ == 0xFF && allowRawAccess) { status = HAL_I2C_Master_Receive(&i2cHandle[device].Handle,addr_ << 1,buf, len, I2C_DEFAULT_TIMEOUT); - else + } + else { status = HAL_I2C_Mem_Read(&i2cHandle[device].Handle,addr_ << 1, reg_, I2C_MEMADD_SIZE_8BIT,buf, len, I2C_DEFAULT_TIMEOUT); + } if (status != HAL_OK) return i2cHandleHardwareFailure(device); diff --git a/src/main/drivers/bus_i2c_soft.c b/src/main/drivers/bus_i2c_soft.c index 09f62a526ac..5c7d866970e 100644 --- a/src/main/drivers/bus_i2c_soft.c +++ b/src/main/drivers/bus_i2c_soft.c @@ -200,7 +200,7 @@ void i2cInit(I2CDevice device) IOConfigGPIOAF(sda, IOCFG_OUT_OD, 0); } -bool i2cWriteBuffer(I2CDevice device, uint8_t addr, uint8_t reg, uint8_t len, const uint8_t * data) +bool i2cWriteBuffer(I2CDevice device, uint8_t addr, uint8_t reg, uint8_t len, const uint8_t * data, bool allowRawAccess) { UNUSED(device); @@ -214,8 +214,10 @@ bool i2cWriteBuffer(I2CDevice device, uint8_t addr, uint8_t reg, uint8_t len, co I2C_Stop(); return false; } - I2C_SendByte(reg); - I2C_WaitAck(); + if (!allowRawAccess || reg != 0xFF) { + I2C_SendByte(reg); + I2C_WaitAck(); + } for (i = 0; i < len; i++) { I2C_SendByte(data[i]); if (!I2C_WaitAck()) { @@ -228,7 +230,7 @@ bool i2cWriteBuffer(I2CDevice device, uint8_t addr, uint8_t reg, uint8_t len, co return true; } -bool i2cWrite(I2CDevice device, uint8_t addr, uint8_t reg, uint8_t data) +bool i2cWrite(I2CDevice device, uint8_t addr, uint8_t reg, uint8_t data, bool allowRawAccess) { UNUSED(device); @@ -241,30 +243,36 @@ bool i2cWrite(I2CDevice device, uint8_t addr, uint8_t reg, uint8_t data) i2cErrorCount++; return false; } - I2C_SendByte(reg); - I2C_WaitAck(); + if (!allowRawAccess || reg != 0xFF) { + I2C_SendByte(reg); + I2C_WaitAck(); + } I2C_SendByte(data); I2C_WaitAck(); I2C_Stop(); return true; } -bool i2cRead(I2CDevice device, uint8_t addr, uint8_t reg, uint8_t len, uint8_t *buf) +bool i2cRead(I2CDevice device, uint8_t addr, uint8_t reg, uint8_t len, uint8_t *buf, bool allowRawAccess) { UNUSED(device); if (!I2C_Start()) { return false; } - I2C_SendByte(addr << 1 | I2C_Direction_Transmitter); - if (!I2C_WaitAck()) { - I2C_Stop(); - i2cErrorCount++; - return false; + + if (!allowRawAccess || reg != 0xFF) { + I2C_SendByte(addr << 1 | I2C_Direction_Transmitter); + if (!I2C_WaitAck()) { + I2C_Stop(); + i2cErrorCount++; + return false; + } + I2C_SendByte(reg); + I2C_WaitAck(); + I2C_Start(); } - I2C_SendByte(reg); - I2C_WaitAck(); - I2C_Start(); + I2C_SendByte(addr << 1 | I2C_Direction_Receiver); I2C_WaitAck(); while (len) { diff --git a/src/main/drivers/bus_i2c_stm32f30x.c b/src/main/drivers/bus_i2c_stm32f30x.c index 35662a5a8e9..425ab5effca 100644 --- a/src/main/drivers/bus_i2c_stm32f30x.c +++ b/src/main/drivers/bus_i2c_stm32f30x.c @@ -90,6 +90,7 @@ typedef struct i2cBusState_s { uint32_t timeout; /* Active transfer */ + bool allowRawAccess; uint8_t addr; // device address i2cTransferDirection_t rw; // direction uint8_t reg; // register @@ -153,7 +154,13 @@ static void i2cStateMachine(i2cBusState_t * i2cBusState, const uint32_t currentT case I2C_STATE_STARTING_WAIT: if (I2C_GetFlagStatus(I2Cx, I2C_ISR_BUSY) == RESET) { if (i2cBusState->rw == I2C_TXN_READ) { - i2cBusState->state = I2C_STATE_R_ADDR; + // Special case - no register address + if (i2cBusState->reg == 0xFF && i2cBusState->allowRawAccess) { + i2cBusState->state = I2C_STATE_R_RESTARTING; + } + else { + i2cBusState->state = I2C_STATE_R_ADDR; + } } else { i2cBusState->state = I2C_STATE_W_ADDR; @@ -233,10 +240,18 @@ static void i2cStateMachine(i2cBusState_t * i2cBusState, const uint32_t currentT case I2C_STATE_W_ADDR: /* Configure slave address, nbytes, reload, end mode and start or stop generation */ - I2C_TransferHandling(I2Cx, i2cBusState->addr, 1, I2C_Reload_Mode, I2C_Generate_Start_Write); - i2cBusState->state = I2C_STATE_W_ADDR_WAIT; - i2cBusState->timeout = currentTicks; - FALLTHROUGH; + if (i2cBusState->reg == 0xFF && i2cBusState->allowRawAccess) { + // Special no-address case, skip address byte transmission + I2C_TransferHandling(I2Cx, i2cBusState->addr, i2cBusState->len, I2C_AutoEnd_Mode, I2C_Generate_Start_Write); + i2cBusState->state = I2C_STATE_W_TRANSFER; + i2cBusState->timeout = currentTicks; + } + else { + I2C_TransferHandling(I2Cx, i2cBusState->addr, 1, I2C_Reload_Mode, I2C_Generate_Start_Write); + i2cBusState->state = I2C_STATE_W_ADDR_WAIT; + i2cBusState->timeout = currentTicks; + } + break; case I2C_STATE_W_ADDR_WAIT: if (I2C_GetFlagStatus(I2Cx, I2C_ISR_TXIS) != RESET) { @@ -389,7 +404,7 @@ static void i2cWaitForCompletion(I2CDevice device) } while (busState[device].state != I2C_STATE_STOPPED); } -bool i2cWriteBuffer(I2CDevice device, uint8_t addr, uint8_t reg, uint8_t len, const uint8_t * data) +bool i2cWriteBuffer(I2CDevice device, uint8_t addr, uint8_t reg, uint8_t len, const uint8_t * data, bool allowRawAccess) { // Don't try to access the non-initialized device if (!busState[device].initialized) @@ -403,6 +418,7 @@ bool i2cWriteBuffer(I2CDevice device, uint8_t addr, uint8_t reg, uint8_t len, co busState[device].buf = CONST_CAST(uint8_t *, data); busState[device].txnOk = false; busState[device].state = I2C_STATE_STARTING; + busState[device].allowRawAccess = allowRawAccess; // Inject I2C_EVENT_START i2cWaitForCompletion(device); @@ -410,13 +426,13 @@ bool i2cWriteBuffer(I2CDevice device, uint8_t addr, uint8_t reg, uint8_t len, co return busState[device].txnOk; } -bool i2cWrite(I2CDevice device, uint8_t addr, uint8_t reg, uint8_t data) +bool i2cWrite(I2CDevice device, uint8_t addr, uint8_t reg, uint8_t data, bool allowRawAccess) { - return i2cWriteBuffer(device, addr, reg, 1, &data); + return i2cWriteBuffer(device, addr, reg, 1, &data, allowRawAccess); } -bool i2cRead(I2CDevice device, uint8_t addr, uint8_t reg, uint8_t len, uint8_t* buf) +bool i2cRead(I2CDevice device, uint8_t addr, uint8_t reg, uint8_t len, uint8_t* buf, bool allowRawAccess) { // Don't try to access the non-initialized device if (!busState[device].initialized) @@ -430,6 +446,7 @@ bool i2cRead(I2CDevice device, uint8_t addr, uint8_t reg, uint8_t len, uint8_t* busState[device].buf = buf; busState[device].txnOk = false; busState[device].state = I2C_STATE_STARTING; + busState[device].allowRawAccess = allowRawAccess; // Inject I2C_EVENT_START i2cWaitForCompletion(device); diff --git a/src/main/drivers/bus_i2c_stm32f40x.c b/src/main/drivers/bus_i2c_stm32f40x.c index 6b43fa59d6f..d19035cbafc 100644 --- a/src/main/drivers/bus_i2c_stm32f40x.c +++ b/src/main/drivers/bus_i2c_stm32f40x.c @@ -124,6 +124,7 @@ typedef struct i2cBusState_s { uint32_t timeout; /* Active transfer */ + bool allowRawAccess; uint8_t addr; // device address i2cTransferDirection_t rw; // direction uint8_t reg; // register @@ -192,7 +193,13 @@ static void i2cStateMachine(i2cBusState_t * i2cBusState, const uint32_t currentT case I2C_STATE_STARTING_WAIT: if (I2C_CheckEvent(I2Cx, I2C_EVENT_MASTER_MODE_SELECT) != ERROR) { if (i2cBusState->rw == I2C_TXN_READ) { - i2cBusState->state = I2C_STATE_R_ADDR; + // Special case - no register address + if (i2cBusState->reg == 0xFF && i2cBusState->allowRawAccess) { + i2cBusState->state = I2C_STATE_R_RESTART_ADDR; + } + else { + i2cBusState->state = I2C_STATE_R_ADDR; + } } else { i2cBusState->state = I2C_STATE_W_ADDR; @@ -390,7 +397,13 @@ static void i2cStateMachine(i2cBusState_t * i2cBusState, const uint32_t currentT case I2C_STATE_W_ADDR_WAIT: if (I2C_CheckEvent(I2Cx, I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED) != ERROR) { - i2cBusState->state = I2C_STATE_W_REGISTER; + // Special no-address case, skip address byte transmission + if (i2cBusState->reg == 0xFF && i2cBusState->allowRawAccess) { + i2cBusState->state = I2C_STATE_W_TRANSFER; + } + else { + i2cBusState->state = I2C_STATE_W_REGISTER; + } } else if (I2C_GetFlagStatus(I2Cx, I2C_FLAG_AF) != RESET) { i2cBusState->state = I2C_STATE_NACK; @@ -531,7 +544,7 @@ static void i2cWaitForCompletion(I2CDevice device) } while (busState[device].state != I2C_STATE_STOPPED); } -bool i2cWriteBuffer(I2CDevice device, uint8_t addr, uint8_t reg, uint8_t len, const uint8_t * data) +bool i2cWriteBuffer(I2CDevice device, uint8_t addr, uint8_t reg, uint8_t len, const uint8_t * data, bool allowRawAccess) { // Don't try to access the non-initialized device if (!busState[device].initialized) @@ -545,6 +558,7 @@ bool i2cWriteBuffer(I2CDevice device, uint8_t addr, uint8_t reg, uint8_t len, co busState[device].buf = CONST_CAST(uint8_t*, data); busState[device].txnOk = false; busState[device].state = I2C_STATE_STARTING; + busState[device].allowRawAccess = allowRawAccess; // Inject I2C_EVENT_START i2cWaitForCompletion(device); @@ -552,12 +566,12 @@ bool i2cWriteBuffer(I2CDevice device, uint8_t addr, uint8_t reg, uint8_t len, co return busState[device].txnOk; } -bool i2cWrite(I2CDevice device, uint8_t addr, uint8_t reg, uint8_t data) +bool i2cWrite(I2CDevice device, uint8_t addr, uint8_t reg, uint8_t data, bool allowRawAccess) { - return i2cWriteBuffer(device, addr, reg, 1, &data); + return i2cWriteBuffer(device, addr, reg, 1, &data, allowRawAccess); } -bool i2cRead(I2CDevice device, uint8_t addr, uint8_t reg, uint8_t len, uint8_t* buf) +bool i2cRead(I2CDevice device, uint8_t addr, uint8_t reg, uint8_t len, uint8_t* buf, bool allowRawAccess) { // Don't try to access the non-initialized device if (!busState[device].initialized) @@ -571,6 +585,7 @@ bool i2cRead(I2CDevice device, uint8_t addr, uint8_t reg, uint8_t len, uint8_t* busState[device].buf = buf; busState[device].txnOk = false; busState[device].state = I2C_STATE_STARTING; + busState[device].allowRawAccess = allowRawAccess; // Inject I2C_EVENT_START i2cWaitForCompletion(device); diff --git a/src/main/drivers/bus_spi.c b/src/main/drivers/bus_spi.c index 0e4a00b6236..c6b4936d421 100644 --- a/src/main/drivers/bus_spi.c +++ b/src/main/drivers/bus_spi.c @@ -72,6 +72,22 @@ #define SPI3_NSS_PIN NONE #endif +#ifdef SPI1_CLOCK_LEADING_EDGE +# define SPI1_LEADING_EDGE true +#else +# define SPI1_LEADING_EDGE false +#endif +#ifdef SPI2_CLOCK_LEADING_EDGE +# define SPI2_LEADING_EDGE true +#else +# define SPI2_LEADING_EDGE false +#endif +#ifdef SPI3_CLOCK_LEADING_EDGE +# define SPI3_LEADING_EDGE true +#else +# define SPI3_LEADING_EDGE false +#endif + #if defined(STM32F3) static const uint16_t spiDivisorMapFast[] = { SPI_BaudRatePrescaler_256, // SPI_CLOCK_INITIALIZATON 281.25 KBits/s @@ -80,6 +96,7 @@ static const uint16_t spiDivisorMapFast[] = { SPI_BaudRatePrescaler_4, // SPI_CLOCK_FAST 18.0 MBits/s SPI_BaudRatePrescaler_4 // SPI_CLOCK_ULTRAFAST 18.0 MBits/s }; + static const uint16_t spiDivisorMapSlow[] = { SPI_BaudRatePrescaler_256, // SPI_CLOCK_INITIALIZATON 140.625 KBits/s SPI_BaudRatePrescaler_64, // SPI_CLOCK_SLOW 562.5 KBits/s @@ -87,6 +104,12 @@ static const uint16_t spiDivisorMapSlow[] = { SPI_BaudRatePrescaler_2, // SPI_CLOCK_FAST 18.0 MBits/s SPI_BaudRatePrescaler_2 // SPI_CLOCK_ULTRAFAST 18.0 MBits/s }; + +static spiDevice_t spiHardwareMap[] = { + { .dev = SPI1, .nss = IO_TAG(SPI1_NSS_PIN), .sck = IO_TAG(SPI1_SCK_PIN), .miso = IO_TAG(SPI1_MISO_PIN), .mosi = IO_TAG(SPI1_MOSI_PIN), .rcc = RCC_APB2(SPI1), .af = GPIO_AF_SPI1, .leadingEdge = SPI1_LEADING_EDGE, .divisorMap = spiDivisorMapFast }, + { .dev = SPI2, .nss = IO_TAG(SPI2_NSS_PIN), .sck = IO_TAG(SPI2_SCK_PIN), .miso = IO_TAG(SPI2_MISO_PIN), .mosi = IO_TAG(SPI2_MOSI_PIN), .rcc = RCC_APB1(SPI2), .af = GPIO_AF_SPI2, .leadingEdge = SPI2_LEADING_EDGE, .divisorMap = spiDivisorMapSlow }, + { .dev = SPI3, .nss = IO_TAG(SPI3_NSS_PIN), .sck = IO_TAG(SPI3_SCK_PIN), .miso = IO_TAG(SPI3_MISO_PIN), .mosi = IO_TAG(SPI3_MOSI_PIN), .rcc = RCC_APB1(SPI3), .af = GPIO_AF_SPI3, .leadingEdge = SPI3_LEADING_EDGE, .divisorMap = spiDivisorMapSlow } +}; #elif defined(STM32F4) static const uint16_t spiDivisorMapFast[] = { SPI_BaudRatePrescaler_256, // SPI_CLOCK_INITIALIZATON 328.125 KBits/s @@ -95,6 +118,7 @@ static const uint16_t spiDivisorMapFast[] = { SPI_BaudRatePrescaler_4, // SPI_CLOCK_FAST 21.0 MBits/s SPI_BaudRatePrescaler_2 // SPI_CLOCK_ULTRAFAST 42.0 MBits/s }; + static const uint16_t spiDivisorMapSlow[] = { SPI_BaudRatePrescaler_256, // SPI_CLOCK_INITIALIZATON 164.062 KBits/s SPI_BaudRatePrescaler_64, // SPI_CLOCK_SLOW 656.25 KBits/s @@ -102,17 +126,15 @@ static const uint16_t spiDivisorMapSlow[] = { SPI_BaudRatePrescaler_2, // SPI_CLOCK_FAST 21.0 MBits/s SPI_BaudRatePrescaler_2 // SPI_CLOCK_ULTRAFAST 21.0 MBits/s }; -#else -#error "Invalid CPU" -#endif static spiDevice_t spiHardwareMap[] = { - { .dev = SPI1, .nss = IO_TAG(SPI1_NSS_PIN), .sck = IO_TAG(SPI1_SCK_PIN), .miso = IO_TAG(SPI1_MISO_PIN), .mosi = IO_TAG(SPI1_MOSI_PIN), .rcc = RCC_APB2(SPI1), .af = GPIO_AF_SPI1, false, .divisorMap = spiDivisorMapFast }, - { .dev = SPI2, .nss = IO_TAG(SPI2_NSS_PIN), .sck = IO_TAG(SPI2_SCK_PIN), .miso = IO_TAG(SPI2_MISO_PIN), .mosi = IO_TAG(SPI2_MOSI_PIN), .rcc = RCC_APB1(SPI2), .af = GPIO_AF_SPI2, false, .divisorMap = spiDivisorMapSlow }, -#if defined(STM32F3) || defined(STM32F4) - { .dev = SPI3, .nss = IO_TAG(SPI3_NSS_PIN), .sck = IO_TAG(SPI3_SCK_PIN), .miso = IO_TAG(SPI3_MISO_PIN), .mosi = IO_TAG(SPI3_MOSI_PIN), .rcc = RCC_APB1(SPI3), .af = GPIO_AF_SPI3, false, .divisorMap = spiDivisorMapSlow } -#endif + { .dev = SPI1, .nss = IO_TAG(SPI1_NSS_PIN), .sck = IO_TAG(SPI1_SCK_PIN), .miso = IO_TAG(SPI1_MISO_PIN), .mosi = IO_TAG(SPI1_MOSI_PIN), .rcc = RCC_APB2(SPI1), .af = GPIO_AF_SPI1, .leadingEdge = SPI1_LEADING_EDGE, .divisorMap = spiDivisorMapFast }, + { .dev = SPI2, .nss = IO_TAG(SPI2_NSS_PIN), .sck = IO_TAG(SPI2_SCK_PIN), .miso = IO_TAG(SPI2_MISO_PIN), .mosi = IO_TAG(SPI2_MOSI_PIN), .rcc = RCC_APB1(SPI2), .af = GPIO_AF_SPI2, .leadingEdge = SPI2_LEADING_EDGE, .divisorMap = spiDivisorMapSlow }, + { .dev = SPI3, .nss = IO_TAG(SPI3_NSS_PIN), .sck = IO_TAG(SPI3_SCK_PIN), .miso = IO_TAG(SPI3_MISO_PIN), .mosi = IO_TAG(SPI3_MOSI_PIN), .rcc = RCC_APB1(SPI3), .af = GPIO_AF_SPI3, .leadingEdge = SPI3_LEADING_EDGE, .divisorMap = spiDivisorMapSlow } }; +#else +#error "Invalid CPU" +#endif SPIDevice spiDeviceByInstance(SPI_TypeDef *instance) { @@ -132,17 +154,6 @@ void spiInitDevice(SPIDevice device) { spiDevice_t *spi = &(spiHardwareMap[device]); -#ifdef SDCARD_SPI_INSTANCE - if (spi->dev == SDCARD_SPI_INSTANCE) { - spi->leadingEdge = true; - } -#endif -#ifdef RX_SPI_INSTANCE - if (spi->dev == RX_SPI_INSTANCE) { - spi->leadingEdge = true; - } -#endif - // Enable SPI clock RCC_ClockCmd(spi->rcc, ENABLE); RCC_ResetCmd(spi->rcc, ENABLE); @@ -180,9 +191,11 @@ void spiInitDevice(SPIDevice device) spiInit.SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_8; if (spi->leadingEdge) { + // SPI_MODE0 spiInit.SPI_CPOL = SPI_CPOL_Low; spiInit.SPI_CPHA = SPI_CPHA_1Edge; } else { + // SPI_MODE3 spiInit.SPI_CPOL = SPI_CPOL_High; spiInit.SPI_CPHA = SPI_CPHA_2Edge; } diff --git a/src/main/drivers/bus_spi.h b/src/main/drivers/bus_spi.h index 0653b03976f..616a89c548f 100644 --- a/src/main/drivers/bus_spi.h +++ b/src/main/drivers/bus_spi.h @@ -19,6 +19,7 @@ #include "drivers/io_types.h" #include "drivers/rcc_types.h" +#include "drivers/dma.h" #if defined(STM32F4) || defined(STM32F3) #define SPI_IO_AF_CFG IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, GPIO_OType_PP, GPIO_PuPd_NOPULL) @@ -69,22 +70,16 @@ typedef struct SPIDevice_s { ioTag_t miso; rccPeriphTag_t rcc; uint8_t af; - volatile uint16_t errorCount; bool leadingEdge; -#if defined(STM32F7) - SPI_HandleTypeDef hspi; - DMA_HandleTypeDef hdma; - uint8_t dmaIrqHandler; -#endif const uint16_t * divisorMap; + volatile uint16_t errorCount; } spiDevice_t; bool spiInit(SPIDevice device); +bool spiIsBusBusy(SPI_TypeDef *instance); void spiSetSpeed(SPI_TypeDef *instance, SPIClockSpeed_e speed); uint8_t spiTransferByte(SPI_TypeDef *instance, uint8_t in); -bool spiIsBusBusy(SPI_TypeDef *instance); - -bool spiTransfer(SPI_TypeDef *instance, uint8_t *out, const uint8_t *in, int len); +bool spiTransfer(SPI_TypeDef *instance, uint8_t *rxData, const uint8_t *txData, int len); uint16_t spiGetErrorCounter(SPI_TypeDef *instance); void spiResetErrorCounter(SPI_TypeDef *instance); diff --git a/src/main/drivers/bus_spi_hal.c b/src/main/drivers/bus_spi_hal.c index e279d254059..b4d5be9724d 100644 --- a/src/main/drivers/bus_spi_hal.c +++ b/src/main/drivers/bus_spi_hal.c @@ -68,26 +68,47 @@ #define SPI4_NSS_PIN NONE #endif +#ifdef SPI1_CLOCK_LEADING_EDGE +# define SPI1_LEADING_EDGE true +#else +# define SPI1_LEADING_EDGE false +#endif +#ifdef SPI2_CLOCK_LEADING_EDGE +# define SPI2_LEADING_EDGE true +#else +# define SPI2_LEADING_EDGE false +#endif +#ifdef SPI3_CLOCK_LEADING_EDGE +# define SPI3_LEADING_EDGE true +#else +# define SPI3_LEADING_EDGE false +#endif +#ifdef SPI4_CLOCK_LEADING_EDGE +# define SPI4_LEADING_EDGE true +#else +# define SPI4_LEADING_EDGE false +#endif + static const uint16_t spiDivisorMapFast[] = { - SPI_BAUDRATEPRESCALER_256, // SPI_CLOCK_INITIALIZATON 421.875 KBits/s - SPI_BAUDRATEPRESCALER_128, // SPI_CLOCK_SLOW 843.75 KBits/s - SPI_BAUDRATEPRESCALER_16, // SPI_CLOCK_STANDARD 6.75 MBits/s - SPI_BAUDRATEPRESCALER_8, // SPI_CLOCK_FAST 13.5 MBits/s - SPI_BAUDRATEPRESCALER_4 // SPI_CLOCK_ULTRAFAST 27.0 MBits/s + LL_SPI_BAUDRATEPRESCALER_DIV256, // SPI_CLOCK_INITIALIZATON 421.875 KBits/s + LL_SPI_BAUDRATEPRESCALER_DIV32, // SPI_CLOCK_SLOW 843.75 KBits/s + LL_SPI_BAUDRATEPRESCALER_DIV16, // SPI_CLOCK_STANDARD 6.75 MBits/s + LL_SPI_BAUDRATEPRESCALER_DIV8, // SPI_CLOCK_FAST 13.5 MBits/s + LL_SPI_BAUDRATEPRESCALER_DIV4 // SPI_CLOCK_ULTRAFAST 27.0 MBits/s }; static const uint16_t spiDivisorMapSlow[] = { - SPI_BAUDRATEPRESCALER_256, // SPI_CLOCK_INITIALIZATON 210.937 KBits/s - SPI_BAUDRATEPRESCALER_64, // SPI_CLOCK_SLOW 843.75 KBits/s - SPI_BAUDRATEPRESCALER_8, // SPI_CLOCK_STANDARD 6.75 MBits/s - SPI_BAUDRATEPRESCALER_4, // SPI_CLOCK_FAST 13.5 MBits/s - SPI_BAUDRATEPRESCALER_2 // SPI_CLOCK_ULTRAFAST 27.0 MBits/s + LL_SPI_BAUDRATEPRESCALER_DIV256, // SPI_CLOCK_INITIALIZATON 210.937 KBits/s + LL_SPI_BAUDRATEPRESCALER_DIV64, // SPI_CLOCK_SLOW 843.75 KBits/s + LL_SPI_BAUDRATEPRESCALER_DIV8, // SPI_CLOCK_STANDARD 6.75 MBits/s + LL_SPI_BAUDRATEPRESCALER_DIV4, // SPI_CLOCK_FAST 13.5 MBits/s + LL_SPI_BAUDRATEPRESCALER_DIV2 // SPI_CLOCK_ULTRAFAST 27.0 MBits/s }; static spiDevice_t spiHardwareMap[] = { - { .dev = SPI1, .nss = IO_TAG(SPI1_NSS_PIN), .sck = IO_TAG(SPI1_SCK_PIN), .miso = IO_TAG(SPI1_MISO_PIN), .mosi = IO_TAG(SPI1_MOSI_PIN), .rcc = RCC_APB2(SPI1), .af = GPIO_AF5_SPI1, .leadingEdge = false, .dmaIrqHandler = DMA2_ST3_HANDLER, .divisorMap = spiDivisorMapFast }, - { .dev = SPI2, .nss = IO_TAG(SPI2_NSS_PIN), .sck = IO_TAG(SPI2_SCK_PIN), .miso = IO_TAG(SPI2_MISO_PIN), .mosi = IO_TAG(SPI2_MOSI_PIN), .rcc = RCC_APB1(SPI2), .af = GPIO_AF5_SPI2, .leadingEdge = false, .dmaIrqHandler = DMA1_ST4_HANDLER, .divisorMap = spiDivisorMapSlow }, - { .dev = SPI3, .nss = IO_TAG(SPI3_NSS_PIN), .sck = IO_TAG(SPI3_SCK_PIN), .miso = IO_TAG(SPI3_MISO_PIN), .mosi = IO_TAG(SPI3_MOSI_PIN), .rcc = RCC_APB1(SPI3), .af = GPIO_AF6_SPI3, .leadingEdge = false, .dmaIrqHandler = DMA1_ST7_HANDLER, .divisorMap = spiDivisorMapSlow }, - { .dev = SPI4, .nss = IO_TAG(SPI4_NSS_PIN), .sck = IO_TAG(SPI4_SCK_PIN), .miso = IO_TAG(SPI4_MISO_PIN), .mosi = IO_TAG(SPI4_MOSI_PIN), .rcc = RCC_APB2(SPI4), .af = GPIO_AF5_SPI4, .leadingEdge = false, .dmaIrqHandler = DMA2_ST1_HANDLER, .divisorMap = spiDivisorMapSlow } + { .dev = SPI1, .nss = IO_TAG(SPI1_NSS_PIN), .sck = IO_TAG(SPI1_SCK_PIN), .miso = IO_TAG(SPI1_MISO_PIN), .mosi = IO_TAG(SPI1_MOSI_PIN), .rcc = RCC_APB2(SPI1), .af = GPIO_AF5_SPI1, .leadingEdge = SPI1_LEADING_EDGE, .divisorMap = spiDivisorMapFast }, + { .dev = SPI2, .nss = IO_TAG(SPI2_NSS_PIN), .sck = IO_TAG(SPI2_SCK_PIN), .miso = IO_TAG(SPI2_MISO_PIN), .mosi = IO_TAG(SPI2_MOSI_PIN), .rcc = RCC_APB1(SPI2), .af = GPIO_AF5_SPI2, .leadingEdge = SPI2_LEADING_EDGE, .divisorMap = spiDivisorMapSlow }, + { .dev = SPI3, .nss = IO_TAG(SPI3_NSS_PIN), .sck = IO_TAG(SPI3_SCK_PIN), .miso = IO_TAG(SPI3_MISO_PIN), .mosi = IO_TAG(SPI3_MOSI_PIN), .rcc = RCC_APB1(SPI3), .af = GPIO_AF6_SPI3, .leadingEdge = SPI3_LEADING_EDGE, .divisorMap = spiDivisorMapSlow }, + { .dev = SPI4, .nss = IO_TAG(SPI4_NSS_PIN), .sck = IO_TAG(SPI4_SCK_PIN), .miso = IO_TAG(SPI4_MISO_PIN), .mosi = IO_TAG(SPI4_MOSI_PIN), .rcc = RCC_APB2(SPI4), .af = GPIO_AF5_SPI4, .leadingEdge = SPI4_LEADING_EDGE, .divisorMap = spiDivisorMapSlow } }; SPIDevice spiDeviceByInstance(SPI_TypeDef *instance) @@ -107,46 +128,23 @@ SPIDevice spiDeviceByInstance(SPI_TypeDef *instance) return SPIINVALID; } -SPI_HandleTypeDef* spiHandleByInstance(SPI_TypeDef *instance) -{ - return &spiHardwareMap[spiDeviceByInstance(instance)].hspi; -} - -DMA_HandleTypeDef* dmaHandleByInstance(SPI_TypeDef *instance) -{ - return &spiHardwareMap[spiDeviceByInstance(instance)].hdma; -} - -void SPI1_IRQHandler(void) -{ - HAL_SPI_IRQHandler(&spiHardwareMap[SPIDEV_1].hspi); -} - -void SPI2_IRQHandler(void) -{ - HAL_SPI_IRQHandler(&spiHardwareMap[SPIDEV_2].hspi); -} - -void SPI3_IRQHandler(void) +void spiTimeoutUserCallback(SPI_TypeDef *instance) { - HAL_SPI_IRQHandler(&spiHardwareMap[SPIDEV_3].hspi); -} + SPIDevice device = spiDeviceByInstance(instance); + if (device == SPIINVALID) { + return; + } -void SPI4_IRQHandler(void) -{ - HAL_SPI_IRQHandler(&spiHardwareMap[SPIDEV_4].hspi); + spiHardwareMap[device].errorCount++; } - void spiInitDevice(SPIDevice device) { spiDevice_t *spi = &(spiHardwareMap[device]); -#ifdef SDCARD_SPI_INSTANCE - if (spi->dev == SDCARD_SPI_INSTANCE) { - spi->leadingEdge = true; + if (!spi->dev) { + return; } -#endif // Enable SPI clock RCC_ClockCmd(spi->rcc, ENABLE); @@ -156,7 +154,6 @@ void spiInitDevice(SPIDevice device) IOInit(IOGetByTag(spi->miso), OWNER_SPI, RESOURCE_SPI_MISO, device + 1); IOInit(IOGetByTag(spi->mosi), OWNER_SPI, RESOURCE_SPI_MOSI, device + 1); -#if defined(STM32F3) || defined(STM32F4) || defined(STM32F7) if (spi->leadingEdge == true) { IOConfigGPIOAF(IOGetByTag(spi->sck), SPI_IO_AF_SCK_CFG_LOW, spi->af); } else { @@ -169,34 +166,32 @@ void spiInitDevice(SPIDevice device) IOInit(IOGetByTag(spi->nss), OWNER_SPI, RESOURCE_SPI_CS, device + 1); IOConfigGPIO(IOGetByTag(spi->nss), SPI_IO_CS_CFG); } -#endif - spiHardwareMap[device].hspi.Instance = spi->dev; - // Init SPI hardware - HAL_SPI_DeInit(&spiHardwareMap[device].hspi); - - spiHardwareMap[device].hspi.Init.Mode = SPI_MODE_MASTER; - spiHardwareMap[device].hspi.Init.Direction = SPI_DIRECTION_2LINES; - spiHardwareMap[device].hspi.Init.DataSize = SPI_DATASIZE_8BIT; - spiHardwareMap[device].hspi.Init.NSS = SPI_NSS_SOFT; - spiHardwareMap[device].hspi.Init.FirstBit = SPI_FIRSTBIT_MSB; - spiHardwareMap[device].hspi.Init.CRCPolynomial = 7; - spiHardwareMap[device].hspi.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_256; - spiHardwareMap[device].hspi.Init.CRCCalculation = SPI_CRCCALCULATION_DISABLE; - spiHardwareMap[device].hspi.Init.TIMode = SPI_TIMODE_DISABLED; - - if (spi->leadingEdge) { - spiHardwareMap[device].hspi.Init.CLKPolarity = SPI_POLARITY_LOW; - spiHardwareMap[device].hspi.Init.CLKPhase = SPI_PHASE_1EDGE; - } else { - spiHardwareMap[device].hspi.Init.CLKPolarity = SPI_POLARITY_HIGH; - spiHardwareMap[device].hspi.Init.CLKPhase = SPI_PHASE_2EDGE; - } + LL_SPI_Disable(spi->dev); + LL_SPI_DeInit(spi->dev); - if (HAL_SPI_Init(&spiHardwareMap[device].hspi) == HAL_OK) { - if (spi->nss) { - IOHi(IOGetByTag(spi->nss)); - } + LL_SPI_InitTypeDef init = + { + .TransferDirection = SPI_DIRECTION_2LINES, + .Mode = SPI_MODE_MASTER, + .DataWidth = SPI_DATASIZE_8BIT, + .ClockPolarity = spi->leadingEdge ? SPI_POLARITY_LOW : SPI_POLARITY_HIGH, + .ClockPhase = spi->leadingEdge ? SPI_PHASE_1EDGE : SPI_PHASE_2EDGE, + .NSS = SPI_NSS_SOFT, + .BaudRate = SPI_BAUDRATEPRESCALER_8, + .BitOrder = SPI_FIRSTBIT_MSB, + .CRCPoly = 7, + .CRCCalculation = SPI_CRCCALCULATION_DISABLE, + }; + LL_SPI_SetRxFIFOThreshold(spi->dev, SPI_RXFIFO_THRESHOLD_QF); + + LL_SPI_Init(spi->dev, &init); + LL_SPI_Enable(spi->dev); + + SET_BIT(spi->dev->CR2, SPI_RXFIFO_THRESHOLD); + + if (spi->nss) { + IOHi(IOGetByTag(spi->nss)); } } @@ -238,20 +233,28 @@ bool spiInit(SPIDevice device) return false; } -uint32_t spiTimeoutUserCallback(SPI_TypeDef *instance) +uint8_t spiTransferByte(SPI_TypeDef *instance, uint8_t txByte) { - SPIDevice device = spiDeviceByInstance(instance); - if (device == SPIINVALID) - return -1; - spiHardwareMap[device].errorCount++; - return spiHardwareMap[device].errorCount; -} + uint16_t spiTimeout = 1000; -// return uint8_t value or -1 when failure -uint8_t spiTransferByte(SPI_TypeDef *instance, uint8_t in) -{ - spiTransfer(instance, &in, &in, 1); - return in; + while (!LL_SPI_IsActiveFlag_TXE(instance)) { + if ((spiTimeout--) == 0) { + spiTimeoutUserCallback(instance); + return 0xFF; + } + } + + LL_SPI_TransmitData8(instance, txByte); + + spiTimeout = 1000; + while (!LL_SPI_IsActiveFlag_RXNE(instance)) { + if ((spiTimeout--) == 0) { + spiTimeoutUserCallback(instance); + return 0xFF; + } + } + + return (uint8_t)LL_SPI_ReceiveData8(instance); } /** @@ -259,108 +262,46 @@ uint8_t spiTransferByte(SPI_TypeDef *instance, uint8_t in) */ bool spiIsBusBusy(SPI_TypeDef *instance) { - SPIDevice device = spiDeviceByInstance(instance); - if (spiHardwareMap[device].hspi.State == HAL_SPI_STATE_BUSY) - return true; - else - return false; + return (LL_SPI_GetTxFIFOLevel(instance) != LL_SPI_TX_FIFO_EMPTY) || LL_SPI_IsActiveFlag_BSY(instance); } -bool spiTransfer(SPI_TypeDef *instance, uint8_t *out, const uint8_t *in, int len) +bool spiTransfer(SPI_TypeDef *instance, uint8_t *rxData, const uint8_t *txData, int len) { - SPIDevice device = spiDeviceByInstance(instance); - HAL_StatusTypeDef status; - -#define SPI_DEFAULT_TIMEOUT 10 - - if (!out) // Tx only - { - status = HAL_SPI_Transmit(&spiHardwareMap[device].hspi, (uint8_t *)in, len, SPI_DEFAULT_TIMEOUT); - } - else if (!in) // Rx only - { - status = HAL_SPI_Receive(&spiHardwareMap[device].hspi, out, len, SPI_DEFAULT_TIMEOUT); - } - else // Tx and Rx - { - status = HAL_SPI_TransmitReceive(&spiHardwareMap[device].hspi, (uint8_t *)in, out, len, SPI_DEFAULT_TIMEOUT); + SET_BIT(instance->CR2, SPI_RXFIFO_THRESHOLD); + while (len) { + int spiTimeout = 1000; + while (!LL_SPI_IsActiveFlag_TXE(instance)) { + if ((spiTimeout--) == 0) { + spiTimeoutUserCallback(instance); + return false; + } + } + uint8_t b = txData ? *(txData++) : 0xFF; + LL_SPI_TransmitData8(instance, b); + + spiTimeout = 1000; + while (!LL_SPI_IsActiveFlag_RXNE(instance)) { + if ((spiTimeout--) == 0) { + spiTimeoutUserCallback(instance); + return false; + } + } + b = LL_SPI_ReceiveData8(instance); + if (rxData) { + *(rxData++) = b; + } + --len; } - if ( status != HAL_OK) - spiTimeoutUserCallback(instance); - return true; } - void spiSetSpeed(SPI_TypeDef *instance, SPIClockSpeed_e speed) { SPIDevice device = spiDeviceByInstance(instance); - HAL_SPI_DeInit(&spiHardwareMap[device].hspi); - - spiHardwareMap[device].hspi.Init.BaudRatePrescaler = spiHardwareMap[device].divisorMap[speed]; - - HAL_SPI_Init(&spiHardwareMap[device].hspi); -} - -uint16_t spiGetErrorCounter(SPI_TypeDef *instance) -{ - SPIDevice device = spiDeviceByInstance(instance); - if (device == SPIINVALID) - return 0; - return spiHardwareMap[device].errorCount; -} - -void spiResetErrorCounter(SPI_TypeDef *instance) -{ - SPIDevice device = spiDeviceByInstance(instance); - if (device != SPIINVALID) - spiHardwareMap[device].errorCount = 0; -} - -void dmaSPIIRQHandler(dmaChannelDescriptor_t* descriptor) -{ - SPIDevice device = descriptor->userParam; - if (device != SPIINVALID) - HAL_DMA_IRQHandler(&spiHardwareMap[device].hdma); -} - - -DMA_HandleTypeDef* spiSetDMATransmit(DMA_Stream_TypeDef *Stream, uint32_t Channel, SPI_TypeDef *Instance, uint8_t *pData, uint16_t Size) -{ - SPIDevice device = spiDeviceByInstance(Instance); - - spiHardwareMap[device].hdma.Instance = Stream; - spiHardwareMap[device].hdma.Init.Channel = Channel; - spiHardwareMap[device].hdma.Init.Direction = DMA_MEMORY_TO_PERIPH; - spiHardwareMap[device].hdma.Init.PeriphInc = DMA_PINC_DISABLE; - spiHardwareMap[device].hdma.Init.MemInc = DMA_MINC_ENABLE; - spiHardwareMap[device].hdma.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE; - spiHardwareMap[device].hdma.Init.MemDataAlignment = DMA_MDATAALIGN_BYTE; - spiHardwareMap[device].hdma.Init.Mode = DMA_NORMAL; - spiHardwareMap[device].hdma.Init.FIFOMode = DMA_FIFOMODE_DISABLE; - spiHardwareMap[device].hdma.Init.FIFOThreshold = DMA_FIFO_THRESHOLD_1QUARTERFULL; - spiHardwareMap[device].hdma.Init.PeriphBurst = DMA_PBURST_SINGLE; - spiHardwareMap[device].hdma.Init.MemBurst = DMA_MBURST_SINGLE; - spiHardwareMap[device].hdma.Init.Priority = DMA_PRIORITY_LOW; - - HAL_DMA_DeInit(&spiHardwareMap[device].hdma); - HAL_DMA_Init(&spiHardwareMap[device].hdma); - - __HAL_DMA_ENABLE(&spiHardwareMap[device].hdma); - __HAL_SPI_ENABLE(&spiHardwareMap[device].hspi); - - /* Associate the initialized DMA handle to the spi handle */ - __HAL_LINKDMA(&spiHardwareMap[device].hspi, hdmatx, spiHardwareMap[device].hdma); - - // DMA TX Interrupt - dmaSetHandler(spiHardwareMap[device].dmaIrqHandler, dmaSPIIRQHandler, NVIC_BUILD_PRIORITY(3, 0), (uint32_t)device); - - //HAL_CLEANCACHE(pData,Size); - // And Transmit - HAL_SPI_Transmit_DMA(&spiHardwareMap[device].hspi, pData, Size); - - return &spiHardwareMap[device].hdma; + LL_SPI_Disable(instance); + LL_SPI_SetBaudRatePrescaler(instance, spiHardwareMap[device].divisorMap[speed]); + LL_SPI_Enable(instance); } SPI_TypeDef * spiInstanceByDevice(SPIDevice device) diff --git a/src/main/drivers/display.c b/src/main/drivers/display.c index ca122435ee6..b26838225ab 100644 --- a/src/main/drivers/display.c +++ b/src/main/drivers/display.c @@ -54,12 +54,10 @@ static bool displayAttributesRequireEmulation(displayPort_t *instance, textAttri } static bool displayEmulateTextAttributes(displayPort_t *instance, - char *buf, - const char *s, size_t length, + char *buf, size_t length, textAttributes_t *attr) { UNUSED(instance); - UNUSED(s); // We only emulate blink for now, so there's no need to test // for it again. @@ -148,7 +146,7 @@ int displayWriteWithAttr(displayPort_t *instance, uint8_t x, uint8_t y, const ch // We can't overwrite s, so we use an intermediate buffer if we need // text attribute emulation. size_t blockSize = length > sizeof(buf) ? sizeof(buf) : length; - if (displayEmulateTextAttributes(instance, buf, s, blockSize, &attr)) { + if (displayEmulateTextAttributes(instance, buf, blockSize, &attr)) { // Emulation required rewriting the string, use buf. s = buf; } @@ -157,26 +155,35 @@ int displayWriteWithAttr(displayPort_t *instance, uint8_t x, uint8_t y, const ch return instance->vTable->writeString(instance, x, y, s, attr); } -int displayWriteChar(displayPort_t *instance, uint8_t x, uint8_t y, uint8_t c) +int displayWriteChar(displayPort_t *instance, uint8_t x, uint8_t y, uint16_t c) { + if (c > instance->maxChar) { + return -1; + } instance->posX = x + 1; instance->posY = y; return instance->vTable->writeChar(instance, x, y, c, TEXT_ATTRIBUTES_NONE); } -int displayWriteCharWithAttr(displayPort_t *instance, uint8_t x, uint8_t y, uint8_t c, textAttributes_t attr) +int displayWriteCharWithAttr(displayPort_t *instance, uint8_t x, uint8_t y, uint16_t c, textAttributes_t attr) { + if (c > instance->maxChar) { + return -1; + } if (displayAttributesRequireEmulation(instance, attr)) { - displayEmulateTextAttributes(instance, (char *)&c, (char *)&c, 1, &attr); + char ec; + if (displayEmulateTextAttributes(instance, &ec, 1, &attr)) { + c = ec; + } } instance->posX = x + 1; instance->posY = y; return instance->vTable->writeChar(instance, x, y, c, attr); } -bool displayReadCharWithAttr(displayPort_t *instance, uint8_t x, uint8_t y, uint8_t *c, textAttributes_t *attr) +bool displayReadCharWithAttr(displayPort_t *instance, uint8_t x, uint8_t y, uint16_t *c, textAttributes_t *attr) { - uint8_t dc; + uint16_t dc; textAttributes_t dattr; if (!instance->vTable->readChar) { @@ -214,6 +221,14 @@ uint16_t displayTxBytesFree(const displayPort_t *instance) return instance->vTable->txBytesFree(instance); } +bool displayGetFontMetadata(displayFontMetadata_t *metadata, const displayPort_t *instance) +{ + if (instance->vTable->getFontMetadata) { + return instance->vTable->getFontMetadata(metadata, instance); + } + return false; +} + void displayInit(displayPort_t *instance, const displayPortVTable_t *vTable) { instance->vTable = vTable; @@ -228,5 +243,13 @@ void displayInit(displayPort_t *instance, const displayPortVTable_t *vTable) if (displayConfig()->force_sw_blink) { TEXT_ATTRIBUTES_REMOVE_BLINK(instance->cachedSupportedTextAttributes); } + + displayFontMetadata_t metadata; + if (displayGetFontMetadata(&metadata, instance)) { + instance->maxChar = metadata.charCount - 1; + } else { + // Assume 8-bit character implementation + instance->maxChar = 255; + } } diff --git a/src/main/drivers/display.h b/src/main/drivers/display.h index c061611792d..4b76a6349a3 100644 --- a/src/main/drivers/display.h +++ b/src/main/drivers/display.h @@ -54,6 +54,11 @@ typedef uint8_t textAttributes_t; static inline void TEXT_ATTRIBUTES_COPY(textAttributes_t *dst, textAttributes_t *src) { *dst = *src; } +typedef struct displayFontMetadata_s { + uint8_t version; + uint16_t charCount; +} displayFontMetadata_t; + struct displayPortVTable_s; typedef struct displayPort_s { const struct displayPortVTable_s *vTable; @@ -68,6 +73,7 @@ typedef struct displayPort_s { int8_t cursorRow; int8_t grabCount; textAttributes_t cachedSupportedTextAttributes; + uint16_t maxChar; } displayPort_t; typedef struct displayPortVTable_s { @@ -77,13 +83,14 @@ typedef struct displayPortVTable_s { int (*drawScreen)(displayPort_t *displayPort); int (*screenSize)(const displayPort_t *displayPort); int (*writeString)(displayPort_t *displayPort, uint8_t x, uint8_t y, const char *text, textAttributes_t attr); - int (*writeChar)(displayPort_t *displayPort, uint8_t x, uint8_t y, uint8_t c, textAttributes_t attr); - bool (*readChar)(displayPort_t *displayPort, uint8_t x, uint8_t y, uint8_t *c, textAttributes_t *attr); + int (*writeChar)(displayPort_t *displayPort, uint8_t x, uint8_t y, uint16_t c, textAttributes_t attr); + bool (*readChar)(displayPort_t *displayPort, uint8_t x, uint8_t y, uint16_t *c, textAttributes_t *attr); bool (*isTransferInProgress)(const displayPort_t *displayPort); int (*heartbeat)(displayPort_t *displayPort); void (*resync)(displayPort_t *displayPort); uint32_t (*txBytesFree)(const displayPort_t *displayPort); textAttributes_t (*supportedTextAttributes)(const displayPort_t *displayPort); + bool (*getFontMetadata)(displayFontMetadata_t *metadata, const displayPort_t *displayPort); } displayPortVTable_t; typedef struct displayPortProfile_s { @@ -104,11 +111,12 @@ int displayScreenSize(const displayPort_t *instance); void displaySetXY(displayPort_t *instance, uint8_t x, uint8_t y); int displayWrite(displayPort_t *instance, uint8_t x, uint8_t y, const char *s); int displayWriteWithAttr(displayPort_t *instance, uint8_t x, uint8_t y, const char *s, textAttributes_t attr); -int displayWriteChar(displayPort_t *instance, uint8_t x, uint8_t y, uint8_t c); -int displayWriteCharWithAttr(displayPort_t *instance, uint8_t x, uint8_t y, uint8_t c, textAttributes_t attr); -bool displayReadCharWithAttr(displayPort_t *instance, uint8_t x, uint8_t y, uint8_t *c, textAttributes_t *attr); +int displayWriteChar(displayPort_t *instance, uint8_t x, uint8_t y, uint16_t c); +int displayWriteCharWithAttr(displayPort_t *instance, uint8_t x, uint8_t y, uint16_t c, textAttributes_t attr); +bool displayReadCharWithAttr(displayPort_t *instance, uint8_t x, uint8_t y, uint16_t *c, textAttributes_t *attr); bool displayIsTransferInProgress(const displayPort_t *instance); void displayHeartbeat(displayPort_t *instance); void displayResync(displayPort_t *instance); uint16_t displayTxBytesFree(const displayPort_t *instance); +bool displayGetFontMetadata(displayFontMetadata_t *metadata, const displayPort_t *instance); void displayInit(displayPort_t *instance, const displayPortVTable_t *vTable); diff --git a/src/main/drivers/dma.c b/src/main/drivers/dma.c deleted file mode 100644 index a830efd0588..00000000000 --- a/src/main/drivers/dma.c +++ /dev/null @@ -1,108 +0,0 @@ -/* - * This file is part of Cleanflight. - * - * Cleanflight is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * Cleanflight is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with Cleanflight. If not, see . - */ - -#include -#include -#include - -#include - -#include "drivers/nvic.h" -#include "dma.h" - -/* - * DMA descriptors. - */ -static dmaChannelDescriptor_t dmaDescriptors[] = { - DEFINE_DMA_CHANNEL(DMA1, DMA1_Channel1, 0, DMA1_Channel1_IRQn, RCC_AHBPeriph_DMA1), - DEFINE_DMA_CHANNEL(DMA1, DMA1_Channel2, 4, DMA1_Channel2_IRQn, RCC_AHBPeriph_DMA1), - DEFINE_DMA_CHANNEL(DMA1, DMA1_Channel3, 8, DMA1_Channel3_IRQn, RCC_AHBPeriph_DMA1), - DEFINE_DMA_CHANNEL(DMA1, DMA1_Channel4, 12, DMA1_Channel4_IRQn, RCC_AHBPeriph_DMA1), - DEFINE_DMA_CHANNEL(DMA1, DMA1_Channel5, 16, DMA1_Channel5_IRQn, RCC_AHBPeriph_DMA1), - DEFINE_DMA_CHANNEL(DMA1, DMA1_Channel6, 20, DMA1_Channel6_IRQn, RCC_AHBPeriph_DMA1), - DEFINE_DMA_CHANNEL(DMA1, DMA1_Channel7, 24, DMA1_Channel7_IRQn, RCC_AHBPeriph_DMA1), -#if defined(STM32F3) - DEFINE_DMA_CHANNEL(DMA2, DMA2_Channel1, 0, DMA2_Channel1_IRQn, RCC_AHBPeriph_DMA2), - DEFINE_DMA_CHANNEL(DMA2, DMA2_Channel2, 4, DMA2_Channel2_IRQn, RCC_AHBPeriph_DMA2), - DEFINE_DMA_CHANNEL(DMA2, DMA2_Channel3, 8, DMA2_Channel3_IRQn, RCC_AHBPeriph_DMA2), - DEFINE_DMA_CHANNEL(DMA2, DMA2_Channel4, 12, DMA2_Channel4_IRQn, RCC_AHBPeriph_DMA2), - DEFINE_DMA_CHANNEL(DMA2, DMA2_Channel5, 16, DMA2_Channel5_IRQn, RCC_AHBPeriph_DMA2), -#endif -}; - -/* - * DMA IRQ Handlers - */ -DEFINE_DMA_IRQ_HANDLER(1, 1, DMA1_CH1_HANDLER) -DEFINE_DMA_IRQ_HANDLER(1, 2, DMA1_CH2_HANDLER) -DEFINE_DMA_IRQ_HANDLER(1, 3, DMA1_CH3_HANDLER) -DEFINE_DMA_IRQ_HANDLER(1, 4, DMA1_CH4_HANDLER) -DEFINE_DMA_IRQ_HANDLER(1, 5, DMA1_CH5_HANDLER) -DEFINE_DMA_IRQ_HANDLER(1, 6, DMA1_CH6_HANDLER) -DEFINE_DMA_IRQ_HANDLER(1, 7, DMA1_CH7_HANDLER) - -#if defined(STM32F3) -DEFINE_DMA_IRQ_HANDLER(2, 1, DMA2_CH1_HANDLER) -DEFINE_DMA_IRQ_HANDLER(2, 2, DMA2_CH2_HANDLER) -DEFINE_DMA_IRQ_HANDLER(2, 3, DMA2_CH3_HANDLER) -DEFINE_DMA_IRQ_HANDLER(2, 4, DMA2_CH4_HANDLER) -DEFINE_DMA_IRQ_HANDLER(2, 5, DMA2_CH5_HANDLER) -#endif - - -void dmaInit(dmaHandlerIdentifier_e identifier, resourceOwner_e owner, uint8_t resourceIndex) -{ - RCC_AHBPeriphClockCmd(dmaDescriptors[identifier].rcc, ENABLE); - dmaDescriptors[identifier].owner = owner; - dmaDescriptors[identifier].resourceIndex = resourceIndex; -} - -dmaHandlerIdentifier_e dmaFindHandlerIdentifier(DMA_Channel_TypeDef* channel) -{ - dmaHandlerIdentifier_e i; - - for (i = 0; i < (sizeof(dmaDescriptors) / sizeof(dmaDescriptors[0])); i++) { - if (channel == dmaDescriptors[i].ref) { - return i; - } - } - - // Shouldn't get here - return 0; -} - -void dmaEnableClock(dmaHandlerIdentifier_e identifier) -{ - RCC_AHBPeriphClockCmd(dmaDescriptors[identifier].rcc, ENABLE); -} - -void dmaSetHandler(dmaHandlerIdentifier_e identifier, dmaCallbackHandlerFuncPtr callback, uint32_t priority, uint32_t userParam) -{ - NVIC_InitTypeDef NVIC_InitStructure; - - dmaEnableClock(identifier); - - dmaDescriptors[identifier].irqHandlerCallback = callback; - dmaDescriptors[identifier].userParam = userParam; - - NVIC_InitStructure.NVIC_IRQChannel = dmaDescriptors[identifier].irqN; - NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(priority); - NVIC_InitStructure.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(priority); - NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; - NVIC_Init(&NVIC_InitStructure); -} - diff --git a/src/main/drivers/dma.h b/src/main/drivers/dma.h index 1adc3182c96..5fe1f7b6fa5 100644 --- a/src/main/drivers/dma.h +++ b/src/main/drivers/dma.h @@ -15,49 +15,61 @@ * along with Cleanflight. If not, see . */ +#pragma once + #include "resource.h" struct dmaChannelDescriptor_s; -typedef void (*dmaCallbackHandlerFuncPtr)(struct dmaChannelDescriptor_s *channelDescriptor); + +typedef uint16_t dmaTag_t; // Packed DMA adapter/channel/stream +typedef struct dmaChannelDescriptor_s * DMA_t; + +#if defined(UNIT_TEST) +typedef uint32_t DMA_TypeDef; +#endif + +#define DMA_TAG(dma, stream, channel) ( (((dma) & 0x03) << 7) | (((stream) & 0x07) << 4) | (((channel) & 0x0F) << 0) ) +#define DMA_NONE (0) + +#define DMATAG_GET_DMA(x) ( ((x) >> 7) & 0x03 ) +#define DMATAG_GET_STREAM(x) ( ((x) >> 4) & 0x07 ) +#define DMATAG_GET_CHANNEL(x) ( ((x) >> 0) & 0x0F ) + +typedef void (*dmaCallbackHandlerFuncPtr)(DMA_t channelDescriptor); typedef struct dmaChannelDescriptor_s { + dmaTag_t tag; DMA_TypeDef* dma; -#if defined(STM32F4) || defined(STM32F7) +#if defined(STM32F4) + DMA_Stream_TypeDef* ref; +#elif defined(STM32F7) DMA_Stream_TypeDef* ref; + DMA_HandleTypeDef hdma; #else DMA_Channel_TypeDef* ref; #endif dmaCallbackHandlerFuncPtr irqHandlerCallback; - uint8_t flagsShift; - IRQn_Type irqN; + uint32_t flagsShift; + IRQn_Type irqNumber; uint32_t rcc; uint32_t userParam; resourceOwner_e owner; uint8_t resourceIndex; } dmaChannelDescriptor_t; -#if defined(STM32F4) || defined(STM32F7) +#if defined(STM32F4) + +#define DEFINE_DMA_CHANNEL(d, s, f) { \ + .tag = DMA_TAG(d, s, 0), \ + .dma = DMA##d, \ + .ref = DMA##d##_Stream##s, \ + .irqHandlerCallback = NULL, \ + .flagsShift = f, \ + .irqNumber = DMA##d##_Stream##s##_IRQn, \ + .rcc = RCC_AHB1Periph_DMA##d, \ + .userParam = 0 \ + } -typedef enum { - DMA1_ST0_HANDLER = 0, - DMA1_ST1_HANDLER, - DMA1_ST2_HANDLER, - DMA1_ST3_HANDLER, - DMA1_ST4_HANDLER, - DMA1_ST5_HANDLER, - DMA1_ST6_HANDLER, - DMA1_ST7_HANDLER, - DMA2_ST0_HANDLER, - DMA2_ST1_HANDLER, - DMA2_ST2_HANDLER, - DMA2_ST3_HANDLER, - DMA2_ST4_HANDLER, - DMA2_ST5_HANDLER, - DMA2_ST6_HANDLER, - DMA2_ST7_HANDLER, -} dmaHandlerIdentifier_e; - -#define DEFINE_DMA_CHANNEL(d, s, f, i, r) {.dma = d, .ref = s, .irqHandlerCallback = NULL, .flagsShift = f, .irqN = i, .rcc = r, .userParam = 0} #define DEFINE_DMA_IRQ_HANDLER(d, s, i) void DMA ## d ## _Stream ## s ## _IRQHandler(void) {\ if (dmaDescriptors[i].irqHandlerCallback)\ dmaDescriptors[i].irqHandlerCallback(&dmaDescriptors[i]);\ @@ -73,24 +85,47 @@ typedef enum { #define DMA_IT_DMEIF ((uint32_t)0x00000004) #define DMA_IT_FEIF ((uint32_t)0x00000001) -#else +#elif defined(STM32F7) + +#define DEFINE_DMA_CHANNEL(d, s, f) { \ + .tag = DMA_TAG(d, s, 0), \ + .dma = DMA##d, \ + .ref = DMA##d##_Stream##s, \ + .irqHandlerCallback = NULL, \ + .flagsShift = f, \ + .irqNumber = DMA##d##_Stream##s##_IRQn, \ + .rcc = RCC_AHB1ENR_DMA##d##EN, \ + .userParam = 0 \ + } + +#define DEFINE_DMA_IRQ_HANDLER(d, s, i) void DMA ## d ## _Stream ## s ## _IRQHandler(void) {\ + if (dmaDescriptors[i].irqHandlerCallback)\ + dmaDescriptors[i].irqHandlerCallback(&dmaDescriptors[i]);\ + } + +#define DMA_CLEAR_FLAG(d, flag) if (d->flagsShift > 31) d->dma->HIFCR = (flag << (d->flagsShift - 32)); else d->dma->LIFCR = (flag << d->flagsShift) +#define DMA_GET_FLAG_STATUS(d, flag) (d->flagsShift > 31 ? d->dma->HISR & (flag << (d->flagsShift - 32)): d->dma->LISR & (flag << d->flagsShift)) + + +#define DMA_IT_TCIF ((uint32_t)0x00000020) +#define DMA_IT_HTIF ((uint32_t)0x00000010) +#define DMA_IT_TEIF ((uint32_t)0x00000008) +#define DMA_IT_DMEIF ((uint32_t)0x00000004) +#define DMA_IT_FEIF ((uint32_t)0x00000001) + +#else // STM32F3 + +#define DEFINE_DMA_CHANNEL(d, c, f) { \ + .tag = DMA_TAG(d, 0, c), \ + .dma = DMA##d, \ + .ref = DMA##d##_Channel##c, \ + .irqHandlerCallback = NULL, \ + .flagsShift = f, \ + .irqNumber = DMA##d##_Channel##c##_IRQn, \ + .rcc = RCC_AHBPeriph_DMA##d, \ + .userParam = 0 \ + } -typedef enum { - DMA1_CH1_HANDLER = 0, - DMA1_CH2_HANDLER, - DMA1_CH3_HANDLER, - DMA1_CH4_HANDLER, - DMA1_CH5_HANDLER, - DMA1_CH6_HANDLER, - DMA1_CH7_HANDLER, - DMA2_CH1_HANDLER, - DMA2_CH2_HANDLER, - DMA2_CH3_HANDLER, - DMA2_CH4_HANDLER, - DMA2_CH5_HANDLER, -} dmaHandlerIdentifier_e; - -#define DEFINE_DMA_CHANNEL(d, c, f, i, r) {.dma = d, .ref = c, .irqHandlerCallback = NULL, .flagsShift = f, .irqN = i, .rcc = r, .userParam = 0} #define DEFINE_DMA_IRQ_HANDLER(d, c, i) void DMA ## d ## _Channel ## c ## _IRQHandler(void) {\ if (dmaDescriptors[i].irqHandlerCallback)\ dmaDescriptors[i].irqHandlerCallback(&dmaDescriptors[i]);\ @@ -105,18 +140,16 @@ typedef enum { #endif -#if defined(STM32F7) -//#define HAL_CLEANINVALIDATECACHE(addr, size) (SCB_CleanInvalidateDCache_by_Addr((uint32_t*)((uint32_t)addr & ~0x1f), ((uint32_t)(addr + size + 0x1f) & ~0x1f) - ((uint32_t)addr & ~0x1f))) -//#define HAL_CLEANCACHE(addr, size) (SCB_CleanDCache_by_Addr((uint32_t*)((uint32_t)addr & ~0x1f), ((uint32_t)(addr + size + 0x1f) & ~0x1f) - ((uint32_t)addr & ~0x1f))) -#endif - #if defined(STM32F4) || defined(STM32F7) -dmaHandlerIdentifier_e dmaFindHandlerIdentifier(DMA_Stream_TypeDef* stream); +DMA_t dmaFindHandlerIdentifier(DMA_Stream_TypeDef* stream); #else -dmaHandlerIdentifier_e dmaFindHandlerIdentifier(DMA_Channel_TypeDef* channel); +DMA_t dmaFindHandlerIdentifier(DMA_Channel_TypeDef* channel); #endif -void dmaInit(dmaHandlerIdentifier_e identifier, resourceOwner_e owner, uint8_t resourceIndex); -void dmaEnableClock(dmaHandlerIdentifier_e identifier); -void dmaSetHandler(dmaHandlerIdentifier_e identifier, dmaCallbackHandlerFuncPtr callback, uint32_t priority, uint32_t userParam); - +DMA_t dmaGetByTag(dmaTag_t tag); +uint32_t dmaGetChannelByTag(dmaTag_t tag); +resourceOwner_e dmaGetOwner(DMA_t dma); +void dmaInit(DMA_t dma, resourceOwner_e owner, uint8_t resourceIndex); +void dmaEnableClock(DMA_t dma); +void dmaSetHandler(DMA_t dma, dmaCallbackHandlerFuncPtr callback, uint32_t priority, uint32_t userParam); +void dmaCleanInterrupts(DMA_t dma); \ No newline at end of file diff --git a/src/main/drivers/dma_stm32f3xx.c b/src/main/drivers/dma_stm32f3xx.c new file mode 100644 index 00000000000..3d39b87958b --- /dev/null +++ b/src/main/drivers/dma_stm32f3xx.c @@ -0,0 +1,118 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include +#include +#include + +#include + +#include "build/debug.h" +#include "common/utils.h" +#include "drivers/nvic.h" +#include "drivers/dma.h" + +/* + * DMA descriptors. + */ +static dmaChannelDescriptor_t dmaDescriptors[] = { + [0] = DEFINE_DMA_CHANNEL(1, 1, 0), // DMA1_CH1 + [1] = DEFINE_DMA_CHANNEL(1, 2, 4), // DMA1_CH2 + [2] = DEFINE_DMA_CHANNEL(1, 3, 8), // DMA1_CH3 + [3] = DEFINE_DMA_CHANNEL(1, 4, 12), // DMA1_CH4 + [4] = DEFINE_DMA_CHANNEL(1, 5, 16), // DMA1_CH5 + [5] = DEFINE_DMA_CHANNEL(1, 6, 20), // DMA1_CH6 + [6] = DEFINE_DMA_CHANNEL(1, 7, 24), // DMA1_CH7 + + [7] = DEFINE_DMA_CHANNEL(2, 1, 0), // DMA2_CH1 + [8] = DEFINE_DMA_CHANNEL(2, 2, 4), // DMA2_CH2 + [9] = DEFINE_DMA_CHANNEL(2, 3, 8), // DMA2_CH3 + [10] = DEFINE_DMA_CHANNEL(2, 4, 12), // DMA2_CH4 + [11] = DEFINE_DMA_CHANNEL(2, 5, 16), // DMA2_CH5 +}; + +/* + * DMA IRQ Handlers + */ +DEFINE_DMA_IRQ_HANDLER(1, 1, 0) // // DMA1_CH1 = dmaDescriptors[0] +DEFINE_DMA_IRQ_HANDLER(1, 2, 1) +DEFINE_DMA_IRQ_HANDLER(1, 3, 2) +DEFINE_DMA_IRQ_HANDLER(1, 4, 3) +DEFINE_DMA_IRQ_HANDLER(1, 5, 4) +DEFINE_DMA_IRQ_HANDLER(1, 6, 5) +DEFINE_DMA_IRQ_HANDLER(1, 7, 6) +DEFINE_DMA_IRQ_HANDLER(2, 1, 7) +DEFINE_DMA_IRQ_HANDLER(2, 2, 8) +DEFINE_DMA_IRQ_HANDLER(2, 3, 9) +DEFINE_DMA_IRQ_HANDLER(2, 4, 10) +DEFINE_DMA_IRQ_HANDLER(2, 5, 11) + +DMA_t dmaGetByTag(dmaTag_t tag) +{ + for (unsigned i = 0; i < ARRAYLEN(dmaDescriptors); i++) { + // On F3 we match DMA and Channel, stream not used + if (DMATAG_GET_DMA(dmaDescriptors[i].tag) == DMATAG_GET_DMA(tag) && DMATAG_GET_CHANNEL(dmaDescriptors[i].tag) == DMATAG_GET_CHANNEL(tag)) { + return (DMA_t)&dmaDescriptors[i]; + } + } + + return (DMA_t) NULL; +} + +void dmaEnableClock(DMA_t dma) +{ + RCC_AHBPeriphClockCmd(dma->rcc, ENABLE); +} + +resourceOwner_e dmaGetOwner(DMA_t dma) +{ + return dma->owner; +} + +void dmaInit(DMA_t dma, resourceOwner_e owner, uint8_t resourceIndex) +{ + dmaEnableClock(dma); + dma->owner = owner; + dma->resourceIndex = resourceIndex; +} + +void dmaSetHandler(DMA_t dma, dmaCallbackHandlerFuncPtr callback, uint32_t priority, uint32_t userParam) +{ + NVIC_InitTypeDef NVIC_InitStructure; + + dmaEnableClock(dma); + + dma->irqHandlerCallback = callback; + dma->userParam = userParam; + + NVIC_InitStructure.NVIC_IRQChannel = dma->irqNumber; + NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(priority); + NVIC_InitStructure.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(priority); + NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; + NVIC_Init(&NVIC_InitStructure); +} + +DMA_t dmaFindHandlerIdentifier(DMA_Channel_TypeDef* channel) +{ + for (unsigned i = 0; i < (sizeof(dmaDescriptors) / sizeof(dmaDescriptors[0])); i++) { + if (channel == dmaDescriptors[i].ref) { + return &dmaDescriptors[i]; + } + } + + return NULL; +} diff --git a/src/main/drivers/dma_stm32f4xx.c b/src/main/drivers/dma_stm32f4xx.c index 259fe942a25..f806685bdc9 100644 --- a/src/main/drivers/dma_stm32f4xx.c +++ b/src/main/drivers/dma_stm32f4xx.c @@ -21,91 +21,112 @@ #include +#include "build/debug.h" +#include "common/utils.h" #include "drivers/nvic.h" -#include "dma.h" +#include "drivers/dma.h" /* * DMA descriptors. */ static dmaChannelDescriptor_t dmaDescriptors[] = { - DEFINE_DMA_CHANNEL(DMA1, DMA1_Stream0, 0, DMA1_Stream0_IRQn, RCC_AHB1Periph_DMA1), - DEFINE_DMA_CHANNEL(DMA1, DMA1_Stream1, 6, DMA1_Stream1_IRQn, RCC_AHB1Periph_DMA1), - DEFINE_DMA_CHANNEL(DMA1, DMA1_Stream2, 16, DMA1_Stream2_IRQn, RCC_AHB1Periph_DMA1), - DEFINE_DMA_CHANNEL(DMA1, DMA1_Stream3, 22, DMA1_Stream3_IRQn, RCC_AHB1Periph_DMA1), - DEFINE_DMA_CHANNEL(DMA1, DMA1_Stream4, 32, DMA1_Stream4_IRQn, RCC_AHB1Periph_DMA1), - DEFINE_DMA_CHANNEL(DMA1, DMA1_Stream5, 38, DMA1_Stream5_IRQn, RCC_AHB1Periph_DMA1), - DEFINE_DMA_CHANNEL(DMA1, DMA1_Stream6, 48, DMA1_Stream6_IRQn, RCC_AHB1Periph_DMA1), - DEFINE_DMA_CHANNEL(DMA1, DMA1_Stream7, 54, DMA1_Stream7_IRQn, RCC_AHB1Periph_DMA1), - - DEFINE_DMA_CHANNEL(DMA2, DMA2_Stream0, 0, DMA2_Stream0_IRQn, RCC_AHB1Periph_DMA2), - DEFINE_DMA_CHANNEL(DMA2, DMA2_Stream1, 6, DMA2_Stream1_IRQn, RCC_AHB1Periph_DMA2), - DEFINE_DMA_CHANNEL(DMA2, DMA2_Stream2, 16, DMA2_Stream2_IRQn, RCC_AHB1Periph_DMA2), - DEFINE_DMA_CHANNEL(DMA2, DMA2_Stream3, 22, DMA2_Stream3_IRQn, RCC_AHB1Periph_DMA2), - DEFINE_DMA_CHANNEL(DMA2, DMA2_Stream4, 32, DMA2_Stream4_IRQn, RCC_AHB1Periph_DMA2), - DEFINE_DMA_CHANNEL(DMA2, DMA2_Stream5, 38, DMA2_Stream5_IRQn, RCC_AHB1Periph_DMA2), - DEFINE_DMA_CHANNEL(DMA2, DMA2_Stream6, 48, DMA2_Stream6_IRQn, RCC_AHB1Periph_DMA2), - DEFINE_DMA_CHANNEL(DMA2, DMA2_Stream7, 54, DMA2_Stream7_IRQn, RCC_AHB1Periph_DMA2), + [0] = DEFINE_DMA_CHANNEL(1, 0, 0), // DMA1_ST0 + [1] = DEFINE_DMA_CHANNEL(1, 1, 6), // DMA1_ST1 + [2] = DEFINE_DMA_CHANNEL(1, 2, 16), // DMA1_ST2 + [3] = DEFINE_DMA_CHANNEL(1, 3, 22), // DMA1_ST3 + [4] = DEFINE_DMA_CHANNEL(1, 4, 32), // DMA1_ST4 + [5] = DEFINE_DMA_CHANNEL(1, 5, 38), // DMA1_ST5 + [6] = DEFINE_DMA_CHANNEL(1, 6, 48), // DMA1_ST6 + [7] = DEFINE_DMA_CHANNEL(1, 7, 54), // DMA1_ST7 + + [8] = DEFINE_DMA_CHANNEL(2, 0, 0), // DMA2_ST0 + [9] = DEFINE_DMA_CHANNEL(2, 1, 6), // DMA2_ST1 + [10] = DEFINE_DMA_CHANNEL(2, 2, 16), // DMA2_ST2 + [11] = DEFINE_DMA_CHANNEL(2, 3, 22), // DMA2_ST3 + [12] = DEFINE_DMA_CHANNEL(2, 4, 32), // DMA2_ST4 + [13] = DEFINE_DMA_CHANNEL(2, 5, 38), // DMA2_ST5 + [14] = DEFINE_DMA_CHANNEL(2, 6, 48), // DMA2_ST6 + [15] = DEFINE_DMA_CHANNEL(2, 7, 54) // DMA2_ST7 }; /* * DMA IRQ Handlers */ -DEFINE_DMA_IRQ_HANDLER(1, 0, DMA1_ST0_HANDLER) -DEFINE_DMA_IRQ_HANDLER(1, 1, DMA1_ST1_HANDLER) -DEFINE_DMA_IRQ_HANDLER(1, 2, DMA1_ST2_HANDLER) -DEFINE_DMA_IRQ_HANDLER(1, 3, DMA1_ST3_HANDLER) -DEFINE_DMA_IRQ_HANDLER(1, 4, DMA1_ST4_HANDLER) -DEFINE_DMA_IRQ_HANDLER(1, 5, DMA1_ST5_HANDLER) -DEFINE_DMA_IRQ_HANDLER(1, 6, DMA1_ST6_HANDLER) -DEFINE_DMA_IRQ_HANDLER(1, 7, DMA1_ST7_HANDLER) -DEFINE_DMA_IRQ_HANDLER(2, 0, DMA2_ST0_HANDLER) -DEFINE_DMA_IRQ_HANDLER(2, 1, DMA2_ST1_HANDLER) -DEFINE_DMA_IRQ_HANDLER(2, 2, DMA2_ST2_HANDLER) -DEFINE_DMA_IRQ_HANDLER(2, 3, DMA2_ST3_HANDLER) -DEFINE_DMA_IRQ_HANDLER(2, 4, DMA2_ST4_HANDLER) -DEFINE_DMA_IRQ_HANDLER(2, 5, DMA2_ST5_HANDLER) -DEFINE_DMA_IRQ_HANDLER(2, 6, DMA2_ST6_HANDLER) -DEFINE_DMA_IRQ_HANDLER(2, 7, DMA2_ST7_HANDLER) - -void dmaInit(dmaHandlerIdentifier_e identifier, resourceOwner_e owner, uint8_t resourceIndex) +DEFINE_DMA_IRQ_HANDLER(1, 0, 0) // DMA1_ST0 = dmaDescriptors[0] +DEFINE_DMA_IRQ_HANDLER(1, 1, 1) +DEFINE_DMA_IRQ_HANDLER(1, 2, 2) +DEFINE_DMA_IRQ_HANDLER(1, 3, 3) +DEFINE_DMA_IRQ_HANDLER(1, 4, 4) +DEFINE_DMA_IRQ_HANDLER(1, 5, 5) +DEFINE_DMA_IRQ_HANDLER(1, 6, 6) +DEFINE_DMA_IRQ_HANDLER(1, 7, 7) +DEFINE_DMA_IRQ_HANDLER(2, 0, 8) +DEFINE_DMA_IRQ_HANDLER(2, 1, 9) +DEFINE_DMA_IRQ_HANDLER(2, 2, 10) +DEFINE_DMA_IRQ_HANDLER(2, 3, 11) +DEFINE_DMA_IRQ_HANDLER(2, 4, 12) +DEFINE_DMA_IRQ_HANDLER(2, 5, 13) +DEFINE_DMA_IRQ_HANDLER(2, 6, 14) +DEFINE_DMA_IRQ_HANDLER(2, 7, 15) + +DMA_t dmaGetByTag(dmaTag_t tag) { - RCC_AHB1PeriphClockCmd(dmaDescriptors[identifier].rcc, ENABLE); - dmaDescriptors[identifier].owner = owner; - dmaDescriptors[identifier].resourceIndex = resourceIndex; + for (unsigned i = 0; i < ARRAYLEN(dmaDescriptors); i++) { + // On F4/F7 we match only DMA and Stream. Channel is needed when connecting DMA to peripheral + if (DMATAG_GET_DMA(dmaDescriptors[i].tag) == DMATAG_GET_DMA(tag) && DMATAG_GET_STREAM(dmaDescriptors[i].tag) == DMATAG_GET_STREAM(tag)) { + return (DMA_t)&dmaDescriptors[i]; + } + } + + return (DMA_t) NULL; } -dmaHandlerIdentifier_e dmaFindHandlerIdentifier(DMA_Stream_TypeDef* stream) +void dmaEnableClock(DMA_t dma) { - dmaHandlerIdentifier_e i; - - for (i = 0; i < (sizeof(dmaDescriptors) / sizeof(dmaDescriptors[0])); i++) { - if (stream == dmaDescriptors[i].ref) { - return i; - } - } + RCC_AHB1PeriphClockCmd(dma->rcc, ENABLE); +} - // Shouldn't get here - return 0; +resourceOwner_e dmaGetOwner(DMA_t dma) +{ + return dma->owner; } -void dmaEnableClock(dmaHandlerIdentifier_e identifier) +void dmaInit(DMA_t dma, resourceOwner_e owner, uint8_t resourceIndex) { - RCC_AHB1PeriphClockCmd(dmaDescriptors[identifier].rcc, ENABLE); + dmaEnableClock(dma); + dma->owner = owner; + dma->resourceIndex = resourceIndex; } -void dmaSetHandler(dmaHandlerIdentifier_e identifier, dmaCallbackHandlerFuncPtr callback, uint32_t priority, uint32_t userParam) +void dmaSetHandler(DMA_t dma, dmaCallbackHandlerFuncPtr callback, uint32_t priority, uint32_t userParam) { NVIC_InitTypeDef NVIC_InitStructure; - dmaEnableClock(identifier); + dmaEnableClock(dma); - dmaDescriptors[identifier].irqHandlerCallback = callback; - dmaDescriptors[identifier].userParam = userParam; + dma->irqHandlerCallback = callback; + dma->userParam = userParam; - NVIC_InitStructure.NVIC_IRQChannel = dmaDescriptors[identifier].irqN; + NVIC_InitStructure.NVIC_IRQChannel = dma->irqNumber; NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(priority); NVIC_InitStructure.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(priority); NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; NVIC_Init(&NVIC_InitStructure); } +uint32_t dmaGetChannelByTag(dmaTag_t tag) +{ + static const uint32_t dmaChannel[8] = { DMA_Channel_0, DMA_Channel_1, DMA_Channel_2, DMA_Channel_3, DMA_Channel_4, DMA_Channel_5, DMA_Channel_6, DMA_Channel_7 }; + return dmaChannel[DMATAG_GET_CHANNEL(tag)]; +} + +DMA_t dmaFindHandlerIdentifier(DMA_Stream_TypeDef * stream) +{ + for (unsigned i = 0; i < (sizeof(dmaDescriptors) / sizeof(dmaDescriptors[0])); i++) { + if (stream == dmaDescriptors[i].ref) { + return &dmaDescriptors[i]; + } + } + + return NULL; +} diff --git a/src/main/drivers/dma_stm32f7xx.c b/src/main/drivers/dma_stm32f7xx.c index baee3efae5c..ff87842d3e2 100644 --- a/src/main/drivers/dma_stm32f7xx.c +++ b/src/main/drivers/dma_stm32f7xx.c @@ -21,6 +21,7 @@ #include +#include "common/utils.h" #include "drivers/nvic.h" #include "drivers/dma.h" @@ -28,82 +29,93 @@ * DMA descriptors. */ static dmaChannelDescriptor_t dmaDescriptors[] = { - DEFINE_DMA_CHANNEL(DMA1, DMA1_Stream0, 0, DMA1_Stream0_IRQn, RCC_AHB1ENR_DMA1EN), - DEFINE_DMA_CHANNEL(DMA1, DMA1_Stream1, 6, DMA1_Stream1_IRQn, RCC_AHB1ENR_DMA1EN), - DEFINE_DMA_CHANNEL(DMA1, DMA1_Stream2, 16, DMA1_Stream2_IRQn, RCC_AHB1ENR_DMA1EN), - DEFINE_DMA_CHANNEL(DMA1, DMA1_Stream3, 22, DMA1_Stream3_IRQn, RCC_AHB1ENR_DMA1EN), - DEFINE_DMA_CHANNEL(DMA1, DMA1_Stream4, 32, DMA1_Stream4_IRQn, RCC_AHB1ENR_DMA1EN), - DEFINE_DMA_CHANNEL(DMA1, DMA1_Stream5, 38, DMA1_Stream5_IRQn, RCC_AHB1ENR_DMA1EN), - DEFINE_DMA_CHANNEL(DMA1, DMA1_Stream6, 48, DMA1_Stream6_IRQn, RCC_AHB1ENR_DMA1EN), - DEFINE_DMA_CHANNEL(DMA1, DMA1_Stream7, 54, DMA1_Stream7_IRQn, RCC_AHB1ENR_DMA1EN), - - DEFINE_DMA_CHANNEL(DMA2, DMA2_Stream0, 0, DMA2_Stream0_IRQn, RCC_AHB1ENR_DMA2EN), - DEFINE_DMA_CHANNEL(DMA2, DMA2_Stream1, 6, DMA2_Stream1_IRQn, RCC_AHB1ENR_DMA2EN), - DEFINE_DMA_CHANNEL(DMA2, DMA2_Stream2, 16, DMA2_Stream2_IRQn, RCC_AHB1ENR_DMA2EN), - DEFINE_DMA_CHANNEL(DMA2, DMA2_Stream3, 22, DMA2_Stream3_IRQn, RCC_AHB1ENR_DMA2EN), - DEFINE_DMA_CHANNEL(DMA2, DMA2_Stream4, 32, DMA2_Stream4_IRQn, RCC_AHB1ENR_DMA2EN), - DEFINE_DMA_CHANNEL(DMA2, DMA2_Stream5, 38, DMA2_Stream5_IRQn, RCC_AHB1ENR_DMA2EN), - DEFINE_DMA_CHANNEL(DMA2, DMA2_Stream6, 48, DMA2_Stream6_IRQn, RCC_AHB1ENR_DMA2EN), - DEFINE_DMA_CHANNEL(DMA2, DMA2_Stream7, 54, DMA2_Stream7_IRQn, RCC_AHB1ENR_DMA2EN), + [0] = DEFINE_DMA_CHANNEL(1, 0, 0), // DMA1_ST0 + [1] = DEFINE_DMA_CHANNEL(1, 1, 6), // DMA1_ST1 + [2] = DEFINE_DMA_CHANNEL(1, 2, 16), // DMA1_ST2 + [3] = DEFINE_DMA_CHANNEL(1, 3, 22), // DMA1_ST3 + [4] = DEFINE_DMA_CHANNEL(1, 4, 32), // DMA1_ST4 + [5] = DEFINE_DMA_CHANNEL(1, 5, 38), // DMA1_ST5 + [6] = DEFINE_DMA_CHANNEL(1, 6, 48), // DMA1_ST6 + [7] = DEFINE_DMA_CHANNEL(1, 7, 54), // DMA1_ST7 + [8] = DEFINE_DMA_CHANNEL(2, 0, 0), // DMA2_ST0 + [9] = DEFINE_DMA_CHANNEL(2, 1, 6), // DMA2_ST1 + [10] = DEFINE_DMA_CHANNEL(2, 2, 16), // DMA2_ST2 + [11] = DEFINE_DMA_CHANNEL(2, 3, 22), // DMA2_ST3 + [12] = DEFINE_DMA_CHANNEL(2, 4, 32), // DMA2_ST4 + [13] = DEFINE_DMA_CHANNEL(2, 5, 38), // DMA2_ST5 + [14] = DEFINE_DMA_CHANNEL(2, 6, 48), // DMA2_ST6 + [15] = DEFINE_DMA_CHANNEL(2, 7, 54) // DMA2_ST7 }; /* * DMA IRQ Handlers */ -DEFINE_DMA_IRQ_HANDLER(1, 0, DMA1_ST0_HANDLER) -DEFINE_DMA_IRQ_HANDLER(1, 1, DMA1_ST1_HANDLER) -DEFINE_DMA_IRQ_HANDLER(1, 2, DMA1_ST2_HANDLER) -DEFINE_DMA_IRQ_HANDLER(1, 3, DMA1_ST3_HANDLER) -DEFINE_DMA_IRQ_HANDLER(1, 4, DMA1_ST4_HANDLER) -DEFINE_DMA_IRQ_HANDLER(1, 5, DMA1_ST5_HANDLER) -DEFINE_DMA_IRQ_HANDLER(1, 6, DMA1_ST6_HANDLER) -DEFINE_DMA_IRQ_HANDLER(1, 7, DMA1_ST7_HANDLER) -DEFINE_DMA_IRQ_HANDLER(2, 0, DMA2_ST0_HANDLER) -DEFINE_DMA_IRQ_HANDLER(2, 1, DMA2_ST1_HANDLER) -DEFINE_DMA_IRQ_HANDLER(2, 2, DMA2_ST2_HANDLER) -DEFINE_DMA_IRQ_HANDLER(2, 3, DMA2_ST3_HANDLER) -DEFINE_DMA_IRQ_HANDLER(2, 4, DMA2_ST4_HANDLER) -DEFINE_DMA_IRQ_HANDLER(2, 5, DMA2_ST5_HANDLER) -DEFINE_DMA_IRQ_HANDLER(2, 6, DMA2_ST6_HANDLER) -DEFINE_DMA_IRQ_HANDLER(2, 7, DMA2_ST7_HANDLER) +DEFINE_DMA_IRQ_HANDLER(1, 0, 0) // DMA1_ST0 = dmaDescriptors[0] +DEFINE_DMA_IRQ_HANDLER(1, 1, 1) +DEFINE_DMA_IRQ_HANDLER(1, 2, 2) +DEFINE_DMA_IRQ_HANDLER(1, 3, 3) +DEFINE_DMA_IRQ_HANDLER(1, 4, 4) +DEFINE_DMA_IRQ_HANDLER(1, 5, 5) +DEFINE_DMA_IRQ_HANDLER(1, 6, 6) +DEFINE_DMA_IRQ_HANDLER(1, 7, 7) +DEFINE_DMA_IRQ_HANDLER(2, 0, 8) +DEFINE_DMA_IRQ_HANDLER(2, 1, 9) +DEFINE_DMA_IRQ_HANDLER(2, 2, 10) +DEFINE_DMA_IRQ_HANDLER(2, 3, 11) +DEFINE_DMA_IRQ_HANDLER(2, 4, 12) +DEFINE_DMA_IRQ_HANDLER(2, 5, 13) +DEFINE_DMA_IRQ_HANDLER(2, 6, 14) +DEFINE_DMA_IRQ_HANDLER(2, 7, 15) + +DMA_t dmaGetByTag(dmaTag_t tag) +{ + for (unsigned i = 0; i < ARRAYLEN(dmaDescriptors); i++) { + // On F4/F7 we match only DMA and Stream. Channel is needed when connecting DMA to peripheral + if (DMATAG_GET_DMA(dmaDescriptors[i].tag) == DMATAG_GET_DMA(tag) && DMATAG_GET_STREAM(dmaDescriptors[i].tag) == DMATAG_GET_STREAM(tag)) { + return (DMA_t)&dmaDescriptors[i]; + } + } + + return (DMA_t) NULL; +} -static void enableDmaClock(uint32_t rcc) +void dmaEnableClock(DMA_t dma) { do { __IO uint32_t tmpreg; - SET_BIT(RCC->AHB1ENR, rcc); + SET_BIT(RCC->AHB1ENR, dma->rcc); /* Delay after an RCC peripheral clock enabling */ - tmpreg = READ_BIT(RCC->AHB1ENR, rcc); + tmpreg = READ_BIT(RCC->AHB1ENR, dma->rcc); UNUSED(tmpreg); } while (0); } -void dmaInit(dmaHandlerIdentifier_e identifier, resourceOwner_e owner, uint8_t resourceIndex) +resourceOwner_e dmaGetOwner(DMA_t dma) { - enableDmaClock(dmaDescriptors[identifier].rcc); - dmaDescriptors[identifier].owner = owner; - dmaDescriptors[identifier].resourceIndex = resourceIndex; + return dma->owner; } -void dmaSetHandler(dmaHandlerIdentifier_e identifier, dmaCallbackHandlerFuncPtr callback, uint32_t priority, uint32_t userParam) +void dmaInit(DMA_t dma, resourceOwner_e owner, uint8_t resourceIndex) { - //clock - //RCC_AHB1PeriphClockCmd(dmaDescriptors[identifier].rcc, ENABLE); - - do { - __IO uint32_t tmpreg; - SET_BIT(RCC->AHB1ENR, dmaDescriptors[identifier].rcc); - /* Delay after an RCC peripheral clock enabling */ - tmpreg = READ_BIT(RCC->AHB1ENR, dmaDescriptors[identifier].rcc); - UNUSED(tmpreg); - } while (0); + dmaEnableClock(dma); + dma->owner = owner; + dma->resourceIndex = resourceIndex; +} - dmaDescriptors[identifier].irqHandlerCallback = callback; - dmaDescriptors[identifier].userParam = userParam; +void dmaSetHandler(DMA_t dma, dmaCallbackHandlerFuncPtr callback, uint32_t priority, uint32_t userParam) +{ + dmaEnableClock(dma); + dma->irqHandlerCallback = callback; + dma->userParam = userParam; - HAL_NVIC_SetPriority(dmaDescriptors[identifier].irqN, NVIC_PRIORITY_BASE(priority), NVIC_PRIORITY_SUB(priority)); - HAL_NVIC_EnableIRQ(dmaDescriptors[identifier].irqN); + HAL_NVIC_SetPriority(dma->irqNumber, NVIC_PRIORITY_BASE(priority), NVIC_PRIORITY_SUB(priority)); + HAL_NVIC_EnableIRQ(dma->irqNumber); } +uint32_t dmaGetChannelByTag(dmaTag_t tag) +{ + static const uint32_t dmaChannel[8] = { DMA_CHANNEL_0, DMA_CHANNEL_1, DMA_CHANNEL_2, DMA_CHANNEL_3, DMA_CHANNEL_4, DMA_CHANNEL_5, DMA_CHANNEL_6, DMA_CHANNEL_7 }; + return dmaChannel[DMATAG_GET_CHANNEL(tag)]; +} diff --git a/src/main/drivers/io.h b/src/main/drivers/io.h index 0cc43017eff..bc888ccef9c 100644 --- a/src/main/drivers/io.h +++ b/src/main/drivers/io.h @@ -41,6 +41,7 @@ #define IOCFG_OUT_PP IO_CONFIG(GPIO_MODE_OUTPUT_PP, GPIO_SPEED_FREQ_LOW, GPIO_NOPULL) #define IOCFG_OUT_PP_25 IO_CONFIG(GPIO_MODE_OUTPUT_PP, GPIO_SPEED_FREQ_HIGH, GPIO_NOPULL) #define IOCFG_OUT_OD IO_CONFIG(GPIO_MODE_OUTPUT_OD, GPIO_SPEED_FREQ_LOW, GPIO_NOPULL) +#define IOCFG_AF_PP_FAST IO_CONFIG(GPIO_MODE_AF_PP, GPIO_SPEED_FREQ_VERY_HIGH, GPIO_PULLDOWN) #define IOCFG_AF_PP IO_CONFIG(GPIO_MODE_AF_PP, GPIO_SPEED_FREQ_LOW, GPIO_NOPULL) #define IOCFG_AF_PP_PD IO_CONFIG(GPIO_MODE_AF_PP, GPIO_SPEED_FREQ_LOW, GPIO_PULLDOWN) #define IOCFG_AF_PP_UP IO_CONFIG(GPIO_MODE_AF_PP, GPIO_SPEED_FREQ_LOW, GPIO_PULLUP) @@ -57,6 +58,7 @@ #define IOCFG_OUT_PP IO_CONFIG(GPIO_Mode_OUT, 0, GPIO_OType_PP, GPIO_PuPd_NOPULL) // TODO #define IOCFG_OUT_PP_25 IO_CONFIG(GPIO_Mode_OUT, GPIO_Speed_25MHz, GPIO_OType_PP, GPIO_PuPd_NOPULL) #define IOCFG_OUT_OD IO_CONFIG(GPIO_Mode_OUT, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL) +#define IOCFG_AF_PP_FAST IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, GPIO_OType_PP, GPIO_PuPd_DOWN) #define IOCFG_AF_PP IO_CONFIG(GPIO_Mode_AF, 0, GPIO_OType_PP, GPIO_PuPd_NOPULL) #define IOCFG_AF_PP_PD IO_CONFIG(GPIO_Mode_AF, 0, GPIO_OType_PP, GPIO_PuPd_DOWN) #define IOCFG_AF_PP_UP IO_CONFIG(GPIO_Mode_AF, 0, GPIO_OType_PP, GPIO_PuPd_UP) diff --git a/src/main/drivers/light_ws2811strip.c b/src/main/drivers/light_ws2811strip.c index cbf0ac93e83..d91d5d5bc5d 100644 --- a/src/main/drivers/light_ws2811strip.c +++ b/src/main/drivers/light_ws2811strip.c @@ -33,22 +33,23 @@ #ifdef USE_LED_STRIP #include "build/build_config.h" +#include "build/debug.h" #include "common/color.h" #include "common/colorconversion.h" -#include "dma.h" + +#include "drivers/dma.h" #include "drivers/io.h" -#include "light_ws2811strip.h" +#include "drivers/timer.h" +#include "drivers/light_ws2811strip.h" -#if defined(STM32F4) || defined(STM32F7) -uint32_t ledStripDMABuffer[WS2811_DMA_BUFFER_SIZE]; -#else -uint8_t ledStripDMABuffer[WS2811_DMA_BUFFER_SIZE]; -#endif -volatile uint8_t ws2811LedDataTransferInProgress = 0; +static uint32_t ledStripDMABuffer[WS2811_DMA_BUFFER_SIZE]; +static IO_t ws2811IO = IO_NONE; +static TCH_t * ws2811TCH = NULL; +static bool ws2811Initialised = false; -uint16_t BIT_COMPARE_1 = 0; -uint16_t BIT_COMPARE_0 = 0; +static uint16_t BIT_COMPARE_1 = 0; +static uint16_t BIT_COMPARE_0 = 0; static hsvColor_t ledColorBuffer[WS2811_LED_STRIP_LENGTH]; @@ -90,22 +91,51 @@ void setStripColors(const hsvColor_t *colors) void ws2811LedStripInit(void) { + const timerHardware_t * timHw = timerGetByTag(IO_TAG(WS2811_PIN), TIM_USE_ANY); + + if (timHw == NULL) { + return; + } + + ws2811TCH = timerGetTCH(timHw); + if (ws2811TCH == NULL) { + return; + } + + /* Compute the prescaler value */ + uint16_t period = WS2811_TIMER_HZ / WS2811_CARRIER_HZ; + + BIT_COMPARE_1 = period * 2 / 3; + BIT_COMPARE_0 = period / 3; + + ws2811IO = IOGetByTag(IO_TAG(WS2811_PIN)); + IOInit(ws2811IO, OWNER_LED_STRIP, RESOURCE_OUTPUT, 0); + IOConfigGPIOAF(ws2811IO, IOCFG_AF_PP_FAST, timHw->alternateFunction); + + timerConfigBase(ws2811TCH, period, WS2811_TIMER_HZ); + timerPWMConfigChannel(ws2811TCH, 0); + + // If DMA failed - abort + if (!timerPWMConfigChannelDMA(ws2811TCH, ledStripDMABuffer, WS2811_DMA_BUFFER_SIZE)) { + ws2811Initialised = false; + return; + } + + // Zero out DMA buffer memset(&ledStripDMABuffer, 0, sizeof(ledStripDMABuffer)); - ws2811LedStripHardwareInit(); + ws2811Initialised = true; + ws2811UpdateStrip(); } bool isWS2811LedStripReady(void) { - return !ws2811LedDataTransferInProgress; + return !timerPWMDMAInProgress(ws2811TCH); } STATIC_UNIT_TESTED uint16_t dmaBufferOffset; static int16_t ledIndex; -#define USE_FAST_DMA_BUFFER_IMPL -#ifdef USE_FAST_DMA_BUFFER_IMPL - STATIC_UNIT_TESTED void fastUpdateLEDDMABuffer(rgbColor24bpp_t *color) { uint32_t grb = (color->rgb.g << 16) | (color->rgb.r << 8) | (color->rgb.b); @@ -114,25 +144,6 @@ STATIC_UNIT_TESTED void fastUpdateLEDDMABuffer(rgbColor24bpp_t *color) ledStripDMABuffer[dmaBufferOffset++] = (grb & (1 << index)) ? BIT_COMPARE_1 : BIT_COMPARE_0; } } -#else -STATIC_UNIT_TESTED void updateLEDDMABuffer(uint8_t componentValue) -{ - uint8_t bitIndex; - - for (bitIndex = 0; bitIndex < 8; bitIndex++) - { - if ((componentValue << bitIndex) & 0x80 ) // data sent MSB first, j = 0 is MSB j = 7 is LSB - { - ledStripDMABuffer[dmaBufferOffset] = BIT_COMPARE_1; - } - else - { - ledStripDMABuffer[dmaBufferOffset] = BIT_COMPARE_0; // compare value for logical 0 - } - dmaBufferOffset++; - } -} -#endif /* * This method is non-blocking unless an existing LED update is in progress. @@ -143,7 +154,7 @@ void ws2811UpdateStrip(void) static rgbColor24bpp_t *rgb24; // don't wait - risk of infinite block, just get an update next time round - if (ws2811LedDataTransferInProgress) { + if (timerPWMDMAInProgress(ws2811TCH)) { return; } @@ -155,20 +166,17 @@ void ws2811UpdateStrip(void) while (ledIndex < WS2811_LED_STRIP_LENGTH) { rgb24 = hsvToRgb24(&ledColorBuffer[ledIndex]); - -#ifdef USE_FAST_DMA_BUFFER_IMPL fastUpdateLEDDMABuffer(rgb24); -#else - updateLEDDMABuffer(rgb24->rgb.g); - updateLEDDMABuffer(rgb24->rgb.r); - updateLEDDMABuffer(rgb24->rgb.b); -#endif - ledIndex++; } - ws2811LedDataTransferInProgress = 1; - ws2811LedStripDMAEnable(); + // Initiate hardware transfer + if (!ws2811Initialised || !ws2811TCH) { + return; + } + + timerPWMPrepareDMA(ws2811TCH, WS2811_DMA_BUFFER_SIZE); + timerPWMStartDMA(ws2811TCH); } #endif diff --git a/src/main/drivers/light_ws2811strip.h b/src/main/drivers/light_ws2811strip.h index 762e704d7ad..85337a9d466 100644 --- a/src/main/drivers/light_ws2811strip.h +++ b/src/main/drivers/light_ws2811strip.h @@ -26,12 +26,13 @@ #define WS2811_DMA_BUFFER_SIZE (WS2811_DATA_BUFFER_SIZE + WS2811_DELAY_BUFFER_LENGTH) // number of bytes needed is #LEDs * 24 bytes + 42 trailing bytes) -#define WS2811_TIMER_MHZ 24 -#define WS2811_CARRIER_HZ 800000 +#define WS2811_TIMER_HZ 24000000 +#define WS2811_CARRIER_HZ 800000 void ws2811LedStripInit(void); void ws2811LedStripHardwareInit(void); void ws2811LedStripDMAEnable(void); +bool ws2811LedStripDMAInProgress(void); void ws2811UpdateStrip(void); @@ -45,13 +46,3 @@ void setStripColor(const hsvColor_t *color); void setStripColors(const hsvColor_t *colors); bool isWS2811LedStripReady(void); - -#if defined(STM32F4) || defined(STM32F7) -extern uint32_t ledStripDMABuffer[WS2811_DMA_BUFFER_SIZE]; -#else -extern uint8_t ledStripDMABuffer[WS2811_DMA_BUFFER_SIZE]; -#endif -extern volatile uint8_t ws2811LedDataTransferInProgress; - -extern uint16_t BIT_COMPARE_1; -extern uint16_t BIT_COMPARE_0; \ No newline at end of file diff --git a/src/main/drivers/light_ws2811strip_hal.c b/src/main/drivers/light_ws2811strip_hal.c deleted file mode 100644 index b6d5cc1b2a0..00000000000 --- a/src/main/drivers/light_ws2811strip_hal.c +++ /dev/null @@ -1,172 +0,0 @@ -/* - * This file is part of Cleanflight. - * - * Cleanflight is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * Cleanflight is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with Cleanflight. If not, see . - */ - -#include -#include - -#include "platform.h" - -#ifdef USE_LED_STRIP - -#include "common/color.h" -#include "light_ws2811strip.h" -#include "drivers/nvic.h" -#include "dma.h" -#include "drivers/io.h" -#include "drivers/system.h" -#include "rcc.h" -#include "timer.h" - -#if !defined(WS2811_PIN) -#define WS2811_PIN PA0 -#define WS2811_TIMER TIM5 -#define WS2811_DMA_HANDLER_IDENTIFER DMA1_ST2_HANDLER -#define WS2811_DMA_STREAM DMA1_Stream2 -#define WS2811_DMA_IT DMA_IT_TCIF2 -#define WS2811_DMA_CHANNEL DMA_Channel_6 -#define WS2811_TIMER_CHANNEL TIM_Channel_1 -#define WS2811_TIMER_GPIO_AF GPIO_AF2_TIM5 -#endif - -static IO_t ws2811IO = IO_NONE; -bool ws2811Initialised = false; - -static TIM_HandleTypeDef TimHandle; -static uint16_t timerChannel = 0; -static bool timerNChannel = false; - -void WS2811_DMA_IRQHandler(dmaChannelDescriptor_t* descriptor) -{ - HAL_DMA_IRQHandler(TimHandle.hdma[descriptor->userParam]); - if(timerNChannel) { - HAL_TIMEx_PWMN_Stop_DMA(&TimHandle,timerChannel); - } else { - HAL_TIM_PWM_Stop_DMA(&TimHandle,timerChannel); - } - ws2811LedDataTransferInProgress = 0; -} - -void ws2811LedStripHardwareInit(void) -{ - const timerHardware_t *timerHardware = timerGetByTag(IO_TAG(WS2811_PIN), TIM_USE_ANY); - TIM_TypeDef *timer = timerHardware->tim; - timerChannel = timerHardware->channel; - - if (timerHardware == NULL) { - return; - } - TimHandle.Instance = timer; - - /* Compute the prescaler value */ - uint16_t prescaler = timerGetPrescalerByDesiredMhz(timer, WS2811_TIMER_MHZ); - uint16_t period = 1000000 * WS2811_TIMER_MHZ / WS2811_CARRIER_HZ; - - BIT_COMPARE_1 = period / 3 * 2; - BIT_COMPARE_0 = period / 3; - - TimHandle.Init.Prescaler = prescaler; - TimHandle.Init.Period = period; // 800kHz - TimHandle.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; - TimHandle.Init.CounterMode = TIM_COUNTERMODE_UP; - if (HAL_TIM_PWM_Init(&TimHandle) != HAL_OK) { - /* Initialization Error */ - return; - } - - static DMA_HandleTypeDef hdma_tim; - - ws2811IO = IOGetByTag(IO_TAG(WS2811_PIN)); - IOInit(ws2811IO, OWNER_LED_STRIP, RESOURCE_OUTPUT, 0); - IOConfigGPIOAF(ws2811IO, IO_CONFIG(GPIO_MODE_AF_PP, GPIO_SPEED_FREQ_VERY_HIGH, GPIO_PULLDOWN), timerHardware->alternateFunction); - - __DMA1_CLK_ENABLE(); - - /* Set the parameters to be configured */ - hdma_tim.Init.Channel = WS2811_DMA_CHANNEL; - hdma_tim.Init.Direction = DMA_MEMORY_TO_PERIPH; - hdma_tim.Init.PeriphInc = DMA_PINC_DISABLE; - hdma_tim.Init.MemInc = DMA_MINC_ENABLE; - hdma_tim.Init.PeriphDataAlignment = DMA_PDATAALIGN_WORD; - hdma_tim.Init.MemDataAlignment = DMA_MDATAALIGN_WORD; - hdma_tim.Init.Mode = DMA_NORMAL; - hdma_tim.Init.Priority = DMA_PRIORITY_HIGH; - hdma_tim.Init.FIFOMode = DMA_FIFOMODE_DISABLE; - hdma_tim.Init.FIFOThreshold = DMA_FIFO_THRESHOLD_FULL; - hdma_tim.Init.MemBurst = DMA_MBURST_SINGLE; - hdma_tim.Init.PeriphBurst = DMA_PBURST_SINGLE; - - /* Set hdma_tim instance */ - hdma_tim.Instance = WS2811_DMA_STREAM; - - uint16_t dmaSource = timerDmaSource(timerChannel); - - /* Link hdma_tim to hdma[x] (channelx) */ - __HAL_LINKDMA(&TimHandle, hdma[dmaSource], hdma_tim); - - dmaInit(WS2811_DMA_HANDLER_IDENTIFER, OWNER_LED_STRIP, 0); - dmaSetHandler(WS2811_DMA_HANDLER_IDENTIFER, WS2811_DMA_IRQHandler, NVIC_PRIO_WS2811_DMA, dmaSource); - - /* Initialize TIMx DMA handle */ - if (HAL_DMA_Init(TimHandle.hdma[dmaSource]) != HAL_OK) { - /* Initialization Error */ - return; - } - - TIM_OC_InitTypeDef TIM_OCInitStructure; - - /* PWM1 Mode configuration: Channel1 */ - TIM_OCInitStructure.OCMode = TIM_OCMODE_PWM1; - if (timerHardware->output & TIMER_OUTPUT_N_CHANNEL) { - timerNChannel = true; - } - TIM_OCInitStructure.OCIdleState = TIM_OCIDLESTATE_RESET; - TIM_OCInitStructure.OCPolarity = (timerHardware->output & TIMER_OUTPUT_INVERTED) ? TIM_OCPOLARITY_LOW: TIM_OCPOLARITY_HIGH; - TIM_OCInitStructure.OCNIdleState = TIM_OCNIDLESTATE_RESET; - TIM_OCInitStructure.OCNPolarity = (timerHardware->output & TIMER_OUTPUT_INVERTED) ? TIM_OCNPOLARITY_LOW : TIM_OCNPOLARITY_HIGH; - - TIM_OCInitStructure.Pulse = 0; - TIM_OCInitStructure.OCFastMode = TIM_OCFAST_DISABLE; - if (HAL_TIM_PWM_ConfigChannel(&TimHandle, &TIM_OCInitStructure, timerChannel) != HAL_OK) { - /* Configuration Error */ - return; - } - - ws2811Initialised = true; -} - -void ws2811LedStripDMAEnable(void) -{ - if (!ws2811Initialised) { - ws2811LedDataTransferInProgress = 0; - return; - } - - if (timerNChannel) { - if (HAL_TIMEx_PWMN_Start_DMA(&TimHandle, timerChannel, ledStripDMABuffer, WS2811_DMA_BUFFER_SIZE) != HAL_OK) { - /* Starting PWM generation Error */ - ws2811LedDataTransferInProgress = 0; - return; - } - } else { - if (HAL_TIM_PWM_Start_DMA(&TimHandle, timerChannel, ledStripDMABuffer, WS2811_DMA_BUFFER_SIZE) != HAL_OK) { - /* Starting PWM generation Error */ - ws2811LedDataTransferInProgress = 0; - return; - } - } -} -#endif diff --git a/src/main/drivers/light_ws2811strip_stdperiph.c b/src/main/drivers/light_ws2811strip_stdperiph.c deleted file mode 100755 index 233fd80bbd1..00000000000 --- a/src/main/drivers/light_ws2811strip_stdperiph.c +++ /dev/null @@ -1,156 +0,0 @@ -/* - * This file is part of Cleanflight. - * - * Cleanflight is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * Cleanflight is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with Cleanflight. If not, see . - */ - -#include -#include - -#include "platform.h" - -#ifdef USE_LED_STRIP - -#include "drivers/io.h" -#include "drivers/nvic.h" - -#include "common/color.h" -#include "light_ws2811strip.h" -#include "dma.h" -#include "drivers/system.h" -#include "rcc.h" -#include "timer.h" - -#if !defined(WS2811_PIN) -#if defined(STM32F4) - #define WS2811_PIN PA0 - #define WS2811_DMA_HANDLER_IDENTIFER DMA1_ST2_HANDLER - #define WS2811_DMA_STREAM DMA1_Stream2 - #define WS2811_DMA_CHANNEL DMA_Channel_6 -#elif defined(STM32F3) - #define WS2811_PIN PB8 // TIM16_CH1 - #define WS2811_DMA_STREAM DMA1_Channel3 - #define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH3_HANDLER -#endif -#endif - - -static IO_t ws2811IO = IO_NONE; -bool ws2811Initialised = false; -#if defined(STM32F4) -static DMA_Stream_TypeDef *dmaRef = NULL; -#elif defined(STM32F3) -static DMA_Channel_TypeDef *dmaRef = NULL; -#else -#error "No MCU definition in light_ws2811strip_stdperiph.c" -#endif -static TIM_TypeDef *timer = NULL; - -static void WS2811_DMA_IRQHandler(dmaChannelDescriptor_t *descriptor) -{ - if (DMA_GET_FLAG_STATUS(descriptor, DMA_IT_TCIF)) { - ws2811LedDataTransferInProgress = 0; - DMA_Cmd(descriptor->ref, DISABLE); - DMA_CLEAR_FLAG(descriptor, DMA_IT_TCIF); - } -} - -void ws2811LedStripHardwareInit(void) -{ - DMA_InitTypeDef DMA_InitStructure; - - const timerHardware_t *timerHardware = timerGetByTag(IO_TAG(WS2811_PIN), TIM_USE_ANY); - - if (timerHardware == NULL) { - return; - } - - timer = timerHardware->tim; - - ws2811IO = IOGetByTag(IO_TAG(WS2811_PIN)); - IOInit(ws2811IO, OWNER_LED_STRIP, RESOURCE_OUTPUT, 0); - IOConfigGPIOAF(ws2811IO, IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, GPIO_OType_PP, GPIO_PuPd_UP), timerHardware->alternateFunction); - - // Stop timer - TIM_Cmd(timer, DISABLE); - - /* Compute the prescaler value */ - uint16_t period = 1000000 * WS2811_TIMER_MHZ / WS2811_CARRIER_HZ; - - BIT_COMPARE_1 = period / 3 * 2; - BIT_COMPARE_0 = period / 3; - - /* PWM1 Mode configuration */ - timerConfigBase(timer, period, WS2811_TIMER_MHZ); - timerPWMConfigChannel(timer, timerHardware->channel, timerHardware->output & TIMER_OUTPUT_N_CHANNEL, timerHardware->output & TIMER_OUTPUT_INVERTED, 0); - - TIM_CtrlPWMOutputs(timer, ENABLE); - TIM_ARRPreloadConfig(timer, ENABLE); - - TIM_CCxCmd(timer, timerHardware->channel, TIM_CCx_Enable); - TIM_Cmd(timer, ENABLE); - - // dmaInit(timerHardware->dmaIrqHandler, OWNER_LED_STRIP, 0); - // dmaSetHandler(timerHardware->dmaIrqHandler, WS2811_DMA_IRQHandler, NVIC_PRIO_WS2811_DMA, 0); - //dmaRef = timerHardware->dmaRef; - dmaRef = WS2811_DMA_STREAM; - dmaInit(WS2811_DMA_HANDLER_IDENTIFER, OWNER_LED_STRIP, 0); - dmaSetHandler(WS2811_DMA_HANDLER_IDENTIFER, WS2811_DMA_IRQHandler, NVIC_PRIO_WS2811_DMA, 0); - - DMA_DeInit(dmaRef); - - /* configure DMA */ - DMA_Cmd(dmaRef, DISABLE); - DMA_DeInit(dmaRef); - DMA_StructInit(&DMA_InitStructure); - DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)timerCCR(timer, timerHardware->channel); - DMA_InitStructure.DMA_BufferSize = WS2811_DMA_BUFFER_SIZE; - DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable; - DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable; - -#if defined(STM32F4) - DMA_InitStructure.DMA_Channel = WS2811_DMA_CHANNEL; //timerHardware->dmaChannel; - DMA_InitStructure.DMA_Memory0BaseAddr = (uint32_t)ledStripDMABuffer; - DMA_InitStructure.DMA_DIR = DMA_DIR_MemoryToPeripheral; - DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Word; - DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Word; - DMA_InitStructure.DMA_Priority = DMA_Priority_VeryHigh; -#elif defined(STM32F3) - DMA_InitStructure.DMA_MemoryBaseAddr = (uint32_t)ledStripDMABuffer; - DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralDST; - DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Word; - DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte; - DMA_InitStructure.DMA_Priority = DMA_Priority_High; - DMA_InitStructure.DMA_M2M = DMA_M2M_Disable; -#endif - DMA_InitStructure.DMA_Mode = DMA_Mode_Normal; - - DMA_Init(dmaRef, &DMA_InitStructure); - TIM_DMACmd(timer, timerDmaSource(timerHardware->channel), ENABLE); - DMA_ITConfig(dmaRef, DMA_IT_TC, ENABLE); - ws2811Initialised = true; -} - -void ws2811LedStripDMAEnable(void) -{ - if (!ws2811Initialised) - return; - - DMA_SetCurrDataCounter(dmaRef, WS2811_DMA_BUFFER_SIZE); // load number of bytes to be transferred - TIM_SetCounter(timer, 0); - TIM_Cmd(timer, ENABLE); - DMA_Cmd(dmaRef, ENABLE); -} - -#endif diff --git a/src/main/drivers/logging.c b/src/main/drivers/logging.c index cc7307d02fe..703ad7d7eb0 100755 --- a/src/main/drivers/logging.c +++ b/src/main/drivers/logging.c @@ -56,6 +56,8 @@ static const char * eventDescription[BOOT_EVENT_CODE_COUNT] = { [BOOT_EVENT_TIMER_CH_SKIPPED] = "TIMER_CHANNEL_SKIPPED", [BOOT_EVENT_TIMER_CH_MAPPED] = "TIMER_CHANNEL_MAPPED", [BOOT_EVENT_PITOT_DETECTION] = "PITOT_DETECTION", + [BOOT_EVENT_TEMP_SENSOR_DETECTION] = "TEMP_SENSOR_DETECTION", + [BOOT_EVENT_1WIRE_DETECTION] = "1WIRE_DETECTION", [BOOT_EVENT_HARDWARE_IO_CONFLICT] = "HARDWARE_CONFLICT", [BOOT_EVENT_OPFLOW_DETECTION] = "OPFLOW_DETECTION", }; diff --git a/src/main/drivers/logging_codes.h b/src/main/drivers/logging_codes.h index 33687cfebc7..7f04c5ab25d 100755 --- a/src/main/drivers/logging_codes.h +++ b/src/main/drivers/logging_codes.h @@ -48,8 +48,10 @@ typedef enum { BOOT_EVENT_TIMER_CH_SKIPPED = 18, // 1 - MAX_MOTORS exceeded, 2 - MAX_SERVOS exceeded, 3 - feature clash BOOT_EVENT_TIMER_CH_MAPPED = 19, // 0 - PPM, 1 - PWM, 2 - MOTOR, 3 - SERVO BOOT_EVENT_PITOT_DETECTION = 20, - BOOT_EVENT_HARDWARE_IO_CONFLICT = 21, // Hardware IO resource conflict, parameters: #1 - current owner, #2 - requested owner - BOOT_EVENT_OPFLOW_DETECTION = 22, + BOOT_EVENT_TEMP_SENSOR_DETECTION = 21, + BOOT_EVENT_1WIRE_DETECTION = 22, + BOOT_EVENT_HARDWARE_IO_CONFLICT = 23, // Hardware IO resource conflict, parameters: #1 - current owner, #2 - requested owner + BOOT_EVENT_OPFLOW_DETECTION = 24, BOOT_EVENT_CODE_COUNT } bootLogEventCode_e; diff --git a/src/main/drivers/max7456.c b/src/main/drivers/max7456.c index 8fcd8386dbe..24c52cc8b20 100644 --- a/src/main/drivers/max7456.c +++ b/src/main/drivers/max7456.c @@ -108,12 +108,18 @@ #define MAX7456_SIGNAL_CHECK_INTERVAL_MS 1000 // msec // DMM special bits +#define DMM_8BIT_MODE (1 << 6) #define DMM_BLINK (1 << 4) #define DMM_INVERT_PIXEL_COLOR (1 << 3) #define DMM_CLEAR_DISPLAY (1 << 2) #define DMM_CLEAR_DISPLAY_VERT (DMM_CLEAR_DISPLAY | 1 << 1) #define DMM_AUTOINCREMENT (1 << 0) +#define DMM_IS_8BIT_MODE(val) (val & DMM_8BIT_MODE) +#define DMM_CHAR_MODE_MASK (MAX7456_MODE_INVERT | MAX7456_MODE_BLINK | MAX7456_MODE_SOLID_BG) + +#define DMAH_8_BIT_DMDI_IS_CHAR_ATTR (1 << 1) + // Special address for terminating incremental write #define END_STRING 0xff @@ -149,17 +155,29 @@ #define MAX7456ADD_RB15 0x1f #define MAX7456ADD_OSDBL 0x6c #define MAX7456ADD_STAT 0xA0 +#define MAX7456ADD_CMDO 0xC0 #define NVM_RAM_SIZE 54 +#define READ_NVR 0x50 #define WRITE_NVR 0xA0 +// Maximum time to wait for video sync. After this we +// default to PAL +#define MAX_SYNC_WAIT_MS 1500 + +// Maximum time to wait for a software reset to complete +// Under normal conditions, it should take 20us +#define MAX_RESET_TIMEOUT_MS 50 + #define CHARS_PER_LINE 30 // XXX Should be related to VIDEO_BUFFER_CHARS_*? -#define MAKE_CHAR_MODE(c, m) ((((uint16_t)c) << 8) | m) +#define MAKE_CHAR_MODE_U8(c, m) ((((uint16_t)c) << 8) | m) +#define MAKE_CHAR_MODE(c, m) (MAKE_CHAR_MODE_U8(c, m) | (c > 255 ? CHAR_MODE_EXT : 0)) #define CHAR_BLANK MAKE_CHAR_MODE(0x20, 0) #define CHAR_BYTE(x) (x >> 8) #define MODE_BYTE(x) (x & 0xFF) - -uint16_t maxScreenSize = VIDEO_BUFFER_CHARS_PAL; +#define CHAR_IS_BLANK(x) ((CHAR_BYTE(x) == 0x20 || CHAR_BYTE(x) == 0x00) && !CHAR_MODE_IS_EXT(MODE_BYTE(x))) +#define CHAR_MODE_EXT (1 << 2) +#define CHAR_MODE_IS_EXT(m) ((m) & CHAR_MODE_EXT) // we write everything in screenBuffer and set a dirty bit // in screenIsDirty to upgrade only changed chars this solution @@ -169,14 +187,94 @@ static BITARRAY_DECLARE(screenIsDirty, VIDEO_BUFFER_CHARS_PAL); //max chars to update in one idle #define MAX_CHARS2UPDATE 10 -#define BYTES_PER_CHAR2UPDATE 8 // [3-4] spi regs + values for them +#define BYTES_PER_CHAR2UPDATE (7 * 2) // SPI regs + values for them + +typedef struct max7456Registers_s { + uint8_t vm0; + uint8_t dmm; +} max7456Registers_t; + +typedef struct max7456State_s { + busDevice_t *dev; + videoSystem_e videoSystem; + bool isInitialized; + bool mutex; + max7456Registers_t registers; +} max7456State_t; + +static max7456State_t state; + +static bool max7456ReadVM0(uint8_t *vm0) +{ + return busRead(state.dev, MAX7456ADD_VM0 | MAX7456ADD_READ, vm0); +} + +static bool max7456IsBusy(void) +{ + uint8_t status; + + if (busRead(state.dev, MAX7456ADD_STAT, &status)) { + return status & STAT_NVR_BUSY; + } + // Read failed or busy + return true; +} + +// Wait until max7456IsBusy() returns false, toggling a LED on each iteration +static void max7456WaitUntilNoBusy(void) +{ + while (1) { + if (!max7456IsBusy()) { + break; + } +#ifdef LED0_TOGGLE + LED0_TOGGLE; +#else + LED1_TOGGLE; +#endif + } +} + +// Sets wether the OSD drawing is enabled. Returns true iff +// changes were succesfully performed. +static bool max7456OSDSetEnabled(bool enabled) +{ + if (enabled && !(state.registers.vm0 | OSD_ENABLE)) { + state.registers.vm0 |= OSD_ENABLE; + } else if (!enabled && (state.registers.vm0 | OSD_ENABLE)) { + state.registers.vm0 &= ~OSD_ENABLE; + } else { + // No changes needed + return false; + } + return busWrite(state.dev, MAX7456ADD_VM0, state.registers.vm0); +} + +static bool max7456OSDIsEnabled(void) +{ + return state.registers.vm0 & OSD_ENABLE; +} + +static void max7456Lock(void) +{ + while(state.mutex); -static bool firstInit = true; -static uint8_t videoSignalCfg = 0; -static uint8_t videoSignalReg = VIDEO_MODE_PAL | OSD_ENABLE; //PAL by default -static bool max7456Lock = false; -static bool fontIsLoading = false; -static busDevice_t * max7456dev = NULL; + state.mutex = true; +} + +static void max7456Unlock(void) +{ + state.mutex = false; +} + +static bool max7456TryLock(void) +{ + if (!state.mutex) { + state.mutex = true; + return true; + } + return false; +} static int max7456PrepareBuffer(uint8_t * buf, int bufPtr, uint8_t add, uint8_t data) { @@ -185,13 +283,26 @@ static int max7456PrepareBuffer(uint8_t * buf, int bufPtr, uint8_t add, uint8_t return bufPtr; } +uint16_t max7456GetScreenSize(void) +{ + // Default to PAL while the display is not yet initialized. This + // was the initial behavior and not all callers might be able to + // deal with a zero returned from here. + // TODO: Inspect all callers, make sure they can handle zero and + // change this function to return zero before initialization. + if (state.isInitialized && ((state.registers.vm0 & VIDEO_MODE_PAL) == 0)) { + return VIDEO_BUFFER_CHARS_NTSC; + } + return VIDEO_BUFFER_CHARS_PAL; +} + uint8_t max7456GetRowsCount(void) { - if (firstInit) { + if (!state.isInitialized) { // Not initialized yet return 0; } - if (videoSignalReg & VIDEO_MODE_PAL) { + if (state.registers.vm0 & VIDEO_MODE_PAL) { return VIDEO_LINES_PAL; } @@ -201,88 +312,102 @@ uint8_t max7456GetRowsCount(void) //because MAX7456 need some time to detect video system etc. we need to wait for a while to initialize it at startup //and in case of restart we need to reinitialize chip. Note that we can't touch screenBuffer here, since //it might already have some data by the first time this function is called. -void max7456ReInit(void) +static void max7456ReInit(void) { - uint8_t buf[(VIDEO_BUFFER_CHARS_PAL + 3) * 2]; - int bufPtr; - uint8_t maxScreenRows; - uint8_t srdata = 0; + uint8_t buf[2 * 2]; + int bufPtr = 0; + uint8_t statVal; + // Check if device is available - if (max7456dev == NULL) { + if (state.dev == NULL) { return; } - //do not init MAX before camera power up correctly - if (millis() < 1500) - return; + uint8_t vm0Mode; - switch (videoSignalCfg) { + switch (state.videoSystem) { case PAL: - videoSignalReg = VIDEO_MODE_PAL | OSD_ENABLE; + vm0Mode = VIDEO_MODE_PAL; break; case NTSC: - videoSignalReg = VIDEO_MODE_NTSC | OSD_ENABLE; + vm0Mode = VIDEO_MODE_NTSC; break; default: - busRead(max7456dev, MAX7456ADD_STAT, &srdata); - if ((0x02 & srdata) == 0x02) - videoSignalReg = VIDEO_MODE_NTSC | OSD_ENABLE; - } - - if (videoSignalReg & VIDEO_MODE_PAL) { //PAL - maxScreenSize = VIDEO_BUFFER_CHARS_PAL; - maxScreenRows = VIDEO_LINES_PAL; - } else { // NTSC - maxScreenSize = VIDEO_BUFFER_CHARS_NTSC; - maxScreenRows = VIDEO_LINES_NTSC; + busRead(state.dev, MAX7456ADD_STAT, &statVal); + if (VIN_IS_PAL(statVal)) { + vm0Mode = VIDEO_MODE_PAL; + } else if (VIN_IS_NTSC_alt(statVal)) { + vm0Mode = VIDEO_MODE_NTSC; + } else if ( millis() > MAX_SYNC_WAIT_MS) { + // Detection timed out, default to PAL + vm0Mode = VIDEO_MODE_PAL; + } else { + // No signal detected yet, wait for detection timeout + return; + } } - // set all rows to same charactor black/white level - bufPtr = 0; - for (int x = 0; x < maxScreenRows; x++) { - bufPtr = max7456PrepareBuffer(buf, bufPtr, MAX7456ADD_RB0 + x, BWBRIGHTNESS); - } + state.registers.vm0 = vm0Mode | OSD_ENABLE; - // make sure the Max7456 is enabled - bufPtr = max7456PrepareBuffer(buf, bufPtr, MAX7456ADD_VM0, videoSignalReg); - bufPtr = max7456PrepareBuffer(buf, bufPtr, MAX7456ADD_VM1, BLINK_DUTY_CYCLE_50_50 | BLINK_TIME_3 | BACKGROUND_BRIGHTNESS_28); + // Enable OSD drawing and clear the display + bufPtr = max7456PrepareBuffer(buf, bufPtr, MAX7456ADD_VM0, state.registers.vm0); bufPtr = max7456PrepareBuffer(buf, bufPtr, MAX7456ADD_DMM, DMM_CLEAR_DISPLAY); // Transfer data to SPI - busTransfer(max7456dev, NULL, buf, bufPtr); + busTransfer(state.dev, NULL, buf, bufPtr); - // force redrawing all screen in non-dma mode - memset(screenIsDirty, 0xFF, sizeof(screenIsDirty)); - if (firstInit) { + // force redrawing all screen + BITARRAY_SET_ALL(screenIsDirty); + if (!state.isInitialized) { max7456RefreshAll(); - firstInit = false; + state.isInitialized = true; } } - //here we init only CS and try to init MAX for first time void max7456Init(const videoSystem_e videoSystem) { - max7456dev = busDeviceInit(BUSTYPE_SPI, DEVHW_MAX7456, 0, OWNER_OSD); + uint8_t buf[(VIDEO_LINES_PAL + 1) * 2]; + int bufPtr; + state.dev = busDeviceInit(BUSTYPE_SPI, DEVHW_MAX7456, 0, OWNER_OSD); - if (max7456dev == NULL) { + if (state.dev == NULL) { return; } - busSetSpeed(max7456dev, BUS_SPEED_STANDARD); + busSetSpeed(state.dev, BUS_SPEED_STANDARD); // force soft reset on Max7456 - busWrite(max7456dev, MAX7456ADD_VM0, MAX7456_RESET); + busWrite(state.dev, MAX7456ADD_VM0, MAX7456_RESET); - videoSignalCfg = videoSystem; + // DMM defaults to all zeroes on reset + state.registers.dmm = 0; + state.videoSystem = videoSystem; // Set screenbuffer to all blanks for (uint_fast16_t ii = 0; ii < ARRAYLEN(screenBuffer); ii++) { screenBuffer[ii] = CHAR_BLANK; } - //real init will be made letter when driver idle detect + // Wait for software reset to finish + timeMs_t timeout = millis() + MAX_RESET_TIMEOUT_MS; + while(max7456ReadVM0(&state.registers.vm0) && + (state.registers.vm0 | MAX7456_RESET) && + millis() < timeout); + + // Set all rows to same charactor black/white level. We + // can do this without finding wether the display is PAL + // NTSC because all the brightness registers can be written + // regardless of the video mode. + bufPtr = 0; + for (int ii = 0; ii < VIDEO_LINES_PAL; ii++) { + bufPtr = max7456PrepareBuffer(buf, bufPtr, MAX7456ADD_RB0 + ii, BWBRIGHTNESS); + } + + // Set the blink duty cycle + bufPtr = max7456PrepareBuffer(buf, bufPtr, MAX7456ADD_VM1, BLINK_DUTY_CYCLE_50_50 | BLINK_TIME_3 | BACKGROUND_BRIGHTNESS_28); + busTransfer(state.dev, NULL, buf, bufPtr); } void max7456ClearScreen(void) @@ -295,7 +420,7 @@ void max7456ClearScreen(void) } } -void max7456WriteChar(uint8_t x, uint8_t y, uint8_t c, uint8_t mode) +void max7456WriteChar(uint8_t x, uint8_t y, uint16_t c, uint8_t mode) { unsigned pos = y * CHARS_PER_LINE + x; uint16_t val = MAKE_CHAR_MODE(c, mode); @@ -305,13 +430,17 @@ void max7456WriteChar(uint8_t x, uint8_t y, uint8_t c, uint8_t mode) } } -bool max7456ReadChar(uint8_t x, uint8_t y, uint8_t *c, uint8_t *mode) +bool max7456ReadChar(uint8_t x, uint8_t y, uint16_t *c, uint8_t *mode) { unsigned pos = y * CHARS_PER_LINE + x; if (pos < ARRAYLEN(screenBuffer)) { uint16_t val = screenBuffer[pos]; - *c = val >> 8; - *mode = val & 0xFF; + *c = CHAR_BYTE(val); + *mode = MODE_BYTE(val); + if (CHAR_MODE_IS_EXT(*mode)) { + *c |= 1 << 8; + *mode &= ~CHAR_MODE_EXT; + } return true; } return false; @@ -327,7 +456,7 @@ void max7456Write(uint8_t x, uint8_t y, const char *buff, uint8_t mode) if (x + i >= CHARS_PER_LINE) { break; } - c = MAKE_CHAR_MODE(*buff, mode); + c = MAKE_CHAR_MODE_U8(*buff, mode); if (screenBuffer[pos] != c) { screenBuffer[pos] = c; bitArraySet(screenIsDirty, pos); @@ -335,102 +464,134 @@ void max7456Write(uint8_t x, uint8_t y, const char *buff, uint8_t mode) } } -void max7456DrawScreenPartial(void) +// Must be called with the lock held. Returns wether any new characters +// were drawn. +static bool max7456DrawScreenPartial(void) { - static uint32_t lastSigCheckMs = 0; - static uint32_t videoDetectTimeMs = 0; - // Save this between updates. The default value - // in the MAX7456 is all bits to zero. - static uint8_t setMode = 0; - - uint8_t stallCheck; - uint8_t videoSense; - uint32_t nowMs; + uint8_t spiBuff[MAX_CHARS2UPDATE * BYTES_PER_CHAR2UPDATE]; + int bufPtr = 0; int pos; uint_fast16_t updatedCharCount; - uint8_t currentMode; + uint8_t charMode; - // Check if device is available - if (max7456dev == NULL) { - return; + for (pos = 0, updatedCharCount = 0;;) { + pos = BITARRAY_FIND_FIRST_SET(screenIsDirty, pos); + if (pos < 0) { + // No more dirty chars. + break; + } + + // Found one dirty character to send + uint8_t ph = pos >> 8; + uint8_t pl = pos & 0xff; + + charMode = MODE_BYTE(screenBuffer[pos]); + uint8_t chr = CHAR_BYTE(screenBuffer[pos]); + if (CHAR_MODE_IS_EXT(charMode)) { + if (!DMM_IS_8BIT_MODE(state.registers.dmm)) { + state.registers.dmm |= DMM_8BIT_MODE; + bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_DMM, state.registers.dmm); + } + + bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_DMAH, ph | DMAH_8_BIT_DMDI_IS_CHAR_ATTR); + bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_DMAL, pl); + // Attribute bit positions on DMDI are 2 bits up relative to DMM. + // DMM uses [5:3] while DMDI uses [7:4] - one bit more for referencing + // characters in the [256, 511] range (which is not possible via DMM). + // Since we write mostly to DMM, the internal representation uses + // the format of the former and we shift it up here. + bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_DMDI, charMode << 2); + + bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_DMAH, ph); + bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_DMAL, pl); + bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_DMDI, chr); + + } else { + if (DMM_IS_8BIT_MODE(state.registers.dmm) || (DMM_CHAR_MODE_MASK & state.registers.dmm) != charMode) { + state.registers.dmm &= ~DMM_8BIT_MODE; + state.registers.dmm = (state.registers.dmm & ~DMM_CHAR_MODE_MASK) | charMode; + // Send the attributes for the character run. They + // will be applied to all characters until we change + // the DMM register. + bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_DMM, state.registers.dmm); + } + + bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_DMAH, ph); + bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_DMAL, pl); + bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_DMDI, chr); + } + + bitArrayClr(screenIsDirty, pos); + if (++updatedCharCount == MAX_CHARS2UPDATE) { + break; + } + // Start next search at next bit + pos++; } - if (!max7456Lock && !fontIsLoading) { - // (Re)Initialize MAX7456 at startup or stall is detected. + if (bufPtr) { + busTransfer(state.dev, NULL, spiBuff, bufPtr); + return true; + } + return false; +} - max7456Lock = true; +// Must be called with the lock held +static void max7456StallCheck(void) +{ + static uint32_t lastSigCheckMs = 0; + static uint32_t videoDetectTimeMs = 0; - // Stall check - busRead(max7456dev, MAX7456ADD_VM0 | MAX7456ADD_READ, &stallCheck); + uint8_t vm0; + uint8_t statReg; - nowMs = millis(); + if (!state.isInitialized || !max7456ReadVM0(&vm0) || vm0 != state.registers.vm0) { + max7456ReInit(); + return; + } - if (stallCheck != videoSignalReg) { - max7456ReInit(); - } else if ((videoSignalCfg == VIDEO_SYSTEM_AUTO) - && ((nowMs - lastSigCheckMs) > MAX7456_SIGNAL_CHECK_INTERVAL_MS)) { + if (state.videoSystem == VIDEO_SYSTEM_AUTO) { + timeMs_t nowMs = millis(); + if ((nowMs - lastSigCheckMs) > MAX7456_SIGNAL_CHECK_INTERVAL_MS) { // Adjust output format based on the current input format. - busRead(max7456dev, MAX7456ADD_STAT, &videoSense); + busRead(state.dev, MAX7456ADD_STAT, &statReg); - if (videoSense & STAT_LOS) { + if (statReg & STAT_LOS) { videoDetectTimeMs = 0; } else { - if ((VIN_IS_PAL(videoSense) && VIDEO_MODE_IS_NTSC(videoSignalReg)) - || (VIN_IS_NTSC_alt(videoSense) && VIDEO_MODE_IS_PAL(videoSignalReg))) { + if ((VIN_IS_PAL(statReg) && VIDEO_MODE_IS_NTSC(state.registers.vm0)) + || (VIN_IS_NTSC_alt(statReg) && VIDEO_MODE_IS_PAL(state.registers.vm0))) { if (videoDetectTimeMs) { - if (millis() - videoDetectTimeMs > VIDEO_SIGNAL_DEBOUNCE_MS) { + if (nowMs - videoDetectTimeMs > VIDEO_SIGNAL_DEBOUNCE_MS) { max7456ReInit(); } } else { // Wait for signal to stabilize - videoDetectTimeMs = millis(); + videoDetectTimeMs = nowMs; } } } lastSigCheckMs = nowMs; } + } +} - //------------ end of (re)init------------------------------------- - uint8_t spiBuff[MAX_CHARS2UPDATE * BYTES_PER_CHAR2UPDATE]; - int bufPtr = 0; - - for (pos = 0, updatedCharCount = 0;;) { - pos = BITARRAY_FIND_FIRST_SET(screenIsDirty, pos); - if (pos < 0 || pos >= maxScreenSize) { - // No more dirty chars. - break; - } - - currentMode = MODE_BYTE(screenBuffer[pos]); - - // Found one dirty character to send - if (setMode != currentMode) { - setMode = currentMode; - // Send the attributes for the character run. They - // will be applied to all characters until we change - // the DMM register. - bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_DMM, currentMode); - } - - bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_DMAH, pos >> 8); - bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_DMAL, pos & 0xff); - bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_DMDI, CHAR_BYTE(screenBuffer[pos])); - - bitArrayClr(screenIsDirty, pos); - if (++updatedCharCount == MAX_CHARS2UPDATE) { - break; - } - // Start next search at next bit - pos++; - } +void max7456Update(void) +{ + // Check if device is available + if (state.dev == NULL) { + return; + } - if (bufPtr) { - busTransfer(max7456dev, NULL, spiBuff, bufPtr); + if ((max7456OSDIsEnabled() && max7456TryLock()) || !state.isInitialized) { + // (Re)Initialize MAX7456 at startup or stall is detected. + max7456StallCheck(); + if (state.isInitialized) { + max7456DrawScreenPartial(); } - - max7456Lock = false; + max7456Unlock(); } } @@ -439,115 +600,126 @@ void max7456DrawScreenPartial(void) // when copter is armed. void max7456RefreshAll(void) { - uint8_t spiBuff[(VIDEO_BUFFER_CHARS_PAL + 4) * 2]; - int bufPtr; + uint8_t dmm; // Check if device is available - if (max7456dev == NULL) { + if (state.dev == NULL) { return; } - if (!max7456Lock) { - uint16_t xx; - max7456Lock = true; + if (max7456TryLock()) { - // Write characters. Start at character zero. - bufPtr = 0; - bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_DMAH, 0); - bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_DMAL, 0); - bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_DMM, DMM_AUTOINCREMENT); + // Issue an OSD clear command + busRead(state.dev, MAX7456ADD_DMM | MAX7456ADD_READ, &dmm); + busWrite(state.dev, MAX7456ADD_DMM, state.registers.dmm | DMM_CLEAR_DISPLAY); - for (xx = 0; xx < maxScreenSize; ++xx) { - bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_DMDI, CHAR_BYTE(screenBuffer[xx])); + // Wait for clear to complete (20us) + while (1) { + busRead(state.dev, MAX7456ADD_DMM | MAX7456ADD_READ, &dmm); + if (!(dmm & DMM_CLEAR_DISPLAY)) { + state.registers.dmm = dmm; + break; + } } - // Exit auto-increment mode by writing the 0xFF escape sequence to DMDI. - bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_DMDI, 0xFF); - - // Write chunk of data - busTransfer(max7456dev, NULL, spiBuff, bufPtr); - - // Write character attributes. Start at zero, but - // set DMAH[1] = 1, to signal that we're sending - // attributes rather than characters. Process is the - // same as for the characters a few lines up. - bufPtr = 0; - bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_DMAH, 1<<1); - bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_DMAL, 0); - bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_DMM, DMM_AUTOINCREMENT); - - for (xx = 0; xx < maxScreenSize; ++xx) { - // Note that atttribute bits in DMDI are in different - // positions than in DMM (DMM is used for partial writes), - // and we store the attributes in the format expected by - // DMM. - // | LBC | BLK | INV - // ---------------------- - // DMDI:| 7 | 6 | 5 - // DMM: | 5 | 4 | 3 - // - // Thus, we need to shift the bits by 2 when writing character - // attributes to DMDI. - bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_DMDI, MODE_BYTE(screenBuffer[xx]) << 2); + // Mark non-blank characters as dirty + BITARRAY_CLR_ALL(screenIsDirty); + for (unsigned ii = 0; ii < ARRAYLEN(screenBuffer); ii++) { + if (!CHAR_IS_BLANK(screenBuffer[ii])) { + bitArraySet(screenIsDirty, ii); + } } - // Exit auto-increment mode by writing the 0xFF escape sequence to DMDI. - bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_DMDI, 0xFF); + // Now perform partial writes until there are no dirty ones + while(max7456DrawScreenPartial()); + + max7456Unlock(); + } +} + +void max7456ReadNvm(uint16_t char_address, max7456Character_t *chr) +{ + // Check if device is available + if (state.dev == NULL) { + return; + } + + max7456Lock(); + // OSD must be disabled to read or write to NVM + bool enabled = max7456OSDSetEnabled(false); + + busWrite(state.dev, MAX7456ADD_CMAH, char_address); + if (char_address > 255) { + // AT7456E and AB7456 have 512 characters of NVM. + // To read/write to NVM they use CMAL[6] as the + // high bits of the character address. + uint8_t addr_h = char_address >> 8; + busWrite(state.dev, MAX7456ADD_CMAL, addr_h << 6); + } + busWrite(state.dev, MAX7456ADD_CMM, READ_NVR); - // Write chunk of data - busTransfer(max7456dev, NULL, spiBuff, bufPtr); + max7456WaitUntilNoBusy(); - // All characters have been set to the MAX7456, none is dirty now. - memset(screenIsDirty, 0, sizeof(screenIsDirty)); - max7456Lock = false; + for (unsigned ii = 0; ii < sizeof(chr->data); ii++) { + busWrite(state.dev, MAX7456ADD_CMAL, ii); + busRead(state.dev, MAX7456ADD_CMDO, &chr->data[ii]); } + + max7456OSDSetEnabled(enabled); + max7456Unlock(); } -void max7456WriteNvm(uint8_t char_address, const uint8_t *font_data) +void max7456WriteNvm(uint16_t char_address, const max7456Character_t *chr) { - uint8_t spiBuff[(54 * 2 + 3) * 2]; + uint8_t spiBuff[(sizeof(chr->data) * 2 + 2) * 2]; int bufPtr = 0; // Check if device is available - if (max7456dev == NULL) { + if (state.dev == NULL) { return; } - while (max7456Lock); - max7456Lock = true; - - // disable display - fontIsLoading = true; - - bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_VM0, 0); - bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_CMAH, char_address); // set start address high + max7456Lock(); + // OSD must be disabled to read or write to NVM + max7456OSDSetEnabled(false); + + bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_CMAH, char_address & 0xFF); // set start address high + + uint8_t or_val = 0; + if (char_address > 255) { + // AT7456E and AB7456 have 512 characters of NVM. + // To read/write to NVM they use CMAL[6] as the + // high bit of the character address. + // + // Instead of issuing an additional write to CMAL when + // we're done uploading to shadow RAM, we set the high + // bits of CMAL on every write since they have no side + // effects while writing from CMDI to RAM and when we + // issue the copy command to NVM, CMAL[6] is already + // set. + uint8_t addr_h = char_address >> 8; + or_val = addr_h << 6; + } - for (int x = 0; x < 54; x++) { - bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_CMAL, x); //set start address low - bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_CMDI, font_data[x]); + for (unsigned x = 0; x < sizeof(chr->data); x++) { + bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_CMAL, x | or_val); //set start address low + bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_CMDI, chr->data[x]); } // transfer 54 bytes from shadow ram to NVM bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_CMM, WRITE_NVR); - busTransfer(max7456dev, NULL, spiBuff, bufPtr); - - while (1) { - uint8_t status; + busTransfer(state.dev, NULL, spiBuff, bufPtr); - busRead(max7456dev, MAX7456ADD_STAT, &status); - if ((status & STAT_NVR_BUSY) == 0x00) { - break; - } + max7456WaitUntilNoBusy(); -#ifdef LED0_TOGGLE - LED0_TOGGLE; -#else - LED1_TOGGLE; -#endif - } + /* XXX: Don't call max7456OSDEnable(), it's intentionally ommited. + * If we continue drawing while characters are being uploaded, we + * get some corrupted characters from time to time. As a workaround, + * we require a reboot after characters have been uploaded to NVM. + */ - max7456Lock = false; + max7456Unlock(); } diff --git a/src/main/drivers/max7456.h b/src/main/drivers/max7456.h index 2227ec7483f..be894adde13 100644 --- a/src/main/drivers/max7456.h +++ b/src/main/drivers/max7456.h @@ -41,16 +41,19 @@ enum VIDEO_TYPES { AUTO = 0, PAL, NTSC }; #define MAX7456_MODE_BLINK (1 << 4) #define MAX7456_MODE_SOLID_BG (1 << 5) -extern uint16_t maxScreenSize; - -struct vcdProfile_s; -void max7456Init(const videoSystem_e videoSystem); -void max7456DrawScreenPartial(void); -void max7456WriteNvm(uint8_t char_address, const uint8_t *font_data); +typedef struct max7456Character_s { + uint8_t data[54]; +} max7456Character_t; + +void max7456Init(const videoSystem_e videoSystem); +void max7456Update(void); +void max7456ReadNvm(uint16_t char_address, max7456Character_t *chr); +void max7456WriteNvm(uint16_t char_address, const max7456Character_t *chr); +uint16_t max7456GetScreenSize(void); uint8_t max7456GetRowsCount(void); -void max7456Write(uint8_t x, uint8_t y, const char *buff, uint8_t mode); -void max7456WriteChar(uint8_t x, uint8_t y, uint8_t c, uint8_t mode); -bool max7456ReadChar(uint8_t x, uint8_t y, uint8_t *c, uint8_t *mode); -void max7456ClearScreen(void); -void max7456RefreshAll(void); +void max7456Write(uint8_t x, uint8_t y, const char *buff, uint8_t mode); +void max7456WriteChar(uint8_t x, uint8_t y, uint16_t c, uint8_t mode); +bool max7456ReadChar(uint8_t x, uint8_t y, uint16_t *c, uint8_t *mode); +void max7456ClearScreen(void); +void max7456RefreshAll(void); uint8_t* max7456GetScreenBuffer(void); diff --git a/src/main/drivers/max7456_symbols.h b/src/main/drivers/max7456_symbols.h index a87630d37ad..56fd7115d0d 100644 --- a/src/main/drivers/max7456_symbols.h +++ b/src/main/drivers/max7456_symbols.h @@ -23,221 +23,191 @@ #ifdef USE_MAX7456 -#define SYM_BLANK 0x20 - -// Satellite Graphics -#define SYM_SAT_L 0x1E -#define SYM_SAT_R 0x1F -#define SYM_HDP_L 0xBD -#define SYM_HDP_R 0xBE -//#define SYM_SAT 0x0F // Not used - -// Degrees symbol (°) for HEADING/DIRECTION HOME -#define SYM_DEGREES 0xA8 -// Heading symbol (looks like a semicircular double arrow) -#define SYM_HEADING 0xA9 - -// Direction arrows -#define SYM_ARROW_UP 0x60 -#define SYM_ARROW_2 0x61 -#define SYM_ARROW_3 0x62 -#define SYM_ARROW_4 0x63 -#define SYM_ARROW_RIGHT 0x64 -#define SYM_ARROW_6 0x65 -#define SYM_ARROW_7 0x66 -#define SYM_ARROW_8 0x67 -#define SYM_ARROW_DOWN 0x68 -#define SYM_ARROW_10 0x69 -#define SYM_ARROW_11 0x6A -#define SYM_ARROW_12 0x6B -#define SYM_ARROW_LEFT 0x6C -#define SYM_ARROW_14 0x6D -#define SYM_ARROW_15 0x6E -#define SYM_ARROW_16 0x6F - -// Heading Graphics -#define SYM_HEADING_N 0x18 -#define SYM_HEADING_S 0x19 -#define SYM_HEADING_E 0x1A -#define SYM_HEADING_W 0x1B -#define SYM_HEADING_DIVIDED_LINE 0x1C -#define SYM_HEADING_LINE 0x1D - -// FRSKY HUB -#define SYM_CELL0 0xF0 -#define SYM_CELL1 0xF1 -#define SYM_CELL2 0xF2 -#define SYM_CELL3 0xF3 -#define SYM_CELL4 0xF4 -#define SYM_CELL5 0xF5 -#define SYM_CELL6 0xF6 -#define SYM_CELL7 0xF7 -#define SYM_CELL8 0xF8 -#define SYM_CELL9 0xF9 -#define SYM_CELLA 0xFA -#define SYM_CELLB 0xFB -#define SYM_CELLC 0xFC -#define SYM_CELLD 0xFD -#define SYM_CELLE 0xFE -#define SYM_CELLF 0xC3 - -// Map mode -#define SYM_SCALE 175 -#define SYM_HOME 191 -#define SYM_AIRCRAFT 0x05 -#define SYM_RANGE_100 0x21 -#define SYM_RANGE_500 0x22 -#define SYM_RANGE_2500 0x23 -#define SYM_RANGE_MAX 0x24 -#define SYM_DIRECTION 0x72 - -// GPS Coordinates and Altitude -#define SYM_LAT 0xA6 -#define SYM_LON 0xA7 -#define SYM_ALT 0xAA - -// GPS Mode and Autopilot -#define SYM_3DFIX 0xDF -#define SYM_HOLD 0xEF -#define SYM_G_HOME 0xFF -#define SYM_GHOME 0x9D -#define SYM_GHOME1 0x9E -#define SYM_GHOLD 0xCD -#define SYM_GHOLD1 0xCE -#define SYM_GMISSION 0xB5 -#define SYM_GMISSION1 0xB6 -#define SYM_GLAND 0xB7 -#define SYM_GLAND1 0xB8 -#define SYM_HOME_DIST 0xA0 -#define SYM_TRIP_DIST 0x22 - -// AH Center screen Graphics -#define SYM_AH_CENTER_LINE 0x26 -#define SYM_AH_CENTER_LINE_RIGHT 0x27 -#define SYM_AH_CENTER 0x7E -#define SYM_AH_RIGHT 0x02 -#define SYM_AH_LEFT 0x03 -#define SYM_AH_DECORATION_UP 5 -#define SYM_AH_DECORATION_DOWN 36 - -#define SYM_AH_CROSSHAIRS_AIRCRAFT0 218 -#define SYM_AH_CROSSHAIRS_AIRCRAFT1 219 -#define SYM_AH_CROSSHAIRS_AIRCRAFT2 220 -#define SYM_AH_CROSSHAIRS_AIRCRAFT3 221 -#define SYM_AH_CROSSHAIRS_AIRCRAFT4 222 - -// AH Bars -#define SYM_AH_BAR9_0 0x80 - -// Temperature -#define SYM_TEMP_F 0x0D -#define SYM_TEMP_C 0x0E - -// Batt evolution -#define SYM_BATT_FULL 0x90 -#define SYM_BATT_5 0x91 -#define SYM_BATT_4 0x92 -#define SYM_BATT_3 0x93 -#define SYM_BATT_2 0x94 -#define SYM_BATT_1 0x95 -#define SYM_BATT_EMPTY 0x96 - -// Vario -#define SYM_VARIO_UP_2A 0xA2 -#define SYM_VARIO_UP_1A 0xA3 -#define SYM_VARIO_DOWN_1A 0xA4 -#define SYM_VARIO_DOWN_2A 0xA5 - -// Glidescope -#define SYM_GLIDESCOPE 0xE0 - -// Batt Icon´s -#define SYM_MAIN_BATT 0x97 -#define SYM_VID_BAT 0xBF - -// Used for battery impedance -#define SYM_MILLIOHM 0x3F - -// Unit Icon´s (Metric) -#define SYM_MS 0x9F -#define SYM_KMH 0xA1 -#define SYM_ALT_M 177 -#define SYM_ALT_KM 178 -#define SYM_DIST_M 181 -#define SYM_DIST_KM 182 -#define SYM_M 185 -#define SYM_KM 186 - -// Unit Icon´s (Imperial) -#define SYM_FTS 0x99 -#define SYM_MPH 0xB0 -#define SYM_ALT_FT 179 -#define SYM_ALT_KFT 180 -#define SYM_DIST_FT 183 -#define SYM_DIST_MI 184 -#define SYM_FT 0x0F -#define SYM_MI 187 - -// Voltage and amperage -#define SYM_VOLT 0x06 -#define SYM_AMP 0x9A -#define SYM_MAH 0x07 -#define SYM_WH 0xAB -#define SYM_WATT 0xAE - -// Efficiency -#define SYM_MAH_KM_0 157 -#define SYM_MAH_KM_1 158 -#define SYM_WH_KM_0 172 -#define SYM_WH_KM_1 173 - -// Note, these change with scrolling enabled (scrolling is TODO) -//#define SYM_AH_DECORATION_LEFT 0x13 -//#define SYM_AH_DECORATION_RIGHT 0x13 -#define SYM_AH_DECORATION_MIN 16 -#define SYM_AH_DECORATION 19 -#define SYM_AH_DECORATION_MAX 21 -#define SYM_AH_DECORATION_COUNT (SYM_AH_DECORATION_MAX - SYM_AH_DECORATION_MIN + 1) - -// Time -#define SYM_ON_M 0x9B -#define SYM_FLY_M 0x9C -#define SYM_ON_H 0x70 -#define SYM_FLY_H 0x71 -#define SYM_CLOCK 0xBC - -// Throttle Position (%) -#define SYM_THR 0x04 -#define SYM_THR1 0x05 - -#define SYM_AUTO_THR0 202 -#define SYM_AUTO_THR1 203 - -// RSSI -#define SYM_RSSI 0x01 - -// Menu cursor -#define SYM_CURSOR SYM_AH_LEFT - -// Air speed and wind -#define SYM_AIR 151 -#define SYM_WIND_HORIZONTAL 22 -#define SYM_WIND_VERTICAL 23 - -//Misc -#define SYM_COLON 0x2D -#define SYM_ZERO_HALF_TRAILING_DOT 192 -#define SYM_ZERO_HALF_LEADING_DOT 208 - -//sport -#define SYM_MIN 0xB3 -#define SYM_AVG 0xB4 - -// Attitude angles -#define SYM_ROLL_LEFT 0xCC -#define SYM_ROLL_LEVEL 0xCD -#define SYM_ROLL_RIGHT 0xCE -#define SYM_PITCH_UP 0xCF -#define SYM_PITCH_DOWN 0xDF +#define SYM_RSSI 0x01 // 001 Icon RSSI +#define SYM_AH_RIGHT 0x02 // 002 Arrow left +#define SYM_AH_LEFT 0x03 // 003 Arrow right +#define SYM_THR 0x04 // 004 Throttle +#define SYM_AH_DECORATION_UP 0x05 // 005 Arrow up AHI +#define SYM_VOLT 0x06 // 006 V +#define SYM_MAH 0x07 // 007 MAH + +// 0x08 // 008 - +// 0x09 // 009 - +// 0x0A // 010 - +// 0x0B // 011 - +// 0x0C // 012 - + +#define SYM_TEMP_F 0x0D // 013 °F +#define SYM_TEMP_C 0x0E // 014 °C +#define SYM_FT 0x0F // 015 FT + +#define SYM_AH_DECORATION_MIN 0x10 // 016 to 021 Scrolling +#define SYM_AH_DECORATION 0x13 // 019 Scrolling +#define SYM_AH_DECORATION_MAX 0x15 // 021 Scrolling +#define SYM_AH_DECORATION_COUNT (SYM_AH_DECORATION_MAX - SYM_AH_DECORATION_MIN + 1) // Scrolling + +#define SYM_WIND_HORIZONTAL 0x16 // 022 Air speed horizontal +#define SYM_WIND_VERTICAL 0x17 // 023 Air speed vertical + +#define SYM_HEADING_N 0x18 // 024 Heading Graphic north +#define SYM_HEADING_S 0x19 // 025 Heading Graphic south +#define SYM_HEADING_E 0x1A // 026 Heading Graphic east +#define SYM_HEADING_W 0x1B // 027 Heading Graphic west +#define SYM_HEADING_DIVIDED_LINE 0x1C // 028 Heading Graphic +#define SYM_HEADING_LINE 0x1D // 029 Heading Graphic + +#define SYM_SAT_L 0x1E // 030 Sats left +#define SYM_SAT_R 0x1F // 031 Sats right + +#define SYM_BLANK 0x20 // 032 blank (space) + +// 0x21 // 033 ASCII ! + +#define SYM_TRIP_DIST 0x22 // 034 Icon total distance + +// 0x23 // 035 ASCII # + +#define SYM_AH_DECORATION_DOWN 0x24 // 036 Arrow down AHI + +// 0x25 // 037 ASCII % + +#define SYM_AH_CH_LEFT 0x26 // 038 Crossair left +#define SYM_AH_CH_RIGHT 0x27 // 039 Crossair right + +// 0x28 // 040 to 062 ASCII + +#define SYM_MILLIOHM 0x3F // 063 battery impedance Mohm + +// 0x40 // 064 to 095 ASCII + +#define SYM_ARROW_UP 0x60 // 096 Direction arrow 0° +#define SYM_ARROW_2 0x61 // 097 Direction arrow 22.5° +#define SYM_ARROW_3 0x62 // 098 Direction arrow 45° +#define SYM_ARROW_4 0x63 // 099 Direction arrow 67.5° +#define SYM_ARROW_RIGHT 0x64 // 100 Direction arrow 90° +#define SYM_ARROW_6 0x65 // 101 Direction arrow 112.5° +#define SYM_ARROW_7 0x66 // 102 Direction arrow 135° +#define SYM_ARROW_8 0x67 // 103 Direction arrow 157.5° +#define SYM_ARROW_DOWN 0x68 // 104 Direction arrow 180° +#define SYM_ARROW_10 0x69 // 105 Direction arrow 202.5° +#define SYM_ARROW_11 0x6A // 106 Direction arrow 225° +#define SYM_ARROW_12 0x6B // 107 Direction arrow 247.5° +#define SYM_ARROW_LEFT 0x6C // 108 Direction arrow 270° +#define SYM_ARROW_14 0x6D // 109 Direction arrow 292.5° +#define SYM_ARROW_15 0x6E // 110 Direction arrow 315° +#define SYM_ARROW_16 0x6F // 111 Direction arrow 337.5° + +#define SYM_ON_H 0x70 // 112 ON HR +#define SYM_FLY_H 0x71 // 113 FLY HR + +#define SYM_DIRECTION 0x72 // 114 to 121, directional little arrows + +#define SYM_HOME_NEAR 0x7A // 122 Home, near + +// 0x7B // 123 to 125 ASCII + +#define SYM_AH_CH_CENTER 0x7E // 126 Crossair center + +// 0x7F // 127 - + +#define SYM_AH_H_START 0x80 // 128 to 136 Horizontal AHI + +#define SYM_3D_KMH 0x89 // 137 KM/H 3D +#define SYM_3D_MPH 0x8A // 138 MPH 3D + +// 0x8B // 139 - +// 0x8C // 140 - +// 0x8D // 141 - +// 0x8E // 142 - +// 0x8F // 143 - + +#define SYM_BATT_FULL 0x90 // 144 Battery full +#define SYM_BATT_5 0x91 // 145 Battery +#define SYM_BATT_4 0x92 // 146 Battery +#define SYM_BATT_3 0x93 // 147 Battery +#define SYM_BATT_2 0x94 // 148 Battery +#define SYM_BATT_1 0x95 // 149 Battery +#define SYM_BATT_EMPTY 0x96 // 150 Battery empty + +#define SYM_AIR 0x97 // 151 Air speed +// 0x98 // 152 Home point map +#define SYM_FTS 0x99 // 153 FT/S +#define SYM_AMP 0x9A // 154 A +#define SYM_ON_M 0x9B // 155 On MN +#define SYM_FLY_M 0x9C // 156 FL MN +#define SYM_MAH_KM_0 0x9D // 157 MAH/KM left +#define SYM_MAH_KM_1 0x9E // 158 MAH/KM right +#define SYM_MS 0x9F // 159 M/S +#define SYM_HOME_DIST 0xA0 // 160 DIS +#define SYM_KMH 0xA1 // 161 KM/H + +#define SYM_VARIO_UP_2A 0xA2 // 162 Vario up up +#define SYM_VARIO_UP_1A 0xA3 // 163 Vario up +#define SYM_VARIO_DOWN_1A 0xA4 // 164 Vario down +#define SYM_VARIO_DOWN_2A 0xA5 // 165 Vario down down + +#define SYM_LAT 0xA6 // 166 GPS LAT +#define SYM_LON 0xA7 // 167 GPS LON +#define SYM_DEGREES 0xA8 // 168 ° heading angle +#define SYM_HEADING 0xA9 // 169 Compass Heading symbol +#define SYM_ALT 0xAA // 170 ALT +#define SYM_WH 0xAB // 171 WH +#define SYM_WH_KM_0 0xAC // 172 WH/KM left +#define SYM_WH_KM_1 0xAD // 173 WH/KM right +#define SYM_WATT 0xAE // 174 W +#define SYM_SCALE 0xAF // 175 Map scale +#define SYM_MPH 0xB0 // 176 MPH +#define SYM_ALT_M 0xB1 // 177 ALT M +#define SYM_ALT_KM 0xB2 // 178 ALT KM +#define SYM_ALT_FT 0xB3 // 179 ALT FT +#define SYM_ALT_KFT 0xB4 // 180 DIS KFT +#define SYM_DIST_M 0xB5 // 181 DIS M +#define SYM_DIST_KM 0xB6 // 182 DIM KM +#define SYM_DIST_FT 0xB7 // 183 DIS FT +#define SYM_DIST_MI 0xB8 // 184 DIS MI +#define SYM_M 0xB9 // 185 M +#define SYM_KM 0xBA // 186 KM +#define SYM_MI 0xBB // 187 MI + +#define SYM_CLOCK 0xBC // 188 Clock +#define SYM_HDP_L 0xBD // 189 HDOP left +#define SYM_HDP_R 0xBE // 190 HDOP right +#define SYM_HOME 0xBF // 191 Home icon + +#define SYM_ZERO_HALF_TRAILING_DOT 0xC0 // 192 to 201 Numbers with trailing dot + +#define SYM_AUTO_THR0 0xCA // 202 Auto-throttle left +#define SYM_AUTO_THR1 0xCB // 203 Auto-throttle right + +#define SYM_ROLL_LEFT 0xCC // 204 Sym roll left +#define SYM_ROLL_LEVEL 0xCD // 205 Sym roll horizontal +#define SYM_ROLL_RIGHT 0xCE // 206 Sym roll right +#define SYM_PITCH_UP 0xCF // 207 Pitch up + +#define SYM_ZERO_HALF_LEADING_DOT 0xD0 // 208 to 217 Numbers with leading dot + +#define SYM_AH_CH_AIRCRAFT0 0xDA // 218 Crossair aircraft left +#define SYM_AH_CH_AIRCRAFT1 0xDB // 219 Crossair aircraft +#define SYM_AH_CH_AIRCRAFT2 0xDC // 220 Crossair aircraft center +#define SYM_AH_CH_AIRCRAFT3 0xDD // 221 Crossair aircraft +#define SYM_AH_CH_AIRCRAFT4 0xDE // 222 Crossair aircraft right + +#define SYM_PITCH_DOWN 0xDF // 223 Pitch down + +#define SYM_AH_V_START 0xE0 // 224 to 229 Vertical AHI + +#define SYM_BARO_TEMP 0xF0 +#define SYM_IMU_TEMP 0xF1 +#define SYM_TEMP 0xF2 + +#define SYM_TEMP_SENSOR_FIRST 0xF2 +#define SYM_TEMP_SENSOR_LAST 0xF7 +#define TEMP_SENSOR_SYM_COUNT (SYM_TEMP_SENSOR_LAST - SYM_TEMP_SENSOR_FIRST + 1) + +#define SYM_LOGO_START 0x101 // 257 to 280, INAV logo +#define SYM_LOGO_WIDTH 6 +#define SYM_LOGO_HEIGHT 4 + +#define SYM_CURSOR SYM_AH_LEFT // Menu cursor #endif // USE_MAX7456 diff --git a/src/main/drivers/nvic.h b/src/main/drivers/nvic.h index 100159b0c63..52aa554e80a 100644 --- a/src/main/drivers/nvic.h +++ b/src/main/drivers/nvic.h @@ -11,29 +11,13 @@ #define NVIC_PRIO_GYRO_INT_EXTI NVIC_BUILD_PRIORITY(0x0f, 0x0f) #define NVIC_PRIO_MAG_INT_EXTI NVIC_BUILD_PRIORITY(0x0f, 0x0f) #define NVIC_PRIO_WS2811_DMA NVIC_BUILD_PRIORITY(1, 2) // TODO - is there some reason to use high priority? (or to use DMA IRQ at all?) -#define NVIC_PRIO_SERIALUART1_TXDMA NVIC_BUILD_PRIORITY(1, 1) -#define NVIC_PRIO_SERIALUART1_RXDMA NVIC_BUILD_PRIORITY(1, 1) #define NVIC_PRIO_SERIALUART1 NVIC_BUILD_PRIORITY(1, 1) -#define NVIC_PRIO_SERIALUART2_TXDMA NVIC_BUILD_PRIORITY(1, 0) -#define NVIC_PRIO_SERIALUART2_RXDMA NVIC_BUILD_PRIORITY(1, 1) #define NVIC_PRIO_SERIALUART2 NVIC_BUILD_PRIORITY(1, 2) -#define NVIC_PRIO_SERIALUART3_TXDMA NVIC_BUILD_PRIORITY(1, 0) -#define NVIC_PRIO_SERIALUART3_RXDMA NVIC_BUILD_PRIORITY(1, 1) #define NVIC_PRIO_SERIALUART3 NVIC_BUILD_PRIORITY(1, 2) -#define NVIC_PRIO_SERIALUART4_TXDMA NVIC_BUILD_PRIORITY(1, 0) -#define NVIC_PRIO_SERIALUART4_RXDMA NVIC_BUILD_PRIORITY(1, 1) #define NVIC_PRIO_SERIALUART4 NVIC_BUILD_PRIORITY(1, 2) -#define NVIC_PRIO_SERIALUART5_TXDMA NVIC_BUILD_PRIORITY(1, 0) -#define NVIC_PRIO_SERIALUART5_RXDMA NVIC_BUILD_PRIORITY(1, 1) #define NVIC_PRIO_SERIALUART5 NVIC_BUILD_PRIORITY(1, 2) -#define NVIC_PRIO_SERIALUART6_TXDMA NVIC_BUILD_PRIORITY(1, 0) -#define NVIC_PRIO_SERIALUART6_RXDMA NVIC_BUILD_PRIORITY(1, 1) #define NVIC_PRIO_SERIALUART6 NVIC_BUILD_PRIORITY(1, 2) -#define NVIC_PRIO_SERIALUART7_TXDMA NVIC_BUILD_PRIORITY(1, 0) -#define NVIC_PRIO_SERIALUART7_RXDMA NVIC_BUILD_PRIORITY(1, 1) #define NVIC_PRIO_SERIALUART7 NVIC_BUILD_PRIORITY(1, 2) -#define NVIC_PRIO_SERIALUART8_TXDMA NVIC_BUILD_PRIORITY(1, 0) -#define NVIC_PRIO_SERIALUART8_RXDMA NVIC_BUILD_PRIORITY(1, 1) #define NVIC_PRIO_SERIALUART8 NVIC_BUILD_PRIORITY(1, 2) #define NVIC_PRIO_I2C_ER NVIC_BUILD_PRIORITY(0, 0) #define NVIC_PRIO_I2C_EV NVIC_BUILD_PRIORITY(0, 0) diff --git a/src/main/drivers/pinio.c b/src/main/drivers/pinio.c new file mode 100644 index 00000000000..c093b78d828 --- /dev/null +++ b/src/main/drivers/pinio.c @@ -0,0 +1,104 @@ +/* + * This file is part of Cleanflight, Betaflight and INAV + * + * Cleanflight, Betaflight and INAV are free software. You can + * redistribute this software and/or modify this software under + * the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, + * or (at your option) any later version. + * + * Cleanflight, Betaflight and INAV are distributed in the hope that + * they will be useful, but WITHOUT ANY WARRANTY; without even the + * implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR + * PURPOSE. See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +#include + +#include "platform.h" + +#ifdef USE_PINIO + +#include "build/debug.h" +#include "common/memory.h" +#include "drivers/io.h" +#include "drivers/pinio.h" + +/*** Hardware definitions ***/ +const pinioHardware_t pinioHardware[] = { +#if defined(PINIO1_PIN) + { .ioTag = IO_TAG(PINIO1_PIN), .ioMode = IOCFG_OUT_PP, .flags = 0 }, +#endif + +#if defined(PINIO2_PIN) + { .ioTag = IO_TAG(PINIO2_PIN), .ioMode = IOCFG_OUT_PP, .flags = 0 }, +#endif + +#if defined(PINIO3_PIN) + { .ioTag = IO_TAG(PINIO3_PIN), .ioMode = IOCFG_OUT_PP, .flags = 0 }, +#endif + +#if defined(PINIO4_PIN) + { .ioTag = IO_TAG(PINIO4_PIN), .ioMode = IOCFG_OUT_PP, .flags = 0 }, +#endif +}; + +const int pinioHardwareCount = sizeof(pinioHardware) / sizeof(pinioHardware[0]); + +/*** Runtime configuration ***/ +typedef struct pinioRuntime_s { + IO_t io; + bool inverted; + bool state; +} pinioRuntime_t; + +static pinioRuntime_t pinioRuntime[PINIO_COUNT]; + +void pinioInit(void) +{ + if (pinioHardwareCount == 0) { + return; + } + + for (int i = 0; i < pinioHardwareCount; i++) { + IO_t io = IOGetByTag(pinioHardware[i].ioTag); + + if (!io) { + continue; + } + + IOInit(io, OWNER_PINIO, RESOURCE_OUTPUT, RESOURCE_INDEX(i)); + IOConfigGPIO(io, pinioHardware[i].ioMode); + + if (pinioHardware[i].flags & PINIO_FLAGS_INVERTED) { + pinioRuntime[i].inverted = true; + IOHi(io); + } else { + pinioRuntime[i].inverted = false; + IOLo(io); + } + + pinioRuntime[i].io = io; + pinioRuntime[i].state = false; + } +} + +void pinioSet(int index, bool on) +{ + const bool newState = on ^ pinioRuntime[index].inverted; + + if (!pinioRuntime[index].io) { + return; + } + + if (newState != pinioRuntime[index].state) { + IOWrite(pinioRuntime[index].io, newState); + pinioRuntime[index].state = newState; + } +} +#endif diff --git a/src/main/drivers/pinio.h b/src/main/drivers/pinio.h new file mode 100644 index 00000000000..baecc35a181 --- /dev/null +++ b/src/main/drivers/pinio.h @@ -0,0 +1,41 @@ +/* + * This file is part of Cleanflight, Betaflight and INAV + * + * Cleanflight, Betaflight and INAV are free software. You can + * redistribute this software and/or modify this software under + * the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, + * or (at your option) any later version. + * + * Cleanflight, Betaflight and INAV are distributed in the hope that + * they will be useful, but WITHOUT ANY WARRANTY; without even the + * implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR + * PURPOSE. See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +#pragma once + +#include +#include + +#include "drivers/io_types.h" + +#define PINIO_COUNT 4 +#define PINIO_FLAGS_INVERTED 0x80 + +typedef struct pinioHardware_s { + ioTag_t ioTag; + ioConfig_t ioMode; + uint16_t flags; +} pinioHardware_t; + +extern const pinioHardware_t pinioHardware[]; +extern const int pinioHardwareCount; + +void pinioInit(void); +void pinioSet(int index, bool on); diff --git a/src/main/drivers/pitotmeter.h b/src/main/drivers/pitotmeter.h index f4a75a64ff8..474e488d6cc 100644 --- a/src/main/drivers/pitotmeter.h +++ b/src/main/drivers/pitotmeter.h @@ -20,7 +20,7 @@ struct pitotDev_s; -typedef void (*pitotOpFuncPtr)(struct pitotDev_s * pitot); // pitot start operation +typedef bool (*pitotOpFuncPtr)(struct pitotDev_s * pitot); // pitot start operation typedef void (*pitotCalculateFuncPtr)(struct pitotDev_s * pitot, float *pressure, float *temperature); // airspeed calculation (filled params are pressure and temperature) typedef struct pitotDev_s { diff --git a/src/main/drivers/pitotmeter_adc.c b/src/main/drivers/pitotmeter_adc.c index 19fd114a295..d0c6a1ac0e7 100755 --- a/src/main/drivers/pitotmeter_adc.c +++ b/src/main/drivers/pitotmeter_adc.c @@ -38,14 +38,16 @@ #define PITOT_ADC_VOLTAGE_ZERO (2.5f) // Pressure offset is 2.5V #define PITOT_ADC_VOLTAGE_TO_PRESSURE (1000.0f) // 1V/kPa = 1000 Pa/V -static void adcPitotStart(pitotDev_t *pitot) +static bool adcPitotStart(pitotDev_t *pitot) { UNUSED(pitot); + return true; } -static void adcPitotRead(pitotDev_t *pitot) +static bool adcPitotRead(pitotDev_t *pitot) { UNUSED(pitot); + return true; } static void adcPitotCalculate(pitotDev_t *pitot, float *pressure, float *temperature) diff --git a/src/main/drivers/pitotmeter_ms4525.c b/src/main/drivers/pitotmeter_ms4525.c index 9e1f30afa49..b21c35bb8f8 100644 --- a/src/main/drivers/pitotmeter_ms4525.c +++ b/src/main/drivers/pitotmeter_ms4525.c @@ -19,86 +19,125 @@ #include #include +#include +#include "common/utils.h" +#include "common/maths.h" #include "drivers/bus_i2c.h" -#include "pitotmeter.h" +#include "drivers/pitotmeter.h" #include "drivers/time.h" -#include "common/utils.h" - // MS4525, Standard address 0x28 #define MS4525_ADDR 0x28 -static uint16_t ms4525_ut; // static result of temperature measurement -static uint16_t ms4525_up; // static result of pressure measurement -static uint8_t rxbuf[4]; +typedef struct __attribute__ ((__packed__)) ms4525Ctx_s { + bool dataValid; + uint32_t ms4525_ut; + uint32_t ms4525_up; +} ms4525Ctx_t; + +STATIC_ASSERT(sizeof(ms4525Ctx_t) < BUS_SCRATCHPAD_MEMORY_SIZE, busDevice_scratchpad_memory_too_small); -static void ms4525_start(pitotDev_t * pitot) +static bool ms4525_start(pitotDev_t * pitot) { - busReadBuf( pitot->busDev, 0xFF, rxbuf, 4 ); + uint8_t rxbuf[1]; + bool ack = busReadBuf(pitot->busDev, 0xFF, rxbuf, 1); + return ack; } -static void ms4525_read(pitotDev_t * pitot) +static bool ms4525_read(pitotDev_t * pitot) { - if (busReadBuf( pitot->busDev, 0xFF, rxbuf, 4 )) { - ms4525_up = (rxbuf[0] << 8) | (rxbuf[1] << 0); - ms4525_ut = ((rxbuf[2] << 8) | (rxbuf[3] << 0))>>5; + uint8_t rxbuf1[4]; + uint8_t rxbuf2[4]; + + ms4525Ctx_t * ctx = busDeviceGetScratchpadMemory(pitot->busDev); + ctx->dataValid = false; + + if (!busReadBuf(pitot->busDev, 0xFF, rxbuf1, 4)) { + return false; } + + if (!busReadBuf(pitot->busDev, 0xFF, rxbuf2, 4)) { + return false; + } + + const uint8_t status = ((rxbuf1[0] & 0xC0) >> 6); + if (status == 2 || status == 3) { + return false; + } + + int16_t dP_raw1, dT_raw1; + int16_t dP_raw2, dT_raw2; + + dP_raw1 = 0x3FFF & ((rxbuf1[0] << 8) + rxbuf1[1]); + dT_raw1 = (0xFFE0 & ((rxbuf1[2] << 8) + rxbuf1[3])) >> 5; + dP_raw2 = 0x3FFF & ((rxbuf2[0] << 8) + rxbuf2[1]); + dT_raw2 = (0xFFE0 & ((rxbuf2[2] << 8) + rxbuf2[3])) >> 5; + + // reject any double reads where the value has shifted in the upper more than 0xFF + if (ABS(dP_raw1 - dP_raw2) > 0xFF || ABS(dT_raw1 - dT_raw2) > 0xFF) { + return false; + } + + // Data valid, update ut/up values + ctx->dataValid = true; + ctx->ms4525_up = (dP_raw1 + dP_raw2) / 2; + ctx->ms4525_ut = (dT_raw1 + dT_raw2) / 2; + return true; } static void ms4525_calculate(pitotDev_t * pitot, float *pressure, float *temperature) { - UNUSED(pitot); - - uint8_t status = (ms4525_up & 0xC000) >> 14; - switch (status) { - case 0: - break; - case 1: - /* fallthrough */ - case 2: - /* fallthrough */ - case 3: - return; - } - int16_t dp_raw = 0, dT_raw = 0; + ms4525Ctx_t * ctx = busDeviceGetScratchpadMemory(pitot->busDev); - /* mask the used bits */ - dp_raw = 0x3FFF & ms4525_up; - dT_raw = ms4525_ut; + const float P_max = 1.0f; + const float P_min = -P_max; + const float PSI_to_Pa = 6894.757f; - float dP = (10 * (int32_t)(dp_raw)) * 0.1052120688f; - float T = (float)(200 * (int32_t)dT_raw - 102350) / 2047 + 273.15f; + //float dP = ctx->ms4525_up * 10.0f * 0.1052120688f; + const float dP_psi = -((ctx->ms4525_up - 0.1f * 16383) * (P_max - P_min) / (0.8f * 16383) + P_min); + float dP = dP_psi * PSI_to_Pa; + float T = (float)(200.0f * (int32_t)ctx->ms4525_ut) / 2047.0f - 50.0f + 273.15f; - if (pressure) + if (pressure) { *pressure = dP; // Pa - if (temperature) + } + + if (temperature) { *temperature = T; // K + } } bool ms4525Detect(pitotDev_t * pitot) { + uint8_t rxbuf[4]; + bool ack = false; + pitot->busDev = busDeviceInit(BUSTYPE_I2C, DEVHW_MS4525, 0, OWNER_AIRSPEED); if (pitot->busDev == NULL) { return false; } - bool ack = false; - // Read twice to fix: // Sending a start-stop condition without any transitions on the SCL line (no clock pulses in between) creates a // communication error for the next communication, even if the next start condition is correct and the clock pulse is applied. // An additional start condition must be sent, which results in restoration of proper communication. - ack = busReadBuf( pitot->busDev, 0xFF, rxbuf, 4 ); - ack = busReadBuf( pitot->busDev, 0xFF, rxbuf, 4 ); + ack = busReadBuf(pitot->busDev, 0xFF, rxbuf, 4); + ack = busReadBuf(pitot->busDev, 0xFF, rxbuf, 4); if (!ack) { return false; } + // Initialize busDev data + ms4525Ctx_t * ctx = busDeviceGetScratchpadMemory(pitot->busDev); + ctx->dataValid = false; + ctx->ms4525_ut = 0; + ctx->ms4525_up = 0; + + // Initialize pitotDev object pitot->delay = 10000; pitot->start = ms4525_start; pitot->get = ms4525_read; pitot->calculate = ms4525_calculate; - ms4525_read(pitot); return true; } diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c index e43b31a0f84..2bf6fbe290e 100644 --- a/src/main/drivers/pwm_mapping.c +++ b/src/main/drivers/pwm_mapping.c @@ -69,7 +69,7 @@ pwmIOConfiguration_t *pwmInit(drv_pwm_config_t *init) const timerHardware_t *timerHardwarePtr = &timerHardware[timerIndex]; int type = MAP_TO_NONE; -#if defined(STM32F303xC) && defined(USE_UART3) +#if defined(STM32F3) && defined(USE_UART3) // skip UART3 ports (PB10/PB11) if (init->useUART3 && (timerHardwarePtr->tag == IO_TAG(UART3_TX_PIN) || timerHardwarePtr->tag == IO_TAG(UART3_RX_PIN))) { addBootlogEvent6(BOOT_EVENT_TIMER_CH_SKIPPED, BOOT_EVENT_FLAGS_WARNING, timerIndex, pwmIOConfiguration.motorCount, pwmIOConfiguration.servoCount, 3); @@ -85,36 +85,34 @@ pwmIOConfiguration_t *pwmInit(drv_pwm_config_t *init) #endif #if defined(USE_SOFTSERIAL1) - const timerHardware_t *ss1TimerHardware = timerGetByTag(IO_TAG(SOFTSERIAL_1_RX_PIN), TIM_USE_ANY); - if (init->useSoftSerial && ss1TimerHardware != NULL && ss1TimerHardware->tim == timerHardwarePtr->tim) { - addBootlogEvent6(BOOT_EVENT_TIMER_CH_SKIPPED, BOOT_EVENT_FLAGS_WARNING, timerIndex, pwmIOConfiguration.motorCount, pwmIOConfiguration.servoCount, 3); - continue; + if (init->useSoftSerial) { + const timerHardware_t *ss1TimerHardware = timerGetByTag(IO_TAG(SOFTSERIAL_1_RX_PIN), TIM_USE_ANY); + if (ss1TimerHardware != NULL && ss1TimerHardware->tim == timerHardwarePtr->tim) { + addBootlogEvent6(BOOT_EVENT_TIMER_CH_SKIPPED, BOOT_EVENT_FLAGS_WARNING, timerIndex, pwmIOConfiguration.motorCount, pwmIOConfiguration.servoCount, 3); + continue; + } } #endif #if defined(USE_SOFTSERIAL2) - const timerHardware_t *ss2TimerHardware = timerGetByTag(IO_TAG(SOFTSERIAL_2_RX_PIN), TIM_USE_ANY); - if (init->useSoftSerial && ss2TimerHardware != NULL && ss2TimerHardware->tim == timerHardwarePtr->tim) { - addBootlogEvent6(BOOT_EVENT_TIMER_CH_SKIPPED, BOOT_EVENT_FLAGS_WARNING, timerIndex, pwmIOConfiguration.motorCount, pwmIOConfiguration.servoCount, 3); - continue; + if (init->useSoftSerial) { + const timerHardware_t *ss2TimerHardware = timerGetByTag(IO_TAG(SOFTSERIAL_2_RX_PIN), TIM_USE_ANY); + if (ss2TimerHardware != NULL && ss2TimerHardware->tim == timerHardwarePtr->tim) { + addBootlogEvent6(BOOT_EVENT_TIMER_CH_SKIPPED, BOOT_EVENT_FLAGS_WARNING, timerIndex, pwmIOConfiguration.motorCount, pwmIOConfiguration.servoCount, 3); + continue; + } } #endif -#ifdef WS2811_TIMER +#if defined(USE_LED_STRIP) // skip LED Strip output if (init->useLEDStrip) { - if (timerHardwarePtr->tim == WS2811_TIMER) { + const timerHardware_t * ledTimHw = timerGetByTag(IO_TAG(WS2811_PIN), TIM_USE_ANY); + if (ledTimHw != NULL && timerHardwarePtr->tim == ledTimHw->tim) { addBootlogEvent6(BOOT_EVENT_TIMER_CH_SKIPPED, BOOT_EVENT_FLAGS_WARNING, timerIndex, pwmIOConfiguration.motorCount, pwmIOConfiguration.servoCount, 3); continue; } -#if defined(STM32F303xC) && defined(WS2811_PIN) - if (timerHardwarePtr->tag == IO_TAG(WS2811_PIN)) { - addBootlogEvent6(BOOT_EVENT_TIMER_CH_SKIPPED, BOOT_EVENT_FLAGS_WARNING, timerIndex, pwmIOConfiguration.motorCount, pwmIOConfiguration.servoCount, 3); - continue; - } -#endif } - #endif #ifdef VBAT_ADC_PIN @@ -166,8 +164,7 @@ pwmIOConfiguration_t *pwmInit(drv_pwm_config_t *init) if (init->useServoOutputs && (timerHardwarePtr->usageFlags & TIM_USE_MC_SERVO)) { type = MAP_TO_SERVO_OUTPUT; } - else - if (timerHardwarePtr->usageFlags & TIM_USE_MC_MOTOR) { + else if (timerHardwarePtr->usageFlags & TIM_USE_MC_MOTOR) { type = MAP_TO_MOTOR_OUTPUT; } } else { @@ -186,7 +183,7 @@ pwmIOConfiguration_t *pwmInit(drv_pwm_config_t *init) if (type == MAP_TO_PPM_INPUT) { #if defined(USE_RX_PPM) - ppmInConfig(timerHardwarePtr, init->pwmProtocolType); + ppmInConfig(timerHardwarePtr); pwmIOConfiguration.ioConfigurations[pwmIOConfiguration.ioCount].flags = PWM_PF_PPM; pwmIOConfiguration.ppmInputCount++; @@ -208,7 +205,7 @@ pwmIOConfiguration_t *pwmInit(drv_pwm_config_t *init) continue; } - if (pwmMotorConfig(timerHardwarePtr, pwmIOConfiguration.motorCount, init->motorPwmRate, init->idlePulse, init->pwmProtocolType, init->enablePWMOutput)) { + if (pwmMotorConfig(timerHardwarePtr, pwmIOConfiguration.motorCount, init->motorPwmRate, init->pwmProtocolType, init->enablePWMOutput)) { if (init->useFastPwm) { pwmIOConfiguration.ioConfigurations[pwmIOConfiguration.ioCount].flags = PWM_PF_MOTOR | PWM_PF_OUTPUT_PROTOCOL_FASTPWM | PWM_PF_OUTPUT_PROTOCOL_PWM; } else if (init->pwmProtocolType == PWM_TYPE_BRUSHED) { diff --git a/src/main/drivers/pwm_mapping.h b/src/main/drivers/pwm_mapping.h index f383995ed2c..11929d12954 100644 --- a/src/main/drivers/pwm_mapping.h +++ b/src/main/drivers/pwm_mapping.h @@ -33,7 +33,7 @@ #define MAX_SERVOS 8 #endif -#define PWM_TIMER_MHZ 1 +#define PWM_TIMER_HZ 1000000 #define PULSE_1MS (1000) // 1ms pulse width @@ -68,8 +68,6 @@ typedef struct drv_pwm_config_s { uint16_t servoCenterPulse; uint8_t pwmProtocolType; uint16_t motorPwmRate; - uint16_t idlePulse; // PWM value to use when initializing the driver. set this to either PULSE_1MS (regular pwm), - // some higher value (used by 3d mode), or 0, for brushed pwm drivers. rangefinderIOConfig_t rangefinderIOConfig; } drv_pwm_config_t; diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index 2c264871836..94925f941f1 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -18,15 +18,18 @@ #include #include #include +#include #include "platform.h" #include "build/debug.h" +#include "common/maths.h" + #include "drivers/io.h" -#include "timer.h" -#include "pwm_mapping.h" -#include "pwm_output.h" -#include "io_pca9685.h" +#include "drivers/timer.h" +#include "drivers/pwm_mapping.h" +#include "drivers/pwm_output.h" +#include "drivers/io_pca9685.h" #include "io/pwmdriver_i2c.h" @@ -35,16 +38,40 @@ #include "fc/config.h" #include "fc/runtime_config.h" -#define MULTISHOT_5US_PW (MULTISHOT_TIMER_MHZ * 5) -#define MULTISHOT_20US_MULT (MULTISHOT_TIMER_MHZ * 20 / 1000.0f) +#define MULTISHOT_5US_PW (MULTISHOT_TIMER_HZ * 5 / 1000000.0f) +#define MULTISHOT_20US_MULT (MULTISHOT_TIMER_HZ * 20 / 1000000.0f / 1000.0f) + +#ifdef USE_DSHOT +#define MOTOR_DSHOT1200_HZ 24000000 +#define MOTOR_DSHOT600_HZ 12000000 +#define MOTOR_DSHOT300_HZ 6000000 +#define MOTOR_DSHOT150_HZ 3000000 + + +#define DSHOT_MOTOR_BIT_0 7 +#define DSHOT_MOTOR_BIT_1 14 +#define DSHOT_MOTOR_BITLENGTH 19 + +#define DSHOT_DMA_BUFFER_SIZE 18 /* resolution + frame reset (2us) */ +#endif typedef void (*pwmWriteFuncPtr)(uint8_t index, uint16_t value); // function pointer used to write motors typedef struct { - volatile timCCR_t *ccr; - TIM_TypeDef *tim; - uint16_t period; + TCH_t * tch; pwmWriteFuncPtr pwmWritePtr; + bool configured; + uint16_t value; // Used to keep track of last motor value + + // PWM parameters + volatile timCCR_t *ccr; // Shortcut for timer CCR register + float pulseOffset; + float pulseScale; + +#ifdef USE_DSHOT + // DSHOT parameters + uint32_t dmaBuffer[DSHOT_DMA_BUFFER_SIZE] __attribute__ ((aligned (4))); +#endif } pwmOutputPort_t; static pwmOutputPort_t pwmOutputPorts[MAX_PWM_OUTPUT_PORTS]; @@ -52,6 +79,13 @@ static pwmOutputPort_t pwmOutputPorts[MAX_PWM_OUTPUT_PORTS]; static pwmOutputPort_t *motors[MAX_PWM_MOTORS]; static pwmOutputPort_t *servos[MAX_PWM_SERVOS]; +#ifdef USE_DSHOT + +static bool isProtocolDshot = false; +static timeUs_t dshotMotorUpdateIntervalUs = 0; +static timeUs_t dshotMotorLastUpdateUs; +#endif + #ifdef BEEPER_PWM static pwmOutputPort_t beeperPwmPort; static pwmOutputPort_t *beeperPwm; @@ -62,38 +96,40 @@ static uint8_t allocatedOutputPortCount = 0; static bool pwmMotorsEnabled = true; -static void pwmOutConfigTimer(pwmOutputPort_t * p, const timerHardware_t *timerHardware, uint8_t mhz, uint16_t period, uint16_t value) +static void pwmOutConfigTimer(pwmOutputPort_t * p, TCH_t * tch, uint32_t hz, uint16_t period, uint16_t value) { - timerConfigBase(timerHardware->tim, period, mhz); - timerPWMConfigChannel(timerHardware->tim, timerHardware->channel, timerHardware->output & TIMER_OUTPUT_N_CHANNEL, timerHardware->output & TIMER_OUTPUT_INVERTED, value); - - if (timerHardware->output & TIMER_OUTPUT_ENABLED) { - timerPWMStart(timerHardware->tim, timerHardware->channel, timerHardware->output & TIMER_OUTPUT_N_CHANNEL); - } + p->tch = tch; - timerEnable(timerHardware->tim); + timerConfigBase(p->tch, period, hz); + timerPWMConfigChannel(p->tch, value); + timerPWMStart(p->tch); - p->ccr = timerCCR(timerHardware->tim, timerHardware->channel); - p->period = period; - p->tim = timerHardware->tim; + timerEnable(p->tch); + p->ccr = timerCCR(p->tch); *p->ccr = 0; } -static pwmOutputPort_t *pwmOutConfigMotor(const timerHardware_t *timerHardware, uint8_t mhz, uint16_t period, uint16_t value, bool enableOutput) +static pwmOutputPort_t *pwmOutConfigMotor(const timerHardware_t *timHw, uint32_t hz, uint16_t period, uint16_t value, bool enableOutput) { if (allocatedOutputPortCount >= MAX_PWM_OUTPUT_PORTS) { DEBUG_TRACE("Attempt to allocate PWM output beyond MAX_PWM_OUTPUT_PORTS"); return NULL; } + // Attempt to allocate TCH + TCH_t * tch = timerGetTCH(timHw); + if (tch == NULL) { + return NULL; + } + pwmOutputPort_t *p = &pwmOutputPorts[allocatedOutputPortCount++]; - const IO_t io = IOGetByTag(timerHardware->tag); + const IO_t io = IOGetByTag(timHw->tag); IOInit(io, OWNER_MOTOR, RESOURCE_OUTPUT, allocatedOutputPortCount); if (enableOutput) { - IOConfigGPIOAF(io, IOCFG_AF_PP, timerHardware->alternateFunction); + IOConfigGPIOAF(io, IOCFG_AF_PP, timHw->alternateFunction); } else { // If PWM outputs are disabled - configure as GPIO and drive low @@ -101,37 +137,15 @@ static pwmOutputPort_t *pwmOutConfigMotor(const timerHardware_t *timerHardware, IOLo(io); } - pwmOutConfigTimer(p, timerHardware, mhz, period, value); + pwmOutConfigTimer(p, tch, hz, period, value); return p; } -static void pwmWriteBrushed(uint8_t index, uint16_t value) -{ - *motors[index]->ccr = (value - 1000) * motors[index]->period / 1000; -} - -#ifndef BRUSHED_MOTORS static void pwmWriteStandard(uint8_t index, uint16_t value) { - *motors[index]->ccr = value; -} - -static void pwmWriteOneShot125(uint8_t index, uint16_t value) -{ - *motors[index]->ccr = lrintf((float)(value * ONESHOT125_TIMER_MHZ/8.0f)); -} - -static void pwmWriteOneShot42(uint8_t index, uint16_t value) -{ - *motors[index]->ccr = lrintf((float)(value * ONESHOT42_TIMER_MHZ/24.0f)); + *motors[index]->ccr = lrintf((value * motors[index]->pulseScale) + motors[index]->pulseOffset); } -static void pwmWriteMultiShot(uint8_t index, uint16_t value) -{ - *motors[index]->ccr = lrintf(((float)(value-1000) * MULTISHOT_20US_MULT) + MULTISHOT_5US_PW); -} -#endif - void pwmWriteMotor(uint8_t index, uint16_t value) { if (motors[index] && index < MAX_MOTORS && pwmMotorsEnabled) { @@ -157,56 +171,190 @@ void pwmEnableMotors(void) pwmMotorsEnabled = true; } -bool isMotorBrushed(uint16_t motorPwmRate) +bool isMotorBrushed(uint16_t motorPwmRateHz) +{ + return (motorPwmRateHz > 500); +} + +static pwmOutputPort_t * motorConfigPwm(const timerHardware_t *timerHardware, float sMin, float sLen, uint32_t motorPwmRateHz, bool enableOutput) +{ + const uint32_t baseClockHz = timerGetBaseClockHW(timerHardware); + const uint32_t prescaler = ((baseClockHz / motorPwmRateHz) + 0xffff) / 0x10000; /* rounding up */ + const uint32_t timerHz = baseClockHz / prescaler; + const uint32_t period = timerHz / motorPwmRateHz; + + pwmOutputPort_t * port = pwmOutConfigMotor(timerHardware, timerHz, period, 0, enableOutput); + + if (port) { + port->pulseScale = ((sLen == 0) ? period : (sLen * timerHz)) / 1000.0f; + port->pulseOffset = (sMin * timerHz) - (port->pulseScale * 1000); + port->configured = true; + } + + return port; +} + +#ifdef USE_DSHOT +uint32_t getDshotHz(motorPwmProtocolTypes_e pwmProtocolType) +{ + switch (pwmProtocolType) { + case(PWM_TYPE_DSHOT1200): + return MOTOR_DSHOT1200_HZ; + case(PWM_TYPE_DSHOT600): + return MOTOR_DSHOT600_HZ; + case(PWM_TYPE_DSHOT300): + return MOTOR_DSHOT300_HZ; + default: + case(PWM_TYPE_DSHOT150): + return MOTOR_DSHOT150_HZ; + } +} + +static pwmOutputPort_t * motorConfigDshot(const timerHardware_t * timerHardware, motorPwmProtocolTypes_e proto, uint16_t motorPwmRateHz, bool enableOutput) +{ + // Try allocating new port + pwmOutputPort_t * port = pwmOutConfigMotor(timerHardware, getDshotHz(proto), DSHOT_MOTOR_BITLENGTH, 0, enableOutput); + + if (!port) { + return NULL; + } + + // Keep track of motor update interval + const timeUs_t motorIntervalUs = 1000000 / motorPwmRateHz; + dshotMotorUpdateIntervalUs = MAX(dshotMotorUpdateIntervalUs, motorIntervalUs); + + // Configure timer DMA + if (timerPWMConfigChannelDMA(port->tch, port->dmaBuffer, DSHOT_DMA_BUFFER_SIZE)) { + // Only mark as DSHOT channel if DMA was set successfully + memset(port->dmaBuffer, 0, sizeof(port->dmaBuffer)); + port->configured = true; + } + + return port; +} + +static void pwmWriteDshot(uint8_t index, uint16_t value) +{ + // DMA operation might still be running. Cache value for future use + motors[index]->value = value; +} + +static void loadDmaBufferDshot(uint32_t * dmaBuffer, uint16_t packet) +{ + for (int i = 0; i < 16; i++) { + dmaBuffer[i] = (packet & 0x8000) ? DSHOT_MOTOR_BIT_1 : DSHOT_MOTOR_BIT_0; // MSB first + packet <<= 1; + } +} + +static uint16_t prepareDshotPacket(const uint16_t value, bool requestTelemetry) +{ + uint16_t packet = (value << 1) | (requestTelemetry ? 1 : 0); + + // compute checksum + int csum = 0; + int csum_data = packet; + for (int i = 0; i < 3; i++) { + csum ^= csum_data; // xor data by nibbles + csum_data >>= 4; + } + csum &= 0xf; + + // append checksum + packet = (packet << 4) | csum; + + return packet; +} + +void pwmCompleteDshotUpdate(uint8_t motorCount) +{ + // Get latest REAL time + timeUs_t currentTimeUs = micros(); + + // Enforce motor update rate + if (!isProtocolDshot || (dshotMotorUpdateIntervalUs == 0) || ((currentTimeUs - dshotMotorLastUpdateUs) <= dshotMotorUpdateIntervalUs)) { + return; + } + + dshotMotorLastUpdateUs = currentTimeUs; + + // Generate DMA buffers + for (int index = 0; index < motorCount; index++) { + if (motors[index] && motors[index]->configured) { + // TODO: ESC telemetry + uint16_t packet = prepareDshotPacket(motors[index]->value, false); + + loadDmaBufferDshot(motors[index]->dmaBuffer, packet); + timerPWMPrepareDMA(motors[index]->tch, DSHOT_DMA_BUFFER_SIZE); + } + } + + // Start DMA on all timers + for (int index = 0; index < motorCount; index++) { + if (motors[index] && motors[index]->configured) { + timerPWMStartDMA(motors[index]->tch); + } + } +} + +bool isMotorProtocolDshot(void) { - return (motorPwmRate > 500); + return isProtocolDshot; } +#endif -bool pwmMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse, motorPwmProtocolTypes_e proto, bool enableOutput) +bool pwmMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRateHz, motorPwmProtocolTypes_e proto, bool enableOutput) { - uint32_t timerMhzCounter; + pwmOutputPort_t * port = NULL; pwmWriteFuncPtr pwmWritePtr; - switch (proto) { #ifdef BRUSHED_MOTORS - default: + proto = PWM_TYPE_BRUSHED; // Override proto #endif + + switch (proto) { case PWM_TYPE_BRUSHED: - timerMhzCounter = PWM_BRUSHED_TIMER_MHZ; - pwmWritePtr = pwmWriteBrushed; - idlePulse = 0; + port = motorConfigPwm(timerHardware, 0.0f, 0.0f, motorPwmRateHz, enableOutput); + pwmWritePtr = pwmWriteStandard; break; -#ifndef BRUSHED_MOTORS case PWM_TYPE_ONESHOT125: - timerMhzCounter = ONESHOT125_TIMER_MHZ; - pwmWritePtr = pwmWriteOneShot125; + port = motorConfigPwm(timerHardware, 125e-6f, 125e-6f, motorPwmRateHz, enableOutput); + pwmWritePtr = pwmWriteStandard; break; case PWM_TYPE_ONESHOT42: - timerMhzCounter = ONESHOT42_TIMER_MHZ; - pwmWritePtr = pwmWriteOneShot42; + port = motorConfigPwm(timerHardware, 42e-6f, 42e-6f, motorPwmRateHz, enableOutput); + pwmWritePtr = pwmWriteStandard; break; case PWM_TYPE_MULTISHOT: - timerMhzCounter = MULTISHOT_TIMER_MHZ; - pwmWritePtr = pwmWriteMultiShot; + port = motorConfigPwm(timerHardware, 5e-6f, 20e-6f, motorPwmRateHz, enableOutput); + pwmWritePtr = pwmWriteStandard; break; - case PWM_TYPE_STANDARD: +#ifdef USE_DSHOT + case PWM_TYPE_DSHOT1200: + case PWM_TYPE_DSHOT600: + case PWM_TYPE_DSHOT300: + case PWM_TYPE_DSHOT150: + port = motorConfigDshot(timerHardware, proto, motorPwmRateHz, enableOutput); + if (port) { + isProtocolDshot = true; + pwmWritePtr = pwmWriteDshot; + } + break; +#endif + default: - timerMhzCounter = PWM_TIMER_MHZ; + case PWM_TYPE_STANDARD: + port = motorConfigPwm(timerHardware, 1e-3f, 1e-3f, motorPwmRateHz, enableOutput); pwmWritePtr = pwmWriteStandard; break; -#endif } - const uint32_t hz = timerMhzCounter * 1000000; - - pwmOutputPort_t * port = pwmOutConfigMotor(timerHardware, timerMhzCounter, hz / motorPwmRate, idlePulse, enableOutput); - if (port) { + port->pwmWritePtr = pwmWritePtr; motors[motorIndex] = port; - motors[motorIndex]->pwmWritePtr = pwmWritePtr; return true; } @@ -215,7 +363,7 @@ bool pwmMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, ui bool pwmServoConfig(const timerHardware_t *timerHardware, uint8_t servoIndex, uint16_t servoPwmRate, uint16_t servoCenterPulse, bool enableOutput) { - pwmOutputPort_t * port = pwmOutConfigMotor(timerHardware, PWM_TIMER_MHZ, 1000000 / servoPwmRate, servoCenterPulse, enableOutput); + pwmOutputPort_t * port = pwmOutConfigMotor(timerHardware, PWM_TIMER_HZ, PWM_TIMER_HZ / servoPwmRate, servoCenterPulse, enableOutput); if (port) { servos[servoIndex] = port; @@ -261,15 +409,21 @@ void pwmWriteBeeper(bool onoffBeep) void beeperPwmInit(ioTag_t tag, uint16_t frequency) { - const timerHardware_t *timer = timerGetByTag(tag, TIM_USE_BEEPER); - if (timer) { + beeperPwm = NULL; + + const timerHardware_t *timHw = timerGetByTag(tag, TIM_USE_BEEPER); + + if (timHw) { + // Attempt to allocate TCH + TCH_t * tch = timerGetTCH(timHw); + if (tch == NULL) { + return NULL; + } + beeperPwm = &beeperPwmPort; beeperFrequency = frequency; - IOConfigGPIOAF(IOGetByTag(tag), IOCFG_AF_PP, timer->alternateFunction); - pwmOutConfigTimer(beeperPwm, timer, PWM_TIMER_MHZ, 1000000 / beeperFrequency, (1000000 / beeperFrequency) / 2); - } - else { - beeperPwm = NULL; + IOConfigGPIOAF(IOGetByTag(tag), IOCFG_AF_PP, timHw->alternateFunction); + pwmOutConfigTimer(beeperPwm, tch, PWM_TIMER_HZ, 1000000 / beeperFrequency, (1000000 / beeperFrequency) / 2); } } #endif diff --git a/src/main/drivers/pwm_output.h b/src/main/drivers/pwm_output.h index 3943e254688..a7b6adfbf47 100644 --- a/src/main/drivers/pwm_output.h +++ b/src/main/drivers/pwm_output.h @@ -18,41 +18,31 @@ #pragma once #include "drivers/io_types.h" - -#if defined(STM32F40_41xxx) // must be multiples of timer clock -#define ONESHOT125_TIMER_MHZ 12 -#define ONESHOT42_TIMER_MHZ 21 -#define MULTISHOT_TIMER_MHZ 84 -#define PWM_BRUSHED_TIMER_MHZ 21 -#elif defined(STM32F7) -#define ONESHOT125_TIMER_MHZ 9 -#define ONESHOT42_TIMER_MHZ 27 -#define MULTISHOT_TIMER_MHZ 54 -#define PWM_BRUSHED_TIMER_MHZ 27 -#else -#define ONESHOT125_TIMER_MHZ 8 -#define ONESHOT42_TIMER_MHZ 24 -#define MULTISHOT_TIMER_MHZ 72 -#define PWM_BRUSHED_TIMER_MHZ 24 -#endif +#include "drivers/time.h" typedef enum { PWM_TYPE_STANDARD = 0, PWM_TYPE_ONESHOT125, PWM_TYPE_ONESHOT42, PWM_TYPE_MULTISHOT, - PWM_TYPE_BRUSHED + PWM_TYPE_BRUSHED, + PWM_TYPE_DSHOT150, + PWM_TYPE_DSHOT300, + PWM_TYPE_DSHOT600, + PWM_TYPE_DSHOT1200, } motorPwmProtocolTypes_e; void pwmWriteMotor(uint8_t index, uint16_t value); void pwmShutdownPulsesForAllMotors(uint8_t motorCount); +void pwmCompleteDshotUpdate(uint8_t motorCount); +bool isMotorProtocolDshot(void); void pwmWriteServo(uint8_t index, uint16_t value); void pwmDisableMotors(void); void pwmEnableMotors(void); struct timerHardware_s; -bool pwmMotorConfig(const struct timerHardware_s *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse, motorPwmProtocolTypes_e proto, bool enableOutput); +bool pwmMotorConfig(const struct timerHardware_s *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, motorPwmProtocolTypes_e proto, bool enableOutput); bool pwmServoConfig(const struct timerHardware_s *timerHardware, uint8_t servoIndex, uint16_t servoPwmRate, uint16_t servoCenterPulse, bool enableOutput); void pwmWriteBeeper(bool onoffBeep); void beeperPwmInit(ioTag_t tag, uint16_t frequency); \ No newline at end of file diff --git a/src/main/drivers/rangefinder/rangefinder_vl53l0x.c b/src/main/drivers/rangefinder/rangefinder_vl53l0x.c index 6c229a8e215..d911348ca67 100755 --- a/src/main/drivers/rangefinder/rangefinder_vl53l0x.c +++ b/src/main/drivers/rangefinder/rangefinder_vl53l0x.c @@ -805,6 +805,9 @@ static void vl53l0x_Init(rangefinderDev_t * rangefinder) uint8_t byte; isInitialized = false; + // During init set timeout to a higher value + setTimeout(500); + // VL53L0X_DataInit() begin // Switch sensor to 2.8V mode byte = readReg(rangefinder->busDev, VL53L0X_REG_VHV_CONFIG_PAD_SCL_SDA__EXTSUP_HV); diff --git a/src/main/drivers/resource.c b/src/main/drivers/resource.c index e2e3d5997b5..dafe60fd3de 100644 --- a/src/main/drivers/resource.c +++ b/src/main/drivers/resource.c @@ -21,7 +21,7 @@ const char * const ownerNames[OWNER_TOTAL_COUNT] = { "FREE", "PWM/IO", "PWM", "PPM", "MOTOR", "SERVO", "SOFTSERIAL", "ADC", "SERIAL", "DEBUG", "TIMER", "RANGEFINDER", "SYSTEM", "SPI", "I2C", "SDCARD", "FLASH", "USB", "BEEPER", "OSD", "BARO", "MPU", "INVERTER", "LED STRIP", "LED", "RECEIVER", "TRANSMITTER", - "NRF24", "VTX", "SPI_PREINIT", "COMPASS", "AIRSPEED", "OLED DISPLAY", + "NRF24", "VTX", "SPI_PREINIT", "COMPASS", "AIRSPEED", "OLED DISPLAY", "PINIO" }; const char * const resourceNames[RESOURCE_TOTAL_COUNT] = { diff --git a/src/main/drivers/resource.h b/src/main/drivers/resource.h index e9cfd3a1ff9..fbbbbdba27f 100644 --- a/src/main/drivers/resource.h +++ b/src/main/drivers/resource.h @@ -51,8 +51,11 @@ typedef enum { OWNER_VTX, OWNER_SPI_PREINIT, OWNER_COMPASS, + OWNER_TEMPERATURE, + OWNER_1WIRE, OWNER_AIRSPEED, OWNER_OLED_DISPLAY, + OWNER_PINIO, OWNER_TOTAL_COUNT } resourceOwner_e; diff --git a/src/main/drivers/rx_pwm.c b/src/main/drivers/rx_pwm.c index 8858c58c2ad..1a9b86bfb8e 100644 --- a/src/main/drivers/rx_pwm.c +++ b/src/main/drivers/rx_pwm.c @@ -71,9 +71,8 @@ typedef struct { uint8_t missedEvents; - const timerHardware_t *timerHardware; - timerCCHandlerRec_t edgeCb; - timerOvrHandlerRec_t overflowCb; + timerCallbacks_t cb; + const TCH_t * tch; } pwmInputPort_t; static pwmInputPort_t pwmInputPorts[PWM_INPUT_PORT_COUNT]; @@ -170,9 +169,9 @@ static void ppmInit(void) ppmDev.overflowed = false; } -static void ppmOverflowCallback(timerOvrHandlerRec_t* cbRec, captureCompare_t capture) +static void ppmOverflowCallback(struct TCH_s * tch, uint32_t capture) { - UNUSED(cbRec); + UNUSED(tch); ppmISREvent(SOURCE_OVERFLOW, capture); ppmDev.largeCounter += capture + 1; @@ -181,9 +180,9 @@ static void ppmOverflowCallback(timerOvrHandlerRec_t* cbRec, captureCompare_t ca } } -static void ppmEdgeCallback(timerCCHandlerRec_t* cbRec, captureCompare_t capture) +static void ppmEdgeCallback(struct TCH_s * tch, uint32_t capture) { - UNUSED(cbRec); + UNUSED(tch); ppmISREvent(SOURCE_EDGE, capture); int32_t i; @@ -303,10 +302,10 @@ bool isPWMDataBeingReceived(void) return false; } -static void pwmOverflowCallback(timerOvrHandlerRec_t* cbRec, captureCompare_t capture) +static void pwmOverflowCallback(struct TCH_s * tch, uint32_t capture) { UNUSED(capture); - pwmInputPort_t *pwmInputPort = container_of(cbRec, pwmInputPort_t, overflowCb); + pwmInputPort_t *pwmInputPort = (pwmInputPort_t *)tch->cb->callbackParam; if (++pwmInputPort->missedEvents > MAX_MISSED_PWM_EVENTS) { captures[pwmInputPort->channel] = PPM_RCVR_TIMEOUT; @@ -314,15 +313,14 @@ static void pwmOverflowCallback(timerOvrHandlerRec_t* cbRec, captureCompare_t ca } } -static void pwmEdgeCallback(timerCCHandlerRec_t *cbRec, captureCompare_t capture) +static void pwmEdgeCallback(struct TCH_s * tch, uint32_t capture) { - pwmInputPort_t *pwmInputPort = container_of(cbRec, pwmInputPort_t, edgeCb); - const timerHardware_t *timerHardwarePtr = pwmInputPort->timerHardware; + pwmInputPort_t *pwmInputPort = (pwmInputPort_t *)tch->cb->callbackParam; if (pwmInputPort->state == 0) { pwmInputPort->rise = capture; pwmInputPort->state = 1; - timerChConfigIC(timerHardwarePtr, false, INPUT_FILTER_TICKS); + timerChConfigIC(tch, false, INPUT_FILTER_TICKS); } else { pwmInputPort->fall = capture; @@ -331,13 +329,13 @@ static void pwmEdgeCallback(timerCCHandlerRec_t *cbRec, captureCompare_t capture pwmInputPort->capture = pwmInputPort->fall - pwmInputPort->rise; } else { - pwmInputPort->capture = (pwmInputPort->fall + timerGetPeriod(timerHardwarePtr)) - pwmInputPort->rise; + pwmInputPort->capture = (pwmInputPort->fall + timerGetPeriod(tch)) - pwmInputPort->rise; } captures[pwmInputPort->channel] = pwmInputPort->capture; // switch state pwmInputPort->state = 0; - timerChConfigIC(timerHardwarePtr, true, INPUT_FILTER_TICKS); + timerChConfigIC(tch, true, INPUT_FILTER_TICKS); pwmInputPort->missedEvents = 0; } } @@ -346,74 +344,52 @@ void pwmInConfig(const timerHardware_t *timerHardwarePtr, uint8_t channel) { pwmInputPort_t *self = &pwmInputPorts[channel]; + TCH_t * tch = timerGetTCH(timerHardwarePtr); + if (tch == NULL) { + return; + } + self->state = 0; self->missedEvents = 0; self->channel = channel; self->mode = INPUT_MODE_PWM; - self->timerHardware = timerHardwarePtr; IO_t io = IOGetByTag(timerHardwarePtr->tag); IOInit(io, OWNER_PWMINPUT, RESOURCE_INPUT, RESOURCE_INDEX(channel)); IOConfigGPIOAF(io, timerHardwarePtr->ioMode, timerHardwarePtr->alternateFunction); - timerConfigure(timerHardwarePtr, (uint16_t)PWM_TIMER_PERIOD, PWM_TIMER_MHZ); - timerChCCHandlerInit(&self->edgeCb, pwmEdgeCallback); - timerChOvrHandlerInit(&self->overflowCb, pwmOverflowCallback); - timerChConfigCallbacks(timerHardwarePtr, &self->edgeCb, &self->overflowCb); - timerChConfigIC(timerHardwarePtr, true, INPUT_FILTER_TICKS); + timerConfigure(tch, (uint16_t)PWM_TIMER_PERIOD, PWM_TIMER_HZ); + timerChInitCallbacks(&self->cb, (void*)self, &pwmEdgeCallback, &pwmOverflowCallback); + timerChConfigCallbacks(tch, &self->cb); + timerChConfigIC(tch, true, INPUT_FILTER_TICKS); + timerChCaptureEnable(tch); } #define UNUSED_PPM_TIMER_REFERENCE 0 #define FIRST_PWM_PORT 0 -void ppmAvoidPWMTimerClash(const timerHardware_t *timerHardwarePtr, uint8_t motorPwmProtocol) +void ppmInConfig(const timerHardware_t *timerHardwarePtr) { - for (int timerIndex = 0; timerIndex < timerHardwareCount; timerIndex++) { - // If PPM input timer is also mapped to motor - set PPM divisor accordingly - if (((timerHardware[timerIndex].usageFlags & (TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR)) == 0) || timerHardware[timerIndex].tim != timerHardwarePtr->tim) - continue; - - switch (motorPwmProtocol) { - case PWM_TYPE_ONESHOT125: - ppmCountDivisor = ONESHOT125_TIMER_MHZ; - break; - case PWM_TYPE_ONESHOT42: - ppmCountDivisor = ONESHOT42_TIMER_MHZ; - break; - case PWM_TYPE_MULTISHOT: - ppmCountDivisor = MULTISHOT_TIMER_MHZ; - break; - case PWM_TYPE_BRUSHED: - ppmCountDivisor = PWM_BRUSHED_TIMER_MHZ; - break; - default: - break; - } - + TCH_t * tch = timerGetTCH(timerHardwarePtr); + if (tch == NULL) { return; } -} -void ppmInConfig(const timerHardware_t *timerHardwarePtr, uint8_t motorPwmProtocol) -{ ppmInit(); - ppmAvoidPWMTimerClash(timerHardwarePtr, motorPwmProtocol); - pwmInputPort_t *self = &pwmInputPorts[FIRST_PWM_PORT]; self->mode = INPUT_MODE_PPM; - self->timerHardware = timerHardwarePtr; IO_t io = IOGetByTag(timerHardwarePtr->tag); IOInit(io, OWNER_PPMINPUT, RESOURCE_INPUT, 0); IOConfigGPIOAF(io, timerHardwarePtr->ioMode, timerHardwarePtr->alternateFunction); - timerConfigure(timerHardwarePtr, (uint16_t)PPM_TIMER_PERIOD, PWM_TIMER_MHZ); - timerChCCHandlerInit(&self->edgeCb, ppmEdgeCallback); - timerChOvrHandlerInit(&self->overflowCb, ppmOverflowCallback); - timerChConfigCallbacks(timerHardwarePtr, &self->edgeCb, &self->overflowCb); - timerChConfigIC(timerHardwarePtr, true, INPUT_FILTER_TICKS); + timerConfigure(tch, (uint16_t)PPM_TIMER_PERIOD, PWM_TIMER_HZ); + timerChInitCallbacks(&self->cb, (void*)self, &ppmEdgeCallback, &ppmOverflowCallback); + timerChConfigCallbacks(tch, &self->cb); + timerChConfigIC(tch, true, INPUT_FILTER_TICKS); + timerChCaptureEnable(tch); } uint16_t ppmRead(uint8_t channel) diff --git a/src/main/drivers/rx_pwm.h b/src/main/drivers/rx_pwm.h index bf213311f8b..8757cf6e1cc 100644 --- a/src/main/drivers/rx_pwm.h +++ b/src/main/drivers/rx_pwm.h @@ -25,7 +25,7 @@ typedef enum { #define PPM_RCVR_TIMEOUT 0 struct timerHardware_s; -void ppmInConfig(const struct timerHardware_s *timerHardwarePtr, uint8_t motorPwmProtocol); +void ppmInConfig(const struct timerHardware_s *timerHardwarePtr); void pwmInConfig(const struct timerHardware_s *timerHardwarePtr, uint8_t channel); uint16_t pwmRead(uint8_t channel); diff --git a/src/main/drivers/sdcard.c b/src/main/drivers/sdcard.c index 204f9e04989..ef982305323 100644 --- a/src/main/drivers/sdcard.c +++ b/src/main/drivers/sdcard.c @@ -1,18 +1,18 @@ /* - * This file is part of Cleanflight. + * This file is part of INAV. * - * Cleanflight is free software: you can redistribute it and/or modify + * INAV is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * - * Cleanflight is distributed in the hope that it will be useful, + * INAV is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License - * along with Cleanflight. If not, see . + * along with INAV. If not, see . */ #include @@ -20,1123 +20,134 @@ #include "platform.h" -#ifdef USE_SDCARD +#if defined(USE_SDCARD) +#include "build/debug.h" #include "common/utils.h" +#include "drivers/time.h" #include "drivers/nvic.h" #include "drivers/io.h" -#include "dma.h" - -#include "drivers/bus_spi.h" -#include "drivers/time.h" - -#include "sdcard.h" -#include "sdcard_standard.h" -#include "blackbox/blackbox.h" - -#include "build/debug.h" - -#ifdef AFATFS_USE_INTROSPECTIVE_LOGGING - #define SDCARD_PROFILING -#endif - -#define SET_CS_HIGH IOHi(sdCardCsPin) -#define SET_CS_LOW IOLo(sdCardCsPin) - -#define SDCARD_INIT_NUM_DUMMY_BYTES 10 -#define SDCARD_MAXIMUM_BYTE_DELAY_FOR_CMD_REPLY 8 -// Chosen so that CMD8 will have the same CRC as CMD0: -#define SDCARD_IF_COND_CHECK_PATTERN 0xAB - -#define SDCARD_TIMEOUT_INIT_MILLIS 200 -#define SDCARD_MAX_CONSECUTIVE_FAILURES 8 - -/* Break up 512-byte SD card sectors into chunks of this size when writing without DMA to reduce the peak overhead - * per call to sdcard_poll(). - */ -#define SDCARD_NON_DMA_CHUNK_SIZE 256 - -#ifndef SDCARD_SPI_CLOCK -#define SDCARD_SPI_CLOCK SPI_CLOCK_STANDARD -#endif - -typedef enum { - // In these states we run at the initialization 400kHz clockspeed: - SDCARD_STATE_NOT_PRESENT = 0, - SDCARD_STATE_RESET, - SDCARD_STATE_CARD_INIT_IN_PROGRESS, - SDCARD_STATE_INITIALIZATION_RECEIVE_CID, - - // In these states we run at full clock speed - SDCARD_STATE_READY, - SDCARD_STATE_READING, - SDCARD_STATE_SENDING_WRITE, - SDCARD_STATE_WAITING_FOR_WRITE, - SDCARD_STATE_WRITING_MULTIPLE_BLOCKS, - SDCARD_STATE_STOPPING_MULTIPLE_BLOCK_WRITE, -} sdcardState_e; - -typedef struct sdcard_t { - struct { - uint8_t *buffer; - uint32_t blockIndex; - uint8_t chunkIndex; - - sdcard_operationCompleteCallback_c callback; - uint32_t callbackData; - -#ifdef SDCARD_PROFILING - timeUs_t profileStartTime; -#endif - } pendingOperation; +#include "drivers/bus.h" - uint32_t operationStartTime; +#include "drivers/sdcard.h" +#include "drivers/sdcard_impl.h" +#include "drivers/sdcard_standard.h" - uint8_t failureCount; +#include "scheduler/protothreads.h" - uint8_t version; - bool highCapacity; - - uint32_t multiWriteNextBlock; - uint32_t multiWriteBlocksRemain; - - sdcardState_e state; - - sdcardMetadata_t metadata; - sdcardCSD_t csd; - -#ifdef SDCARD_PROFILING - sdcard_profilerCallback_c profiler; -#endif -} sdcard_t; - -static sdcard_t sdcard; - -#ifdef SDCARD_DMA_CHANNEL_TX - static bool useDMAForTx; -#if defined(USE_HAL_DRIVER) - DMA_HandleTypeDef *sdDMAHandle; -#endif -#else - // DMA channel not available so we can hard-code this to allow the non-DMA paths to be stripped by optimization - static const bool useDMAForTx = false; -#endif - -STATIC_ASSERT(sizeof(sdcardCSD_t) == 16, sdcard_csd_bitfields_didnt_pack_properly); - -#ifdef SDCARD_DETECT_PIN -static IO_t sdCardDetectPin = IO_NONE; +#ifndef SDCARD_DETECT_PIN +#define SDCARD_DETECT_PIN NONE #endif -static IO_t sdCardCsPin = IO_NONE; +sdcard_t sdcard; void sdcardInsertionDetectDeinit(void) { -#ifdef SDCARD_DETECT_PIN - sdCardDetectPin = IOGetByTag(IO_TAG(SDCARD_DETECT_PIN)); - IOInit(sdCardDetectPin, OWNER_FREE, RESOURCE_NONE, 0); - IOConfigGPIO(sdCardDetectPin, IOCFG_IN_FLOATING); -#endif -} - -void sdcardInsertionDetectInit(void) -{ -#ifdef SDCARD_DETECT_PIN - sdCardDetectPin = IOGetByTag(IO_TAG(SDCARD_DETECT_PIN)); - IOInit(sdCardDetectPin, OWNER_SDCARD, RESOURCE_INPUT, 0); - IOConfigGPIO(sdCardDetectPin, IOCFG_IPU); -#endif -} - -/** - * Detect if a SD card is physically present in the memory slot. - */ -bool sdcard_isInserted(void) -{ - bool result = true; - -#ifdef SDCARD_DETECT_PIN - - result = IORead(sdCardDetectPin) != 0; - - if (blackboxConfig()->invertedCardDetection) { - result = !result; - } - -#endif - - return result; -} - -/** - * Returns true if the card has already been, or is currently, initializing and hasn't encountered enough errors to - * trip our error threshold and be disabled (i.e. our card is in and working!) - */ -bool sdcard_isFunctional(void) -{ - return sdcard.state != SDCARD_STATE_NOT_PRESENT; -} - -static void sdcard_select(void) -{ - SET_CS_LOW; -} - -static void sdcard_deselect(void) -{ - // As per the SD-card spec, give the card 8 dummy clocks so it can finish its operation - //spiTransferByte(SDCARD_SPI_INSTANCE, 0xFF); - - while (spiIsBusBusy(SDCARD_SPI_INSTANCE)) { - } - - SET_CS_HIGH; -} - -/** - * Handle a failure of an SD card operation by resetting the card back to its initialization phase. - * - * Increments the failure counter, and when the failure threshold is reached, disables the card until - * the next call to sdcard_init(). - */ -static void sdcard_reset(void) -{ - if (!sdcard_isInserted()) { - sdcard.state = SDCARD_STATE_NOT_PRESENT; - return; - } - - if (sdcard.state >= SDCARD_STATE_READY) { - spiSetSpeed(SDCARD_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON); - } + sdcard.cardDetectPin = IOGetByTag(IO_TAG(SDCARD_DETECT_PIN)); - sdcard.failureCount++; - if (sdcard.failureCount >= SDCARD_MAX_CONSECUTIVE_FAILURES) { - sdcard.state = SDCARD_STATE_NOT_PRESENT; - } else { - sdcard.operationStartTime = millis(); - sdcard.state = SDCARD_STATE_RESET; + if (sdcard.cardDetectPin) { + IOInit(sdcard.cardDetectPin, OWNER_FREE, RESOURCE_NONE, 0); + IOConfigGPIO(sdcard.cardDetectPin, IOCFG_IN_FLOATING); } } -/** - * The SD card spec requires 8 clock cycles to be sent by us on the bus after most commands so it can finish its - * processing of that command. The easiest way for us to do this is to just wait for the bus to become idle before - * we transmit a command, sending at least 8-bits onto the bus when we do so. - */ -static bool sdcard_waitForIdle(int maxBytesToWait) -{ - while (maxBytesToWait > 0) { - uint8_t b = spiTransferByte(SDCARD_SPI_INSTANCE, 0xFF); - if (b == 0xFF) { - return true; - } - maxBytesToWait--; - } - - return false; -} - -/** - * Wait for up to maxDelay 0xFF idle bytes to arrive from the card, returning the first non-idle byte found. - * - * Returns 0xFF on failure. - */ -static uint8_t sdcard_waitForNonIdleByte(int maxDelay) -{ - for (int i = 0; i < maxDelay + 1; i++) { // + 1 so we can wait for maxDelay '0xFF' bytes before reading a response byte afterwards - uint8_t response = spiTransferByte(SDCARD_SPI_INSTANCE, 0xFF); - - if (response != 0xFF) { - return response; - } - } - - return 0xFF; -} - -/** - * Waits up to SDCARD_MAXIMUM_BYTE_DELAY_FOR_CMD_REPLY bytes for the card to become ready, send a command to the card - * with the given argument, waits up to SDCARD_MAXIMUM_BYTE_DELAY_FOR_CMD_REPLY bytes for a reply, and returns the - * first non-0xFF byte of the reply. - * - * You must select the card first with sdcard_select() and deselect it afterwards with sdcard_deselect(). - * - * Upon failure, 0xFF is returned. - */ -static uint8_t sdcard_sendCommand(uint8_t commandCode, uint32_t commandArgument) -{ - uint8_t command[6] = { - 0x40 | commandCode, - commandArgument >> 24, - commandArgument >> 16, - commandArgument >> 8, - commandArgument, - 0x95 /* Static CRC. This CRC is valid for CMD0 with a 0 argument, and CMD8 with 0x1AB argument, which are the only - commands that require a CRC */ - }; - - // Go ahead and send the command even if the card isn't idle if this is the reset command - if (!sdcard_waitForIdle(SDCARD_MAXIMUM_BYTE_DELAY_FOR_CMD_REPLY) && commandCode != SDCARD_COMMAND_GO_IDLE_STATE) - return 0xFF; - - spiTransfer(SDCARD_SPI_INSTANCE, NULL, command, sizeof(command)); - - /* - * The card can take up to SDCARD_MAXIMUM_BYTE_DELAY_FOR_CMD_REPLY bytes to send the response, in the meantime - * it'll transmit 0xFF filler bytes. - */ - return sdcard_waitForNonIdleByte(SDCARD_MAXIMUM_BYTE_DELAY_FOR_CMD_REPLY); -} - -static uint8_t sdcard_sendAppCommand(uint8_t commandCode, uint32_t commandArgument) -{ - sdcard_sendCommand(SDCARD_COMMAND_APP_CMD, 0); - - return sdcard_sendCommand(commandCode, commandArgument); -} - -/** - * Sends an IF_COND message to the card to check its version and validate its voltage requirements. Sets the global - * sdCardVersion with the detected version (0, 1, or 2) and returns true if the card is compatible. - */ -static bool sdcard_validateInterfaceCondition(void) -{ - uint8_t ifCondReply[4]; - - sdcard.version = 0; - - sdcard_select(); - - uint8_t status = sdcard_sendCommand(SDCARD_COMMAND_SEND_IF_COND, (SDCARD_VOLTAGE_ACCEPTED_2_7_to_3_6 << 8) | SDCARD_IF_COND_CHECK_PATTERN); - - // Don't deselect the card right away, because we'll want to read the rest of its reply if it's a V2 card - - if (status == (SDCARD_R1_STATUS_BIT_ILLEGAL_COMMAND | SDCARD_R1_STATUS_BIT_IDLE)) { - // V1 cards don't support this command - sdcard.version = 1; - } else if (status == SDCARD_R1_STATUS_BIT_IDLE) { - spiTransfer(SDCARD_SPI_INSTANCE, ifCondReply, NULL, sizeof(ifCondReply)); - - /* - * We don't bother to validate the SDCard's operating voltage range since the spec requires it to accept our - * 3.3V, but do check that it echoed back our check pattern properly. - */ - if (ifCondReply[3] == SDCARD_IF_COND_CHECK_PATTERN) { - sdcard.version = 2; - } - } - - sdcard_deselect(); - - return sdcard.version > 0; -} - -static bool sdcard_readOCRRegister(uint32_t *result) -{ - sdcard_select(); - - uint8_t status = sdcard_sendCommand(SDCARD_COMMAND_READ_OCR, 0); - - uint8_t response[4]; - - spiTransfer(SDCARD_SPI_INSTANCE, response, NULL, sizeof(response)); - - if (status == 0) { - sdcard_deselect(); - - *result = (response[0] << 24) | (response[1] << 16) | (response[2] << 8) | response[3]; - - return true; - } else { - sdcard_deselect(); - - return false; - } -} - -typedef enum { - SDCARD_RECEIVE_SUCCESS, - SDCARD_RECEIVE_BLOCK_IN_PROGRESS, - SDCARD_RECEIVE_ERROR, -} sdcardReceiveBlockStatus_e; - -/** - * Attempt to receive a data block from the SD card. - * - * Return true on success, otherwise the card has not responded yet and you should retry later. - */ -static sdcardReceiveBlockStatus_e sdcard_receiveDataBlock(uint8_t *buffer, int count) +void sdcardInsertionDetectInit(void) { - uint8_t dataToken = sdcard_waitForNonIdleByte(8); - - if (dataToken == 0xFF) { - return SDCARD_RECEIVE_BLOCK_IN_PROGRESS; - } + sdcard.cardDetectPin = IOGetByTag(IO_TAG(SDCARD_DETECT_PIN)); - if (dataToken != SDCARD_SINGLE_BLOCK_READ_START_TOKEN) { - return SDCARD_RECEIVE_ERROR; + if (sdcard.cardDetectPin) { + IOInit(sdcard.cardDetectPin, OWNER_SDCARD, RESOURCE_INPUT, 0); + IOConfigGPIO(sdcard.cardDetectPin, IOCFG_IPU); } - - spiTransfer(SDCARD_SPI_INSTANCE, buffer, NULL, count); - - // Discard trailing CRC, we don't care - spiTransferByte(SDCARD_SPI_INSTANCE, 0xFF); - spiTransferByte(SDCARD_SPI_INSTANCE, 0xFF); - - return SDCARD_RECEIVE_SUCCESS; -} - -static bool sdcard_sendDataBlockFinish(void) -{ - // Send a dummy CRC - spiTransferByte(SDCARD_SPI_INSTANCE, 0x00); - spiTransferByte(SDCARD_SPI_INSTANCE, 0x00); - - uint8_t dataResponseToken = spiTransferByte(SDCARD_SPI_INSTANCE, 0xFF); - - /* - * Check if the card accepted the write (no CRC error / no address error) - * - * The lower 5 bits are structured as follows: - * | 0 | Status | 1 | - * | 0 | x x x | 1 | - * - * Statuses: - * 010 - Data accepted - * 101 - CRC error - * 110 - Write error - */ - return (dataResponseToken & 0x1F) == 0x05; } /** - * Begin sending a buffer of SDCARD_BLOCK_SIZE bytes to the SD card. + * Returns true if the card is physically inserted into the slot */ -static void sdcard_sendDataBlockBegin(uint8_t *buffer, bool multiBlockWrite) +bool sdcard_isInserted(void) { - // Card wants 8 dummy clock cycles between the write command's response and a data block beginning: - spiTransferByte(SDCARD_SPI_INSTANCE, 0xFF); - - spiTransferByte(SDCARD_SPI_INSTANCE, multiBlockWrite ? SDCARD_MULTIPLE_BLOCK_WRITE_START_TOKEN : SDCARD_SINGLE_BLOCK_WRITE_START_TOKEN); - - if (useDMAForTx) { -#ifdef SDCARD_DMA_CHANNEL_TX -#if defined(USE_HAL_DRIVER) - sdDMAHandle = spiSetDMATransmit(SDCARD_DMA_CHANNEL_TX, SDCARD_DMA_CHANNEL, SDCARD_SPI_INSTANCE, buffer, SDCARD_BLOCK_SIZE); + if (sdcard.cardDetectPin) { + bool result = (IORead(sdcard.cardDetectPin) != 0); +#if defined(SDCARD_DETECT_INVERTED) + return !result; #else - // Queue the transmission of the sector payload - DMA_InitTypeDef DMA_InitStructure; - - DMA_StructInit(&DMA_InitStructure); -#ifdef SDCARD_DMA_CHANNEL - DMA_InitStructure.DMA_Channel = SDCARD_DMA_CHANNEL; - DMA_InitStructure.DMA_Memory0BaseAddr = (uint32_t) buffer; - DMA_InitStructure.DMA_DIR = DMA_DIR_MemoryToPeripheral; -#else - DMA_InitStructure.DMA_M2M = DMA_M2M_Disable; - DMA_InitStructure.DMA_MemoryBaseAddr = (uint32_t) buffer; - DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralDST; -#endif - DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t) &SDCARD_SPI_INSTANCE->DR; - DMA_InitStructure.DMA_Priority = DMA_Priority_Low; - DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable; - DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte; - - DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable; - DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte; - - DMA_InitStructure.DMA_BufferSize = SDCARD_BLOCK_SIZE; - DMA_InitStructure.DMA_Mode = DMA_Mode_Normal; - - DMA_DeInit(SDCARD_DMA_CHANNEL_TX); - DMA_Init(SDCARD_DMA_CHANNEL_TX, &DMA_InitStructure); - - DMA_Cmd(SDCARD_DMA_CHANNEL_TX, ENABLE); - - SPI_I2S_DMACmd(SDCARD_SPI_INSTANCE, SPI_I2S_DMAReq_Tx, ENABLE); -#endif + return result; #endif } else { - // Send the first chunk now - spiTransfer(SDCARD_SPI_INSTANCE, NULL, buffer, SDCARD_NON_DMA_CHUNK_SIZE); - } -} - -static bool sdcard_receiveCID(void) -{ - uint8_t cid[16]; - - if (sdcard_receiveDataBlock(cid, sizeof(cid)) != SDCARD_RECEIVE_SUCCESS) { - return false; - } - - sdcard.metadata.manufacturerID = cid[0]; - sdcard.metadata.oemID = (cid[1] << 8) | cid[2]; - sdcard.metadata.productName[0] = cid[3]; - sdcard.metadata.productName[1] = cid[4]; - sdcard.metadata.productName[2] = cid[5]; - sdcard.metadata.productName[3] = cid[6]; - sdcard.metadata.productName[4] = cid[7]; - sdcard.metadata.productRevisionMajor = cid[8] >> 4; - sdcard.metadata.productRevisionMinor = cid[8] & 0x0F; - sdcard.metadata.productSerial = (cid[9] << 24) | (cid[10] << 16) | (cid[11] << 8) | cid[12]; - sdcard.metadata.productionYear = (((cid[13] & 0x0F) << 4) | (cid[14] >> 4)) + 2000; - sdcard.metadata.productionMonth = cid[14] & 0x0F; - - return true; -} - -static bool sdcard_fetchCSD(void) -{ - uint32_t readBlockLen, blockCount, blockCountMult; - uint64_t capacityBytes; - - sdcard_select(); - - /* The CSD command's data block should always arrive within 8 idle clock cycles (SD card spec). This is because - * the information about card latency is stored in the CSD register itself, so we can't use that yet! - */ - bool success = - sdcard_sendCommand(SDCARD_COMMAND_SEND_CSD, 0) == 0 - && sdcard_receiveDataBlock((uint8_t*) &sdcard.csd, sizeof(sdcard.csd)) == SDCARD_RECEIVE_SUCCESS - && SDCARD_GET_CSD_FIELD(sdcard.csd, 1, TRAILER) == 1; - - if (success) { - switch (SDCARD_GET_CSD_FIELD(sdcard.csd, 1, CSD_STRUCTURE_VER)) { - case SDCARD_CSD_STRUCTURE_VERSION_1: - // Block size in bytes (doesn't have to be 512) - readBlockLen = 1 << SDCARD_GET_CSD_FIELD(sdcard.csd, 1, READ_BLOCK_LEN); - blockCountMult = 1 << (SDCARD_GET_CSD_FIELD(sdcard.csd, 1, CSIZE_MULT) + 2); - blockCount = (SDCARD_GET_CSD_FIELD(sdcard.csd, 1, CSIZE) + 1) * blockCountMult; - - // We could do this in 32 bits but it makes the 2GB case awkward - capacityBytes = (uint64_t) blockCount * readBlockLen; - - // Re-express that capacity (max 2GB) in our standard 512-byte block size - sdcard.metadata.numBlocks = capacityBytes / SDCARD_BLOCK_SIZE; - break; - case SDCARD_CSD_STRUCTURE_VERSION_2: - sdcard.metadata.numBlocks = (SDCARD_GET_CSD_FIELD(sdcard.csd, 2, CSIZE) + 1) * 1024; - break; - default: - success = false; - } + return true; } - - sdcard_deselect(); - - return success; } /** - * Check if the SD Card has completed its startup sequence. Must be called with sdcard.state == SDCARD_STATE_INITIALIZATION. - * - * Returns true if the card has finished its init process. + * Dispatch */ -static bool sdcard_checkInitDone(void) -{ - sdcard_select(); - - uint8_t status = sdcard_sendAppCommand(SDCARD_ACOMMAND_SEND_OP_COND, sdcard.version == 2 ? 1 << 30 /* We support high capacity cards */ : 0); +sdcardVTable_t *sdcardVTable; - sdcard_deselect(); - - // When card init is complete, the idle bit in the response becomes zero. - return status == 0x00; -} - -/** - * Begin the initialization process for the SD card. This must be called first before any other sdcard_ routine. - */ -void sdcard_init(bool useDMA) +void sdcard_init(void) { -#ifdef SDCARD_DMA_CHANNEL_TX - useDMAForTx = useDMA; -#if !defined(USE_HAL_DRIVER) - dmaEnableClock(dmaFindHandlerIdentifier(SDCARD_DMA_CHANNEL_TX)); - DMA_Cmd(SDCARD_DMA_CHANNEL_TX, DISABLE); +#if defined(USE_SDCARD_SPI) + sdcardVTable = &sdcardSpiVTable; +#elif defined(USE_SDCARD_SDIO) + sdcardVTable = &sdcardSdioVTable; #endif -#else - // DMA is not available - (void) useDMA; -#endif - -#ifdef SDCARD_SPI_CS_PIN - sdCardCsPin = IOGetByTag(IO_TAG(SDCARD_SPI_CS_PIN)); - IOInit(sdCardCsPin, OWNER_SDCARD, RESOURCE_SPI_CS, 0); - IOConfigGPIO(sdCardCsPin, SPI_IO_CS_CFG); -#endif // SDCARD_SPI_CS_PIN - - // Max frequency is initially 400kHz - spiSetSpeed(SDCARD_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON); - - // SDCard wants 1ms minimum delay after power is applied to it - delay(1000); - - // Transmit at least 74 dummy clock cycles with CS high so the SD card can start up - SET_CS_HIGH; - spiTransfer(SDCARD_SPI_INSTANCE, NULL, NULL, SDCARD_INIT_NUM_DUMMY_BYTES); - - // Wait for that transmission to finish before we enable the SDCard, so it receives the required number of cycles: - int time = 100000; - while (spiIsBusBusy(SDCARD_SPI_INSTANCE)) { - if (time-- == 0) { - sdcard.state = SDCARD_STATE_NOT_PRESENT; - sdcard.failureCount++; - return; - } + if (sdcardVTable) { + sdcardVTable->init(); } - - sdcard.operationStartTime = millis(); - sdcard.state = SDCARD_STATE_RESET; - sdcard.failureCount = 0; } -static bool sdcard_setBlockLength(uint32_t blockLen) -{ - sdcard_select(); - - uint8_t status = sdcard_sendCommand(SDCARD_COMMAND_SET_BLOCKLEN, blockLen); - - sdcard_deselect(); - - return status == 0; -} - -/* - * Returns true if the card is ready to accept read/write commands. - */ -static bool sdcard_isReady(void) +bool sdcard_readBlock(uint32_t blockIndex, uint8_t *buffer, sdcard_operationCompleteCallback_c callback, uint32_t callbackData) { - return sdcard.state == SDCARD_STATE_READY || sdcard.state == SDCARD_STATE_WRITING_MULTIPLE_BLOCKS; + return sdcardVTable->readBlock(blockIndex, buffer, callback, callbackData); } -/** - * Send the stop-transmission token to complete a multi-block write. - * - * Returns: - * SDCARD_OPERATION_IN_PROGRESS - We're now waiting for that stop to complete, the card will enter - * the SDCARD_STATE_STOPPING_MULTIPLE_BLOCK_WRITE state. - * SDCARD_OPERATION_SUCCESS - The multi-block write finished immediately, the card will enter - * the SDCARD_READY state. - * - */ -static sdcardOperationStatus_e sdcard_endWriteBlocks(void) +sdcardOperationStatus_e sdcard_beginWriteBlocks(uint32_t blockIndex, uint32_t blockCount) { - sdcard.multiWriteBlocksRemain = 0; - - // 8 dummy clocks to guarantee N_WR clocks between the last card response and this token - spiTransferByte(SDCARD_SPI_INSTANCE, 0xFF); - - spiTransferByte(SDCARD_SPI_INSTANCE, SDCARD_MULTIPLE_BLOCK_WRITE_STOP_TOKEN); - - // Card may choose to raise a busy (non-0xFF) signal after at most N_BR (1 byte) delay - if (sdcard_waitForNonIdleByte(1) == 0xFF) { - sdcard.state = SDCARD_STATE_READY; - return SDCARD_OPERATION_SUCCESS; - } else { - sdcard.state = SDCARD_STATE_STOPPING_MULTIPLE_BLOCK_WRITE; - sdcard.operationStartTime = millis(); - - return SDCARD_OPERATION_IN_PROGRESS; - } + return sdcardVTable->beginWriteBlocks(blockIndex, blockCount); } -/** - * Call periodically for the SD card to perform in-progress transfers. - * - * Returns true if the card is ready to accept commands. - */ -bool sdcard_poll(void) -{ - uint8_t initStatus; - bool sendComplete; - -#ifdef SDCARD_PROFILING - bool profilingComplete; -#endif - - doMore: - switch (sdcard.state) { - case SDCARD_STATE_RESET: - sdcard_select(); - - initStatus = sdcard_sendCommand(SDCARD_COMMAND_GO_IDLE_STATE, 0); - - sdcard_deselect(); - - if (initStatus == SDCARD_R1_STATUS_BIT_IDLE) { - // Check card voltage and version - if (sdcard_validateInterfaceCondition()) { - - sdcard.state = SDCARD_STATE_CARD_INIT_IN_PROGRESS; - goto doMore; - } else { - // Bad reply/voltage, we ought to refrain from accessing the card. - sdcard.state = SDCARD_STATE_NOT_PRESENT; - } - } - break; - - case SDCARD_STATE_CARD_INIT_IN_PROGRESS: - if (sdcard_checkInitDone()) { - if (sdcard.version == 2) { - // Check for high capacity card - uint32_t ocr; - - if (!sdcard_readOCRRegister(&ocr)) { - sdcard_reset(); - goto doMore; - } - - sdcard.highCapacity = (ocr & (1 << 30)) != 0; - } else { - // Version 1 cards are always low-capacity - sdcard.highCapacity = false; - } - - // Now fetch the CSD and CID registers - if (sdcard_fetchCSD()) { - sdcard_select(); - - uint8_t status = sdcard_sendCommand(SDCARD_COMMAND_SEND_CID, 0); - - if (status == 0) { - // Keep the card selected to receive the response block - sdcard.state = SDCARD_STATE_INITIALIZATION_RECEIVE_CID; - goto doMore; - } else { - sdcard_deselect(); - - sdcard_reset(); - goto doMore; - } - } - } - break; - case SDCARD_STATE_INITIALIZATION_RECEIVE_CID: - if (sdcard_receiveCID()) { - sdcard_deselect(); - - /* The spec is a little iffy on what the default block size is for Standard Size cards (it can be changed on - * standard size cards) so let's just set it to 512 explicitly so we don't have a problem. - */ - if (!sdcard.highCapacity && !sdcard_setBlockLength(SDCARD_BLOCK_SIZE)) { - sdcard_reset(); - goto doMore; - } - - // Now we're done with init and we can switch to the full speed clock (<25MHz) - spiSetSpeed(SDCARD_SPI_INSTANCE, SDCARD_SPI_CLOCK); - - sdcard.multiWriteBlocksRemain = 0; - - sdcard.state = SDCARD_STATE_READY; - goto doMore; - } // else keep waiting for the CID to arrive - break; - case SDCARD_STATE_SENDING_WRITE: - // Have we finished sending the write yet? - sendComplete = false; - -#ifdef SDCARD_DMA_CHANNEL_TX -#if defined(USE_HAL_DRIVER) - //if (useDMAForTx && __HAL_DMA_GET_FLAG(sdDMAHandle, SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG) == SET) { - //if (useDMAForTx && HAL_DMA_PollForTransfer(sdDMAHandle, HAL_DMA_FULL_TRANSFER, HAL_MAX_DELAY) == HAL_OK) { - if (useDMAForTx && (sdDMAHandle->State == HAL_DMA_STATE_READY)) { - //__HAL_DMA_CLEAR_FLAG(sdDMAHandle, SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG); - - //__HAL_DMA_DISABLE(sdDMAHandle); - - // Drain anything left in the Rx FIFO (we didn't read it during the write) - while (__HAL_SPI_GET_FLAG(spiHandleByInstance(SDCARD_SPI_INSTANCE), SPI_FLAG_RXNE) == SET) { - SDCARD_SPI_INSTANCE->DR; - } - - // Wait for the final bit to be transmitted - while (spiIsBusBusy(SDCARD_SPI_INSTANCE)) { - } - - HAL_SPI_DMAStop(spiHandleByInstance(SDCARD_SPI_INSTANCE)); - - sendComplete = true; - } -#else -#ifdef SDCARD_DMA_CHANNEL - if (useDMAForTx && DMA_GetFlagStatus(SDCARD_DMA_CHANNEL_TX, SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG) == SET) { - DMA_ClearFlag(SDCARD_DMA_CHANNEL_TX, SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG); -#else - if (useDMAForTx && DMA_GetFlagStatus(SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG) == SET) { - DMA_ClearFlag(SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG); -#endif - - DMA_Cmd(SDCARD_DMA_CHANNEL_TX, DISABLE); - - // Drain anything left in the Rx FIFO (we didn't read it during the write) - while (SPI_I2S_GetFlagStatus(SDCARD_SPI_INSTANCE, SPI_I2S_FLAG_RXNE) == SET) { - SDCARD_SPI_INSTANCE->DR; - } - - // Wait for the final bit to be transmitted - while (spiIsBusBusy(SDCARD_SPI_INSTANCE)) { - } - - SPI_I2S_DMACmd(SDCARD_SPI_INSTANCE, SPI_I2S_DMAReq_Tx, DISABLE); - - sendComplete = true; - } -#endif -#endif - if (!useDMAForTx) { - // Send another chunk - spiTransfer(SDCARD_SPI_INSTANCE, NULL, sdcard.pendingOperation.buffer + SDCARD_NON_DMA_CHUNK_SIZE * sdcard.pendingOperation.chunkIndex, SDCARD_NON_DMA_CHUNK_SIZE); - - sdcard.pendingOperation.chunkIndex++; - - sendComplete = sdcard.pendingOperation.chunkIndex == SDCARD_BLOCK_SIZE / SDCARD_NON_DMA_CHUNK_SIZE; - } - - if (sendComplete) { - // Finish up by sending the CRC and checking the SD-card's acceptance/rejectance - if (sdcard_sendDataBlockFinish()) { - // The SD card is now busy committing that write to the card - sdcard.state = SDCARD_STATE_WAITING_FOR_WRITE; - sdcard.operationStartTime = millis(); - - // Since we've transmitted the buffer we can go ahead and tell the caller their operation is complete - if (sdcard.pendingOperation.callback) { - sdcard.pendingOperation.callback(SDCARD_BLOCK_OPERATION_WRITE, sdcard.pendingOperation.blockIndex, sdcard.pendingOperation.buffer, sdcard.pendingOperation.callbackData); - } - } else { - /* Our write was rejected! This could be due to a bad address but we hope not to attempt that, so assume - * the card is broken and needs reset. - */ - sdcard_reset(); - - // Announce write failure: - if (sdcard.pendingOperation.callback) { - sdcard.pendingOperation.callback(SDCARD_BLOCK_OPERATION_WRITE, sdcard.pendingOperation.blockIndex, NULL, sdcard.pendingOperation.callbackData); - } - - goto doMore; - } - } - break; - case SDCARD_STATE_WAITING_FOR_WRITE: - if (sdcard_waitForIdle(SDCARD_MAXIMUM_BYTE_DELAY_FOR_CMD_REPLY)) { -#ifdef SDCARD_PROFILING - profilingComplete = true; -#endif - - sdcard.failureCount = 0; // Assume the card is good if it can complete a write - - // Still more blocks left to write in a multi-block chain? - if (sdcard.multiWriteBlocksRemain > 1) { - sdcard.multiWriteBlocksRemain--; - sdcard.multiWriteNextBlock++; - sdcard.state = SDCARD_STATE_WRITING_MULTIPLE_BLOCKS; - } else if (sdcard.multiWriteBlocksRemain == 1) { - // This function changes the sd card state for us whether immediately succesful or delayed: - if (sdcard_endWriteBlocks() == SDCARD_OPERATION_SUCCESS) { - sdcard_deselect(); - } else { -#ifdef SDCARD_PROFILING - // Wait for the multi-block write to be terminated before finishing timing - profilingComplete = false; -#endif - } - } else { - sdcard.state = SDCARD_STATE_READY; - sdcard_deselect(); - } - -#ifdef SDCARD_PROFILING - if (profilingComplete && sdcard.profiler) { - sdcard.profiler(SDCARD_BLOCK_OPERATION_WRITE, sdcard.pendingOperation.blockIndex, micros() - sdcard.pendingOperation.profileStartTime); - } -#endif - } else if (millis() > sdcard.operationStartTime + SDCARD_TIMEOUT_WRITE_MSEC) { - /* - * The caller has already been told that their write has completed, so they will have discarded - * their buffer and have no hope of retrying the operation. But this should be very rare and it allows - * them to reuse their buffer milliseconds faster than they otherwise would. - */ - sdcard_reset(); - goto doMore; - } - break; - case SDCARD_STATE_READING: - switch (sdcard_receiveDataBlock(sdcard.pendingOperation.buffer, SDCARD_BLOCK_SIZE)) { - case SDCARD_RECEIVE_SUCCESS: - sdcard_deselect(); - - sdcard.state = SDCARD_STATE_READY; - sdcard.failureCount = 0; // Assume the card is good if it can complete a read - -#ifdef SDCARD_PROFILING - if (sdcard.profiler) { - sdcard.profiler(SDCARD_BLOCK_OPERATION_READ, sdcard.pendingOperation.blockIndex, micros() - sdcard.pendingOperation.profileStartTime); - } -#endif - - if (sdcard.pendingOperation.callback) { - sdcard.pendingOperation.callback( - SDCARD_BLOCK_OPERATION_READ, - sdcard.pendingOperation.blockIndex, - sdcard.pendingOperation.buffer, - sdcard.pendingOperation.callbackData - ); - } - break; - case SDCARD_RECEIVE_BLOCK_IN_PROGRESS: - if (millis() <= sdcard.operationStartTime + SDCARD_TIMEOUT_READ_MSEC) { - break; // Timeout not reached yet so keep waiting - } - // Timeout has expired, so fall through to convert to a fatal error - FALLTHROUGH; - - case SDCARD_RECEIVE_ERROR: - sdcard_deselect(); - - sdcard_reset(); - - if (sdcard.pendingOperation.callback) { - sdcard.pendingOperation.callback( - SDCARD_BLOCK_OPERATION_READ, - sdcard.pendingOperation.blockIndex, - NULL, - sdcard.pendingOperation.callbackData - ); - } - - goto doMore; - break; - } - break; - case SDCARD_STATE_STOPPING_MULTIPLE_BLOCK_WRITE: - if (sdcard_waitForIdle(SDCARD_MAXIMUM_BYTE_DELAY_FOR_CMD_REPLY)) { - sdcard_deselect(); - - sdcard.state = SDCARD_STATE_READY; - -#ifdef SDCARD_PROFILING - if (sdcard.profiler) { - sdcard.profiler(SDCARD_BLOCK_OPERATION_WRITE, sdcard.pendingOperation.blockIndex, micros() - sdcard.pendingOperation.profileStartTime); - } -#endif - } else if (millis() > sdcard.operationStartTime + SDCARD_TIMEOUT_WRITE_MSEC) { - sdcard_reset(); - goto doMore; - } - break; - case SDCARD_STATE_NOT_PRESENT: - default: - ; - } - - // Is the card's initialization taking too long? - if (sdcard.state >= SDCARD_STATE_RESET && sdcard.state < SDCARD_STATE_READY - && millis() - sdcard.operationStartTime > SDCARD_TIMEOUT_INIT_MILLIS) { - sdcard_reset(); - } - - return sdcard_isReady(); -} - -/** - * Write the 512-byte block from the given buffer into the block with the given index. - * - * If the write does not complete immediately, your callback will be called later. If the write was successful, the - * buffer pointer will be the same buffer you originally passed in, otherwise the buffer will be set to NULL. - * - * Returns: - * SDCARD_OPERATION_IN_PROGRESS - Your buffer is currently being transmitted to the card and your callback will be - * called later to report the completion. The buffer pointer must remain valid until - * that time. - * SDCARD_OPERATION_SUCCESS - Your buffer has been transmitted to the card now. - * SDCARD_OPERATION_BUSY - The card is already busy and cannot accept your write - * SDCARD_OPERATION_FAILURE - Your write was rejected by the card, card will be reset - */ sdcardOperationStatus_e sdcard_writeBlock(uint32_t blockIndex, uint8_t *buffer, sdcard_operationCompleteCallback_c callback, uint32_t callbackData) { - uint8_t status; - -#ifdef SDCARD_PROFILING - sdcard.pendingOperation.profileStartTime = micros(); -#endif - - doMore: - switch (sdcard.state) { - case SDCARD_STATE_WRITING_MULTIPLE_BLOCKS: - // Do we need to cancel the previous multi-block write? - if (blockIndex != sdcard.multiWriteNextBlock) { - if (sdcard_endWriteBlocks() == SDCARD_OPERATION_SUCCESS) { - // Now we've entered the ready state, we can try again - goto doMore; - } else { - return SDCARD_OPERATION_BUSY; - } - } - - // We're continuing a multi-block write - break; - case SDCARD_STATE_READY: - // We're not continuing a multi-block write so we need to send a single-block write command - sdcard_select(); - - // Standard size cards use byte addressing, high capacity cards use block addressing - status = sdcard_sendCommand(SDCARD_COMMAND_WRITE_BLOCK, sdcard.highCapacity ? blockIndex : blockIndex * SDCARD_BLOCK_SIZE); - - if (status != 0) { - sdcard_deselect(); - - sdcard_reset(); - - return SDCARD_OPERATION_FAILURE; - } - break; - default: - return SDCARD_OPERATION_BUSY; - } - - sdcard_sendDataBlockBegin(buffer, sdcard.state == SDCARD_STATE_WRITING_MULTIPLE_BLOCKS); - - sdcard.pendingOperation.buffer = buffer; - sdcard.pendingOperation.blockIndex = blockIndex; - sdcard.pendingOperation.callback = callback; - sdcard.pendingOperation.callbackData = callbackData; - sdcard.pendingOperation.chunkIndex = 1; // (for non-DMA transfers) we've sent chunk #0 already - sdcard.state = SDCARD_STATE_SENDING_WRITE; - - return SDCARD_OPERATION_IN_PROGRESS; + return sdcardVTable->writeBlock(blockIndex, buffer, callback, callbackData); } -/** - * Begin writing a series of consecutive blocks beginning at the given block index. This will allow (but not require) - * the SD card to pre-erase the number of blocks you specifiy, which can allow the writes to complete faster. - * - * Afterwards, just call sdcard_writeBlock() as normal to write those blocks consecutively. - * - * It's okay to abort the multi-block write at any time by writing to a non-consecutive address, or by performing a read. - * - * Returns: - * SDCARD_OPERATION_SUCCESS - Multi-block write has been queued - * SDCARD_OPERATION_BUSY - The card is already busy and cannot accept your write - * SDCARD_OPERATION_FAILURE - A fatal error occured, card will be reset - */ -sdcardOperationStatus_e sdcard_beginWriteBlocks(uint32_t blockIndex, uint32_t blockCount) +bool sdcard_poll(void) { - if (sdcard.state != SDCARD_STATE_READY) { - if (sdcard.state == SDCARD_STATE_WRITING_MULTIPLE_BLOCKS) { - if (blockIndex == sdcard.multiWriteNextBlock) { - // Assume that the caller wants to continue the multi-block write they already have in progress! - return SDCARD_OPERATION_SUCCESS; - } else if (sdcard_endWriteBlocks() != SDCARD_OPERATION_SUCCESS) { - return SDCARD_OPERATION_BUSY; - } // Else we've completed the previous multi-block write and can fall through to start the new one - } else { - return SDCARD_OPERATION_BUSY; - } - } - - sdcard_select(); - - if ( - sdcard_sendAppCommand(SDCARD_ACOMMAND_SET_WR_BLOCK_ERASE_COUNT, blockCount) == 0 - && sdcard_sendCommand(SDCARD_COMMAND_WRITE_MULTIPLE_BLOCK, sdcard.highCapacity ? blockIndex : blockIndex * SDCARD_BLOCK_SIZE) == 0 - ) { - sdcard.state = SDCARD_STATE_WRITING_MULTIPLE_BLOCKS; - sdcard.multiWriteBlocksRemain = blockCount; - sdcard.multiWriteNextBlock = blockIndex; - - // Leave the card selected - return SDCARD_OPERATION_SUCCESS; + if (sdcardVTable) { + return sdcardVTable->poll(); } else { - sdcard_deselect(); - - sdcard_reset(); - - return SDCARD_OPERATION_FAILURE; + return false; } } -/** - * Read the 512-byte block with the given index into the given 512-byte buffer. - * - * When the read completes, your callback will be called. If the read was successful, the buffer pointer will be the - * same buffer you originally passed in, otherwise the buffer will be set to NULL. - * - * You must keep the pointer to the buffer valid until the operation completes! - * - * Returns: - * true - The operation was successfully queued for later completion, your callback will be called later - * false - The operation could not be started due to the card being busy (try again later). - */ -bool sdcard_readBlock(uint32_t blockIndex, uint8_t *buffer, sdcard_operationCompleteCallback_c callback, uint32_t callbackData) +bool sdcard_isFunctional(void) { - if (sdcard.state != SDCARD_STATE_READY) { - if (sdcard.state == SDCARD_STATE_WRITING_MULTIPLE_BLOCKS) { - if (sdcard_endWriteBlocks() != SDCARD_OPERATION_SUCCESS) { - return false; - } - } else { - return false; - } - } - -#ifdef SDCARD_PROFILING - sdcard.pendingOperation.profileStartTime = micros(); -#endif - - sdcard_select(); - - // Standard size cards use byte addressing, high capacity cards use block addressing - uint8_t status = sdcard_sendCommand(SDCARD_COMMAND_READ_SINGLE_BLOCK, sdcard.highCapacity ? blockIndex : blockIndex * SDCARD_BLOCK_SIZE); - - if (status == 0) { - sdcard.pendingOperation.buffer = buffer; - sdcard.pendingOperation.blockIndex = blockIndex; - sdcard.pendingOperation.callback = callback; - sdcard.pendingOperation.callbackData = callbackData; - - sdcard.state = SDCARD_STATE_READING; - - sdcard.operationStartTime = millis(); - - // Leave the card selected for the whole transaction - - return true; + if (sdcardVTable) { + return sdcardVTable->isFunctional(); } else { - sdcard_deselect(); - return false; } } -/** - * Returns true if the SD card has successfully completed its startup procedures. - */ bool sdcard_isInitialized(void) { - return sdcard.state >= SDCARD_STATE_READY; + if (sdcardVTable) { + return sdcardVTable->isInitialized(); + } else { + return false; + } } const sdcardMetadata_t* sdcard_getMetadata(void) { - return &sdcard.metadata; -} - -#ifdef SDCARD_PROFILING - -void sdcard_setProfilerCallback(sdcard_profilerCallback_c callback) -{ - sdcard.profiler = callback; + if (sdcardVTable) { + return sdcardVTable->getMetadata(); + } + else { + return NULL; + } } #endif - -#endif diff --git a/src/main/drivers/sdcard.h b/src/main/drivers/sdcard.h index 22fb987ea60..60f4bbe7353 100644 --- a/src/main/drivers/sdcard.h +++ b/src/main/drivers/sdcard.h @@ -21,19 +21,18 @@ #include typedef struct sdcardMetadata_t { - uint8_t manufacturerID; + uint32_t numBlocks; /* Card capacity in 512-byte blocks*/ uint16_t oemID; + uint8_t manufacturerID; char productName[5]; + uint32_t productSerial; uint8_t productRevisionMajor; uint8_t productRevisionMinor; - uint32_t productSerial; uint16_t productionYear; uint8_t productionMonth; - - uint32_t numBlocks; /* Card capacity in 512-byte blocks*/ } sdcardMetadata_t; typedef enum { @@ -51,9 +50,7 @@ typedef enum { typedef void(*sdcard_operationCompleteCallback_c)(sdcardBlockOperation_e operation, uint32_t blockIndex, uint8_t *buffer, uint32_t callbackData); -typedef void(*sdcard_profilerCallback_c)(sdcardBlockOperation_e operation, uint32_t blockIndex, uint32_t duration); - -void sdcard_init(bool useDMA); +void sdcard_init(void); bool sdcard_readBlock(uint32_t blockIndex, uint8_t *buffer, sdcard_operationCompleteCallback_c callback, uint32_t callbackData); @@ -69,5 +66,3 @@ bool sdcard_isFunctional(void); bool sdcard_poll(void); const sdcardMetadata_t* sdcard_getMetadata(void); - -void sdcard_setProfilerCallback(sdcard_profilerCallback_c callback); diff --git a/src/main/drivers/sdcard_impl.h b/src/main/drivers/sdcard_impl.h new file mode 100644 index 00000000000..8e2b5a15fb8 --- /dev/null +++ b/src/main/drivers/sdcard_impl.h @@ -0,0 +1,106 @@ +/* + * This file is part of Cleanflight, INAV and Betaflight. + * + * Cleanflight, INAV and Betaflight are free software. You can redistribute + * this software and/or modify this software under the terms of the + * GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) + * any later version. + * + * Cleanflight and Betaflight are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +#pragma once + +#include "drivers/nvic.h" +#include "drivers/io.h" + +#include "drivers/bus.h" +#include "drivers/bus_spi.h" +#include "drivers/time.h" + +#include "drivers/sdcard.h" +#include "drivers/sdcard_standard.h" + +#define SDCARD_TIMEOUT_INIT_MILLIS 200 +#define SDCARD_MAX_CONSECUTIVE_FAILURES 8 + +typedef enum { + // In these states we run at the initialization 400kHz clockspeed: + SDCARD_STATE_NOT_PRESENT = 0, + SDCARD_STATE_RESET, + SDCARD_STATE_CARD_INIT_IN_PROGRESS, + SDCARD_STATE_INITIALIZATION_RECEIVE_CID, + + // In these states we run at full clock speed + SDCARD_STATE_READY, + SDCARD_STATE_READING, + SDCARD_STATE_SENDING_WRITE, + SDCARD_STATE_WAITING_FOR_WRITE, + SDCARD_STATE_WRITING_MULTIPLE_BLOCKS, + SDCARD_STATE_STOPPING_MULTIPLE_BLOCK_WRITE, +} sdcardState_e; + +typedef struct sdcard_t { + busDevice_t * dev; + + struct { + uint8_t *buffer; + uint32_t blockIndex; + uint8_t chunkIndex; + + sdcard_operationCompleteCallback_c callback; + uint32_t callbackData; + } pendingOperation; + + uint32_t operationStartTime; + + uint8_t failureCount; + + uint8_t version; + bool highCapacity; + + uint32_t multiWriteNextBlock; + uint32_t multiWriteBlocksRemain; + + sdcardState_e state; + sdcardMetadata_t metadata; + sdcardCSD_t csd; + + IO_t cardDetectPin; +} sdcard_t; + +extern sdcard_t sdcard; + +STATIC_ASSERT(sizeof(sdcardCSD_t) == 16, sdcard_csd_bitfields_didnt_pack_properly); + +void sdcardInsertionDetectInit(void); +void sdcardInsertionDetectDeinit(void); +bool sdcard_isInserted(void); + +typedef struct sdcardVTable_s { + void (*init)(void); + bool (*readBlock)(uint32_t blockIndex, uint8_t *buffer, sdcard_operationCompleteCallback_c callback, uint32_t callbackData); + sdcardOperationStatus_e (*beginWriteBlocks)(uint32_t blockIndex, uint32_t blockCount); + sdcardOperationStatus_e (*writeBlock)(uint32_t blockIndex, uint8_t *buffer, sdcard_operationCompleteCallback_c callback, uint32_t callbackData); + bool (*poll)(void); + bool (*isFunctional)(void); + bool (*isInitialized)(void); + const sdcardMetadata_t* (*getMetadata)(void); +} sdcardVTable_t; + +#ifdef USE_SDCARD_SPI +extern sdcardVTable_t sdcardSpiVTable; +#endif + +#ifdef USE_SDCARD_SDIO +extern sdcardVTable_t sdcardSdioVTable; +#endif diff --git a/src/main/drivers/sdcard_spi.c b/src/main/drivers/sdcard_spi.c new file mode 100644 index 00000000000..c2f8790693a --- /dev/null +++ b/src/main/drivers/sdcard_spi.c @@ -0,0 +1,888 @@ +/* + * This file is part of INAV, Cleanflight and Betaflight. + * + * INAV, Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software under the terms of the + * GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) + * any later version. + * + * Cleanflight and Betaflight are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +#include +#include + +#include "platform.h" + +#include "build/debug.h" +#include "common/utils.h" + +#include "drivers/time.h" +#include "drivers/nvic.h" +#include "drivers/io.h" +#include "drivers/bus.h" +#include "drivers/bus_spi.h" + +#include "drivers/sdcard.h" +#include "drivers/sdcard_impl.h" +#include "drivers/sdcard_standard.h" + +#include "scheduler/protothreads.h" + +#ifdef USE_SDCARD_SPI + +#define SDCARD_INIT_NUM_DUMMY_BYTES 10 +#define SDCARD_MAXIMUM_BYTE_DELAY_FOR_CMD_REPLY 8 +// Chosen so that CMD8 will have the same CRC as CMD0: +#define SDCARD_IF_COND_CHECK_PATTERN 0xAB + +/* Break up 512-byte SD card sectors into chunks of this size to reduce the peak overhead per call to sdcard_poll(). */ +#define SDCARD_BLOCK_CHUNK_SIZE 128 + +#ifndef SDCARD_BUS_SPEED +#define SDCARD_BUS_SPEED BUS_SPEED_STANDARD +#endif + +static void sdcardSpi_select(void) +{ + busSelectDevice(sdcard.dev); +} + +static void sdcardSpi_deselect(void) +{ + // As per the SD-card spec, give the card 8 dummy clocks so it can finish its operation + //spiTransferByte(SDCARD_SPI_INSTANCE, 0xFF); + + while (busIsBusy(sdcard.dev)) { __NOP(); } + + busDeselectDevice(sdcard.dev); +} + + +/** + * Returns true if the card has already been, or is currently, initializing and hasn't encountered enough errors to + * trip our error threshold and be disabled (i.e. our card is in and working!) + */ +bool sdcardSpi_isFunctional(void) +{ + return sdcard.state != SDCARD_STATE_NOT_PRESENT; +} + +/** + * Handle a failure of an SD card operation by resetting the card back to its initialization phase. + * + * Increments the failure counter, and when the failure threshold is reached, disables the card until + * the next call to sdcard_init(). + */ +static void sdcardSpi_reset(void) +{ + if (!sdcard_isInserted()) { + sdcard.state = SDCARD_STATE_NOT_PRESENT; + return; + } + + if (sdcard.state >= SDCARD_STATE_READY) { + busSetSpeed(sdcard.dev, BUS_SPEED_INITIALIZATION); + } + + sdcard.failureCount++; + if (sdcard.failureCount >= SDCARD_MAX_CONSECUTIVE_FAILURES) { + sdcard.state = SDCARD_STATE_NOT_PRESENT; + } else { + sdcard.operationStartTime = millis(); + sdcard.state = SDCARD_STATE_RESET; + } +} + +/** + * The SD card spec requires 8 clock cycles to be sent by us on the bus after most commands so it can finish its + * processing of that command. The easiest way for us to do this is to just wait for the bus to become idle before + * we transmit a command, sending at least 8-bits onto the bus when we do so. + */ +static bool sdcardSpi_waitForIdle(int maxBytesToWait) +{ + while (maxBytesToWait > 0) { + uint8_t response; + + busTransfer(sdcard.dev, &response, NULL, 1); + + if (response == 0xFF) { + return true; + } + + maxBytesToWait--; + } + + return false; +} + +/** + * Wait for up to maxDelay 0xFF idle bytes to arrive from the card, returning the first non-idle byte found. + * + * Returns 0xFF on failure. + */ +static uint8_t sdcardSpi_waitForNonIdleByte(int maxDelay) +{ + for (int i = 0; i <= maxDelay; i++) { // <= so we can wait for maxDelay '0xFF' bytes before reading a response byte afterwards + uint8_t response; + + busTransfer(sdcard.dev, &response, NULL, 1); + + if (response != 0xFF) { + return response; + } + } + + return 0xFF; +} + +/** + * Waits up to SDCARD_MAXIMUM_BYTE_DELAY_FOR_CMD_REPLY bytes for the card to become ready, send a command to the card + * with the given argument, waits up to SDCARD_MAXIMUM_BYTE_DELAY_FOR_CMD_REPLY bytes for a reply, and returns the + * first non-0xFF byte of the reply. + * + * You must select the card first with sdcardSpi_select() and deselect it afterwards with sdcardSpi_deselect(). + * + * Upon failure, 0xFF is returned. + */ +static uint8_t sdcardSpi_sendCommand(uint8_t commandCode, uint32_t commandArgument) +{ + uint8_t command[6] = { + 0x40 | commandCode, + commandArgument >> 24, + commandArgument >> 16, + commandArgument >> 8, + commandArgument, + 0x95 /* Static CRC. This CRC is valid for CMD0 with a 0 argument, and CMD8 with 0x1AB argument, which are the only + commands that require a CRC */ + }; + + // Go ahead and send the command even if the card isn't idle if this is the reset command + if (!sdcardSpi_waitForIdle(SDCARD_MAXIMUM_BYTE_DELAY_FOR_CMD_REPLY) && commandCode != SDCARD_COMMAND_GO_IDLE_STATE) + return 0xFF; + + busTransfer(sdcard.dev, NULL, command, sizeof(command)); + + /* + * The card can take up to SDCARD_MAXIMUM_BYTE_DELAY_FOR_CMD_REPLY bytes to send the response, in the meantime + * it'll transmit 0xFF filler bytes. + */ + return sdcardSpi_waitForNonIdleByte(SDCARD_MAXIMUM_BYTE_DELAY_FOR_CMD_REPLY); +} + +static uint8_t sdcardSpi_sendAppCommand(uint8_t commandCode, uint32_t commandArgument) +{ + sdcardSpi_sendCommand(SDCARD_COMMAND_APP_CMD, 0); + return sdcardSpi_sendCommand(commandCode, commandArgument); +} + +/** + * Sends an IF_COND message to the card to check its version and validate its voltage requirements. Sets the global + * sdCardVersion with the detected version (0, 1, or 2) and returns true if the card is compatible. + */ +static bool sdcardSpi_validateInterfaceCondition(void) +{ + uint8_t ifCondReply[4]; + + sdcard.version = 0; + + sdcardSpi_select(); + + uint8_t status = sdcardSpi_sendCommand(SDCARD_COMMAND_SEND_IF_COND, (SDCARD_VOLTAGE_ACCEPTED_2_7_to_3_6 << 8) | SDCARD_IF_COND_CHECK_PATTERN); + + // Don't deselect the card right away, because we'll want to read the rest of its reply if it's a V2 card + + if (status == (SDCARD_R1_STATUS_BIT_ILLEGAL_COMMAND | SDCARD_R1_STATUS_BIT_IDLE)) { + // V1 cards don't support this command + sdcard.version = 1; + } else if (status == SDCARD_R1_STATUS_BIT_IDLE) { + busTransfer(sdcard.dev, ifCondReply, NULL, sizeof(ifCondReply)); + + /* + * We don't bother to validate the SDCard's operating voltage range since the spec requires it to accept our + * 3.3V, but do check that it echoed back our check pattern properly. + */ + if (ifCondReply[3] == SDCARD_IF_COND_CHECK_PATTERN) { + sdcard.version = 2; + } + } + + sdcardSpi_deselect(); + + return sdcard.version > 0; +} + +static bool sdcardSpi_readOCRRegister(uint32_t *result) +{ + uint8_t response[4]; + + sdcardSpi_select(); + + uint8_t status = sdcardSpi_sendCommand(SDCARD_COMMAND_READ_OCR, 0); + + busTransfer(sdcard.dev, response, NULL, sizeof(response)); + + sdcardSpi_deselect(); + + if (status == 0) { + *result = (response[0] << 24) | (response[1] << 16) | (response[2] << 8) | response[3]; + return true; + } else { + return false; + } +} + +typedef enum { + SDCARD_RECEIVE_SUCCESS, + SDCARD_RECEIVE_BLOCK_IN_PROGRESS, + SDCARD_RECEIVE_ERROR, +} sdcardReceiveBlockStatus_e; + +/** + * Attempt to receive a data block from the SD card. + * + * Return true on success, otherwise the card has not responded yet and you should retry later. + */ +static sdcardReceiveBlockStatus_e sdcardSpi_receiveDataBlock(uint8_t *buffer, int count) +{ + uint8_t dataToken = sdcardSpi_waitForNonIdleByte(8); + + if (dataToken == 0xFF) { + return SDCARD_RECEIVE_BLOCK_IN_PROGRESS; + } + + if (dataToken != SDCARD_SINGLE_BLOCK_READ_START_TOKEN) { + return SDCARD_RECEIVE_ERROR; + } + + busTransfer(sdcard.dev, buffer, NULL, count); + + // Discard trailing CRC, we don't care + busTransfer(sdcard.dev, NULL, NULL, 2); + + return SDCARD_RECEIVE_SUCCESS; +} + +static bool sdcardSpi_sendDataBlockFinish(void) +{ + static const uint8_t dummyCRC[2] = { 0x00, 0x00 }; + + // Send a dummy CRC + busTransfer(sdcard.dev, NULL, dummyCRC, 2); + + uint8_t dataResponseToken; + busTransfer(sdcard.dev, &dataResponseToken, NULL, 1); + + /* + * Check if the card accepted the write (no CRC error / no address error) + * + * The lower 5 bits are structured as follows: + * | 0 | Status | 1 | + * | 0 | x x x | 1 | + * + * Statuses: + * 010 - Data accepted + * 101 - CRC error + * 110 - Write error + */ + return (dataResponseToken & 0x1F) == 0x05; +} + +/** + * Begin sending a buffer of SDCARD_BLOCK_SIZE bytes to the SD card. + */ +static void sdcardSpi_sendDataBlockBegin(uint8_t *buffer, bool multiBlockWrite) +{ + uint8_t blockStartToken[2] = { 0xFF, multiBlockWrite ? SDCARD_MULTIPLE_BLOCK_WRITE_START_TOKEN : SDCARD_SINGLE_BLOCK_WRITE_START_TOKEN }; + + // Card wants 8 dummy clock cycles between the write command's response and a data block beginning: + busTransfer(sdcard.dev, NULL, blockStartToken, 2); + + // Send the first chunk now + busTransfer(sdcard.dev, NULL, buffer, SDCARD_BLOCK_CHUNK_SIZE); +} + +static bool sdcardSpi_receiveCID(void) +{ + uint8_t cid[16]; + + if (sdcardSpi_receiveDataBlock(cid, sizeof(cid)) != SDCARD_RECEIVE_SUCCESS) { + return false; + } + + sdcard.metadata.manufacturerID = cid[0]; + sdcard.metadata.oemID = (cid[1] << 8) | cid[2]; + sdcard.metadata.productName[0] = cid[3]; + sdcard.metadata.productName[1] = cid[4]; + sdcard.metadata.productName[2] = cid[5]; + sdcard.metadata.productName[3] = cid[6]; + sdcard.metadata.productName[4] = cid[7]; + sdcard.metadata.productRevisionMajor = cid[8] >> 4; + sdcard.metadata.productRevisionMinor = cid[8] & 0x0F; + sdcard.metadata.productSerial = (cid[9] << 24) | (cid[10] << 16) | (cid[11] << 8) | cid[12]; + sdcard.metadata.productionYear = (((cid[13] & 0x0F) << 4) | (cid[14] >> 4)) + 2000; + sdcard.metadata.productionMonth = cid[14] & 0x0F; + + return true; +} + +static bool sdcardSpi_fetchCSD(void) +{ + uint32_t readBlockLen, blockCount, blockCountMult; + uint64_t capacityBytes; + + sdcardSpi_select(); + + /* The CSD command's data block should always arrive within 8 idle clock cycles (SD card spec). This is because + * the information about card latency is stored in the CSD register itself, so we can't use that yet! + */ + bool success = + sdcardSpi_sendCommand(SDCARD_COMMAND_SEND_CSD, 0) == 0 + && sdcardSpi_receiveDataBlock((uint8_t*) &sdcard.csd, sizeof(sdcard.csd)) == SDCARD_RECEIVE_SUCCESS + && SDCARD_GET_CSD_FIELD(sdcard.csd, 1, TRAILER) == 1; + + if (success) { + switch (SDCARD_GET_CSD_FIELD(sdcard.csd, 1, CSD_STRUCTURE_VER)) { + case SDCARD_CSD_STRUCTURE_VERSION_1: + // Block size in bytes (doesn't have to be 512) + readBlockLen = 1 << SDCARD_GET_CSD_FIELD(sdcard.csd, 1, READ_BLOCK_LEN); + blockCountMult = 1 << (SDCARD_GET_CSD_FIELD(sdcard.csd, 1, CSIZE_MULT) + 2); + blockCount = (SDCARD_GET_CSD_FIELD(sdcard.csd, 1, CSIZE) + 1) * blockCountMult; + + // We could do this in 32 bits but it makes the 2GB case awkward + capacityBytes = (uint64_t) blockCount * readBlockLen; + + // Re-express that capacity (max 2GB) in our standard 512-byte block size + sdcard.metadata.numBlocks = capacityBytes / SDCARD_BLOCK_SIZE; + break; + case SDCARD_CSD_STRUCTURE_VERSION_2: + sdcard.metadata.numBlocks = (SDCARD_GET_CSD_FIELD(sdcard.csd, 2, CSIZE) + 1) * 1024; + break; + default: + success = false; + } + } + + sdcardSpi_deselect(); + + return success; +} + +/** + * Check if the SD Card has completed its startup sequence. Must be called with sdcard.state == SDCARD_STATE_INITIALIZATION. + * + * Returns true if the card has finished its init process. + */ +static bool sdcardSpi_checkInitDone(void) +{ + sdcardSpi_select(); + uint8_t status = sdcardSpi_sendAppCommand(SDCARD_ACOMMAND_SEND_OP_COND, sdcard.version == 2 ? 1 << 30 /* We support high capacity cards */ : 0); + sdcardSpi_deselect(); + + // When card init is complete, the idle bit in the response becomes zero. + return status == 0x00; +} + +static bool sdcardSpi_setBlockLength(uint32_t blockLen) +{ + sdcardSpi_select(); + uint8_t status = sdcardSpi_sendCommand(SDCARD_COMMAND_SET_BLOCKLEN, blockLen); + sdcardSpi_deselect(); + + return status == 0; +} + +/* + * Returns true if the card is ready to accept read/write commands. + */ +static bool sdcardSpi_isReady(void) +{ + return sdcard.state == SDCARD_STATE_READY || sdcard.state == SDCARD_STATE_WRITING_MULTIPLE_BLOCKS; +} + +/** + * Send the stop-transmission token to complete a multi-block write. + * + * Returns: + * SDCARD_OPERATION_IN_PROGRESS - We're now waiting for that stop to complete, the card will enter + * the SDCARD_STATE_STOPPING_MULTIPLE_BLOCK_WRITE state. + * SDCARD_OPERATION_SUCCESS - The multi-block write finished immediately, the card will enter + * the SDCARD_READY state. + * + */ +static sdcardOperationStatus_e sdcardSpi_endWriteBlocks(void) +{ + uint8_t writeEndToken[2] = { 0xFF, SDCARD_MULTIPLE_BLOCK_WRITE_STOP_TOKEN }; + + sdcard.multiWriteBlocksRemain = 0; + + // 8 dummy clocks to guarantee N_WR clocks between the last card response and this token + busTransfer(sdcard.dev, NULL, writeEndToken, 2); + + // Card may choose to raise a busy (non-0xFF) signal after at most N_BR (1 byte) delay + if (sdcardSpi_waitForNonIdleByte(1) == 0xFF) { + sdcard.state = SDCARD_STATE_READY; + return SDCARD_OPERATION_SUCCESS; + } else { + sdcard.state = SDCARD_STATE_STOPPING_MULTIPLE_BLOCK_WRITE; + sdcard.operationStartTime = millis(); + + return SDCARD_OPERATION_IN_PROGRESS; + } +} + +/** + * Call periodically for the SD card to perform in-progress transfers. + * + * Returns true if the card is ready to accept commands. + */ +static bool sdcardSpi_poll(void) +{ + if (sdcard.dev == NULL) { + return false; + } + + uint8_t initStatus; + bool sendComplete; + + doMore: + switch (sdcard.state) { + case SDCARD_STATE_RESET: + sdcardSpi_select(); + + initStatus = sdcardSpi_sendCommand(SDCARD_COMMAND_GO_IDLE_STATE, 0); + + sdcardSpi_deselect(); + + if (initStatus == SDCARD_R1_STATUS_BIT_IDLE) { + // Check card voltage and version + if (sdcardSpi_validateInterfaceCondition()) { + + sdcard.state = SDCARD_STATE_CARD_INIT_IN_PROGRESS; + goto doMore; + } else { + // Bad reply/voltage, we ought to refrain from accessing the card. + sdcard.state = SDCARD_STATE_NOT_PRESENT; + } + } + break; + + case SDCARD_STATE_CARD_INIT_IN_PROGRESS: + if (sdcardSpi_checkInitDone()) { + if (sdcard.version == 2) { + // Check for high capacity card + uint32_t ocr; + + if (!sdcardSpi_readOCRRegister(&ocr)) { + sdcardSpi_reset(); + goto doMore; + } + + sdcard.highCapacity = (ocr & (1 << 30)) != 0; + } else { + // Version 1 cards are always low-capacity + sdcard.highCapacity = false; + } + + // Now fetch the CSD and CID registers + if (sdcardSpi_fetchCSD()) { + sdcardSpi_select(); + + uint8_t status = sdcardSpi_sendCommand(SDCARD_COMMAND_SEND_CID, 0); + + if (status == 0) { + // Keep the card selected to receive the response block + sdcard.state = SDCARD_STATE_INITIALIZATION_RECEIVE_CID; + goto doMore; + } else { + sdcardSpi_deselect(); + + sdcardSpi_reset(); + goto doMore; + } + } + } + break; + case SDCARD_STATE_INITIALIZATION_RECEIVE_CID: + if (sdcardSpi_receiveCID()) { + sdcardSpi_deselect(); + + /* The spec is a little iffy on what the default block size is for Standard Size cards (it can be changed on + * standard size cards) so let's just set it to 512 explicitly so we don't have a problem. + */ + if (!sdcard.highCapacity && !sdcardSpi_setBlockLength(SDCARD_BLOCK_SIZE)) { + sdcardSpi_reset(); + goto doMore; + } + + // Now we're done with init and we can switch to the full speed clock (<25MHz) + busSetSpeed(sdcard.dev, SDCARD_BUS_SPEED); + + sdcard.multiWriteBlocksRemain = 0; + + sdcard.state = SDCARD_STATE_READY; + goto doMore; + } // else keep waiting for the CID to arrive + break; + case SDCARD_STATE_SENDING_WRITE: + // Have we finished sending the write yet? + sendComplete = false; + + // Send another chunk + busTransfer(sdcard.dev, NULL, sdcard.pendingOperation.buffer + SDCARD_BLOCK_CHUNK_SIZE * sdcard.pendingOperation.chunkIndex, SDCARD_BLOCK_CHUNK_SIZE); + sdcard.pendingOperation.chunkIndex++; + sendComplete = sdcard.pendingOperation.chunkIndex == SDCARD_BLOCK_SIZE / SDCARD_BLOCK_CHUNK_SIZE; + + if (sendComplete) { + // Finish up by sending the CRC and checking the SD-card's acceptance/rejectance + if (sdcardSpi_sendDataBlockFinish()) { + // The SD card is now busy committing that write to the card + sdcard.state = SDCARD_STATE_WAITING_FOR_WRITE; + sdcard.operationStartTime = millis(); + + // Since we've transmitted the buffer we can go ahead and tell the caller their operation is complete + if (sdcard.pendingOperation.callback) { + sdcard.pendingOperation.callback(SDCARD_BLOCK_OPERATION_WRITE, sdcard.pendingOperation.blockIndex, sdcard.pendingOperation.buffer, sdcard.pendingOperation.callbackData); + } + } else { + /* Our write was rejected! This could be due to a bad address but we hope not to attempt that, so assume + * the card is broken and needs reset. + */ + sdcardSpi_reset(); + + // Announce write failure: + if (sdcard.pendingOperation.callback) { + sdcard.pendingOperation.callback(SDCARD_BLOCK_OPERATION_WRITE, sdcard.pendingOperation.blockIndex, NULL, sdcard.pendingOperation.callbackData); + } + + goto doMore; + } + } + break; + case SDCARD_STATE_WAITING_FOR_WRITE: + if (sdcardSpi_waitForIdle(SDCARD_MAXIMUM_BYTE_DELAY_FOR_CMD_REPLY)) { + sdcard.failureCount = 0; // Assume the card is good if it can complete a write + + // Still more blocks left to write in a multi-block chain? + if (sdcard.multiWriteBlocksRemain > 1) { + sdcard.multiWriteBlocksRemain--; + sdcard.multiWriteNextBlock++; + sdcard.state = SDCARD_STATE_WRITING_MULTIPLE_BLOCKS; + } else if (sdcard.multiWriteBlocksRemain == 1) { + // This function changes the sd card state for us whether immediately succesful or delayed: + if (sdcardSpi_endWriteBlocks() == SDCARD_OPERATION_SUCCESS) { + sdcardSpi_deselect(); + } + } else { + sdcard.state = SDCARD_STATE_READY; + sdcardSpi_deselect(); + } + } else if (millis() > sdcard.operationStartTime + SDCARD_TIMEOUT_WRITE_MSEC) { + /* + * The caller has already been told that their write has completed, so they will have discarded + * their buffer and have no hope of retrying the operation. But this should be very rare and it allows + * them to reuse their buffer milliseconds faster than they otherwise would. + */ + sdcardSpi_reset(); + goto doMore; + } + break; + case SDCARD_STATE_READING: + switch (sdcardSpi_receiveDataBlock(sdcard.pendingOperation.buffer, SDCARD_BLOCK_SIZE)) { + case SDCARD_RECEIVE_SUCCESS: + sdcardSpi_deselect(); + + sdcard.state = SDCARD_STATE_READY; + sdcard.failureCount = 0; // Assume the card is good if it can complete a read + + if (sdcard.pendingOperation.callback) { + sdcard.pendingOperation.callback( + SDCARD_BLOCK_OPERATION_READ, + sdcard.pendingOperation.blockIndex, + sdcard.pendingOperation.buffer, + sdcard.pendingOperation.callbackData + ); + } + break; + case SDCARD_RECEIVE_BLOCK_IN_PROGRESS: + if (millis() <= sdcard.operationStartTime + SDCARD_TIMEOUT_READ_MSEC) { + break; // Timeout not reached yet so keep waiting + } + // Timeout has expired, so fall through to convert to a fatal error + FALLTHROUGH; + + case SDCARD_RECEIVE_ERROR: + sdcardSpi_deselect(); + + sdcardSpi_reset(); + + if (sdcard.pendingOperation.callback) { + sdcard.pendingOperation.callback( + SDCARD_BLOCK_OPERATION_READ, + sdcard.pendingOperation.blockIndex, + NULL, + sdcard.pendingOperation.callbackData + ); + } + + goto doMore; + break; + } + break; + case SDCARD_STATE_STOPPING_MULTIPLE_BLOCK_WRITE: + if (sdcardSpi_waitForIdle(SDCARD_MAXIMUM_BYTE_DELAY_FOR_CMD_REPLY)) { + sdcardSpi_deselect(); + + sdcard.state = SDCARD_STATE_READY; + } else if (millis() > sdcard.operationStartTime + SDCARD_TIMEOUT_WRITE_MSEC) { + sdcardSpi_reset(); + goto doMore; + } + break; + case SDCARD_STATE_NOT_PRESENT: + default: + ; + } + + // Is the card's initialization taking too long? + if (sdcard.state >= SDCARD_STATE_RESET && sdcard.state < SDCARD_STATE_READY + && millis() - sdcard.operationStartTime > SDCARD_TIMEOUT_INIT_MILLIS) { + sdcardSpi_reset(); + } + + return sdcardSpi_isReady(); +} + +/** + * Write the 512-byte block from the given buffer into the block with the given index. + * + * If the write does not complete immediately, your callback will be called later. If the write was successful, the + * buffer pointer will be the same buffer you originally passed in, otherwise the buffer will be set to NULL. + * + * Returns: + * SDCARD_OPERATION_IN_PROGRESS - Your buffer is currently being transmitted to the card and your callback will be + * called later to report the completion. The buffer pointer must remain valid until + * that time. + * SDCARD_OPERATION_SUCCESS - Your buffer has been transmitted to the card now. + * SDCARD_OPERATION_BUSY - The card is already busy and cannot accept your write + * SDCARD_OPERATION_FAILURE - Your write was rejected by the card, card will be reset + */ +static sdcardOperationStatus_e sdcardSpi_writeBlock(uint32_t blockIndex, uint8_t *buffer, sdcard_operationCompleteCallback_c callback, uint32_t callbackData) +{ + uint8_t status; + + doMore: + switch (sdcard.state) { + case SDCARD_STATE_WRITING_MULTIPLE_BLOCKS: + // Do we need to cancel the previous multi-block write? + if (blockIndex != sdcard.multiWriteNextBlock) { + if (sdcardSpi_endWriteBlocks() == SDCARD_OPERATION_SUCCESS) { + // Now we've entered the ready state, we can try again + goto doMore; + } else { + return SDCARD_OPERATION_BUSY; + } + } + + // We're continuing a multi-block write + break; + case SDCARD_STATE_READY: + // We're not continuing a multi-block write so we need to send a single-block write command + sdcardSpi_select(); + + // Standard size cards use byte addressing, high capacity cards use block addressing + status = sdcardSpi_sendCommand(SDCARD_COMMAND_WRITE_BLOCK, sdcard.highCapacity ? blockIndex : blockIndex * SDCARD_BLOCK_SIZE); + + if (status != 0) { + sdcardSpi_deselect(); + + sdcardSpi_reset(); + + return SDCARD_OPERATION_FAILURE; + } + break; + default: + return SDCARD_OPERATION_BUSY; + } + + sdcardSpi_sendDataBlockBegin(buffer, sdcard.state == SDCARD_STATE_WRITING_MULTIPLE_BLOCKS); + + sdcard.pendingOperation.buffer = buffer; + sdcard.pendingOperation.blockIndex = blockIndex; + sdcard.pendingOperation.callback = callback; + sdcard.pendingOperation.callbackData = callbackData; + sdcard.pendingOperation.chunkIndex = 1; + sdcard.state = SDCARD_STATE_SENDING_WRITE; + + return SDCARD_OPERATION_IN_PROGRESS; +} + +/** + * Begin writing a series of consecutive blocks beginning at the given block index. This will allow (but not require) + * the SD card to pre-erase the number of blocks you specifiy, which can allow the writes to complete faster. + * + * Afterwards, just call sdcard_writeBlock() as normal to write those blocks consecutively. + * + * It's okay to abort the multi-block write at any time by writing to a non-consecutive address, or by performing a read. + * + * Returns: + * SDCARD_OPERATION_SUCCESS - Multi-block write has been queued + * SDCARD_OPERATION_BUSY - The card is already busy and cannot accept your write + * SDCARD_OPERATION_FAILURE - A fatal error occured, card will be reset + */ +static sdcardOperationStatus_e sdcardSpi_beginWriteBlocks(uint32_t blockIndex, uint32_t blockCount) +{ + if (sdcard.state != SDCARD_STATE_READY) { + if (sdcard.state == SDCARD_STATE_WRITING_MULTIPLE_BLOCKS) { + if (blockIndex == sdcard.multiWriteNextBlock) { + // Assume that the caller wants to continue the multi-block write they already have in progress! + return SDCARD_OPERATION_SUCCESS; + } else if (sdcardSpi_endWriteBlocks() != SDCARD_OPERATION_SUCCESS) { + return SDCARD_OPERATION_BUSY; + } // Else we've completed the previous multi-block write and can fall through to start the new one + } else { + return SDCARD_OPERATION_BUSY; + } + } + + sdcardSpi_select(); + + if ( + sdcardSpi_sendAppCommand(SDCARD_ACOMMAND_SET_WR_BLOCK_ERASE_COUNT, blockCount) == 0 + && sdcardSpi_sendCommand(SDCARD_COMMAND_WRITE_MULTIPLE_BLOCK, sdcard.highCapacity ? blockIndex : blockIndex * SDCARD_BLOCK_SIZE) == 0 + ) { + sdcard.state = SDCARD_STATE_WRITING_MULTIPLE_BLOCKS; + sdcard.multiWriteBlocksRemain = blockCount; + sdcard.multiWriteNextBlock = blockIndex; + + // Leave the card selected + return SDCARD_OPERATION_SUCCESS; + } else { + sdcardSpi_deselect(); + + sdcardSpi_reset(); + + return SDCARD_OPERATION_FAILURE; + } +} + +/** + * Read the 512-byte block with the given index into the given 512-byte buffer. + * + * When the read completes, your callback will be called. If the read was successful, the buffer pointer will be the + * same buffer you originally passed in, otherwise the buffer will be set to NULL. + * + * You must keep the pointer to the buffer valid until the operation completes! + * + * Returns: + * true - The operation was successfully queued for later completion, your callback will be called later + * false - The operation could not be started due to the card being busy (try again later). + */ +static bool sdcardSpi_readBlock(uint32_t blockIndex, uint8_t *buffer, sdcard_operationCompleteCallback_c callback, uint32_t callbackData) +{ + if (sdcard.state != SDCARD_STATE_READY) { + if (sdcard.state == SDCARD_STATE_WRITING_MULTIPLE_BLOCKS) { + if (sdcardSpi_endWriteBlocks() != SDCARD_OPERATION_SUCCESS) { + return false; + } + } else { + return false; + } + } + + sdcardSpi_select(); + + // Standard size cards use byte addressing, high capacity cards use block addressing + uint8_t status = sdcardSpi_sendCommand(SDCARD_COMMAND_READ_SINGLE_BLOCK, sdcard.highCapacity ? blockIndex : blockIndex * SDCARD_BLOCK_SIZE); + + if (status == 0) { + sdcard.pendingOperation.buffer = buffer; + sdcard.pendingOperation.blockIndex = blockIndex; + sdcard.pendingOperation.callback = callback; + sdcard.pendingOperation.callbackData = callbackData; + + sdcard.state = SDCARD_STATE_READING; + + sdcard.operationStartTime = millis(); + + // Leave the card selected for the whole transaction + + return true; + } else { + sdcardSpi_deselect(); + return false; + } +} + +/** + * Returns true if the SD card has successfully completed its startup procedures. + */ +static bool sdcardSpi_isInitialized(void) +{ + return sdcard.state >= SDCARD_STATE_READY; +} + +static const sdcardMetadata_t* sdcardSpi_getMetadata(void) +{ + return &sdcard.metadata; +} + +/** + * Begin the initialization process for the SD card. This must be called first before any other sdcard_ routine. + */ +void sdcardSpi_init(void) +{ + sdcard.dev = busDeviceInit(BUSTYPE_SPI, DEVHW_SDCARD, 0, OWNER_SDCARD); + if (!sdcard.dev) { + sdcard.state = SDCARD_STATE_NOT_PRESENT; + return; + } + + // Max frequency is initially 400kHz + busSetSpeed(sdcard.dev, BUS_SPEED_INITIALIZATION); + + // SDCard wants 1ms minimum delay after power is applied to it + delay(1000); + + // Transmit at least 74 dummy clock cycles with CS high so the SD card can start up + busDeselectDevice(sdcard.dev); + busTransfer(sdcard.dev, NULL, NULL, SDCARD_INIT_NUM_DUMMY_BYTES); + + // Wait for that transmission to finish before we enable the SDCard, so it receives the required number of cycles: + int time = 100000; + while (busIsBusy(sdcard.dev)) { + if (time-- == 0) { + busDeviceDeInit(sdcard.dev); + sdcard.dev = NULL; + sdcard.state = SDCARD_STATE_NOT_PRESENT; + sdcard.failureCount++; + return; + } + } + + sdcard.operationStartTime = millis(); + sdcard.state = SDCARD_STATE_RESET; + sdcard.failureCount = 0; +} + +sdcardVTable_t sdcardSpiVTable = { + .init = &sdcardSpi_init, + .readBlock = &sdcardSpi_readBlock, + .beginWriteBlocks = &sdcardSpi_beginWriteBlocks, + .writeBlock = &sdcardSpi_writeBlock, + .poll = &sdcardSpi_poll, + .isFunctional = &sdcardSpi_isFunctional, + .isInitialized = &sdcardSpi_isInitialized, + .getMetadata = &sdcardSpi_getMetadata, +}; + +#endif diff --git a/src/main/drivers/sensor.h b/src/main/drivers/sensor.h index 050d2c37f51..be828d5007c 100644 --- a/src/main/drivers/sensor.h +++ b/src/main/drivers/sensor.h @@ -50,6 +50,8 @@ typedef bool (*sensorGyroInterruptStatusFuncPtr)(struct gyroDev_s *gyro); struct magDev_s; typedef bool (*sensorMagInitFuncPtr)(struct magDev_s *mag); typedef bool (*sensorMagReadFuncPtr)(struct magDev_s *mag); +struct temperatureDev_s; +typedef bool (*sensorTempReadFuncPtr)(struct temperatureDev_s *tempDev, int16_t *temperature); struct opflowDev_s; typedef bool (*sensorOpflowInitFuncPtr)(struct opflowDev_s *mag); typedef bool (*sensorOpflowUpdateFuncPtr)(struct opflowDev_s *mag); diff --git a/src/main/drivers/serial_softserial.c b/src/main/drivers/serial_softserial.c index e9f77cbb109..ddee3e8808e 100644 --- a/src/main/drivers/serial_softserial.c +++ b/src/main/drivers/serial_softserial.c @@ -37,7 +37,7 @@ #include "drivers/nvic.h" #include "drivers/io.h" -#include "timer.h" +#include "drivers/timer.h" #include "serial.h" #include "serial_softserial.h" @@ -67,11 +67,11 @@ typedef struct softSerial_s { IO_t rxIO; IO_t txIO; - const timerHardware_t *timerHardware; -#ifdef USE_HAL_DRIVER - const TIM_HandleTypeDef *timerHandle; -#endif - const timerHardware_t *exTimerHardware; + TCH_t * tch; + TCH_t * exTch; + + timerCallbacks_t tchCallbacks; + timerCallbacks_t exTchCallbacks; volatile uint8_t rxBuffer[SOFTSERIAL_BUFFER_SIZE]; volatile uint8_t txBuffer[SOFTSERIAL_BUFFER_SIZE]; @@ -93,17 +93,14 @@ typedef struct softSerial_s { uint8_t softSerialPortIndex; timerMode_e timerMode; - - timerOvrHandlerRec_t overCb; - timerCCHandlerRec_t edgeCb; } softSerial_t; static const struct serialPortVTable softSerialVTable; // Forward static softSerial_t softSerialPorts[MAX_SOFTSERIAL_PORTS]; -void onSerialTimerOverflow(timerOvrHandlerRec_t *cbRec, captureCompare_t capture); -void onSerialRxPinChange(timerCCHandlerRec_t *cbRec, captureCompare_t capture); +void onSerialTimerOverflow(TCH_t * tch, uint32_t capture); +void onSerialRxPinChange(TCH_t * tch, uint32_t capture); static void setTxSignal(softSerial_t *softSerial, uint8_t state) { @@ -120,21 +117,17 @@ static void setTxSignal(softSerial_t *softSerial, uint8_t state) static void serialEnableCC(softSerial_t *softSerial) { -#ifdef USE_HAL_DRIVER - TIM_CCxChannelCmd(softSerial->timerHardware->tim, softSerial->timerHardware->channel, TIM_CCx_ENABLE); -#else - TIM_CCxCmd(softSerial->timerHardware->tim, softSerial->timerHardware->channel, TIM_CCx_Enable); -#endif + timerChCaptureEnable(softSerial->tch); } static void serialInputPortActivate(softSerial_t *softSerial) { if (softSerial->port.options & SERIAL_INVERTED) { const uint8_t pinConfig = (softSerial->port.options & SERIAL_BIDIR_NOPULL) ? IOCFG_AF_PP : IOCFG_AF_PP_PD; - IOConfigGPIOAF(softSerial->rxIO, pinConfig, softSerial->timerHardware->alternateFunction); + IOConfigGPIOAF(softSerial->rxIO, pinConfig, softSerial->tch->timHw->alternateFunction); } else { const uint8_t pinConfig = (softSerial->port.options & SERIAL_BIDIR_NOPULL) ? IOCFG_AF_PP : IOCFG_AF_PP_UP; - IOConfigGPIOAF(softSerial->rxIO, pinConfig, softSerial->timerHardware->alternateFunction); + IOConfigGPIOAF(softSerial->rxIO, pinConfig, softSerial->tch->timHw->alternateFunction); } softSerial->rxActive = true; @@ -142,36 +135,29 @@ static void serialInputPortActivate(softSerial_t *softSerial) softSerial->rxBitIndex = 0; // Enable input capture - serialEnableCC(softSerial); } static void serialInputPortDeActivate(softSerial_t *softSerial) { // Disable input capture - -#ifdef USE_HAL_DRIVER - TIM_CCxChannelCmd(softSerial->timerHardware->tim, softSerial->timerHardware->channel, TIM_CCx_DISABLE); -#else - TIM_CCxCmd(softSerial->timerHardware->tim, softSerial->timerHardware->channel, TIM_CCx_Disable); -#endif - - IOConfigGPIOAF(softSerial->rxIO, IOCFG_IN_FLOATING, softSerial->timerHardware->alternateFunction); + timerChCaptureDisable(softSerial->tch); + IOConfigGPIOAF(softSerial->rxIO, IOCFG_IN_FLOATING, softSerial->tch->timHw->alternateFunction); softSerial->rxActive = false; } static void serialOutputPortActivate(softSerial_t *softSerial) { - if (softSerial->exTimerHardware) - IOConfigGPIOAF(softSerial->txIO, IOCFG_OUT_PP, softSerial->exTimerHardware->alternateFunction); + if (softSerial->exTch) + IOConfigGPIOAF(softSerial->txIO, IOCFG_OUT_PP, softSerial->exTch->timHw->alternateFunction); else IOConfigGPIO(softSerial->txIO, IOCFG_OUT_PP); } static void serialOutputPortDeActivate(softSerial_t *softSerial) { - if (softSerial->exTimerHardware) - IOConfigGPIOAF(softSerial->txIO, IOCFG_IN_FLOATING, softSerial->exTimerHardware->alternateFunction); + if (softSerial->exTch) + IOConfigGPIOAF(softSerial->txIO, IOCFG_IN_FLOATING, softSerial->exTch->timHw->alternateFunction); else IOConfigGPIO(softSerial->txIO, IOCFG_IN_FLOATING); } @@ -181,9 +167,9 @@ static bool isTimerPeriodTooLarge(uint32_t timerPeriod) return timerPeriod > 0xFFFF; } -static void serialTimerConfigureTimebase(const timerHardware_t *timerHardwarePtr, uint32_t baud) +static void serialTimerConfigureTimebase(TCH_t * tch, uint32_t baud) { - uint32_t baseClock = SystemCoreClock/timerClockDivisor(timerHardwarePtr->tim); + uint32_t baseClock = SystemCoreClock / timerClockDivisor(tch->timHw->tim); uint32_t clock = baseClock; uint32_t timerPeriod; @@ -199,9 +185,7 @@ static void serialTimerConfigureTimebase(const timerHardware_t *timerHardwarePtr } } while (isTimerPeriodTooLarge(timerPeriod)); - uint16_t mhz = baseClock / 1000000; // XXX Prepare for mhz > 255 - - timerConfigure(timerHardwarePtr, timerPeriod, mhz); + timerConfigure(tch, timerPeriod, baseClock); } static void resetBuffers(softSerial_t *softSerial) @@ -251,7 +235,11 @@ serialPort_t *openSoftSerial(softSerialPortIndex_e portIndex, serialReceiveCallb return NULL; } - softSerial->timerHardware = timerTx; + softSerial->tch = timerGetTCH(timerTx); + if (!softSerial->tch) { + return NULL; + } + softSerial->txIO = txIO; softSerial->rxIO = txIO; IOInit(txIO, OWNER_SOFTSERIAL, RESOURCE_UART_TXRX, RESOURCE_INDEX(portIndex)); @@ -262,26 +250,39 @@ serialPort_t *openSoftSerial(softSerialPortIndex_e portIndex, serialReceiveCallb return NULL; } + softSerial->tch = timerGetTCH(timerRx); + if (!softSerial->tch) { + return NULL; + } + softSerial->rxIO = rxIO; - softSerial->timerHardware = timerRx; IOInit(rxIO, OWNER_SOFTSERIAL, RESOURCE_UART_RX, RESOURCE_INDEX(portIndex)); } if (mode & MODE_TX) { // Need a pin on TX - if (!tagTx) + if (!tagTx) { return NULL; + } softSerial->txIO = txIO; if (!(mode & MODE_RX)) { // TX Simplex, must have a timer - if (!timerTx) + if (!timerTx) { + return NULL; + } + + softSerial->tch = timerGetTCH(timerTx); + if (!softSerial->tch) { return NULL; - softSerial->timerHardware = timerTx; + } } else { // Duplex - softSerial->exTimerHardware = timerTx; + softSerial->exTch = timerGetTCH(timerTx); + if (!softSerial->tch) { + return NULL; + } } IOInit(txIO, OWNER_SOFTSERIAL, RESOURCE_UART_TX, RESOURCE_INDEX(portIndex)); } @@ -305,33 +306,28 @@ serialPort_t *openSoftSerial(softSerialPortIndex_e portIndex, serialReceiveCallb softSerial->isTransmittingData = false; // Configure master timer (on RX); time base and input capture - - serialTimerConfigureTimebase(softSerial->timerHardware, baud); - timerChConfigIC(softSerial->timerHardware, (options & SERIAL_INVERTED) ? ICPOLARITY_RISING : ICPOLARITY_FALLING, 0); - - // Initialize callbacks - timerChCCHandlerInit(&softSerial->edgeCb, onSerialRxPinChange); - timerChOvrHandlerInit(&softSerial->overCb, onSerialTimerOverflow); + serialTimerConfigureTimebase(softSerial->tch, baud); + timerChConfigIC(softSerial->tch, options & SERIAL_INVERTED, 0); // Configure bit clock interrupt & handler. // If we have an extra timer (on TX), it is initialized and configured // for overflow interrupt. // Receiver input capture is configured when input is activated. - - if ((mode & MODE_TX) && softSerial->exTimerHardware && softSerial->exTimerHardware->tim != softSerial->timerHardware->tim) { + if ((mode & MODE_TX) && softSerial->exTch && softSerial->exTch->timHw->tim != softSerial->tch->timHw->tim) { softSerial->timerMode = TIMER_MODE_DUAL; - serialTimerConfigureTimebase(softSerial->exTimerHardware, baud); - timerChConfigCallbacks(softSerial->exTimerHardware, NULL, &softSerial->overCb); - timerChConfigCallbacks(softSerial->timerHardware, &softSerial->edgeCb, NULL); + serialTimerConfigureTimebase(softSerial->exTch, baud); + + timerChInitCallbacks(&softSerial->tchCallbacks, softSerial, onSerialRxPinChange, NULL); + timerChInitCallbacks(&softSerial->exTchCallbacks, softSerial, NULL, onSerialTimerOverflow); + + timerChConfigCallbacks(softSerial->tch, &softSerial->tchCallbacks); + timerChConfigCallbacks(softSerial->exTch, &softSerial->exTchCallbacks); } else { softSerial->timerMode = TIMER_MODE_SINGLE; - timerChConfigCallbacks(softSerial->timerHardware, &softSerial->edgeCb, &softSerial->overCb); + timerChInitCallbacks(&softSerial->tchCallbacks, softSerial, onSerialRxPinChange, onSerialTimerOverflow); + timerChConfigCallbacks(softSerial->tch, &softSerial->tchCallbacks); } -#ifdef USE_HAL_DRIVER - softSerial->timerHandle = timerFindTimerHandle(softSerial->timerHardware->tim); -#endif - if (!(options & SERIAL_BIDIR)) { serialOutputPortActivate(softSerial); setTxSignal(softSerial, ENABLE); @@ -416,7 +412,7 @@ void prepareForNextRxByte(softSerial_t *softSerial) softSerial->isSearchingForStartBit = true; if (softSerial->rxEdge == LEADING) { softSerial->rxEdge = TRAILING; - timerChConfigIC(softSerial->timerHardware, (softSerial->port.options & SERIAL_INVERTED) ? ICPOLARITY_RISING : ICPOLARITY_FALLING, 0); + timerChConfigIC(softSerial->tch, softSerial->port.options & SERIAL_INVERTED, 0); serialEnableCC(softSerial); } } @@ -472,11 +468,11 @@ void processRxState(softSerial_t *softSerial) } } -void onSerialTimerOverflow(timerOvrHandlerRec_t *cbRec, captureCompare_t capture) +void onSerialTimerOverflow(TCH_t * tch, uint32_t capture) { UNUSED(capture); - softSerial_t *self = container_of(cbRec, softSerial_t, overCb); + softSerial_t * self = (softSerial_t *)tch->cb->callbackParam; if (self->port.mode & MODE_TX) processTxState(self); @@ -485,11 +481,11 @@ void onSerialTimerOverflow(timerOvrHandlerRec_t *cbRec, captureCompare_t capture processRxState(self); } -void onSerialRxPinChange(timerCCHandlerRec_t *cbRec, captureCompare_t capture) +void onSerialRxPinChange(TCH_t * tch, uint32_t capture) { UNUSED(capture); - softSerial_t *self = container_of(cbRec, softSerial_t, edgeCb); + softSerial_t * self = (softSerial_t *)tch->cb->callbackParam; bool inverted = self->port.options & SERIAL_INVERTED; if ((self->port.mode & MODE_RX) == 0) { @@ -501,9 +497,9 @@ void onSerialRxPinChange(timerCCHandlerRec_t *cbRec, captureCompare_t capture) // of the bit period. #ifdef USE_HAL_DRIVER - __HAL_TIM_SetCounter(self->timerHandle, __HAL_TIM_GetAutoreload(self->timerHandle) / 2); + __HAL_TIM_SET_COUNTER(self->tch->timCtx->timHandle, __HAL_TIM_GET_AUTORELOAD(self->tch->timCtx->timHandle) / 2); #else - TIM_SetCounter(self->timerHardware->tim, self->timerHardware->tim->ARR / 2); + TIM_SetCounter(self->tch->timHw->tim, timerGetPeriod(self->tch) / 2); #endif // For a mono-timer full duplex configuration, this may clobber the @@ -515,7 +511,7 @@ void onSerialRxPinChange(timerCCHandlerRec_t *cbRec, captureCompare_t capture) self->transmissionErrors++; } - timerChConfigIC(self->timerHardware, inverted ? ICPOLARITY_FALLING : ICPOLARITY_RISING, 0); + timerChConfigIC(self->tch, !inverted, 0); #ifdef STM32F7 serialEnableCC(self); #endif @@ -536,10 +532,10 @@ void onSerialRxPinChange(timerCCHandlerRec_t *cbRec, captureCompare_t capture) if (self->rxEdge == TRAILING) { self->rxEdge = LEADING; - timerChConfigIC(self->timerHardware, inverted ? ICPOLARITY_FALLING : ICPOLARITY_RISING, 0); + timerChConfigIC(self->tch, !inverted, 0); } else { self->rxEdge = TRAILING; - timerChConfigIC(self->timerHardware, inverted ? ICPOLARITY_RISING : ICPOLARITY_FALLING, 0); + timerChConfigIC(self->tch, inverted, 0); } #ifdef STM32F7 serialEnableCC(self); @@ -608,7 +604,7 @@ void softSerialSetBaudRate(serialPort_t *s, uint32_t baudRate) softSerial->port.baudRate = baudRate; - serialTimerConfigureTimebase(softSerial->timerHardware, baudRate); + serialTimerConfigureTimebase(softSerial->tch, baudRate); } void softSerialSetMode(serialPort_t *instance, portMode_t mode) diff --git a/src/main/drivers/serial_uart.c b/src/main/drivers/serial_uart.c index f04676bff2f..02becdcea51 100644 --- a/src/main/drivers/serial_uart.c +++ b/src/main/drivers/serial_uart.c @@ -36,7 +36,7 @@ #include "serial_uart_impl.h" static void usartConfigurePinInversion(uartPort_t *uartPort) { -#if !defined(USE_UART_INVERTER) && !defined(STM32F303xC) +#if !defined(USE_UART_INVERTER) && !defined(STM32F303xC) && !defined(STM32F7) UNUSED(uartPort); #else bool inverted = uartPort->port.options & SERIAL_INVERTED; @@ -146,7 +146,6 @@ serialPort_t *uartOpen(USART_TypeDef *USARTx, serialReceiveCallbackPtr rxCallbac } else { return (serialPort_t *)s; } - s->txDMAEmpty = true; // common serial initialisation code should move to serialPort::init() s->port.rxBufferHead = s->port.rxBufferTail = 0; @@ -160,110 +159,13 @@ serialPort_t *uartOpen(USART_TypeDef *USARTx, serialReceiveCallbackPtr rxCallbac uartReconfigure(s); - // Receive DMA or IRQ - DMA_InitTypeDef DMA_InitStructure; if (mode & MODE_RX) { -#ifdef STM32F4 - if (s->rxDMAStream) { - DMA_StructInit(&DMA_InitStructure); - DMA_InitStructure.DMA_PeripheralBaseAddr = s->rxDMAPeripheralBaseAddr; - DMA_InitStructure.DMA_Priority = DMA_Priority_Medium; - DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable; - DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte; - DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable; - DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte; - DMA_InitStructure.DMA_FIFOMode = DMA_FIFOMode_Disable ; - DMA_InitStructure.DMA_FIFOThreshold = DMA_FIFOThreshold_1QuarterFull ; - DMA_InitStructure.DMA_MemoryBurst = DMA_MemoryBurst_Single ; - DMA_InitStructure.DMA_PeripheralBurst = DMA_PeripheralBurst_Single; -#else - if (s->rxDMAChannel) { - DMA_StructInit(&DMA_InitStructure); - DMA_InitStructure.DMA_PeripheralBaseAddr = s->rxDMAPeripheralBaseAddr; - DMA_InitStructure.DMA_Priority = DMA_Priority_Medium; - DMA_InitStructure.DMA_M2M = DMA_M2M_Disable; - DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable; - DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte; - DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable; - DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte; -#endif - DMA_InitStructure.DMA_BufferSize = s->port.rxBufferSize; - -#ifdef STM32F4 - DMA_InitStructure.DMA_Channel = s->rxDMAChannel; - DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralToMemory; - DMA_InitStructure.DMA_Mode = DMA_Mode_Circular; - DMA_InitStructure.DMA_Memory0BaseAddr = (uint32_t)s->port.rxBuffer; - DMA_DeInit(s->rxDMAStream); - DMA_Init(s->rxDMAStream, &DMA_InitStructure); - DMA_Cmd(s->rxDMAStream, ENABLE); - USART_DMACmd(s->USARTx, USART_DMAReq_Rx, ENABLE); - s->rxDMAPos = DMA_GetCurrDataCounter(s->rxDMAStream); -#else - DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralSRC; - DMA_InitStructure.DMA_Mode = DMA_Mode_Circular; - DMA_InitStructure.DMA_MemoryBaseAddr = (uint32_t)s->port.rxBuffer; - DMA_DeInit(s->rxDMAChannel); - DMA_Init(s->rxDMAChannel, &DMA_InitStructure); - DMA_Cmd(s->rxDMAChannel, ENABLE); - USART_DMACmd(s->USARTx, USART_DMAReq_Rx, ENABLE); - s->rxDMAPos = DMA_GetCurrDataCounter(s->rxDMAChannel); -#endif - } else { - USART_ClearITPendingBit(s->USARTx, USART_IT_RXNE); - USART_ITConfig(s->USARTx, USART_IT_RXNE, ENABLE); - } + USART_ClearITPendingBit(s->USARTx, USART_IT_RXNE); + USART_ITConfig(s->USARTx, USART_IT_RXNE, ENABLE); } - // Transmit DMA or IRQ if (mode & MODE_TX) { -#ifdef STM32F4 - if (s->txDMAStream) { - DMA_StructInit(&DMA_InitStructure); - DMA_InitStructure.DMA_PeripheralBaseAddr = s->txDMAPeripheralBaseAddr; - DMA_InitStructure.DMA_Priority = DMA_Priority_Medium; - DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable; - DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte; - DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable; - DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte; - DMA_InitStructure.DMA_FIFOMode = DMA_FIFOMode_Disable ; - DMA_InitStructure.DMA_FIFOThreshold = DMA_FIFOThreshold_1QuarterFull ; - DMA_InitStructure.DMA_MemoryBurst = DMA_MemoryBurst_Single ; - DMA_InitStructure.DMA_PeripheralBurst = DMA_PeripheralBurst_Single; -#else - if (s->txDMAChannel) { - DMA_StructInit(&DMA_InitStructure); - DMA_InitStructure.DMA_PeripheralBaseAddr = s->txDMAPeripheralBaseAddr; - DMA_InitStructure.DMA_Priority = DMA_Priority_Medium; - DMA_InitStructure.DMA_M2M = DMA_M2M_Disable; - DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable; - DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte; - DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable; - DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte; -#endif - DMA_InitStructure.DMA_BufferSize = s->port.txBufferSize; - -#ifdef STM32F4 - DMA_InitStructure.DMA_Channel = s->txDMAChannel; - DMA_InitStructure.DMA_DIR = DMA_DIR_MemoryToPeripheral; - DMA_InitStructure.DMA_Mode = DMA_Mode_Normal; - DMA_DeInit(s->txDMAStream); - DMA_Init(s->txDMAStream, &DMA_InitStructure); - DMA_ITConfig(s->txDMAStream, DMA_IT_TC | DMA_IT_FE | DMA_IT_TE | DMA_IT_DME, ENABLE); - DMA_SetCurrDataCounter(s->txDMAStream, 0); -#else - DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralDST; - DMA_InitStructure.DMA_Mode = DMA_Mode_Normal; - DMA_DeInit(s->txDMAChannel); - DMA_Init(s->txDMAChannel, &DMA_InitStructure); - DMA_ITConfig(s->txDMAChannel, DMA_IT_TC, ENABLE); - DMA_SetCurrDataCounter(s->txDMAChannel, 0); - s->txDMAChannel->CNDTR = 0; -#endif - USART_DMACmd(s->USARTx, USART_DMAReq_Tx, ENABLE); - } else { - USART_ITConfig(s->USARTx, USART_IT_TXE, ENABLE); - } + USART_ITConfig(s->USARTx, USART_IT_TXE, ENABLE); } USART_Cmd(s->USARTx, ENABLE); @@ -285,52 +187,9 @@ void uartSetMode(serialPort_t *instance, portMode_t mode) uartReconfigure(uartPort); } -void uartStartTxDMA(uartPort_t *s) -{ -#ifdef STM32F4 - DMA_Cmd(s->txDMAStream, DISABLE); - DMA_MemoryTargetConfig(s->txDMAStream, (uint32_t)&s->port.txBuffer[s->port.txBufferTail], DMA_Memory_0); - //s->txDMAStream->M0AR = (uint32_t)&s->port.txBuffer[s->port.txBufferTail]; - if (s->port.txBufferHead > s->port.txBufferTail) { - s->txDMAStream->NDTR = s->port.txBufferHead - s->port.txBufferTail; - s->port.txBufferTail = s->port.txBufferHead; - } - else { - s->txDMAStream->NDTR = s->port.txBufferSize - s->port.txBufferTail; - s->port.txBufferTail = 0; - } - s->txDMAEmpty = false; - DMA_Cmd(s->txDMAStream, ENABLE); -#else - s->txDMAChannel->CMAR = (uint32_t)&s->port.txBuffer[s->port.txBufferTail]; - if (s->port.txBufferHead > s->port.txBufferTail) { - s->txDMAChannel->CNDTR = s->port.txBufferHead - s->port.txBufferTail; - s->port.txBufferTail = s->port.txBufferHead; - } else { - s->txDMAChannel->CNDTR = s->port.txBufferSize - s->port.txBufferTail; - s->port.txBufferTail = 0; - } - s->txDMAEmpty = false; - DMA_Cmd(s->txDMAChannel, ENABLE); -#endif -} - uint32_t uartTotalRxBytesWaiting(const serialPort_t *instance) { const uartPort_t *s = (const uartPort_t*)instance; -#ifdef STM32F4 - if (s->rxDMAStream) { - uint32_t rxDMAHead = s->rxDMAStream->NDTR; -#else - if (s->rxDMAChannel) { - uint32_t rxDMAHead = s->rxDMAChannel->CNDTR; -#endif - if (rxDMAHead >= s->rxDMAPos) { - return rxDMAHead - s->rxDMAPos; - } else { - return s->port.rxBufferSize + rxDMAHead - s->rxDMAPos; - } - } if (s->port.rxBufferHead >= s->port.rxBufferTail) { return s->port.rxBufferHead - s->port.rxBufferTail; @@ -351,48 +210,13 @@ uint32_t uartTotalTxBytesFree(const serialPort_t *instance) bytesUsed = s->port.txBufferSize + s->port.txBufferHead - s->port.txBufferTail; } -#ifdef STM32F4 - if (s->txDMAStream) { - /* - * When we queue up a DMA request, we advance the Tx buffer tail before the transfer finishes, so we must add - * the remaining size of that in-progress transfer here instead: - */ - bytesUsed += s->txDMAStream->NDTR; -#else - if (s->txDMAChannel) { - /* - * When we queue up a DMA request, we advance the Tx buffer tail before the transfer finishes, so we must add - * the remaining size of that in-progress transfer here instead: - */ - bytesUsed += s->txDMAChannel->CNDTR; -#endif - /* - * If the Tx buffer is being written to very quickly, we might have advanced the head into the buffer - * space occupied by the current DMA transfer. In that case the "bytesUsed" total will actually end up larger - * than the total Tx buffer size, because we'll end up transmitting the same buffer region twice. (So we'll be - * transmitting a garbage mixture of old and new bytes). - * - * Be kind to callers and pretend like our buffer can only ever be 100% full. - */ - if (bytesUsed >= s->port.txBufferSize - 1) { - return 0; - } - } - return (s->port.txBufferSize - 1) - bytesUsed; } bool isUartTransmitBufferEmpty(const serialPort_t *instance) { const uartPort_t *s = (const uartPort_t *)instance; -#ifdef STM32F4 - if (s->txDMAStream) -#else - if (s->txDMAChannel) -#endif - return s->txDMAEmpty; - else - return s->port.txBufferTail == s->port.txBufferHead; + return s->port.txBufferTail == s->port.txBufferHead; } uint8_t uartRead(serialPort_t *instance) @@ -400,21 +224,11 @@ uint8_t uartRead(serialPort_t *instance) uint8_t ch; uartPort_t *s = (uartPort_t *)instance; -#ifdef STM32F4 - if (s->rxDMAStream) { -#else - if (s->rxDMAChannel) { -#endif - ch = s->port.rxBuffer[s->port.rxBufferSize - s->rxDMAPos]; - if (--s->rxDMAPos == 0) - s->rxDMAPos = s->port.rxBufferSize; + ch = s->port.rxBuffer[s->port.rxBufferTail]; + if (s->port.rxBufferTail + 1 >= s->port.rxBufferSize) { + s->port.rxBufferTail = 0; } else { - ch = s->port.rxBuffer[s->port.rxBufferTail]; - if (s->port.rxBufferTail + 1 >= s->port.rxBufferSize) { - s->port.rxBufferTail = 0; - } else { - s->port.rxBufferTail++; - } + s->port.rxBufferTail++; } return ch; @@ -430,17 +244,7 @@ void uartWrite(serialPort_t *instance, uint8_t ch) s->port.txBufferHead++; } -#ifdef STM32F4 - if (s->txDMAStream) { - if (!(s->txDMAStream->CR & 1)) -#else - if (s->txDMAChannel) { - if (!(s->txDMAChannel->CCR & 1)) -#endif - uartStartTxDMA(s); - } else { - USART_ITConfig(s->USARTx, USART_IT_TXE, ENABLE); - } + USART_ITConfig(s->USARTx, USART_IT_TXE, ENABLE); } const struct serialPortVTable uartVTable[] = { diff --git a/src/main/drivers/serial_uart.h b/src/main/drivers/serial_uart.h index 3b318f16f97..0f30b7d4a7b 100644 --- a/src/main/drivers/serial_uart.h +++ b/src/main/drivers/serial_uart.h @@ -54,32 +54,10 @@ typedef enum { typedef struct { serialPort_t port; -#if defined(STM32F7) - DMA_HandleTypeDef rxDMAHandle; - DMA_HandleTypeDef txDMAHandle; -#endif -#if defined(STM32F4) || defined(STM32F7) - DMA_Stream_TypeDef *rxDMAStream; - DMA_Stream_TypeDef *txDMAStream; - uint32_t rxDMAChannel; - uint32_t txDMAChannel; -#else - DMA_Channel_TypeDef *rxDMAChannel; - DMA_Channel_TypeDef *txDMAChannel; -#endif - uint32_t rxDMAIrq; - uint32_t txDMAIrq; - - uint32_t rxDMAPos; - bool txDMAEmpty; - - uint32_t txDMAPeripheralBaseAddr; - uint32_t rxDMAPeripheralBaseAddr; - #ifdef USE_HAL_DRIVER - // All USARTs can also be used as UART, and we use them only as UART. UART_HandleTypeDef Handle; #endif + USART_TypeDef *USARTx; } uartPort_t; diff --git a/src/main/drivers/serial_uart_hal.c b/src/main/drivers/serial_uart_hal.c index 12540cc81fb..b14f984ef89 100644 --- a/src/main/drivers/serial_uart_hal.c +++ b/src/main/drivers/serial_uart_hal.c @@ -31,7 +31,6 @@ #include "common/utils.h" #include "drivers/io.h" #include "drivers/nvic.h" -#include "dma.h" #include "serial.h" #include "serial_uart.h" @@ -96,82 +95,21 @@ static void uartReconfigure(uartPort_t *uartPort) HAL_UART_Init(&uartPort->Handle); } - // Receive DMA or IRQ - if (uartPort->port.mode & MODE_RX) - { - if (uartPort->rxDMAStream) - { - uartPort->rxDMAHandle.Instance = uartPort->rxDMAStream; - uartPort->rxDMAHandle.Init.Channel = uartPort->rxDMAChannel; - uartPort->rxDMAHandle.Init.Direction = DMA_PERIPH_TO_MEMORY; - uartPort->rxDMAHandle.Init.PeriphInc = DMA_PINC_DISABLE; - uartPort->rxDMAHandle.Init.MemInc = DMA_MINC_ENABLE; - uartPort->rxDMAHandle.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE; - uartPort->rxDMAHandle.Init.MemDataAlignment = DMA_MDATAALIGN_BYTE; - uartPort->rxDMAHandle.Init.Mode = DMA_CIRCULAR; - uartPort->rxDMAHandle.Init.FIFOMode = DMA_FIFOMODE_DISABLE; - uartPort->rxDMAHandle.Init.FIFOThreshold = DMA_FIFO_THRESHOLD_1QUARTERFULL; - uartPort->rxDMAHandle.Init.PeriphBurst = DMA_PBURST_SINGLE; - uartPort->rxDMAHandle.Init.MemBurst = DMA_MBURST_SINGLE; - uartPort->rxDMAHandle.Init.Priority = DMA_PRIORITY_MEDIUM; - - - HAL_DMA_DeInit(&uartPort->rxDMAHandle); - HAL_DMA_Init(&uartPort->rxDMAHandle); - /* Associate the initialized DMA handle to the UART handle */ - __HAL_LINKDMA(&uartPort->Handle, hdmarx, uartPort->rxDMAHandle); - - HAL_UART_Receive_DMA(&uartPort->Handle, (uint8_t*)uartPort->port.rxBuffer, uartPort->port.rxBufferSize); - - uartPort->rxDMAPos = __HAL_DMA_GET_COUNTER(&uartPort->rxDMAHandle); + if (uartPort->port.mode & MODE_RX) { + /* Enable the UART Parity Error Interrupt */ + SET_BIT(uartPort->USARTx->CR1, USART_CR1_PEIE); - } - else - { - /* Enable the UART Parity Error Interrupt */ - SET_BIT(uartPort->USARTx->CR1, USART_CR1_PEIE); - - /* Enable the UART Error Interrupt: (Frame error, noise error, overrun error) */ - SET_BIT(uartPort->USARTx->CR3, USART_CR3_EIE); + /* Enable the UART Error Interrupt: (Frame error, noise error, overrun error) */ + SET_BIT(uartPort->USARTx->CR3, USART_CR3_EIE); - /* Enable the UART Data Register not empty Interrupt */ - SET_BIT(uartPort->USARTx->CR1, USART_CR1_RXNEIE); - } + /* Enable the UART Data Register not empty Interrupt */ + SET_BIT(uartPort->USARTx->CR1, USART_CR1_RXNEIE); } - // Transmit DMA or IRQ + // Transmit IRQ if (uartPort->port.mode & MODE_TX) { - - if (uartPort->txDMAStream) { - uartPort->txDMAHandle.Instance = uartPort->txDMAStream; - uartPort->txDMAHandle.Init.Channel = uartPort->txDMAChannel; - uartPort->txDMAHandle.Init.Direction = DMA_MEMORY_TO_PERIPH; - uartPort->txDMAHandle.Init.PeriphInc = DMA_PINC_DISABLE; - uartPort->txDMAHandle.Init.MemInc = DMA_MINC_ENABLE; - uartPort->txDMAHandle.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE; - uartPort->txDMAHandle.Init.MemDataAlignment = DMA_MDATAALIGN_BYTE; - uartPort->txDMAHandle.Init.Mode = DMA_NORMAL; - uartPort->txDMAHandle.Init.FIFOMode = DMA_FIFOMODE_DISABLE; - uartPort->txDMAHandle.Init.FIFOThreshold = DMA_FIFO_THRESHOLD_1QUARTERFULL; - uartPort->txDMAHandle.Init.PeriphBurst = DMA_PBURST_SINGLE; - uartPort->txDMAHandle.Init.MemBurst = DMA_MBURST_SINGLE; - uartPort->txDMAHandle.Init.Priority = DMA_PRIORITY_MEDIUM; - - - HAL_DMA_DeInit(&uartPort->txDMAHandle); - HAL_StatusTypeDef status = HAL_DMA_Init(&uartPort->txDMAHandle); - if (status != HAL_OK) - { - while (1); - } - /* Associate the initialized DMA handle to the UART handle */ - __HAL_LINKDMA(&uartPort->Handle, hdmatx, uartPort->txDMAHandle); - - __HAL_DMA_SET_COUNTER(&uartPort->txDMAHandle, 0); - } else { - /* Enable the UART Transmit Data Register Empty Interrupt */ - SET_BIT(uartPort->USARTx->CR1, USART_CR1_TXEIE); - } + /* Enable the UART Transmit Data Register Empty Interrupt */ + SET_BIT(uartPort->USARTx->CR1, USART_CR1_TXEIE); } return; } @@ -218,8 +156,6 @@ serialPort_t *uartOpen(USART_TypeDef *USARTx, serialReceiveCallbackPtr callback, } - s->txDMAEmpty = true; - // common serial initialisation code should move to serialPort::init() s->port.rxBufferHead = s->port.rxBufferTail = 0; s->port.txBufferHead = s->port.txBufferTail = 0; @@ -249,42 +185,10 @@ void uartSetMode(serialPort_t *instance, portMode_t mode) uartReconfigure(uartPort); } -void uartStartTxDMA(uartPort_t *s) -{ - uint16_t size = 0; - uint32_t fromwhere=0; - HAL_UART_StateTypeDef state = HAL_UART_GetState(&s->Handle); - if ((state & HAL_UART_STATE_BUSY_TX) == HAL_UART_STATE_BUSY_TX) - return; - - if (s->port.txBufferHead > s->port.txBufferTail) { - size = s->port.txBufferHead - s->port.txBufferTail; - fromwhere = s->port.txBufferTail; - s->port.txBufferTail = s->port.txBufferHead; - } else { - size = s->port.txBufferSize - s->port.txBufferTail; - fromwhere = s->port.txBufferTail; - s->port.txBufferTail = 0; - } - s->txDMAEmpty = false; - //HAL_CLEANCACHE((uint8_t *)&s->port.txBuffer[fromwhere],size); - HAL_UART_Transmit_DMA(&s->Handle, (uint8_t *)&s->port.txBuffer[fromwhere], size); -} - uint32_t uartTotalRxBytesWaiting(const serialPort_t *instance) { uartPort_t *s = (uartPort_t*)instance; - if (s->rxDMAStream) { - uint32_t rxDMAHead = __HAL_DMA_GET_COUNTER(s->Handle.hdmarx); - - if (rxDMAHead >= s->rxDMAPos) { - return rxDMAHead - s->rxDMAPos; - } else { - return s->port.rxBufferSize + rxDMAHead - s->rxDMAPos; - } - } - if (s->port.rxBufferHead >= s->port.rxBufferTail) { return s->port.rxBufferHead - s->port.rxBufferTail; } else { @@ -304,37 +208,13 @@ uint32_t uartTotalTxBytesFree(const serialPort_t *instance) bytesUsed = s->port.txBufferSize + s->port.txBufferHead - s->port.txBufferTail; } - if (s->txDMAStream) { - /* - * When we queue up a DMA request, we advance the Tx buffer tail before the transfer finishes, so we must add - * the remaining size of that in-progress transfer here instead: - */ - bytesUsed += __HAL_DMA_GET_COUNTER(s->Handle.hdmatx); - - /* - * If the Tx buffer is being written to very quickly, we might have advanced the head into the buffer - * space occupied by the current DMA transfer. In that case the "bytesUsed" total will actually end up larger - * than the total Tx buffer size, because we'll end up transmitting the same buffer region twice. (So we'll be - * transmitting a garbage mixture of old and new bytes). - * - * Be kind to callers and pretend like our buffer can only ever be 100% full. - */ - if (bytesUsed >= s->port.txBufferSize - 1) { - return 0; - } - } - return (s->port.txBufferSize - 1) - bytesUsed; } bool isUartTransmitBufferEmpty(const serialPort_t *instance) { uartPort_t *s = (uartPort_t *)instance; - if (s->txDMAStream) - - return s->txDMAEmpty; - else - return s->port.txBufferTail == s->port.txBufferHead; + return s->port.txBufferTail == s->port.txBufferHead; } uint8_t uartRead(serialPort_t *instance) @@ -343,18 +223,11 @@ uint8_t uartRead(serialPort_t *instance) uartPort_t *s = (uartPort_t *)instance; - if (s->rxDMAStream) { - - ch = s->port.rxBuffer[s->port.rxBufferSize - s->rxDMAPos]; - if (--s->rxDMAPos == 0) - s->rxDMAPos = s->port.rxBufferSize; + ch = s->port.rxBuffer[s->port.rxBufferTail]; + if (s->port.rxBufferTail + 1 >= s->port.rxBufferSize) { + s->port.rxBufferTail = 0; } else { - ch = s->port.rxBuffer[s->port.rxBufferTail]; - if (s->port.rxBufferTail + 1 >= s->port.rxBufferSize) { - s->port.rxBufferTail = 0; - } else { - s->port.rxBufferTail++; - } + s->port.rxBufferTail++; } return ch; @@ -370,12 +243,7 @@ void uartWrite(serialPort_t *instance, uint8_t ch) s->port.txBufferHead++; } - if (s->txDMAStream) { - if (!(s->txDMAStream->CR & 1)) - uartStartTxDMA(s); - } else { - __HAL_UART_ENABLE_IT(&s->Handle, UART_IT_TXE); - } + __HAL_UART_ENABLE_IT(&s->Handle, UART_IT_TXE); } const struct serialPortVTable uartVTable[] = { diff --git a/src/main/drivers/serial_uart_stm32f30x.c b/src/main/drivers/serial_uart_stm32f30x.c index e71e145f4fc..b76cd2bf6bf 100644 --- a/src/main/drivers/serial_uart_stm32f30x.c +++ b/src/main/drivers/serial_uart_stm32f30x.c @@ -31,7 +31,6 @@ #include "drivers/time.h" #include "drivers/io.h" #include "drivers/nvic.h" -#include "dma.h" #include "rcc.h" #include "serial.h" @@ -99,18 +98,6 @@ static uartPort_t uartPort4; static uartPort_t uartPort5; #endif -static void handleUsartTxDma(dmaChannelDescriptor_t* descriptor) -{ - uartPort_t *s = (uartPort_t*)(descriptor->userParam); - DMA_CLEAR_FLAG(descriptor, DMA_IT_TCIF); - DMA_Cmd(descriptor->ref, DISABLE); - - if (s->port.txBufferHead != s->port.txBufferTail) - uartStartTxDMA(s); - else - s->txDMAEmpty = true; -} - void serialUARTInit(IO_t tx, IO_t rx, portMode_t mode, portOptions_t options, uint8_t af, uint8_t index) { if (options & SERIAL_BIDIR) { @@ -141,7 +128,9 @@ void serialUARTInit(IO_t tx, IO_t rx, portMode_t mode, portOptions_t options, ui #ifdef USE_UART1 uartPort_t *serialUART1(uint32_t baudRate, portMode_t mode, portOptions_t options) { + NVIC_InitTypeDef NVIC_InitStructure; uartPort_t *s; + static volatile uint8_t rx1Buffer[UART1_RX_BUFFER_SIZE]; static volatile uint8_t tx1Buffer[UART1_TX_BUFFER_SIZE]; @@ -155,32 +144,17 @@ uartPort_t *serialUART1(uint32_t baudRate, portMode_t mode, portOptions_t option s->port.rxBufferSize = UART1_RX_BUFFER_SIZE; s->port.txBufferSize = UART1_TX_BUFFER_SIZE; -#ifdef USE_UART1_RX_DMA - s->rxDMAChannel = DMA1_Channel5; -#endif - s->txDMAChannel = DMA1_Channel4; - s->USARTx = USART1; - s->rxDMAPeripheralBaseAddr = (uint32_t)&s->USARTx->RDR; - s->txDMAPeripheralBaseAddr = (uint32_t)&s->USARTx->TDR; - RCC_ClockCmd(RCC_APB2(USART1), ENABLE); - RCC_ClockCmd(RCC_AHB(DMA1), ENABLE); serialUARTInit(IOGetByTag(IO_TAG(UART1_TX_PIN)), IOGetByTag(IO_TAG(UART1_RX_PIN)), mode, options, GPIO_AF_7, 1); - dmaSetHandler(DMA1_CH4_HANDLER, handleUsartTxDma, NVIC_PRIO_SERIALUART1_TXDMA, (uint32_t)&uartPort1); - -#ifndef USE_UART1_RX_DMA - NVIC_InitTypeDef NVIC_InitStructure; - NVIC_InitStructure.NVIC_IRQChannel = USART1_IRQn; - NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(NVIC_PRIO_SERIALUART1_RXDMA); - NVIC_InitStructure.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(NVIC_PRIO_SERIALUART1_RXDMA); + NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(NVIC_PRIO_SERIALUART1); + NVIC_InitStructure.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(NVIC_PRIO_SERIALUART1); NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; NVIC_Init(&NVIC_InitStructure); -#endif return s; } @@ -189,7 +163,9 @@ uartPort_t *serialUART1(uint32_t baudRate, portMode_t mode, portOptions_t option #ifdef USE_UART2 uartPort_t *serialUART2(uint32_t baudRate, portMode_t mode, portOptions_t options) { + NVIC_InitTypeDef NVIC_InitStructure; uartPort_t *s; + static volatile uint8_t rx2Buffer[UART2_RX_BUFFER_SIZE]; static volatile uint8_t tx2Buffer[UART2_TX_BUFFER_SIZE]; @@ -205,37 +181,15 @@ uartPort_t *serialUART2(uint32_t baudRate, portMode_t mode, portOptions_t option s->USARTx = USART2; -#ifdef USE_UART2_RX_DMA - s->rxDMAChannel = DMA1_Channel6; - s->rxDMAPeripheralBaseAddr = (uint32_t)&s->USARTx->RDR; -#endif -#ifdef USE_UART2_TX_DMA - s->txDMAChannel = DMA1_Channel7; - s->txDMAPeripheralBaseAddr = (uint32_t)&s->USARTx->TDR; -#endif - RCC_ClockCmd(RCC_APB1(USART2), ENABLE); -#if defined(USE_UART2_TX_DMA) || defined(USE_UART2_RX_DMA) - RCC_ClockCmd(RCC_AHB(DMA1), ENABLE); -#endif - serialUARTInit(IOGetByTag(IO_TAG(UART2_TX_PIN)), IOGetByTag(IO_TAG(UART2_RX_PIN)), mode, options, GPIO_AF_7, 2); -#ifdef USE_UART2_TX_DMA - // DMA TX Interrupt - dmaSetHandler(DMA1_CH7_HANDLER, handleUsartTxDma, NVIC_PRIO_SERIALUART2_TXDMA, (uint32_t)&uartPort2); -#endif - -#ifndef USE_UART2_RX_DMA - NVIC_InitTypeDef NVIC_InitStructure; - NVIC_InitStructure.NVIC_IRQChannel = USART2_IRQn; - NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(NVIC_PRIO_SERIALUART2_RXDMA); - NVIC_InitStructure.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(NVIC_PRIO_SERIALUART2_RXDMA); + NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(NVIC_PRIO_SERIALUART2); + NVIC_InitStructure.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(NVIC_PRIO_SERIALUART2); NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; NVIC_Init(&NVIC_InitStructure); -#endif return s; } @@ -244,7 +198,9 @@ uartPort_t *serialUART2(uint32_t baudRate, portMode_t mode, portOptions_t option #ifdef USE_UART3 uartPort_t *serialUART3(uint32_t baudRate, portMode_t mode, portOptions_t options) { + NVIC_InitTypeDef NVIC_InitStructure; uartPort_t *s; + static volatile uint8_t rx3Buffer[UART3_RX_BUFFER_SIZE]; static volatile uint8_t tx3Buffer[UART3_TX_BUFFER_SIZE]; @@ -260,37 +216,15 @@ uartPort_t *serialUART3(uint32_t baudRate, portMode_t mode, portOptions_t option s->USARTx = USART3; -#ifdef USE_UART3_RX_DMA - s->rxDMAChannel = DMA1_Channel3; - s->rxDMAPeripheralBaseAddr = (uint32_t)&s->USARTx->RDR; -#endif -#ifdef USE_UART3_TX_DMA - s->txDMAChannel = DMA1_Channel2; - s->txDMAPeripheralBaseAddr = (uint32_t)&s->USARTx->TDR; -#endif - RCC_ClockCmd(RCC_APB1(USART3), ENABLE); -#if defined(USE_UART3_TX_DMA) || defined(USE_UART3_RX_DMA) - RCC_AHBClockCmd(RCC_AHB(DMA1), ENABLE); -#endif - serialUARTInit(IOGetByTag(IO_TAG(UART3_TX_PIN)), IOGetByTag(IO_TAG(UART3_RX_PIN)), mode, options, GPIO_AF_7, 3); -#ifdef USE_UART3_TX_DMA - // DMA TX Interrupt - dmaSetHandler(DMA1_CH2_HANDLER, handleUsartTxDma, NVIC_PRIO_SERIALUART3_TXDMA, (uint32_t)&uartPort3); -#endif - -#ifndef USE_UART3_RX_DMA - NVIC_InitTypeDef NVIC_InitStructure; - NVIC_InitStructure.NVIC_IRQChannel = USART3_IRQn; - NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(NVIC_PRIO_SERIALUART3_RXDMA); - NVIC_InitStructure.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(NVIC_PRIO_SERIALUART3_RXDMA); + NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(NVIC_PRIO_SERIALUART3); + NVIC_InitStructure.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(NVIC_PRIO_SERIALUART3); NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; NVIC_Init(&NVIC_InitStructure); -#endif return s; } @@ -368,7 +302,7 @@ void usartIrqHandler(uartPort_t *s) { uint32_t ISR = s->USARTx->ISR; - if (!s->rxDMAChannel && (ISR & USART_FLAG_RXNE)) { + if (ISR & USART_FLAG_RXNE) { if (s->port.rxCallback) { s->port.rxCallback(s->USARTx->RDR, s->port.rxCallbackData); } else { @@ -379,7 +313,7 @@ void usartIrqHandler(uartPort_t *s) } } - if (!s->txDMAChannel && (ISR & USART_FLAG_TXE)) { + if (ISR & USART_FLAG_TXE) { if (s->port.txBufferTail != s->port.txBufferHead) { USART_SendData(s->USARTx, s->port.txBuffer[s->port.txBufferTail++]); if (s->port.txBufferTail >= s->port.txBufferSize) { diff --git a/src/main/drivers/serial_uart_stm32f4xx.c b/src/main/drivers/serial_uart_stm32f4xx.c index df4343ed66b..aaafc2b25a7 100644 --- a/src/main/drivers/serial_uart_stm32f4xx.c +++ b/src/main/drivers/serial_uart_stm32f4xx.c @@ -24,7 +24,6 @@ #include "drivers/io.h" #include "rcc.h" #include "drivers/nvic.h" -#include "dma.h" #include "serial.h" #include "serial_uart.h" @@ -36,9 +35,6 @@ typedef struct uartDevice_s { USART_TypeDef* dev; uartPort_t port; - uint32_t DMAChannel; - DMA_Stream_TypeDef *txDMAStream; - DMA_Stream_TypeDef *rxDMAStream; ioTag_t rx; ioTag_t tx; volatile uint8_t rxBuffer[UART_RX_BUFFER_SIZE]; @@ -47,23 +43,14 @@ typedef struct uartDevice_s { rccPeriphTag_t rcc_apb2; rccPeriphTag_t rcc_apb1; uint8_t af; - uint8_t txIrq; - uint8_t rxIrq; - uint32_t txPriority; - uint32_t rxPriority; + uint8_t irq; + uint32_t irqPriority; } uartDevice_t; //static uartPort_t uartPort[MAX_UARTS]; #ifdef USE_UART1 static uartDevice_t uart1 = { - .DMAChannel = DMA_Channel_4, -#ifdef USE_UART1_RX_DMA - .rxDMAStream = DMA2_Stream5, -#endif -#ifdef USE_UART1_TX_DMA - .txDMAStream = DMA2_Stream7, -#endif .dev = USART1, .rx = IO_TAG(UART1_RX_PIN), .tx = IO_TAG(UART1_TX_PIN), @@ -72,23 +59,14 @@ static uartDevice_t uart1 = .rcc_ahb1 = UART1_AHB1_PERIPHERALS, #endif .rcc_apb2 = RCC_APB2(USART1), - .txIrq = DMA2_ST7_HANDLER, - .rxIrq = USART1_IRQn, - .txPriority = NVIC_PRIO_SERIALUART1_TXDMA, - .rxPriority = NVIC_PRIO_SERIALUART1 + .irq = USART1_IRQn, + .irqPriority = NVIC_PRIO_SERIALUART1 }; #endif #ifdef USE_UART2 static uartDevice_t uart2 = { - .DMAChannel = DMA_Channel_4, -#ifdef USE_UART2_RX_DMA - .rxDMAStream = DMA1_Stream5, -#endif -#ifdef USE_UART2_TX_DMA - .txDMAStream = DMA1_Stream6, -#endif .dev = USART2, .rx = IO_TAG(UART2_RX_PIN), .tx = IO_TAG(UART2_TX_PIN), @@ -97,23 +75,14 @@ static uartDevice_t uart2 = .rcc_ahb1 = UART2_AHB1_PERIPHERALS, #endif .rcc_apb1 = RCC_APB1(USART2), - .txIrq = DMA1_ST6_HANDLER, - .rxIrq = USART2_IRQn, - .txPriority = NVIC_PRIO_SERIALUART2_TXDMA, - .rxPriority = NVIC_PRIO_SERIALUART2 + .irq = USART2_IRQn, + .irqPriority = NVIC_PRIO_SERIALUART2 }; #endif #ifdef USE_UART3 static uartDevice_t uart3 = { - .DMAChannel = DMA_Channel_4, -#ifdef USE_UART3_RX_DMA - .rxDMAStream = DMA1_Stream1, -#endif -#ifdef USE_UART3_TX_DMA - .txDMAStream = DMA1_Stream3, -#endif .dev = USART3, .rx = IO_TAG(UART3_RX_PIN), .tx = IO_TAG(UART3_TX_PIN), @@ -122,23 +91,14 @@ static uartDevice_t uart3 = .rcc_ahb1 = UART3_AHB1_PERIPHERALS, #endif .rcc_apb1 = RCC_APB1(USART3), - .txIrq = DMA1_ST3_HANDLER, - .rxIrq = USART3_IRQn, - .txPriority = NVIC_PRIO_SERIALUART3_TXDMA, - .rxPriority = NVIC_PRIO_SERIALUART3 + .irq = USART3_IRQn, + .irqPriority = NVIC_PRIO_SERIALUART3 }; #endif #ifdef USE_UART4 static uartDevice_t uart4 = { - .DMAChannel = DMA_Channel_4, -#ifdef USE_UART4_RX_DMA - .rxDMAStream = DMA1_Stream2, -#endif -#ifdef USE_UART4_TX_DMA - .txDMAStream = DMA1_Stream4, -#endif .dev = UART4, .rx = IO_TAG(UART4_RX_PIN), .tx = IO_TAG(UART4_TX_PIN), @@ -147,23 +107,14 @@ static uartDevice_t uart4 = .rcc_ahb1 = UART4_AHB1_PERIPHERALS, #endif .rcc_apb1 = RCC_APB1(UART4), - .txIrq = DMA1_ST4_HANDLER, - .rxIrq = UART4_IRQn, - .txPriority = NVIC_PRIO_SERIALUART4_TXDMA, - .rxPriority = NVIC_PRIO_SERIALUART4 + .irq = UART4_IRQn, + .irqPriority = NVIC_PRIO_SERIALUART4 }; #endif #ifdef USE_UART5 static uartDevice_t uart5 = { - .DMAChannel = DMA_Channel_4, -#ifdef USE_UART5_RX_DMA - .rxDMAStream = DMA1_Stream0, -#endif -#ifdef USE_UART5_TX_DMA - .txDMAStream = DMA1_Stream7, -#endif .dev = UART5, .rx = IO_TAG(UART5_RX_PIN), .tx = IO_TAG(UART5_TX_PIN), @@ -172,23 +123,14 @@ static uartDevice_t uart5 = .rcc_ahb1 = UART5_AHB1_PERIPHERALS, #endif .rcc_apb1 = RCC_APB1(UART5), - .txIrq = DMA1_ST7_HANDLER, - .rxIrq = UART5_IRQn, - .txPriority = NVIC_PRIO_SERIALUART5_TXDMA, - .rxPriority = NVIC_PRIO_SERIALUART5 + .irq = UART5_IRQn, + .irqPriority = NVIC_PRIO_SERIALUART5 }; #endif #ifdef USE_UART6 static uartDevice_t uart6 = { - .DMAChannel = DMA_Channel_5, -#ifdef USE_UART6_RX_DMA - .rxDMAStream = DMA2_Stream1, -#endif -#ifdef USE_UART6_RX_DMA - .txDMAStream = DMA2_Stream6, -#endif .dev = USART6, .rx = IO_TAG(UART6_RX_PIN), .tx = IO_TAG(UART6_TX_PIN), @@ -197,52 +139,34 @@ static uartDevice_t uart6 = .rcc_ahb1 = UART6_AHB1_PERIPHERALS, #endif .rcc_apb2 = RCC_APB2(USART6), - .txIrq = DMA2_ST6_HANDLER, - .rxIrq = USART6_IRQn, - .txPriority = NVIC_PRIO_SERIALUART6_TXDMA, - .rxPriority = NVIC_PRIO_SERIALUART6 + .irq = USART6_IRQn, + .irqPriority = NVIC_PRIO_SERIALUART6 }; #endif #ifdef USE_UART7 static uartDevice_t uart7 = { - .DMAChannel = DMA_Channel_5, -#ifdef USE_UART7_RX_DMA - .rxDMAStream = DMA1_Stream3, -#endif -#ifdef USE_UART7_TX_DMA - .txDMAStream = DMA1_Stream1, -#endif .dev = UART7, .rx = IO_TAG(UART7_RX_PIN), .tx = IO_TAG(UART7_TX_PIN), .af = GPIO_AF_UART7, .rcc_apb1 = RCC_APB1(UART7), - .rxIrq = UART7_IRQn, - .txPriority = NVIC_PRIO_SERIALUART7_TXDMA, - .rxPriority = NVIC_PRIO_SERIALUART7 + .irq = UART7_IRQn, + .irqPriority = NVIC_PRIO_SERIALUART7 }; #endif #ifdef USE_UART8 static uartDevice_t uart8 = { - .DMAChannel = DMA_Channel_5, -#ifdef USE_UART8_RX_DMA - .rxDMAStream = DMA1_Stream6, -#endif -#ifdef USE_UART8_TX_DMA - .txDMAStream = DMA1_Stream0, -#endif .dev = UART8, .rx = IO_TAG(UART8_RX_PIN), .tx = IO_TAG(UART8_TX_PIN), .af = GPIO_AF_UART8, .rcc_apb1 = RCC_APB1(UART8), - .rxIrq = UART8_IRQn, - .txPriority = NVIC_PRIO_SERIALUART8_TXDMA, - .rxPriority = NVIC_PRIO_SERIALUART8 + .irq = UART8_IRQn, + .irqPriority = NVIC_PRIO_SERIALUART8 }; #endif @@ -291,7 +215,7 @@ static uartDevice_t* uartHardwareMap[] = { void uartIrqHandler(uartPort_t *s) { - if (!s->rxDMAStream && (USART_GetITStatus(s->USARTx, USART_IT_RXNE) == SET)) { + if (USART_GetITStatus(s->USARTx, USART_IT_RXNE) == SET) { if (s->port.rxCallback) { s->port.rxCallback(s->USARTx->DR, s->port.rxCallbackData); } else { @@ -300,7 +224,7 @@ void uartIrqHandler(uartPort_t *s) } } - if (!s->txDMAStream && (USART_GetITStatus(s->USARTx, USART_IT_TXE) == SET)) { + if (USART_GetITStatus(s->USARTx, USART_IT_TXE) == SET) { if (s->port.txBufferTail != s->port.txBufferHead) { USART_SendData(s->USARTx, s->port.txBuffer[s->port.txBufferTail]); s->port.txBufferTail = (s->port.txBufferTail + 1) % s->port.txBufferSize; @@ -315,41 +239,6 @@ void uartIrqHandler(uartPort_t *s) } } -#if defined(USE_UART1_TX_DMA) || defined(USE_UART2_TX_DMA) || defined(USE_UART3_TX_DMA) || defined(USE_UART4_TX_DMA) || defined(USE_UART5_TX_DMA) || defined(USE_UART6_TX_DMA) -static void handleUsartTxDma(uartPort_t *s) -{ - DMA_Cmd(s->txDMAStream, DISABLE); - - if (s->port.txBufferHead != s->port.txBufferTail) - uartStartTxDMA(s); - else - s->txDMAEmpty = true; -} - -void dmaIRQHandler(dmaChannelDescriptor_t* descriptor) -{ - uartPort_t *s = &(((uartDevice_t*)(descriptor->userParam))->port); - if (DMA_GET_FLAG_STATUS(descriptor, DMA_IT_TCIF)) - { - DMA_CLEAR_FLAG(descriptor, DMA_IT_TCIF); - DMA_CLEAR_FLAG(descriptor, DMA_IT_HTIF); - if (DMA_GET_FLAG_STATUS(descriptor, DMA_IT_FEIF)) - { - DMA_CLEAR_FLAG(descriptor, DMA_IT_FEIF); - } - handleUsartTxDma(s); - } - if (DMA_GET_FLAG_STATUS(descriptor, DMA_IT_TEIF)) - { - DMA_CLEAR_FLAG(descriptor, DMA_IT_TEIF); - } - if (DMA_GET_FLAG_STATUS(descriptor, DMA_IT_DMEIF)) - { - DMA_CLEAR_FLAG(descriptor, DMA_IT_DMEIF); - } -} -#endif - uartPort_t *serialUART(UARTDevice_e device, uint32_t baudRate, portMode_t mode, portOptions_t options) { uartPort_t *s; @@ -369,15 +258,6 @@ uartPort_t *serialUART(UARTDevice_e device, uint32_t baudRate, portMode_t mode, s->port.txBufferSize = sizeof(uart->txBuffer); s->USARTx = uart->dev; - if (uart->rxDMAStream) { - s->rxDMAChannel = uart->DMAChannel; - s->rxDMAStream = uart->rxDMAStream; - } - s->txDMAChannel = uart->DMAChannel; - s->txDMAStream = uart->txDMAStream; - - s->txDMAPeripheralBaseAddr = (uint32_t)&s->USARTx->DR; - s->rxDMAPeripheralBaseAddr = (uint32_t)&s->USARTx->DR; IO_t tx = IOGetByTag(uart->tx); IO_t rx = IOGetByTag(uart->rx); @@ -410,18 +290,11 @@ uartPort_t *serialUART(UARTDevice_e device, uint32_t baudRate, portMode_t mode, } } -#if defined(USE_UART1_TX_DMA) || defined(USE_UART2_TX_DMA) || defined(USE_UART3_TX_DMA) || defined(USE_UART4_TX_DMA) || defined(USE_UART5_TX_DMA) || defined(USE_UART6_TX_DMA) - // DMA TX Interrupt - dmaSetHandler(uart->txIrq, dmaIRQHandler, uart->txPriority, (uint32_t)uart); -#endif - - if (!(s->rxDMAChannel)) { - NVIC_InitStructure.NVIC_IRQChannel = uart->rxIrq; - NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(uart->rxPriority); - NVIC_InitStructure.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(uart->rxPriority); - NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; - NVIC_Init(&NVIC_InitStructure); - } + NVIC_InitStructure.NVIC_IRQChannel = uart->irq; + NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(uart->irqPriority); + NVIC_InitStructure.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(uart->irqPriority); + NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; + NVIC_Init(&NVIC_InitStructure); return s; } diff --git a/src/main/drivers/serial_uart_stm32f7xx.c b/src/main/drivers/serial_uart_stm32f7xx.c index 71b31820280..3a4674a72ec 100644 --- a/src/main/drivers/serial_uart_stm32f7xx.c +++ b/src/main/drivers/serial_uart_stm32f7xx.c @@ -24,23 +24,17 @@ #include "drivers/io.h" #include "rcc.h" #include "drivers/nvic.h" -#include "dma.h" #include "serial.h" #include "serial_uart.h" #include "serial_uart_impl.h" -static void handleUsartTxDma(uartPort_t *s); - #define UART_RX_BUFFER_SIZE UART1_RX_BUFFER_SIZE #define UART_TX_BUFFER_SIZE UART1_TX_BUFFER_SIZE typedef struct uartDevice_s { USART_TypeDef* dev; uartPort_t port; - uint32_t DMAChannel; - DMA_Stream_TypeDef *txDMAStream; - DMA_Stream_TypeDef *rxDMAStream; ioTag_t rx; ioTag_t tx; volatile uint8_t rxBuffer[UART_RX_BUFFER_SIZE]; @@ -49,23 +43,14 @@ typedef struct uartDevice_s { rccPeriphTag_t rcc_apb2; rccPeriphTag_t rcc_apb1; uint8_t af; - uint8_t txIrq; - uint8_t rxIrq; - uint32_t txPriority; - uint32_t rxPriority; + uint8_t irq; + uint32_t irqPriority; } uartDevice_t; //static uartPort_t uartPort[MAX_UARTS]; #ifdef USE_UART1 static uartDevice_t uart1 = { - .DMAChannel = DMA_CHANNEL_4, -#ifdef USE_UART1_RX_DMA - .rxDMAStream = DMA2_Stream5, -#endif -#ifdef USE_UART1_TX_DMA - .txDMAStream = DMA2_Stream7, -#endif .dev = USART1, .rx = IO_TAG(UART1_RX_PIN), .tx = IO_TAG(UART1_TX_PIN), @@ -74,23 +59,14 @@ static uartDevice_t uart1 = .rcc_ahb1 = UART1_AHB1_PERIPHERALS, #endif .rcc_apb2 = RCC_APB2(USART1), - .txIrq = DMA2_ST7_HANDLER, - .rxIrq = USART1_IRQn, - .txPriority = NVIC_PRIO_SERIALUART1_TXDMA, - .rxPriority = NVIC_PRIO_SERIALUART1 + .irq = USART1_IRQn, + .irqPriority = NVIC_PRIO_SERIALUART1 }; #endif #ifdef USE_UART2 static uartDevice_t uart2 = { - .DMAChannel = DMA_CHANNEL_4, -#ifdef USE_UART2_RX_DMA - .rxDMAStream = DMA1_Stream5, -#endif -#ifdef USE_UART2_TX_DMA - .txDMAStream = DMA1_Stream6, -#endif .dev = USART2, .rx = IO_TAG(UART2_RX_PIN), .tx = IO_TAG(UART2_TX_PIN), @@ -99,23 +75,14 @@ static uartDevice_t uart2 = .rcc_ahb1 = UART2_AHB1_PERIPHERALS, #endif .rcc_apb1 = RCC_APB1(USART2), - .txIrq = DMA1_ST6_HANDLER, - .rxIrq = USART2_IRQn, - .txPriority = NVIC_PRIO_SERIALUART2_TXDMA, - .rxPriority = NVIC_PRIO_SERIALUART2 + .irq = USART2_IRQn, + .irqPriority = NVIC_PRIO_SERIALUART2 }; #endif #ifdef USE_UART3 static uartDevice_t uart3 = { - .DMAChannel = DMA_CHANNEL_4, -#ifdef USE_UART3_RX_DMA - .rxDMAStream = DMA1_Stream1, -#endif -#ifdef USE_UART3_TX_DMA - .txDMAStream = DMA1_Stream3, -#endif .dev = USART3, .rx = IO_TAG(UART3_RX_PIN), .tx = IO_TAG(UART3_TX_PIN), @@ -124,23 +91,14 @@ static uartDevice_t uart3 = .rcc_ahb1 = UART3_AHB1_PERIPHERALS, #endif .rcc_apb1 = RCC_APB1(USART3), - .txIrq = DMA1_ST3_HANDLER, - .rxIrq = USART3_IRQn, - .txPriority = NVIC_PRIO_SERIALUART3_TXDMA, - .rxPriority = NVIC_PRIO_SERIALUART3 + .irq = USART3_IRQn, + .irqPriority = NVIC_PRIO_SERIALUART3 }; #endif #ifdef USE_UART4 static uartDevice_t uart4 = { - .DMAChannel = DMA_CHANNEL_4, -#ifdef USE_UART4_RX_DMA - .rxDMAStream = DMA1_Stream2, -#endif -#ifdef USE_UART4_TX_DMA - .txDMAStream = DMA1_Stream4, -#endif .dev = UART4, .rx = IO_TAG(UART4_RX_PIN), .tx = IO_TAG(UART4_TX_PIN), @@ -149,23 +107,14 @@ static uartDevice_t uart4 = .rcc_ahb1 = UART4_AHB1_PERIPHERALS, #endif .rcc_apb1 = RCC_APB1(UART4), - .txIrq = DMA1_ST4_HANDLER, - .rxIrq = UART4_IRQn, - .txPriority = NVIC_PRIO_SERIALUART4_TXDMA, - .rxPriority = NVIC_PRIO_SERIALUART4 + .irq = UART4_IRQn, + .irqPriority = NVIC_PRIO_SERIALUART4 }; #endif #ifdef USE_UART5 static uartDevice_t uart5 = { - .DMAChannel = DMA_CHANNEL_4, -#ifdef USE_UART5_RX_DMA - .rxDMAStream = DMA1_Stream0, -#endif -#ifdef USE_UART5_TX_DMA - .txDMAStream = DMA1_Stream7, -#endif .dev = UART5, .rx = IO_TAG(UART5_RX_PIN), .tx = IO_TAG(UART5_TX_PIN), @@ -174,23 +123,14 @@ static uartDevice_t uart5 = .rcc_ahb1 = UART5_AHB1_PERIPHERALS, #endif .rcc_apb1 = RCC_APB1(UART5), - .txIrq = DMA1_ST7_HANDLER, - .rxIrq = UART5_IRQn, - .txPriority = NVIC_PRIO_SERIALUART5_TXDMA, - .rxPriority = NVIC_PRIO_SERIALUART5 + .irq = UART5_IRQn, + .irqPriority = NVIC_PRIO_SERIALUART5 }; #endif #ifdef USE_UART6 static uartDevice_t uart6 = { - .DMAChannel = DMA_CHANNEL_5, -#ifdef USE_UART6_RX_DMA - .rxDMAStream = DMA2_Stream1, -#endif -#ifdef USE_UART6_TX_DMA - .txDMAStream = DMA2_Stream6, -#endif .dev = USART6, .rx = IO_TAG(UART6_RX_PIN), .tx = IO_TAG(UART6_TX_PIN), @@ -199,23 +139,14 @@ static uartDevice_t uart6 = .rcc_ahb1 = UART6_AHB1_PERIPHERALS, #endif .rcc_apb2 = RCC_APB2(USART6), - .txIrq = DMA2_ST6_HANDLER, - .rxIrq = USART6_IRQn, - .txPriority = NVIC_PRIO_SERIALUART6_TXDMA, - .rxPriority = NVIC_PRIO_SERIALUART6 + .irq = USART6_IRQn, + .irqPriority = NVIC_PRIO_SERIALUART6 }; #endif #ifdef USE_UART7 static uartDevice_t uart7 = { - .DMAChannel = DMA_CHANNEL_5, -#ifdef USE_UART7_RX_DMA - .rxDMAStream = DMA1_Stream3, -#endif -#ifdef USE_UART7_TX_DMA - .txDMAStream = DMA1_Stream1, -#endif .dev = UART7, .rx = IO_TAG(UART7_RX_PIN), .tx = IO_TAG(UART7_TX_PIN), @@ -224,22 +155,13 @@ static uartDevice_t uart7 = .rcc_ahb1 = UART7_AHB1_PERIPHERALS, #endif .rcc_apb1 = RCC_APB1(UART7), - .txIrq = DMA1_ST1_HANDLER, - .rxIrq = UART7_IRQn, - .txPriority = NVIC_PRIO_SERIALUART7_TXDMA, - .rxPriority = NVIC_PRIO_SERIALUART7 + .irq = UART7_IRQn, + .irqPriority = NVIC_PRIO_SERIALUART7 }; #endif #ifdef USE_UART8 static uartDevice_t uart8 = { - .DMAChannel = DMA_CHANNEL_5, -#ifdef USE_UART8_RX_DMA - .rxDMAStream = DMA1_Stream6, -#endif -#ifdef USE_UART8_TX_DMA - .txDMAStream = DMA1_Stream0, -#endif .dev = UART8, .rx = IO_TAG(UART8_RX_PIN), .tx = IO_TAG(UART8_TX_PIN), @@ -248,10 +170,8 @@ static uartDevice_t uart8 = .rcc_ahb1 = UART8_AHB1_PERIPHERALS, #endif .rcc_apb1 = RCC_APB1(UART8), - .txIrq = DMA1_ST0_HANDLER, - .rxIrq = UART8_IRQn, - .txPriority = NVIC_PRIO_SERIALUART8_TXDMA, - .rxPriority = NVIC_PRIO_SERIALUART8 + .irq = UART8_IRQn, + .irqPriority = NVIC_PRIO_SERIALUART8 }; #endif @@ -342,7 +262,7 @@ void uartIrqHandler(uartPort_t *s) } /* UART in mode Transmitter ------------------------------------------------*/ - if (!s->txDMAStream && (__HAL_UART_GET_IT(huart, UART_IT_TXE) != RESET)) { + if (__HAL_UART_GET_IT(huart, UART_IT_TXE) != RESET) { /* Check that a Tx process is ongoing */ if (huart->gState != HAL_UART_STATE_BUSY_TX) { if (s->port.txBufferTail == s->port.txBufferHead) { @@ -363,28 +283,9 @@ void uartIrqHandler(uartPort_t *s) /* UART in mode Transmitter (transmission end) -----------------------------*/ if ((__HAL_UART_GET_IT(huart, UART_IT_TC) != RESET)) { HAL_UART_IRQHandler(huart); - if (s->txDMAStream) { - handleUsartTxDma(s); - } - } -} - -static void handleUsartTxDma(uartPort_t *s) -{ - if (s->port.txBufferHead != s->port.txBufferTail) - uartStartTxDMA(s); - else - { - s->txDMAEmpty = true; } } -void dmaIRQHandler(dmaChannelDescriptor_t* descriptor) -{ - uartPort_t *s = &(((uartDevice_t*)(descriptor->userParam))->port); - HAL_DMA_IRQHandler(&s->txDMAHandle); -} - uartPort_t *serialUART(UARTDevice_e device, uint32_t baudRate, portMode_t mode, portOptions_t options) { uartPort_t *s; @@ -403,22 +304,6 @@ uartPort_t *serialUART(UARTDevice_e device, uint32_t baudRate, portMode_t mode, s->port.txBufferSize = sizeof(uart->txBuffer); s->USARTx = uart->dev; - if (uart->rxDMAStream) { - s->rxDMAChannel = uart->DMAChannel; - s->rxDMAStream = uart->rxDMAStream; - } - - if (uart->txDMAStream) { - s->txDMAChannel = uart->DMAChannel; - s->txDMAStream = uart->txDMAStream; - - // DMA TX Interrupt - dmaInit(uart->txIrq, OWNER_SERIAL, RESOURCE_INDEX(device)); - dmaSetHandler(uart->txIrq, dmaIRQHandler, uart->txPriority, (uint32_t)uart); - } - - s->txDMAPeripheralBaseAddr = (uint32_t)&s->USARTx->TDR; - s->rxDMAPeripheralBaseAddr = (uint32_t)&s->USARTx->RDR; s->Handle.Instance = uart->dev; @@ -441,10 +326,8 @@ uartPort_t *serialUART(UARTDevice_e device, uint32_t baudRate, portMode_t mode, } } - if (!s->rxDMAChannel) { - HAL_NVIC_SetPriority(uart->rxIrq, NVIC_PRIORITY_BASE(uart->rxPriority), NVIC_PRIORITY_SUB(uart->rxPriority)); - HAL_NVIC_EnableIRQ(uart->rxIrq); - } + HAL_NVIC_SetPriority(uart->irq, NVIC_PRIORITY_BASE(uart->irqPriority), NVIC_PRIORITY_SUB(uart->irqPriority)); + HAL_NVIC_EnableIRQ(uart->irq); return s; } diff --git a/src/main/drivers/temperature/ds18b20.c b/src/main/drivers/temperature/ds18b20.c new file mode 100644 index 00000000000..918ff870f4b --- /dev/null +++ b/src/main/drivers/temperature/ds18b20.c @@ -0,0 +1,148 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include +#include + +#include "platform.h" + +#include "build/debug.h" + +#include "drivers/1-wire.h" +#include "drivers/1-wire/ds2482.h" +#include "drivers/1-wire/ds_crc.h" +#include "drivers/temperature/ds18b20.h" + + +#define DS18B20_FAMILY_CODE 0x28 + +#define DS18B20_ALARM_SEARCH_CMD 0xEC +#define DS18B20_START_CONVERSION_CMD 0x44 +#define DS18B20_WRITE_SCRATCHPAD_CMD 0x4E +#define DS18B20_READ_SCRATCHPAD_CMD 0xBE +#define DS18B20_COPY_SCRATCHPAD_CMD 0x48 +#define DS18B20_RECALL_EEPROM_CMD 0xB8 +#define DS18B20_READ_POWER_SUPPLY_CMD 0xB4 + +#if defined(USE_1WIRE) && defined(USE_TEMPERATURE_DS18B20) + + +bool ds18b20Enumerate(owDev_t *owDev, uint64_t *rom_table, uint8_t *rom_table_len) +{ + return owDev->owSearchRom(owDev, DS18B20_FAMILY_CODE, rom_table, rom_table_len); +} + +bool ds18b20Configure(owDev_t *owDev, uint64_t rom, uint8_t config) +{ + bool ack = owDev->owMatchRom(owDev, rom); + if (!ack) return false; + + uint8_t buf[4] = { DS18B20_WRITE_SCRATCHPAD_CMD, 0, 0, config }; + return owDev->owWriteBuf(owDev, buf, sizeof(buf)); +} + +static bool readPowerSupply(owDev_t *owDev, bool *result) +{ + bool ack = owDev->owWriteByte(owDev, DS18B20_READ_POWER_SUPPLY_CMD); + if (!ack) return false; + + bool sbr; + ack = owDev->owSingleBit(owDev, OW_SINGLE_BIT_WRITE1_READ, &sbr); + if (!ack) return false; + + *result = !sbr; + return true; +} + +bool ds18b20ParasiticPoweredPresent(owDev_t *owDev, bool *result) +{ + bool ack = owDev->owSkipRom(owDev); + if (!ack) return false; + + return readPowerSupply(owDev, result); +} + +bool ds18b20ReadPowerSupply(owDev_t *owDev, uint64_t rom, bool *parasiticPowered) +{ + bool ack = owDev->owMatchRom(owDev, rom); + if (!ack) return false; + + return readPowerSupply(owDev, parasiticPowered); +} + +bool ds18b20ReadScratchpadCommand(owDev_t *owDev) +{ + return owDev->owWriteByteCommand(owDev, DS18B20_READ_SCRATCHPAD_CMD); +} + +static bool ds18b20ReadScratchpadBuf(owDev_t *owDev, uint8_t *buf, uint8_t len) +{ + bool ack = ds18b20ReadScratchpadCommand(owDev); + if (!ack) return false; + ack = owDev->waitForBus(owDev); + if (!ack) return false; + return owDev->owReadBuf(owDev, buf, len); +} + +bool ds18b20StartConversionCommand(owDev_t *owDev) +{ + return owDev->owWriteByteCommand(owDev, DS18B20_START_CONVERSION_CMD); +} + +// start conversion on all devices present on the bus +// note: parasitic power only supports one device converting at a time +bool ds18b20StartConversion(owDev_t *owDev) +{ + bool ack = owDev->owSkipRom(owDev); + if (!ack) return false; + return ds18b20StartConversionCommand(owDev); +} + +bool ds18b20WaitForConversion(owDev_t *owDev) +{ + bool ack = owDev->waitForBus(owDev); + if (!ack) return false; + + bool read_bit; + do { + ack = owDev->owSingleBit(owDev, OW_SINGLE_BIT_WRITE1_READ, &read_bit); + if (!ack) return false; + } while (!read_bit); + + return true; +} + +bool ds18b20ReadTemperatureFromScratchPadBuf(const uint8_t *buf, int16_t *temperature) +{ + if (buf[8] != ds_crc8(buf, 8)) return false; + *temperature = (int16_t)(((buf[0] | (buf[1] << 8)) >> 3) | ((buf[1] & 0x80) ? 0xE000 : 0)) * 5; + return true; +} + +bool ds18b20ReadTemperature(owDev_t *owDev, uint64_t rom, int16_t *temperature) +{ + bool ack = owDev->owMatchRom(owDev, rom); + if (!ack) return false; + + uint8_t buf[9]; + ack = ds18b20ReadScratchpadBuf(owDev, buf, 9); + if (!ack) return false; + + return ds18b20ReadTemperatureFromScratchPadBuf(buf, temperature); +} + +#endif /* defined(USE_1WIRE) && defined(USE_TEMPERATURE_DS18B20) */ diff --git a/src/main/drivers/temperature/ds18b20.h b/src/main/drivers/temperature/ds18b20.h new file mode 100644 index 00000000000..aaecf160d03 --- /dev/null +++ b/src/main/drivers/temperature/ds18b20.h @@ -0,0 +1,36 @@ + +#pragma once + +#include +#include +#include "drivers/1-wire.h" + +#if defined(USE_1WIRE) && defined(USE_TEMPERATURE_DS18B20) + +#define USE_TEMPERATURE_SENSOR +#define DS18B20_DRIVER_AVAILABLE + +#define DS18B20_CONFIG_9BIT 0x1F +#define DS18B20_CONFIG_10BIT 0x3F +#define DS18B20_CONFIG_11BIT 0x5F +#define DS18B20_CONFIG_12BIT 0x7F + +// milliseconds +#define DS18B20_9BIT_CONVERSION_TIME 94 +#define DS18B20_10BIT_CONVERSION_TIME 188 +#define DS18B20_11BIT_CONVERSION_TIME 375 +#define DS18B20_12BIT_CONVERSION_TIME 750 + + +bool ds18b20Enumerate(owDev_t *owDev, uint64_t *rom_table, uint8_t *rom_table_len); +bool ds18b20Configure(owDev_t *owDev, uint64_t rom, uint8_t config); +bool ds18b20ParasiticPoweredPresent(owDev_t *owDev, bool *result); +bool ds18b20ReadPowerSupply(owDev_t *owDev, uint64_t rom, bool *parasiticPowered); +bool ds18b20StartConversionCommand(owDev_t *owDev); +bool ds18b20StartConversion(owDev_t *owDev); +bool ds18b20WaitForConversion(owDev_t *owDev); +bool ds18b20ReadScratchpadCommand(owDev_t *owDev); +bool ds18b20ReadTemperatureFromScratchPadBuf(const uint8_t *buf, int16_t *temperature); +bool ds18b20ReadTemperature(owDev_t *owDev, uint64_t rom, int16_t *temperature); + +#endif /* defined(USE_1WIRE) && defined(USE_TEMPERATURE_DS18B20) */ diff --git a/src/main/drivers/temperature/lm75.c b/src/main/drivers/temperature/lm75.c new file mode 100644 index 00000000000..98275e411a4 --- /dev/null +++ b/src/main/drivers/temperature/lm75.c @@ -0,0 +1,81 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include +#include + +#include + +#include "platform.h" + +#include "build/debug.h" + +#include "drivers/sensor.h" +#include "drivers/temperature/lm75.h" +#include "drivers/temperature/temperature.h" +#include "drivers/time.h" + +#define LM75_TEMPERATURE_REG_ADDR 0x0 + + +#ifdef USE_TEMPERATURE_LM75 + +static bool lm75Read(temperatureDev_t *tempDev, int16_t *temperature) +{ + uint8_t buf[2]; + + bool ack = busReadBuf(tempDev->busDev, LM75_TEMPERATURE_REG_ADDR, buf, 2); + + if (ack) { + if (temperature) *temperature = (int8_t)buf[0] * 10 + (buf[1] >> 7) * 5; + return true; + } + + return false; +} + +#define DETECTION_MAX_RETRY_COUNT 5 +static bool deviceDetect(temperatureDev_t *tempDev) +{ + for (int retryCount = 0; retryCount < DETECTION_MAX_RETRY_COUNT; retryCount++) { + delay(10); + if (lm75Read(tempDev, NULL)) return true; + } + + return false; +} + +bool lm75Detect(temperatureDev_t *tempDev, uint8_t partialAddress) +{ + if (partialAddress > 7) return false; // invalid address + + tempDev->busDev = busDeviceInit(BUSTYPE_I2C, DEVHW_LM75_0 + partialAddress, 0, OWNER_TEMPERATURE); + if (tempDev->busDev == NULL) { + return false; + } + + if (!deviceDetect(tempDev)) { + busDeviceDeInit(tempDev->busDev); + return false; + } + + tempDev->read = lm75Read; + + return true; +} + +#endif /* USE_TEMPERATURE_LM75 */ diff --git a/src/main/drivers/temperature/lm75.h b/src/main/drivers/temperature/lm75.h new file mode 100644 index 00000000000..2fc4e9f9362 --- /dev/null +++ b/src/main/drivers/temperature/lm75.h @@ -0,0 +1,29 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once + +#include "drivers/io_types.h" +#include "drivers/temperature/temperature.h" + +#ifdef USE_TEMPERATURE_LM75 + +#define USE_TEMPERATURE_SENSOR + +bool lm75Detect(temperatureDev_t *tempDev, uint8_t partial_address); + +#endif /* USE_TEMPERATURE_LM75 */ diff --git a/src/main/drivers/temperature/temperature.h b/src/main/drivers/temperature/temperature.h new file mode 100644 index 00000000000..f10001a687c --- /dev/null +++ b/src/main/drivers/temperature/temperature.h @@ -0,0 +1,8 @@ +#pragma once + +#include "drivers/sensor.h" + +typedef struct temperatureDev_s { + busDevice_t *busDev; + sensorTempReadFuncPtr read; +} temperatureDev_t; diff --git a/src/main/drivers/timer.c b/src/main/drivers/timer.c index 06c1c0be435..fcb8c9430c8 100755 --- a/src/main/drivers/timer.c +++ b/src/main/drivers/timer.c @@ -23,6 +23,7 @@ #include "platform.h" #include "build/atomic.h" +#include "build/debug.h" #include "common/utils.h" #include "common/memory.h" @@ -35,17 +36,7 @@ #include "drivers/timer.h" #include "drivers/timer_impl.h" -#define TIM_N(n) (1 << (n)) - -/* - Groups that allow running different period (ex 50Hz servos + 400Hz throttle + etc): - TIM1 2 channels - TIM2 4 channels - TIM3 4 channels - TIM4 4 channels -*/ - -timerConfig_t * timerConfig[HARDWARE_TIMER_DEFINITION_COUNT]; +timHardwareContext_t * timerCtx[HARDWARE_TIMER_DEFINITION_COUNT]; // return index of timer in timer table. Lowest timer has index 0 uint8_t lookupTimerIndex(const TIM_TypeDef *tim) @@ -63,127 +54,114 @@ uint8_t lookupTimerIndex(const TIM_TypeDef *tim) return ~1; } -static inline uint8_t lookupChannelIndex(const uint16_t channel) -{ - return channel >> 2; -} - -void timerNVICConfigure(uint8_t irq, int irqPriority) +void timerConfigBase(TCH_t * tch, uint16_t period, uint32_t hz) { - impl_timerNVICConfigure(irq, irqPriority); -} + if (tch == NULL) { + return; + } -void timerConfigBase(TIM_TypeDef *tim, uint16_t period, uint8_t mhz) -{ - impl_timerConfigBase(tim, period, mhz); + impl_timerConfigBase(tch, period, hz); } // old interface for PWM inputs. It should be replaced -void timerConfigure(const timerHardware_t *timerHardwarePtr, uint16_t period, uint8_t mhz) +void timerConfigure(TCH_t * tch, uint16_t period, uint32_t hz) { - unsigned timer = lookupTimerIndex(timerHardwarePtr->tim); - - impl_timerConfigBase(timerDefinitions[timer].tim, period, mhz); - impl_enableTimer(timerDefinitions[timer].tim); - - if (timerDefinitions[timer].irq != 0) { - impl_timerNVICConfigure(timerDefinitions[timer].irq, NVIC_PRIO_TIMER); + if (tch == NULL) { + return; } - if (timerDefinitions[timer].secondIrq != 0) { - impl_timerNVICConfigure(timerDefinitions[timer].secondIrq, NVIC_PRIO_TIMER); - } + impl_timerConfigBase(tch, period, hz); + impl_timerNVICConfigure(tch, NVIC_PRIO_TIMER); + impl_enableTimer(tch); } -void timerChCCHandlerInit(timerCCHandlerRec_t *self, timerCCHandlerCallback *fn) +TCH_t * timerGetTCH(const timerHardware_t * timHw) { - self->fn = fn; -} + const int timerIndex = lookupTimerIndex(timHw->tim); + + if (timerIndex >= HARDWARE_TIMER_DEFINITION_COUNT) { + DEBUG_TRACE("Can't find hardware timer definition"); + return NULL; + } -void timerChOvrHandlerInit(timerOvrHandlerRec_t *self, timerOvrHandlerCallback *fn) -{ - self->fn = fn; - self->next = NULL; -} + // If timer context does not exist - allocate memory + if (timerCtx[timerIndex] == NULL) { + timerCtx[timerIndex] = memAllocate(sizeof(timHardwareContext_t), OWNER_TIMER); + + // Check for OOM + if (timerCtx[timerIndex] == NULL) { + DEBUG_TRACE("Can't allocate TCH object"); + return NULL; + } -// update overflow callback list -// some synchronization mechanism is neccesary to avoid disturbing other channels (BASEPRI used now) -static void timerChConfig_UpdateOverflow(timerConfig_t *cfg, TIM_TypeDef *tim) { - timerOvrHandlerRec_t **chain = &cfg->overflowCallbackActive; - ATOMIC_BLOCK(NVIC_PRIO_TIMER) { - for (int i = 0; i < CC_CHANNELS_PER_TIMER; i++) - if (cfg->overflowCallback[i]) { - *chain = cfg->overflowCallback[i]; - chain = &cfg->overflowCallback[i]->next; - } - *chain = NULL; - } + // Initialize parent object + memset(timerCtx[timerIndex], 0, sizeof(timHardwareContext_t)); + timerCtx[timerIndex]->timDef = &timerDefinitions[timerIndex]; + timerCtx[timerIndex]->ch[0].timCtx = timerCtx[timerIndex]; + timerCtx[timerIndex]->ch[1].timCtx = timerCtx[timerIndex]; + timerCtx[timerIndex]->ch[2].timCtx = timerCtx[timerIndex]; + timerCtx[timerIndex]->ch[3].timCtx = timerCtx[timerIndex]; - // enable or disable IRQ - if (cfg->overflowCallbackActive) { - impl_timerEnableIT(tim, IMPL_TIM_IT_UPDATE_INTERRUPT); + // Implementation-specific init + impl_timerInitContext(timerCtx[timerIndex]); } - else { - impl_timerDisableIT(tim, IMPL_TIM_IT_UPDATE_INTERRUPT); - } -} -timerConfig_t * timerGetConfigContext(int timerIndex) -{ - // If timer context does not exist - allocate memory - if (timerConfig[timerIndex] == NULL) { - timerConfig[timerIndex] = memAllocate(sizeof(timerConfig_t), OWNER_TIMER); - } + // Initialize timer channel object + timerCtx[timerIndex]->ch[timHw->channelIndex].timHw = timHw; + timerCtx[timerIndex]->ch[timHw->channelIndex].dma = NULL; + timerCtx[timerIndex]->ch[timHw->channelIndex].cb = NULL; + timerCtx[timerIndex]->ch[timHw->channelIndex].dmaState = TCH_DMA_IDLE; - return timerConfig[timerIndex]; + return &timerCtx[timerIndex]->ch[timHw->channelIndex]; } // config edge and overflow callback for channel. Try to avoid overflowCallback, it is a bit expensive -void timerChConfigCallbacks(const timerHardware_t *timHw, timerCCHandlerRec_t *edgeCallback, timerOvrHandlerRec_t *overflowCallback) +void timerChInitCallbacks(timerCallbacks_t * cb, void * callbackParam, timerCallbackFn * edgeCallback, timerCallbackFn * overflowCallback) { - uint8_t timerIndex = lookupTimerIndex(timHw->tim); + cb->callbackParam = callbackParam; + cb->callbackEdge = edgeCallback; + cb->callbackOvr = overflowCallback; +} - if (timerIndex >= HARDWARE_TIMER_DEFINITION_COUNT) { +void timerChConfigCallbacks(TCH_t * tch, timerCallbacks_t * cb) +{ + if (tch == NULL) { return; } - uint8_t channelIndex = lookupChannelIndex(timHw->channel); - if (edgeCallback == NULL) // disable irq before changing callback to NULL - impl_timerDisableIT(timHw->tim, TIM_IT_CCx(timHw->channel)); - - timerConfig_t * cfg = timerGetConfigContext(timerIndex); - - if (cfg) { - // setup callback info - cfg->edgeCallback[channelIndex] = edgeCallback; - cfg->overflowCallback[channelIndex] = overflowCallback; + if (cb->callbackEdge == NULL) { + impl_timerDisableIT(tch, TIM_IT_CCx(tch->timHw->channelIndex)); + } + + if (cb->callbackOvr == NULL) { + impl_timerDisableIT(tch, IMPL_TIM_IT_UPDATE_INTERRUPT); + } - // enable channel IRQ - if (edgeCallback) - impl_timerEnableIT(timHw->tim, TIM_IT_CCx(timHw->channel)); + tch->cb = cb; - timerChConfig_UpdateOverflow(cfg, timHw->tim); + if (cb->callbackEdge) { + impl_timerEnableIT(tch, TIM_IT_CCx(tch->timHw->channelIndex)); } - else { - // OOM error, disable IRQs for this timer - impl_timerDisableIT(timHw->tim, TIM_IT_CCx(timHw->channel)); + + if (cb->callbackOvr) { + impl_timerEnableIT(tch, IMPL_TIM_IT_UPDATE_INTERRUPT); } } // Configure input captupre -void timerChConfigIC(const timerHardware_t *timHw, bool polarityRising, unsigned inputFilterTicks) +void timerChConfigIC(TCH_t * tch, bool polarityRising, unsigned inputFilterSamples) { - impl_timerChConfigIC(timHw, polarityRising, inputFilterTicks); + impl_timerChConfigIC(tch, polarityRising, inputFilterSamples); } -uint16_t timerGetPeriod(const timerHardware_t *timHw) +uint16_t timerGetPeriod(TCH_t * tch) { - return timHw->tim->ARR; + return tch->timHw->tim->ARR; } void timerInit(void) { - memset(timerConfig, 0, sizeof (timerConfig)); + memset(timerCtx, 0, sizeof (timerCtx)); /* enable the timer peripherals */ for (int i = 0; i < timerHardwareCount; i++) { @@ -199,7 +177,7 @@ void timerInit(void) } } -const timerHardware_t *timerGetByTag(ioTag_t tag, timerUsageFlag_e flag) +const timerHardware_t * timerGetByTag(ioTag_t tag, timerUsageFlag_e flag) { if (!tag) { return NULL; @@ -215,36 +193,67 @@ const timerHardware_t *timerGetByTag(ioTag_t tag, timerUsageFlag_e flag) return NULL; } -void timerPWMConfigChannel(TIM_TypeDef * tim, uint8_t channel, bool isNChannel, bool inverted, uint16_t value) +void timerPWMConfigChannel(TCH_t * tch, uint16_t value) { - impl_timerPWMConfigChannel(tim, channel, isNChannel, inverted, value); + impl_timerPWMConfigChannel(tch, value); } -void timerEnable(TIM_TypeDef * tim) +void timerEnable(TCH_t * tch) { - impl_enableTimer(tim); + impl_enableTimer(tch); } -void timerPWMStart(TIM_TypeDef * tim, uint8_t channel, bool isNChannel) +void timerPWMStart(TCH_t * tch) { - impl_timerPWMStart(tim, channel, isNChannel); + impl_timerPWMStart(tch); } -uint16_t timerDmaSource(uint8_t channel) +volatile timCCR_t *timerCCR(TCH_t * tch) { - return impl_timerDmaSource(channel); + return impl_timerCCR(tch); } -volatile timCCR_t * timerCCR(TIM_TypeDef *tim, uint8_t channel) +void timerChCaptureEnable(TCH_t * tch) { - return impl_timerCCR(tim, channel); + impl_timerChCaptureCompareEnable(tch, true); } -uint16_t timerGetPrescalerByDesiredMhz(TIM_TypeDef *tim, uint16_t mhz) +void timerChCaptureDisable(TCH_t * tch) { - // protection here for desired MHZ > SystemCoreClock??? - if ((uint32_t)(mhz * 1000000) > (SystemCoreClock / timerClockDivisor(tim))) { - return 0; - } - return (uint16_t)(round((SystemCoreClock / timerClockDivisor(tim) / (mhz * 1000000)) - 1)); + impl_timerChCaptureCompareEnable(tch, false); +} + +uint32_t timerGetBaseClock(TCH_t * tch) +{ + return timerGetBaseClockHW(tch->timHw); +} + +uint32_t timerGetBaseClockHW(const timerHardware_t * timHw) +{ + return SystemCoreClock / timerClockDivisor(timHw->tim); } + +bool timerPWMConfigChannelDMA(TCH_t * tch, void * dmaBuffer, uint32_t dmaBufferSize) +{ + return impl_timerPWMConfigChannelDMA(tch, dmaBuffer, dmaBufferSize); +} + +void timerPWMPrepareDMA(TCH_t * tch, uint32_t dmaBufferSize) +{ + impl_timerPWMPrepareDMA(tch, dmaBufferSize); +} + +void timerPWMStartDMA(TCH_t * tch) +{ + impl_timerPWMStartDMA(tch); +} + +void timerPWMStopDMA(TCH_t * tch) +{ + impl_timerPWMStopDMA(tch); +} + +bool timerPWMDMAInProgress(TCH_t * tch) +{ + return tch->dmaState != TCH_DMA_IDLE; +} \ No newline at end of file diff --git a/src/main/drivers/timer.h b/src/main/drivers/timer.h index c762bc907e8..2fe3c39e3f8 100644 --- a/src/main/drivers/timer.h +++ b/src/main/drivers/timer.h @@ -21,7 +21,11 @@ #include #include "drivers/io_types.h" -#include "rcc_types.h" +#include "drivers/dma.h" +#include "drivers/rcc_types.h" +#include "drivers/timer_def.h" + +#define CC_CHANNELS_PER_TIMER 4 // TIM_Channel_1..4 typedef uint16_t captureCompare_t; // 16 bit on both 103 and 303, just register access must be 32bit sometimes (use timCCR_t) @@ -49,22 +53,6 @@ typedef uint32_t timCNT_t; #error "Unknown CPU defined" #endif - -// use different types from capture and overflow - multiple overflow handlers are implemented as linked list -struct timerCCHandlerRec_s; -struct timerOvrHandlerRec_s; -typedef void timerCCHandlerCallback(struct timerCCHandlerRec_s* self, uint16_t capture); -typedef void timerOvrHandlerCallback(struct timerOvrHandlerRec_s* self, uint16_t capture); - -typedef struct timerCCHandlerRec_s { - timerCCHandlerCallback* fn; -} timerCCHandlerRec_t; - -typedef struct timerOvrHandlerRec_s { - timerOvrHandlerCallback* fn; - struct timerOvrHandlerRec_s* next; -} timerOvrHandlerRec_t; - typedef struct timerDef_s { TIM_TypeDef * tim; rccPeriphTag_t rcc; @@ -85,24 +73,62 @@ typedef enum { TIM_USE_BEEPER = (1 << 25), } timerUsageFlag_e; +// TCH hardware definition (listed in target.c) typedef struct timerHardware_s { TIM_TypeDef *tim; ioTag_t tag; - uint8_t channel; + uint8_t channelIndex; uint8_t output; ioConfig_t ioMode; -#if defined(STM32F3) || defined(STM32F4) || defined(STM32F7) uint8_t alternateFunction; -#endif uint32_t usageFlags; + dmaTag_t dmaTag; } timerHardware_t; enum { - TIMER_OUTPUT_ENABLED = 0x01, + TIMER_OUTPUT_NONE = 0x00, TIMER_OUTPUT_INVERTED = 0x02, TIMER_OUTPUT_N_CHANNEL= 0x04 }; +typedef enum { + TCH_DMA_IDLE = 0, + TCH_DMA_READY, + TCH_DMA_ACTIVE, +} tchDmaState_e; + +// Some forward declarations for types +struct TCH_s; +struct timHardwareContext_s; + +// Timer generic callback +typedef void timerCallbackFn(struct TCH_s * tch, uint32_t value); + +typedef struct timerCallbacks_s { + void * callbackParam; + timerCallbackFn * callbackEdge; + timerCallbackFn * callbackOvr; +} timerCallbacks_t; + +// Run-time TCH (Timer CHannel) context +typedef struct TCH_s { + struct timHardwareContext_s * timCtx; // Run-time initialized to parent timer + const timerHardware_t * timHw; // Link to timerHardware_t definition (target-specific) + const timerCallbacks_t * cb; + DMA_t dma; // Timer channel DMA handle + volatile tchDmaState_e dmaState; + void * dmaBuffer; +} TCH_t; + +// Run-time timer context (dynamically allocated), includes 4x TCH +typedef struct timHardwareContext_s { + const timerDef_t * timDef; +#ifdef USE_HAL_DRIVER + TIM_HandleTypeDef * timHandle; +#endif + TCH_t ch[CC_CHANNELS_PER_TIMER]; +} timHardwareContext_t; + #if defined(STM32F3) #define HARDWARE_TIMER_DEFINITION_COUNT 17 #elif defined(STM32F4) @@ -113,9 +139,13 @@ enum { #error "Unknown CPU defined" #endif +// Per MCU timer definitions +extern timHardwareContext_t * timerCtx[HARDWARE_TIMER_DEFINITION_COUNT]; +extern const timerDef_t timerDefinitions[HARDWARE_TIMER_DEFINITION_COUNT]; + +// Per target timer output definitions extern const timerHardware_t timerHardware[]; extern const int timerHardwareCount; -extern const timerDef_t timerDefinitions[]; typedef enum { TYPE_FREE, @@ -135,35 +165,37 @@ typedef enum { TYPE_TIMER } channelType_t; -const timerHardware_t *timerGetByTag(ioTag_t tag, timerUsageFlag_e flag); +uint8_t timerClockDivisor(TIM_TypeDef *tim); +uint32_t timerGetBaseClockHW(const timerHardware_t * timHw); -void timerConfigure(const timerHardware_t *timHw, uint16_t period, uint8_t mhz); // This interface should be replaced. +const timerHardware_t * timerGetByTag(ioTag_t tag, timerUsageFlag_e flag); +TCH_t * timerGetTCH(const timerHardware_t * timHw); -void timerChConfigIC(const timerHardware_t *timHw, bool polarityRising, unsigned inputFilterSamples); +uint32_t timerGetBaseClock(TCH_t * tch); +void timerConfigure(TCH_t * tch, uint16_t period, uint32_t hz); // This interface should be replaced. -void timerChCCHandlerInit(timerCCHandlerRec_t *self, timerCCHandlerCallback *fn); -void timerChOvrHandlerInit(timerOvrHandlerRec_t *self, timerOvrHandlerCallback *fn); -void timerChConfigCallbacks(const timerHardware_t *channel, timerCCHandlerRec_t *edgeCallback, timerOvrHandlerRec_t *overflowCallback); +void timerChInitCallbacks(timerCallbacks_t * cb, void * callbackParam, timerCallbackFn * edgeCallback, timerCallbackFn * overflowCallback); +void timerChConfigIC(TCH_t * tch, bool polarityRising, unsigned inputFilterSamples); +void timerChConfigCallbacks(TCH_t * tch, timerCallbacks_t * cb); +void timerChCaptureEnable(TCH_t * tch); +void timerChCaptureDisable(TCH_t * tch); void timerInit(void); void timerStart(void); -uint8_t timerClockDivisor(TIM_TypeDef *tim); -uint32_t timerClock(TIM_TypeDef *tim); +void timerConfigBase(TCH_t * tch, uint16_t period, uint32_t hz); // TODO - just for migration +uint16_t timerGetPeriod(TCH_t * tch); -void timerConfigBase(TIM_TypeDef *tim, uint16_t period, uint8_t mhz); // TODO - just for migration - -uint16_t timerGetPeriod(const timerHardware_t *timHw); - -#if defined(USE_HAL_DRIVER) -TIM_HandleTypeDef * timerFindTimerHandle(TIM_TypeDef *tim); -#endif +void timerEnable(TCH_t * tch); +void timerPWMConfigChannel(TCH_t * tch, uint16_t value); +void timerPWMStart(TCH_t * tch); -void timerEnable(TIM_TypeDef * tim); -void timerPWMConfigChannel(TIM_TypeDef * tim, uint8_t channel, bool isNChannel, bool inverted, uint16_t value); -void timerPWMStart(TIM_TypeDef * tim, uint8_t channel, bool isNChannel); +bool timerPWMConfigChannelDMA(TCH_t * tch, void * dmaBuffer, uint32_t dmaBufferSize); +void timerPWMPrepareDMA(TCH_t * tch, uint32_t dmaBufferSize); +void timerPWMStartDMA(TCH_t * tch); +void timerPWMStopDMA(TCH_t * tch); +bool timerPWMDMAInProgress(TCH_t * tch); -volatile timCCR_t *timerCCR(TIM_TypeDef *tim, uint8_t channel); -uint16_t timerDmaSource(uint8_t channel); +volatile timCCR_t *timerCCR(TCH_t * tch); uint16_t timerGetPrescalerByDesiredMhz(TIM_TypeDef *tim, uint16_t mhz); diff --git a/src/main/drivers/timer_def.h b/src/main/drivers/timer_def.h new file mode 100644 index 00000000000..28c1fe2ec49 --- /dev/null +++ b/src/main/drivers/timer_def.h @@ -0,0 +1,75 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + */ + +#pragma once + +#include "drivers/dma.h" + +// Macros expand to keep DMA descriptor table compatible with Betaflight +#define DEF_TIM_DMAMAP(variant, timch) CONCAT(DEF_TIM_DMAMAP__, PP_CALL(CONCAT(DEF_TIM_DMAMAP_VARIANT__, variant), CONCAT(DEF_TIM_DMA__, DEF_TIM_TCH2BTCH(timch)), DMA_VARIANT_MISSING, DMA_VARIANT_MISSING)) +#define DEF_TIM_DMAMAP_VARIANT__0(_0, ...) _0 +#define DEF_TIM_DMAMAP_VARIANT__1(_0, _1, ...) _1 +#define DEF_TIM_DMAMAP_VARIANT__2(_0, _1, _2, ...) _2 + +// Timer channel indexes +#define DEF_TIM_CHNL_CH1 0 +#define DEF_TIM_CHNL_CH1N 0 +#define DEF_TIM_CHNL_CH2 1 +#define DEF_TIM_CHNL_CH2N 1 +#define DEF_TIM_CHNL_CH3 2 +#define DEF_TIM_CHNL_CH3N 2 +#define DEF_TIM_CHNL_CH4 3 +#define DEF_TIM_CHNL_CH4N 3 + +// map to base channel (strip N from channel); works only when channel N exists +#define DEF_TIM_TCH2BTCH(timch) CONCAT(BTCH_, timch) +#define BTCH_TIM1_CH1N BTCH_TIM1_CH1 +#define BTCH_TIM1_CH2N BTCH_TIM1_CH2 +#define BTCH_TIM1_CH3N BTCH_TIM1_CH3 + +#define BTCH_TIM8_CH1N BTCH_TIM8_CH1 +#define BTCH_TIM8_CH2N BTCH_TIM8_CH2 +#define BTCH_TIM8_CH3N BTCH_TIM8_CH3 + +#define BTCH_TIM20_CH1N BTCH_TIM20_CH1 +#define BTCH_TIM20_CH2N BTCH_TIM20_CH2 +#define BTCH_TIM20_CH3N BTCH_TIM20_CH3 + +#define BTCH_TIM15_CH1N BTCH_TIM15_CH1 +#define BTCH_TIM16_CH1N BTCH_TIM16_CH1 + +// Default output flags +#define DEF_TIM_OUTPUT(ch) DEF_TIM_OUTPUT__ ## ch +#define DEF_TIM_OUTPUT__CH1 (TIMER_OUTPUT_NONE) +#define DEF_TIM_OUTPUT__CH2 (TIMER_OUTPUT_NONE) +#define DEF_TIM_OUTPUT__CH3 (TIMER_OUTPUT_NONE) +#define DEF_TIM_OUTPUT__CH4 (TIMER_OUTPUT_NONE) +#define DEF_TIM_OUTPUT__CH1N (TIMER_OUTPUT_N_CHANNEL) +#define DEF_TIM_OUTPUT__CH2N (TIMER_OUTPUT_N_CHANNEL) +#define DEF_TIM_OUTPUT__CH3N (TIMER_OUTPUT_N_CHANNEL) +#define DEF_TIM_OUTPUT__CH4N (TIMER_OUTPUT_N_CHANNEL) + +#if defined(STM32F3) + #include "timer_def_stm32f3xx.h" +#elif defined(STM32F4) + #include "timer_def_stm32f4xx.h" +#elif defined(STM32F7) + #include "timer_def_stm32f7xx.h" +#else + #error "Unknown CPU defined" +#endif + diff --git a/src/main/drivers/timer_def_stm32f3xx.h b/src/main/drivers/timer_def_stm32f3xx.h new file mode 100644 index 00000000000..3c5e7469ad8 --- /dev/null +++ b/src/main/drivers/timer_def_stm32f3xx.h @@ -0,0 +1,189 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + */ + +#pragma once + +#define DEF_TIM_DMAMAP__D(dma, channel) DMA_TAG(dma, 0, channel) +#define DEF_TIM_DMAMAP__NONE DMA_NONE + +#define DEF_TIM(tim, ch, pin, usage, flags) { tim, IO_TAG(pin), DEF_TIM_CHNL_##ch, DEF_TIM_OUTPUT(ch) | flags, IOCFG_AF_PP, DEF_TIM_AF(TCH_## tim ## _ ## ch, pin), usage, DEF_TIM_DMAMAP(0, tim ## _ ## ch) } + +// AF mappings +#define DEF_TIM_AF(timch, pin) CONCAT(DEF_TIM_AF__, DEF_TIM_AF__ ## pin ## __ ## timch) +#define DEF_TIM_AF__D(af_n) GPIO_AF_ ## af_n + +/* add the DMA mappings here */ +// D(dma_n, channel_n) + +#define DEF_TIM_DMA__BTCH_TIM1_CH1 D(1, 2) +#define DEF_TIM_DMA__BTCH_TIM1_CH2 D(1, 3) +#define DEF_TIM_DMA__BTCH_TIM1_CH4 D(1, 4) +#define DEF_TIM_DMA__BTCH_TIM1_TRIG D(1, 4) +#define DEF_TIM_DMA__BTCH_TIM1_COM D(1, 4) +#define DEF_TIM_DMA__BTCH_TIM1_CH3 D(1, 6) + +#define DEF_TIM_DMA__BTCH_TIM2_CH3 D(1, 1) +#define DEF_TIM_DMA__BTCH_TIM2_CH1 D(1, 5) +#define DEF_TIM_DMA__BTCH_TIM2_CH2 D(1, 7) +#define DEF_TIM_DMA__BTCH_TIM2_CH4 D(1, 7) + +#define DEF_TIM_DMA__BTCH_TIM3_CH2 NONE +#define DEF_TIM_DMA__BTCH_TIM3_CH3 D(1, 2) +#define DEF_TIM_DMA__BTCH_TIM3_CH4 D(1, 3) +#define DEF_TIM_DMA__BTCH_TIM3_CH1 D(1, 6) +#define DEF_TIM_DMA__BTCH_TIM3_TRIG D(1, 6) + +#define DEF_TIM_DMA__BTCH_TIM4_CH1 D(1, 1) +#define DEF_TIM_DMA__BTCH_TIM4_CH2 D(1, 4) +#define DEF_TIM_DMA__BTCH_TIM4_CH3 D(1, 5) +#define DEF_TIM_DMA__BTCH_TIM4_CH4 NONE + +#define DEF_TIM_DMA__BTCH_TIM15_CH1 D(1, 5) +#define DEF_TIM_DMA__BTCH_TIM15_CH2 NONE +#define DEF_TIM_DMA__BTCH_TIM15_UP D(1, 5) +#define DEF_TIM_DMA__BTCH_TIM15_TRIG D(1, 5) +#define DEF_TIM_DMA__BTCH_TIM15_COM D(1, 5) + +// #ifdef REMAP_TIM16_DMA +// #define DEF_TIM_DMA__BTCH_TIM16_CH1 D(1, 6) +// #else +#define DEF_TIM_DMA__BTCH_TIM16_CH1 D(1, 3) +// #endif + +// #ifdef REMAP_TIM17_DMA +// #define DEF_TIM_DMA__BTCH_TIM17_CH1 D(1, 7) +// #else +#define DEF_TIM_DMA__BTCH_TIM17_CH1 D(1, 1) +// #endif + +#define DEF_TIM_DMA__BTCH_TIM8_CH3 D(2, 1) +#define DEF_TIM_DMA__BTCH_TIM8_CH4 D(2, 2) +#define DEF_TIM_DMA__BTCH_TIM8_TRIG D(2, 2) +#define DEF_TIM_DMA__BTCH_TIM8_COM D(2, 2) +#define DEF_TIM_DMA__BTCH_TIM8_CH1 D(2, 3) +#define DEF_TIM_DMA__BTCH_TIM8_CH2 D(2, 5) + +// AF table + +#define DEF_TIM_AF__PA0__TCH_TIM2_CH1 D(1) +#define DEF_TIM_AF__PA1__TCH_TIM2_CH2 D(1) +#define DEF_TIM_AF__PA2__TCH_TIM2_CH3 D(1) +#define DEF_TIM_AF__PA3__TCH_TIM2_CH4 D(1) +#define DEF_TIM_AF__PA5__TCH_TIM2_CH1 D(1) +#define DEF_TIM_AF__PA6__TCH_TIM16_CH1 D(1) +#define DEF_TIM_AF__PA7__TCH_TIM17_CH1 D(1) +#define DEF_TIM_AF__PA12__TCH_TIM16_CH1 D(1) +#define DEF_TIM_AF__PA13__TCH_TIM16_CH1N D(1) +#define DEF_TIM_AF__PA15__TCH_TIM2_CH1 D(1) + +#define DEF_TIM_AF__PA4__TCH_TIM3_CH2 D(2) +#define DEF_TIM_AF__PA6__TCH_TIM3_CH1 D(2) +#define DEF_TIM_AF__PA7__TCH_TIM3_CH2 D(2) +#define DEF_TIM_AF__PA15__TCH_TIM8_CH1 D(2) + +#define DEF_TIM_AF__PA7__TCH_TIM8_CH1N D(4) + +#define DEF_TIM_AF__PA14__TCH_TIM8_CH2 D(5) + +#define DEF_TIM_AF__PA7__TCH_TIM1_CH1N D(6) +#define DEF_TIM_AF__PA8__TCH_TIM1_CH1 D(6) +#define DEF_TIM_AF__PA9__TCH_TIM1_CH2 D(6) +#define DEF_TIM_AF__PA10__TCH_TIM1_CH3 D(6) +#define DEF_TIM_AF__PA11__TCH_TIM1_CH1N D(6) +#define DEF_TIM_AF__PA12__TCH_TIM1_CH2N D(6) + +#define DEF_TIM_AF__PA1__TCH_TIM15_CH1N D(9) +#define DEF_TIM_AF__PA2__TCH_TIM15_CH1 D(9) +#define DEF_TIM_AF__PA3__TCH_TIM15_CH2 D(9) + +#define DEF_TIM_AF__PA9__TCH_TIM2_CH3 D(10) +#define DEF_TIM_AF__PA10__TCH_TIM2_CH4 D(10) +#define DEF_TIM_AF__PA11__TCH_TIM4_CH1 D(10) +#define DEF_TIM_AF__PA12__TCH_TIM4_CH2 D(10) +#define DEF_TIM_AF__PA13__TCH_TIM4_CH3 D(10) +#define DEF_TIM_AF__PA11__TCH_TIM1_CH4 D(11) + +#define DEF_TIM_AF__PB3__TCH_TIM2_CH2 D(1) +#define DEF_TIM_AF__PB4__TCH_TIM16_CH1 D(1) +#define DEF_TIM_AF__PB6__TCH_TIM16_CH1N D(1) +#define DEF_TIM_AF__PB7__TCH_TIM17_CH1N D(1) +#define DEF_TIM_AF__PB8__TCH_TIM16_CH1 D(1) +#define DEF_TIM_AF__PB9__TCH_TIM17_CH1 D(1) +#define DEF_TIM_AF__PB10__TCH_TIM2_CH3 D(1) +#define DEF_TIM_AF__PB11__TCH_TIM2_CH4 D(1) +#define DEF_TIM_AF__PB14__TCH_TIM15_CH1 D(1) +#define DEF_TIM_AF__PB15__TCH_TIM15_CH2 D(1) + +#define DEF_TIM_AF__PB0__TCH_TIM3_CH3 D(2) +#define DEF_TIM_AF__PB1__TCH_TIM3_CH4 D(2) +#define DEF_TIM_AF__PB4__TCH_TIM3_CH1 D(2) +#define DEF_TIM_AF__PB5__TCH_TIM3_CH2 D(2) +#define DEF_TIM_AF__PB6__TCH_TIM4_CH1 D(2) +#define DEF_TIM_AF__PB7__TCH_TIM4_CH2 D(2) +#define DEF_TIM_AF__PB8__TCH_TIM4_CH3 D(2) +#define DEF_TIM_AF__PB9__TCH_TIM4_CH4 D(2) +#define DEF_TIM_AF__PB15__TCH_TIM15_CH1N D(2) + +#define DEF_TIM_AF__PB5__TCH_TIM8_CH3N D(3) + +#define DEF_TIM_AF__PB0__TCH_TIM8_CH2N D(4) +#define DEF_TIM_AF__PB1__TCH_TIM8_CH3N D(4) +#define DEF_TIM_AF__PB3__TCH_TIM8_CH1N D(4) +#define DEF_TIM_AF__PB4__TCH_TIM8_CH2N D(4) +#define DEF_TIM_AF__PB15__TCH_TIM1_CH3N D(4) + +#define DEF_TIM_AF__PB6__TCH_TIM8_CH1 D(5) + +#define DEF_TIM_AF__PB0__TCH_TIM1_CH2N D(6) +#define DEF_TIM_AF__PB1__TCH_TIM1_CH3N D(6) +#define DEF_TIM_AF__PB13__TCH_TIM1_CH1N D(6) +#define DEF_TIM_AF__PB14__TCH_TIM1_CH2N D(6) + +#define DEF_TIM_AF__PB5__TCH_TIM17_CH1 D(10) +#define DEF_TIM_AF__PB7__TCH_TIM3_CH4 D(10) +#define DEF_TIM_AF__PB8__TCH_TIM8_CH2 D(10) +#define DEF_TIM_AF__PB9__TCH_TIM8_CH3 D(10) + +#define DEF_TIM_AF__PC6__TCH_TIM3_CH1 D(2) +#define DEF_TIM_AF__PC7__TCH_TIM3_CH2 D(2) +#define DEF_TIM_AF__PC8__TCH_TIM3_CH3 D(2) +#define DEF_TIM_AF__PC9__TCH_TIM3_CH4 D(2) + +#define DEF_TIM_AF__PC6__TCH_TIM8_CH1 D(4) +#define DEF_TIM_AF__PC7__TCH_TIM8_CH2 D(4) +#define DEF_TIM_AF__PC8__TCH_TIM8_CH3 D(4) +#define DEF_TIM_AF__PC9__TCH_TIM8_CH4 D(4) + +#define DEF_TIM_AF__PC10__TCH_TIM8_CH1N D(4) +#define DEF_TIM_AF__PC11__TCH_TIM8_CH2N D(4) +#define DEF_TIM_AF__PC12__TCH_TIM8_CH3N D(4) +#define DEF_TIM_AF__PC13__TCH_TIM8_CH1N D(4) + +#define DEF_TIM_AF__PD3__TCH_TIM2_CH1 D(2) +#define DEF_TIM_AF__PD4__TCH_TIM2_CH2 D(2) +#define DEF_TIM_AF__PD6__TCH_TIM2_CH4 D(2) +#define DEF_TIM_AF__PD7__TCH_TIM2_CH3 D(2) + +#define DEF_TIM_AF__PD12__TCH_TIM4_CH1 D(2) +#define DEF_TIM_AF__PD13__TCH_TIM4_CH2 D(2) +#define DEF_TIM_AF__PD14__TCH_TIM4_CH3 D(2) +#define DEF_TIM_AF__PD15__TCH_TIM4_CH4 D(2) + +#define DEF_TIM_AF__PD1__TCH_TIM8_CH4 D(4) + +#define DEF_TIM_AF__PF9__TCH_TIM15_CH1 D(3) +#define DEF_TIM_AF__PF10__TCH_TIM15_CH2 D(3) diff --git a/src/main/drivers/timer_def_stm32f4xx.h b/src/main/drivers/timer_def_stm32f4xx.h new file mode 100644 index 00000000000..cd51c1f49d8 --- /dev/null +++ b/src/main/drivers/timer_def_stm32f4xx.h @@ -0,0 +1,75 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + */ + +#pragma once + +#define DEF_TIM_DMAMAP__D(dma, stream, channel) DMA_TAG(dma, stream, channel) +#define DEF_TIM_DMAMAP__NONE DMA_NONE + +#define DEF_TIM(tim, ch, pin, usage, flags, dmavar) { tim, IO_TAG(pin), DEF_TIM_CHNL_ ## ch, DEF_TIM_OUTPUT(ch) | flags, IOCFG_AF_PP, GPIO_AF_ ## tim, usage, DEF_TIM_DMAMAP(dmavar, tim ## _ ## ch) } + +// AF mappings +#define DEF_TIM_AF(timch, pin) CONCAT(DEF_TIM_AF__, DEF_TIM_AF__ ## pin ## __ ## timch) +#define DEF_TIM_AF__D(af_n) GPIO_AF_ ## af_n + +/* add the DMA mappings here */ + +#define DEF_TIM_DMA__BTCH_TIM1_CH1 D(2, 6, 0),D(2, 1, 6),D(2, 3, 6) +#define DEF_TIM_DMA__BTCH_TIM1_CH2 D(2, 6, 0),D(2, 2, 6) +#define DEF_TIM_DMA__BTCH_TIM1_CH3 D(2, 6, 0),D(2, 6, 6) +#define DEF_TIM_DMA__BTCH_TIM1_CH4 D(2, 4, 6) + +#define DEF_TIM_DMA__BTCH_TIM2_CH1 D(1, 5, 3) +#define DEF_TIM_DMA__BTCH_TIM2_CH2 D(1, 6, 3) +#define DEF_TIM_DMA__BTCH_TIM2_CH3 D(1, 1, 3) +#define DEF_TIM_DMA__BTCH_TIM2_CH4 D(1, 7, 3),D(1, 6, 3) + +#define DEF_TIM_DMA__BTCH_TIM3_CH1 D(1, 4, 5) +#define DEF_TIM_DMA__BTCH_TIM3_CH2 D(1, 5, 5) +#define DEF_TIM_DMA__BTCH_TIM3_CH3 D(1, 7, 5) +#define DEF_TIM_DMA__BTCH_TIM3_CH4 D(1, 2, 5) + +#define DEF_TIM_DMA__BTCH_TIM4_CH1 D(1, 0, 2) +#define DEF_TIM_DMA__BTCH_TIM4_CH2 D(1, 3, 2) +#define DEF_TIM_DMA__BTCH_TIM4_CH3 D(1, 7, 2) + +#define DEF_TIM_DMA__BTCH_TIM5_CH1 D(1, 2, 6) +#define DEF_TIM_DMA__BTCH_TIM5_CH2 D(1, 4, 6) +#define DEF_TIM_DMA__BTCH_TIM5_CH3 D(1, 0, 6) +#define DEF_TIM_DMA__BTCH_TIM5_CH4 D(1, 1, 6),D(1, 3, 6) + +#define DEF_TIM_DMA__BTCH_TIM8_CH1 D(2, 2, 0),D(2, 2, 7) +#define DEF_TIM_DMA__BTCH_TIM8_CH2 D(2, 2, 0),D(2, 3, 7) +#define DEF_TIM_DMA__BTCH_TIM8_CH3 D(2, 2, 0),D(2, 4, 7) +#define DEF_TIM_DMA__BTCH_TIM8_CH4 D(2, 7, 7) + +#define DEF_TIM_DMA__BTCH_TIM4_CH4 NONE + +#define DEF_TIM_DMA__BTCH_TIM9_CH1 NONE +#define DEF_TIM_DMA__BTCH_TIM9_CH2 NONE + +#define DEF_TIM_DMA__BTCH_TIM10_CH1 NONE + +#define DEF_TIM_DMA__BTCH_TIM11_CH1 NONE + +#define DEF_TIM_DMA__BTCH_TIM12_CH1 NONE +#define DEF_TIM_DMA__BTCH_TIM12_CH2 NONE + +#define DEF_TIM_DMA__BTCH_TIM13_CH1 NONE + +#define DEF_TIM_DMA__BTCH_TIM14_CH1 NONE + diff --git a/src/main/drivers/timer_def_stm32f7xx.h b/src/main/drivers/timer_def_stm32f7xx.h new file mode 100644 index 00000000000..85d763a9043 --- /dev/null +++ b/src/main/drivers/timer_def_stm32f7xx.h @@ -0,0 +1,212 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + */ + +#pragma once + +// Mappings for STDLIB defines +// #define DEF_TIM_CHNL_CH1 TIM_CHANNEL_1 +// #define DEF_TIM_CHNL_CH1N TIM_CHANNEL_1 +// #define DEF_TIM_CHNL_CH2 TIM_CHANNEL_2 +// #define DEF_TIM_CHNL_CH2N TIM_CHANNEL_2 +// #define DEF_TIM_CHNL_CH3 TIM_CHANNEL_3 +// #define DEF_TIM_CHNL_CH3N TIM_CHANNEL_3 +// #define DEF_TIM_CHNL_CH4 TIM_CHANNEL_4 +// #define DEF_TIM_CHNL_CH4N TIM_CHANNEL_4 + +#define DEF_TIM_DMAMAP__D(dma, stream, channel) DMA_TAG(dma, stream, channel) +#define DEF_TIM_DMAMAP__NONE DMA_NONE + +#define DEF_TIM(tim, ch, pin, usage, flags, dmavar) { tim, IO_TAG(pin), DEF_TIM_CHNL_ ## ch, DEF_TIM_OUTPUT(ch) | flags, IOCFG_AF_PP, DEF_TIM_AF(TCH_## tim ## _ ## ch, pin), usage, DEF_TIM_DMAMAP(dmavar, tim ## _ ## ch) } + +// AF mappings +#define DEF_TIM_AF(timch, pin) CONCAT(DEF_TIM_AF__, DEF_TIM_AF__ ## pin ## __ ## timch) +#define DEF_TIM_AF__D(af_n, tim_n) GPIO_AF ## af_n ## _TIM ## tim_n + +/* F7 Stream Mappings */ +// D(DMAx, Stream, Channel) +#define DEF_TIM_DMA__BTCH_TIM1_CH1 D(2, 6, 0),D(2, 1, 6),D(2, 3, 6) +#define DEF_TIM_DMA__BTCH_TIM1_CH2 D(2, 6, 0),D(2, 2, 6) +#define DEF_TIM_DMA__BTCH_TIM1_CH3 D(2, 6, 0),D(2, 6, 6) +#define DEF_TIM_DMA__BTCH_TIM1_CH4 D(2, 4, 6) + +#define DEF_TIM_DMA__BTCH_TIM2_CH1 D(1, 5, 3) +#define DEF_TIM_DMA__BTCH_TIM2_CH2 D(1, 6, 3) +#define DEF_TIM_DMA__BTCH_TIM2_CH3 D(1, 1, 3) +#define DEF_TIM_DMA__BTCH_TIM2_CH4 D(1, 7, 3),D(1, 6, 3) + +#define DEF_TIM_DMA__BTCH_TIM3_CH1 D(1, 4, 5) +#define DEF_TIM_DMA__BTCH_TIM3_CH2 D(1, 5, 5) +#define DEF_TIM_DMA__BTCH_TIM3_CH3 D(1, 7, 5) +#define DEF_TIM_DMA__BTCH_TIM3_CH4 D(1, 2, 5) + +#define DEF_TIM_DMA__BTCH_TIM4_CH1 D(1, 0, 2) +#define DEF_TIM_DMA__BTCH_TIM4_CH2 D(1, 3, 2) +#define DEF_TIM_DMA__BTCH_TIM4_CH3 D(1, 7, 2) + +#define DEF_TIM_DMA__BTCH_TIM5_CH1 D(1, 2, 6) +#define DEF_TIM_DMA__BTCH_TIM5_CH2 D(1, 4, 6) +#define DEF_TIM_DMA__BTCH_TIM5_CH3 D(1, 0, 6) +#define DEF_TIM_DMA__BTCH_TIM5_CH4 D(1, 1, 6),D(1, 3, 6) + +#define DEF_TIM_DMA__BTCH_TIM8_CH1 D(2, 2, 7),D(2, 2, 0) +#define DEF_TIM_DMA__BTCH_TIM8_CH2 D(2, 3, 7),D(2, 2, 0) +#define DEF_TIM_DMA__BTCH_TIM8_CH3 D(2, 4, 7),D(2, 2, 0) +#define DEF_TIM_DMA__BTCH_TIM8_CH4 D(2, 7, 7) + +#define DEF_TIM_DMA__BTCH_TIM4_CH4 NONE + +#define DEF_TIM_DMA__BTCH_TIM9_CH1 NONE +#define DEF_TIM_DMA__BTCH_TIM9_CH2 NONE + +#define DEF_TIM_DMA__BTCH_TIM10_CH1 NONE + +#define DEF_TIM_DMA__BTCH_TIM11_CH1 NONE + +#define DEF_TIM_DMA__BTCH_TIM12_CH1 NONE +#define DEF_TIM_DMA__BTCH_TIM12_CH2 NONE + +#define DEF_TIM_DMA__BTCH_TIM13_CH1 NONE + +#define DEF_TIM_DMA__BTCH_TIM14_CH1 NONE + +// TIM_UP table +#define DEF_TIM_DMA__BTCH_TIM1_UP D(2, 5, 6) +#define DEF_TIM_DMA__BTCH_TIM2_UP D(1, 7, 3) +#define DEF_TIM_DMA__BTCH_TIM3_UP D(1, 2, 5) +#define DEF_TIM_DMA__BTCH_TIM4_UP D(1, 6, 2) +#define DEF_TIM_DMA__BTCH_TIM5_UP D(1, 0, 6) +#define DEF_TIM_DMA__BTCH_TIM6_UP D(1, 1, 7) +#define DEF_TIM_DMA__BTCH_TIM7_UP D(1, 4, 1) +#define DEF_TIM_DMA__BTCH_TIM8_UP D(2, 1, 7) +#define DEF_TIM_DMA__BTCH_TIM9_UP NONE +#define DEF_TIM_DMA__BTCH_TIM10_UP NONE +#define DEF_TIM_DMA__BTCH_TIM11_UP NONE +#define DEF_TIM_DMA__BTCH_TIM12_UP NONE +#define DEF_TIM_DMA__BTCH_TIM13_UP NONE +#define DEF_TIM_DMA__BTCH_TIM14_UP NONE + +// AF table + +//PORTA +#define DEF_TIM_AF__PA0__TCH_TIM2_CH1 D(1, 2) +#define DEF_TIM_AF__PA1__TCH_TIM2_CH2 D(1, 2) +#define DEF_TIM_AF__PA2__TCH_TIM2_CH3 D(1, 2) +#define DEF_TIM_AF__PA3__TCH_TIM2_CH4 D(1, 2) +#define DEF_TIM_AF__PA5__TCH_TIM2_CH1 D(1, 2) +#define DEF_TIM_AF__PA7__TCH_TIM1_CH1N D(1, 1) +#define DEF_TIM_AF__PA8__TCH_TIM1_CH1 D(1, 1) +#define DEF_TIM_AF__PA9__TCH_TIM1_CH2 D(1, 1) +#define DEF_TIM_AF__PA10__TCH_TIM1_CH3 D(1, 1) +#define DEF_TIM_AF__PA11__TCH_TIM1_CH1N D(1, 1) +#define DEF_TIM_AF__PA15__TCH_TIM2_CH1 D(1, 2) + +#define DEF_TIM_AF__PA0__TCH_TIM5_CH1 D(2, 5) +#define DEF_TIM_AF__PA1__TCH_TIM5_CH2 D(2, 5) +#define DEF_TIM_AF__PA2__TCH_TIM5_CH3 D(2, 5) +#define DEF_TIM_AF__PA3__TCH_TIM5_CH4 D(2, 5) +#define DEF_TIM_AF__PA6__TCH_TIM3_CH1 D(2, 3) +#define DEF_TIM_AF__PA7__TCH_TIM3_CH2 D(2, 3) + +#define DEF_TIM_AF__PA2__TCH_TIM9_CH1 D(3, 9) +#define DEF_TIM_AF__PA3__TCH_TIM9_CH2 D(3, 9) +#define DEF_TIM_AF__PA5__TCH_TIM8_CH1N D(3, 8) +#define DEF_TIM_AF__PA7__TCH_TIM8_CH1N D(3, 8) + +#define DEF_TIM_AF__PA6__TCH_TIM13_CH1 D(9, 13) +#define DEF_TIM_AF__PA7__TCH_TIM14_CH1 D(9, 14) + +//PORTB +#define DEF_TIM_AF__PB0__TCH_TIM1_CH2N D(1, 1) +#define DEF_TIM_AF__PB1__TCH_TIM1_CH3N D(1, 1) +#define DEF_TIM_AF__PB3__TCH_TIM2_CH2 D(1, 2) +#define DEF_TIM_AF__PB10__TCH_TIM2_CH3 D(1, 2) +#define DEF_TIM_AF__PB11__TCH_TIM2_CH4 D(1, 2) +#define DEF_TIM_AF__PB13__TCH_TIM1_CH1N D(1, 1) +#define DEF_TIM_AF__PB14__TCH_TIM1_CH2N D(1, 1) +#define DEF_TIM_AF__PB15__TCH_TIM1_CH3N D(1, 1) + +#define DEF_TIM_AF__PB0__TCH_TIM3_CH3 D(2, 3) +#define DEF_TIM_AF__PB1__TCH_TIM3_CH4 D(2, 3) +#define DEF_TIM_AF__PB4__TCH_TIM3_CH1 D(2, 3) +#define DEF_TIM_AF__PB5__TCH_TIM3_CH2 D(2, 3) +#define DEF_TIM_AF__PB6__TCH_TIM4_CH1 D(2, 4) +#define DEF_TIM_AF__PB7__TCH_TIM4_CH2 D(2, 4) +#define DEF_TIM_AF__PB8__TCH_TIM4_CH3 D(2, 4) +#define DEF_TIM_AF__PB9__TCH_TIM4_CH4 D(2, 4) + +#define DEF_TIM_AF__PB0__TCH_TIM8_CH2N D(3, 8) +#define DEF_TIM_AF__PB1__TCH_TIM8_CH3N D(3, 8) +#define DEF_TIM_AF__PB8__TCH_TIM10_CH1 D(3, 10) +#define DEF_TIM_AF__PB9__TCH_TIM11_CH1 D(3, 11) +#define DEF_TIM_AF__PB14__TCH_TIM8_CH2N D(3, 8) +#define DEF_TIM_AF__PB15__TCH_TIM8_CH3N D(3, 8) + +#define DEF_TIM_AF__PB14__TCH_TIM12_CH1 D(9, 12) +#define DEF_TIM_AF__PB15__TCH_TIM12_CH2 D(9, 12) + +//PORTC +#define DEF_TIM_AF__PC6__TCH_TIM3_CH1 D(2, 3) +#define DEF_TIM_AF__PC7__TCH_TIM3_CH2 D(2, 3) +#define DEF_TIM_AF__PC8__TCH_TIM3_CH3 D(2, 3) +#define DEF_TIM_AF__PC9__TCH_TIM3_CH4 D(2, 3) + +#define DEF_TIM_AF__PC6__TCH_TIM8_CH1 D(3, 8) +#define DEF_TIM_AF__PC7__TCH_TIM8_CH2 D(3, 8) +#define DEF_TIM_AF__PC8__TCH_TIM8_CH3 D(3, 8) +#define DEF_TIM_AF__PC9__TCH_TIM8_CH4 D(3, 8) + +//PORTD +#define DEF_TIM_AF__PD12__TCH_TIM4_CH1 D(2, 4) +#define DEF_TIM_AF__PD13__TCH_TIM4_CH2 D(2, 4) +#define DEF_TIM_AF__PD14__TCH_TIM4_CH3 D(2, 4) +#define DEF_TIM_AF__PD15__TCH_TIM4_CH4 D(2, 4) + +//PORTE +#define DEF_TIM_AF__PE8__TCH_TIM1_CH1N D(1, 1) +#define DEF_TIM_AF__PE9__TCH_TIM1_CH1 D(1, 1) +#define DEF_TIM_AF__PE10__TCH_TIM1_CH2N D(1, 1) +#define DEF_TIM_AF__PE11__TCH_TIM1_CH2 D(1, 1) +#define DEF_TIM_AF__PE12__TCH_TIM1_CH3N D(1, 1) +#define DEF_TIM_AF__PE13__TCH_TIM1_CH3 D(1, 1) +#define DEF_TIM_AF__PE14__TCH_TIM1_CH4 D(1, 1) + +#define DEF_TIM_AF__PE5__TCH_TIM9_CH1 D(3, 9) +#define DEF_TIM_AF__PE6__TCH_TIM9_CH2 D(3, 9) + +//PORTF +#define DEF_TIM_AF__PF6__TCH_TIM10_CH1 D(3, 10) +#define DEF_TIM_AF__PF7__TCH_TIM11_CH1 D(3, 11) + +//PORTH +#define DEF_TIM_AF__PH10__TCH_TIM5_CH1 D(2, 5) +#define DEF_TIM_AF__PH11__TCH_TIM5_CH2 D(2, 5) +#define DEF_TIM_AF__PH12__TCH_TIM5_CH3 D(2, 5) + +#define DEF_TIM_AF__PH13__TCH_TIM8_CH1N D(3, 8) +#define DEF_TIM_AF__PH14__TCH_TIM8_CH2N D(3, 8) +#define DEF_TIM_AF__PH15__TCH_TIM8_CH3N D(3, 8) + +#define DEF_TIM_AF__PH6__TCH_TIM12_CH1 D(9, 12) +#define DEF_TIM_AF__PH9__TCH_TIM12_CH2 D(9, 12) + +//PORTI +#define DEF_TIM_AF__PI0__TCH_TIM5_CH4 D(2, 5) + +#define DEF_TIM_AF__PI2__TCH_TIM8_CH4 D(3, 8) +#define DEF_TIM_AF__PI5__TCH_TIM8_CH1 D(3, 8) +#define DEF_TIM_AF__PI6__TCH_TIM8_CH2 D(3, 8) +#define DEF_TIM_AF__PI7__TCH_TIM8_CH3 D(3, 8) diff --git a/src/main/drivers/timer_impl.h b/src/main/drivers/timer_impl.h index 5f413d67b54..aa9b7fee271 100644 --- a/src/main/drivers/timer_impl.h +++ b/src/main/drivers/timer_impl.h @@ -1,64 +1,61 @@ /* - * This file is part of Cleanflight. + * This file is part of INAV. * - * Cleanflight is free software: you can redistribute it and/or modify + * INAV is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * - * Cleanflight is distributed in the hope that it will be useful, + * INAV is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License - * along with Cleanflight. If not, see . + * along with INAV. If not, see . */ #pragma once -#define CC_CHANNELS_PER_TIMER 4 // TIM_Channel_1..4 - #if defined(USE_HAL_DRIVER) # define IMPL_TIM_IT_UPDATE_INTERRUPT TIM_IT_UPDATE -# define TIM_IT_CCx(ch) (TIM_IT_CC1 << ((ch) / 4)) +# define TIM_IT_CCx(chIdx) (TIM_IT_CC1 << (chIdx)) #else # define IMPL_TIM_IT_UPDATE_INTERRUPT TIM_IT_Update -# define TIM_IT_CCx(ch) (TIM_IT_CC1 << ((ch) / 4)) +# define TIM_IT_CCx(chIdx) (TIM_IT_CC1 << (chIdx)) #endif -typedef struct timerConfig_s { - timerCCHandlerRec_t *edgeCallback[CC_CHANNELS_PER_TIMER]; - timerOvrHandlerRec_t *overflowCallback[CC_CHANNELS_PER_TIMER]; - timerOvrHandlerRec_t *overflowCallbackActive; // null-terminated linkded list of active overflow callbacks -} timerConfig_t; - -extern timerConfig_t * timerConfig[HARDWARE_TIMER_DEFINITION_COUNT]; - #define _TIM_IRQ_HANDLER2(name, i, j) \ void name(void) \ { \ - impl_timerCaptureCompareHandler(TIM ## i, timerConfig[i - 1]); \ - impl_timerCaptureCompareHandler(TIM ## j, timerConfig[j - 1]); \ + impl_timerCaptureCompareHandler(TIM ## i, timerCtx[i - 1]); \ + impl_timerCaptureCompareHandler(TIM ## j, timerCtx[j - 1]); \ } struct dummy #define _TIM_IRQ_HANDLER(name, i) \ void name(void) \ { \ - impl_timerCaptureCompareHandler(TIM ## i, timerConfig[i - 1]); \ + impl_timerCaptureCompareHandler(TIM ## i, timerCtx[i - 1]); \ } struct dummy uint8_t lookupTimerIndex(const TIM_TypeDef *tim); -void impl_timerNVICConfigure(uint8_t irq, int irqPriority); -void impl_timerConfigBase(TIM_TypeDef *tim, uint16_t period, uint8_t mhz); -void impl_enableTimer(TIM_TypeDef * tim); -void impl_timerEnableIT(TIM_TypeDef * tim, uint32_t interrupt); -void impl_timerDisableIT(TIM_TypeDef * tim, uint32_t interrupt); -void impl_timerClearFlag(TIM_TypeDef * tim, uint32_t flag); -void impl_timerChConfigIC(const timerHardware_t *timHw, bool polarityRising, unsigned inputFilterTicks); -void impl_timerCaptureCompareHandler(TIM_TypeDef *tim, timerConfig_t *timerConfig); -void impl_timerPWMConfigChannel(TIM_TypeDef * tim, uint8_t channel, bool isNChannel, bool inverted, uint16_t value); -void impl_timerPWMStart(TIM_TypeDef * tim, unsigned channel, bool isNChannel); -uint16_t impl_timerDmaSource(uint8_t channel); -volatile timCCR_t * impl_timerCCR(TIM_TypeDef *tim, uint8_t channel); - +void impl_timerInitContext(timHardwareContext_t * timCtx); + +volatile timCCR_t * impl_timerCCR(TCH_t * tch); +void impl_timerCaptureCompareHandler(TIM_TypeDef *tim, timHardwareContext_t * timerCtx); + +void impl_timerNVICConfigure(TCH_t * tch, int irqPriority); +void impl_timerConfigBase(TCH_t * tch, uint16_t period, uint32_t hz); +void impl_enableTimer(TCH_t * tch); +void impl_timerEnableIT(TCH_t * tch, uint32_t interrupt); +void impl_timerDisableIT(TCH_t * tch, uint32_t interrupt); +void impl_timerClearFlag(TCH_t * tch, uint32_t flag); +void impl_timerChConfigIC(TCH_t * tch, bool polarityRising, unsigned inputFilterTicks); +void impl_timerChCaptureCompareEnable(TCH_t * tch, bool enable); + +void impl_timerPWMConfigChannel(TCH_t * tch, uint16_t value); +void impl_timerPWMStart(TCH_t * tch); +bool impl_timerPWMConfigChannelDMA(TCH_t * tch, void * dmaBuffer, uint32_t dmaBufferSize); +void impl_timerPWMPrepareDMA(TCH_t * tch, uint32_t dmaBufferSize); +void impl_timerPWMStartDMA(TCH_t * tch); +void impl_timerPWMStopDMA(TCH_t * tch); diff --git a/src/main/drivers/timer_impl_hal.c b/src/main/drivers/timer_impl_hal.c index cb1e0af4fad..1834d84c748 100644 --- a/src/main/drivers/timer_impl_hal.c +++ b/src/main/drivers/timer_impl_hal.c @@ -1,9 +1,19 @@ /* - modified version of StdPeriph function is located here. - TODO - what license does apply here? - original file was lincesed under MCD-ST Liberty SW License Agreement V2 - http://www.st.com/software_license_agreement_liberty_v2 -*/ + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + */ #include #include @@ -12,6 +22,7 @@ #include "platform.h" #include "build/atomic.h" +#include "build/debug.h" #include "common/utils.h" @@ -22,9 +33,17 @@ #include "drivers/timer.h" #include "drivers/timer_impl.h" +extern uint32_t timerClock(TIM_TypeDef *tim); + +const uint16_t lookupDMASourceTable[] = { TIM_DMA_CC1, TIM_DMA_CC2, TIM_DMA_CC3, TIM_DMA_CC4 }; +const uint8_t lookupTIMChannelTable[] = { TIM_CHANNEL_1, TIM_CHANNEL_2, TIM_CHANNEL_3, TIM_CHANNEL_4 }; + +static const uint32_t lookupDMALLStreamTable[] = { LL_DMA_STREAM_0, LL_DMA_STREAM_1, LL_DMA_STREAM_2, LL_DMA_STREAM_3, LL_DMA_STREAM_4, LL_DMA_STREAM_5, LL_DMA_STREAM_6, LL_DMA_STREAM_7 }; +static const uint32_t lookupDMALLChannelTable[] = { LL_DMA_CHANNEL_0, LL_DMA_CHANNEL_1, LL_DMA_CHANNEL_2, LL_DMA_CHANNEL_3, LL_DMA_CHANNEL_4, LL_DMA_CHANNEL_5, LL_DMA_CHANNEL_6, LL_DMA_CHANNEL_7 }; + static TIM_HandleTypeDef timerHandle[HARDWARE_TIMER_DEFINITION_COUNT]; -TIM_HandleTypeDef * timerFindTimerHandle(TIM_TypeDef *tim) +static TIM_HandleTypeDef * timerFindTimerHandle(TIM_TypeDef *tim) { uint8_t timerIndex = lookupTimerIndex(tim); if (timerIndex >= HARDWARE_TIMER_DEFINITION_COUNT) { @@ -34,105 +53,126 @@ TIM_HandleTypeDef * timerFindTimerHandle(TIM_TypeDef *tim) return &timerHandle[timerIndex]; } -void impl_timerNVICConfigure(uint8_t irq, int irqPriority) +void impl_timerInitContext(timHardwareContext_t * timCtx) { - HAL_NVIC_SetPriority(irq, NVIC_PRIORITY_BASE(irqPriority), NVIC_PRIORITY_SUB(irqPriority)); - HAL_NVIC_EnableIRQ(irq); + timCtx->timHandle = timerFindTimerHandle(timCtx->timDef->tim); } -void impl_timerConfigBase(TIM_TypeDef *tim, uint16_t period, uint8_t mhz) +void impl_timerNVICConfigure(TCH_t * tch, int irqPriority) { - uint8_t timerIndex = lookupTimerIndex(tim); + if (tch->timCtx->timDef->irq) { + HAL_NVIC_SetPriority(tch->timCtx->timDef->irq, NVIC_PRIORITY_BASE(irqPriority), NVIC_PRIORITY_SUB(irqPriority)); + HAL_NVIC_EnableIRQ(tch->timCtx->timDef->irq); + } - if (timerIndex >= HARDWARE_TIMER_DEFINITION_COUNT) { - return; + if (tch->timCtx->timDef->secondIrq) { + HAL_NVIC_SetPriority(tch->timCtx->timDef->secondIrq, NVIC_PRIORITY_BASE(irqPriority), NVIC_PRIORITY_SUB(irqPriority)); + HAL_NVIC_EnableIRQ(tch->timCtx->timDef->secondIrq); } +} + +void impl_timerConfigBase(TCH_t * tch, uint16_t period, uint32_t hz) +{ + // Get and verify HAL TIM_Handle object + TIM_HandleTypeDef * timHandle = tch->timCtx->timHandle; + TIM_TypeDef * timer = tch->timCtx->timDef->tim; - if (timerHandle[timerIndex].Instance == tim) - { - // already configured + if (timHandle->Instance == timer) { return; } - timerHandle[timerIndex].Instance = tim; - timerHandle[timerIndex].Init.Period = (period - 1) & 0xffff; // AKA TIMx_ARR - timerHandle[timerIndex].Init.Prescaler = (timerClock(tim) / ((uint32_t)mhz * 1000000)) - 1; - timerHandle[timerIndex].Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; - timerHandle[timerIndex].Init.CounterMode = TIM_COUNTERMODE_UP; - timerHandle[timerIndex].Init.RepetitionCounter = 0x0000; + timHandle->Instance = timer; + timHandle->Init.Prescaler = (timerGetBaseClock(tch) / hz) - 1; + timHandle->Init.Period = (period - 1) & 0xffff; // AKA TIMx_ARR + timHandle->Init.RepetitionCounter = 0; + timHandle->Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; + timHandle->Init.CounterMode = TIM_COUNTERMODE_UP; + timHandle->Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_ENABLE; - HAL_TIM_Base_Init(&timerHandle[timerIndex]); - if (tim == TIM1 || tim == TIM2 || tim == TIM3 || tim == TIM4 || tim == TIM5 || tim == TIM8 || tim == TIM9) - { + HAL_TIM_Base_Init(timHandle); + if (timer == TIM1 || timer == TIM2 || timer == TIM3 || timer == TIM4 || timer == TIM5 || timer == TIM8 || timer == TIM9) { TIM_ClockConfigTypeDef sClockSourceConfig; memset(&sClockSourceConfig, 0, sizeof(sClockSourceConfig)); sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL; - if (HAL_TIM_ConfigClockSource(&timerHandle[timerIndex], &sClockSourceConfig) != HAL_OK) { + if (HAL_TIM_ConfigClockSource(timHandle, &sClockSourceConfig) != HAL_OK) { return; } } - if (tim == TIM1 || tim == TIM2 || tim == TIM3 || tim == TIM4 || tim == TIM5 || tim == TIM8 ) - { + + if (timer == TIM1 || timer == TIM2 || timer == TIM3 || timer == TIM4 || timer == TIM5 || timer == TIM8) { TIM_MasterConfigTypeDef sMasterConfig; memset(&sMasterConfig, 0, sizeof(sMasterConfig)); sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE; - if (HAL_TIMEx_MasterConfigSynchronization(&timerHandle[timerIndex], &sMasterConfig) != HAL_OK) { + if (HAL_TIMEx_MasterConfigSynchronization(timHandle, &sMasterConfig) != HAL_OK) { return; } } } -void impl_enableTimer(TIM_TypeDef * tim) +void impl_timerPWMConfigChannel(TCH_t * tch, uint16_t value) { - TIM_HandleTypeDef * Handle = timerFindTimerHandle(tim); - if (Handle == NULL) { - return; - } + const bool inverted = tch->timHw->output & TIMER_OUTPUT_INVERTED; - HAL_TIM_Base_Start(Handle); -} + TIM_OC_InitTypeDef TIM_OCInitStructure; -void impl_timerPWMStart(TIM_TypeDef * tim, unsigned channel, bool isNChannel) -{ - TIM_HandleTypeDef * Handle = timerFindTimerHandle(tim); - if (Handle == NULL) { - return; - } + TIM_OCInitStructure.OCMode = TIM_OCMODE_PWM1; + TIM_OCInitStructure.OCIdleState = TIM_OCIDLESTATE_SET; + TIM_OCInitStructure.OCPolarity = inverted ? TIM_OCPOLARITY_LOW : TIM_OCPOLARITY_HIGH; + TIM_OCInitStructure.OCNIdleState = TIM_OCNIDLESTATE_SET; + TIM_OCInitStructure.OCNPolarity = inverted ? TIM_OCNPOLARITY_LOW : TIM_OCNPOLARITY_HIGH; + TIM_OCInitStructure.Pulse = value; + TIM_OCInitStructure.OCFastMode = TIM_OCFAST_DISABLE; - if (isNChannel) - HAL_TIMEx_PWMN_Start(Handle, channel); - else - HAL_TIM_PWM_Start(Handle, channel); + HAL_TIM_PWM_ConfigChannel(tch->timCtx->timHandle, &TIM_OCInitStructure, lookupTIMChannelTable[tch->timHw->channelIndex]); } -void impl_timerEnableIT(TIM_TypeDef * tim, uint32_t interrupt) +volatile timCCR_t * impl_timerCCR(TCH_t * tch) { - TIM_HandleTypeDef * Handle = timerFindTimerHandle(tim); - if (Handle == NULL) { - return; + switch (tch->timHw->channelIndex) { + case 0: + return &tch->timHw->tim->CCR1; + break; + case 1: + return &tch->timHw->tim->CCR2; + break; + case 2: + return &tch->timHw->tim->CCR3; + break; + case 3: + return &tch->timHw->tim->CCR4; + break; } + return NULL; +} - __HAL_TIM_ENABLE_IT(Handle, interrupt); +void impl_enableTimer(TCH_t * tch) +{ + HAL_TIM_Base_Start(tch->timCtx->timHandle); } -void impl_timerDisableIT(TIM_TypeDef * tim, uint32_t interrupt) +void impl_timerPWMStart(TCH_t * tch) { - TIM_HandleTypeDef * Handle = timerFindTimerHandle(tim); - if (Handle == NULL) { - return; + if (tch->timHw->output & TIMER_OUTPUT_N_CHANNEL) { + HAL_TIMEx_PWMN_Start(tch->timCtx->timHandle, lookupTIMChannelTable[tch->timHw->channelIndex]); + } + else { + HAL_TIM_PWM_Start(tch->timCtx->timHandle, lookupTIMChannelTable[tch->timHw->channelIndex]); } +} - __HAL_TIM_DISABLE_IT(Handle, interrupt); +void impl_timerEnableIT(TCH_t * tch, uint32_t interrupt) +{ + __HAL_TIM_ENABLE_IT(tch->timCtx->timHandle, interrupt); } -void impl_timerClearFlag(TIM_TypeDef * tim, uint32_t flag) +void impl_timerDisableIT(TCH_t * tch, uint32_t interrupt) { - TIM_HandleTypeDef * Handle = timerFindTimerHandle(tim); - if (Handle == NULL) { - return; - } + __HAL_TIM_DISABLE_IT(tch->timCtx->timHandle, interrupt); +} - __HAL_TIM_CLEAR_FLAG(Handle, flag); +void impl_timerClearFlag(TCH_t * tch, uint32_t flag) +{ + __HAL_TIM_CLEAR_FLAG(tch->timCtx->timHandle, flag); } // calculate input filter constant @@ -147,29 +187,28 @@ static unsigned getFilter(unsigned ticks) 16*5, 16*6, 16*8, 32*5, 32*6, 32*8 }; - for (unsigned i = 1; i < ARRAYLEN(ftab); i++) - if (ftab[i] > ticks) + + for (unsigned i = 1; i < ARRAYLEN(ftab); i++) { + if (ftab[i] > ticks) { return i - 1; + } + } + return 0x0f; } -void impl_timerChConfigIC(const timerHardware_t *timHw, bool polarityRising, unsigned inputFilterTicks) +void impl_timerChConfigIC(TCH_t * tch, bool polarityRising, unsigned inputFilterTicks) { - TIM_HandleTypeDef * Handle = timerFindTimerHandle(timHw->tim); - if (Handle == NULL) { - return; - } - TIM_IC_InitTypeDef TIM_ICInitStructure; TIM_ICInitStructure.ICPolarity = polarityRising ? TIM_ICPOLARITY_RISING : TIM_ICPOLARITY_FALLING; TIM_ICInitStructure.ICSelection = TIM_ICSELECTION_DIRECTTI; TIM_ICInitStructure.ICPrescaler = TIM_ICPSC_DIV1; TIM_ICInitStructure.ICFilter = getFilter(inputFilterTicks); - HAL_TIM_IC_ConfigChannel(Handle,&TIM_ICInitStructure, timHw->channel); + HAL_TIM_IC_ConfigChannel(tch->timCtx->timHandle, &TIM_ICInitStructure, lookupTIMChannelTable[tch->timHw->channelIndex]); } -void impl_timerCaptureCompareHandler(TIM_TypeDef *tim, timerConfig_t * timerConfig) +void impl_timerCaptureCompareHandler(TIM_TypeDef *tim, timHardwareContext_t *timerCtx) { unsigned tim_status = tim->SR & tim->DIER; @@ -181,28 +220,35 @@ void impl_timerCaptureCompareHandler(TIM_TypeDef *tim, timerConfig_t * timerConf tim->SR = mask; tim_status &= mask; - if (timerConfig) { + if (timerCtx) { switch (bit) { case __builtin_clz(TIM_IT_UPDATE): { const uint16_t capture = tim->ARR; - timerOvrHandlerRec_t *cb = timerConfig->overflowCallbackActive; - while (cb) { - cb->fn(cb, capture); - cb = cb->next; + if (timerCtx->ch[0].cb && timerCtx->ch[0].cb->callbackOvr) { + timerCtx->ch[0].cb->callbackOvr(&timerCtx->ch[0], capture); + } + if (timerCtx->ch[1].cb && timerCtx->ch[1].cb->callbackOvr) { + timerCtx->ch[1].cb->callbackOvr(&timerCtx->ch[1], capture); + } + if (timerCtx->ch[2].cb && timerCtx->ch[2].cb->callbackOvr) { + timerCtx->ch[2].cb->callbackOvr(&timerCtx->ch[2], capture); + } + if (timerCtx->ch[3].cb && timerCtx->ch[3].cb->callbackOvr) { + timerCtx->ch[3].cb->callbackOvr(&timerCtx->ch[3], capture); } break; } case __builtin_clz(TIM_IT_CC1): - timerConfig->edgeCallback[0]->fn(timerConfig->edgeCallback[0], tim->CCR1); + timerCtx->ch[0].cb->callbackEdge(&timerCtx->ch[0], tim->CCR1); break; case __builtin_clz(TIM_IT_CC2): - timerConfig->edgeCallback[1]->fn(timerConfig->edgeCallback[1], tim->CCR2); + timerCtx->ch[1].cb->callbackEdge(&timerCtx->ch[1], tim->CCR2); break; case __builtin_clz(TIM_IT_CC3): - timerConfig->edgeCallback[2]->fn(timerConfig->edgeCallback[2], tim->CCR3); + timerCtx->ch[2].cb->callbackEdge(&timerCtx->ch[2], tim->CCR3); break; case __builtin_clz(TIM_IT_CC4): - timerConfig->edgeCallback[3]->fn(timerConfig->edgeCallback[3], tim->CCR4); + timerCtx->ch[3].cb->callbackEdge(&timerCtx->ch[3], tim->CCR4); break; } } @@ -233,58 +279,156 @@ void impl_timerCaptureCompareHandler(TIM_TypeDef *tim, timerConfig_t * timerConf } } -void impl_timerPWMConfigChannel(TIM_TypeDef * tim, uint8_t channel, bool isNChannel, bool inverted, uint16_t value) +void impl_timerChCaptureCompareEnable(TCH_t * tch, bool enable) { - UNUSED(isNChannel); + static const uint32_t lookupTIMLLChannelTable[] = { LL_TIM_CHANNEL_CH1, LL_TIM_CHANNEL_CH2, LL_TIM_CHANNEL_CH3, LL_TIM_CHANNEL_CH4 }; - TIM_HandleTypeDef * Handle = timerFindTimerHandle(tim); - if (Handle == NULL) { - return; + if (enable) { + LL_TIM_CC_EnableChannel(tch->timHw->tim, lookupTIMLLChannelTable[tch->timHw->channelIndex]); + } + else { + LL_TIM_CC_DisableChannel(tch->timHw->tim, lookupTIMLLChannelTable[tch->timHw->channelIndex]); } +} - TIM_OC_InitTypeDef TIM_OCInitStructure; +// HAL_LL additionan implementation for enabling multiple DMA channels in one operation +static inline void LL_TIM_EnableDMAReq_CCx(TIM_TypeDef * TIMx, uint16_t dmaSources) +{ + SET_BIT(TIMx->DIER, dmaSources & (TIM_DMA_CC1 | TIM_DMA_CC2 | TIM_DMA_CC3 | TIM_DMA_CC4)); +} - TIM_OCInitStructure.OCMode = TIM_OCMODE_PWM1; - TIM_OCInitStructure.OCIdleState = TIM_OCIDLESTATE_SET; - TIM_OCInitStructure.OCPolarity = inverted ? TIM_OCPOLARITY_LOW : TIM_OCPOLARITY_HIGH; - TIM_OCInitStructure.OCNIdleState = TIM_OCNIDLESTATE_SET; - TIM_OCInitStructure.OCNPolarity = inverted ? TIM_OCNPOLARITY_LOW : TIM_OCNPOLARITY_HIGH; - TIM_OCInitStructure.Pulse = value; - TIM_OCInitStructure.OCFastMode = TIM_OCFAST_DISABLE; +static inline void LL_TIM_DisableDMAReq_CCx(TIM_TypeDef * TIMx, uint16_t dmaSources) +{ + CLEAR_BIT(TIMx->DIER, dmaSources & (TIM_DMA_CC1 | TIM_DMA_CC2 | TIM_DMA_CC3 | TIM_DMA_CC4)); +} + +static void impl_timerDMA_IRQHandler(DMA_t descriptor) +{ + if (DMA_GET_FLAG_STATUS(descriptor, DMA_IT_TCIF)) { + TCH_t * tch = (TCH_t *)descriptor->userParam; + + // If it was ACTIVE - switch to IDLE + if (tch->dmaState == TCH_DMA_ACTIVE) { + tch->dmaState = TCH_DMA_IDLE; + } + + LL_DMA_DisableStream(tch->dma->dma, lookupDMALLStreamTable[DMATAG_GET_STREAM(tch->timHw->dmaTag)]); + LL_TIM_DisableDMAReq_CCx(tch->timHw->tim, lookupDMASourceTable[tch->timHw->channelIndex]); + + DMA_CLEAR_FLAG(descriptor, DMA_IT_TCIF); + } +} + +bool impl_timerPWMConfigChannelDMA(TCH_t * tch, void * dmaBuffer, uint32_t dmaBufferSize) +{ + tch->dma = dmaGetByTag(tch->timHw->dmaTag); + tch->dmaBuffer = dmaBuffer; + if (tch->dma == NULL) { + return false; + } + + // If DMA is already in use - abort + if (dmaGetOwner(tch->dma) != OWNER_FREE) { + return false; + } - HAL_TIM_PWM_ConfigChannel(Handle, &TIM_OCInitStructure, channel); + // We assume that timer channels are already initialized by calls to: + // timerConfigBase + // timerPWMConfigChannel + const uint32_t streamLL = lookupDMALLStreamTable[DMATAG_GET_STREAM(tch->timHw->dmaTag)]; + const uint32_t channelLL = lookupDMALLChannelTable[DMATAG_GET_CHANNEL(tch->timHw->dmaTag)]; + + LL_DMA_DeInit(tch->dma->dma, streamLL); + + LL_DMA_InitTypeDef init; + LL_DMA_StructInit(&init); + + init.Channel = channelLL; + init.PeriphOrM2MSrcAddress = (uint32_t)impl_timerCCR(tch); + init.PeriphOrM2MSrcIncMode = LL_DMA_PERIPH_NOINCREMENT; + init.PeriphOrM2MSrcDataSize = LL_DMA_PDATAALIGN_WORD; + init.MemoryOrM2MDstAddress = (uint32_t)dmaBuffer; + init.MemoryOrM2MDstIncMode = LL_DMA_MEMORY_INCREMENT; + init.MemoryOrM2MDstDataSize = LL_DMA_MDATAALIGN_WORD; + init.NbData = dmaBufferSize; + init.Direction = LL_DMA_DIRECTION_MEMORY_TO_PERIPH; + init.Mode = LL_DMA_MODE_NORMAL; + init.Priority = LL_DMA_PRIORITY_HIGH; + init.FIFOMode = LL_DMA_FIFOMODE_ENABLE; + init.FIFOThreshold = LL_DMA_FIFOTHRESHOLD_FULL; + init.MemBurst = LL_DMA_MBURST_SINGLE; + init.PeriphBurst = LL_DMA_PBURST_SINGLE; + + dmaInit(tch->dma, OWNER_TIMER, 0); + dmaSetHandler(tch->dma, impl_timerDMA_IRQHandler, NVIC_PRIO_WS2811_DMA, (uint32_t)tch); + + LL_DMA_Init(tch->dma->dma, streamLL, &init); + + // Start PWM generation + if (tch->timHw->output & TIMER_OUTPUT_N_CHANNEL) { + HAL_TIMEx_PWMN_Start(tch->timCtx->timHandle, lookupTIMChannelTable[tch->timHw->channelIndex]); + } + else { + HAL_TIM_PWM_Start(tch->timCtx->timHandle, lookupTIMChannelTable[tch->timHw->channelIndex]); + } + + return true; } -uint16_t impl_timerDmaSource(uint8_t channel) +void impl_timerPWMPrepareDMA(TCH_t * tch, uint32_t dmaBufferSize) { - switch (channel) { - case TIM_CHANNEL_1: - return TIM_DMA_ID_CC1; - case TIM_CHANNEL_2: - return TIM_DMA_ID_CC2; - case TIM_CHANNEL_3: - return TIM_DMA_ID_CC3; - case TIM_CHANNEL_4: - return TIM_DMA_ID_CC4; + const uint32_t streamLL = lookupDMALLStreamTable[DMATAG_GET_STREAM(tch->timHw->dmaTag)]; + DMA_TypeDef * dmaBase = tch->dma->dma; + + // Make sure we terminate any DMA transaction currently in progress + // Clear the flag as well, so even if DMA transfer finishes while within ATOMIC_BLOCK + // the resulting IRQ won't mess up the DMA state + ATOMIC_BLOCK(NVIC_PRIO_MAX) { + LL_TIM_DisableDMAReq_CCx(tch->timHw->tim, lookupDMASourceTable[tch->timHw->channelIndex]); + LL_DMA_DisableStream(dmaBase, streamLL); + DMA_CLEAR_FLAG(tch->dma, DMA_IT_TCIF); } - return 0; + + LL_DMA_SetDataLength(dmaBase, streamLL, dmaBufferSize); + LL_DMA_ConfigAddresses(dmaBase, streamLL, (uint32_t)tch->dmaBuffer, (uint32_t)impl_timerCCR(tch), LL_DMA_DIRECTION_MEMORY_TO_PERIPH); + LL_DMA_EnableIT_TC(dmaBase, streamLL); + LL_DMA_EnableStream(dmaBase, streamLL); + tch->dmaState = TCH_DMA_READY; } -volatile timCCR_t * impl_timerCCR(TIM_TypeDef *tim, uint8_t channel) +void impl_timerPWMStartDMA(TCH_t * tch) { - switch (channel) { - case TIM_CHANNEL_1: - return &tim->CCR1; - break; - case TIM_CHANNEL_2: - return &tim->CCR2; - break; - case TIM_CHANNEL_3: - return &tim->CCR3; - break; - case TIM_CHANNEL_4: - return &tim->CCR4; - break; + uint16_t dmaSources = 0; + timHardwareContext_t * timCtx = tch->timCtx; + + if (timCtx->ch[0].dmaState == TCH_DMA_READY) { + timCtx->ch[0].dmaState = TCH_DMA_ACTIVE; + dmaSources |= TIM_DMA_CC1; } - return NULL; -} \ No newline at end of file + + if (timCtx->ch[1].dmaState == TCH_DMA_READY) { + timCtx->ch[1].dmaState = TCH_DMA_ACTIVE; + dmaSources |= TIM_DMA_CC2; + } + + if (timCtx->ch[2].dmaState == TCH_DMA_READY) { + timCtx->ch[2].dmaState = TCH_DMA_ACTIVE; + dmaSources |= TIM_DMA_CC3; + } + + if (timCtx->ch[3].dmaState == TCH_DMA_READY) { + timCtx->ch[3].dmaState = TCH_DMA_ACTIVE; + dmaSources |= TIM_DMA_CC4; + } + + if (dmaSources) { + LL_TIM_SetCounter(timCtx->timDef->tim, 0); + LL_TIM_EnableDMAReq_CCx(timCtx->timDef->tim, dmaSources); + } +} + +void impl_timerPWMStopDMA(TCH_t * tch) +{ + (void)tch; + // FIXME +} diff --git a/src/main/drivers/timer_impl_stdperiph.c b/src/main/drivers/timer_impl_stdperiph.c index ae076c12de3..17ccae91c22 100644 --- a/src/main/drivers/timer_impl_stdperiph.c +++ b/src/main/drivers/timer_impl_stdperiph.c @@ -1,9 +1,19 @@ /* - modified version of StdPeriph function is located here. - TODO - what license does apply here? - original file was lincesed under MCD-ST Liberty SW License Agreement V2 - http://www.st.com/software_license_agreement_liberty_v2 -*/ + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + */ #include #include @@ -11,6 +21,7 @@ #include "platform.h" #include "build/atomic.h" +#include "build/debug.h" #include "common/utils.h" @@ -18,57 +29,73 @@ #include "drivers/rcc.h" #include "drivers/time.h" #include "drivers/nvic.h" +#include "drivers/dma.h" #include "drivers/timer.h" #include "drivers/timer_impl.h" -void impl_timerNVICConfigure(uint8_t irq, int irqPriority) +const uint16_t lookupDMASourceTable[4] = { TIM_DMA_CC1, TIM_DMA_CC2, TIM_DMA_CC3, TIM_DMA_CC4 }; +const uint8_t lookupTIMChannelTable[4] = { TIM_Channel_1, TIM_Channel_2, TIM_Channel_3, TIM_Channel_4 }; + +void impl_timerInitContext(timHardwareContext_t * timCtx) +{ + (void)timCtx; // NoOp +} + +void impl_timerNVICConfigure(TCH_t * tch, int irqPriority) { NVIC_InitTypeDef NVIC_InitStructure; - NVIC_InitStructure.NVIC_IRQChannel = irq; NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(irqPriority); NVIC_InitStructure.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(irqPriority); NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; - NVIC_Init(&NVIC_InitStructure); + + if (tch->timCtx->timDef->irq) { + NVIC_InitStructure.NVIC_IRQChannel = tch->timCtx->timDef->irq; + NVIC_Init(&NVIC_InitStructure); + } + + if (tch->timCtx->timDef->secondIrq) { + NVIC_InitStructure.NVIC_IRQChannel = tch->timCtx->timDef->secondIrq; + NVIC_Init(&NVIC_InitStructure); + } } -void impl_timerConfigBase(TIM_TypeDef *tim, uint16_t period, uint8_t mhz) +void impl_timerConfigBase(TCH_t * tch, uint16_t period, uint32_t hz) { + TIM_TypeDef * tim = tch->timCtx->timDef->tim; TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; TIM_TimeBaseStructInit(&TIM_TimeBaseStructure); TIM_TimeBaseStructure.TIM_Period = (period - 1) & 0xffff; // AKA TIMx_ARR - TIM_TimeBaseStructure.TIM_Prescaler = timerGetPrescalerByDesiredMhz(tim, mhz); + TIM_TimeBaseStructure.TIM_Prescaler = (timerGetBaseClock(tch) / hz) - 1; TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1; TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; TIM_TimeBaseInit(tim, &TIM_TimeBaseStructure); } -void impl_enableTimer(TIM_TypeDef * tim) +void impl_enableTimer(TCH_t * tch) { - TIM_Cmd(tim, ENABLE); + TIM_Cmd(tch->timHw->tim, ENABLE); } -void impl_timerPWMStart(TIM_TypeDef * tim, unsigned channel, bool isNChannel) +void impl_timerPWMStart(TCH_t * tch) { - UNUSED(channel); - UNUSED(isNChannel); - TIM_CtrlPWMOutputs(tim, ENABLE); + TIM_CtrlPWMOutputs(tch->timHw->tim, ENABLE); } -void impl_timerEnableIT(TIM_TypeDef * tim, uint32_t interrupt) +void impl_timerEnableIT(TCH_t * tch, uint32_t interrupt) { - TIM_ITConfig(tim, interrupt, ENABLE); + TIM_ITConfig(tch->timHw->tim, interrupt, ENABLE); } -void impl_timerDisableIT(TIM_TypeDef * tim, uint32_t interrupt) +void impl_timerDisableIT(TCH_t * tch, uint32_t interrupt) { - TIM_ITConfig(tim, interrupt, DISABLE); + TIM_ITConfig(tch->timHw->tim, interrupt, DISABLE); } -void impl_timerClearFlag(TIM_TypeDef * tim, uint32_t flag) +void impl_timerClearFlag(TCH_t * tch, uint32_t flag) { - TIM_ClearFlag(tim, flag); + TIM_ClearFlag(tch->timHw->tim, flag); } // calculate input filter constant @@ -83,27 +110,31 @@ static unsigned getFilter(unsigned ticks) 16*5, 16*6, 16*8, 32*5, 32*6, 32*8 }; - for (unsigned i = 1; i < ARRAYLEN(ftab); i++) - if (ftab[i] > ticks) + + for (unsigned i = 1; i < ARRAYLEN(ftab); i++) { + if (ftab[i] > ticks) { return i - 1; + } + } + return 0x0f; } -void impl_timerChConfigIC(const timerHardware_t *timHw, bool polarityRising, unsigned inputFilterTicks) +void impl_timerChConfigIC(TCH_t * tch, bool polarityRising, unsigned inputFilterTicks) { TIM_ICInitTypeDef TIM_ICInitStructure; TIM_ICStructInit(&TIM_ICInitStructure); - TIM_ICInitStructure.TIM_Channel = timHw->channel; + TIM_ICInitStructure.TIM_Channel = lookupTIMChannelTable[tch->timHw->channelIndex]; TIM_ICInitStructure.TIM_ICPolarity = polarityRising ? TIM_ICPolarity_Rising : TIM_ICPolarity_Falling; TIM_ICInitStructure.TIM_ICSelection = TIM_ICSelection_DirectTI; TIM_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1; TIM_ICInitStructure.TIM_ICFilter = getFilter(inputFilterTicks); - TIM_ICInit(timHw->tim, &TIM_ICInitStructure); + TIM_ICInit(tch->timHw->tim, &TIM_ICInitStructure); } -void impl_timerCaptureCompareHandler(TIM_TypeDef *tim, timerConfig_t *timerConfig) +void impl_timerCaptureCompareHandler(TIM_TypeDef *tim, timHardwareContext_t *timerCtx) { unsigned tim_status = tim->SR & tim->DIER; @@ -115,28 +146,35 @@ void impl_timerCaptureCompareHandler(TIM_TypeDef *tim, timerConfig_t *timerConfi tim->SR = mask; tim_status &= mask; - if (timerConfig) { + if (timerCtx) { switch (bit) { case __builtin_clz(TIM_IT_Update): { const uint16_t capture = tim->ARR; - timerOvrHandlerRec_t *cb = timerConfig->overflowCallbackActive; - while (cb) { - cb->fn(cb, capture); - cb = cb->next; + if (timerCtx->ch[0].cb && timerCtx->ch[0].cb->callbackOvr) { + timerCtx->ch[0].cb->callbackOvr(&timerCtx->ch[0], capture); + } + if (timerCtx->ch[1].cb && timerCtx->ch[1].cb->callbackOvr) { + timerCtx->ch[1].cb->callbackOvr(&timerCtx->ch[1], capture); + } + if (timerCtx->ch[2].cb && timerCtx->ch[2].cb->callbackOvr) { + timerCtx->ch[2].cb->callbackOvr(&timerCtx->ch[2], capture); + } + if (timerCtx->ch[3].cb && timerCtx->ch[3].cb->callbackOvr) { + timerCtx->ch[3].cb->callbackOvr(&timerCtx->ch[3], capture); } break; } case __builtin_clz(TIM_IT_CC1): - timerConfig->edgeCallback[0]->fn(timerConfig->edgeCallback[0], tim->CCR1); + timerCtx->ch[0].cb->callbackEdge(&timerCtx->ch[0], tim->CCR1); break; case __builtin_clz(TIM_IT_CC2): - timerConfig->edgeCallback[1]->fn(timerConfig->edgeCallback[1], tim->CCR2); + timerCtx->ch[1].cb->callbackEdge(&timerCtx->ch[1], tim->CCR2); break; case __builtin_clz(TIM_IT_CC3): - timerConfig->edgeCallback[2]->fn(timerConfig->edgeCallback[2], tim->CCR3); + timerCtx->ch[2].cb->callbackEdge(&timerCtx->ch[2], tim->CCR3); break; case __builtin_clz(TIM_IT_CC4): - timerConfig->edgeCallback[3]->fn(timerConfig->edgeCallback[3], tim->CCR4); + timerCtx->ch[3].cb->callbackEdge(&timerCtx->ch[3], tim->CCR4); break; } } @@ -167,15 +205,17 @@ void impl_timerCaptureCompareHandler(TIM_TypeDef *tim, timerConfig_t *timerConfi } } -void impl_timerPWMConfigChannel(TIM_TypeDef * tim, uint8_t channel, bool isNChannel, bool inverted, uint16_t value) +void impl_timerPWMConfigChannel(TCH_t * tch, uint16_t value) { + const bool inverted = tch->timHw->output & TIMER_OUTPUT_INVERTED; + TIM_OCInitTypeDef TIM_OCInitStructure; TIM_OCStructInit(&TIM_OCInitStructure); TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1; TIM_OCInitStructure.TIM_Pulse = value; - if (isNChannel) { + if (tch->timHw->output & TIMER_OUTPUT_N_CHANNEL) { TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Disable; TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Enable; TIM_OCInitStructure.TIM_OCNPolarity = inverted ? TIM_OCPolarity_Low : TIM_OCPolarity_High; @@ -187,56 +227,174 @@ void impl_timerPWMConfigChannel(TIM_TypeDef * tim, uint8_t channel, bool isNChan TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Set; } - switch (channel) { - case TIM_Channel_1: - TIM_OC1Init(tim, &TIM_OCInitStructure); - TIM_OC1PreloadConfig(tim, TIM_OCPreload_Enable); + switch (tch->timHw->channelIndex) { + case 0: + TIM_OC1Init(tch->timHw->tim, &TIM_OCInitStructure); + TIM_OC1PreloadConfig(tch->timHw->tim, TIM_OCPreload_Enable); break; - case TIM_Channel_2: - TIM_OC2Init(tim, &TIM_OCInitStructure); - TIM_OC2PreloadConfig(tim, TIM_OCPreload_Enable); + case 1: + TIM_OC2Init(tch->timHw->tim, &TIM_OCInitStructure); + TIM_OC2PreloadConfig(tch->timHw->tim, TIM_OCPreload_Enable); break; - case TIM_Channel_3: - TIM_OC3Init(tim, &TIM_OCInitStructure); - TIM_OC3PreloadConfig(tim, TIM_OCPreload_Enable); + case 2: + TIM_OC3Init(tch->timHw->tim, &TIM_OCInitStructure); + TIM_OC3PreloadConfig(tch->timHw->tim, TIM_OCPreload_Enable); break; - case TIM_Channel_4: - TIM_OC4Init(tim, &TIM_OCInitStructure); - TIM_OC4PreloadConfig(tim, TIM_OCPreload_Enable); + case 3: + TIM_OC4Init(tch->timHw->tim, &TIM_OCInitStructure); + TIM_OC4PreloadConfig(tch->timHw->tim, TIM_OCPreload_Enable); break; } } -uint16_t impl_timerDmaSource(uint8_t channel) -{ - switch (channel) { - case TIM_Channel_1: - return TIM_DMA_CC1; - case TIM_Channel_2: - return TIM_DMA_CC2; - case TIM_Channel_3: - return TIM_DMA_CC3; - case TIM_Channel_4: - return TIM_DMA_CC4; - } - return 0; -} - -volatile timCCR_t * impl_timerCCR(TIM_TypeDef *tim, uint8_t channel) +volatile timCCR_t * impl_timerCCR(TCH_t * tch) { - switch (channel) { - case TIM_Channel_1: - return &tim->CCR1; + switch (tch->timHw->channelIndex) { + case 0: + return &tch->timHw->tim->CCR1; break; - case TIM_Channel_2: - return &tim->CCR2; + case 1: + return &tch->timHw->tim->CCR2; break; - case TIM_Channel_3: - return &tim->CCR3; + case 2: + return &tch->timHw->tim->CCR3; break; - case TIM_Channel_4: - return &tim->CCR4; + case 3: + return &tch->timHw->tim->CCR4; break; } return NULL; -} \ No newline at end of file +} + +void impl_timerChCaptureCompareEnable(TCH_t * tch, bool enable) +{ + TIM_CCxCmd(tch->timHw->tim, lookupTIMChannelTable[tch->timHw->channelIndex], (enable ? TIM_CCx_Enable : TIM_CCx_Disable)); +} + +static void impl_timerDMA_IRQHandler(DMA_t descriptor) +{ + if (DMA_GET_FLAG_STATUS(descriptor, DMA_IT_TCIF)) { + TCH_t * tch = (TCH_t *)descriptor->userParam; + tch->dmaState = TCH_DMA_IDLE; + + DMA_Cmd(tch->dma->ref, DISABLE); + TIM_DMACmd(tch->timHw->tim, lookupDMASourceTable[tch->timHw->channelIndex], DISABLE); + + DMA_CLEAR_FLAG(descriptor, DMA_IT_TCIF); + } +} + +bool impl_timerPWMConfigChannelDMA(TCH_t * tch, void * dmaBuffer, uint32_t dmaBufferSize) +{ + DMA_InitTypeDef DMA_InitStructure; + TIM_TypeDef * timer = tch->timHw->tim; + + tch->dma = dmaGetByTag(tch->timHw->dmaTag); + if (tch->dma == NULL) { + return false; + } + + // If DMA is already in use - abort + if (tch->dma->owner != OWNER_FREE) { + return false; + } + + // We assume that timer channels are already initialized by calls to: + // timerConfigBase + // timerPWMConfigChannel + + TIM_CtrlPWMOutputs(timer, ENABLE); + TIM_ARRPreloadConfig(timer, ENABLE); + + TIM_CCxCmd(timer, lookupTIMChannelTable[tch->timHw->channelIndex], TIM_CCx_Enable); + TIM_Cmd(timer, ENABLE); + + dmaInit(tch->dma, OWNER_TIMER, 0); + dmaSetHandler(tch->dma, impl_timerDMA_IRQHandler, NVIC_PRIO_WS2811_DMA, (uint32_t)tch); + + DMA_DeInit(tch->dma->ref); + DMA_Cmd(tch->dma->ref, DISABLE); + + DMA_DeInit(tch->dma->ref); + DMA_StructInit(&DMA_InitStructure); + + DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)impl_timerCCR(tch); + DMA_InitStructure.DMA_BufferSize = dmaBufferSize; + DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable; + DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable; + DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Word; + DMA_InitStructure.DMA_Mode = DMA_Mode_Normal; + +#ifdef STM32F4 + DMA_InitStructure.DMA_Channel = dmaGetChannelByTag(tch->timHw->dmaTag); + DMA_InitStructure.DMA_Memory0BaseAddr = (uint32_t)dmaBuffer; + DMA_InitStructure.DMA_DIR = DMA_DIR_MemoryToPeripheral; + DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Word; + DMA_InitStructure.DMA_Priority = DMA_Priority_High; +#else // F3 + DMA_InitStructure.DMA_MemoryBaseAddr = (uint32_t)dmaBuffer; + DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralDST; + DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Word; + DMA_InitStructure.DMA_Priority = DMA_Priority_High; + DMA_InitStructure.DMA_M2M = DMA_M2M_Disable; +#endif + + DMA_Init(tch->dma->ref, &DMA_InitStructure); + DMA_ITConfig(tch->dma->ref, DMA_IT_TC, ENABLE); + + return true; +} + +void impl_timerPWMPrepareDMA(TCH_t * tch, uint32_t dmaBufferSize) +{ + // Make sure we terminate any DMA transaction currently in progress + // Clear the flag as well, so even if DMA transfer finishes while within ATOMIC_BLOCK + // the resulting IRQ won't mess up the DMA state + ATOMIC_BLOCK(NVIC_PRIO_MAX) { + DMA_Cmd(tch->dma->ref, DISABLE); + TIM_DMACmd(tch->timHw->tim, lookupDMASourceTable[tch->timHw->channelIndex], DISABLE); + DMA_CLEAR_FLAG(tch->dma, DMA_IT_TCIF); + } + + DMA_SetCurrDataCounter(tch->dma->ref, dmaBufferSize); + DMA_Cmd(tch->dma->ref, ENABLE); + tch->dmaState = TCH_DMA_READY; +} + +void impl_timerPWMStartDMA(TCH_t * tch) +{ + uint16_t dmaSources = 0; + timHardwareContext_t * timCtx = tch->timCtx; + + if (timCtx->ch[0].dmaState == TCH_DMA_READY) { + timCtx->ch[0].dmaState = TCH_DMA_ACTIVE; + dmaSources |= TIM_DMA_CC1; + } + + if (timCtx->ch[1].dmaState == TCH_DMA_READY) { + timCtx->ch[1].dmaState = TCH_DMA_ACTIVE; + dmaSources |= TIM_DMA_CC2; + } + + if (timCtx->ch[2].dmaState == TCH_DMA_READY) { + timCtx->ch[2].dmaState = TCH_DMA_ACTIVE; + dmaSources |= TIM_DMA_CC3; + } + + if (timCtx->ch[3].dmaState == TCH_DMA_READY) { + timCtx->ch[3].dmaState = TCH_DMA_ACTIVE; + dmaSources |= TIM_DMA_CC4; + } + + if (dmaSources) { + TIM_SetCounter(tch->timHw->tim, 0); + TIM_DMACmd(tch->timHw->tim, dmaSources, ENABLE); + } +} + +void impl_timerPWMStopDMA(TCH_t * tch) +{ + DMA_Cmd(tch->dma->ref, DISABLE); + TIM_DMACmd(tch->timHw->tim, lookupDMASourceTable[tch->timHw->channelIndex], DISABLE); + TIM_Cmd(tch->timHw->tim, ENABLE); +} diff --git a/src/main/drivers/timer_stm32f30x.c b/src/main/drivers/timer_stm32f30x.c index 5a2647695ec..5aaa4df2055 100644 --- a/src/main/drivers/timer_stm32f30x.c +++ b/src/main/drivers/timer_stm32f30x.c @@ -1,10 +1,20 @@ /* - modified version of StdPeriph function is located here. - TODO - what license does apply here? - original file was lincesed under MCD-ST Liberty SW License Agreement V2 - http://www.st.com/software_license_agreement_liberty_v2 -*/ - + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + */ + #include #include diff --git a/src/main/drivers/timer_stm32f30x.h b/src/main/drivers/timer_stm32f30x.h deleted file mode 100644 index 78138587589..00000000000 --- a/src/main/drivers/timer_stm32f30x.h +++ /dev/null @@ -1,6 +0,0 @@ - -#pragma once - -#include "stm32f30x.h" - -void TIM_SelectOCxM_NoDisable(TIM_TypeDef* TIMx, uint16_t TIM_Channel, uint16_t TIM_OCMode); diff --git a/src/main/drivers/timer_stm32f4xx.c b/src/main/drivers/timer_stm32f4xx.c index 041178ed35d..75b46f1cf8f 100644 --- a/src/main/drivers/timer_stm32f4xx.c +++ b/src/main/drivers/timer_stm32f4xx.c @@ -1,9 +1,19 @@ /* - modified version of StdPeriph function is located here. - TODO - what license does apply here? - original file was lincesed under MCD-ST Liberty SW License Agreement V2 - http://www.st.com/software_license_agreement_liberty_v2 -*/ + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + */ #include #include diff --git a/src/main/drivers/timer_stm32f4xx.h b/src/main/drivers/timer_stm32f4xx.h deleted file mode 100644 index 52094cbc03c..00000000000 --- a/src/main/drivers/timer_stm32f4xx.h +++ /dev/null @@ -1,6 +0,0 @@ - -#pragma once - -#include "stm32f4xx.h" - -void TIM_SelectOCxM_NoDisable(TIM_TypeDef* TIMx, uint16_t TIM_Channel, uint16_t TIM_OCMode); diff --git a/src/main/drivers/timer_stm32f7xx.c b/src/main/drivers/timer_stm32f7xx.c index fa1253e461f..4f8057baf1a 100644 --- a/src/main/drivers/timer_stm32f7xx.c +++ b/src/main/drivers/timer_stm32f7xx.c @@ -1,9 +1,19 @@ /* - modified version of StdPeriph function is located here. - TODO - what license does apply here? - original file was lincesed under MCD-ST Liberty SW License Agreement V2 - http://www.st.com/software_license_agreement_liberty_v2 -*/ + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + */ #include #include diff --git a/src/main/drivers/timer_stm32f7xx.h b/src/main/drivers/timer_stm32f7xx.h deleted file mode 100644 index 286291aab87..00000000000 --- a/src/main/drivers/timer_stm32f7xx.h +++ /dev/null @@ -1,6 +0,0 @@ - -#pragma once - -#include "stm32f7xx.h" - -void TIM_SelectOCxM_NoDisable(TIM_TypeDef* TIMx, uint16_t TIM_Channel, uint16_t TIM_OCMode); diff --git a/src/main/drivers/vtx_common.c b/src/main/drivers/vtx_common.c index d916f03bc85..34b2cf27e80 100644 --- a/src/main/drivers/vtx_common.c +++ b/src/main/drivers/vtx_common.c @@ -25,8 +25,6 @@ #include "platform.h" #include "build/debug.h" -#if defined(USE_VTX_COMMON) - #include "vtx_common.h" static vtxDevice_t *commonVtxDevice = NULL; @@ -152,4 +150,3 @@ bool vtxCommonGetDeviceCapability(vtxDevice_t *vtxDevice, vtxDeviceCapability_t } return false; } -#endif diff --git a/src/main/drivers/vtx_common.h b/src/main/drivers/vtx_common.h index bd98501cb86..7e7ee7c9c24 100644 --- a/src/main/drivers/vtx_common.h +++ b/src/main/drivers/vtx_common.h @@ -51,17 +51,17 @@ #define VTX_SETTINGS_MIN_USER_FREQ 5000 #define VTX_SETTINGS_MAX_USER_FREQ 5999 #define VTX_SETTINGS_FREQCMD +#define VTX_SETTINGS_MAX_POWER (VTX_SETTINGS_POWER_COUNT - VTX_SETTINGS_MIN_POWER + 1) #elif defined(USE_VTX_RTC6705) #define VTX_SETTINGS_POWER_COUNT VTX_RTC6705_POWER_COUNT #define VTX_SETTINGS_DEFAULT_POWER VTX_RTC6705_DEFAULT_POWER #define VTX_SETTINGS_MIN_POWER VTX_RTC6705_MIN_POWER +#define VTX_SETTINGS_MAX_POWER (VTX_SETTINGS_POWER_COUNT - 1) #endif -#define VTX_SETTINGS_MAX_POWER (VTX_SETTINGS_POWER_COUNT - 1) - // check value for MSP_SET_VTX_CONFIG to determine if value is encoded // band/channel or frequency in MHz (3 bits for band and 3 bits for channel) #define VTXCOMMON_MSP_BANDCHAN_CHKVAL ((uint16_t)((7 << 3) + 7)) @@ -72,6 +72,7 @@ typedef enum { // 2 reserved VTXDEV_SMARTAUDIO = 3, VTXDEV_TRAMP = 4, + VTXDEV_FFPV = 5, VTXDEV_UNKNOWN = 0xFF, } vtxDevType_e; diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index cc5b44d43f7..202c2abfc66 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -56,6 +56,7 @@ extern uint8_t __config_end; #include "drivers/io.h" #include "drivers/io_impl.h" #include "drivers/logging.h" +#include "drivers/max7456_symbols.h" #include "drivers/rx_pwm.h" #include "drivers/sdcard.h" #include "drivers/sensor.h" @@ -108,6 +109,7 @@ extern uint8_t __config_end; #include "sensors/rangefinder.h" #include "sensors/opflow.h" #include "sensors/sensors.h" +#include "sensors/temperature.h" #include "telemetry/frsky_d.h" #include "telemetry/telemetry.h" @@ -117,6 +119,10 @@ extern uint8_t __config_end; #define PLAY_SOUND #endif +#ifndef USE_MAX7456 +#define TEMP_SENSOR_SYM_COUNT 0 +#endif + extern timeDelta_t cycleTime; // FIXME dependency on mw.c extern uint8_t detectedSensors[SENSOR_INDEX_COUNT]; @@ -149,7 +155,7 @@ static const char * const featureNames[] = { /* Sensor names (used in lookup tables for *_hardware settings and in status command output) */ // sync with gyroSensor_e -static const char * const gyroNames[] = { "NONE", "AUTO", "MPU6050", "L3G4200D", "MPU3050", "L3GD20", "MPU6000", "MPU6500", "MPU9250", "BMI160", "FAKE"}; +static const char * const gyroNames[] = { "NONE", "AUTO", "MPU6050", "L3G4200D", "MPU3050", "L3GD20", "MPU6000", "MPU6500", "MPU9250", "BMI160", "ICM20689", "FAKE"}; // sync this with sensors_e static const char * const sensorTypeNames[] = { @@ -748,7 +754,7 @@ static void cliSerial(char *cmdline) ptr = nextArg(ptr); if (ptr) { val = fastA2I(ptr); - portConfig.functionMask = val & 0xFFFF; + portConfig.functionMask = val & 0xFFFFFFFF; validArgumentCount++; } @@ -1126,6 +1132,121 @@ static void cliRxRange(char *cmdline) } } +#ifdef USE_TEMPERATURE_SENSOR +static void printTempSensor(uint8_t dumpMask, const tempSensorConfig_t *tempSensorConfigs, const tempSensorConfig_t *defaultTempSensorConfigs) +{ + const char *format = "temp_sensor %u %u %s %d %d %u %s"; + for (uint8_t i = 0; i < MAX_TEMP_SENSORS; i++) { + bool equalsDefault = false; + char label[5], hex_address[17]; + strncpy(label, tempSensorConfigs[i].label, TEMPERATURE_LABEL_LEN); + label[4] = '\0'; + tempSensorAddressToString(tempSensorConfigs[i].address, hex_address); + if (defaultTempSensorConfigs) { + equalsDefault = tempSensorConfigs[i].type == defaultTempSensorConfigs[i].type + && tempSensorConfigs[i].address == defaultTempSensorConfigs[i].address + && tempSensorConfigs[i].osdSymbol == defaultTempSensorConfigs[i].osdSymbol + && !memcmp(tempSensorConfigs[i].label, defaultTempSensorConfigs[i].label, TEMPERATURE_LABEL_LEN) + && tempSensorConfigs[i].alarm_min == defaultTempSensorConfigs[i].alarm_min + && tempSensorConfigs[i].alarm_max == defaultTempSensorConfigs[i].alarm_max; + cliDefaultPrintLinef(dumpMask, equalsDefault, format, + i, + defaultTempSensorConfigs[i].type, + "0", + defaultTempSensorConfigs[i].alarm_min, + defaultTempSensorConfigs[i].alarm_max, + 0, + "" + ); + } + cliDumpPrintLinef(dumpMask, equalsDefault, format, + i, + tempSensorConfigs[i].type, + hex_address, + tempSensorConfigs[i].alarm_min, + tempSensorConfigs[i].alarm_max, + tempSensorConfigs[i].osdSymbol, + label + ); + } +} + +static void cliTempSensor(char *cmdline) +{ + if (isEmpty(cmdline)) { + printTempSensor(DUMP_MASTER, tempSensorConfig(0), NULL); + } else if (sl_strcasecmp(cmdline, "reset") == 0) { + resetTempSensorConfig(); + } else { + int16_t i; + const char *ptr = cmdline, *label; + int16_t type, alarm_min, alarm_max; + bool addressValid = false; + uint64_t address; + int8_t osdSymbol; + uint8_t validArgumentCount = 0; + i = fastA2I(ptr); + if (i >= 0 && i < MAX_TEMP_SENSORS) { + + ptr = nextArg(ptr); + if (ptr) { + type = fastA2I(ptr); + validArgumentCount++; + } + + ptr = nextArg(ptr); + if (ptr) { + addressValid = tempSensorStringToAddress(ptr, &address); + validArgumentCount++; + } + + ptr = nextArg(ptr); + if (ptr) { + alarm_min = fastA2I(ptr); + validArgumentCount++; + } + + ptr = nextArg(ptr); + if (ptr) { + alarm_max = fastA2I(ptr); + validArgumentCount++; + } + + ptr = nextArg(ptr); + if (ptr) { + osdSymbol = fastA2I(ptr); + validArgumentCount++; + } + + label = nextArg(ptr); + if (label) + ++validArgumentCount; + else + label = ""; + + if (validArgumentCount < 4) { + cliShowParseError(); + } else if (type < 0 || type > TEMP_SENSOR_DS18B20 || alarm_min < -550 || alarm_min > 1250 || alarm_max < -550 || alarm_max > 1250 || osdSymbol < 0 || osdSymbol > TEMP_SENSOR_SYM_COUNT || strlen(label) > TEMPERATURE_LABEL_LEN || !addressValid) { + cliShowParseError(); + } else { + tempSensorConfig_t *sensorConfig = tempSensorConfigMutable(i); + sensorConfig->type = type; + sensorConfig->address = address; + sensorConfig->alarm_min = alarm_min; + sensorConfig->alarm_max = alarm_max; + sensorConfig->osdSymbol = osdSymbol; + for (uint8_t index; index < TEMPERATURE_LABEL_LEN; ++index) { + sensorConfig->label[index] = toupper(label[index]); + if (label[index] == '\0') break; + } + } + } else { + cliShowArgumentRangeError("sensor index", 0, MAX_TEMP_SENSORS - 1); + } + } +} +#endif + #ifdef USE_LED_STRIP static void printLed(uint8_t dumpMask, const ledConfig_t *ledConfigs, const ledConfig_t *defaultLedConfigs) { @@ -1293,25 +1414,6 @@ static void printServo(uint8_t dumpMask, const servoParam_t *servoParam, const s servoConf->rate ); } - - // print servo directions - if (defaultServoParam) { - for (uint32_t i = 0; i < MAX_SUPPORTED_SERVOS; i++) { - const servoParam_t *servoConf = &servoParam[i]; - const servoParam_t *servoConfDefault = &defaultServoParam[i]; - bool equalsDefault = servoConf->reversedSources == servoConfDefault->reversedSources; - for (uint32_t channel = 0; channel < INPUT_SOURCE_COUNT; channel++) { - equalsDefault = ~(servoConf->reversedSources ^ servoConfDefault->reversedSources) & (1 << channel); - const char *format = "smix reverse %d %d r"; - if (servoConfDefault->reversedSources & (1 << channel)) { - cliDefaultPrintLinef(dumpMask, equalsDefault, format, i , channel); - } - if (servoConf->reversedSources & (1 << channel)) { - cliDumpPrintLinef(dumpMask, equalsDefault, format, i , channel); - } - } - } - } } static void cliServo(char *cmdline) @@ -1430,51 +1532,6 @@ static void cliServoMix(char *cmdline) } else if (sl_strncasecmp(cmdline, "reset", 5) == 0) { // erase custom mixer pgResetCopy(customServoMixersMutable(0), PG_SERVO_MIXER); - for (uint32_t i = 0; i < MAX_SUPPORTED_SERVOS; i++) { - servoParamsMutable(i)->reversedSources = 0; - } - } else if (sl_strncasecmp(cmdline, "reverse", 7) == 0) { - enum {SERVO = 0, INPUT, REVERSE, ARGS_COUNT}; - char *ptr = strchr(cmdline, ' '); - - len = strlen(ptr); - if (len == 0) { - cliPrintf("s"); - for (uint32_t inputSource = 0; inputSource < INPUT_SOURCE_COUNT; inputSource++) - cliPrintf("\ti%d", inputSource); - cliPrintLinefeed(); - - for (uint32_t servoIndex = 0; servoIndex < MAX_SUPPORTED_SERVOS; servoIndex++) { - cliPrintf("%d", servoIndex); - for (uint32_t inputSource = 0; inputSource < INPUT_SOURCE_COUNT; inputSource++) - cliPrintf("\t%s ", (servoParams(servoIndex)->reversedSources & (1 << inputSource)) ? "r" : "n"); - cliPrintLinefeed(); - } - return; - } - - ptr = strtok_r(ptr, " ", &saveptr); - while (ptr != NULL && check < ARGS_COUNT - 1) { - args[check++] = fastA2I(ptr); - ptr = strtok_r(NULL, " ", &saveptr); - } - - if (ptr == NULL || check != ARGS_COUNT - 1) { - cliShowParseError(); - return; - } - - if (args[SERVO] >= 0 && args[SERVO] < MAX_SUPPORTED_SERVOS - && args[INPUT] >= 0 && args[INPUT] < INPUT_SOURCE_COUNT - && (*ptr == 'r' || *ptr == 'n')) { - if (*ptr == 'r') - servoParamsMutable(args[SERVO])->reversedSources |= 1 << args[INPUT]; - else - servoParamsMutable(args[SERVO])->reversedSources &= ~(1 << args[INPUT]); - } else - cliShowParseError(); - - cliServoMix("reverse"); } else { enum {RULE = 0, TARGET, INPUT, RATE, SPEED, ARGS_COUNT}; char *ptr = strtok_r(cmdline, " ", &saveptr); @@ -1492,7 +1549,7 @@ static void cliServoMix(char *cmdline) if (i >= 0 && i < MAX_SERVO_RULES && args[TARGET] >= 0 && args[TARGET] < MAX_SUPPORTED_SERVOS && args[INPUT] >= 0 && args[INPUT] < INPUT_SOURCE_COUNT && - args[RATE] >= -125 && args[RATE] <= 125 && + args[RATE] >= -1000 && args[RATE] <= 1000 && args[SPEED] >= 0 && args[SPEED] <= MAX_SERVO_SPEED) { customServoMixersMutable(i)->targetChannel = args[TARGET]; customServoMixersMutable(i)->inputSource = args[INPUT]; @@ -2357,7 +2414,7 @@ static void cliSet(char *cmdline) cliPrintf("%s set to ", name); cliPrintVar(val, 0); } else { - cliPrint("Invalid value."); + cliPrint("Invalid value. "); cliPrintVarRange(val); cliPrintLinefeed(); } @@ -2390,7 +2447,7 @@ static void cliStatus(char *cmdline) rtcGetDateTime(&dt); dateTimeFormatLocal(buf, &dt); cliPrintLinef("Current Time: %s", buf); - cliPrintLinef("Voltage: %d.%dV (%dS battery - %s)", getBatteryVoltage() / 100, getBatteryVoltage() % 100, getBatteryCellCount(), getBatteryStateString()); + cliPrintLinef("Voltage: %d.%02dV (%dS battery - %s)", getBatteryVoltage() / 100, getBatteryVoltage() % 100, getBatteryCellCount(), getBatteryStateString()); cliPrintf("CPU Clock=%dMHz", (SystemCoreClock / 1000000)); const uint32_t detectedSensorsMask = sensorsMask(); @@ -2474,11 +2531,7 @@ static void cliStatus(char *cmdline) #endif cliPrintf("System load: %d", averageSystemLoadPercent); -#ifdef USE_ASYNC_GYRO_PROCESSING - const timeDelta_t pidTaskDeltaTime = getTaskDeltaTime(TASK_PID); -#else const timeDelta_t pidTaskDeltaTime = getTaskDeltaTime(TASK_GYROPID); -#endif const int pidRate = pidTaskDeltaTime == 0 ? 0 : (int)(1000000.0f / ((float)pidTaskDeltaTime)); const int rxRate = getTaskDeltaTime(TASK_RX) == 0 ? 0 : (int)(1000000.0f / ((float)getTaskDeltaTime(TASK_RX))); const int systemRate = getTaskDeltaTime(TASK_SYSTEM) == 0 ? 0 : (int)(1000000.0f / ((float)getTaskDeltaTime(TASK_SYSTEM))); @@ -2547,6 +2600,9 @@ static void cliVersion(char *cmdline) buildTime, shortGitRevision ); + cliPrintLinef("# GCC-%s", + compilerVersion + ); } static void cliMemory(char *cmdline) @@ -2703,6 +2759,11 @@ static void printConfig(const char *cmdline, bool doDiff) cliPrintHashLine("rxrange"); printRxRange(dumpMask, rxChannelRangeConfigs_CopyArray, rxChannelRangeConfigs(0)); +#ifdef USE_TEMPERATURE_SENSOR + cliPrintHashLine("temp_sensor"); + printTempSensor(dumpMask, tempSensorConfig_CopyArray, tempSensorConfig(0)); +#endif + #ifdef USE_OSD cliPrintHashLine("osd_layout"); printOsdLayout(dumpMask, &osdConfig_Copy, osdConfig(), -1, -1); @@ -2855,15 +2916,16 @@ const clicmd_t cmdTable[] = { CLI_COMMAND_DEF("set", "change setting", "[=]", cliSet), CLI_COMMAND_DEF("smix", "servo mixer", " \r\n" - "\treset\r\n" - "\tload \r\n" - "\treverse r|n", cliServoMix), + "\treset\r\n", cliServoMix), #ifdef USE_SDCARD CLI_COMMAND_DEF("sd_info", "sdcard info", NULL, cliSdInfo), #endif CLI_COMMAND_DEF("status", "show status", NULL, cliStatus), #ifndef SKIP_TASK_STATISTICS CLI_COMMAND_DEF("tasks", "show task stats", NULL, cliTasks), +#endif +#ifdef USE_TEMPERATURE_SENSOR + CLI_COMMAND_DEF("temp_sensor", "change temp sensor settings", NULL, cliTempSensor), #endif CLI_COMMAND_DEF("version", "show version", NULL, cliVersion), #ifdef USE_OSD diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 2ba10c0ff63..2a5905f8bb6 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -106,7 +106,7 @@ PG_RESET_TEMPLATE(featureConfig_t, featureConfig, .enabledFeatures = DEFAULT_FEATURES | COMMON_DEFAULT_FEATURES ); -PG_REGISTER_WITH_RESET_TEMPLATE(systemConfig_t, systemConfig, PG_SYSTEM_CONFIG, 1); +PG_REGISTER_WITH_RESET_TEMPLATE(systemConfig_t, systemConfig, PG_SYSTEM_CONFIG, 2); PG_RESET_TEMPLATE(systemConfig_t, systemConfig, .current_profile_index = 0, @@ -114,9 +114,6 @@ PG_RESET_TEMPLATE(systemConfig_t, systemConfig, .debug_mode = DEBUG_NONE, .i2c_speed = I2C_SPEED_400KHZ, .cpuUnderclock = 0, - .accTaskFrequency = ACC_TASK_FREQUENCY_DEFAULT, - .attitudeTaskFrequency = ATTITUDE_TASK_FREQUENCY_DEFAULT, - .asyncMode = ASYNC_MODE_NONE, .throttle_tilt_compensation_strength = 0, // 0-100, 0 - disabled .pwmRxInputFilteringMode = INPUT_FILTERING_DISABLED, .name = { 0 } @@ -144,60 +141,29 @@ void validateNavConfig(void) #endif -#ifdef SWAP_SERIAL_PORT_0_AND_1_DEFAULTS -#define FIRST_PORT_INDEX 1 -#define SECOND_PORT_INDEX 0 -#else -#define FIRST_PORT_INDEX 0 -#define SECOND_PORT_INDEX 1 -#endif - -uint32_t getPidUpdateRate(void) +// Stubs to handle target-specific configs +__attribute__((weak)) void validateAndFixTargetConfig(void) { -#ifdef USE_ASYNC_GYRO_PROCESSING - if (systemConfig()->asyncMode == ASYNC_MODE_NONE) { - return getGyroUpdateRate(); - } else { - return gyroConfig()->looptime; - } -#else - return gyro.targetLooptime; -#endif + __NOP(); } -timeDelta_t getGyroUpdateRate(void) +__attribute__((weak)) void targetConfiguration(void) { - return gyro.targetLooptime; + __NOP(); } -uint16_t getAccUpdateRate(void) -{ -#ifdef USE_ASYNC_GYRO_PROCESSING - // ACC will be updated at its own rate - if (systemConfig()->asyncMode == ASYNC_MODE_ALL) { - return 1000000 / systemConfig()->accTaskFrequency; - } else { - return getPidUpdateRate(); - } + +#ifdef SWAP_SERIAL_PORT_0_AND_1_DEFAULTS +#define FIRST_PORT_INDEX 1 +#define SECOND_PORT_INDEX 0 #else - // ACC updated at same frequency in taskMainPidLoop in mw.c - return gyro.targetLooptime; +#define FIRST_PORT_INDEX 0 +#define SECOND_PORT_INDEX 1 #endif -} - -#ifdef USE_ASYNC_GYRO_PROCESSING -uint16_t getAttitudeUpdateRate(void) { - if (systemConfig()->asyncMode == ASYNC_MODE_ALL) { - return 1000000 / systemConfig()->attitudeTaskFrequency; - } else { - return getPidUpdateRate(); - } -} -uint8_t getAsyncMode(void) { - return systemConfig()->asyncMode; -} -#endif +uint32_t getLooptime(void) { + return gyro.targetLooptime; +} void validateAndFixConfig(void) { @@ -249,15 +215,6 @@ void validateAndFixConfig(void) featureClear(FEATURE_SOFTSERIAL); } -#ifdef USE_ASYNC_GYRO_PROCESSING - /* - * When async processing mode is enabled, gyroSync has to be forced to "ON" - */ - if (getAsyncMode() != ASYNC_MODE_NONE) { - gyroConfigMutable()->gyroSync = 1; - } -#endif - #if defined(USE_LED_STRIP) && (defined(USE_SOFTSERIAL1) || defined(USE_SOFTSERIAL2)) if (featureConfigured(FEATURE_SOFTSERIAL) && featureConfigured(FEATURE_LED_STRIP)) { const timerHardware_t *ledTimerHardware = timerGetByTag(IO_TAG(WS2811_PIN), TIM_USE_ANY); @@ -298,10 +255,17 @@ void validateAndFixConfig(void) #endif // Limitations of different protocols +#if !defined(USE_DSHOT) + if (motorConfig()->motorPwmProtocol > PWM_TYPE_BRUSHED) { + motorConfigMutable()->motorPwmProtocol = PWM_TYPE_STANDARD; + } +#endif + #ifdef BRUSHED_MOTORS motorConfigMutable()->motorPwmRate = constrain(motorConfig()->motorPwmRate, 500, 32000); #else switch (motorConfig()->motorPwmProtocol) { + default: case PWM_TYPE_STANDARD: // Limited to 490 Hz motorConfigMutable()->motorPwmRate = MIN(motorConfig()->motorPwmRate, 490); break; @@ -317,14 +281,33 @@ void validateAndFixConfig(void) case PWM_TYPE_BRUSHED: // 500Hz - 32kHz motorConfigMutable()->motorPwmRate = constrain(motorConfig()->motorPwmRate, 500, 32000); break; +#ifdef USE_DSHOT + // One DSHOT packet takes 16 bits x 19 ticks + 2uS = 304 timer ticks + 2uS + case PWM_TYPE_DSHOT150: + motorConfigMutable()->motorPwmRate = MIN(motorConfig()->motorPwmRate, 4000); + break; + case PWM_TYPE_DSHOT300: + motorConfigMutable()->motorPwmRate = MIN(motorConfig()->motorPwmRate, 8000); + break; + // Although DSHOT 600+ support >16kHz update rate it's not practical because of increased CPU load + // It's more reasonable to use slower-speed DSHOT at higher rate for better reliability + case PWM_TYPE_DSHOT600: + motorConfigMutable()->motorPwmRate = MIN(motorConfig()->motorPwmRate, 16000); + break; + case PWM_TYPE_DSHOT1200: + motorConfigMutable()->motorPwmRate = MIN(motorConfig()->motorPwmRate, 32000); + break; +#endif } #endif #if !defined(USE_MPU_DATA_READY_SIGNAL) gyroConfigMutable()->gyroSync = false; - systemConfigMutable()->asyncMode = ASYNC_MODE_NONE; #endif + // Call target-specific validation function + validateAndFixTargetConfig(); + if (settingsValidate(NULL)) { DISABLE_ARMING_FLAG(ARMING_DISABLED_INVALID_SETTING); } else { @@ -354,9 +337,9 @@ void createDefaultConfig(void) #endif #endif -#if defined(TARGET_CONFIG) + featureSet(FEATURE_AIRMODE); + targetConfiguration(); -#endif } void resetConfigs(void) diff --git a/src/main/fc/config.h b/src/main/fc/config.h index 05cb7f2a4b1..e3832cffe3f 100644 --- a/src/main/fc/config.h +++ b/src/main/fc/config.h @@ -29,13 +29,6 @@ #define ONESHOT_FEATURE_CHANGED_DELAY_ON_BOOT_MS 1500 #define MAX_NAME_LENGTH 16 -#define ACC_TASK_FREQUENCY_DEFAULT 500 -#define ACC_TASK_FREQUENCY_MIN 100 -#define ACC_TASK_FREQUENCY_MAX 1000 -#define ATTITUDE_TASK_FREQUENCY_DEFAULT 250 -#define ATTITUDE_TASK_FREQUENCY_MIN 100 -#define ATTITUDE_TASK_FREQUENCY_MAX 1000 - typedef enum { ASYNC_MODE_NONE, ASYNC_MODE_GYRO, @@ -78,15 +71,12 @@ typedef enum { } features_e; typedef struct systemConfig_s { - uint16_t accTaskFrequency; - uint16_t attitudeTaskFrequency; uint8_t current_profile_index; uint8_t current_battery_profile_index; - uint8_t asyncMode; uint8_t debug_mode; uint8_t i2c_speed; uint8_t cpuUnderclock; - uint8_t throttle_tilt_compensation_strength; // the correction that will be applied at throttle_correction_angle. + uint8_t throttle_tilt_compensation_strength; // the correction that will be applied at throttle_correction_angle. inputFilteringMode_e pwmRxInputFilteringMode; char name[MAX_NAME_LENGTH + 1]; } systemConfig_t; @@ -130,6 +120,7 @@ void ensureEEPROMContainsValidData(void); void saveConfigAndNotify(void); void validateAndFixConfig(void); +void validateAndFixTargetConfig(void); uint8_t getConfigProfile(void); bool setConfigProfile(uint8_t profileIndex); @@ -146,10 +137,4 @@ void createDefaultConfig(void); void resetConfigs(void); void targetConfiguration(void); -uint32_t getPidUpdateRate(void); -timeDelta_t getGyroUpdateRate(void); -uint16_t getAccUpdateRate(void); -#ifdef USE_ASYNC_GYRO_PROCESSING -uint16_t getAttitudeUpdateRate(void); -uint8_t getAsyncMode(void); -#endif +uint32_t getLooptime(void); diff --git a/src/main/fc/controlrate_profile.c b/src/main/fc/controlrate_profile.c index c7b2b696338..49b5b2b2275 100644 --- a/src/main/fc/controlrate_profile.c +++ b/src/main/fc/controlrate_profile.c @@ -33,7 +33,7 @@ const controlRateConfig_t *currentControlRateProfile; -PG_REGISTER_ARRAY_WITH_RESET_FN(controlRateConfig_t, MAX_CONTROL_RATE_PROFILE_COUNT, controlRateProfiles, PG_CONTROL_RATE_PROFILES, 2); +PG_REGISTER_ARRAY_WITH_RESET_FN(controlRateConfig_t, MAX_CONTROL_RATE_PROFILE_COUNT, controlRateProfiles, PG_CONTROL_RATE_PROFILES, 3); void pgResetFn_controlRateProfiles(controlRateConfig_t *instance) { @@ -61,6 +61,10 @@ void pgResetFn_controlRateProfiles(controlRateConfig_t *instance) .rates[FD_ROLL] = 100, .rates[FD_PITCH] = 100, .rates[FD_YAW] = 100 + }, + + .misc = { + .fpvCamAngleDegrees = 0 } ); } diff --git a/src/main/fc/controlrate_profile.h b/src/main/fc/controlrate_profile.h index 9adc678b395..b8428347aaf 100644 --- a/src/main/fc/controlrate_profile.h +++ b/src/main/fc/controlrate_profile.h @@ -62,6 +62,10 @@ typedef struct controlRateConfig_s { uint8_t rates[3]; } manual; + struct { + uint8_t fpvCamAngleDegrees; // Camera angle to treat as "forward" base axis in ACRO (Roll and Yaw sticks will command rotation considering this axis) + } misc; + } controlRateConfig_t; PG_DECLARE_ARRAY(controlRateConfig_t, MAX_CONTROL_RATE_PROFILE_COUNT, controlRateProfiles); diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 09ab4a0a919..a9ce120e2d6 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -34,8 +34,9 @@ #include "drivers/light_led.h" #include "drivers/serial.h" #include "drivers/time.h" - #include "drivers/system.h" +#include "drivers/pwm_output.h" + #include "sensors/sensors.h" #include "sensors/diagnostics.h" #include "sensors/boardalignment.h" @@ -52,6 +53,7 @@ #include "fc/config.h" #include "fc/controlrate_profile.h" #include "fc/rc_adjustments.h" +#include "fc/rc_smoothing.h" #include "fc/rc_controls.h" #include "fc/rc_curves.h" #include "fc/rc_modes.h" @@ -63,6 +65,7 @@ #include "io/serial.h" #include "io/statusindicator.h" #include "io/asyncfatfs/asyncfatfs.h" +#include "io/piniobox.h" #include "msp/msp_serial.h" @@ -417,6 +420,7 @@ void processRx(timeUs_t currentTimeUs) { static bool armedBeeperOn = false; + // Calculate RPY channel data calculateRxChannelsAndUpdateFailsafe(currentTimeUs); // in 3D mode, we need to be able to disarm by switch at any time @@ -484,6 +488,10 @@ void processRx(timeUs_t currentTimeUs) updateActivatedModes(); +#ifdef USE_PINIOBOX + pinioBoxUpdate(); +#endif + if (!cliMode) { bool canUseRxData = rxIsReceivingSignal() && !FLIGHT_MODE(FAILSAFE_MODE); updateAdjustmentStates(canUseRxData); @@ -628,44 +636,6 @@ void processRx(timeUs_t currentTimeUs) } -void filterRc(bool isRXDataNew) -{ - static int16_t lastCommand[4] = { 0, 0, 0, 0 }; - static int16_t deltaRC[4] = { 0, 0, 0, 0 }; - static int16_t factor, rcInterpolationFactor; - static biquadFilter_t filteredCycleTimeState; - static bool filterInitialised; - - // Calculate average cycle time (1Hz LPF on cycle time) - if (!filterInitialised) { - biquadFilterInitLPF(&filteredCycleTimeState, 1, getPidUpdateRate()); - filterInitialised = true; - } - - const timeDelta_t filteredCycleTime = biquadFilterApply(&filteredCycleTimeState, (float) cycleTime); - rcInterpolationFactor = rxGetRefreshRate() / filteredCycleTime + 1; - - if (isRXDataNew) { - for (int channel=0; channel < 4; channel++) { - deltaRC[channel] = rcCommand[channel] - (lastCommand[channel] - deltaRC[channel] * factor / rcInterpolationFactor); - lastCommand[channel] = rcCommand[channel]; - } - - factor = rcInterpolationFactor - 1; - } else { - factor--; - } - - // Interpolate steps of rcCommand - if (factor > 0) { - for (int channel=0; channel < 4; channel++) { - rcCommand[channel] = lastCommand[channel] - deltaRC[channel] * factor/rcInterpolationFactor; - } - } else { - factor = 0; - } -} - // Function for loop trigger void taskGyro(timeUs_t currentTimeUs) { // getTaskDeltaTime() returns delta time frozen at the moment of entering the scheduler. currentTime is frozen at the very same point. @@ -676,14 +646,14 @@ void taskGyro(timeUs_t currentTimeUs) { if (gyroConfig()->gyroSync) { while (true) { gyroUpdateUs = micros(); - if (gyroSyncCheckUpdate() || ((currentDeltaTime + cmpTimeUs(gyroUpdateUs, currentTimeUs)) >= (getGyroUpdateRate() + GYRO_WATCHDOG_DELAY))) { + if (gyroSyncCheckUpdate() || ((currentDeltaTime + cmpTimeUs(gyroUpdateUs, currentTimeUs)) >= (timeDelta_t)(getLooptime() + GYRO_WATCHDOG_DELAY))) { break; } } } /* Update actual hardware readings */ - gyroUpdate(currentDeltaTime + (timeDelta_t)(gyroUpdateUs - currentTimeUs)); + gyroUpdate(); #ifdef USE_OPTICAL_FLOW if (sensors(SENSOR_OPFLOW)) { @@ -707,31 +677,18 @@ void taskMainPidLoop(timeUs_t currentTimeUs) cycleTime = getTaskDeltaTime(TASK_SELF); dT = (float)cycleTime * 0.000001f; - if (ARMING_FLAG(ARMED) && ((!STATE(FIXED_WING)) || (!isNavLaunchEnabled()) || (isNavLaunchEnabled() && isFixedWingLaunchDetected()))) { + if (ARMING_FLAG(ARMED) && (!STATE(FIXED_WING) || !isNavLaunchEnabled() || (isNavLaunchEnabled() && (isFixedWingLaunchDetected() || isFixedWingLaunchFinishedOrAborted())))) { flightTime += cycleTime; } -#ifdef USE_ASYNC_GYRO_PROCESSING - if (getAsyncMode() == ASYNC_MODE_NONE) { - taskGyro(currentTimeUs); - } - - if (getAsyncMode() != ASYNC_MODE_ALL && sensors(SENSOR_ACC)) { - imuUpdateAccelerometer(); - imuUpdateAttitude(currentTimeUs); - } -#else - /* Update gyroscope */ taskGyro(currentTimeUs); imuUpdateAccelerometer(); imuUpdateAttitude(currentTimeUs); -#endif - annexCode(); - if (rxConfig()->rcSmoothing) { - filterRc(isRXDataNew); + if (rxConfig()->rcFilterFrequency) { + rcInterpolationApply(isRXDataNew); } #if defined(USE_NAV) @@ -809,10 +766,6 @@ void taskMainPidLoop(timeUs_t currentTimeUs) writeMotors(); } -#ifdef USE_SDCARD - afatfs_poll(); -#endif - #ifdef USE_BLACKBOX if (!cliMode && feature(FEATURE_BLACKBOX)) { blackboxUpdate(micros()); @@ -820,6 +773,21 @@ void taskMainPidLoop(timeUs_t currentTimeUs) #endif } +// This function is called in a busy-loop, everything called from here should do it's own +// scheduling and avoid doing heavy calculations +void taskRunRealtimeCallbacks(timeUs_t currentTimeUs) +{ + UNUSED(currentTimeUs); + +#ifdef USE_SDCARD + afatfs_poll(); +#endif + +#ifdef USE_DSHOT + pwmCompleteDshotUpdate(getMotorCount()); +#endif +} + bool taskUpdateRxCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTime) { UNUSED(currentDeltaTime); diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index bbcdc2d70df..3aab52166c4 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -41,6 +41,7 @@ #include "cms/cms.h" +#include "drivers/1-wire.h" #include "drivers/accgyro/accgyro.h" #include "drivers/adc.h" #include "drivers/compass/compass.h" @@ -107,6 +108,8 @@ #include "io/vtx_control.h" #include "io/vtx_smartaudio.h" #include "io/vtx_tramp.h" +#include "io/vtx_ffpv24g.h" +#include "io/piniobox.h" #include "msp/msp_serial.h" @@ -319,15 +322,10 @@ void init(void) (motorConfig()->motorPwmProtocol == PWM_TYPE_MULTISHOT); #endif pwm_params.motorPwmRate = motorConfig()->motorPwmRate; - pwm_params.idlePulse = motorConfig()->mincommand; - if (feature(FEATURE_3D)) { - pwm_params.idlePulse = flight3DConfig()->neutral3d; - } if (motorConfig()->motorPwmProtocol == PWM_TYPE_BRUSHED) { pwm_params.useFastPwm = false; featureClear(FEATURE_3D); - pwm_params.idlePulse = 0; // brushed motors } pwm_params.enablePWMOutput = feature(FEATURE_PWM_OUTPUT_ENABLE); @@ -487,6 +485,14 @@ void init(void) adcInit(&adc_params); #endif +#ifdef USE_PINIO + pinioInit(); +#endif + +#ifdef USE_PINIOBOX + pinioBoxInit(); +#endif + #if defined(USE_GPS) || defined(USE_MAG) delay(500); @@ -526,6 +532,11 @@ void init(void) } #endif + // 1-Wire IF chip +#ifdef USE_1WIRE + owInit(); +#endif + if (!sensorsAutodetect()) { // if gyro was not detected due to whatever reason, we give up now. failureMode(FAILURE_MISSING_ACC); @@ -615,27 +626,8 @@ void init(void) #endif #ifdef USE_SDCARD - bool sdcardUseDMA = false; - sdcardInsertionDetectInit(); - -#ifdef SDCARD_DMA_CHANNEL_TX - -#if defined(USE_LED_STRIP) && defined(WS2811_DMA_CHANNEL) - // Ensure the SPI Tx DMA doesn't overlap with the led strip -#if defined(STM32F4) || defined(STM32F7) - sdcardUseDMA = !feature(FEATURE_LED_STRIP) || SDCARD_DMA_CHANNEL_TX != WS2811_DMA_STREAM; -#else - sdcardUseDMA = !feature(FEATURE_LED_STRIP) || SDCARD_DMA_CHANNEL_TX != WS2811_DMA_CHANNEL; -#endif -#else - sdcardUseDMA = true; -#endif - -#endif - - sdcard_init(sdcardUseDMA); - + sdcard_init(); afatfs_init(); #endif @@ -643,7 +635,8 @@ void init(void) blackboxInit(); #endif - gyroSetCalibrationCycles(CALIBRATING_GYRO_CYCLES); + gyroStartCalibration(); + #ifdef USE_BARO baroStartCalibration(); #endif @@ -652,13 +645,10 @@ void init(void) pitotStartCalibration(); #endif -#if defined(USE_VTX_COMMON) && defined(USE_VTX_CONTROL) +#if defined(USE_VTX_CONTROL) vtxControlInit(); - -#if defined(USE_VTX_COMMON) vtxCommonInit(); vtxInit(); -#endif #ifdef USE_VTX_SMARTAUDIO vtxSmartAudioInit(); @@ -668,7 +658,11 @@ void init(void) vtxTrampInit(); #endif -#endif // USE_VTX_COMMON && USE_VTX_CONTROL +#ifdef USE_VTX_FFPV + vtxFuriousFPVInit(); +#endif + +#endif // USE_VTX_CONTROL // Now that everything has powered up the voltage and cell count be determined. if (feature(FEATURE_VBAT | FEATURE_CURRENT_METER)) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index b9ab3f4f793..ccdf23cab4f 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -15,6 +15,7 @@ * along with Cleanflight. If not, see . */ +#include #include #include #include @@ -104,6 +105,7 @@ #include "sensors/compass.h" #include "sensors/gyro.h" #include "sensors/opflow.h" +#include "sensors/temperature.h" #include "telemetry/telemetry.h" @@ -447,14 +449,14 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF sbufWriteU8(dst, 0); sbufWriteU8(dst, 0); sbufWriteU8(dst, 255); // used to be forwardFromChannel, not used anymore, send 0xff for compatibility reasons - sbufWriteU32(dst, servoParams(i)->reversedSources); + sbufWriteU32(dst, 0); //Input reversing is not required since it can be done on mixer level } break; case MSP_SERVO_MIX_RULES: for (int i = 0; i < MAX_SERVO_RULES; i++) { sbufWriteU8(dst, customServoMixers(i)->targetChannel); sbufWriteU8(dst, customServoMixers(i)->inputSource); - sbufWriteU8(dst, customServoMixers(i)->rate); + sbufWriteU16(dst, customServoMixers(i)->rate); sbufWriteU8(dst, customServoMixers(i)->speed); sbufWriteU8(dst, 0); sbufWriteU8(dst, 100); @@ -787,11 +789,17 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF // output some useful QA statistics // debug[x] = ((hse_value / 1000000) * 1000) + (SystemCoreClock / 1000000); // XX0YY [crystal clock : core clock] - for (int i = 0; i < DEBUG16_VALUE_COUNT; i++) { + for (int i = 0; i < 4; i++) { sbufWriteU16(dst, debug[i]); // 4 variables are here for general monitoring purpose } break; + case MSP2_INAV_DEBUG: + for (int i = 0; i < DEBUG32_VALUE_COUNT; i++) { + sbufWriteU32(dst, debug[i]); // 8 variables are here for general monitoring purpose + } + break; + case MSP_UID: sbufWriteU32(dst, U_ID_0); sbufWriteU32(dst, U_ID_1); @@ -897,6 +905,20 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF } break; + case MSP2_COMMON_SERIAL_CONFIG: + for (int i = 0; i < SERIAL_PORT_COUNT; i++) { + if (!serialIsPortAvailable(serialConfig()->portConfigs[i].identifier)) { + continue; + }; + sbufWriteU8(dst, serialConfig()->portConfigs[i].identifier); + sbufWriteU32(dst, serialConfig()->portConfigs[i].functionMask); + sbufWriteU8(dst, serialConfig()->portConfigs[i].msp_baudrateIndex); + sbufWriteU8(dst, serialConfig()->portConfigs[i].gps_baudrateIndex); + sbufWriteU8(dst, serialConfig()->portConfigs[i].telemetry_baudrateIndex); + sbufWriteU8(dst, serialConfig()->portConfigs[i].peripheral_baudrateIndex); + } + break; + #ifdef USE_LED_STRIP case MSP_LED_COLORS: for (int i = 0; i < LED_CONFIGURABLE_COLOR_COUNT; i++) { @@ -936,16 +958,23 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF break; case MSP_BLACKBOX_CONFIG: + sbufWriteU8(dst, 0); // API no longer supported + sbufWriteU8(dst, 0); + sbufWriteU8(dst, 0); + sbufWriteU8(dst, 0); + break; + + case MSP2_BLACKBOX_CONFIG: #ifdef USE_BLACKBOX sbufWriteU8(dst, 1); //Blackbox supported sbufWriteU8(dst, blackboxConfig()->device); - sbufWriteU8(dst, blackboxConfig()->rate_num); - sbufWriteU8(dst, blackboxConfig()->rate_denom); + sbufWriteU16(dst, blackboxConfig()->rate_num); + sbufWriteU16(dst, blackboxConfig()->rate_denom); #else sbufWriteU8(dst, 0); // Blackbox not supported sbufWriteU8(dst, 0); - sbufWriteU8(dst, 0); - sbufWriteU8(dst, 0); + sbufWriteU16(dst, 0); + sbufWriteU16(dst, 0); #endif break; @@ -1080,20 +1109,14 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF break; case MSP_INAV_PID: - #ifdef USE_ASYNC_GYRO_PROCESSING - sbufWriteU8(dst, systemConfig()->asyncMode); - sbufWriteU16(dst, systemConfig()->accTaskFrequency); - sbufWriteU16(dst, systemConfig()->attitudeTaskFrequency); - #else - sbufWriteU8(dst, 0); - sbufWriteU16(dst, 0); - sbufWriteU16(dst, 0); - #endif + sbufWriteU8(dst, 0); //Legacy, no longer in use async processing value + sbufWriteU16(dst, 0); //Legacy, no longer in use async processing value + sbufWriteU16(dst, 0); //Legacy, no longer in use async processing value sbufWriteU8(dst, pidProfile()->heading_hold_rate_limit); sbufWriteU8(dst, HEADING_HOLD_ERROR_LPF_FREQ); sbufWriteU16(dst, mixerConfig()->yaw_jump_prevention_limit); sbufWriteU8(dst, gyroConfig()->gyro_lpf); - sbufWriteU8(dst, pidProfile()->acc_soft_lpf_hz); + sbufWriteU8(dst, accelerometerConfig()->acc_lpf_hz); sbufWriteU8(dst, 0); //reserved sbufWriteU8(dst, 0); //reserved sbufWriteU8(dst, 0); //reserved @@ -1266,7 +1289,6 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF } break; -#if defined(USE_VTX_COMMON) case MSP_VTX_CONFIG: { vtxDevice_t *vtxDevice = vtxCommonDevice(); @@ -1295,7 +1317,6 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF } } break; -#endif case MSP_NAME: { @@ -1335,6 +1356,15 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF sbufWriteU16(dst, osdConfig()->alt_alarm); sbufWriteU16(dst, osdConfig()->dist_alarm); sbufWriteU16(dst, osdConfig()->neg_alt_alarm); + sbufWriteU16(dst, osdConfig()->imu_temp_alarm_min); + sbufWriteU16(dst, osdConfig()->imu_temp_alarm_max); +#ifdef USE_BARO + sbufWriteU16(dst, osdConfig()->baro_temp_alarm_min); + sbufWriteU16(dst, osdConfig()->baro_temp_alarm_max); +#else + sbufWriteU16(dst, 0); + sbufWriteU16(dst, 0); +#endif break; case MSP2_INAV_OSD_PREFERENCES: @@ -1357,6 +1387,44 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF sbufWriteU8(dst, timerHardware[i].usageFlags); break; + case MSP2_INAV_MC_BRAKING: +#ifdef USE_MR_BRAKING_MODE + sbufWriteU16(dst, navConfig()->mc.braking_speed_threshold); + sbufWriteU16(dst, navConfig()->mc.braking_disengage_speed); + sbufWriteU16(dst, navConfig()->mc.braking_timeout); + sbufWriteU8(dst, navConfig()->mc.braking_boost_factor); + sbufWriteU16(dst, navConfig()->mc.braking_boost_timeout); + sbufWriteU16(dst, navConfig()->mc.braking_boost_speed_threshold); + sbufWriteU16(dst, navConfig()->mc.braking_boost_disengage_speed); + sbufWriteU8(dst, navConfig()->mc.braking_bank_angle); +#endif + break; + +#ifdef USE_TEMPERATURE_SENSOR + case MSP2_INAV_TEMP_SENSOR_CONFIG: + for (uint8_t index = 0; index < MAX_TEMP_SENSORS; ++index) { + const tempSensorConfig_t *sensorConfig = tempSensorConfig(index); + sbufWriteU8(dst, sensorConfig->type); + for (uint8_t addrIndex; addrIndex < 8; ++addrIndex) + sbufWriteU8(dst, ((uint8_t *)&sensorConfig->address)[addrIndex]); + for (uint8_t labelIndex; labelIndex < 4; ++labelIndex) + sbufWriteU8(dst, sensorConfig->label[labelIndex]); + sbufWriteU16(dst, sensorConfig->alarm_min); + sbufWriteU16(dst, sensorConfig->alarm_max); + } + break; +#endif + +#ifdef USE_TEMPERATURE_SENSOR + case MSP2_INAV_TEMPERATURES: + for (uint8_t index = 0; index < MAX_TEMP_SENSORS; ++index) { + int16_t temperature; + bool valid = getSensorTemperature(index, &temperature); + sbufWriteU16(dst, valid ? temperature : -1000); + } + break; +#endif + default: return false; } @@ -1607,9 +1675,10 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) #endif sbufReadU8(src); // multiwiiCurrentMeterOutput tmp_u8 = sbufReadU8(src); - if (tmp_u8 <= MAX_SUPPORTED_RC_CHANNEL_COUNT) + if (tmp_u8 <= MAX_SUPPORTED_RC_CHANNEL_COUNT) { rxConfigMutable()->rssi_channel = tmp_u8; - rxUpdateRSSISource(); // Changing rssi_channel might change the RSSI source + rxUpdateRSSISource(); // Changing rssi_channel might change the RSSI source + } sbufReadU8(src); #ifdef USE_MAG @@ -1736,17 +1805,17 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) sbufReadU8(src); sbufReadU8(src); sbufReadU8(src); // used to be forwardFromChannel, ignored - servoParamsMutable(tmp_u8)->reversedSources = sbufReadU32(src); + sbufReadU32(src); // used to be reversedSources servoComputeScalingFactors(tmp_u8); } break; case MSP_SET_SERVO_MIX_RULE: sbufReadU8Safe(&tmp_u8, src); - if ((dataSize >= 8) && (tmp_u8 < MAX_SERVO_RULES)) { + if ((dataSize >= 9) && (tmp_u8 < MAX_SERVO_RULES)) { customServoMixersMutable(tmp_u8)->targetChannel = sbufReadU8(src); customServoMixersMutable(tmp_u8)->inputSource = sbufReadU8(src); - customServoMixersMutable(tmp_u8)->rate = sbufReadU8(src); + customServoMixersMutable(tmp_u8)->rate = sbufReadU16(src); customServoMixersMutable(tmp_u8)->speed = sbufReadU8(src); sbufReadU16(src); //Read 2bytes for min/max and ignore it sbufReadU8(src); //Read 1 byte for `box` and ignore it @@ -1891,20 +1960,14 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) case MSP_SET_INAV_PID: if (dataSize >= 15) { -#ifdef USE_ASYNC_GYRO_PROCESSING - systemConfigMutable()->asyncMode = sbufReadU8(src); - systemConfigMutable()->accTaskFrequency = sbufReadU16(src); - systemConfigMutable()->attitudeTaskFrequency = sbufReadU16(src); -#else - sbufReadU8(src); - sbufReadU16(src); - sbufReadU16(src); -#endif + sbufReadU8(src); //Legacy, no longer in use async processing value + sbufReadU16(src); //Legacy, no longer in use async processing value + sbufReadU16(src); //Legacy, no longer in use async processing value pidProfileMutable()->heading_hold_rate_limit = sbufReadU8(src); sbufReadU8(src); //HEADING_HOLD_ERROR_LPF_FREQ mixerConfigMutable()->yaw_jump_prevention_limit = sbufReadU16(src); gyroConfigMutable()->gyro_lpf = sbufReadU8(src); - pidProfileMutable()->acc_soft_lpf_hz = sbufReadU8(src); + accelerometerConfigMutable()->acc_lpf_hz = sbufReadU8(src); sbufReadU8(src); //reserved sbufReadU8(src); //reserved sbufReadU8(src); //reserved @@ -2049,7 +2112,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) case MSP_ACC_CALIBRATION: if (!ARMING_FLAG(ARMED)) - accSetCalibrationCycles(CALIBRATING_ACC_CYCLES); + accStartCalibration(); else return MSP_RESULT_ERROR; break; @@ -2070,12 +2133,12 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) break; #ifdef USE_BLACKBOX - case MSP_SET_BLACKBOX_CONFIG: + case MSP2_SET_BLACKBOX_CONFIG: // Don't allow config to be updated while Blackbox is logging - if ((dataSize >= 3) && blackboxMayEditConfig()) { + if ((dataSize >= 5) && blackboxMayEditConfig()) { blackboxConfigMutable()->device = sbufReadU8(src); - blackboxConfigMutable()->rate_num = sbufReadU8(src); - blackboxConfigMutable()->rate_denom = sbufReadU8(src); + blackboxConfigMutable()->rate_num = sbufReadU16(src); + blackboxConfigMutable()->rate_denom = sbufReadU16(src); } else return MSP_RESULT_ERROR; break; @@ -2119,20 +2182,24 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) case MSP_OSD_CHAR_WRITE: #ifdef USE_MAX7456 if (dataSize >= 55) { - uint8_t font_data[64]; - const uint8_t addr = sbufReadU8(src); - for (int i = 0; i < 54; i++) { - font_data[i] = sbufReadU8(src); + max7456Character_t chr; + uint16_t addr; + if (dataSize >= 56) { + addr = sbufReadU16(src); + } else { + addr = sbufReadU8(src); + } + for (unsigned ii = 0; ii < sizeof(chr.data); ii++) { + chr.data[ii] = sbufReadU8(src); } // !!TODO - replace this with a device independent implementation - max7456WriteNvm(addr, font_data); + max7456WriteNvm(addr, &chr); } else return MSP_RESULT_ERROR; #endif // USE_MAX7456 break; #endif // USE_OSD -#if defined(USE_VTX_COMMON) case MSP_SET_VTX_CONFIG: if (dataSize >= 2) { vtxDevice_t *vtxDevice = vtxCommonDevice(); @@ -2171,7 +2238,6 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) return MSP_RESULT_ERROR; } break; -#endif #ifdef USE_FLASHFS case MSP_DATAFLASH_ERASE: @@ -2315,10 +2381,12 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) case MSP_SET_RSSI_CONFIG: sbufReadU8Safe(&tmp_u8, src); - if ((dataSize >= 1) && (tmp_u8 <= MAX_SUPPORTED_RC_CHANNEL_COUNT)) + if ((dataSize >= 1) && (tmp_u8 <= MAX_SUPPORTED_RC_CHANNEL_COUNT)) { rxConfigMutable()->rssi_channel = tmp_u8; - else + rxUpdateRSSISource(); + } else { return MSP_RESULT_ERROR; + } break; case MSP_SET_RX_MAP: @@ -2378,6 +2446,34 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) } break; + case MSP2_COMMON_SET_SERIAL_CONFIG: + { + uint8_t portConfigSize = sizeof(uint8_t) + sizeof(uint32_t) + (sizeof(uint8_t) * 4); + + if (dataSize % portConfigSize != 0) { + return MSP_RESULT_ERROR; + } + + uint8_t remainingPortsInPacket = dataSize / portConfigSize; + + while (remainingPortsInPacket--) { + uint8_t identifier = sbufReadU8(src); + + serialPortConfig_t *portConfig = serialFindPortConfiguration(identifier); + if (!portConfig) { + return MSP_RESULT_ERROR; + } + + portConfig->identifier = identifier; + portConfig->functionMask = sbufReadU32(src); + portConfig->msp_baudrateIndex = sbufReadU8(src); + portConfig->gps_baudrateIndex = sbufReadU8(src); + portConfig->telemetry_baudrateIndex = sbufReadU8(src); + portConfig->peripheral_baudrateIndex = sbufReadU8(src); + } + } + break; + #ifdef USE_LED_STRIP case MSP_SET_LED_COLORS: if (dataSize >= LED_CONFIGURABLE_COLOR_COUNT * 4) { @@ -2498,38 +2594,76 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) if (!sbufReadU16Safe(&osdConfigMutable()->item_pos[layout][item], src)) { return MSP_RESULT_ERROR; } - osdStartFullRedraw(); + // If the layout is not already overriden and it's different + // than the layout for the item that was just configured, + // override it for 10 seconds. + bool overridden; + int activeLayout = osdGetActiveLayout(&overridden); + if (activeLayout != layout && !overridden) { + osdOverrideLayout(layout, 10000); + } else { + osdStartFullRedraw(); + } } break; + case MSP2_INAV_OSD_SET_ALARMS: { - sbufReadU8Safe(&osdConfigMutable()->rssi_alarm, src); - sbufReadU16Safe(&osdConfigMutable()->time_alarm, src); - sbufReadU16Safe(&osdConfigMutable()->alt_alarm, src); - sbufReadU16Safe(&osdConfigMutable()->dist_alarm, src); - sbufReadU16Safe(&osdConfigMutable()->neg_alt_alarm, src); - osdStartFullRedraw(); + if (dataSize >= 17) { + osdConfigMutable()->rssi_alarm = sbufReadU8(src); + osdConfigMutable()->time_alarm = sbufReadU16(src); + osdConfigMutable()->alt_alarm = sbufReadU16(src); + osdConfigMutable()->dist_alarm = sbufReadU16(src); + osdConfigMutable()->neg_alt_alarm = sbufReadU16(src); + osdConfigMutable()->imu_temp_alarm_min = sbufReadU16(src); + osdConfigMutable()->imu_temp_alarm_max = sbufReadU16(src); +#ifdef USE_BARO + osdConfigMutable()->baro_temp_alarm_min = sbufReadU16(src); + osdConfigMutable()->baro_temp_alarm_max = sbufReadU16(src); +#endif + } else + return MSP_RESULT_ERROR; } break; + case MSP2_INAV_OSD_SET_PREFERENCES: { - sbufReadU8Safe(&osdConfigMutable()->video_system, src); - sbufReadU8Safe(&osdConfigMutable()->main_voltage_decimals, src); - sbufReadU8Safe(&osdConfigMutable()->ahi_reverse_roll, src); - sbufReadU8Safe(&osdConfigMutable()->crosshairs_style, src); - sbufReadU8Safe(&osdConfigMutable()->left_sidebar_scroll, src); - sbufReadU8Safe(&osdConfigMutable()->right_sidebar_scroll, src); - sbufReadU8Safe(&osdConfigMutable()->sidebar_scroll_arrows, src); - sbufReadU8Safe(&osdConfigMutable()->units, src); - sbufReadU8Safe(&osdConfigMutable()->stats_energy_unit, src); - osdStartFullRedraw(); + if (dataSize == 9) { + osdConfigMutable()->video_system = sbufReadU8(src); + osdConfigMutable()->main_voltage_decimals = sbufReadU8(src); + osdConfigMutable()->ahi_reverse_roll = sbufReadU8(src); + osdConfigMutable()->crosshairs_style = sbufReadU8(src); + osdConfigMutable()->left_sidebar_scroll = sbufReadU8(src); + osdConfigMutable()->right_sidebar_scroll = sbufReadU8(src); + osdConfigMutable()->sidebar_scroll_arrows = sbufReadU8(src); + osdConfigMutable()->units = sbufReadU8(src); + osdConfigMutable()->stats_energy_unit = sbufReadU8(src); + osdStartFullRedraw(); + } else + return MSP_RESULT_ERROR; } break; #endif + case MSP2_INAV_SET_MC_BRAKING: +#ifdef USE_MR_BRAKING_MODE + if (dataSize == 14) { + navConfigMutable()->mc.braking_speed_threshold = sbufReadU16(src); + navConfigMutable()->mc.braking_disengage_speed = sbufReadU16(src); + navConfigMutable()->mc.braking_timeout = sbufReadU16(src); + navConfigMutable()->mc.braking_boost_factor = sbufReadU8(src); + navConfigMutable()->mc.braking_boost_timeout = sbufReadU16(src); + navConfigMutable()->mc.braking_boost_speed_threshold = sbufReadU16(src); + navConfigMutable()->mc.braking_boost_disengage_speed = sbufReadU16(src); + navConfigMutable()->mc.braking_bank_angle = sbufReadU8(src); + } else +#endif + return MSP_RESULT_ERROR; + break; + case MSP2_INAV_SELECT_BATTERY_PROFILE: if (!ARMING_FLAG(ARMED)) { if (sbufReadU8Safe(&tmp_u8, src)) @@ -2537,6 +2671,24 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) } break; +#ifdef USE_TEMPERATURE_SENSOR + case MSP2_INAV_SET_TEMP_SENSOR_CONFIG: + if (dataSize == sizeof(tempSensorConfig_t) * MAX_TEMP_SENSORS) { + for (uint8_t index = 0; index < MAX_TEMP_SENSORS; ++index) { + tempSensorConfig_t *sensorConfig = tempSensorConfigMutable(index); + sensorConfig->type = sbufReadU8(src); + for (uint8_t addrIndex; addrIndex < 8; ++addrIndex) + ((uint8_t *)&sensorConfig->address)[addrIndex] = sbufReadU8(src); + for (uint8_t labelIndex; labelIndex < 4; ++labelIndex) + sensorConfig->label[labelIndex] = toupper(sbufReadU8(src)); + sensorConfig->alarm_min = sbufReadU16(src); + sensorConfig->alarm_max = sbufReadU16(src); + } + } else + return MSP_RESULT_ERROR; + break; +#endif + default: return MSP_RESULT_ERROR; } diff --git a/src/main/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c index f02ee43cd76..b8ff879a4c9 100644 --- a/src/main/fc/fc_msp_box.c +++ b/src/main/fc/fc_msp_box.c @@ -29,6 +29,7 @@ #include "fc/config.h" #include "fc/fc_msp_box.h" #include "fc/runtime_config.h" +#include "flight/mixer.h" #include "io/osd.h" @@ -66,6 +67,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = { { BOXAIRMODE, "AIR MODE", 29 }, { BOXHOMERESET, "HOME RESET", 30 }, { BOXGCSNAV, "GCS NAV", 31 }, + { BOXFPVANGLEMIX, "FPV ANGLE MIX", 32 }, { BOXSURFACE, "SURFACE", 33 }, { BOXFLAPERON, "FLAPERON", 34 }, { BOXTURNASSIST, "TURN ASSIST", 35 }, @@ -79,6 +81,10 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = { { BOXOSDALT2, "OSD ALT 2", 43 }, { BOXOSDALT3, "OSD ALT 3", 44 }, { BOXNAVCRUISE, "NAV CRUISE", 45 }, + { BOXBRAKING, "MC BRAKING", 46 }, + { BOXUSER1, "USER1", 47 }, + { BOXUSER2, "USER2", 48 }, + { BOXLOITERDIRCHN, "LOITER CHANGE", 49 }, { CHECKBOX_ITEM_COUNT, NULL, 0xFF } }; @@ -174,6 +180,8 @@ void initActiveBoxIds(void) activeBoxIds[activeBoxIdCount++] = BOXHEADADJ; } + activeBoxIds[activeBoxIdCount++] = BOXFPVANGLEMIX; + //Camstab mode is enabled always activeBoxIds[activeBoxIdCount++] = BOXCAMSTAB; @@ -188,6 +196,9 @@ void initActiveBoxIds(void) const bool navFlowDeadReckoning = sensors(SENSOR_OPFLOW) && sensors(SENSOR_ACC) && positionEstimationConfig()->allow_dead_reckoning; if (navFlowDeadReckoning || navReadyQuads || navReadyPlanes) { activeBoxIds[activeBoxIdCount++] = BOXNAVPOSHOLD; + if (STATE(FIXED_WING)) { + activeBoxIds[activeBoxIdCount++] = BOXLOITERDIRCHN; + } } if (navReadyQuads || navReadyPlanes) { @@ -202,6 +213,13 @@ void initActiveBoxIds(void) } } } + +#ifdef USE_MR_BRAKING_MODE + if (mixerConfig()->platformType == PLATFORM_MULTIROTOR) { + activeBoxIds[activeBoxIdCount++] = BOXBRAKING; + } +#endif + #endif if (STATE(FIXED_WING)) { @@ -257,6 +275,12 @@ void initActiveBoxIds(void) activeBoxIds[activeBoxIdCount++] = BOXCAMERA3; #endif +#ifdef USE_PINIOBOX + // USER modes are only used for PINIO at the moment + activeBoxIds[activeBoxIdCount++] = BOXUSER1; + activeBoxIds[activeBoxIdCount++] = BOXUSER2; +#endif + #if defined(USE_OSD) && defined(OSD_LAYOUT_COUNT) #if OSD_LAYOUT_COUNT > 0 activeBoxIds[activeBoxIdCount++] = BOXOSDALT1; @@ -287,6 +311,7 @@ void packBoxModeFlags(boxBitmask_t * mspBoxModeFlags) CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(HEADFREE_MODE)), BOXHEADFREE); CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXHEADADJ)), BOXHEADADJ); CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXCAMSTAB)), BOXCAMSTAB); + CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXFPVANGLEMIX)), BOXFPVANGLEMIX); CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(MANUAL_MODE)), BOXMANUAL); CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBEEPERON)), BOXBEEPERON); CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXLEDLOW)), BOXLEDLOW); @@ -319,6 +344,10 @@ void packBoxModeFlags(boxBitmask_t * mspBoxModeFlags) CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXOSDALT2)), BOXOSDALT2); CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXOSDALT3)), BOXOSDALT3); CHECK_ACTIVE_BOX(IS_ENABLED(navigationTerrainFollowingEnabled()), BOXSURFACE); + CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBRAKING)), BOXBRAKING); + CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXUSER1)), BOXUSER1); + CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXUSER2)), BOXUSER2); + CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXLOITERDIRCHN)),BOXLOITERDIRCHN); memset(mspBoxModeFlags, 0, sizeof(boxBitmask_t)); for (uint32_t i = 0; i < activeBoxIdCount; i++) { @@ -338,7 +367,8 @@ uint16_t packSensorStatus(void) IS_ENABLED(sensors(SENSOR_GPS)) << 3 | IS_ENABLED(sensors(SENSOR_RANGEFINDER)) << 4 | IS_ENABLED(sensors(SENSOR_OPFLOW)) << 5 | - IS_ENABLED(sensors(SENSOR_PITOT)) << 6; + IS_ENABLED(sensors(SENSOR_PITOT)) << 6 | + IS_ENABLED(sensors(SENSOR_TEMP)) << 7; // Hardware failure indication bit if (!isHardwareHealthy()) { diff --git a/src/main/fc/fc_msp_box.h b/src/main/fc/fc_msp_box.h index 0c3d1a8dfef..b5ca2559915 100644 --- a/src/main/fc/fc_msp_box.h +++ b/src/main/fc/fc_msp_box.h @@ -19,6 +19,8 @@ #include "fc/rc_modes.h" +#define PERMANENT_ID_NONE 255 // A permanent ID for no box mode + typedef struct box_s { const uint8_t boxId; // see boxId_e const char *boxName; // GUI-readable box name diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index 7c7ebd09401..f8bf2827f7a 100755 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -154,11 +154,13 @@ void taskUpdateBaro(timeUs_t currentTimeUs) { UNUSED(currentTimeUs); - if (sensors(SENSOR_BARO)) { - const uint32_t newDeadline = baroUpdate(); - if (newDeadline != 0) { - rescheduleTask(TASK_SELF, newDeadline); - } + if (!sensors(SENSOR_BARO)) { + return; + } + + const uint32_t newDeadline = baroUpdate(); + if (newDeadline != 0) { + rescheduleTask(TASK_SELF, newDeadline); } updatePositionEstimator_BaroTopic(currentTimeUs); @@ -168,11 +170,12 @@ void taskUpdateBaro(timeUs_t currentTimeUs) #ifdef USE_PITOT void taskUpdatePitot(timeUs_t currentTimeUs) { - UNUSED(currentTimeUs); - - if (sensors(SENSOR_PITOT)) { - pitotUpdate(); + if (!sensors(SENSOR_PITOT)) { + return; } + + pitotUpdate(); + updatePositionEstimator_PitotTopic(currentTimeUs); } #endif @@ -250,20 +253,6 @@ void taskSyncPwmDriver(timeUs_t currentTimeUs) } #endif -#ifdef USE_ASYNC_GYRO_PROCESSING -void taskAttitude(timeUs_t currentTimeUs) -{ - imuUpdateAttitude(currentTimeUs); -} - -void taskAcc(timeUs_t currentTimeUs) -{ - UNUSED(currentTimeUs); - - imuUpdateAccelerometer(); -} -#endif - #ifdef USE_OSD void taskUpdateOsd(timeUs_t currentTimeUs) { @@ -277,27 +266,8 @@ void fcTasksInit(void) { schedulerInit(); -#ifdef USE_ASYNC_GYRO_PROCESSING - rescheduleTask(TASK_PID, getPidUpdateRate()); - setTaskEnabled(TASK_PID, true); - - if (getAsyncMode() != ASYNC_MODE_NONE) { - rescheduleTask(TASK_GYRO, getGyroUpdateRate()); - setTaskEnabled(TASK_GYRO, true); - } - - if (getAsyncMode() == ASYNC_MODE_ALL && sensors(SENSOR_ACC)) { - rescheduleTask(TASK_ACC, getAccUpdateRate()); - setTaskEnabled(TASK_ACC, true); - - rescheduleTask(TASK_ATTI, getAttitudeUpdateRate()); - setTaskEnabled(TASK_ATTI, true); - } - -#else - rescheduleTask(TASK_GYROPID, getGyroUpdateRate()); + rescheduleTask(TASK_GYROPID, getLooptime()); setTaskEnabled(TASK_GYROPID, true); -#endif setTaskEnabled(TASK_SERIAL, true); #ifdef BEEPER @@ -376,51 +346,12 @@ cfTask_t cfTasks[TASK_COUNT] = { .desiredPeriod = TASK_PERIOD_HZ(10), // run every 100 ms, 10Hz .staticPriority = TASK_PRIORITY_HIGH, }, - - #ifdef USE_ASYNC_GYRO_PROCESSING - [TASK_PID] = { - .taskName = "PID", - .taskFunc = taskMainPidLoop, - .desiredPeriod = TASK_PERIOD_HZ(500), // Run at 500Hz - .staticPriority = TASK_PRIORITY_HIGH, - }, - - [TASK_GYRO] = { - .taskName = "GYRO", - .taskFunc = taskGyro, - .desiredPeriod = TASK_PERIOD_HZ(1000), //Run at 1000Hz - .staticPriority = TASK_PRIORITY_REALTIME, - }, - - [TASK_ACC] = { - .taskName = "ACC", - .taskFunc = taskAcc, - .desiredPeriod = TASK_PERIOD_HZ(520), //520Hz is ACC bandwidth (260Hz) * 2 - .staticPriority = TASK_PRIORITY_HIGH, - }, - - [TASK_ATTI] = { - .taskName = "ATTITUDE", - .taskFunc = taskAttitude, - .desiredPeriod = TASK_PERIOD_HZ(60), //With acc LPF at 15Hz 60Hz attitude refresh should be enough - .staticPriority = TASK_PRIORITY_HIGH, - }, - - #else - - /* - * Legacy synchronous PID/gyro/acc/atti mode - * for 64kB targets and other smaller targets - */ - - [TASK_GYROPID] = { - .taskName = "GYRO/PID", - .taskFunc = taskMainPidLoop, - .desiredPeriod = TASK_PERIOD_US(1000), - .staticPriority = TASK_PRIORITY_REALTIME, - }, - #endif - + [TASK_GYROPID] = { + .taskName = "GYRO/PID", + .taskFunc = taskMainPidLoop, + .desiredPeriod = TASK_PERIOD_US(1000), + .staticPriority = TASK_PRIORITY_REALTIME, + }, [TASK_SERIAL] = { .taskName = "SERIAL", .taskFunc = taskHandleSerial, @@ -456,7 +387,7 @@ cfTask_t cfTasks[TASK_COUNT] = { [TASK_TEMPERATURE] = { .taskName = "TEMPERATURE", .taskFunc = taskUpdateTemperature, - .desiredPeriod = TASK_PERIOD_HZ(1), // 1 Hz + .desiredPeriod = TASK_PERIOD_HZ(100), // 100 Hz .staticPriority = TASK_PRIORITY_LOW, }, @@ -499,7 +430,7 @@ cfTask_t cfTasks[TASK_COUNT] = { [TASK_PITOT] = { .taskName = "PITOT", .taskFunc = taskUpdatePitot, - .desiredPeriod = TASK_PERIOD_HZ(10), + .desiredPeriod = TASK_PERIOD_HZ(100), .staticPriority = TASK_PRIORITY_MEDIUM, }, #endif @@ -603,7 +534,7 @@ cfTask_t cfTasks[TASK_COUNT] = { }, #endif -#if defined(USE_VTX_COMMON) && defined(USE_VTX_CONTROL) +#if defined(USE_VTX_CONTROL) [TASK_VTXCTRL] = { .taskName = "VTXCTRL", .taskFunc = vtxUpdate, diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c index f7c3d49b29f..be595437492 100644 --- a/src/main/fc/rc_controls.c +++ b/src/main/fc/rc_controls.c @@ -235,7 +235,7 @@ void processRcStickPositions(throttleStatus_e throttleStatus) // GYRO calibration if (rcSticks == THR_LO + YAW_LO + PIT_LO + ROL_CE) { - gyroSetCalibrationCycles(CALIBRATING_GYRO_CYCLES); + gyroStartCalibration(); return; } @@ -317,7 +317,7 @@ void processRcStickPositions(throttleStatus_e throttleStatus) // Calibrating Acc if (rcSticks == THR_HI + YAW_LO + PIT_LO + ROL_CE) { - accSetCalibrationCycles(CALIBRATING_ACC_CYCLES); + accStartCalibration(); return; } diff --git a/src/main/fc/rc_modes.c b/src/main/fc/rc_modes.c index b523970f9bf..dfe2e5ec174 100755 --- a/src/main/fc/rc_modes.c +++ b/src/main/fc/rc_modes.c @@ -99,9 +99,12 @@ bool isRangeActive(uint8_t auxChannelIndex, const channelRange_t *range) return false; } - uint16_t channelValue = constrain(rcData[auxChannelIndex + NON_AUX_CHANNEL_COUNT], CHANNEL_RANGE_MIN, CHANNEL_RANGE_MAX - 1); - return (channelValue >= 900 + (range->startStep * 25) && - channelValue < 900 + (range->endStep * 25)); + // No need to constrain() here, since we're testing for a closed range defined + // by the channelRange_t. If channelValue has an invalid value, the test will + // be false anyway. + uint16_t channelValue = rcData[auxChannelIndex + NON_AUX_CHANNEL_COUNT]; + return (channelValue >= CHANNEL_RANGE_MIN + (range->startStep * CHANNEL_RANGE_STEP_WIDTH) && + channelValue < CHANNEL_RANGE_MIN + (range->endStep * CHANNEL_RANGE_STEP_WIDTH)); } void updateActivatedModes(void) diff --git a/src/main/fc/rc_modes.h b/src/main/fc/rc_modes.h index 2b4523e8372..1a90da24577 100755 --- a/src/main/fc/rc_modes.h +++ b/src/main/fc/rc_modes.h @@ -23,6 +23,8 @@ #include "config/parameter_group.h" +#define BOXID_NONE 255 + typedef enum { BOXARM = 0, BOXANGLE = 1, @@ -60,6 +62,11 @@ typedef enum { BOXOSDALT2 = 33, BOXOSDALT3 = 34, BOXNAVCRUISE = 35, + BOXBRAKING = 36, + BOXUSER1 = 37, + BOXUSER2 = 38, + BOXFPVANGLEMIX = 39, + BOXLOITERDIRCHN = 40, CHECKBOX_ITEM_COUNT } boxId_e; @@ -71,11 +78,13 @@ typedef struct boxBitmask_s { BITARRAY_DECLARE(bits, CHECKBOX_ITEM_COUNT); } box #define CHANNEL_RANGE_MIN 900 #define CHANNEL_RANGE_MAX 2100 -#define MODE_STEP_TO_CHANNEL_VALUE(step) (CHANNEL_RANGE_MIN + 25 * step) -#define CHANNEL_VALUE_TO_STEP(channelValue) ((constrain(channelValue, CHANNEL_RANGE_MIN, CHANNEL_RANGE_MAX) - CHANNEL_RANGE_MIN) / 25) +#define CHANNEL_RANGE_STEP_WIDTH 25 + +#define MODE_STEP_TO_CHANNEL_VALUE(step) (CHANNEL_RANGE_MIN + CHANNEL_RANGE_STEP_WIDTH * step) +#define CHANNEL_VALUE_TO_STEP(channelValue) ((constrain(channelValue, CHANNEL_RANGE_MIN, CHANNEL_RANGE_MAX) - CHANNEL_RANGE_MIN) / CHANNEL_RANGE_STEP_WIDTH) #define MIN_MODE_RANGE_STEP 0 -#define MAX_MODE_RANGE_STEP ((CHANNEL_RANGE_MAX - CHANNEL_RANGE_MIN) / 25) +#define MAX_MODE_RANGE_STEP ((CHANNEL_RANGE_MAX - CHANNEL_RANGE_MIN) / CHANNEL_RANGE_STEP_WIDTH) #define IS_RANGE_USABLE(range) ((range)->startStep < (range)->endStep) diff --git a/src/main/fc/rc_smoothing.c b/src/main/fc/rc_smoothing.c new file mode 100644 index 00000000000..acfa9c815c3 --- /dev/null +++ b/src/main/fc/rc_smoothing.c @@ -0,0 +1,110 @@ +/* + * This file is part of INAV. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute and/or modify + * it under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or (at your + * option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see http://www.gnu.org/licenses/. + */ + +#include +#include +#include + +#include "platform.h" + +#include "blackbox/blackbox.h" + +#include "build/debug.h" + +#include "common/maths.h" +#include "common/filter.h" + +#include "drivers/time.h" + +#include "rx/rx.h" + +#include "fc/config.h" +#include "fc/fc_core.h" +#include "fc/rc_controls.h" +#include "fc/rc_smoothing.h" +#include "fc/runtime_config.h" + +#include "flight/mixer.h" + +static biquadFilter_t rcSmoothFilter[4]; +static float rcStickUnfiltered[4]; + +static void rcInterpolationInit(int rcFilterFreqency) +{ + for (int stick = 0; stick < 4; stick++) { + biquadFilterInitLPF(&rcSmoothFilter[stick], rcFilterFreqency, getLooptime()); + } +} + +void rcInterpolationApply(bool isRXDataNew) +{ + static bool initDone = false; + static float initFilterFreqency = 0; + + if (isRXDataNew) { + if (!initDone || (initFilterFreqency != rxConfig()->rcFilterFrequency)) { + rcInterpolationInit(rxConfig()->rcFilterFrequency); + initFilterFreqency = rxConfig()->rcFilterFrequency; + initDone = true; + } + + for (int stick = 0; stick < 4; stick++) { + rcStickUnfiltered[stick] = rcCommand[stick]; + } + } + + // Don't filter if not initialized + if (!initDone) { + return; + } + + for (int stick = 0; stick < 4; stick++) { + rcCommand[stick] = biquadFilterApply(&rcSmoothFilter[stick], rcStickUnfiltered[stick]); + } +} + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/main/fc/rc_smoothing.h b/src/main/fc/rc_smoothing.h new file mode 100644 index 00000000000..4c1ad66bcb6 --- /dev/null +++ b/src/main/fc/rc_smoothing.h @@ -0,0 +1,31 @@ +/* + * This file is part of INAV. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute and/or modify + * it under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or (at your + * option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see http://www.gnu.org/licenses/. + */ + +#pragma once + +#include +#include +#include + +void rcInterpolationApply(bool isRXDataNew); \ No newline at end of file diff --git a/src/main/fc/runtime_config.h b/src/main/fc/runtime_config.h index 1dd5f13a81f..b84fa7ec357 100644 --- a/src/main/fc/runtime_config.h +++ b/src/main/fc/runtime_config.h @@ -100,7 +100,10 @@ typedef enum { COMPASS_CALIBRATED = (1 << 8), ACCELEROMETER_CALIBRATED= (1 << 9), PWM_DRIVER_AVAILABLE = (1 << 10), - HELICOPTER = (1 << 11) + HELICOPTER = (1 << 11), + NAV_CRUISE_BRAKING = (1 << 12), + NAV_CRUISE_BRAKING_BOOST = (1 << 13), + NAV_CRUISE_BRAKING_LOCKED = (1 << 14), } stateFlags_t; #define DISABLE_STATE(mask) (stateFlags &= ~(mask)) diff --git a/src/main/fc/settings.c b/src/main/fc/settings.c index 11ecedcd5f3..f426148aaa7 100644 --- a/src/main/fc/settings.c +++ b/src/main/fc/settings.c @@ -10,10 +10,54 @@ #include "settings_generated.c" +static bool settingGetWord(char *buf, int idx) +{ + if (idx == 0) { + return false; + } + const uint8_t *ptr = settingNamesWords; + char *bufPtr = buf; + int used_bits = 0; + int word = 1; + for(;;) { + int shift = 8 - SETTINGS_WORDS_BITS_PER_CHAR - used_bits; + char chr; + if (shift > 0) { + chr = (*ptr >> shift) & (0xff >> (8 - SETTINGS_WORDS_BITS_PER_CHAR)); + } else { + chr = (*ptr & (0xff >> (8 - (SETTINGS_WORDS_BITS_PER_CHAR + shift)))) << -shift; + ptr++; + chr |= (*ptr) >> (8 + shift); + } + if (word == idx) { + if (chr == 0) { + // Finished copying the word + *bufPtr++ = '\0'; + break; + } + char c; + if (chr < 27) { + c = 'a' + (chr - 1); + } else { + c = wordSymbols[chr - 27]; + } + *bufPtr++ = c; + } else { + if (chr == 0) { + // Word end + word++; + } + } + used_bits = (used_bits + SETTINGS_WORDS_BITS_PER_CHAR) % 8; + } + return true; +} + void settingGetName(const setting_t *val, char *buf) { uint8_t bpos = 0; uint16_t n = 0; + char word[SETTING_MAX_WORD_LENGTH]; #ifndef SETTING_ENCODED_NAME_USES_BYTE_INDEXING uint8_t shift = 0; #endif @@ -32,8 +76,7 @@ void settingGetName(const setting_t *val, char *buf) // Final byte n |= b << shift; #endif - const char *word = settingNamesWords[n]; - if (!word) { + if (!settingGetWord(word, n)) { // No more words break; } diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index deaaa3ae157..bb718961cd4 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -4,10 +4,10 @@ tables: - name: gyro_lpf values: ["256HZ", "188HZ", "98HZ", "42HZ", "20HZ", "10HZ"] - name: acc_hardware - values: ["NONE", "AUTO", "ADXL345", "MPU6050", "MMA845x", "BMA280", "LSM303DLHC", "MPU6000", "MPU6500", "MPU9250", "BMI160", "FAKE"] + values: ["NONE", "AUTO", "ADXL345", "MPU6050", "MMA845x", "BMA280", "LSM303DLHC", "MPU6000", "MPU6500", "MPU9250", "BMI160", "ICM20689", "FAKE"] enum: accelerationSensor_e - name: rangefinder_hardware - values: ["NONE", "HCSR04", "SRF10", "INAV_I2C", "VL53L0X", "MSP", "UIB"] + values: ["NONE", "HCSR04", "SRF10", "INAV_I2C", "VL53L0X", "MSP", "UIB", "BENEWAKE"] enum: rangefinderType_e - name: mag_hardware values: ["NONE", "AUTO", "HMC5883", "AK8975", "GPSMAG", "MAG3110", "AK8963", "IST8310", "QMC5883", "MPU9250", "IST8308", "LIS3MDL", "FAKE"] @@ -32,7 +32,7 @@ tables: - name: blackbox_device values: ["SERIAL", "SPIFLASH", "SDCARD"] - name: motor_pwm_protocol - values: ["STANDARD", "ONESHOT125", "ONESHOT42", "MULTISHOT", "BRUSHED"] + values: ["STANDARD", "ONESHOT125", "ONESHOT42", "MULTISHOT", "BRUSHED", "DSHOT150", "DSHOT300", "DSHOT600", "DSHOT1200"] - name: failsafe_procedure values: ["SET-THR", "DROP", "RTH", "NONE"] - name: current_sensor @@ -49,6 +49,8 @@ tables: enum: gpsDynModel_e - name: reset_type values: ["NEVER", "FIRST_ARM", "EACH_ARM"] + - name: direction + values: ["RIGHT", "LEFT", "YAW"] - name: nav_user_control_mode values: ["ATTI", "CRUISE"] - name: nav_rth_alt_mode @@ -62,6 +64,9 @@ tables: - name: osd_video_system values: ["AUTO", "PAL", "NTSC"] enum: videoSystem_e + - name: osd_alignment + values: ["LEFT", "RIGHT"] + enum: osd_alignment_e - name: frsky_unit values: ["METRIC", "IMPERIAL"] enum: frskyUnit_e @@ -72,7 +77,7 @@ tables: - name: debug_modes values: ["NONE", "GYRO", "NOTCH", "NAV_LANDING", "FW_ALTITUDE", "AGL", "FLOW_RAW", "FLOW", "SBUS", "FPORT", "ALWAYS", "STAGE2", "WIND_ESTIMATOR", "SAG_COMP_VOLTAGE", - "VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO"] + "VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO", "ACC", "GENERIC"] - name: async_mode values: ["NONE", "GYRO", "ALL"] - name: aux_operator @@ -146,7 +151,7 @@ groups: field: gyro_soft_notch_cutoff_2 condition: USE_GYRO_NOTCH_2 min: 1 - max: 500 + max: 500 - name: gyro_stage2_lowpass_hz field: gyro_stage2_lowpass_hz condition: USE_GYRO_BIQUAD_RC_FIR2 @@ -307,6 +312,10 @@ groups: - name: baro_median_filter field: use_median_filtering type: bool + - name: baro_cal_tolerance + field: baro_calibration_tolerance + min: 0 + max: 1000 - name: PG_PITOTMETER_CONFIG type: pitotmeterConfig_t @@ -315,12 +324,9 @@ groups: members: - name: pitot_hardware table: pitot_hardware - - name: pitot_use_median_filter - field: use_median_filtering - type: bool - - name: pitot_noise_lpf + - name: pitot_lpf_milli_hz min: 0 - max: 1 + max: 10000 - name: pitot_scale min: 0 max: 100 @@ -355,9 +361,10 @@ groups: field: sbusSyncInterval min: 500 max: 10000 - - name: rc_smoothing - field: rcSmoothing - type: bool + - name: rc_filter_frequency + field: rcFilterFrequency + min: 0 + max: 100 - name: serialrx_provider condition: USE_SERIAL_RX table: serial_rx @@ -396,11 +403,11 @@ groups: - name: blackbox_rate_num field: rate_num min: 1 - max: 255 + max: 65535 - name: blackbox_rate_denom field: rate_denom min: 1 - max: 255 + max: 65535 - name: blackbox_device field: device table: blackbox_device @@ -733,6 +740,10 @@ groups: field: manual.rates[FD_YAW] min: 0 max: 100 + - name: fpv_mix_degrees + field: misc.fpvCamAngleDegrees + min: 0 + max: 50 - name: PG_SERIAL_CONFIG type: serialConfig_t @@ -950,6 +961,10 @@ groups: field: fixedWingItermThrowLimit min: FW_ITERM_THROW_LIMIT_MIN max: FW_ITERM_THROW_LIMIT_MAX + - name: fw_loiter_direction + field: loiter_direction + condition: USE_NAV + table: direction - name: fw_reference_airspeed field: fixedWingReferenceAirspeed min: 1 @@ -958,6 +973,10 @@ groups: field: fixedWingCoordinatedYawGain min: 0 max: 2 + - name: fw_iterm_limit_stick_position + field: fixedWingItermLimitOnStickPosition + min: 0 + max: 1 - name: dterm_notch_hz field: dterm_soft_notch_hz condition: USE_DTERM_NOTCH @@ -1307,7 +1326,46 @@ groups: field: mc.auto_disarm_delay min: 100 max: 10000 - + - name: nav_mc_braking_speed_threshold + field: mc.braking_speed_threshold + condition: USE_MR_BRAKING_MODE + min: 0 + max: 1000 + - name: nav_mc_braking_disengage_speed + field: mc.braking_disengage_speed + condition: USE_MR_BRAKING_MODE + min: 0 + max: 1000 + - name: nav_mc_braking_timeout + field: mc.braking_timeout + condition: USE_MR_BRAKING_MODE + min: 100 + max: 5000 + - name: nav_mc_braking_boost_factor + field: mc.braking_boost_factor + condition: USE_MR_BRAKING_MODE + min: 0 + max: 200 + - name: nav_mc_braking_boost_timeout + field: mc.braking_boost_timeout + condition: USE_MR_BRAKING_MODE + min: 0 + max: 5000 + - name: nav_mc_braking_boost_speed_threshold + field: mc.braking_boost_speed_threshold + condition: USE_MR_BRAKING_MODE + min: 100 + max: 1000 + - name: nav_mc_braking_boost_disengage_speed + field: mc.braking_boost_disengage_speed + condition: USE_MR_BRAKING_MODE + min: 0 + max: 1000 + - name: nav_mc_braking_bank_angle + field: mc.braking_bank_angle + condition: USE_MR_BRAKING_MODE + min: 15 + max: 60 - name: nav_fw_cruise_thr field: fw.cruise_throttle min: 1000 @@ -1541,10 +1599,37 @@ groups: field: neg_alt_alarm min: 0 max: 10000 + - name: osd_imu_temp_alarm_min + field: imu_temp_alarm_min + min: -550 + max: 1250 + - name: osd_imu_temp_alarm_max + field: imu_temp_alarm_max + min: -550 + max: 1250 + - name: osd_baro_temp_alarm_min + field: baro_temp_alarm_min + condition: USE_BARO + min: -550 + max: 1250 + - name: osd_baro_temp_alarm_max + field: baro_temp_alarm_max + condition: USE_BARO + min: -550 + max: 1250 + - name: osd_temp_label_align + field: temp_label_align + condition: USE_TEMPERATURE_SENSOR + table: osd_alignment + type: uint8_t - name: osd_artificial_horizon_reverse_roll field: ahi_reverse_roll type: bool + - name: osd_artificial_horizon_max_pitch + field: ahi_max_pitch + min: 10 + max: 90 - name: osd_crosshairs_style field: crosshairs_style table: osd_crosshairs_style @@ -1574,6 +1659,14 @@ groups: field: estimations_wind_compensation type: bool + - name: osd_failsafe_switch_layout + type: bool + + - name: osd_plus_code_digits + field: plus_code_digits + min: 10 + max: 13 + - name: PG_SYSTEM_CONFIG type: systemConfig_t headers: ["fc/config.h"] @@ -1587,20 +1680,6 @@ groups: type: bool - name: debug_mode table: debug_modes - - name: acc_task_frequency - field: accTaskFrequency - condition: USE_ASYNC_GYRO_PROCESSING - min: ACC_TASK_FREQUENCY_MIN - max: ACC_TASK_FREQUENCY_MAX - - name: attitude_task_frequency - field: attitudeTaskFrequency - condition: USE_ASYNC_GYRO_PROCESSING - min: ATTITUDE_TASK_FREQUENCY_MIN - max: ATTITUDE_TASK_FREQUENCY_MAX - - name: async_mode - field: asyncMode - condition: USE_ASYNC_GYRO_PROCESSING - table: async_mode - name: throttle_tilt_comp_str field: throttle_tilt_compensation_strength min: 0 @@ -1657,7 +1736,7 @@ groups: - name: PG_VTX_CONFIG type: vtxConfig_t headers: ["io/vtx_control.h"] - condition: USE_VTX_CONTROL && USE_VTX_COMMON + condition: USE_VTX_CONTROL members: - name: vtx_halfduplex field: halfDuplex @@ -1666,7 +1745,6 @@ groups: - name: PG_VTX_SETTINGS_CONFIG type: vtxSettingsConfig_t headers: ["drivers/vtx_common.h", "io/vtx.h"] - condition: USE_VTX_COMMON members: - name: vtx_band field: band @@ -1694,3 +1772,29 @@ groups: min: VTX_SETTINGS_MIN_FREQUENCY_MHZ max: VTX_SETTINGS_MAX_FREQUENCY_MHZ condition: VTX_SETTINGS_FREQCMD + + - name: PG_PINIOBOX_CONFIG + type: pinioBoxConfig_t + headers: ["io/piniobox.h"] + condition: USE_PINIOBOX + members: + - name: pinio_box1 + field: permanentId[0] + min: 0 + max: 255 + type: uint8_t + - name: pinio_box2 + field: permanentId[1] + min: 0 + max: 255 + type: uint8_t + - name: pinio_box3 + field: permanentId[2] + min: 0 + max: 255 + type: uint8_t + - name: pinio_box4 + field: permanentId[3] + min: 0 + max: 255 + type: uint8_t diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index 3b219a42e35..8f0bb9a7ebb 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -75,7 +75,6 @@ #define SPIN_RATE_LIMIT 20 #define MAX_ACC_SQ_NEARNESS 25 // 25% or G^2, accepted acceleration of (0.87 - 1.12G) -#define MAX_GPS_HEADING_ERROR_DEG 60 // Amount of error between GPS CoG and estimated Yaw at witch we stop trusting GPS and fallback to MAG FASTRAM fpVector3_t imuMeasuredAccelBF; FASTRAM fpVector3_t imuMeasuredRotationBF; @@ -236,27 +235,48 @@ static void imuResetOrientationQuaternion(const fpVector3_t * accBF) quaternionNormalize(&orientation, &orientation); } -static void imuCheckAndResetOrientationQuaternion(const fpVector3_t * accBF) +static bool imuValidateQuaternion(const fpQuaternion_t * quat) { - // Check if some calculation in IMU update yield NAN or zero quaternion - // Reset quaternion from accelerometer - this might be incorrect, but it's better than no attitude at all - const float check = fabs(orientation.q0) + fabs(orientation.q1) + fabs(orientation.q2) + fabs(orientation.q3); + const float check = fabs(quat->q0) + fabs(quat->q1) + fabs(quat->q2) + fabs(quat->q3); if (!isnan(check) && !isinf(check)) { - return; + return true; } const float normSq = quaternionNormSqared(&orientation); if (normSq > (1.0f - 1e-6f) && normSq < (1.0f + 1e-6f)) { + return true; + } + + return false; +} + +static void imuCheckAndResetOrientationQuaternion(const fpQuaternion_t * quat, const fpVector3_t * accBF) +{ + // Check if some calculation in IMU update yield NAN or zero quaternion + if (imuValidateQuaternion(&orientation)) { return; } - imuResetOrientationQuaternion(accBF); - DEBUG_TRACE("AHRS orientation quaternion error"); + flightLogEvent_IMUError_t imuErrorEvent; + + // Orientation is invalid. We need to reset it + if (imuValidateQuaternion(quat)) { + // Previous quaternion valid. Reset to it + orientation = *quat; + imuErrorEvent.errorCode = 1; + DEBUG_TRACE("AHRS orientation quaternion error. Reset to last known good value"); + } + else { + // No valid reference. Best guess from accelerometer + imuResetOrientationQuaternion(accBF); + imuErrorEvent.errorCode = 2; + DEBUG_TRACE("AHRS orientation quaternion error. Best guess from ACC"); + } #ifdef USE_BLACKBOX if (feature(FEATURE_BLACKBOX)) { - blackboxLogEvent(FLIGHT_LOG_EVENT_IMU_FAILURE, NULL); + blackboxLogEvent(FLIGHT_LOG_EVENT_IMU_FAILURE, (flightLogEventData_t*)&imuErrorEvent); } #endif } @@ -265,6 +285,7 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe { STATIC_FASTRAM fpVector3_t vGyroDriftEstimate = { 0 }; + fpQuaternion_t prevOrientation = orientation; fpVector3_t vRotation = *gyroBF; /* Calculate general spin rate (rad/s) */ @@ -290,15 +311,18 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe // Ignore magnetic inclination vMag.z = 0.0f; - // Normalize to unit vector - vectorNormalize(&vMag, &vMag); + // We zeroed out vMag.z - make sure the whole vector didn't go to zero + if (vectorNormSquared(&vMag) > 0.01f) { + // Normalize to unit vector + vectorNormalize(&vMag, &vMag); - // Reference mag field vector heading is Magnetic North in EF. We compute that by rotating True North vector by declination and assuming Z-component is zero - // magnetometer error is cross product between estimated magnetic north and measured magnetic north (calculated in EF) - vectorCrossProduct(&vErr, &vMag, &vCorrectedMagNorth); + // Reference mag field vector heading is Magnetic North in EF. We compute that by rotating True North vector by declination and assuming Z-component is zero + // magnetometer error is cross product between estimated magnetic north and measured magnetic north (calculated in EF) + vectorCrossProduct(&vErr, &vMag, &vCorrectedMagNorth); - // Rotate error back into body frame - quaternionRotateVector(&vErr, &vErr, &orientation); + // Rotate error back into body frame + quaternionRotateVector(&vErr, &vErr, &orientation); + } } else if (useCOG) { fpVector3_t vHeadingEF; @@ -318,14 +342,17 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe quaternionRotateVectorInv(&vHeadingEF, &vForward, &orientation); vHeadingEF.z = 0.0f; - // Normalize to unit vector - vectorNormalize(&vHeadingEF, &vHeadingEF); + // We zeroed out vHeadingEF.z - make sure the whole vector didn't go to zero + if (vectorNormSquared(&vHeadingEF) > 0.01f) { + // Normalize to unit vector + vectorNormalize(&vHeadingEF, &vHeadingEF); - // error is cross product between reference heading and estimated heading (calculated in EF) - vectorCrossProduct(&vErr, &vCoG, &vHeadingEF); + // error is cross product between reference heading and estimated heading (calculated in EF) + vectorCrossProduct(&vErr, &vCoG, &vHeadingEF); - // Rotate error back into body frame - quaternionRotateVector(&vErr, &vErr, &orientation); + // Rotate error back into body frame + quaternionRotateVector(&vErr, &vErr, &orientation); + } } // Compute and apply integral feedback if enabled @@ -408,8 +435,8 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe quaternionNormalize(&orientation, &orientation); } - // Check for invalid quaternion - imuCheckAndResetOrientationQuaternion(accBF); + // Check for invalid quaternion and reset to previous known good one + imuCheckAndResetOrientationQuaternion(&prevOrientation, accBF); // Pre-compute rotation matrix from quaternion imuComputeRotationMatrix(); @@ -465,13 +492,15 @@ static void imuCalculateEstimatedAttitude(float dT) if (STATE(FIXED_WING)) { bool canUseCOG = isGPSHeadingValid(); - if (canUseCOG) { + // Prefer compass (if available) + if (canUseMAG) { + useMag = true; + gpsHeadingInitialized = true; // GPS heading initialised from MAG, continue on GPS if compass fails + } + else if (canUseCOG) { if (gpsHeadingInitialized) { - // Use GPS heading if error is acceptable or if it's the only source of heading - if (ABS(gpsSol.groundCourse - attitude.values.yaw) < DEGREES_TO_DECIDEGREES(MAX_GPS_HEADING_ERROR_DEG) || !canUseMAG) { - courseOverGround = DECIDEGREES_TO_RADIANS(gpsSol.groundCourse); - useCOG = true; - } + courseOverGround = DECIDEGREES_TO_RADIANS(gpsSol.groundCourse); + useCOG = true; } else { // Re-initialize quaternion from known Roll, Pitch and GPS heading @@ -481,15 +510,6 @@ static void imuCalculateEstimatedAttitude(float dT) // Force reset of heading hold target resetHeadingHoldTarget(DECIDEGREES_TO_DEGREES(attitude.values.yaw)); } - - // If we can't use COG and there's MAG available - fallback - if (!useCOG && canUseMAG) { - useMag = true; - } - } - else if (canUseMAG) { - useMag = true; - gpsHeadingInitialized = true; // GPS heading initialised from MAG, continue on GPS if possible } } else { diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index df13efa515f..5e9e484f4eb 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -101,6 +101,7 @@ PG_RESET_TEMPLATE(motorConfig_t, motorConfig, .mincommand = 1000, .motorAccelTimeMs = 0, .motorDecelTimeMs = 0, + .digitalIdleOffsetValue = 450 // Same scale as in Betaflight ); static motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS]; @@ -181,7 +182,45 @@ void mixerResetDisarmedMotors(void) void writeMotors(void) { for (int i = 0; i < motorCount; i++) { - pwmWriteMotor(i, motor[i]); + uint16_t motorValue; + +#ifdef USE_DSHOT + // If we use DSHOT we need to convert motorValue to DSHOT ranges + if (isMotorProtocolDshot()) { + const float dshotMinThrottleOffset = (DSHOT_MAX_THROTTLE - DSHOT_MIN_THROTTLE) / 10000.0f * motorConfig()->digitalIdleOffsetValue; + + if (feature(FEATURE_3D)) { + if (motor[i] >= motorConfig()->minthrottle && motor[i] <= flight3DConfig()->deadband3d_low) { + motorValue = scaleRangef(motor[i], motorConfig()->minthrottle, flight3DConfig()->deadband3d_low, DSHOT_3D_DEADBAND_LOW, dshotMinThrottleOffset + DSHOT_MIN_THROTTLE); + motorValue = constrain(motorValue, DSHOT_MIN_THROTTLE, DSHOT_3D_DEADBAND_LOW); + } + else if (motor[i] >= flight3DConfig()->deadband3d_high && motor[i] <= motorConfig()->maxthrottle) { + motorValue = scaleRangef(motor[i], flight3DConfig()->deadband3d_high, motorConfig()->maxthrottle, dshotMinThrottleOffset + DSHOT_3D_DEADBAND_HIGH, DSHOT_MAX_THROTTLE); + motorValue = constrain(motorValue, DSHOT_3D_DEADBAND_HIGH, DSHOT_MAX_THROTTLE); + } + else { + motorValue = DSHOT_DISARM_COMMAND; + } + } + else { + if (motor[i] < motorConfig()->minthrottle) { // motor disarmed + motorValue = DSHOT_DISARM_COMMAND; + } + else { + motorValue = scaleRangef(motor[i], motorConfig()->minthrottle, motorConfig()->maxthrottle, (dshotMinThrottleOffset + DSHOT_MIN_THROTTLE), DSHOT_MAX_THROTTLE); + motorValue = constrain(motorValue, (dshotMinThrottleOffset + DSHOT_MIN_THROTTLE), DSHOT_MAX_THROTTLE); + } + } + } + else { + motorValue = motor[i]; + } +#else + // We don't define USE_DSHOT + motorValue = motor[i]; +#endif + + pwmWriteMotor(i, motorValue); } } diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index 97bf57cde95..5d4df59e006 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -30,6 +30,13 @@ #define FW_MIN_THROTTLE_DOWN_PITCH_ANGLE_MAX 450 +// Digital protocol has fixed values +#define DSHOT_DISARM_COMMAND 0 +#define DSHOT_MIN_THROTTLE 48 +#define DSHOT_MAX_THROTTLE 2047 +#define DSHOT_3D_DEADBAND_LOW 1047 +#define DSHOT_3D_DEADBAND_HIGH 1048 + typedef enum { PLATFORM_MULTIROTOR = 0, PLATFORM_AIRPLANE = 1, @@ -83,6 +90,7 @@ typedef struct motorConfig_s { uint8_t motorPwmProtocol; uint16_t motorAccelTimeMs; // Time limit for motor to accelerate from 0 to 100% throttle [ms] uint16_t motorDecelTimeMs; // Time limit for motor to decelerate from 0 to 100% throttle [ms] + uint16_t digitalIdleOffsetValue; } motorConfig_t; PG_DECLARE(motorConfig_t, motorConfig); diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 02a31b4f260..2be7449788f 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -34,6 +34,7 @@ #include "fc/config.h" #include "fc/controlrate_profile.h" #include "fc/rc_controls.h" +#include "fc/rc_modes.h" #include "fc/runtime_config.h" #include "flight/pid.h" @@ -84,6 +85,7 @@ typedef struct { #ifdef USE_DTERM_NOTCH biquadFilter_t deltaNotchFilter; #endif + float stickPosition; } pidState_t; #ifdef USE_DTERM_NOTCH @@ -107,7 +109,7 @@ int32_t axisPID_P[FLIGHT_DYNAMICS_INDEX_COUNT], axisPID_I[FLIGHT_DYNAMICS_INDEX_ STATIC_FASTRAM pidState_t pidState[FLIGHT_DYNAMICS_INDEX_COUNT]; -PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 4); +PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 6); PG_RESET_TEMPLATE(pidProfile_t, pidProfile, .bank_mc = { @@ -168,7 +170,6 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile, } }, - .acc_soft_lpf_hz = 15, .dterm_soft_notch_hz = 0, .dterm_soft_notch_cutoff = 1, .dterm_lpf_hz = 40, @@ -191,6 +192,9 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile, .fixedWingItermThrowLimit = FW_ITERM_THROW_LIMIT_DEFAULT, .fixedWingReferenceAirspeed = 1000, .fixedWingCoordinatedYawGain = 1.0f, + .fixedWingItermLimitOnStickPosition = 0.5f, + + .loiter_direction = NAV_LOITER_RIGHT, ); void pidInit(void) @@ -214,14 +218,14 @@ void pidInit(void) bool pidInitFilters(void) { - const uint32_t refreshRate = getPidUpdateRate(); - notchFilterApplyFn = nullFilterApply; + const uint32_t refreshRate = getLooptime(); if (refreshRate == 0) { return false; } #ifdef USE_DTERM_NOTCH + notchFilterApplyFn = nullFilterApply; if (pidProfile()->dterm_soft_notch_hz != 0) { notchFilterApplyFn = (filterApplyFnPtr)biquadFilterApply; for (int axis = 0; axis < 3; ++ axis) { @@ -241,7 +245,7 @@ bool pidInitFilters(void) void pidResetTPAFilter(void) { if (STATE(FIXED_WING) && currentControlRateProfile->throttle.fixedWingTauMs > 0) { - pt1FilterInitRC(&fixedWingTpaFilter, currentControlRateProfile->throttle.fixedWingTauMs * 1e-3f, getPidUpdateRate() * 1e-6f); + pt1FilterInitRC(&fixedWingTpaFilter, currentControlRateProfile->throttle.fixedWingTauMs * 1e-3f, getLooptime() * 1e-6f); pt1FilterReset(&fixedWingTpaFilter, motorConfig()->minthrottle); } } @@ -352,6 +356,13 @@ void updatePIDCoefficients(void) } } + /* + * Compute stick position in range of [-1.0f : 1.0f] without deadband and expo + */ + for (int axis = 0; axis < 3; axis++) { + pidState[axis].stickPosition = constrain(rcData[axis] - PWM_RANGE_MIDDLE, -500, 500) / 500.0f; + } + // If nothing changed - don't waste time recalculating coefficients if (!pidGainsUpdateRequired) { return; @@ -457,6 +468,19 @@ static void pidApplySetpointRateLimiting(pidState_t *pidState, flight_dynamics_i } } +bool isFixedWingItermLimitActive(float stickPosition) +{ + /* + * Iterm anti windup whould be active only when pilot controls the rotation + * velocity directly, not when ANGLE or HORIZON are used + */ + if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) { + return false; + } + + return fabsf(stickPosition) > pidProfile()->fixedWingItermLimitOnStickPosition; +} + static void pidApplyFixedWingRateController(pidState_t *pidState, flight_dynamics_index_t axis) { const float rateError = pidState->rateTarget - pidState->gyroRate; @@ -473,10 +497,10 @@ static void pidApplyFixedWingRateController(pidState_t *pidState, flight_dynamic // Calculate integral pidState->errorGyroIf += rateError * pidState->kI * dT; - if (STATE(ANTI_WINDUP)) { + if (STATE(ANTI_WINDUP) || isFixedWingItermLimitActive(pidState->stickPosition)) { pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -pidState->errorGyroIfLimit, pidState->errorGyroIfLimit); } else { - pidState->errorGyroIfLimit = ABS(pidState->errorGyroIf); + pidState->errorGyroIfLimit = fabsf(pidState->errorGyroIf); } if (pidProfile()->fixedWingItermThrowLimit != 0) { @@ -557,7 +581,7 @@ static void pidApplyMulticopterRateController(pidState_t *pidState, flight_dynam if (STATE(ANTI_WINDUP) || mixerIsOutputSaturated()) { pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -pidState->errorGyroIfLimit, pidState->errorGyroIfLimit); } else { - pidState->errorGyroIfLimit = ABS(pidState->errorGyroIf); + pidState->errorGyroIfLimit = fabsf(pidState->errorGyroIf); } axisPID[axis] = newOutputLimited; @@ -731,8 +755,28 @@ static void pidTurnAssistant(pidState_t *pidState) } } +static void pidApplyFpvCameraAngleMix(pidState_t *pidState, uint8_t fpvCameraAngle) +{ + static uint8_t lastFpvCamAngleDegrees = 0; + static float cosCameraAngle = 1.0; + static float sinCameraAngle = 0.0; + + if (lastFpvCamAngleDegrees != fpvCameraAngle) { + lastFpvCamAngleDegrees = fpvCameraAngle; + cosCameraAngle = cos_approx(DEGREES_TO_RADIANS(fpvCameraAngle)); + sinCameraAngle = sin_approx(DEGREES_TO_RADIANS(fpvCameraAngle)); + } + + // Rotate roll/yaw command from camera-frame coordinate system to body-frame coordinate system + const float rollRate = pidState[ROLL].rateTarget; + const float yawRate = pidState[YAW].rateTarget; + pidState[ROLL].rateTarget = constrainf(rollRate * cosCameraAngle - yawRate * sinCameraAngle, -GYRO_SATURATION_LIMIT, GYRO_SATURATION_LIMIT); + pidState[YAW].rateTarget = constrainf(yawRate * cosCameraAngle + rollRate * sinCameraAngle, -GYRO_SATURATION_LIMIT, GYRO_SATURATION_LIMIT); +} + void pidController(void) { + bool canUseFpvCameraMix = true; uint8_t headingHoldState = getHeadingHoldState(); if (headingHoldState == HEADING_HOLD_UPDATE_HEADING) { @@ -761,10 +805,16 @@ void pidController(void) const float horizonRateMagnitude = calcHorizonRateMagnitude(); pidLevel(&pidState[FD_ROLL], FD_ROLL, horizonRateMagnitude); pidLevel(&pidState[FD_PITCH], FD_PITCH, horizonRateMagnitude); + canUseFpvCameraMix = false; // FPVANGLEMIX is incompatible with ANGLE/HORIZON } if (FLIGHT_MODE(TURN_ASSISTANT) || navigationRequiresTurnAssistance()) { pidTurnAssistant(pidState); + canUseFpvCameraMix = false; // FPVANGLEMIX is incompatible with TURN_ASSISTANT + } + + if (canUseFpvCameraMix && IS_RC_MODE_ACTIVE(BOXFPVANGLEMIX) && currentControlRateProfile->misc.fpvCamAngleDegrees) { + pidApplyFpvCameraAngleMix(pidState, currentControlRateProfile->misc.fpvCamAngleDegrees); } // Apply setpoint rate of change limits diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 68a28e675bf..a0943320890 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -84,7 +84,6 @@ typedef struct pidProfile_s { uint8_t dterm_lpf_hz; // (default 17Hz, Range 1-50Hz) Used for PT1 element in PID1, PID2 and PID5 uint8_t yaw_pterm_lpf_hz; // Used for filering Pterm noise on noisy frames - uint8_t acc_soft_lpf_hz; // Set the Low Pass Filter factor for ACC. Reducing this value would reduce ACC noise (visible in GUI), but would increase ACC lag time. Zero = no filter uint8_t yaw_lpf_hz; uint16_t yaw_p_limit; @@ -104,6 +103,9 @@ typedef struct pidProfile_s { uint16_t fixedWingItermThrowLimit; float fixedWingReferenceAirspeed; // Reference tuning airspeed for the airplane - the speed for which PID gains are tuned float fixedWingCoordinatedYawGain; // This is the gain of the yaw rate required to keep the yaw rate consistent with the turn rate for a coordinated turn. + float fixedWingItermLimitOnStickPosition; //Do not allow Iterm to grow when stick position is above this point + + uint8_t loiter_direction; // Direction of loitering center point on right wing (clockwise - as before), or center point on left wing (counterclockwise) } pidProfile_t; typedef struct pidAutotuneConfig_s { diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c index ca8b8ab4282..71b71683c77 100755 --- a/src/main/flight/servos.c +++ b/src/main/flight/servos.c @@ -64,7 +64,7 @@ PG_RESET_TEMPLATE(servoConfig_t, servoConfig, PG_REGISTER_ARRAY(servoMixer_t, MAX_SERVO_RULES, customServoMixers, PG_SERVO_MIXER, 0); -PG_REGISTER_ARRAY_WITH_RESET_FN(servoParam_t, MAX_SUPPORTED_SERVOS, servoParams, PG_SERVO_PARAMS, 1); +PG_REGISTER_ARRAY_WITH_RESET_FN(servoParam_t, MAX_SUPPORTED_SERVOS, servoParams, PG_SERVO_PARAMS, 2); void pgResetFn_servoParams(servoParam_t *instance) { @@ -103,15 +103,6 @@ int16_t getFlaperonDirection(uint8_t servoPin) } } -int servoDirection(int servoIndex, int inputSource) -{ - // determine the direction (reversed or not) from the direction bitfield of the servo - if (servoParams(servoIndex)->reversedSources & (1 << inputSource)) - return -1; - else - return 1; -} - /* * Compute scaling factor for upper and lower servo throw */ @@ -240,8 +231,8 @@ void servoMixer(float dT) input[INPUT_FEATURE_FLAPS] = FLIGHT_MODE(FLAPERON) ? servoConfig()->flaperon_throw_offset : 0; if (IS_RC_MODE_ACTIVE(BOXCAMSTAB)) { - input[INPUT_GIMBAL_PITCH] = scaleRange(attitude.values.pitch, -1800, 1800, -360, +360); - input[INPUT_GIMBAL_ROLL] = scaleRange(attitude.values.roll, -1800, 1800, -360, +360); + input[INPUT_GIMBAL_PITCH] = scaleRange(attitude.values.pitch, -900, 900, -500, +500); + input[INPUT_GIMBAL_ROLL] = scaleRange(attitude.values.roll, -1800, 1800, -500, +500); } else { input[INPUT_GIMBAL_PITCH] = 0; input[INPUT_GIMBAL_ROLL] = 0; @@ -290,7 +281,7 @@ void servoMixer(float dT) */ int16_t inputLimited = (int16_t) rateLimitFilterApply4(&servoSpeedLimitFilter[i], input[from], currentServoMixer[i].speed * 10, dT); - servo[target] += servoDirection(target, from) * ((int32_t)inputLimited * currentServoMixer[i].rate) / 100; + servo[target] += ((int32_t)inputLimited * currentServoMixer[i].rate) / 100; } for (int i = 0; i < MAX_SUPPORTED_SERVOS; i++) { @@ -415,4 +406,4 @@ bool isServoOutputEnabled(void) bool isMixerUsingServos(void) { return mixerUsesServos; -} \ No newline at end of file +} diff --git a/src/main/flight/servos.h b/src/main/flight/servos.h index c6fc0a2ad39..20f988acb4a 100644 --- a/src/main/flight/servos.h +++ b/src/main/flight/servos.h @@ -21,8 +21,8 @@ #define MAX_SUPPORTED_SERVOS 8 -// These must be consecutive, see 'reversedSources' -enum { +// These must be consecutive +typedef enum { INPUT_STABILIZED_ROLL = 0, INPUT_STABILIZED_PITCH = 1, INPUT_STABILIZED_YAW = 2, @@ -91,7 +91,7 @@ typedef enum { typedef struct servoMixer_s { uint8_t targetChannel; // servo that receives the output of the rule uint8_t inputSource; // input channel for this rule - int8_t rate; // range [-125;+125] ; can be used to adjust a rate 0-125% and a direction + int16_t rate; // range [-1000;+1000] ; can be used to adjust a rate 0-1000% and a direction uint8_t speed; // reduces the speed of the rule, 0=unlimited speed } servoMixer_t; @@ -104,7 +104,6 @@ typedef struct servoMixer_s { PG_DECLARE_ARRAY(servoMixer_t, MAX_SERVO_RULES, customServoMixers); typedef struct servoParam_s { - uint32_t reversedSources; // the direction of servo movement for each input source of the servo mixer, bit set=inverted int16_t min; // servo min int16_t max; // servo max int16_t middle; // servo middle @@ -136,7 +135,6 @@ bool isServoOutputEnabled(void); bool isMixerUsingServos(void); void writeServos(void); void loadCustomServoMixer(void); -int servoDirection(int servoIndex, int fromChannel); void servoMixer(float dT); void servoComputeScalingFactors(uint8_t servoIndex); void servosInit(void); diff --git a/src/main/io/asyncfatfs/asyncfatfs.c b/src/main/io/asyncfatfs/asyncfatfs.c index 53a7fb59817..a8adb363e92 100644 --- a/src/main/io/asyncfatfs/asyncfatfs.c +++ b/src/main/io/asyncfatfs/asyncfatfs.c @@ -418,11 +418,6 @@ typedef enum { AFATFS_INITIALIZATION_FREEFILE_LAST = AFATFS_INITIALIZATION_FREEFILE_SAVE_DIR_ENTRY, #endif -#ifdef AFATFS_USE_INTROSPECTIVE_LOGGING - AFATFS_INITIALIZATION_INTROSPEC_LOG_CREATE, - AFATFS_INITIALIZATION_INTROSPEC_LOG_CREATING, -#endif - AFATFS_INITIALIZATION_DONE } afatfsInitializationPhase_e; @@ -453,10 +448,6 @@ typedef struct afatfs_t { afatfsFile_t freeFile; #endif -#ifdef AFATFS_USE_INTROSPECTIVE_LOGGING - afatfsFile_t introSpecLog; -#endif - afatfsError_e lastError; bool filesystemFull; @@ -3225,10 +3216,6 @@ static void afatfs_fileOperationsPoll(void) { afatfs_fileOperationContinue(&afatfs.currentDirectory); -#ifdef AFATFS_USE_INTROSPECTIVE_LOGGING - afatfs_fileOperationContinue(&afatfs.introSpecLog); -#endif - for (int i = 0; i < AFATFS_MAX_OPEN_FILES; i++) { afatfs_fileOperationContinue(&afatfs.openFiles[i]); } @@ -3357,20 +3344,6 @@ static void afatfs_freeFileCreated(afatfsFile_t *file) #endif -#ifdef AFATFS_USE_INTROSPECTIVE_LOGGING - -static void afatfs_introspecLogCreated(afatfsFile_t *file) -{ - if (file) { - afatfs.initPhase++; - } else { - afatfs.lastError = AFATFS_ERROR_GENERIC; - afatfs.filesystemState = AFATFS_FILESYSTEM_STATE_FATAL; - } -} - -#endif - static void afatfs_initContinue(void) { #ifdef AFATFS_USE_FREEFILE @@ -3479,18 +3452,6 @@ static void afatfs_initContinue(void) break; #endif -#ifdef AFATFS_USE_INTROSPECTIVE_LOGGING - case AFATFS_INITIALIZATION_INTROSPEC_LOG_CREATE: - afatfs.initPhase = AFATFS_INITIALIZATION_INTROSPEC_LOG_CREATING; - - afatfs_createFile(&afatfs.introSpecLog, AFATFS_INTROSPEC_LOG_FILENAME, FAT_FILE_ATTRIBUTE_ARCHIVE, - AFATFS_FILE_MODE_CREATE | AFATFS_FILE_MODE_APPEND, afatfs_introspecLogCreated); - break; - case AFATFS_INITIALIZATION_INTROSPEC_LOG_CREATING: - afatfs_fileOperationContinue(&afatfs.introSpecLog); - break; -#endif - case AFATFS_INITIALIZATION_DONE: afatfs.filesystemState = AFATFS_FILESYSTEM_STATE_READY; break; @@ -3520,50 +3481,6 @@ void afatfs_poll(void) } } -#ifdef AFATFS_USE_INTROSPECTIVE_LOGGING - -void afatfs_sdcardProfilerCallback(sdcardBlockOperation_e operation, uint32_t blockIndex, uint32_t duration) -{ - // Make sure the log file has actually been opened before we try to log to it: - if (afatfs.introSpecLog.type == AFATFS_FILE_TYPE_NONE) { - return; - } - - enum { - LOG_ENTRY_SIZE = 16 // Log entry size should be a power of two to avoid partial fwrites() - }; - - uint8_t buffer[LOG_ENTRY_SIZE]; - - buffer[0] = operation; - - // Padding/reserved: - buffer[1] = 0; - buffer[2] = 0; - buffer[3] = 0; - - buffer[4] = blockIndex & 0xFF; - buffer[5] = (blockIndex >> 8) & 0xFF; - buffer[6] = (blockIndex >> 16) & 0xFF; - buffer[7] = (blockIndex >> 24) & 0xFF; - - buffer[8] = duration & 0xFF; - buffer[9] = (duration >> 8) & 0xFF; - buffer[10] = (duration >> 16) & 0xFF; - buffer[11] = (duration >> 24) & 0xFF; - - // Padding/reserved: - buffer[12] = 0; - buffer[13] = 0; - buffer[14] = 0; - buffer[15] = 0; - - // Ignore write failures - afatfs_fwrite(&afatfs.introSpecLog, buffer, LOG_ENTRY_SIZE); -} - -#endif - afatfsFilesystemState_e afatfs_getFilesystemState(void) { return afatfs.filesystemState; @@ -3579,10 +3496,6 @@ void afatfs_init(void) afatfs.filesystemState = AFATFS_FILESYSTEM_STATE_INITIALIZATION; afatfs.initPhase = AFATFS_INITIALIZATION_READ_MBR; afatfs.lastClusterAllocated = FAT_SMALLEST_LEGAL_CLUSTER_NUMBER; - -#ifdef AFATFS_USE_INTROSPECTIVE_LOGGING - sdcard_setProfilerCallback(afatfs_sdcardProfilerCallback); -#endif } /** @@ -3604,13 +3517,6 @@ bool afatfs_destroy(bool dirty) } } -#ifdef AFATFS_USE_INTROSPECTIVE_LOGGING - if (afatfs.introSpecLog.type != AFATFS_FILE_TYPE_NONE) { - afatfs_fclose(&afatfs.introSpecLog, NULL); - openFileCount++; - } -#endif - #ifdef AFATFS_USE_FREEFILE if (afatfs.freeFile.type != AFATFS_FILE_TYPE_NONE) { afatfs_fclose(&afatfs.freeFile, NULL); diff --git a/src/main/io/dashboard.c b/src/main/io/dashboard.c index 6c3d475caf8..fc230bee19d 100644 --- a/src/main/io/dashboard.c +++ b/src/main/io/dashboard.c @@ -357,7 +357,7 @@ static void showStatusPage(void) if (feature(FEATURE_VBAT)) { i2c_OLED_set_line(rowIndex++); - tfp_sprintf(lineBuffer, "V: %d.%1d ", getBatteryVoltage() / 100, getBatteryVoltage() % 100); + tfp_sprintf(lineBuffer, "V: %d.%02d ", getBatteryVoltage() / 100, getBatteryVoltage() % 100); padLineBufferToChar(12); i2c_OLED_send_string(lineBuffer); @@ -387,7 +387,7 @@ static void showStatusPage(void) i2c_OLED_set_xy(HALF_SCREEN_CHARACTER_COLUMN_COUNT, rowIndex++); i2c_OLED_send_string(lineBuffer); - tfp_sprintf(lineBuffer, "HDOP: %d.%1d", gpsSol.hdop / 100, gpsSol.hdop % 100); + tfp_sprintf(lineBuffer, "HDOP: %d.%02d", gpsSol.hdop / 100, gpsSol.hdop % 100); padLineBuffer(); i2c_OLED_set_line(rowIndex++); i2c_OLED_send_string(lineBuffer); diff --git a/src/main/io/displayport_max7456.c b/src/main/io/displayport_max7456.c index d086b36f326..cba52cbac1d 100644 --- a/src/main/io/displayport_max7456.c +++ b/src/main/io/displayport_max7456.c @@ -33,6 +33,13 @@ #include "io/displayport_max7456.h" +// 'I', 'N', 'A', 'V', 1 +#define CHR_IS_METADATA(chr) ((chr)->data[0] == 'I' && \ + (chr)->data[1] == 'N' && \ + (chr)->data[2] == 'A' && \ + (chr)->data[3] == 'V' && \ + (chr)->data[4] == 1) + displayPort_t max7456DisplayPort; static uint8_t max7456Mode(textAttributes_t attr) @@ -72,7 +79,7 @@ static int clearScreen(displayPort_t *displayPort) static int drawScreen(displayPort_t *displayPort) { UNUSED(displayPort); - max7456DrawScreenPartial(); + max7456Update(); return 0; } @@ -80,7 +87,7 @@ static int drawScreen(displayPort_t *displayPort) static int screenSize(const displayPort_t *displayPort) { UNUSED(displayPort); - return maxScreenSize; + return max7456GetScreenSize(); } static int writeString(displayPort_t *displayPort, uint8_t x, uint8_t y, const char *s, textAttributes_t attr) @@ -91,7 +98,7 @@ static int writeString(displayPort_t *displayPort, uint8_t x, uint8_t y, const c return 0; } -static int writeChar(displayPort_t *displayPort, uint8_t x, uint8_t y, uint8_t c, textAttributes_t attr) +static int writeChar(displayPort_t *displayPort, uint8_t x, uint8_t y, uint16_t c, textAttributes_t attr) { UNUSED(displayPort); max7456WriteChar(x, y, c, max7456Mode(attr)); @@ -99,7 +106,7 @@ static int writeChar(displayPort_t *displayPort, uint8_t x, uint8_t y, uint8_t c return 0; } -static bool readChar(displayPort_t *displayPort, uint8_t x, uint8_t y, uint8_t *c, textAttributes_t *attr) +static bool readChar(displayPort_t *displayPort, uint8_t x, uint8_t y, uint16_t *c, textAttributes_t *attr) { UNUSED(displayPort); return max7456ReadChar(x, y, c, attr); @@ -142,6 +149,31 @@ static textAttributes_t supportedTextAttributes(const displayPort_t *displayPort return attr; } +static bool getFontMetadata(displayFontMetadata_t *metadata, const displayPort_t *displayPort) +{ + UNUSED(displayPort); + + max7456Character_t chr; + + max7456ReadNvm(255, &chr); + + if (CHR_IS_METADATA(&chr)) { + metadata->version = chr.data[5]; + // Not all MAX7456 chips support 512 characters. To detect this, + // we place metadata in both characters 255 and 256. This way we + // can find out how many characters the font in NVM has. + max7456ReadNvm(256, &chr); + if (CHR_IS_METADATA(&chr)) { + metadata->charCount = 512; + } else { + metadata->charCount = 256; + } + return true; + } + + return false; +} + static const displayPortVTable_t max7456VTable = { .grab = grab, .release = release, @@ -156,12 +188,13 @@ static const displayPortVTable_t max7456VTable = { .resync = resync, .txBytesFree = txBytesFree, .supportedTextAttributes = supportedTextAttributes, + .getFontMetadata = getFontMetadata, }; displayPort_t *max7456DisplayPortInit(const videoSystem_e videoSystem) { - displayInit(&max7456DisplayPort, &max7456VTable); max7456Init(videoSystem); + displayInit(&max7456DisplayPort, &max7456VTable); resync(&max7456DisplayPort); return &max7456DisplayPort; } diff --git a/src/main/io/displayport_msp.c b/src/main/io/displayport_msp.c index f12969c81b0..27fc439e0a2 100644 --- a/src/main/io/displayport_msp.c +++ b/src/main/io/displayport_msp.c @@ -113,7 +113,7 @@ static int writeString(displayPort_t *displayPort, uint8_t col, uint8_t row, con return output(displayPort, MSP_DISPLAYPORT, buf, len + 4); } -static int writeChar(displayPort_t *displayPort, uint8_t col, uint8_t row, uint8_t c, textAttributes_t attr) +static int writeChar(displayPort_t *displayPort, uint8_t col, uint8_t row, uint16_t c, textAttributes_t attr) { char buf[2]; diff --git a/src/main/io/displayport_oled.c b/src/main/io/displayport_oled.c index 51989bdcfb1..bc132e524b6 100644 --- a/src/main/io/displayport_oled.c +++ b/src/main/io/displayport_oled.c @@ -66,7 +66,7 @@ static int oledWriteString(displayPort_t *displayPort, uint8_t x, uint8_t y, con return 0; } -static int oledWriteChar(displayPort_t *displayPort, uint8_t x, uint8_t y, uint8_t c, textAttributes_t attr) +static int oledWriteChar(displayPort_t *displayPort, uint8_t x, uint8_t y, uint16_t c, textAttributes_t attr) { UNUSED(displayPort); UNUSED(attr); diff --git a/src/main/io/gps.c b/src/main/io/gps.c index 0cd40ac401a..df63a806dc0 100755 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -312,7 +312,7 @@ static bool gpsFakeGPSUpdate(void) gpsSetProtocolTimeout(GPS_TIMEOUT); - gpsSetState(GPS_RECEIVING_DATA); + gpsSetState(GPS_RUNNING); return true; } return false; diff --git a/src/main/io/gps_nmea.c b/src/main/io/gps_nmea.c index 62fa12d4123..f6af1cc4ac9 100755 --- a/src/main/io/gps_nmea.c +++ b/src/main/io/gps_nmea.c @@ -90,7 +90,7 @@ typedef struct gpsDataNmea_s { int32_t latitude; int32_t longitude; uint8_t numSat; - uint16_t altitude; + int32_t altitude; uint16_t speed; uint16_t ground_course; uint16_t hdop; @@ -329,37 +329,39 @@ STATIC_PROTOTHREAD(gpsProtocolStateThreadMTK) } // Send configuration commands - // Disable all messages except GGA and RMC - serialPrint(gpsState.gpsPort, "$PMTK314,0,1,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0*28\r\n"); - ptWait(isSerialTransmitBufferEmpty(gpsState.gpsPort)); - - // Set Nav Threshold (the minimum speed the GPS must be moving to update the position) to 0 m/s - serialPrint(gpsState.gpsPort, "$PMTK397,0*23\r\n"); - ptWait(isSerialTransmitBufferEmpty(gpsState.gpsPort)); - - // SBAS/WAAS - if (gpsState.gpsConfig->sbasMode != SBAS_NONE) { - serialPrint(gpsState.gpsPort, "$PMTK313,1*2E\r\n"); // SBAS ON + if (gpsState.gpsConfig->autoConfig) { + // Disable all messages except GGA and RMC + serialPrint(gpsState.gpsPort, "$PMTK314,0,1,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0*28\r\n"); ptWait(isSerialTransmitBufferEmpty(gpsState.gpsPort)); - serialPrint(gpsState.gpsPort, "$PMTK301,2*2E\r\n"); // WAAS ON - ptWait(isSerialTransmitBufferEmpty(gpsState.gpsPort)); - } - else { - serialPrint(gpsState.gpsPort, "$PMTK313,0*2F\r\n"); // SBAS OFF + // Set Nav Threshold (the minimum speed the GPS must be moving to update the position) to 0 m/s + serialPrint(gpsState.gpsPort, "$PMTK397,0*23\r\n"); ptWait(isSerialTransmitBufferEmpty(gpsState.gpsPort)); - serialPrint(gpsState.gpsPort, "$PMTK301,0*2C\r\n"); // WAAS OFF - ptWait(isSerialTransmitBufferEmpty(gpsState.gpsPort)); - } + // SBAS/WAAS + if (gpsState.gpsConfig->sbasMode != SBAS_NONE) { + serialPrint(gpsState.gpsPort, "$PMTK313,1*2E\r\n"); // SBAS ON + ptWait(isSerialTransmitBufferEmpty(gpsState.gpsPort)); - // 5Hz update, should works for most modules - serialPrint(gpsState.gpsPort, "$PMTK220,200*2C\r\n"); - ptWait(isSerialTransmitBufferEmpty(gpsState.gpsPort)); + serialPrint(gpsState.gpsPort, "$PMTK301,2*2E\r\n"); // WAAS ON + ptWait(isSerialTransmitBufferEmpty(gpsState.gpsPort)); + } + else { + serialPrint(gpsState.gpsPort, "$PMTK313,0*2F\r\n"); // SBAS OFF + ptWait(isSerialTransmitBufferEmpty(gpsState.gpsPort)); - // Try set 10Hz update rate. Module will ignore it if can't support - serialPrint(gpsState.gpsPort, "$PMTK220,100*2F\r\n"); - ptWait(isSerialTransmitBufferEmpty(gpsState.gpsPort)); + serialPrint(gpsState.gpsPort, "$PMTK301,0*2C\r\n"); // WAAS OFF + ptWait(isSerialTransmitBufferEmpty(gpsState.gpsPort)); + } + + // 5Hz update, should works for most modules + serialPrint(gpsState.gpsPort, "$PMTK220,200*2C\r\n"); + ptWait(isSerialTransmitBufferEmpty(gpsState.gpsPort)); + + // Try set 10Hz update rate. Module will ignore it if can't support + serialPrint(gpsState.gpsPort, "$PMTK220,100*2F\r\n"); + ptWait(isSerialTransmitBufferEmpty(gpsState.gpsPort)); + } // Reset protocol timeout gpsSetProtocolTimeout(GPS_TIMEOUT); diff --git a/src/main/io/gps_private.h b/src/main/io/gps_private.h index c000c15c353..4195529d05b 100755 --- a/src/main/io/gps_private.h +++ b/src/main/io/gps_private.h @@ -26,7 +26,7 @@ // GPS timeout for wrong baud rate/disconnection/etc in milliseconds (default 2000 ms) #define GPS_TIMEOUT (1000) #define GPS_SHORT_TIMEOUT (500) -#define GPS_BAUD_CHANGE_DELAY (200) +#define GPS_BAUD_CHANGE_DELAY (100) #define GPS_INIT_DELAY (500) #define GPS_BOOT_DELAY (3000) diff --git a/src/main/io/gps_ublox.c b/src/main/io/gps_ublox.c index 84d38f691a6..bae5e0be395 100755 --- a/src/main/io/gps_ublox.c +++ b/src/main/io/gps_ublox.c @@ -48,7 +48,7 @@ #include "scheduler/protothreads.h" -#define GPS_VERSION_DETECTION_TIMEOUT_MS 200 +#define GPS_CFG_CMD_TIMEOUT_MS 200 #define GPS_VERSION_RETRY_TIMES 2 #define MAX_UBLOX_PAYLOAD_SIZE 256 #define UBLOX_BUFFER_SIZE MAX_UBLOX_PAYLOAD_SIZE @@ -71,7 +71,7 @@ static const uint32_t ubloxScanMode1[] = { 0x00000000, 0x00000851, 0x0004E004, 0x00020200, 0x00000180, 0x00000000, }; -static const char * baudInitData[GPS_BAUDRATE_COUNT] = { +static const char * baudInitDataNMEA[GPS_BAUDRATE_COUNT] = { "$PUBX,41,1,0003,0001,115200,0*1E\r\n", // GPS_BAUDRATE_115200 "$PUBX,41,1,0003,0001,57600,0*2D\r\n", // GPS_BAUDRATE_57600 "$PUBX,41,1,0003,0001,38400,0*26\r\n", // GPS_BAUDRATE_38400 @@ -696,6 +696,9 @@ STATIC_PROTOTHREAD(gpsConfigure) { ptBegin(gpsConfigure); + // Reset timeout + gpsSetProtocolTimeout(GPS_SHORT_TIMEOUT); + // Set dynamic model switch (gpsState.gpsConfig->dynModel) { case GPS_DYNMODEL_PEDESTRIAN: @@ -793,7 +796,7 @@ STATIC_PROTOTHREAD(gpsConfigure) // however GPS would be functional. We are waiting for any response - ACK/NACK gpsSetProtocolTimeout(GPS_SHORT_TIMEOUT); configureSBAS(); - ptWait(_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK); + ptWaitTimeout((_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK), GPS_CFG_CMD_TIMEOUT_MS); // Enable GALILEO if (gpsState.gpsConfig->ubloxUseGalileo && capGalileo) { @@ -801,7 +804,7 @@ STATIC_PROTOTHREAD(gpsConfigure) // however GPS would otherwise be perfectly initialized, so we are just waiting for any response gpsSetProtocolTimeout(GPS_SHORT_TIMEOUT); configureGalileo(); - ptWait(_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK); + ptWaitTimeout((_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK), GPS_CFG_CMD_TIMEOUT_MS); } ptEnd(0); @@ -836,6 +839,7 @@ STATIC_PROTOTHREAD(gpsProtocolStateThread) // Change baud rate if (gpsState.gpsConfig->autoBaud != GPS_AUTOBAUD_OFF) { +#if 0 // Autobaud logic: // 0. Wait for TX buffer to be empty ptWait(isSerialTransmitBufferEmpty(gpsState.gpsPort)); @@ -845,7 +849,7 @@ STATIC_PROTOTHREAD(gpsProtocolStateThread) gpsState.autoBaudrateIndex = (gpsState.autoBaudrateIndex + 1) % GPS_BAUDRATE_COUNT; // 2. Send an $UBX command to switch the baud rate specified by portConfig [baudrateIndex] - serialPrint(gpsState.gpsPort, baudInitData[gpsState.baudrateIndex]); + serialPrint(gpsState.gpsPort, baudInitDataNMEA[gpsState.baudrateIndex]); // 3. Wait for command to be received and processed by GPS ptWait(isSerialTransmitBufferEmpty(gpsState.gpsPort)); @@ -855,6 +859,26 @@ STATIC_PROTOTHREAD(gpsProtocolStateThread) // 5. Attempt to configure the GPS ptDelayMs(GPS_BAUD_CHANGE_DELAY); +#else + // 0. Wait for TX buffer to be empty + ptWait(isSerialTransmitBufferEmpty(gpsState.gpsPort)); + + // Try sending baud rate switch command at all common baud rates + gpsSetProtocolTimeout((GPS_BAUD_CHANGE_DELAY + 50) * (GPS_BAUDRATE_COUNT)); + for (gpsState.autoBaudrateIndex = 0; gpsState.autoBaudrateIndex < GPS_BAUDRATE_COUNT; gpsState.autoBaudrateIndex++) { + // 2. Set serial port to baud rate and send an $UBX command to switch the baud rate specified by portConfig [baudrateIndex] + serialSetBaudRate(gpsState.gpsPort, baudRates[gpsToSerialBaudRate[gpsState.autoBaudrateIndex]]); + serialPrint(gpsState.gpsPort, baudInitDataNMEA[gpsState.baudrateIndex]); + + // 3. Wait for serial port to finish transmitting + ptWait(isSerialTransmitBufferEmpty(gpsState.gpsPort)); + + // 4. Extra wait to make sure GPS processed the command + ptDelayMs(GPS_BAUD_CHANGE_DELAY); + } + + serialSetBaudRate(gpsState.gpsPort, baudRates[gpsToSerialBaudRate[gpsState.baudrateIndex]]); +#endif } else { // No auto baud - set port baud rate to [baudrateIndex] @@ -865,21 +889,24 @@ STATIC_PROTOTHREAD(gpsProtocolStateThread) serialSetBaudRate(gpsState.gpsPort, baudRates[gpsToSerialBaudRate[gpsState.baudrateIndex]]); } - // Reset protocol timeout - gpsSetProtocolTimeout(MIN(GPS_TIMEOUT, ((GPS_VERSION_RETRY_TIMES + 1) * GPS_VERSION_DETECTION_TIMEOUT_MS))); + // Configure GPS module if enabled + if (gpsState.gpsConfig->autoConfig) { + // Reset protocol timeout + gpsSetProtocolTimeout(MAX(GPS_TIMEOUT, ((GPS_VERSION_RETRY_TIMES + 3) * GPS_CFG_CMD_TIMEOUT_MS))); - // Attempt to detect GPS hw version - gpsState.hwVersion = 0; - gpsState.autoConfigStep = 0; + // Attempt to detect GPS hw version + gpsState.hwVersion = 0; + gpsState.autoConfigStep = 0; - do { - pollVersion(); - gpsState.autoConfigStep++; - ptWaitTimeout((gpsState.hwVersion != 0), GPS_VERSION_DETECTION_TIMEOUT_MS); - } while(gpsState.autoConfigStep < GPS_VERSION_RETRY_TIMES && gpsState.hwVersion == 0); + do { + pollVersion(); + gpsState.autoConfigStep++; + ptWaitTimeout((gpsState.hwVersion != 0), GPS_CFG_CMD_TIMEOUT_MS); + } while(gpsState.autoConfigStep < GPS_VERSION_RETRY_TIMES && gpsState.hwVersion == 0); - // Configure GPS - ptSpawn(gpsConfigure); + // Configure GPS + ptSpawn(gpsConfigure); + } // GPS setup done, reset timeout gpsSetProtocolTimeout(GPS_TIMEOUT); diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 2807239cad5..d834bec5e2c 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -42,6 +42,7 @@ #include "common/axis.h" #include "common/filter.h" +#include "common/olc.h" #include "common/printf.h" #include "common/string_light.h" #include "common/time.h" @@ -79,6 +80,7 @@ #include "flight/wind_estimator.h" #include "navigation/navigation.h" +#include "navigation/navigation_private.h" #include "rx/rx.h" @@ -87,6 +89,7 @@ #include "sensors/diagnostics.h" #include "sensors/sensors.h" #include "sensors/pitotmeter.h" +#include "sensors/temperature.h" #ifdef USE_HARDWARE_REVISION_DETECTION #include "hardware_revision.h" @@ -123,8 +126,17 @@ x; \ }) +#define OSD_CHR_IS_NUM(c) (c >= '0' && c <= '9') + +#define OSD_CENTER_LEN(x) ((osdDisplayPort->cols - x) / 2) +#define OSD_CENTER_S(s) OSD_CENTER_LEN(strlen(s)) + +#define OSD_MIN_FONT_VERSION 1 + static unsigned currentLayout = 0; static int layoutOverride = -1; +static bool hasExtendedFont = false; // Wether the font supports characters > 256 +static timeMs_t layoutOverrideUntil = 0; typedef struct statistic_s { uint16_t max_speed; @@ -160,13 +172,16 @@ static uint8_t armState; static displayPort_t *osdDisplayPort; -#define AH_MAX_PITCH 200 // Specify maximum AHI pitch value displayed. Default 200 = 20.0 degrees -#define AH_MAX_ROLL 400 // Specify maximum AHI roll value displayed. Default 400 = 40.0 degrees -#define AH_SYMBOL_COUNT 9 +#define AH_MAX_PITCH_DEFAULT 20 // Specify default maximum AHI pitch value displayed (degrees) +#define AH_HEIGHT 9 +#define AH_WIDTH 11 +#define AH_PREV_SIZE (AH_WIDTH > AH_HEIGHT ? AH_WIDTH : AH_HEIGHT) +#define AH_H_SYM_COUNT 9 +#define AH_V_SYM_COUNT 6 #define AH_SIDEBAR_WIDTH_POS 7 #define AH_SIDEBAR_HEIGHT_POS 3 -PG_REGISTER_WITH_RESET_FN(osdConfig_t, osdConfig, PG_OSD_CONFIG, 3); +PG_REGISTER_WITH_RESET_FN(osdConfig_t, osdConfig, PG_OSD_CONFIG, 7); static int digitCount(int32_t value) { @@ -346,16 +361,16 @@ static int32_t osdConvertVelocityToUnit(int32_t vel) * Converts velocity into a string based on the current unit system. * @param alt Raw velocity (i.e. as taken from gpsSol.groundSpeed in centimeters/seconds) */ -static void osdFormatVelocityStr(char* buff, int32_t vel) +static void osdFormatVelocityStr(char* buff, int32_t vel, bool _3D) { switch ((osd_unit_e)osdConfig()->units) { case OSD_UNIT_UK: FALLTHROUGH; case OSD_UNIT_IMPERIAL: - tfp_sprintf(buff, "%3d%c", osdConvertVelocityToUnit(vel), SYM_MPH); + tfp_sprintf(buff, "%3d%c", osdConvertVelocityToUnit(vel), (_3D ? SYM_3D_MPH : SYM_MPH)); break; case OSD_UNIT_METRIC: - tfp_sprintf(buff, "%3d%c", osdConvertVelocityToUnit(vel), SYM_KMH); + tfp_sprintf(buff, "%3d%c", osdConvertVelocityToUnit(vel), (_3D ? SYM_3D_KMH : SYM_KMH)); break; } } @@ -484,6 +499,63 @@ static uint16_t osdConvertRSSI(void) return constrain(getRSSI() * 100 / RSSI_MAX_VALUE, 0, 99); } +/** +* Displays a temperature postfixed with a symbol depending on the current unit system +* @param label to display +* @param valid true if measurement is valid +* @param temperature in deciDegrees Celcius +*/ +static void osdDisplayTemperature(uint8_t elemPosX, uint8_t elemPosY, uint16_t symbol, const char *label, bool valid, int16_t temperature, int16_t alarm_min, int16_t alarm_max) +{ + char buff[TEMPERATURE_LABEL_LEN + 2 < 6 ? 6 : TEMPERATURE_LABEL_LEN + 2]; + textAttributes_t elemAttr = valid ? TEXT_ATTRIBUTES_NONE : _TEXT_ATTRIBUTES_BLINK_BIT; + uint8_t valueXOffset = 0; + + if (symbol) { + buff[0] = symbol; + buff[1] = '\0'; + displayWriteWithAttr(osdDisplayPort, elemPosX, elemPosY, buff, elemAttr); + valueXOffset = 1; + } +#ifdef USE_TEMPERATURE_SENSOR + else if (label[0] != '\0') { + uint8_t label_len = strnlen(label, TEMPERATURE_LABEL_LEN); + memcpy(buff, label, label_len); + memset(buff + label_len, ' ', TEMPERATURE_LABEL_LEN + 1 - label_len); + buff[5] = '\0'; + displayWriteWithAttr(osdDisplayPort, elemPosX, elemPosY, buff, elemAttr); + valueXOffset = osdConfig()->temp_label_align == OSD_ALIGN_LEFT ? 5 : label_len + 1; + } +#else + UNUSED(label); +#endif + + if (valid) { + + if ((temperature <= alarm_min) || (temperature >= alarm_max)) TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + if (osdConfig()->units == OSD_UNIT_IMPERIAL) temperature = temperature * 9 / 5.0f + 320; + tfp_sprintf(buff, "%3d", temperature / 10); + + } else + strcpy(buff, "---"); + + buff[3] = osdConfig()->units == OSD_UNIT_IMPERIAL ? SYM_TEMP_F : SYM_TEMP_C; + buff[4] = '\0'; + + displayWriteWithAttr(osdDisplayPort, elemPosX + valueXOffset, elemPosY, buff, elemAttr); +} + +#ifdef USE_TEMPERATURE_SENSOR +static void osdDisplayTemperatureSensor(uint8_t elemPosX, uint8_t elemPosY, uint8_t sensorIndex) +{ + int16_t temperature; + const bool valid = getSensorTemperature(sensorIndex, &temperature); + const tempSensorConfig_t *sensorConfig = tempSensorConfig(sensorIndex); + uint16_t symbol = sensorConfig->osdSymbol ? SYM_TEMP_SENSOR_FIRST + sensorConfig->osdSymbol - 1 : 0; + osdDisplayTemperature(elemPosX, elemPosY, symbol, sensorConfig->label, valid, temperature, sensorConfig->alarm_min, sensorConfig->alarm_max); +} +#endif + static void osdFormatCoordinate(char *buff, char sym, int32_t val) { // up to 4 for number + 1 for the symbol + null terminator + fill the rest with decimals @@ -760,8 +832,8 @@ static void osdCrosshairsBounds(uint8_t *x, uint8_t *y, uint8_t *length) **/ static void osdFormatThrottlePosition(char *buff, bool autoThr, textAttributes_t *elemAttr) { - buff[0] = SYM_THR; - buff[1] = SYM_THR1; + buff[0] = SYM_BLANK; + buff[1] = SYM_THR; int16_t thr = rcData[THROTTLE]; if (autoThr && navigationIsControllingThrottle()) { buff[0] = SYM_AUTO_THR0; @@ -784,6 +856,17 @@ static inline int32_t osdGetAltitude(void) #endif } +static inline int32_t osdGetAltitudeMsl(void) +{ +#if defined(USE_NAV) + return getEstimatedActualPosition(Z)+GPS_home.alt; +#elif defined(USE_BARO) + return baro.alt+GPS_home.alt; +#else + return 0; +#endif +} + static uint8_t osdUpdateSidebar(osd_sidebar_scroll_e scroll, osd_sidebar_t *sidebar, timeMs_t currentTimeMs) { // Scroll between SYM_AH_DECORATION_MIN and SYM_AH_DECORATION_MAX. @@ -993,7 +1076,7 @@ static void osdDrawMap(int referenceHeading, uint8_t referenceSym, uint8_t cente } } else { - uint8_t c; + uint16_t c; if (displayReadCharWithAttr(osdDisplayPort, poiX, poiY, &c, NULL) && c != SYM_BLANK) { // Something else written here, increase scale. If the display doesn't support reading // back characters, we assume there's nothing. @@ -1053,6 +1136,13 @@ static void osdDrawRadar(uint16_t *drawn, uint32_t *usedScale) osdDrawMap(reference, 0, SYM_ARROW_UP, GPS_distanceToHome, poiDirection, SYM_HOME, drawn, usedScale); } +static int16_t osdGet3DSpeed(void) +{ + int16_t vert_speed = getEstimatedActualVelocity(Z); + int16_t hor_speed = gpsSol.groundSpeed; + return (int16_t)sqrtf(sq(hor_speed) + sq(vert_speed)); +} + #endif static void osdFormatPidControllerOutput(char *buff, const char *label, const pidController_t *pidController, uint8_t scale, bool showDecimal) { @@ -1216,9 +1306,15 @@ static bool osdDrawSingleElement(uint8_t item) break; case OSD_GPS_SPEED: - osdFormatVelocityStr(buff, gpsSol.groundSpeed); + osdFormatVelocityStr(buff, gpsSol.groundSpeed, false); break; + case OSD_3D_SPEED: + { + osdFormatVelocityStr(buff, osdGet3DSpeed(), true); + break; + } + case OSD_GPS_LAT: osdFormatCoordinate(buff, SYM_LAT, gpsSol.llh.lat); break; @@ -1230,14 +1326,20 @@ static bool osdDrawSingleElement(uint8_t item) case OSD_HOME_DIR: { if (STATE(GPS_FIX) && STATE(GPS_FIX_HOME) && isImuHeadingValid()) { - // There are 16 orientations for the home direction arrow. - // so we use 22.5deg per image, where the 1st image is used - // for [349, 11], the 2nd for [12, 33], etc... - int homeDirection = GPS_directionToHome - DECIDEGREES_TO_DEGREES(osdGetHeading()); - // Add 11 to the angle, so first character maps to [349, 11] - int homeArrowDir = osdGetHeadingAngle(homeDirection + 11); - unsigned arrowOffset = homeArrowDir * 2 / 45; - buff[0] = SYM_ARROW_UP + arrowOffset; + if (GPS_distanceToHome < (navConfig()->general.min_rth_distance / 100) ) { + buff[0] = SYM_HOME_NEAR; + } + else + { + // There are 16 orientations for the home direction arrow. + // so we use 22.5deg per image, where the 1st image is used + // for [349, 11], the 2nd for [12, 33], etc... + int homeDirection = GPS_directionToHome - DECIDEGREES_TO_DEGREES(osdGetHeading()); + // Add 11 to the angle, so first character maps to [349, 11] + int homeArrowDir = osdGetHeadingAngle(homeDirection + 11); + unsigned arrowOffset = homeArrowDir * 2 / 45; + buff[0] = SYM_ARROW_UP + arrowOffset; + } } else { // No home or no fix or unknown heading, blink. // If we're unarmed, show the arrow pointing up so users can see the arrow @@ -1392,6 +1494,13 @@ static bool osdDrawSingleElement(uint8_t item) break; } + case OSD_ALTITUDE_MSL: + { + int32_t alt = osdGetAltitudeMsl(); + osdFormatAltitudeSymbol(buff, alt); + break; + } + case OSD_ONTIME: { osdFormatOnTime(buff); @@ -1472,39 +1581,31 @@ static bool osdDrawSingleElement(uint8_t item) { char *p = "ACRO"; - if (isAirmodeActive()) { - p = "AIR "; - } - - if (FLIGHT_MODE(FAILSAFE_MODE)) { + if (FLIGHT_MODE(FAILSAFE_MODE)) p = "!FS!"; - } else if (FLIGHT_MODE(MANUAL_MODE)) { + else if (FLIGHT_MODE(MANUAL_MODE)) p = "MANU"; - } else if (FLIGHT_MODE(NAV_RTH_MODE)) { + else if (FLIGHT_MODE(NAV_RTH_MODE)) p = "RTH "; - } else if (FLIGHT_MODE(NAV_POSHOLD_MODE)) { - if (FLIGHT_MODE(NAV_ALTHOLD_MODE)) { - // 3D HOLD - p = "HOLD"; - } else { - p = " PH "; - } - } else if (FLIGHT_MODE(NAV_CRUISE_MODE) && FLIGHT_MODE(NAV_ALTHOLD_MODE)) { + else if (FLIGHT_MODE(NAV_POSHOLD_MODE)) + p = "HOLD"; + else if (FLIGHT_MODE(NAV_CRUISE_MODE) && FLIGHT_MODE(NAV_ALTHOLD_MODE)) p = "3CRS"; - } else if (FLIGHT_MODE(NAV_CRUISE_MODE)) { + else if (FLIGHT_MODE(NAV_CRUISE_MODE)) p = "CRS "; - } else if (FLIGHT_MODE(NAV_ALTHOLD_MODE) && navigationRequiresAngleMode()) { + else if (FLIGHT_MODE(NAV_ALTHOLD_MODE) && navigationRequiresAngleMode()) { // If navigationRequiresAngleMode() returns false when ALTHOLD is active, // it means it can be combined with ANGLE, HORIZON, ACRO, etc... // and its display is handled by OSD_MESSAGES rather than OSD_FLYMODE. p = " AH "; - } else if (FLIGHT_MODE(NAV_WP_MODE)) { + } else if (FLIGHT_MODE(NAV_WP_MODE)) p = " WP "; - } else if (FLIGHT_MODE(ANGLE_MODE)) { + else if (FLIGHT_MODE(ANGLE_MODE)) p = "ANGL"; - } else if (FLIGHT_MODE(HORIZON_MODE)) { + else if (FLIGHT_MODE(HORIZON_MODE)) p = "HOR "; - } + else if (isAirmodeActive()) + p = "AIR "; displayWrite(osdDisplayPort, elemPosX, elemPosY, p); return true; @@ -1528,7 +1629,6 @@ static bool osdDrawSingleElement(uint8_t item) break; } -#if defined(VTX) || defined(USE_VTX_COMMON) case OSD_VTX_CHANNEL: #if defined(VTX) // FIXME: This doesn't actually work. It's for boards with @@ -1556,23 +1656,22 @@ static bool osdDrawSingleElement(uint8_t item) } #endif break; -#endif // VTX || VTX_COMMON case OSD_CROSSHAIRS: osdCrosshairsBounds(&elemPosX, &elemPosY, NULL); switch ((osd_crosshairs_style_e)osdConfig()->crosshairs_style) { case OSD_CROSSHAIRS_STYLE_DEFAULT: - buff[0] = SYM_AH_CENTER_LINE; - buff[1] = SYM_AH_CENTER; - buff[2] = SYM_AH_CENTER_LINE_RIGHT; + buff[0] = SYM_AH_CH_LEFT; + buff[1] = SYM_AH_CH_CENTER; + buff[2] = SYM_AH_CH_RIGHT; buff[3] = '\0'; break; case OSD_CROSSHAIRS_STYLE_AIRCRAFT: - buff[0] = SYM_AH_CROSSHAIRS_AIRCRAFT0; - buff[1] = SYM_AH_CROSSHAIRS_AIRCRAFT1; - buff[2] = SYM_AH_CROSSHAIRS_AIRCRAFT2; - buff[3] = SYM_AH_CROSSHAIRS_AIRCRAFT3; - buff[4] = SYM_AH_CROSSHAIRS_AIRCRAFT4; + buff[0] = SYM_AH_CH_AIRCRAFT0; + buff[1] = SYM_AH_CH_AIRCRAFT1; + buff[2] = SYM_AH_CH_AIRCRAFT2; + buff[3] = SYM_AH_CH_AIRCRAFT3; + buff[4] = SYM_AH_CH_AIRCRAFT4; buff[5] = '\0'; break; } @@ -1597,21 +1696,18 @@ static bool osdDrawSingleElement(uint8_t item) case OSD_ARTIFICIAL_HORIZON: { + elemPosX = 14; - elemPosY = 6 - 4; // Top center of the AH area - // Erase only the positions we drew over. Avoids - // thrashing the video driver buffer. - // writtenY[x] contains the Y position that was - // written to the OSD, counting from elemPosY. - // A negative value indicates that the whole - // column is blank. - static int8_t writtenY[AH_SYMBOL_COUNT]; + elemPosY = 6; // Center of the AH area + + // Store the positions we draw over to erase only these at the next iteration + static int8_t previous_written[AH_PREV_SIZE]; + static int8_t previous_orient = -1; - bool crosshairsVisible; - int crosshairsX, crosshairsY, crosshairsXEnd; + float pitch_rad_to_char = (float)(AH_HEIGHT / 2 + 0.5) / DEGREES_TO_RADIANS(osdConfig()->ahi_max_pitch); - int rollAngle = constrain(attitude.values.roll, -AH_MAX_ROLL, AH_MAX_ROLL); - int pitchAngle = constrain(attitude.values.pitch, -AH_MAX_PITCH, AH_MAX_PITCH); + float rollAngle = DECIDEGREES_TO_RADIANS(attitude.values.roll); + float pitchAngle = DECIDEGREES_TO_RADIANS(attitude.values.pitch); if (osdConfig()->ahi_reverse_roll) { rollAngle = -rollAngle; @@ -1621,60 +1717,54 @@ static bool osdDrawSingleElement(uint8_t item) ++elemPosY; } - // Convert pitchAngle to y compensation value - pitchAngle = ((pitchAngle * 25) / AH_MAX_PITCH) - 41; // 41 = 4 * 9 + 5 - crosshairsVisible = OSD_VISIBLE(osdConfig()->item_pos[currentLayout][OSD_CROSSHAIRS]); - if (crosshairsVisible) { - uint8_t cx, cy, cl; - osdCrosshairsBounds(&cx, &cy, &cl); - crosshairsX = cx - elemPosX; - crosshairsY = cy - elemPosY; - crosshairsXEnd = crosshairsX + cl - 1; + float ky = sin_approx(rollAngle); + float kx = cos_approx(rollAngle); + + if (previous_orient != -1) { + for (int i = 0; i < AH_PREV_SIZE; ++i) { + if (previous_written[i] > -1) { + int8_t dx = (previous_orient ? previous_written[i] : i) - AH_PREV_SIZE / 2; + int8_t dy = (previous_orient ? i : previous_written[i]) - AH_PREV_SIZE / 2; + displayWriteChar(osdDisplayPort, elemPosX + dx, elemPosY - dy, SYM_BLANK); + previous_written[i] = -1; + } + } } - for (int x = -4; x <= 4; x++) { - // Don't clear the whole area to save some time. Instead, clear - // only the positions we previously wrote iff we're writing - // at a different Y coordinate. The video buffer will take care - // of ignoring the write if we're writing the same character - // at the same Y coordinate. - // - // Note that this implementation leaves an untouched character - // in the bottom center of the indicator, which allows positioning - // the home directorion indicator there. - const int y = (-rollAngle * x) / 64 - pitchAngle; - int wx = x + 4; // map the -4 to the 1st element in the writtenY array - int pwy = writtenY[wx]; // previously written Y at this X value - int wy = (y / AH_SYMBOL_COUNT); - - unsigned chX = elemPosX + x; - unsigned chY = elemPosY + wy; - uint8_t c; - - // Check if we're overlapping with the crosshairs directly. Saves a few - // trips to the video driver. Also, test for other arbitrary overlapping - // elements if the display supports reading back characters. - bool overlaps = (crosshairsVisible && - crosshairsY == wy && - x >= crosshairsX && x <= crosshairsXEnd) || - (pwy != wy && displayReadCharWithAttr(osdDisplayPort, chX, chY, &c, NULL) && c != SYM_BLANK); - - if (y >= 0 && y <= 80 && !overlaps) { - if (pwy != -1 && pwy != wy) { - // Erase previous character at pwy rows below elemPosY - // iff we're writing at a different Y coordinate. Otherwise - // we just overwrite the previous one. - displayWriteChar(osdDisplayPort, chX, elemPosY + pwy, SYM_BLANK); + if (fabsf(ky) < fabsf(kx)) { + + previous_orient = 0; + + for (int8_t dx = -AH_WIDTH / 2; dx <= AH_WIDTH / 2; dx++) { + float fy = dx * (ky / kx) + pitchAngle * pitch_rad_to_char + 0.49f; + int8_t dy = floorf(fy); + const uint8_t chX = elemPosX + dx, chY = elemPosY - dy; + uint16_t c; + + if ((dy >= -AH_HEIGHT / 2) && (dy <= AH_HEIGHT / 2) && displayReadCharWithAttr(osdDisplayPort, chX, chY, &c, NULL) && (c == SYM_BLANK)) { + c = SYM_AH_H_START + ((AH_H_SYM_COUNT - 1) - (uint8_t)((fy - dy) * AH_H_SYM_COUNT)); + displayWriteChar(osdDisplayPort, elemPosX + dx, elemPosY - dy, c); + previous_written[dx + AH_PREV_SIZE / 2] = dy + AH_PREV_SIZE / 2; } - uint8_t ch = SYM_AH_BAR9_0 + (y % AH_SYMBOL_COUNT); - displayWriteChar(osdDisplayPort, chX, chY, ch); - writtenY[wx] = wy; - } else { - if (pwy != -1) { - displayWriteChar(osdDisplayPort, chX, elemPosY + pwy, SYM_BLANK); - writtenY[wx] = -1; + } + + } else { + + previous_orient = 1; + + for (int8_t dy = -AH_HEIGHT / 2; dy <= AH_HEIGHT / 2; dy++) { + const float fx = (dy - pitchAngle * pitch_rad_to_char) * (kx / ky) + 0.5f; + const int8_t dx = floorf(fx); + const uint8_t chX = elemPosX + dx, chY = elemPosY - dy; + uint16_t c; + + if ((dx >= -AH_WIDTH / 2) && (dx <= AH_WIDTH / 2) && displayReadCharWithAttr(osdDisplayPort, chX, chY, &c, NULL) && (c == SYM_BLANK)) { + c = SYM_AH_V_START + (fx - dx) * AH_V_SYM_COUNT; + displayWriteChar(osdDisplayPort, chX, chY, c); + previous_written[dy + AH_PREV_SIZE / 2] = dx + AH_PREV_SIZE / 2; } } + } osdDrawSingleElement(OSD_HORIZON_SIDEBARS); @@ -1980,7 +2070,7 @@ static bool osdDrawSingleElement(uint8_t item) { #ifdef USE_PITOT buff[0] = SYM_AIR; - osdFormatVelocityStr(buff + 1, pitot.airSpeed); + osdFormatVelocityStr(buff + 1, pitot.airSpeed, false); #else return false; #endif @@ -2002,9 +2092,9 @@ static bool osdDrawSingleElement(uint8_t item) const char *message = NULL; char messageBuf[MAX(SETTING_MAX_NAME_LENGTH, OSD_MESSAGE_LENGTH+1)]; if (ARMING_FLAG(ARMED)) { - // Aircraft is armed. We might have up to 4 + // Aircraft is armed. We might have up to 5 // messages to show. - const char *messages[4]; + const char *messages[5]; unsigned messageCount = 0; if (FLIGHT_MODE(FAILSAFE_MODE)) { // In FS mode while being armed too @@ -2040,6 +2130,8 @@ static bool osdDrawSingleElement(uint8_t item) if (navStateMessage) { messages[messageCount++] = navStateMessage; } + } else if (STATE(FIXED_WING) && (navGetCurrentStateFlags() & NAV_CTL_LAUNCH)) { + messages[messageCount++] = "AUTOLAUNCH"; } else { if (FLIGHT_MODE(NAV_ALTHOLD_MODE) && !navigationRequiresAngleMode()) { // ALTHOLD might be enabled alongside ANGLE/HORIZON/ACRO @@ -2231,6 +2323,72 @@ static bool osdDrawSingleElement(uint8_t item) break; } + case OSD_IMU_TEMPERATURE: + { + int16_t temperature; + const bool valid = getIMUTemperature(&temperature); + osdDisplayTemperature(elemPosX, elemPosY, SYM_IMU_TEMP, NULL, valid, temperature, osdConfig()->imu_temp_alarm_min, osdConfig()->imu_temp_alarm_max); + return true; + } + + case OSD_BARO_TEMPERATURE: + { + int16_t temperature; + const bool valid = getBaroTemperature(&temperature); + osdDisplayTemperature(elemPosX, elemPosY, SYM_BARO_TEMP, NULL, valid, temperature, osdConfig()->imu_temp_alarm_min, osdConfig()->imu_temp_alarm_max); + return true; + } + +#ifdef USE_TEMPERATURE_SENSOR + case OSD_TEMP_SENSOR_0_TEMPERATURE: + { + osdDisplayTemperatureSensor(elemPosX, elemPosY, 0); + return true; + } + + case OSD_TEMP_SENSOR_1_TEMPERATURE: + { + osdDisplayTemperatureSensor(elemPosX, elemPosY, 1); + return true; + } + + case OSD_TEMP_SENSOR_2_TEMPERATURE: + { + osdDisplayTemperatureSensor(elemPosX, elemPosY, 2); + return true; + } + + case OSD_TEMP_SENSOR_3_TEMPERATURE: + { + osdDisplayTemperatureSensor(elemPosX, elemPosY, 3); + return true; + } + + case OSD_TEMP_SENSOR_4_TEMPERATURE: + { + osdDisplayTemperatureSensor(elemPosX, elemPosY, 4); + return true; + } + + case OSD_TEMP_SENSOR_5_TEMPERATURE: + { + osdDisplayTemperatureSensor(elemPosX, elemPosY, 5); + return true; + } + + case OSD_TEMP_SENSOR_6_TEMPERATURE: + { + osdDisplayTemperatureSensor(elemPosX, elemPosY, 6); + return true; + } + + case OSD_TEMP_SENSOR_7_TEMPERATURE: + { + osdDisplayTemperatureSensor(elemPosX, elemPosY, 7); + return true; + } +#endif /* ifdef USE_TEMPERATURE_SENSOR */ + case OSD_WIND_SPEED_HORIZONTAL: #ifdef USE_WIND_ESTIMATOR { @@ -2278,6 +2436,22 @@ static bool osdDrawSingleElement(uint8_t item) return false; #endif + case OSD_PLUS_CODE: + { + STATIC_ASSERT(GPS_DEGREES_DIVIDER == OLC_DEG_MULTIPLIER, invalid_olc_deg_multiplier); + int digits = osdConfig()->plus_code_digits; + if (STATE(GPS_FIX)) { + olc_encode(gpsSol.llh.lat, gpsSol.llh.lon, digits, buff, sizeof(buff)); + } else { + // +codes with > 8 digits have a + at the 9th digit + // and we only support 10 and up. + memset(buff, '-', digits + 1); + buff[8] = '+'; + buff[digits + 1] = '\0'; + } + break; + } + default: return false; } @@ -2290,6 +2464,14 @@ static uint8_t osdIncElementIndex(uint8_t elementIndex) { ++elementIndex; + if (elementIndex == OSD_ARTIFICIAL_HORIZON) + ++elementIndex; + +#ifndef USE_TEMPERATURE_SENSOR + if (elementIndex == OSD_TEMP_SENSOR_0_TEMPERATURE) + elementIndex = OSD_ALTITUDE_MSL; +#endif + if (!sensors(SENSOR_ACC)) { if (elementIndex == OSD_CROSSHAIRS) { elementIndex = OSD_ONTIME; @@ -2333,6 +2515,9 @@ static uint8_t osdIncElementIndex(uint8_t elementIndex) if (elementIndex == OSD_MAP_NORTH) { elementIndex = OSD_SAG_COMPENSATED_MAIN_BATT_VOLTAGE; } + if (elementIndex == OSD_3D_SPEED) { + elementIndex++; + } } if (elementIndex == OSD_ITEM_COUNT) { @@ -2349,6 +2534,9 @@ void osdDrawNextElement(void) do { elementIndex = osdIncElementIndex(elementIndex); } while(!osdDrawSingleElement(elementIndex) && index != elementIndex); + + // Draw artificial horizon last + osdDrawSingleElement(OSD_ARTIFICIAL_HORIZON); } void pgResetFn_osdConfig(osdConfig_t *osdConfig) @@ -2364,6 +2552,7 @@ void pgResetFn_osdConfig(osdConfig_t *osdConfig) osdConfig->item_pos[0][OSD_MAIN_BATT_CELL_VOLTAGE] = OSD_POS(12, 1); osdConfig->item_pos[0][OSD_MAIN_BATT_SAG_COMPENSATED_CELL_VOLTAGE] = OSD_POS(12, 1); osdConfig->item_pos[0][OSD_GPS_SPEED] = OSD_POS(23, 1); + osdConfig->item_pos[0][OSD_3D_SPEED] = OSD_POS(23, 1); osdConfig->item_pos[0][OSD_THROTTLE_POS] = OSD_POS(1, 2) | OSD_VISIBLE_FLAG; osdConfig->item_pos[0][OSD_THROTTLE_POS_AUTO_THR] = OSD_POS(6, 2); @@ -2406,7 +2595,10 @@ void pgResetFn_osdConfig(osdConfig_t *osdConfig) osdConfig->item_pos[0][OSD_GPS_HDOP] = OSD_POS(0, 10); osdConfig->item_pos[0][OSD_GPS_LAT] = OSD_POS(0, 12); - osdConfig->item_pos[0][OSD_FLYMODE] = OSD_POS(12, 12) | OSD_VISIBLE_FLAG; + // Put this on top of the latitude, since it's very unlikely + // that users will want to use both at the same time. + osdConfig->item_pos[0][OSD_PLUS_CODE] = OSD_POS(0, 12); + osdConfig->item_pos[0][OSD_FLYMODE] = OSD_POS(13, 12) | OSD_VISIBLE_FLAG; osdConfig->item_pos[0][OSD_GPS_LON] = OSD_POS(18, 12); osdConfig->item_pos[0][OSD_ROLL_PIDS] = OSD_POS(2, 10); @@ -2443,6 +2635,17 @@ void pgResetFn_osdConfig(osdConfig_t *osdConfig) osdConfig->item_pos[0][OSD_POWER] = OSD_POS(15, 1); + osdConfig->item_pos[0][OSD_IMU_TEMPERATURE] = OSD_POS(19, 2); + osdConfig->item_pos[0][OSD_BARO_TEMPERATURE] = OSD_POS(19, 3); + osdConfig->item_pos[0][OSD_TEMP_SENSOR_0_TEMPERATURE] = OSD_POS(19, 4); + osdConfig->item_pos[0][OSD_TEMP_SENSOR_1_TEMPERATURE] = OSD_POS(19, 5); + osdConfig->item_pos[0][OSD_TEMP_SENSOR_2_TEMPERATURE] = OSD_POS(19, 6); + osdConfig->item_pos[0][OSD_TEMP_SENSOR_3_TEMPERATURE] = OSD_POS(19, 7); + osdConfig->item_pos[0][OSD_TEMP_SENSOR_4_TEMPERATURE] = OSD_POS(19, 8); + osdConfig->item_pos[0][OSD_TEMP_SENSOR_5_TEMPERATURE] = OSD_POS(19, 9); + osdConfig->item_pos[0][OSD_TEMP_SENSOR_6_TEMPERATURE] = OSD_POS(19, 10); + osdConfig->item_pos[0][OSD_TEMP_SENSOR_7_TEMPERATURE] = OSD_POS(19, 11); + osdConfig->item_pos[0][OSD_AIR_SPEED] = OSD_POS(3, 5); osdConfig->item_pos[0][OSD_WIND_SPEED_HORIZONTAL] = OSD_POS(3, 6); osdConfig->item_pos[0][OSD_WIND_SPEED_VERTICAL] = OSD_POS(3, 7); @@ -2461,10 +2664,21 @@ void pgResetFn_osdConfig(osdConfig_t *osdConfig) osdConfig->alt_alarm = 100; osdConfig->dist_alarm = 1000; osdConfig->neg_alt_alarm = 5; + osdConfig->imu_temp_alarm_min = -200; + osdConfig->imu_temp_alarm_max = 600; +#ifdef USE_BARO + osdConfig->baro_temp_alarm_min = -200; + osdConfig->baro_temp_alarm_max = 600; +#endif + +#ifdef USE_TEMPERATURE_SENSOR + osdConfig->temp_label_align = OSD_ALIGN_LEFT; +#endif osdConfig->video_system = VIDEO_SYSTEM_AUTO; osdConfig->ahi_reverse_roll = 0; + osdConfig->ahi_max_pitch = AH_MAX_PITCH_DEFAULT; osdConfig->crosshairs_style = OSD_CROSSHAIRS_STYLE_DEFAULT; osdConfig->left_sidebar_scroll = OSD_SIDEBAR_SCROLL_NONE; osdConfig->right_sidebar_scroll = OSD_SIDEBAR_SCROLL_NONE; @@ -2475,6 +2689,10 @@ void pgResetFn_osdConfig(osdConfig_t *osdConfig) osdConfig->estimations_wind_compensation = true; osdConfig->coordinate_digits = 9; + + osdConfig->osd_failsafe_switch_layout = false; + + osdConfig->plus_code_digits = 11; } static void osdSetNextRefreshIn(uint32_t timeMs) { @@ -2499,7 +2717,29 @@ void osdInit(displayPort_t *osdDisplayPortToUse) displayClearScreen(osdDisplayPort); - uint8_t y = 4; + uint8_t y = 1; + displayFontMetadata_t metadata; + bool fontHasMetadata = displayGetFontMetadata(&metadata, osdDisplayPort); + + if (fontHasMetadata && metadata.charCount > 256) { + hasExtendedFont = true; + unsigned logo_c = SYM_LOGO_START; + unsigned logo_x = OSD_CENTER_LEN(SYM_LOGO_WIDTH); + for (unsigned ii = 0; ii < SYM_LOGO_HEIGHT; ii++) { + for (unsigned jj = 0; jj < SYM_LOGO_WIDTH; jj++) { + displayWriteChar(osdDisplayPort, logo_x + jj, y, logo_c++); + } + y++; + } + y++; + } else { + if (!fontHasMetadata || metadata.version < OSD_MIN_FONT_VERSION) { + const char *m = "INVALID FONT"; + displayWrite(osdDisplayPort, OSD_CENTER_S(m), 3, m); + } + y = 4; + } + char string_buffer[30]; tfp_sprintf(string_buffer, "INAV VERSION: %s", FC_VERSION_STRING); displayWrite(osdDisplayPort, 5, y++, string_buffer); @@ -2570,7 +2810,7 @@ static void osdUpdateStats(void) int16_t value; if (feature(FEATURE_GPS)) { - value = gpsSol.groundSpeed; + value = osdGet3DSpeed(); if (stats.max_speed < value) stats.max_speed = value; @@ -2612,7 +2852,7 @@ static void osdShowStats(void) if (STATE(GPS_FIX)) { displayWrite(osdDisplayPort, statNameX, top, "MAX SPEED :"); - osdFormatVelocityStr(buff, stats.max_speed); + osdFormatVelocityStr(buff, stats.max_speed, true); displayWrite(osdDisplayPort, statValuesX, top++, buff); displayWrite(osdDisplayPort, statNameX, top, "MAX DISTANCE :"); @@ -2695,8 +2935,8 @@ static void osdShowArmed(void) char buf[MAX(32, FORMATTED_DATE_TIME_BUFSIZE)]; char *date; char *time; - // We need 6 visible rows - uint8_t y = MIN((osdDisplayPort->rows / 2) - 1, osdDisplayPort->rows - 6 - 1); + // We need 7 visible rows + uint8_t y = MIN((osdDisplayPort->rows / 2) - 1, osdDisplayPort->rows - 7 - 1); displayClearScreen(osdDisplayPort); displayWrite(osdDisplayPort, 12, y, "ARMED"); @@ -2709,7 +2949,10 @@ static void osdShowArmed(void) displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, y, buf); osdFormatCoordinate(buf, SYM_LON, GPS_home.lon); displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, y + 1, buf); - y += 3; + int digits = osdConfig()->plus_code_digits; + olc_encode(GPS_home.lat, GPS_home.lon, digits, buf, sizeof(buf)); + displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, y + 2, buf); + y += 4; } else { strcpy(buf, "!NO HOME POSITION!"); displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, y, buf); @@ -2730,9 +2973,9 @@ static void osdShowArmed(void) static void osdRefresh(timeUs_t currentTimeUs) { #ifdef USE_CMS - if (IS_RC_MODE_ACTIVE(BOXOSD) && (!cmsInMenu)) { + if (IS_RC_MODE_ACTIVE(BOXOSD) && (!cmsInMenu) && !(osdConfig()->osd_failsafe_switch_layout && FLIGHT_MODE(FAILSAFE_MODE))) { #else - if (IS_RC_MODE_ACTIVE(BOXOSD)) { + if (IS_RC_MODE_ACTIVE(BOXOSD) && !(osdConfig()->osd_failsafe_switch_layout && FLIGHT_MODE(FAILSAFE_MODE))) { #endif displayClearScreen(osdDisplayPort); armState = ARMING_FLAG(ARMED); @@ -2805,6 +3048,14 @@ void osdUpdate(timeUs_t currentTimeUs) unsigned activeLayout; if (layoutOverride >= 0) { activeLayout = layoutOverride; + // Check for timed override, it will go into effect on + // the next OSD iteration + if (layoutOverrideUntil > 0 && millis() > layoutOverrideUntil) { + layoutOverrideUntil = 0; + layoutOverride = -1; + } + } else if (osdConfig()->osd_failsafe_switch_layout && FLIGHT_MODE(FAILSAFE_MODE)) { + activeLayout = 0; } else { #if OSD_ALTERNATE_LAYOUT_COUNT > 2 if (IS_RC_MODE_ACTIVE(BOXOSDALT3)) @@ -2858,9 +3109,22 @@ void osdStartFullRedraw(void) fullRedraw = true; } -void osdOverrideLayout(int layout) +void osdOverrideLayout(int layout, timeMs_t duration) { layoutOverride = constrain(layout, -1, ARRAYLEN(osdConfig()->item_pos) - 1); + if (layoutOverride >= 0 && duration > 0) { + layoutOverrideUntil = millis() + duration; + } else { + layoutOverrideUntil = 0; + } +} + +int osdGetActiveLayout(bool *overridden) +{ + if (overridden) { + *overridden = layoutOverride >= 0; + } + return currentLayout; } bool osdItemIsFixed(osd_items_e item) diff --git a/src/main/io/osd.h b/src/main/io/osd.h index cffa557eb04..b65027cfb12 100755 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -121,6 +121,19 @@ typedef enum { OSD_MC_VEL_Y_PID_OUTPUTS, OSD_MC_VEL_Z_PID_OUTPUTS, OSD_MC_POS_XYZ_P_OUTPUTS, + OSD_3D_SPEED, + OSD_IMU_TEMPERATURE, + OSD_BARO_TEMPERATURE, + OSD_TEMP_SENSOR_0_TEMPERATURE, + OSD_TEMP_SENSOR_1_TEMPERATURE, + OSD_TEMP_SENSOR_2_TEMPERATURE, + OSD_TEMP_SENSOR_3_TEMPERATURE, + OSD_TEMP_SENSOR_4_TEMPERATURE, + OSD_TEMP_SENSOR_5_TEMPERATURE, + OSD_TEMP_SENSOR_6_TEMPERATURE, + OSD_TEMP_SENSOR_7_TEMPERATURE, + OSD_ALTITUDE_MSL, + OSD_PLUS_CODE, OSD_ITEM_COUNT // MUST BE LAST } osd_items_e; @@ -147,6 +160,11 @@ typedef enum { OSD_SIDEBAR_SCROLL_HOME_DISTANCE, } osd_sidebar_scroll_e; +typedef enum { + OSD_ALIGN_LEFT, + OSD_ALIGN_RIGHT +} osd_alignment_e; + typedef struct osdConfig_s { // Layouts uint16_t item_pos[OSD_LAYOUT_COUNT][OSD_ITEM_COUNT]; @@ -157,6 +175,15 @@ typedef struct osdConfig_s { uint16_t alt_alarm; // positive altitude in m uint16_t dist_alarm; // home distance in m uint16_t neg_alt_alarm; // abs(negative altitude) in m + int16_t imu_temp_alarm_min; + int16_t imu_temp_alarm_max; +#ifdef USE_BARO + int16_t baro_temp_alarm_min; + int16_t baro_temp_alarm_max; +#endif +#ifdef USE_TEMPERATURE_SENSOR + osd_alignment_e temp_label_align; +#endif videoSystem_e video_system; uint8_t row_shiftdown; @@ -164,6 +191,7 @@ typedef struct osdConfig_s { // Preferences uint8_t main_voltage_decimals; uint8_t ahi_reverse_roll; + uint8_t ahi_max_pitch; uint8_t crosshairs_style; // from osd_crosshairs_style_e uint8_t left_sidebar_scroll; // from osd_sidebar_scroll_e uint8_t right_sidebar_scroll; // from osd_sidebar_scroll_e @@ -174,6 +202,9 @@ typedef struct osdConfig_s { bool estimations_wind_compensation; // use wind compensation for estimated remaining flight/distance uint8_t coordinate_digits; + + bool osd_failsafe_switch_layout; + uint8_t plus_code_digits; // Number of digits to use in OSD_PLUS_CODE } osdConfig_t; PG_DECLARE(osdConfig_t, osdConfig); @@ -183,6 +214,12 @@ void osdInit(struct displayPort_s *osdDisplayPort); void osdUpdate(timeUs_t currentTimeUs); void osdStartFullRedraw(void); // Sets a fixed OSD layout ignoring the RC input. Set it -// to -1 to disable the override. -void osdOverrideLayout(int layout); +// to -1 to disable the override. If layout is >= 0 and +// duration is > 0, the override is automatically cleared by +// the OSD after the given duration. Otherwise, the caller must +// explicitely remove it. +void osdOverrideLayout(int layout, timeMs_t duration); +// Returns the current current layout as well as wether its +// set by the user configuration (modes, etc..) or by overriding it. +int osdGetActiveLayout(bool *overridden); bool osdItemIsFixed(osd_items_e item); diff --git a/src/main/io/piniobox.c b/src/main/io/piniobox.c new file mode 100644 index 00000000000..3bcc7cd5b86 --- /dev/null +++ b/src/main/io/piniobox.c @@ -0,0 +1,71 @@ +/* + * This file is part of Cleanflight, Betaflight and INAV + * + * Cleanflight, Betaflight and INAV are free software. You can + * redistribute this software and/or modify this software under + * the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, + * or (at your option) any later version. + * + * Cleanflight, Betaflight and INAV are distributed in the hope that + * they will be useful, but WITHOUT ANY WARRANTY; without even the + * implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR + * PURPOSE. See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +#include +#include + +#include "platform.h" + +#ifdef USE_PINIOBOX + +#include "build/debug.h" +#include "common/utils.h" + +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + +#include "fc/fc_msp.h" +#include "fc/fc_msp_box.h" + +#include "io/piniobox.h" + + +PG_REGISTER_WITH_RESET_TEMPLATE(pinioBoxConfig_t, pinioBoxConfig, PG_PINIOBOX_CONFIG, 1); + +PG_RESET_TEMPLATE(pinioBoxConfig_t, pinioBoxConfig, + { PERMANENT_ID_NONE, PERMANENT_ID_NONE, PERMANENT_ID_NONE, PERMANENT_ID_NONE } +); + +typedef struct pinioBoxRuntimeConfig_s { + uint8_t boxId[PINIO_COUNT]; +} pinioBoxRuntimeConfig_t; + +static pinioBoxRuntimeConfig_t pinioBoxRuntimeConfig; + +void pinioBoxInit(void) +{ + // Convert permanentId to boxId_e + for (int i = 0; i < PINIO_COUNT; i++) { + const box_t *box = findBoxByPermanentId(pinioBoxConfig()->permanentId[i]); + + pinioBoxRuntimeConfig.boxId[i] = box ? box->boxId : BOXID_NONE; + } +} + +void pinioBoxUpdate(void) +{ + for (int i = 0; i < PINIO_COUNT; i++) { + if (pinioBoxRuntimeConfig.boxId[i] != BOXID_NONE) { + pinioSet(i, IS_RC_MODE_ACTIVE(pinioBoxRuntimeConfig.boxId[i])); + } + } +} + +#endif \ No newline at end of file diff --git a/src/main/io/piniobox.h b/src/main/io/piniobox.h new file mode 100644 index 00000000000..74ba398a32d --- /dev/null +++ b/src/main/io/piniobox.h @@ -0,0 +1,36 @@ +/* + * This file is part of Cleanflight, Betaflight and INAV + * + * Cleanflight, Betaflight and INAV are free software. You can + * redistribute this software and/or modify this software under + * the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, + * or (at your option) any later version. + * + * Cleanflight, Betaflight and INAV are distributed in the hope that + * they will be useful, but WITHOUT ANY WARRANTY; without even the + * implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR + * PURPOSE. See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +#pragma once + +#include +#include + +#include "common/time.h" +#include "drivers/pinio.h" + +typedef struct pinioBoxConfig_s { + uint8_t permanentId[PINIO_COUNT]; +} pinioBoxConfig_t; + +PG_DECLARE(pinioBoxConfig_t, pinioBoxConfig); + +void pinioBoxInit(void); +void pinioBoxUpdate(void); diff --git a/src/main/io/rangefinder.h b/src/main/io/rangefinder.h index 8a50eca45e4..275a4afb6b4 100644 --- a/src/main/io/rangefinder.h +++ b/src/main/io/rangefinder.h @@ -30,5 +30,6 @@ #include "drivers/rangefinder/rangefinder_virtual.h" extern virtualRangefinderVTable_t rangefinderMSPVtable; +extern virtualRangefinderVTable_t rangefinderBenewakeVtable; void mspRangefinderReceiveNewData(uint8_t * bufferPtr); \ No newline at end of file diff --git a/src/main/io/rangefinder_benewake.c b/src/main/io/rangefinder_benewake.c new file mode 100644 index 00000000000..c0a0518211a --- /dev/null +++ b/src/main/io/rangefinder_benewake.c @@ -0,0 +1,159 @@ +/* + * This file is part of INAV Project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute and/or modify + * it under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or (at your + * option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see http://www.gnu.org/licenses/. + */ + +#include +#include +#include +#include + +#include "platform.h" + +#include "build/build_config.h" +#include "build/debug.h" + +#include "common/maths.h" + +#include "io/serial.h" + +#if defined(USE_RANGEFINDER_BENEWAKE) +#include "drivers/rangefinder/rangefinder_virtual.h" +#include "drivers/time.h" +#include "io/rangefinder.h" + +typedef struct __attribute__((packed)) { + uint8_t header0; + uint8_t header1; + uint8_t distL; + uint8_t distH; + uint8_t strengthL; + uint8_t strengthH; + uint8_t res; + uint8_t rawQual; + uint8_t checksum; +} tfminiPacket_t; + +#define BENEWAKE_PACKET_SIZE sizeof(tfminiPacket_t) +#define BENEWAKE_MIN_QUALITY 0 + +static serialPort_t * serialPort = NULL; +static serialPortConfig_t * portConfig; +static uint8_t buffer[BENEWAKE_PACKET_SIZE]; +static unsigned bufferPtr; + +static bool hasNewData = false; +static int32_t sensorData = RANGEFINDER_NO_NEW_DATA; + +static bool benewakeRangefinderDetect(void) +{ + portConfig = findSerialPortConfig(FUNCTION_RANGEFINDER); + if (!portConfig) { + return false; + } + + return true; +} + +static void benewakeRangefinderInit(void) +{ + if (!portConfig) { + return; + } + + serialPort = openSerialPort(portConfig->identifier, FUNCTION_RANGEFINDER, NULL, NULL, baudRates[BAUD_115200], MODE_RX, SERIAL_NOT_INVERTED); + if (!serialPort) { + return; + } + + bufferPtr = 0; +} + +static void benewakeRangefinderUpdate(void) +{ + tfminiPacket_t *tfminiPacket = (tfminiPacket_t *)buffer; + while (serialRxBytesWaiting(serialPort) > 0) { + uint8_t c = serialRead(serialPort); + + // Add byte to buffer + if (bufferPtr < BENEWAKE_PACKET_SIZE) { + buffer[bufferPtr++] = c; + } + + // Check header bytes + if ((bufferPtr == 1) && (tfminiPacket->header0 != 0x59)) { + bufferPtr = 0; + continue; + } + + if ((bufferPtr == 2) && (tfminiPacket->header1 != 0x59)) { + bufferPtr = 0; + continue; + } + + // Check for complete packet + if (bufferPtr == BENEWAKE_PACKET_SIZE) { + const uint8_t checksum = buffer[0] + buffer[1] + buffer[2] + buffer[3] + buffer[4] + buffer[5] + buffer[6] + buffer[7]; + if (tfminiPacket->checksum == checksum) { + // Valid packet + hasNewData = true; + sensorData = (tfminiPacket->distL << 0) | (tfminiPacket->distH << 8); + + uint16_t qual = (tfminiPacket->strengthL << 0) | (tfminiPacket->strengthH << 8); + + if (sensorData == 0 || qual <= BENEWAKE_MIN_QUALITY) { + sensorData = -1; + } + + /* + debug[0] = ((tfminiPacket_t *) &buffer[0])->distL; + debug[1] = ((tfminiPacket_t *) &buffer[0])->distH; + debug[2] = ((tfminiPacket_t *) &buffer[0])->strengthH; + debug[3] = ((tfminiPacket_t *) &buffer[0])->strengthL; + */ + } + + // Prepare for new packet + bufferPtr = 0; + } + } +} + +static int32_t benewakeRangefinderGetDistance(void) +{ + if (hasNewData) { + hasNewData = false; + return (sensorData > 0) ? (sensorData) : RANGEFINDER_OUT_OF_RANGE; + } + else { + return RANGEFINDER_NO_NEW_DATA; + } +} + +virtualRangefinderVTable_t rangefinderBenewakeVtable = { + .detect = benewakeRangefinderDetect, + .init = benewakeRangefinderInit, + .update = benewakeRangefinderUpdate, + .read = benewakeRangefinderGetDistance +}; + +#endif diff --git a/src/main/io/rcdevice_cam.h b/src/main/io/rcdevice_cam.h index c99cf97a620..0e61bec1392 100644 --- a/src/main/io/rcdevice_cam.h +++ b/src/main/io/rcdevice_cam.h @@ -39,4 +39,4 @@ void rcdeviceUpdate(timeUs_t currentTimeUs); bool rcdeviceIsEnabled(void); // used for unit test -rcdeviceSwitchState_t switchStates[BOXCAMERA3 - BOXCAMERA1 + 1]; +extern rcdeviceSwitchState_t switchStates[BOXCAMERA3 - BOXCAMERA1 + 1]; diff --git a/src/main/io/serial.c b/src/main/io/serial.c index 2ba9e25bc2e..3b6cd3d4bac 100644 --- a/src/main/io/serial.c +++ b/src/main/io/serial.c @@ -101,7 +101,7 @@ const uint32_t baudRates[] = { 0, 1200, 2400, 4800, 9600, 19200, 38400, 57600, 1 #define BAUD_RATE_COUNT (sizeof(baudRates) / sizeof(baudRates[0])) -PG_REGISTER_WITH_RESET_FN(serialConfig_t, serialConfig, PG_SERIAL_CONFIG, 0); +PG_REGISTER_WITH_RESET_FN(serialConfig_t, serialConfig, PG_SERIAL_CONFIG, 1); void pgResetFn_serialConfig(serialConfig_t *serialConfig) { @@ -110,12 +110,12 @@ void pgResetFn_serialConfig(serialConfig_t *serialConfig) for (int i = 0; i < SERIAL_PORT_COUNT; i++) { serialConfig->portConfigs[i].identifier = serialPortIdentifiers[i]; serialConfig->portConfigs[i].msp_baudrateIndex = BAUD_115200; - serialConfig->portConfigs[i].gps_baudrateIndex = BAUD_38400; + serialConfig->portConfigs[i].gps_baudrateIndex = BAUD_115200; serialConfig->portConfigs[i].telemetry_baudrateIndex = BAUD_AUTO; serialConfig->portConfigs[i].peripheral_baudrateIndex = BAUD_115200; } - serialConfig->portConfigs[0].functionMask = FUNCTION_MSP; + serialConfig->portConfigs[0].functionMask = FUNCTION_MSP | FUNCTION_DEBUG_TRACE; #ifdef SERIALRX_UART serialPortConfig_t *serialRxUartConfig = serialFindPortConfiguration(SERIALRX_UART); @@ -232,21 +232,21 @@ portSharing_e determinePortSharing(const serialPortConfig_t *portConfig, serialP return portConfig->functionMask == function ? PORTSHARING_NOT_SHARED : PORTSHARING_SHARED; } -bool isSerialPortShared(const serialPortConfig_t *portConfig, uint16_t functionMask, serialPortFunction_e sharedWithFunction) +bool isSerialPortShared(const serialPortConfig_t *portConfig, uint32_t functionMask, serialPortFunction_e sharedWithFunction) { return (portConfig) && (portConfig->functionMask & sharedWithFunction) && (portConfig->functionMask & functionMask); } static findSharedSerialPortState_t findSharedSerialPortState; -serialPort_t *findSharedSerialPort(uint16_t functionMask, serialPortFunction_e sharedWithFunction) +serialPort_t *findSharedSerialPort(uint32_t functionMask, serialPortFunction_e sharedWithFunction) { memset(&findSharedSerialPortState, 0, sizeof(findSharedSerialPortState)); return findNextSharedSerialPort(functionMask, sharedWithFunction); } -serialPort_t *findNextSharedSerialPort(uint16_t functionMask, serialPortFunction_e sharedWithFunction) +serialPort_t *findNextSharedSerialPort(uint32_t functionMask, serialPortFunction_e sharedWithFunction) { while (findSharedSerialPortState.lastIndex < SERIAL_PORT_COUNT) { const serialPortConfig_t *candidate = &serialConfig()->portConfigs[findSharedSerialPortState.lastIndex++]; diff --git a/src/main/io/serial.h b/src/main/io/serial.h index 81a07afcd19..565543c6f9b 100644 --- a/src/main/io/serial.h +++ b/src/main/io/serial.h @@ -47,6 +47,8 @@ typedef enum { FUNCTION_UAV_INTERCONNECT = (1 << 13), // 8192 FUNCTION_OPTICAL_FLOW = (1 << 14), // 16384 FUNCTION_DEBUG_TRACE = (1 << 15), // 32768 + FUNCTION_RANGEFINDER = (1 << 16), // 65536 + FUNCTION_VTX_FFPV = (1 << 17), // 131072 } serialPortFunction_e; typedef enum { @@ -99,14 +101,14 @@ typedef struct serialPortUsage_s { serialPortFunction_e function; } serialPortUsage_t; -serialPort_t *findSharedSerialPort(uint16_t functionMask, serialPortFunction_e sharedWithFunction); -serialPort_t *findNextSharedSerialPort(uint16_t functionMask, serialPortFunction_e sharedWithFunction); +serialPort_t *findSharedSerialPort(uint32_t functionMask, serialPortFunction_e sharedWithFunction); +serialPort_t *findNextSharedSerialPort(uint32_t functionMask, serialPortFunction_e sharedWithFunction); // // configuration // typedef struct serialPortConfig_s { - uint16_t functionMask; + uint32_t functionMask; serialPortIdentifier_e identifier; uint8_t msp_baudrateIndex; uint8_t gps_baudrateIndex; @@ -137,7 +139,7 @@ serialPortConfig_t *findSerialPortConfig(serialPortFunction_e function); serialPortConfig_t *findNextSerialPortConfig(serialPortFunction_e function); portSharing_e determinePortSharing(const serialPortConfig_t *portConfig, serialPortFunction_e function); -bool isSerialPortShared(const serialPortConfig_t *portConfig, uint16_t functionMask, serialPortFunction_e sharedWithFunction); +bool isSerialPortShared(const serialPortConfig_t *portConfig, uint32_t functionMask, serialPortFunction_e sharedWithFunction); serialPortUsage_t *findSerialPortUsageByIdentifier(serialPortIdentifier_e identifier); int findSerialPortIndexByIdentifier(serialPortIdentifier_e identifier); diff --git a/src/main/io/vtx.c b/src/main/io/vtx.c index 1dc0e0d50aa..16274209a55 100644 --- a/src/main/io/vtx.c +++ b/src/main/io/vtx.c @@ -23,8 +23,6 @@ #include "platform.h" -#if defined(USE_VTX_COMMON) - #include "common/maths.h" #include "common/time.h" @@ -275,5 +273,3 @@ void vtxUpdate(timeUs_t currentTimeUs) } } } - -#endif diff --git a/src/main/io/vtx_control.c b/src/main/io/vtx_control.c index 6daebbe8e74..4853a6fd060 100644 --- a/src/main/io/vtx_control.c +++ b/src/main/io/vtx_control.c @@ -37,7 +37,7 @@ #include "io/vtx_control.h" -#if defined(USE_VTX_CONTROL) && defined(USE_VTX_COMMON) +#if defined(USE_VTX_CONTROL) PG_REGISTER_WITH_RESET_TEMPLATE(vtxConfig_t, vtxConfig, PG_VTX_CONFIG, 2); diff --git a/src/main/io/vtx_control.h b/src/main/io/vtx_control.h index e39e447a9f0..3b071688916 100644 --- a/src/main/io/vtx_control.h +++ b/src/main/io/vtx_control.h @@ -33,6 +33,15 @@ typedef struct vtxConfig_s { uint8_t halfDuplex; } vtxConfig_t; +typedef struct vtxRunState_s { + int pitMode; + int band; + int channel; + int frequency; + int powerIndex; + int powerMilliwatt; +} vtxRunState_t; + PG_DECLARE(vtxConfig_t, vtxConfig); void vtxControlInit(void); diff --git a/src/main/io/vtx_ffpv24g.c b/src/main/io/vtx_ffpv24g.c new file mode 100644 index 00000000000..32521f89bf5 --- /dev/null +++ b/src/main/io/vtx_ffpv24g.c @@ -0,0 +1,531 @@ +/* + * This file is part of INAV Project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute and/or modify + * it under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or (at your + * option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see http://www.gnu.org/licenses/. + */ + +#include "platform.h" + +#include +#include +#include +#include + +#if defined(USE_VTX_FFPV) && defined(USE_VTX_CONTROL) + +#include "build/debug.h" + +#include "drivers/time.h" +#include "drivers/vtx_common.h" + +#include "common/maths.h" +#include "common/utils.h" + +#include "scheduler/protothreads.h" + +//#include "cms/cms_menu_vtx_ffpv24g.h" + +#include "io/vtx.h" +#include "io/vtx_ffpv24g.h" +#include "io/vtx_control.h" +#include "io/vtx_string.h" +#include "io/serial.h" + + +#define VTX_FFPV_CMD_TIMEOUT_MS 250 +#define VTX_FFPV_HEARTBEAT_MS 1000 + +#define VTX_FFPV_MIN_BAND (1) +#define VTX_FFPV_MAX_BAND (VTX_FFPV_MIN_BAND + VTX_FFPV_BAND_COUNT - 1) +#define VTX_FFPV_MIN_CHANNEL (1) +#define VTX_FFPV_MAX_CHANNEL (VTX_FFPV_MIN_CHANNEL + VTX_FFPV_CHANNEL_COUNT -1) + +#define VTX_UPDATE_REQ_NONE 0x00 +#define VTX_UPDATE_REQ_FREQUENCY 0x01 +#define VTX_UPDATE_REQ_POWER 0x02 + +typedef struct __attribute__((__packed__)) ffpvPacket_s { + uint8_t header; + uint8_t cmd; + uint8_t data[12]; + uint8_t checksum; + uint8_t footer; +} ffpvPacket_t; + +typedef struct { + bool ready; + int protoTimeouts; + unsigned updateReqMask; + + // VTX capabilities + struct { + unsigned freqMin; + unsigned freqMax; + unsigned powerMin; + unsigned powerMax; + } capabilities; + + // Requested VTX state + struct { + bool setByFrequency; + int band; + int channel; + unsigned freq; + unsigned power; + unsigned powerIndex; + } request; + + // Actual VTX state + struct { + unsigned freq; + unsigned power; + } state; + + // Comms flags and state + ffpvPacket_t sendPkt; + ffpvPacket_t recvPkt; + unsigned recvPtr; + bool pktReceived; +} vtxProtoState_t; + +/*****************************************************************************/ +const char * const ffpvBandNames[VTX_FFPV_BAND_COUNT + 1] = { + "--------", + "FFPV 2.4 A", + "FFPV 2.4 B", +}; + +const char * ffpvBandLetters = "-AB"; + +const uint16_t ffpvFrequencyTable[VTX_FFPV_BAND_COUNT][VTX_FFPV_CHANNEL_COUNT] = +{ + { 2410, 2430, 2450, 2470, 2370, 2390, 2490, 2510 }, // FFPV 2.4 A + { 2414, 2432, 2450, 2468, 2411, 2433, 2453, 2473 }, // FFPV 2.4 A +}; + +const char * const ffpvChannelNames[VTX_FFPV_CHANNEL_COUNT + 1] = { + "-", "1", "2", "3", "4", "5", "6", "7", "8", +}; + +const char * const ffpvPowerNames[VTX_FFPV_POWER_COUNT + 1] = { + "---", "25 ", "200", "500", "800" +}; + +const unsigned ffpvPowerTable[VTX_FFPV_POWER_COUNT] = { + 25, 200, 500, 800 +}; + + +/*******************************************************************************/ +static serialPort_t * vtxSerialPort = NULL; +static vtxProtoState_t vtxState; + +static uint8_t vtxCalcChecksum(ffpvPacket_t * pkt) +{ + uint8_t sum = pkt->cmd; + for (int i = 0; i < 12; i++) { + sum += pkt->data[i]; + } + return sum; +} + +static bool vtxProtoRecv(void) +{ + // Return success instantly if packet is already awaiting processing + if (vtxState.pktReceived) { + return true; + } + + uint8_t * bufPtr = (uint8_t*)&vtxState.recvPkt; + while (serialRxBytesWaiting(vtxSerialPort) && !vtxState.pktReceived) { + const uint8_t c = serialRead(vtxSerialPort); + + if (vtxState.recvPtr == 0) { + // Wait for sync byte + if (c == 0x0F) { + bufPtr[vtxState.recvPtr++] = c; + } + } + else { + // Sync byte ok - wait for full packet + if (vtxState.recvPtr < sizeof(vtxState.recvPkt)) { + bufPtr[vtxState.recvPtr++] = c; + } + + // Received full packet - do some processing + if (vtxState.recvPtr == sizeof(vtxState.recvPkt)) { + // Full packet received - validate packet, make sure it's the one we expect + const bool pktValid = (vtxState.recvPkt.header == 0x0F && vtxState.recvPkt.cmd == vtxState.sendPkt.cmd && vtxState.recvPkt.footer == 0x00 && vtxState.recvPkt.checksum == vtxCalcChecksum(&vtxState.recvPkt)); + if (!pktValid) { + // Reset the receiver state - keep waiting + vtxState.pktReceived = false; + vtxState.recvPtr = 0; + } + // Make sure it's not the echo one (half-duplex serial might receive it's own data) + else if (memcmp(&vtxState.recvPkt.data, &vtxState.sendPkt.data, sizeof(vtxState.recvPkt.data)) == 0) { + vtxState.pktReceived = false; + vtxState.recvPtr = 0; + } + // Valid receive packet + else { + vtxState.pktReceived = true; + return true; + } + } + } + } + + return false; +} + +static void vtxProtoSend(uint8_t cmd, const uint8_t * data) +{ + // Craft and send FPV packet + vtxState.sendPkt.header = 0x0F; + vtxState.sendPkt.cmd = cmd; + + if (data) { + memcpy(vtxState.sendPkt.data, data, sizeof(vtxState.sendPkt.data)); + } + else { + memset(vtxState.sendPkt.data, 0, sizeof(vtxState.sendPkt.data)); + } + + vtxState.sendPkt.checksum = vtxCalcChecksum(&vtxState.sendPkt); + vtxState.sendPkt.footer = 0x00; + + // Send data + serialWriteBuf(vtxSerialPort, (uint8_t *)&vtxState.sendPkt, sizeof(vtxState.sendPkt)); + + // Reset cmd response state + vtxState.pktReceived = false; + vtxState.recvPtr = 0; +} + +static void vtxProtoSend_SetFreqency(unsigned freq) +{ + uint8_t data[12] = {0}; + data[0] = freq & 0xFF; + data[1] = (freq >> 8) & 0xFF; + vtxProtoSend(0x46, data); +} + +static void vtxProtoSend_SetPower(unsigned power) +{ + uint8_t data[12] = {0}; + data[0] = power & 0xFF; + data[1] = (power >> 8) & 0xFF; + vtxProtoSend(0x50, data); +} + +STATIC_PROTOTHREAD(impl_VtxProtocolThread) +{ + ptBegin(impl_VtxProtocolThread); + + // 0: Detect VTX. Dwell here infinitely until we get a valid response from VTX + vtxState.ready = false; + while(!vtxState.ready) { + // Send capabilities request and wait + vtxProtoSend(0x72, NULL); + ptWaitTimeout(vtxProtoRecv(), VTX_FFPV_CMD_TIMEOUT_MS); + + // Check if we got a valid response + if (vtxState.pktReceived) { + vtxState.capabilities.freqMin = vtxState.recvPkt.data[0] | (vtxState.recvPkt.data[1] << 8); + vtxState.capabilities.freqMax = vtxState.recvPkt.data[2] | (vtxState.recvPkt.data[3] << 8); + vtxState.capabilities.powerMin = 0; + vtxState.capabilities.powerMax = vtxState.recvPkt.data[4] | (vtxState.recvPkt.data[5] << 8); + vtxState.ready = true; + } + } + + // 1 : Periodically poll VTX for current channel and power, send updates + vtxState.protoTimeouts = 0; + vtxState.updateReqMask = VTX_UPDATE_REQ_NONE; + + while(vtxState.ready) { + // Wait for request for update or time to check liveness + ptWaitTimeout(vtxState.updateReqMask != VTX_UPDATE_REQ_NONE, VTX_FFPV_HEARTBEAT_MS); + + if (vtxState.updateReqMask != VTX_UPDATE_REQ_NONE) { + if (vtxState.updateReqMask & VTX_UPDATE_REQ_FREQUENCY) { + vtxProtoSend_SetFreqency(vtxState.request.freq); + vtxState.updateReqMask &= ~VTX_UPDATE_REQ_FREQUENCY; + ptDelayMs(VTX_FFPV_CMD_TIMEOUT_MS); + } + else if (vtxState.updateReqMask & VTX_UPDATE_REQ_POWER) { + vtxProtoSend_SetPower(vtxState.request.power); + vtxState.updateReqMask &= ~VTX_UPDATE_REQ_POWER; + } + else { + // Unsupported request - reset + vtxState.updateReqMask = VTX_UPDATE_REQ_NONE; + } + } + else { + // Periodic check for VTX liveness + vtxProtoSend(0x76, NULL); + ptWaitTimeout(vtxProtoRecv(), VTX_FFPV_CMD_TIMEOUT_MS); + + if (vtxState.pktReceived) { + // Got a valid state from VTX + vtxState.state.freq = (uint16_t)vtxState.recvPkt.data[0] | ((uint16_t)vtxState.recvPkt.data[1] << 8); + vtxState.state.power = (uint16_t)vtxState.recvPkt.data[2] | ((uint16_t)vtxState.recvPkt.data[3] << 8); + vtxState.protoTimeouts = 0; + + // Check if VTX state matches VTX request + if (vtxState.state.freq != vtxState.request.freq) { + vtxState.updateReqMask |= VTX_UPDATE_REQ_FREQUENCY; + } + + if (vtxState.state.power != vtxState.request.power) { + vtxState.updateReqMask |= VTX_UPDATE_REQ_POWER; + } + } + else { + vtxState.protoTimeouts++; + } + } + + // Sanity check. If we got more than 3 protocol erros + if (vtxState.protoTimeouts >= 3) { + // Reset ready flag - thread will terminate and restart + vtxState.ready = false; + } + } + + ptEnd(0); +} + + +static void impl_Process(vtxDevice_t *vtxDevice, timeUs_t currentTimeUs) +{ + // Glue function betwen VTX VTable and actual driver protothread + UNUSED(vtxDevice); + UNUSED(currentTimeUs); + + impl_VtxProtocolThread(); + + // If thread stopped - vtx comms failed - restart thread and re-init VTX comms + if (ptIsStopped(ptGetHandle(impl_VtxProtocolThread))) { + ptRestart(ptGetHandle(impl_VtxProtocolThread)); + } +} + +static vtxDevType_e impl_GetDeviceType(const vtxDevice_t *vtxDevice) +{ + UNUSED(vtxDevice); + return VTXDEV_FFPV; +} + +static bool impl_IsReady(const vtxDevice_t *vtxDevice) +{ + return vtxDevice != NULL && vtxSerialPort != NULL && vtxState.ready; +} + +static bool impl_DevSetFreq(uint16_t freq) +{ + if (!vtxState.ready || freq < vtxState.capabilities.freqMin || freq > vtxState.capabilities.freqMax) { + return false; + } + + vtxState.request.freq = freq; + vtxState.updateReqMask |= VTX_UPDATE_REQ_FREQUENCY; + + return true; +} + +static void impl_SetFreq(vtxDevice_t * vtxDevice, uint16_t freq) +{ + UNUSED(vtxDevice); + + if (impl_DevSetFreq(freq)) { + // Keep track that we set frequency directly + vtxState.request.setByFrequency = true; + } +} + +void ffpvSetBandAndChannel(uint8_t band, uint8_t channel) +{ + // Validate band and channel + if (band < VTX_FFPV_MIN_BAND || band > VTX_FFPV_MAX_BAND || channel < VTX_FFPV_MIN_CHANNEL || channel > VTX_FFPV_MAX_CHANNEL) { + return; + } + + if (impl_DevSetFreq(ffpvFrequencyTable[band - 1][channel - 1])) { + // Keep track of band/channel data + vtxState.request.setByFrequency = false; + vtxState.request.band = band; + vtxState.request.channel = channel; + } +} + + +static void impl_SetBandAndChannel(vtxDevice_t * vtxDevice, uint8_t band, uint8_t channel) +{ + UNUSED(vtxDevice); + ffpvSetBandAndChannel(band, channel); +} + +void ffpvSetRFPowerByIndex(uint16_t index) +{ + // Validate index + if (index < 1 || index > VTX_FFPV_POWER_COUNT) { + return; + } + + const unsigned power = ffpvPowerTable[index - 1]; + if (!vtxState.ready || power < vtxState.capabilities.powerMin || power > vtxState.capabilities.powerMax) { + return; + } + + vtxState.request.power = power; + vtxState.request.powerIndex = index; + vtxState.updateReqMask |= VTX_UPDATE_REQ_POWER; +} + +static void impl_SetPowerByIndex(vtxDevice_t * vtxDevice, uint8_t index) +{ + UNUSED(vtxDevice); + ffpvSetRFPowerByIndex(index); +} + +static void impl_SetPitMode(vtxDevice_t *vtxDevice, uint8_t onoff) +{ + // TODO: Not implemented + UNUSED(vtxDevice); + UNUSED(onoff); +} + +static bool impl_GetBandAndChannel(const vtxDevice_t *vtxDevice, uint8_t *pBand, uint8_t *pChannel) +{ + if (!impl_IsReady(vtxDevice)) { + return false; + } + + // if in user-freq mode then report band as zero + *pBand = vtxState.request.setByFrequency ? 0 : vtxState.request.band; + *pChannel = vtxState.request.channel; + return true; +} + +static bool impl_GetPowerIndex(const vtxDevice_t *vtxDevice, uint8_t *pIndex) +{ + if (!impl_IsReady(vtxDevice)) { + return false; + } + + *pIndex = vtxState.request.powerIndex; + + return true; +} + +static bool impl_GetPitMode(const vtxDevice_t *vtxDevice, uint8_t *pOnOff) +{ + if (!impl_IsReady(vtxDevice)) { + return false; + } + + // TODO: Not inplemented + *pOnOff = 0; + return true; +} + +static bool impl_GetFreq(const vtxDevice_t *vtxDevice, uint16_t *pFreq) +{ + if (!impl_IsReady(vtxDevice)) { + return false; + } + + *pFreq = vtxState.request.freq; + return true; +} + +vtxRunState_t * ffpvGetRuntimeState(void) +{ + static vtxRunState_t state; + + if (vtxState.ready) { + state.pitMode = 0; + state.band = vtxState.request.band; + state.channel = vtxState.request.channel; + state.frequency = vtxState.request.freq; + state.powerIndex = vtxState.request.powerIndex; + state.powerMilliwatt = vtxState.request.power; + } + else { + state.pitMode = 0; + state.band = 1; + state.channel = 1; + state.frequency = ffpvFrequencyTable[0][0]; + state.powerIndex = 1; + state.powerMilliwatt = 25; + } + return &state; +} + +/*****************************************************************************/ +static const vtxVTable_t impl_vtxVTable = { + .process = impl_Process, + .getDeviceType = impl_GetDeviceType, + .isReady = impl_IsReady, + .setBandAndChannel = impl_SetBandAndChannel, + .setPowerByIndex = impl_SetPowerByIndex, + .setPitMode = impl_SetPitMode, + .setFrequency = impl_SetFreq, + .getBandAndChannel = impl_GetBandAndChannel, + .getPowerIndex = impl_GetPowerIndex, + .getPitMode = impl_GetPitMode, + .getFrequency = impl_GetFreq, +}; + +static vtxDevice_t impl_vtxDevice = { + .vTable = &impl_vtxVTable, + .capability.bandCount = VTX_FFPV_BAND_COUNT, + .capability.channelCount = VTX_FFPV_CHANNEL_COUNT, + .capability.powerCount = VTX_FFPV_POWER_COUNT, + .bandNames = (char **)ffpvBandNames, + .channelNames = (char **)ffpvChannelNames, + .powerNames = (char **)ffpvPowerNames, +}; + +bool vtxFuriousFPVInit(void) +{ + serialPortConfig_t * portConfig = findSerialPortConfig(FUNCTION_VTX_FFPV); + + if (portConfig) { + portOptions_t portOptions = 0; + portOptions = portOptions | (vtxConfig()->halfDuplex ? SERIAL_BIDIR : SERIAL_UNIDIR); + vtxSerialPort = openSerialPort(portConfig->identifier, FUNCTION_VTX_FFPV, NULL, NULL, 9600, MODE_RXTX, portOptions); + } + + if (!vtxSerialPort) { + return false; + } + + vtxCommonSetDevice(&impl_vtxDevice); + + ptRestart(ptGetHandle(impl_VtxProtocolThread)); + + return true; +} + +#endif \ No newline at end of file diff --git a/src/main/io/vtx_ffpv24g.h b/src/main/io/vtx_ffpv24g.h new file mode 100644 index 00000000000..d4110b68cff --- /dev/null +++ b/src/main/io/vtx_ffpv24g.h @@ -0,0 +1,51 @@ +/* + * This file is part of INAV Project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute and/or modify + * it under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or (at your + * option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see http://www.gnu.org/licenses/. + */ + +#pragma once + +#include "platform.h" + +#include +#include +#include +#include + +#include "io/vtx.h" +#include "io/vtx_control.h" + +#define VTX_FFPV_BAND_COUNT 2 +#define VTX_FFPV_CHANNEL_COUNT 8 +#define VTX_FFPV_POWER_COUNT 4 + +extern const char * ffpvBandLetters; +extern const char * const ffpvBandNames[VTX_FFPV_BAND_COUNT + 1]; +extern const char * const ffpvChannelNames[VTX_FFPV_CHANNEL_COUNT + 1]; +extern const char * const ffpvPowerNames[VTX_FFPV_POWER_COUNT + 1]; +extern const uint16_t ffpvFrequencyTable[VTX_FFPV_BAND_COUNT][VTX_FFPV_CHANNEL_COUNT]; + +bool vtxFuriousFPVInit(void); +void ffpvSetBandAndChannel(uint8_t band, uint8_t channel); +void ffpvSetRFPowerByIndex(uint16_t index); + +vtxRunState_t * ffpvGetRuntimeState(void); \ No newline at end of file diff --git a/src/main/io/vtx_smartaudio.c b/src/main/io/vtx_smartaudio.c index aeda5748ad3..bc86209927d 100644 --- a/src/main/io/vtx_smartaudio.c +++ b/src/main/io/vtx_smartaudio.c @@ -63,13 +63,10 @@ serialPort_t *debugSerialPort = NULL; static serialPort_t *smartAudioSerialPort = NULL; -#if defined(USE_CMS) || defined(USE_VTX_COMMON) const char * const saPowerNames[VTX_SMARTAUDIO_POWER_COUNT+1] = { "---", "25 ", "200", "500", "800", }; -#endif -#ifdef USE_VTX_COMMON static const vtxVTable_t saVTable; // Forward static vtxDevice_t vtxSmartAudio = { .vTable = &saVTable, @@ -80,7 +77,6 @@ static vtxDevice_t vtxSmartAudio = { .channelNames = (char **)vtx58ChannelNames, .powerNames = (char **)saPowerNames, }; -#endif // SmartAudio command and response codes enum { @@ -340,10 +336,6 @@ static void saProcessResponse(uint8_t *buf, int len) } saDevicePrev = saDevice; -#ifdef USE_VTX_COMMON - // Todo: Update states in saVtxDevice? -#endif - #ifdef USE_CMS // Export current device status for CMS saCmsUpdate(); @@ -671,12 +663,7 @@ bool vtxSmartAudioInit(void) serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_VTX_SMARTAUDIO); if (portConfig) { portOptions_t portOptions = SERIAL_BIDIR_NOPULL; -#if defined(USE_VTX_COMMON) portOptions = portOptions | (vtxConfig()->halfDuplex ? SERIAL_BIDIR | SERIAL_BIDIR_PP : SERIAL_UNIDIR); -#else - portOptions = SERIAL_BIDIR; -#endif - smartAudioSerialPort = openSerialPort(portConfig->identifier, FUNCTION_VTX_SMARTAUDIO, NULL, NULL, 4800, MODE_RXTX, portOptions); } @@ -766,7 +753,6 @@ static void vtxSAProcess(vtxDevice_t *vtxDevice, timeUs_t currentTimeUs) } } -#ifdef USE_VTX_COMMON // Interface to common VTX API vtxDevType_e vtxSAGetDeviceType(const vtxDevice_t *vtxDevice) @@ -893,7 +879,6 @@ static const vtxVTable_t saVTable = { .getPitMode = vtxSAGetPitMode, .getFrequency = vtxSAGetFreq, }; -#endif // VTX_COMMON #endif // VTX_SMARTAUDIO diff --git a/src/main/io/vtx_string.c b/src/main/io/vtx_string.c index bbaa3eb7b11..f0f4c5382fc 100644 --- a/src/main/io/vtx_string.c +++ b/src/main/io/vtx_string.c @@ -25,8 +25,6 @@ #include "platform.h" #include "build/debug.h" -#if defined(USE_VTX_COMMON) - #define VTX_STRING_BAND_COUNT 5 #define VTX_STRING_CHAN_COUNT 8 @@ -89,5 +87,3 @@ uint16_t vtx58_Bandchan2Freq(uint8_t band, uint8_t channel) } return 0; } - -#endif diff --git a/src/main/io/vtx_tramp.c b/src/main/io/vtx_tramp.c index 9a984c9926f..c9c6b7694fd 100644 --- a/src/main/io/vtx_tramp.c +++ b/src/main/io/vtx_tramp.c @@ -44,7 +44,7 @@ #include "io/vtx.h" #include "io/vtx_string.h" -#if defined(USE_CMS) || defined(USE_VTX_COMMON) +#if defined(USE_CMS) const uint16_t trampPowerTable[VTX_TRAMP_POWER_COUNT] = { 25, 100, 200, 400, 600 }; @@ -54,7 +54,6 @@ const char * const trampPowerNames[VTX_TRAMP_POWER_COUNT+1] = { }; #endif -#if defined(USE_VTX_COMMON) static const vtxVTable_t trampVTable; // forward static vtxDevice_t vtxTramp = { .vTable = &trampVTable, @@ -65,7 +64,6 @@ static vtxDevice_t vtxTramp = { .channelNames = (char **)vtx58ChannelNames, .powerNames = (char **)trampPowerNames, }; -#endif static serialPort_t *trampSerialPort = NULL; @@ -479,8 +477,6 @@ static void vtxTrampProcess(vtxDevice_t *vtxDevice, timeUs_t currentTimeUs) } -#ifdef USE_VTX_COMMON - // Interface to common VTX API static vtxDevType_e vtxTrampGetDeviceType(const vtxDevice_t *vtxDevice) @@ -588,7 +584,6 @@ static const vtxVTable_t trampVTable = { .getFrequency = vtxTrampGetFreq, }; -#endif bool vtxTrampInit(void) { @@ -596,11 +591,7 @@ bool vtxTrampInit(void) if (portConfig) { portOptions_t portOptions = 0; -#if defined(USE_VTX_COMMON) portOptions = portOptions | (vtxConfig()->halfDuplex ? SERIAL_BIDIR : SERIAL_UNIDIR); -#else - portOptions = SERIAL_BIDIR; -#endif trampSerialPort = openSerialPort(portConfig->identifier, FUNCTION_VTX_TRAMP, NULL, NULL, 9600, MODE_RXTX, portOptions); } @@ -609,9 +600,7 @@ bool vtxTrampInit(void) return false; } -#if defined(USE_VTX_COMMON) vtxCommonSetDevice(&vtxTramp); -#endif return true; } diff --git a/src/main/msp/msp_protocol.h b/src/main/msp/msp_protocol.h index 19af0cced36..d3032690183 100644 --- a/src/main/msp/msp_protocol.h +++ b/src/main/msp/msp_protocol.h @@ -60,7 +60,7 @@ #define MSP_PROTOCOL_VERSION 0 // Same version over MSPv1 & MSPv2 - message format didn't change and it backward compatible #define API_VERSION_MAJOR 2 // increment when major changes are made -#define API_VERSION_MINOR 2 // increment when any change is made, reset to zero when major changes are released after changing API_VERSION_MAJOR +#define API_VERSION_MINOR 3 // increment when any change is made, reset to zero when major changes are released after changing API_VERSION_MAJOR #define API_VERSION_LENGTH 2 @@ -328,4 +328,4 @@ // MSPv2 includes #include "msp_protocol_v2_common.h" #include "msp_protocol_v2_sensor.h" -#include "msp_protocol_v2_inav.h" \ No newline at end of file +#include "msp_protocol_v2_inav.h" diff --git a/src/main/msp/msp_protocol_v2_common.h b/src/main/msp/msp_protocol_v2_common.h index 98ebdf5e3fd..80a4c8faf44 100644 --- a/src/main/msp/msp_protocol_v2_common.h +++ b/src/main/msp/msp_protocol_v2_common.h @@ -24,4 +24,7 @@ #define MSP2_COMMON_SET_MOTOR_MIXER 0x1006 #define MSP2_COMMON_SETTING_INFO 0x1007 //in/out message Returns info about a setting (PG, type, flags, min/max, etc..). -#define MSP2_COMMON_PG_LIST 0x1008 //in/out message Returns a list of the PG ids used by the settings \ No newline at end of file +#define MSP2_COMMON_PG_LIST 0x1008 //in/out message Returns a list of the PG ids used by the settings + +#define MSP2_COMMON_SERIAL_CONFIG 0x1009 +#define MSP2_COMMON_SET_SERIAL_CONFIG 0x100A \ No newline at end of file diff --git a/src/main/msp/msp_protocol_v2_inav.h b/src/main/msp/msp_protocol_v2_inav.h index e6fa570dcdc..89ac4e433ff 100755 --- a/src/main/msp/msp_protocol_v2_inav.h +++ b/src/main/msp/msp_protocol_v2_inav.h @@ -29,6 +29,8 @@ #define MSP2_INAV_SET_RATE_PROFILE 0x2008 #define MSP2_INAV_AIR_SPEED 0x2009 #define MSP2_INAV_OUTPUT_MAPPING 0x200A +#define MSP2_INAV_MC_BRAKING 0x200B +#define MSP2_INAV_SET_MC_BRAKING 0x200C #define MSP2_INAV_MIXER 0x2010 #define MSP2_INAV_SET_MIXER 0x2011 @@ -41,3 +43,12 @@ #define MSP2_INAV_OSD_SET_PREFERENCES 0x2017 #define MSP2_INAV_SELECT_BATTERY_PROFILE 0x2018 + +#define MSP2_INAV_DEBUG 0x2019 + +#define MSP2_BLACKBOX_CONFIG 0x201A +#define MSP2_SET_BLACKBOX_CONFIG 0x201B + +#define MSP2_INAV_TEMP_SENSOR_CONFIG 0x201C +#define MSP2_INAV_SET_TEMP_SENSOR_CONFIG 0x201D +#define MSP2_INAV_TEMPERATURES 0x201E diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 2fec6a61704..5575dc8348a 100755 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -116,9 +116,17 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig, // MC-specific .mc = { - .max_bank_angle = 30, // 30 deg + .max_bank_angle = 30, // 30 deg .hover_throttle = 1500, .auto_disarm_delay = 2000, + .braking_speed_threshold = 100, // Braking can become active above 1m/s + .braking_disengage_speed = 75, // Stop when speed goes below 0.75m/s + .braking_timeout = 2000, // Timeout barking after 2s + .braking_boost_factor = 100, // A 100% boost by default + .braking_boost_timeout = 750, // Timout boost after 750ms + .braking_boost_speed_threshold = 150, // Boost can happen only above 1.5m/s + .braking_boost_disengage_speed = 100, // Disable boost at 1m/s + .braking_bank_angle = 40, // Max braking angle }, // Fixed wing @@ -566,7 +574,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .persistentId = NAV_PERSISTENT_ID_RTH_FINISHED, .onEntry = navOnEnteringState_NAV_STATE_RTH_FINISHED, .timeoutMs = 10, - .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH, + .stateFlags = NAV_CTL_ALT | NAV_REQUIRE_ANGLE | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH, .mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE, .mwState = MW_NAV_STATE_LANDED, .mwError = MW_NAV_ERROR_NONE, @@ -1098,8 +1106,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(n // If we have valid pos sensor OR we are configured to ignore GPS loss if ((posControl.flags.estPosStatus >= EST_USABLE) || !checkForPositionSensorTimeout() || navConfig()->general.flags.rth_climb_ignore_emerg) { const float rthAltitudeMargin = STATE(FIXED_WING) ? - MAX(FW_RTH_CLIMB_MARGIN_MIN_CM, (FW_RTH_CLIMB_MARGIN_PERCENT/100.0) * ABS(posControl.homeWaypointAbove.pos.z - posControl.homePosition.pos.z)) : // Airplane - MAX(MR_RTH_CLIMB_MARGIN_MIN_CM, (MR_RTH_CLIMB_MARGIN_PERCENT/100.0) * ABS(posControl.homeWaypointAbove.pos.z - posControl.homePosition.pos.z)); // Copters + MAX(FW_RTH_CLIMB_MARGIN_MIN_CM, (FW_RTH_CLIMB_MARGIN_PERCENT/100.0) * fabsf(posControl.homeWaypointAbove.pos.z - posControl.homePosition.pos.z)) : // Airplane + MAX(MR_RTH_CLIMB_MARGIN_MIN_CM, (MR_RTH_CLIMB_MARGIN_PERCENT/100.0) * fabsf(posControl.homeWaypointAbove.pos.z - posControl.homePosition.pos.z)); // Copters if (((navGetCurrentActualPositionAndVelocity()->pos.z - posControl.homeWaypointAbove.pos.z) > -rthAltitudeMargin) || (!navConfig()->general.flags.rth_climb_first)) { // Delayed initialization for RTH sanity check on airplanes - allow to finish climb first as it can take some distance @@ -1304,6 +1312,10 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_FINISHED(navigation // Stay in this state UNUSED(previousState); updateClimbRateToAltitudeController(-0.3f * navConfig()->general.land_descent_rate, ROC_TO_ALT_NORMAL); // FIXME + + // Prevent I-terms growing when already landed + pidResetErrorAccumulators(); + return NAV_FSM_EVENT_NONE; } @@ -1469,6 +1481,10 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_FINIS { // TODO: UNUSED(previousState); + + // Prevent I-terms growing when already landed + pidResetErrorAccumulators(); + return NAV_FSM_EVENT_SUCCESS; } @@ -1496,6 +1512,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_LAUNCH_WAIT(navigationF if (feature(FEATURE_FW_LAUNCH)) { throttleStatus_e throttleStatus = calculateThrottleStatus(); if ((throttleStatus == THROTTLE_LOW) && (areSticksDeflectedMoreThanPosHoldDeadband())) { + abortFixedWingLaunch(); return NAV_FSM_EVENT_SWITCH_TO_IDLE; } } @@ -1658,7 +1675,7 @@ float navPidApply3(pidController_t *pid, const float setpoint, const float measu if (pidFlags & PID_SHRINK_INTEGRATOR) { // Only allow integrator to shrink - if (ABS(newIntegrator) < ABS(pid->integrator)) { + if (fabsf(newIntegrator) < fabsf(pid->integrator)) { pid->integrator = newIntegrator; } } @@ -1891,20 +1908,49 @@ const navEstimatedPosVel_t * navGetCurrentActualPositionAndVelocity(void) /*----------------------------------------------------------- * Calculates distance and bearing to destination point *-----------------------------------------------------------*/ +static uint32_t calculateDistanceFromDelta(float deltaX, float deltaY) +{ + return sqrtf(sq(deltaX) + sq(deltaY)); +} + +static int32_t calculateBearingFromDelta(float deltaX, float deltaY) +{ + return wrap_36000(RADIANS_TO_CENTIDEGREES(atan2_approx(deltaY, deltaX))); +} + uint32_t calculateDistanceToDestination(const fpVector3_t * destinationPos) { - const float deltaX = destinationPos->x - navGetCurrentActualPositionAndVelocity()->pos.x; - const float deltaY = destinationPos->y - navGetCurrentActualPositionAndVelocity()->pos.y; + const navEstimatedPosVel_t *posvel = navGetCurrentActualPositionAndVelocity(); + const float deltaX = destinationPos->x - posvel->pos.x; + const float deltaY = destinationPos->y - posvel->pos.y; - return sqrtf(sq(deltaX) + sq(deltaY)); + return calculateDistanceFromDelta(deltaX, deltaY); } int32_t calculateBearingToDestination(const fpVector3_t * destinationPos) { - const float deltaX = destinationPos->x - navGetCurrentActualPositionAndVelocity()->pos.x; - const float deltaY = destinationPos->y - navGetCurrentActualPositionAndVelocity()->pos.y; + const navEstimatedPosVel_t *posvel = navGetCurrentActualPositionAndVelocity(); + const float deltaX = destinationPos->x - posvel->pos.x; + const float deltaY = destinationPos->y - posvel->pos.y; - return wrap_36000(RADIANS_TO_CENTIDEGREES(atan2_approx(deltaY, deltaX))); + return calculateBearingFromDelta(deltaX, deltaY); +} + +bool navCalculatePathToDestination(navDestinationPath_t *result, const fpVector3_t * destinationPos) +{ + if (posControl.flags.estPosStatus == EST_NONE || + posControl.flags.estHeadingStatus == EST_NONE) { + + return false; + } + + const navEstimatedPosVel_t *posvel = navGetCurrentActualPositionAndVelocity(); + const float deltaX = destinationPos->x - posvel->pos.x; + const float deltaY = destinationPos->y - posvel->pos.y; + + result->distance = calculateDistanceFromDelta(deltaX, deltaY); + result->bearing = calculateBearingFromDelta(deltaX, deltaY); + return true; } /*----------------------------------------------------------- @@ -1935,7 +1981,7 @@ bool isWaypointReached(const navWaypointPosition_t * waypoint, const bool isWayp static void updateHomePositionCompatibility(void) { - geoConvertLocalToGeodetic(&posControl.gpsOrigin, &posControl.homePosition.pos, &GPS_home); + geoConvertLocalToGeodetic(&GPS_home, &posControl.gpsOrigin, &posControl.homePosition.pos); GPS_distanceToHome = posControl.homeDistance / 100; GPS_directionToHome = posControl.homeDirection / 100; } @@ -2161,8 +2207,8 @@ void calculateInitialHoldPosition(fpVector3_t * pos) *-----------------------------------------------------------*/ void setDesiredPosition(const fpVector3_t * pos, int32_t yaw, navSetWaypointFlags_t useMask) { - // XY-position - if ((useMask & NAV_POS_UPDATE_XY) != 0) { + // XY-position update is allowed only when not braking in NAV_CRUISE_BRAKING + if ((useMask & NAV_POS_UPDATE_XY) != 0 && !STATE(NAV_CRUISE_BRAKING)) { posControl.desiredState.pos.x = pos->x; posControl.desiredState.pos.y = pos->y; } @@ -2330,6 +2376,7 @@ static void resetPositionController(void) } else { resetMulticopterPositionController(); + resetMulticopterBrakingMode(); } } @@ -2341,7 +2388,11 @@ static bool adjustPositionFromRCInput(void) retValue = adjustFixedWingPositionFromRCInput(); } else { - retValue = adjustMulticopterPositionFromRCInput(); + + const int16_t rcPitchAdjustment = applyDeadband(rcCommand[PITCH], rcControlsConfig()->pos_hold_deadband); + const int16_t rcRollAdjustment = applyDeadband(rcCommand[ROLL], rcControlsConfig()->pos_hold_deadband); + + retValue = adjustMulticopterPositionFromRCInput(rcPitchAdjustment, rcRollAdjustment); } return retValue; @@ -2380,7 +2431,7 @@ void getWaypoint(uint8_t wpNumber, navWaypoint_t * wpData) else if (wpNumber == 255) { gpsLocation_t wpLLH; - geoConvertLocalToGeodetic(&posControl.gpsOrigin, &navGetCurrentActualPositionAndVelocity()->pos, &wpLLH); + geoConvertLocalToGeodetic(&wpLLH, &posControl.gpsOrigin, &navGetCurrentActualPositionAndVelocity()->pos); wpData->lat = wpLLH.lat; wpData->lon = wpLLH.lon; @@ -2407,7 +2458,7 @@ void setWaypoint(uint8_t wpNumber, const navWaypoint_t * wpData) // WP #0 - special waypoint - HOME if ((wpNumber == 0) && ARMING_FLAG(ARMED) && (posControl.flags.estPosStatus >= EST_USABLE) && posControl.gpsOrigin.valid && posControl.flags.isGCSAssistedNavigationEnabled) { // Forcibly set home position. Note that this is only valid if already armed, otherwise home will be reset instantly - geoConvertGeodeticToLocal(&posControl.gpsOrigin, &wpLLH, &wpPos.pos, GEO_ALT_RELATIVE); + geoConvertGeodeticToLocal(&wpPos.pos, &posControl.gpsOrigin, &wpLLH, GEO_ALT_RELATIVE); setHomePosition(&wpPos.pos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, NAV_HOME_VALID_ALL); } // WP #255 - special waypoint - directly set desiredPosition @@ -2416,7 +2467,7 @@ void setWaypoint(uint8_t wpNumber, const navWaypoint_t * wpData) ARMING_FLAG(ARMED) && (posControl.flags.estPosStatus == EST_TRUSTED) && posControl.gpsOrigin.valid && posControl.flags.isGCSAssistedNavigationEnabled && (posControl.navState == NAV_STATE_POSHOLD_3D_IN_PROGRESS)) { // Convert to local coordinates - geoConvertGeodeticToLocal(&posControl.gpsOrigin, &wpLLH, &wpPos.pos, GEO_ALT_RELATIVE); + geoConvertGeodeticToLocal(&wpPos.pos, &posControl.gpsOrigin, &wpLLH, GEO_ALT_RELATIVE); navSetWaypointFlags_t waypointUpdateFlags = NAV_POS_UPDATE_XY; @@ -2511,7 +2562,7 @@ static void mapWaypointToLocalPosition(fpVector3_t * localPos, const navWaypoint wpLLH.lon = waypoint->lon; wpLLH.alt = waypoint->alt; - geoConvertGeodeticToLocal(&posControl.gpsOrigin, &wpLLH, localPos, GEO_ALT_RELATIVE); + geoConvertGeodeticToLocal(localPos, &posControl.gpsOrigin, &wpLLH, GEO_ALT_RELATIVE); } static void calculateAndSetActiveWaypointToLocalPosition(const fpVector3_t * pos) @@ -2596,8 +2647,15 @@ static void processNavigationRCAdjustments(void) posControl.flags.isAdjustingAltitude = false; } - if ((navStateFlags & NAV_RC_POS) && (!FLIGHT_MODE(FAILSAFE_MODE))) { - posControl.flags.isAdjustingPosition = adjustPositionFromRCInput(); + if (navStateFlags & NAV_RC_POS) { + if (!FLIGHT_MODE(FAILSAFE_MODE)) { + posControl.flags.isAdjustingPosition = adjustPositionFromRCInput(); + } + else { + if (!STATE(FIXED_WING)) { + resetMulticopterBrakingMode(); + } + } } else { posControl.flags.isAdjustingPosition = false; @@ -3050,8 +3108,10 @@ void activateForcedRTH(void) void abortForcedRTH(void) { + // Disable failsafe RTH and make sure we back out of navigation mode to IDLE + // If any navigation mode was active prior to RTH it will be re-enabled with next RX update posControl.flags.forcedRTHActivated = false; - navProcessFSMEvents(selectNavEventFromBoxModeInput()); + navProcessFSMEvents(NAV_FSM_EVENT_SWITCH_TO_IDLE); } rthState_e getStateOfForcedRTH(void) @@ -3179,7 +3239,7 @@ void onNewGPSData(void) GPS_home.alt = gpsSol.llh.alt; GPS_distanceToHome = 0; GPS_directionToHome = 0; - GPS_scaleLonDown = cos_approx((ABS((float)gpsSol.llh.lat) / 10000000.0f) * 0.0174532925f); + GPS_scaleLonDown = cos_approx((fabsf((float)gpsSol.llh.lat) / 10000000.0f) * 0.0174532925f); } } diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index b25b1120164..6a7c9d19e99 100755 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -17,6 +17,8 @@ #pragma once +#include + #include "common/axis.h" #include "common/filter.h" #include "common/maths.h" @@ -52,6 +54,12 @@ enum { NAV_GPS_CRUISE = 1 // Pitch/roll stick controls velocity (forward/right speed) }; +enum { + NAV_LOITER_RIGHT = 0, // Loitering direction right + NAV_LOITER_LEFT = 1, // Loitering direction left + NAV_LOITER_YAW = 2 +}; + enum { NAV_RTH_NO_ALT = 0, // Maintain current altitude NAV_RTH_EXTRA_ALT = 1, // Maintain current altitude + predefined safety margin @@ -148,9 +156,17 @@ typedef struct navConfig_s { } general; struct { - uint8_t max_bank_angle; // multicopter max banking angle (deg) - uint16_t hover_throttle; // multicopter hover throttle - uint16_t auto_disarm_delay; // multicopter safety delay for landing detector + uint8_t max_bank_angle; // multicopter max banking angle (deg) + uint16_t hover_throttle; // multicopter hover throttle + uint16_t auto_disarm_delay; // multicopter safety delay for landing detector + uint16_t braking_speed_threshold; // above this speed braking routine might kick in + uint16_t braking_disengage_speed; // below this speed braking will be disengaged + uint16_t braking_timeout; // Timeout for braking mode + uint8_t braking_boost_factor; // Acceleration boost multiplier at max speed + uint16_t braking_boost_timeout; // Timeout for boost mode + uint16_t braking_boost_speed_threshold; // Above this speed braking boost mode can engage + uint16_t braking_boost_disengage_speed; // Below this speed braking boost will disengage + uint8_t braking_bank_angle; // Max angle [deg] that MR is allowed duing braking boost phase } mc; struct { @@ -189,7 +205,7 @@ typedef struct gpsOrigin_s { int32_t lat; // Lattitude * 1e+7 int32_t lon; // Longitude * 1e+7 int32_t alt; // Altitude in centimeters (meters * 100) -} gpsOrigin_s; +} gpsOrigin_t; typedef enum { NAV_WP_ACTION_WAYPOINT = 0x01, @@ -214,6 +230,11 @@ typedef struct { int32_t yaw; // deg * 100 } navWaypointPosition_t; +typedef struct navDestinationPath_s { + uint32_t distance; // meters * 100 + int32_t bearing; // deg * 100 +} navDestinationPath_t; + typedef struct { float kP; float kI; @@ -317,6 +338,7 @@ void navigationInit(void); void updatePositionEstimator_BaroTopic(timeUs_t currentTimeUs); void updatePositionEstimator_OpticalFlowTopic(timeUs_t currentTimeUs); void updatePositionEstimator_SurfaceTopic(timeUs_t currentTimeUs, float newSurfaceAlt); +void updatePositionEstimator_PitotTopic(timeUs_t currentTimeUs); /* Navigation system updates */ void updateWaypointsAndNavigationMode(void); @@ -371,11 +393,30 @@ typedef enum { GEO_ORIGIN_RESET_ALTITUDE } geoOriginResetMode_e; -void geoSetOrigin(gpsOrigin_s * origin, const gpsLocation_t * llh, geoOriginResetMode_e resetMode); -void geoConvertGeodeticToLocal(gpsOrigin_s * origin, const gpsLocation_t * llh, fpVector3_t * pos, geoAltitudeConversionMode_e altConv); -void geoConvertLocalToGeodetic(const gpsOrigin_s * origin, const fpVector3_t * pos, gpsLocation_t * llh); +// geoSetOrigin stores the location provided in llh as a GPS origin in the +// provided origin parameter. resetMode indicates wether all origin coordinates +// should be overwritten by llh (GEO_ORIGIN_SET) or just the altitude, leaving +// other fields untouched (GEO_ORIGIN_RESET_ALTITUDE). +void geoSetOrigin(gpsOrigin_t *origin, const gpsLocation_t *llh, geoOriginResetMode_e resetMode); +// geoConvertGeodeticToLocal converts the geodetic location given in llh to +// the local coordinate space and stores the result in pos. The altConv +// indicates wether the altitude in llh is relative to the default GPS +// origin (GEO_ALT_RELATIVE) or absolute (e.g. Earth frame) +// (GEO_ALT_ABSOLUTE). If origin is invalid pos is set to +// (0, 0, 0) and false is returned. It returns true otherwise. +bool geoConvertGeodeticToLocal(fpVector3_t *pos, const gpsOrigin_t *origin, const gpsLocation_t *llh, geoAltitudeConversionMode_e altConv); +// geoConvertGeodeticToLocalOrigin calls geoConvertGeodeticToLocal with the +// default GPS origin. +bool geoConvertGeodeticToLocalOrigin(fpVector3_t * pos, const gpsLocation_t *llh, geoAltitudeConversionMode_e altConv); +// geoConvertLocalToGeodetic converts a local point as provided in pos to +// geodetic coordinates using the provided GPS origin. It returns wether +// the provided origin is valid and the conversion could be performed. +bool geoConvertLocalToGeodetic(gpsLocation_t *llh, const gpsOrigin_t *origin, const fpVector3_t *pos); float geoCalculateMagDeclination(const gpsLocation_t * llh); // degrees units +/* Distance/bearing calculation */ +bool navCalculatePathToDestination(navDestinationPath_t *result, const fpVector3_t * destinationPos); + /* Failsafe-forced RTH mode */ void activateForcedRTH(void); void abortForcedRTH(void); @@ -393,6 +434,7 @@ bool navigationRTHAllowsLanding(void); bool isNavLaunchEnabled(void); bool isFixedWingLaunchDetected(void); +bool isFixedWingLaunchFinishedOrAborted(void); float calculateAverageSpeed(); diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index dde97449efe..99f058c21c9 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -43,6 +43,7 @@ #include "fc/config.h" #include "fc/controlrate_profile.h" #include "fc/rc_controls.h" +#include "fc/rc_modes.h" #include "fc/runtime_config.h" #include "navigation/navigation.h" @@ -63,6 +64,7 @@ static bool isRollAdjustmentValid = false; static float throttleSpeedAdjustment = 0; static bool isAutoThrottleManuallyIncreased = false; static int32_t navHeadingError; +static int8_t loiterDirYaw = 1; /*----------------------------------------------------------- @@ -213,6 +215,18 @@ void resetFixedWingPositionController(void) pt1FilterReset(&fwPosControllerCorrectionFilterState, 0.0f); } +static int8_t loiterDirection(void) { + int8_t dir = 1; //NAV_LOITER_RIGHT + if (pidProfile()->loiter_direction == NAV_LOITER_LEFT) dir = -1; + if (pidProfile()->loiter_direction == NAV_LOITER_YAW) { + if (rcCommand[YAW] < -250) loiterDirYaw = 1; //RIGHT //yaw is contrariwise + if (rcCommand[YAW] > 250) loiterDirYaw = -1; //LEFT //see annexCode in fc_core.c + dir = loiterDirYaw; + } + if (IS_RC_MODE_ACTIVE(BOXLOITERDIRCHN)) dir *= -1; + return dir; +} + static void calculateVirtualPositionTarget_FW(float trackingPeriod) { float posErrorX = posControl.desiredState.pos.x - navGetCurrentActualPositionAndVelocity()->pos.x; @@ -233,7 +247,7 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod) // Calculate virtual position for straight movement if (needToCalculateCircularLoiter) { // We are closing in on a waypoint, calculate circular loiter - float loiterAngle = atan2_approx(-posErrorY, -posErrorX) + DEGREES_TO_RADIANS(45.0f); + float loiterAngle = atan2_approx(-posErrorY, -posErrorX) + DEGREES_TO_RADIANS(loiterDirection() * 45.0f); float loiterTargetX = posControl.desiredState.pos.x + navConfig()->fw.loiter_radius * cos_approx(loiterAngle); float loiterTargetY = posControl.desiredState.pos.y + navConfig()->fw.loiter_radius * sin_approx(loiterAngle); @@ -258,8 +272,6 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod) // Rotate this target shift from body frame to to earth frame and apply to position target virtualDesiredPosition.x += -rcShiftY * posControl.actualState.sinYaw; virtualDesiredPosition.y += rcShiftY * posControl.actualState.cosYaw; - - posControl.flags.isAdjustingPosition = true; } } } @@ -273,7 +285,7 @@ bool adjustFixedWingPositionFromRCInput(void) static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta_t deltaMicros) { static timeUs_t previousTimeMonitoringUpdate; - static float previousHeadingError; + static int32_t previousHeadingError; static bool errorIsDecreasing; static bool forceTurnDirection = false; @@ -294,7 +306,7 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta // If forced turn direction flag is enabled we fix the sign of the direction if (forceTurnDirection) { - navHeadingError = ABS(navHeadingError); + navHeadingError = loiterDirection() * ABS(navHeadingError); } // Slow error monitoring (2Hz rate) @@ -310,7 +322,7 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta // Only allow PID integrator to shrink if error is decreasing over time const pidControllerFlags_e pidFlags = PID_DTERM_FROM_ERROR | (errorIsDecreasing ? PID_SHRINK_INTEGRATOR : 0); - // Input error in (deg*100), output pitch angle (deg*100) + // Input error in (deg*100), output roll angle (deg*100) float rollAdjustment = navPidApply2(&posControl.pids.fw_nav, posControl.actualState.yaw + navHeadingError, posControl.actualState.yaw, US2S(deltaMicros), -DEGREES_TO_CENTIDEGREES(navConfig()->fw.max_bank_angle), DEGREES_TO_CENTIDEGREES(navConfig()->fw.max_bank_angle), @@ -398,7 +410,7 @@ int16_t applyFixedWingMinSpeedController(timeUs_t currentTimeUs) float velThrottleBoost = (NAV_FW_MIN_VEL_SPEED_BOOST - posControl.actualState.velXY) * NAV_FW_THROTTLE_SPEED_BOOST_GAIN * US2S(deltaMicrosPositionUpdate); // If we are in the deadband of 50cm/s - don't update speed boost - if (ABS(posControl.actualState.velXY - NAV_FW_MIN_VEL_SPEED_BOOST) > 50) { + if (fabsf(posControl.actualState.velXY - NAV_FW_MIN_VEL_SPEED_BOOST) > 50) { throttleSpeedAdjustment += velThrottleBoost; } diff --git a/src/main/navigation/navigation_fw_launch.c b/src/main/navigation/navigation_fw_launch.c index de0a8f9547b..5613808e5cc 100755 --- a/src/main/navigation/navigation_fw_launch.c +++ b/src/main/navigation/navigation_fw_launch.c @@ -76,7 +76,7 @@ static FixedWingLaunchState_t launchState; #define SWING_LAUNCH_MIN_ROTATION_RATE DEGREES_TO_RADIANS(100) // expect minimum 100dps rotation rate static void updateFixedWingLaunchDetector(timeUs_t currentTimeUs) { - const float swingVelocity = (ABS(imuMeasuredRotationBF.z) > SWING_LAUNCH_MIN_ROTATION_RATE) ? (imuMeasuredAccelBF.y / imuMeasuredRotationBF.z) : 0; + const float swingVelocity = (fabsf(imuMeasuredRotationBF.z) > SWING_LAUNCH_MIN_ROTATION_RATE) ? (imuMeasuredAccelBF.y / imuMeasuredRotationBF.z) : 0; const bool isForwardAccelerationHigh = (imuMeasuredAccelBF.x > navConfig()->fw.launch_accel_thresh); const bool isAircraftAlmostLevel = (calculateCosTiltAngle() >= cos_approx(DEGREES_TO_RADIANS(navConfig()->fw.launch_max_angle))); @@ -122,6 +122,13 @@ bool isFixedWingLaunchFinishedOrAborted(void) return launchState.launchFinished; } +void abortFixedWingLaunch(void) +{ + launchState.launchFinished = true; +} + +#define LAUNCH_MOTOR_IDLE_SPINUP_TIME 1500 //ms + static void applyFixedWingLaunchIdleLogic(void) { // Until motors are started don't use PID I-term @@ -131,12 +138,25 @@ static void applyFixedWingLaunchIdleLogic(void) pidResetTPAFilter(); // Throttle control logic - if (navConfig()->fw.launch_idle_throttle <= motorConfig()->minthrottle) { - ENABLE_STATE(NAV_MOTOR_STOP_OR_IDLE); // If MOTOR_STOP is enabled mixer will keep motor stopped - rcCommand[THROTTLE] = motorConfig()->minthrottle; // If MOTOR_STOP is disabled, motors will spin at minthrottle + if (navConfig()->fw.launch_idle_throttle <= motorConfig()->minthrottle) + { + ENABLE_STATE(NAV_MOTOR_STOP_OR_IDLE); // If MOTOR_STOP is enabled mixer will keep motor stopped + rcCommand[THROTTLE] = motorConfig()->minthrottle; // If MOTOR_STOP is disabled, motors will spin at minthrottle } - else { - rcCommand[THROTTLE] = navConfig()->fw.launch_idle_throttle; + else + { + static float timeThrottleRaisedMs; + if (calculateThrottleStatus() == THROTTLE_LOW) + { + timeThrottleRaisedMs = millis(); + } + else + { + const float timeSinceMotorStartMs = MIN(millis() - timeThrottleRaisedMs, LAUNCH_MOTOR_IDLE_SPINUP_TIME); + rcCommand[THROTTLE] = scaleRangef(timeSinceMotorStartMs, + 0.0f, LAUNCH_MOTOR_IDLE_SPINUP_TIME, + motorConfig()->minthrottle, navConfig()->fw.launch_idle_throttle); + } } } diff --git a/src/main/navigation/navigation_geo.c b/src/main/navigation/navigation_geo.c index cf388ca914f..b6e9f3f8447 100755 --- a/src/main/navigation/navigation_geo.c +++ b/src/main/navigation/navigation_geo.c @@ -133,7 +133,7 @@ float geoCalculateMagDeclination(const gpsLocation_t * llh) // degrees units } #endif -void geoSetOrigin(gpsOrigin_s * origin, const gpsLocation_t * llh, geoOriginResetMode_e resetMode) +void geoSetOrigin(gpsOrigin_t *origin, const gpsLocation_t *llh, geoOriginResetMode_e resetMode) { if (resetMode == GEO_ORIGIN_SET) { origin->valid = true; @@ -147,7 +147,7 @@ void geoSetOrigin(gpsOrigin_s * origin, const gpsLocation_t * llh, geoOriginRese } } -void geoConvertGeodeticToLocal(gpsOrigin_s * origin, const gpsLocation_t * llh, fpVector3_t * pos, geoAltitudeConversionMode_e altConv) +bool geoConvertGeodeticToLocal(fpVector3_t *pos, const gpsOrigin_t *origin, const gpsLocation_t *llh, geoAltitudeConversionMode_e altConv) { if (origin->valid) { pos->x = (llh->lat - origin->lat) * DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR; @@ -159,15 +159,21 @@ void geoConvertGeodeticToLocal(gpsOrigin_s * origin, const gpsLocation_t * llh, } else { pos->z = llh->alt - origin->alt; } + return true; } - else { - pos->x = 0.0f; - pos->y = 0.0f; - pos->z = 0.0f; - } + + pos->x = 0.0f; + pos->y = 0.0f; + pos->z = 0.0f; + return false; +} + +bool geoConvertGeodeticToLocalOrigin(fpVector3_t * pos, const gpsLocation_t *llh, geoAltitudeConversionMode_e altConv) +{ + return geoConvertGeodeticToLocal(pos, &posControl.gpsOrigin, llh, altConv); } -void geoConvertLocalToGeodetic(const gpsOrigin_s * origin, const fpVector3_t * pos, gpsLocation_t * llh) +bool geoConvertLocalToGeodetic(gpsLocation_t *llh, const gpsOrigin_t * origin, const fpVector3_t *pos) { float scaleLonDown; @@ -187,6 +193,7 @@ void geoConvertLocalToGeodetic(const gpsOrigin_s * origin, const fpVector3_t * p llh->lat += lrintf(pos->x / DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR); llh->lon += lrintf(pos->y / (DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR * scaleLonDown)); llh->alt += lrintf(pos->z); + return origin->valid; } diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c index 3da55c5aa54..eb4ad2da7cc 100755 --- a/src/main/navigation/navigation_multicopter.c +++ b/src/main/navigation/navigation_multicopter.c @@ -39,6 +39,7 @@ #include "fc/config.h" #include "fc/rc_controls.h" #include "fc/rc_curves.h" +#include "fc/rc_modes.h" #include "fc/runtime_config.h" #include "flight/pid.h" @@ -74,9 +75,10 @@ static void updateAltitudeVelocityController_MC(timeDelta_t deltaMicros) posControl.pids.pos[Z].output_constrained = targetVel; - // limit max vertical acceleration to 1/5G (~200 cm/s/s) if we are increasing velocity. + // limit max vertical acceleration to 1/5G (~200 cm/s/s) if we are increasing RoC or RoD (only if vel is of the same sign) // if we are decelerating - don't limit (allow better recovery from falling) - if (ABS(targetVel) > ABS(posControl.desiredState.vel.z)) { + const bool isSameDirection = (signbit(targetVel) == signbit(posControl.desiredState.vel.z)) && (targetVel != 0) && (posControl.desiredState.vel.z != 0); + if (isSameDirection && (fabsf(targetVel) > fabsf(posControl.desiredState.vel.z))) { const float maxVelDifference = US2S(deltaMicros) * (GRAVITY_CMSS / 5.0f); posControl.desiredState.vel.z = constrainf(targetVel, posControl.desiredState.vel.z - maxVelDifference, posControl.desiredState.vel.z + maxVelDifference); } @@ -264,6 +266,98 @@ bool adjustMulticopterHeadingFromRCInput(void) static pt1Filter_t mcPosControllerAccFilterStateX, mcPosControllerAccFilterStateY; static float lastAccelTargetX = 0.0f, lastAccelTargetY = 0.0f; +void resetMulticopterBrakingMode(void) +{ + DISABLE_STATE(NAV_CRUISE_BRAKING); + DISABLE_STATE(NAV_CRUISE_BRAKING_BOOST); + DISABLE_STATE(NAV_CRUISE_BRAKING_LOCKED); +} + +static void processMulticopterBrakingMode(const bool isAdjusting) +{ +#ifdef USE_MR_BRAKING_MODE + static uint32_t brakingModeDisengageAt = 0; + static uint32_t brakingBoostModeDisengageAt = 0; + + if (!(NAV_Status.state == MW_NAV_STATE_NONE || NAV_Status.state == MW_NAV_STATE_HOLD_INFINIT)) { + resetMulticopterBrakingMode(); + return; + } + + const bool brakingEntryAllowed = + IS_RC_MODE_ACTIVE(BOXBRAKING) && + !STATE(NAV_CRUISE_BRAKING_LOCKED) && + posControl.actualState.velXY > navConfig()->mc.braking_speed_threshold && + !isAdjusting && + navConfig()->general.flags.user_control_mode == NAV_GPS_CRUISE && + navConfig()->mc.braking_speed_threshold > 0; + + + /* + * Case one, when we order to brake (sticks to the center) and we are moving above threshold + * Speed is above 1m/s and sticks are centered + * Extra condition: BRAKING flight mode has to be enabled + */ + if (brakingEntryAllowed) { + /* + * Set currnt position and target position + * Enabling NAV_CRUISE_BRAKING locks other routines from setting position! + */ + setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, 0, NAV_POS_UPDATE_XY); + + ENABLE_STATE(NAV_CRUISE_BRAKING_LOCKED); + ENABLE_STATE(NAV_CRUISE_BRAKING); + + //Set forced BRAKING disengage moment + brakingModeDisengageAt = millis() + navConfig()->mc.braking_timeout; + + //If speed above threshold, start boost mode as well + if (posControl.actualState.velXY > navConfig()->mc.braking_boost_speed_threshold) { + ENABLE_STATE(NAV_CRUISE_BRAKING_BOOST); + + brakingBoostModeDisengageAt = millis() + navConfig()->mc.braking_boost_timeout; + } + + } + + // We can enter braking only after user started to move the sticks + if (STATE(NAV_CRUISE_BRAKING_LOCKED) && isAdjusting) { + DISABLE_STATE(NAV_CRUISE_BRAKING_LOCKED); + } + + /* + * Case when speed dropped, disengage BREAKING_BOOST + */ + if ( + STATE(NAV_CRUISE_BRAKING_BOOST) && ( + posControl.actualState.velXY <= navConfig()->mc.braking_boost_disengage_speed || + brakingBoostModeDisengageAt < millis() + )) { + DISABLE_STATE(NAV_CRUISE_BRAKING_BOOST); + } + + /* + * Case when we were braking but copter finally stopped or we started to move the sticks + */ + if (STATE(NAV_CRUISE_BRAKING) && ( + posControl.actualState.velXY <= navConfig()->mc.braking_disengage_speed || //We stopped + isAdjusting || //Moved the sticks + brakingModeDisengageAt < millis() //Braking is done to timed disengage + )) { + DISABLE_STATE(NAV_CRUISE_BRAKING); + DISABLE_STATE(NAV_CRUISE_BRAKING_BOOST); + + /* + * When braking is done, store current position as desired one + * We do not want to go back to the place where braking has started + */ + setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, 0, NAV_POS_UPDATE_XY); + } +#else + UNUSED(isAdjusting); +#endif +} + void resetMulticopterPositionController(void) { for (int axis = 0; axis < 2; axis++) { @@ -276,11 +370,12 @@ void resetMulticopterPositionController(void) } } -bool adjustMulticopterPositionFromRCInput(void) +bool adjustMulticopterPositionFromRCInput(int16_t rcPitchAdjustment, int16_t rcRollAdjustment) { - const int16_t rcPitchAdjustment = applyDeadband(rcCommand[PITCH], rcControlsConfig()->pos_hold_deadband); - const int16_t rcRollAdjustment = applyDeadband(rcCommand[ROLL], rcControlsConfig()->pos_hold_deadband); + // Process braking mode + processMulticopterBrakingMode(rcPitchAdjustment || rcRollAdjustment); + // Actually change position if (rcPitchAdjustment || rcRollAdjustment) { // If mode is GPS_CRUISE, move target position, otherwise POS controller will passthru the RC input to ANGLE PID if (navConfig()->general.flags.user_control_mode == NAV_GPS_CRUISE) { @@ -382,15 +477,22 @@ static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxA if (velErrorMagnitude > 0.1f) { accelLimitX = maxAccelLimit / velErrorMagnitude * fabsf(velErrorX); accelLimitY = maxAccelLimit / velErrorMagnitude * fabsf(velErrorY); - } - else { + } else { accelLimitX = maxAccelLimit / 1.414213f; accelLimitY = accelLimitX; } // Apply additional jerk limiting of 1700 cm/s^3 (~100 deg/s), almost any copter should be able to achieve this rate // This will assure that we wont't saturate out LEVEL and RATE PID controller - const float maxAccelChange = US2S(deltaMicros) * 1700.0f; + + float maxAccelChange = US2S(deltaMicros) * 1700.0f; + //When braking, raise jerk limit even if we are not boosting acceleration +#ifdef USE_MR_BRAKING_MODE + if (STATE(NAV_CRUISE_BRAKING)) { + maxAccelChange = maxAccelChange * 2; + } +#endif + const float accelLimitXMin = constrainf(lastAccelTargetX - maxAccelChange, -accelLimitX, +accelLimitX); const float accelLimitXMax = constrainf(lastAccelTargetX + maxAccelChange, -accelLimitX, +accelLimitX); const float accelLimitYMin = constrainf(lastAccelTargetY - maxAccelChange, -accelLimitY, +accelLimitY); @@ -401,16 +503,46 @@ static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxA // Apply PID with output limiting and I-term anti-windup // Pre-calculated accelLimit and the logic of navPidApply2 function guarantee that our newAccel won't exceed maxAccelLimit // Thus we don't need to do anything else with calculated acceleration - const float newAccelX = navPidApply2(&posControl.pids.vel[X], posControl.desiredState.vel.x, navGetCurrentActualPositionAndVelocity()->vel.x, US2S(deltaMicros), accelLimitXMin, accelLimitXMax, 0); - const float newAccelY = navPidApply2(&posControl.pids.vel[Y], posControl.desiredState.vel.y, navGetCurrentActualPositionAndVelocity()->vel.y, US2S(deltaMicros), accelLimitYMin, accelLimitYMax, 0); + float newAccelX = navPidApply2(&posControl.pids.vel[X], posControl.desiredState.vel.x, navGetCurrentActualPositionAndVelocity()->vel.x, US2S(deltaMicros), accelLimitXMin, accelLimitXMax, 0); + float newAccelY = navPidApply2(&posControl.pids.vel[Y], posControl.desiredState.vel.y, navGetCurrentActualPositionAndVelocity()->vel.y, US2S(deltaMicros), accelLimitYMin, accelLimitYMax, 0); + + int32_t maxBankAngle = DEGREES_TO_DECIDEGREES(navConfig()->mc.max_bank_angle); + uint8_t accCutoffFrequency = NAV_ACCEL_CUTOFF_FREQUENCY_HZ; + +#ifdef USE_MR_BRAKING_MODE + //Boost required accelerations + if (STATE(NAV_CRUISE_BRAKING_BOOST) && navConfig()->mc.braking_boost_factor > 0) { + const float rawBoostFactor = (float)navConfig()->mc.braking_boost_factor / 100.0f; + + //Scale boost factor according to speed + const float boostFactor = constrainf( + scaleRangef( + posControl.actualState.velXY, + navConfig()->mc.braking_boost_speed_threshold, + navConfig()->general.max_manual_speed, + 0.0f, + rawBoostFactor + ), + 0.0f, + rawBoostFactor + ); + + //Boost required acceleration for harder braking + newAccelX = newAccelX * (1.0f + boostFactor); + newAccelY = newAccelY * (1.0f + boostFactor); + + maxBankAngle = DEGREES_TO_DECIDEGREES(navConfig()->mc.braking_bank_angle); + accCutoffFrequency = NAV_ACCEL_CUTOFF_FREQUENCY_HZ * 2; + } +#endif // Save last acceleration target lastAccelTargetX = newAccelX; lastAccelTargetY = newAccelY; // Apply LPF to jerk limited acceleration target - const float accelN = pt1FilterApply4(&mcPosControllerAccFilterStateX, newAccelX, NAV_ACCEL_CUTOFF_FREQUENCY_HZ, US2S(deltaMicros)); - const float accelE = pt1FilterApply4(&mcPosControllerAccFilterStateY, newAccelY, NAV_ACCEL_CUTOFF_FREQUENCY_HZ, US2S(deltaMicros)); + const float accelN = pt1FilterApply4(&mcPosControllerAccFilterStateX, newAccelX, accCutoffFrequency, US2S(deltaMicros)); + const float accelE = pt1FilterApply4(&mcPosControllerAccFilterStateY, newAccelY, accCutoffFrequency, US2S(deltaMicros)); // Rotate acceleration target into forward-right frame (aircraft) const float accelForward = accelN * posControl.actualState.cosYaw + accelE * posControl.actualState.sinYaw; @@ -420,7 +552,6 @@ static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxA const float desiredPitch = atan2_approx(accelForward, GRAVITY_CMSS); const float desiredRoll = atan2_approx(accelRight * cos_approx(desiredPitch), GRAVITY_CMSS); - const int16_t maxBankAngle = DEGREES_TO_DECIDEGREES(navConfig()->mc.max_bank_angle); posControl.rcAdjustment[ROLL] = constrain(RADIANS_TO_DECIDEGREES(desiredRoll), -maxBankAngle, maxBankAngle); posControl.rcAdjustment[PITCH] = constrain(RADIANS_TO_DECIDEGREES(desiredPitch), -maxBankAngle, maxBankAngle); } diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index 2c46baa74f6..cf9445786da 100755 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -228,7 +228,7 @@ void onNewGPSData(void) if (posControl.gpsOrigin.valid) { /* Convert LLH position to local coordinates */ - geoConvertGeodeticToLocal(&posControl.gpsOrigin, &newLLH, & posEstimator.gps.pos, GEO_ALT_ABSOLUTE); + geoConvertGeodeticToLocal(&posEstimator.gps.pos, &posControl.gpsOrigin, &newLLH, GEO_ALT_ABSOLUTE); /* If not the first update - calculate velocities */ if (!isFirstGPSUpdate) { @@ -309,9 +309,15 @@ void updatePositionEstimator_BaroTopic(timeUs_t currentTimeUs) } if (sensors(SENSOR_BARO) && baroIsCalibrationComplete()) { + const timeUs_t baroDtUs = currentTimeUs - posEstimator.baro.lastUpdateTime; + posEstimator.baro.alt = newBaroAlt - initialBaroAltitudeOffset; posEstimator.baro.epv = positionEstimationConfig()->baro_epv; posEstimator.baro.lastUpdateTime = currentTimeUs; + + if (baroDtUs <= MS2US(INAV_BARO_TIMEOUT_MS)) { + pt1FilterApply3(&posEstimator.baro.avgFilter, posEstimator.baro.alt, US2S(baroDtUs)); + } } else { posEstimator.baro.alt = 0; @@ -325,19 +331,10 @@ void updatePositionEstimator_BaroTopic(timeUs_t currentTimeUs) * Read Pitot and update airspeed topic * Function is called at main loop rate, updates happen at reduced rate */ -static void updatePitotTopic(timeUs_t currentTimeUs) +void updatePositionEstimator_PitotTopic(timeUs_t currentTimeUs) { - static navigationTimer_t pitotUpdateTimer; - - if (updateTimer(&pitotUpdateTimer, HZ2US(INAV_PITOT_UPDATE_RATE), currentTimeUs)) { - float newTAS = pitotCalculateAirSpeed(); - if (sensors(SENSOR_PITOT) && pitotIsCalibrationComplete()) { - posEstimator.pitot.airspeed = newTAS; - } - else { - posEstimator.pitot.airspeed = 0; - } - } + posEstimator.pitot.airspeed = pitot.airSpeed; + posEstimator.pitot.lastUpdateTime = currentTimeUs; } #endif @@ -345,18 +342,24 @@ static void updatePitotTopic(timeUs_t currentTimeUs) * Update IMU topic * Function is called at main loop rate */ -static void updateIMUTopic(void) +static void restartGravityCalibration(void) { - static float calibratedGravityCMSS = GRAVITY_CMSS; - static timeMs_t gravityCalibrationTimeout = 0; + zeroCalibrationStartS(&posEstimator.imu.gravityCalibration, CALIBRATING_GRAVITY_TIME_MS, positionEstimationConfig()->gravity_calibration_tolerance, false); +} +static bool gravityCalibrationComplete(void) +{ + return zeroCalibrationIsCompleteS(&posEstimator.imu.gravityCalibration); +} + +static void updateIMUTopic(void) +{ if (!isImuReady()) { posEstimator.imu.accelNEU.x = 0; posEstimator.imu.accelNEU.y = 0; posEstimator.imu.accelNEU.z = 0; - gravityCalibrationTimeout = millis(); - posEstimator.imu.gravityCalibrationComplete = false; + restartGravityCalibration(); } else { fpVector3_t accelBF; @@ -380,24 +383,18 @@ static void updateIMUTopic(void) posEstimator.imu.accelNEU.z = accelBF.z; /* When unarmed, assume that accelerometer should measure 1G. Use that to correct accelerometer gain */ - if (!ARMING_FLAG(ARMED) && !posEstimator.imu.gravityCalibrationComplete) { - // Slowly converge on calibrated gravity while level - const float gravityOffsetError = posEstimator.imu.accelNEU.z - calibratedGravityCMSS; - calibratedGravityCMSS += gravityOffsetError * 0.0025f; - - if (ABS(gravityOffsetError) < positionEstimationConfig()->gravity_calibration_tolerance) { // Error should be within 0.5% of calibrated gravity - if ((millis() - gravityCalibrationTimeout) > 250) { - posEstimator.imu.gravityCalibrationComplete = true; - } - } - else { - gravityCalibrationTimeout = millis(); + if (!ARMING_FLAG(ARMED) && !gravityCalibrationComplete()) { + zeroCalibrationAddValueS(&posEstimator.imu.gravityCalibration, posEstimator.imu.accelNEU.z); + + if (gravityCalibrationComplete()) { + zeroCalibrationGetZeroS(&posEstimator.imu.gravityCalibration, &posEstimator.imu.calibratedGravityCMSS); + DEBUG_TRACE_SYNC("Gravity calibration complete (%d)", lrintf(posEstimator.imu.calibratedGravityCMSS)); } } /* If calibration is incomplete - report zero acceleration */ - if (posEstimator.imu.gravityCalibrationComplete) { - posEstimator.imu.accelNEU.z -= calibratedGravityCMSS; + if (gravityCalibrationComplete()) { + posEstimator.imu.accelNEU.z -= posEstimator.imu.calibratedGravityCMSS; } else { posEstimator.imu.accelNEU.x = 0; @@ -771,13 +768,16 @@ void initializePositionEstimator(void) posEstimator.est.flowCoordinates[X] = 0; posEstimator.est.flowCoordinates[Y] = 0; - posEstimator.imu.gravityCalibrationComplete = false; + restartGravityCalibration(); for (axis = 0; axis < 3; axis++) { posEstimator.imu.accelBias.v[axis] = 0; posEstimator.est.pos.v[axis] = 0; posEstimator.est.vel.v[axis] = 0; } + + pt1FilterInit(&posEstimator.baro.avgFilter, INAV_BARO_AVERAGE_HZ, 0.0f); + pt1FilterInit(&posEstimator.surface.avgFilter, INAV_SURFACE_AVERAGE_HZ, 0.0f); } /** @@ -795,11 +795,6 @@ void updatePositionEstimator(void) const timeUs_t currentTimeUs = micros(); - /* Periodic sensor updates */ -#if defined(USE_PITOT) - updatePitotTopic(currentTimeUs); -#endif - /* Read updates from IMU, preprocess */ updateIMUTopic(); @@ -812,7 +807,7 @@ void updatePositionEstimator(void) bool navIsCalibrationComplete(void) { - return posEstimator.imu.gravityCalibrationComplete; + return gravityCalibrationComplete(); } #endif diff --git a/src/main/navigation/navigation_pos_estimator_agl.c b/src/main/navigation/navigation_pos_estimator_agl.c index cba35d40692..21c09909dec 100644 --- a/src/main/navigation/navigation_pos_estimator_agl.c +++ b/src/main/navigation/navigation_pos_estimator_agl.c @@ -50,14 +50,16 @@ extern navigationPosEstimator_t posEstimator; */ void updatePositionEstimator_SurfaceTopic(timeUs_t currentTimeUs, float newSurfaceAlt) { - const float dt = US2S(currentTimeUs - posEstimator.surface.lastUpdateTime); + const float surfaceDtUs = currentTimeUs - posEstimator.surface.lastUpdateTime; float newReliabilityMeasurement = 0; + bool surfaceMeasurementWithinRange = false; posEstimator.surface.lastUpdateTime = currentTimeUs; if (newSurfaceAlt >= 0) { if (newSurfaceAlt <= positionEstimationConfig()->max_surface_altitude) { newReliabilityMeasurement = 1.0f; + surfaceMeasurementWithinRange = true; posEstimator.surface.alt = newSurfaceAlt; } else { @@ -70,12 +72,18 @@ void updatePositionEstimator_SurfaceTopic(timeUs_t currentTimeUs, float newSurfa } /* Reliability is a measure of confidence of rangefinder measurement. It's increased with each valid sample and decreased with each invalid sample */ - if (dt > 0.5f) { + if (surfaceDtUs > MS2US(INAV_SURFACE_TIMEOUT_MS)) { posEstimator.surface.reliability = 0.0f; } else { - const float relAlpha = dt / (dt + RANGEFINDER_RELIABILITY_RC_CONSTANT); + const float surfaceDt = US2S(surfaceDtUs); + const float relAlpha = surfaceDt / (surfaceDt + RANGEFINDER_RELIABILITY_RC_CONSTANT); posEstimator.surface.reliability = posEstimator.surface.reliability * (1.0f - relAlpha) + newReliabilityMeasurement * relAlpha; + + // Update average sonar altitude if range is good + if (surfaceMeasurementWithinRange) { + pt1FilterApply3(&posEstimator.surface.avgFilter, newSurfaceAlt, surfaceDt); + } } } #endif @@ -128,7 +136,8 @@ void estimationCalculateAGL(estimationContext_t * ctx) posEstimator.est.aglQual = newAglQuality; if (resetSurfaceEstimate) { - posEstimator.est.aglAlt = posEstimator.surface.alt; + posEstimator.est.aglAlt = pt1FilterGetLastOutput(&posEstimator.surface.avgFilter); + // If we have acceptable average estimate if (posEstimator.est.epv < positionEstimationConfig()->max_eph_epv) { posEstimator.est.aglVel = posEstimator.est.vel.z; posEstimator.est.aglOffset = posEstimator.est.pos.z - posEstimator.surface.alt; @@ -140,9 +149,9 @@ void estimationCalculateAGL(estimationContext_t * ctx) } // Update estimate - posEstimator.est.aglAlt = posEstimator.est.aglAlt + posEstimator.est.aglVel * ctx->dt; - posEstimator.est.aglAlt = posEstimator.imu.accelNEU.z * sq(ctx->dt) * 0.5f; - posEstimator.est.aglVel = posEstimator.est.aglVel + posEstimator.imu.accelNEU.z * ctx->dt; + posEstimator.est.aglAlt += posEstimator.est.aglVel * ctx->dt; + posEstimator.est.aglAlt += posEstimator.imu.accelNEU.z * sq(ctx->dt) / 2.0f; + posEstimator.est.aglVel += posEstimator.imu.accelNEU.z * ctx->dt; // Apply correction if (posEstimator.est.aglQual == SURFACE_QUAL_HIGH) { @@ -155,7 +164,7 @@ void estimationCalculateAGL(estimationContext_t * ctx) // Update estimate offset if ((posEstimator.est.aglQual == SURFACE_QUAL_HIGH) && (posEstimator.est.epv < positionEstimationConfig()->max_eph_epv)) { - posEstimator.est.aglOffset = posEstimator.est.pos.z - posEstimator.surface.alt; + posEstimator.est.aglOffset = posEstimator.est.pos.z - pt1FilterGetLastOutput(&posEstimator.surface.avgFilter); } } else if (posEstimator.est.aglQual == SURFACE_QUAL_MID) { diff --git a/src/main/navigation/navigation_pos_estimator_private.h b/src/main/navigation/navigation_pos_estimator_private.h index 6b2b6c5d687..46c15509789 100644 --- a/src/main/navigation/navigation_pos_estimator_private.h +++ b/src/main/navigation/navigation_pos_estimator_private.h @@ -22,6 +22,8 @@ #include "common/axis.h" #include "common/maths.h" +#include "common/filter.h" +#include "common/calibration.h" #include "sensors/sensors.h" @@ -40,9 +42,15 @@ #define INAV_GPS_TIMEOUT_MS 1500 // GPS timeout #define INAV_BARO_TIMEOUT_MS 200 // Baro timeout -#define INAV_SURFACE_TIMEOUT_MS 300 // Surface timeout (missed 3 readings in a row) +#define INAV_SURFACE_TIMEOUT_MS 400 // Surface timeout (missed 3 readings in a row) #define INAV_FLOW_TIMEOUT_MS 200 +#define CALIBRATING_GRAVITY_TIME_MS 2000 + +// Time constants for calculating Baro/Sonar averages. Should be the same value to impose same amount of group delay +#define INAV_BARO_AVERAGE_HZ 1.0f +#define INAV_SURFACE_AVERAGE_HZ 1.0f + #define RANGEFINDER_RELIABILITY_RC_CONSTANT (0.47802f) #define RANGEFINDER_RELIABILITY_LIGHT_THRESHOLD (0.15f) #define RANGEFINDER_RELIABILITY_LOW_THRESHOLD (0.33f) @@ -67,6 +75,7 @@ typedef struct { typedef struct { timeUs_t lastUpdateTime; // Last update time (us) + pt1Filter_t avgFilter; float alt; // Raw barometric altitude (cm) float epv; } navPositionEstimatorBARO_t; @@ -84,6 +93,7 @@ typedef enum { typedef struct { timeUs_t lastUpdateTime; // Last update time (us) + pt1Filter_t avgFilter; float alt; // Raw altitude measurement (cm) float reliability; } navPositionEstimatorSURFACE_t; @@ -116,9 +126,10 @@ typedef struct { } navPositionEstimatorESTIMATE_t; typedef struct { - fpVector3_t accelNEU; - fpVector3_t accelBias; - bool gravityCalibrationComplete; + fpVector3_t accelNEU; + fpVector3_t accelBias; + float calibratedGravityCMSS; + zeroCalibrationScalar_t gravityCalibration; } navPosisitonEstimatorIMU_t; typedef enum { diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h index e577a6adcf3..29dc704ca3b 100755 --- a/src/main/navigation/navigation_private.h +++ b/src/main/navigation/navigation_private.h @@ -318,7 +318,7 @@ typedef struct { uint32_t lastValidAltitudeTimeMs; /* INAV GPS origin (position where GPS fix was first acquired) */ - gpsOrigin_s gpsOrigin; + gpsOrigin_t gpsOrigin; /* Home parameters (NEU coordinated), geodetic position of home (LLH) is stores in GPS_home variable */ rthSanityChecker_t rthSanityChecker; @@ -389,10 +389,11 @@ void setupMulticopterAltitudeController(void); void resetMulticopterAltitudeController(void); void resetMulticopterPositionController(void); void resetMulticopterHeadingController(void); +void resetMulticopterBrakingMode(void); bool adjustMulticopterAltitudeFromRCInput(void); bool adjustMulticopterHeadingFromRCInput(void); -bool adjustMulticopterPositionFromRCInput(void); +bool adjustMulticopterPositionFromRCInput(int16_t rcPitchAdjustment, int16_t rcRollAdjustment); void applyMulticopterNavigationController(navigationFSMStateFlags_t navStateFlags, timeUs_t currentTimeUs); @@ -424,6 +425,7 @@ void resetFixedWingLaunchController(timeUs_t currentTimeUs); bool isFixedWingLaunchDetected(void); void enableFixedWingLaunchController(timeUs_t currentTimeUs); bool isFixedWingLaunchFinishedOrAborted(void); +void abortFixedWingLaunch(void); void applyFixedWingLaunchController(timeUs_t currentTimeUs); #endif diff --git a/src/main/platform.h b/src/main/platform.h index b2eb9a21e7f..67365d6ba69 100644 --- a/src/main/platform.h +++ b/src/main/platform.h @@ -20,6 +20,12 @@ #if defined(STM32F7) #include "stm32f7xx.h" #include "stm32f7xx_hal.h" +#include "stm32f7xx_ll_spi.h" +#include "stm32f7xx_ll_gpio.h" +#include "stm32f7xx_ll_dma.h" +#include "stm32f7xx_ll_rcc.h" +#include "stm32f7xx_ll_bus.h" +#include "stm32f7xx_ll_tim.h" // Chip Unique ID on F7 #if defined(STM32F722xx) diff --git a/src/main/rx/crsf.c b/src/main/rx/crsf.c index 04c0a83b0b8..97762ded586 100755 --- a/src/main/rx/crsf.c +++ b/src/main/rx/crsf.c @@ -40,11 +40,13 @@ #include "rx/rx.h" #include "rx/crsf.h" -#define CRSF_TIME_NEEDED_PER_FRAME_US 1000 -#define CRSF_TIME_BETWEEN_FRAMES_US 4000 // a frame is sent by the transmitter every 4 milliseconds +#include "telemetry/crsf.h" +#define CRSF_TIME_NEEDED_PER_FRAME_US 1100 // 700 ms + 400 ms for potential ad-hoc request +#define CRSF_TIME_BETWEEN_FRAMES_US 6667 // At fastest, frames are sent by the transmitter every 6.667 milliseconds, 150 Hz #define CRSF_DIGITAL_CHANNEL_MIN 172 #define CRSF_DIGITAL_CHANNEL_MAX 1811 +#define CRSF_PAYLOAD_OFFSET offsetof(crsfFrameDef_t, type) STATIC_UNIT_TESTED bool crsfFrameDone = false; STATIC_UNIT_TESTED crsfFrame_t crsfFrame; @@ -69,10 +71,10 @@ static uint8_t telemetryBufLen = 0; * 1 Stop bit * Big endian * 420000 bit/s = 46667 byte/s (including stop bit) = 21.43us per byte - * Assume a max payload of 32 bytes (needs confirming with TBS), so max frame size of 36 bytes - * A 36 byte frame can be transmitted in 771 microseconds. + * Max frame size is 64 bytes + * A 64 byte frame plus 1 sync byte can be transmitted in 1393 microseconds. * - * CRSF_TIME_NEEDED_PER_FRAME_US is set conservatively at 1000 microseconds + * CRSF_TIME_NEEDED_PER_FRAME_US is set conservatively at 1500 microseconds * * Every frame has the structure: * < Type> < CRC> @@ -121,6 +123,16 @@ struct crsfPayloadLinkStatistics_s { typedef struct crsfPayloadLinkStatistics_s crsfPayloadLinkStatistics_t; +STATIC_UNIT_TESTED uint8_t crsfFrameCRC(void) +{ + // CRC includes type and payload + uint8_t crc = crc8_dvb_s2(0, crsfFrame.frame.type); + for (int ii = 0; ii < crsfFrame.frame.frameLength - CRSF_FRAME_LENGTH_TYPE_CRC; ++ii) { + crc = crc8_dvb_s2(crc, crsfFrame.frame.payload[ii]); + } + return crc; +} + // Receive ISR callback, called back from serial port STATIC_UNIT_TESTED void crsfDataReceive(uint16_t c, void *rxCallbackData) { @@ -149,19 +161,32 @@ STATIC_UNIT_TESTED void crsfDataReceive(uint16_t c, void *rxCallbackData) if (crsfFramePosition < fullFrameLength) { crsfFrame.bytes[crsfFramePosition++] = (uint8_t)c; crsfFrameDone = crsfFramePosition < fullFrameLength ? false : true; + if (crsfFrameDone) { + crsfFramePosition = 0; + if (crsfFrame.frame.type != CRSF_FRAMETYPE_RC_CHANNELS_PACKED) { + const uint8_t crc = crsfFrameCRC(); + if (crc == crsfFrame.bytes[fullFrameLength - 1]) { + switch (crsfFrame.frame.type) + { +#if defined(USE_MSP_OVER_TELEMETRY) + case CRSF_FRAMETYPE_MSP_REQ: + case CRSF_FRAMETYPE_MSP_WRITE: { + uint8_t *frameStart = (uint8_t *)&crsfFrame.frame.payload + CRSF_FRAME_ORIGIN_DEST_SIZE; + if (bufferCrsfMspFrame(frameStart, CRSF_FRAME_RX_MSP_FRAME_SIZE)) { + crsfScheduleMspResponse(); + } + break; + } +#endif + default: + break; + } + } + } + } } } -STATIC_UNIT_TESTED uint8_t crsfFrameCRC(void) -{ - // CRC includes type and payload - uint8_t crc = crc8_dvb_s2(0, crsfFrame.frame.type); - for (int ii = 0; ii < crsfFrame.frame.frameLength - CRSF_FRAME_LENGTH_TYPE_CRC; ++ii) { - crc = crc8_dvb_s2(crc, crsfFrame.frame.payload[ii]); - } - return crc; -} - STATIC_UNIT_TESTED uint8_t crsfFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig) { UNUSED(rxRuntimeConfig); @@ -261,7 +286,7 @@ bool crsfRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) } rxRuntimeConfig->channelCount = CRSF_MAX_CHANNEL; - rxRuntimeConfig->rxRefreshRate = 11000; //!!TODO this needs checking + rxRuntimeConfig->rxRefreshRate = CRSF_TIME_BETWEEN_FRAMES_US; //!!TODO this needs checking rxRuntimeConfig->rcReadRawFn = crsfReadRawRC; rxRuntimeConfig->rcFrameStatusFn = crsfFrameStatus; diff --git a/src/main/rx/crsf.h b/src/main/rx/crsf.h index 5ac37bbe7f3..a0e0cf6389a 100755 --- a/src/main/rx/crsf.h +++ b/src/main/rx/crsf.h @@ -23,14 +23,23 @@ #define CRSF_MAX_CHANNEL 17 -typedef enum { - CRSF_FRAMETYPE_GPS = 0x02, - CRSF_FRAMETYPE_BATTERY_SENSOR = 0x08, - CRSF_FRAMETYPE_LINK_STATISTICS = 0x14, - CRSF_FRAMETYPE_RC_CHANNELS_PACKED = 0x16, - CRSF_FRAMETYPE_ATTITUDE = 0x1E, - CRSF_FRAMETYPE_FLIGHT_MODE = 0x21 -} crsfFrameTypes_e; +enum { CRSF_SYNC_BYTE = 0xC8 }; + +enum { CRSF_FRAME_SIZE_MAX = 64 }; // 62 bytes frame plus 2 bytes frame header() +enum { CRSF_PAYLOAD_SIZE_MAX = CRSF_FRAME_SIZE_MAX - 6 }; + +enum { + CRSF_DISPLAYPORT_SUBCMD_UPDATE = 0x01, // transmit displayport buffer to remote + CRSF_DISPLAYPORT_SUBCMD_CLEAR = 0X02, // clear client screen + CRSF_DISPLAYPORT_SUBCMD_OPEN = 0x03, // client request to open cms menu + CRSF_DISPLAYPORT_SUBCMD_CLOSE = 0x04, // client request to close cms menu + CRSF_DISPLAYPORT_SUBCMD_POLL = 0x05, // client request to poll/refresh cms menu +}; + +enum { + CRSF_DISPLAYPORT_OPEN_ROWS_OFFSET = 1, + CRSF_DISPLAYPORT_OPEN_COLS_OFFSET = 2, +}; enum { CRSF_FRAME_GPS_PAYLOAD_SIZE = 15, @@ -42,25 +51,56 @@ enum { CRSF_FRAME_LENGTH_FRAMELENGTH = 1, // length of FRAMELENGTH field CRSF_FRAME_LENGTH_TYPE = 1, // length of TYPE field CRSF_FRAME_LENGTH_CRC = 1, // length of CRC field - CRSF_FRAME_LENGTH_TYPE_CRC = 2 // length of TYPE and CRC fields combined + CRSF_FRAME_LENGTH_TYPE_CRC = 2, // length of TYPE and CRC fields combined + CRSF_FRAME_LENGTH_EXT_TYPE_CRC = 4, // length of Extended Dest/Origin, TYPE and CRC fields combined + CRSF_FRAME_LENGTH_NON_PAYLOAD = 4, // combined length of all fields except payload }; enum { + CRSF_FRAME_TX_MSP_FRAME_SIZE = 58, + CRSF_FRAME_RX_MSP_FRAME_SIZE = 8, + CRSF_FRAME_ORIGIN_DEST_SIZE = 2, +}; + +// Clashes with CRSF_ADDRESS_FLIGHT_CONTROLLER +#define CRSF_TELEMETRY_SYNC_BYTE 0XC8 + +typedef enum { CRSF_ADDRESS_BROADCAST = 0x00, - CRSF_ADDRESS_TBS_CORE_PNP_PRO = 0x8, + CRSF_ADDRESS_USB = 0x10, + CRSF_ADDRESS_TBS_CORE_PNP_PRO = 0x80, CRSF_ADDRESS_RESERVED1 = 0x8A, CRSF_ADDRESS_CURRENT_SENSOR = 0xC0, + CRSF_ADDRESS_GPS = 0xC2, CRSF_ADDRESS_TBS_BLACKBOX = 0xC4, - CRSF_ADDRESS_COLIBRI_RACE_FC = 0xC8, + CRSF_ADDRESS_FLIGHT_CONTROLLER = 0xC8, CRSF_ADDRESS_RESERVED2 = 0xCA, CRSF_ADDRESS_RACE_TAG = 0xCC, CRSF_ADDRESS_RADIO_TRANSMITTER = 0xEA, CRSF_ADDRESS_CRSF_RECEIVER = 0xEC, CRSF_ADDRESS_CRSF_TRANSMITTER = 0xEE -}; +} crsfAddress_e; -#define CRSF_PAYLOAD_SIZE_MAX 62 -#define CRSF_FRAME_SIZE_MAX (CRSF_PAYLOAD_SIZE_MAX + 4) +typedef enum { + CRSF_FRAMETYPE_GPS = 0x02, + CRSF_FRAMETYPE_BATTERY_SENSOR = 0x08, + CRSF_FRAMETYPE_LINK_STATISTICS = 0x14, + CRSF_FRAMETYPE_RC_CHANNELS_PACKED = 0x16, + CRSF_FRAMETYPE_ATTITUDE = 0x1E, + CRSF_FRAMETYPE_FLIGHT_MODE = 0x21, + // Extended Header Frames, range: 0x28 to 0x96 + CRSF_FRAMETYPE_DEVICE_PING = 0x28, + CRSF_FRAMETYPE_DEVICE_INFO = 0x29, + CRSF_FRAMETYPE_PARAMETER_SETTINGS_ENTRY = 0x2B, + CRSF_FRAMETYPE_PARAMETER_READ = 0x2C, + CRSF_FRAMETYPE_PARAMETER_WRITE = 0x2D, + CRSF_FRAMETYPE_COMMAND = 0x32, + // MSP commands + CRSF_FRAMETYPE_MSP_REQ = 0x7A, // response request using msp sequence as command + CRSF_FRAMETYPE_MSP_RESP = 0x7B, // reply with 58 byte chunked binary + CRSF_FRAMETYPE_MSP_WRITE = 0x7C, // write with 8 byte chunked binary (OpenTX outbound telemetry buffer limit) + CRSF_FRAMETYPE_DISPLAYPORT_CMD = 0x7D, // displayport control command +} crsfFrameType_e; typedef struct crsfFrameDef_s { uint8_t deviceAddress; @@ -81,4 +121,4 @@ void crsfRxSendTelemetryData(void); struct rxConfig_s; struct rxRuntimeConfig_s; bool crsfRxInit(const struct rxConfig_s *initialRxConfig, struct rxRuntimeConfig_s *rxRuntimeConfig); -bool crsfRxIsActive(void); \ No newline at end of file +bool crsfRxIsActive(void); diff --git a/src/main/rx/eleres.c b/src/main/rx/eleres.c index bf17950f362..ba693ddb5d6 100755 --- a/src/main/rx/eleres.c +++ b/src/main/rx/eleres.c @@ -40,6 +40,7 @@ #include "sensors/battery.h" #include "sensors/sensors.h" #include "sensors/barometer.h" +#include "sensors/temperature.h" #include "scheduler/scheduler.h" #ifdef USE_RX_ELERES @@ -271,8 +272,12 @@ static void telemetryRX(void) presfil -= presfil/4; presfil += baro.baroPressure; + + int16_t temperature; + const bool temp_valid = sensors(SENSOR_BARO) ? getBaroTemperature(&temperature) : getIMUTemperature(&temperature); + if (!temp_valid) temperature = TEMPERATURE_INVALID_VALUE; // If temperature not valid report value outside of range thempfil -= thempfil/8; - thempfil += baro.baroTemperature/10; + thempfil += temperature; switch (telem_state++) { case 0: diff --git a/src/main/rx/nrf24_h8_3d.c b/src/main/rx/nrf24_h8_3d.c index d9463b2cf54..25a7074c687 100644 --- a/src/main/rx/nrf24_h8_3d.c +++ b/src/main/rx/nrf24_h8_3d.c @@ -87,7 +87,7 @@ STATIC_UNIT_TESTED uint8_t payloadSize; STATIC_UNIT_TESTED uint8_t rxTxAddrXN297[RX_TX_ADDR_LEN] = {0x41, 0xbd, 0x42, 0xd4, 0xc2}; // converted XN297 address #define TX_ID_LEN 4 STATIC_UNIT_TESTED uint8_t txId[TX_ID_LEN]; -uint32_t *rxSpiIdPtr; +static uint32_t *rxSpiIdPtr; // radio channels for frequency hopping #define H8_3D_RF_CHANNEL_COUNT 4 diff --git a/src/main/rx/nrf24_inav.c b/src/main/rx/nrf24_inav.c index 2a0b09db4cb..ee28e30564c 100644 --- a/src/main/rx/nrf24_inav.c +++ b/src/main/rx/nrf24_inav.c @@ -135,7 +135,7 @@ uint8_t receivedPowerSnapshot; #define RX_TX_ADDR_LEN 5 // set rxTxAddr to the bind address STATIC_UNIT_TESTED uint8_t rxTxAddr[RX_TX_ADDR_LEN] = {0x4b,0x5c,0x6d,0x7e,0x8f}; -uint32_t *rxSpiIdPtr; +static uint32_t *rxSpiIdPtr; #define RX_TX_ADDR_4 0xD2 // rxTxAddr[4] always set to this value // radio channels for frequency hopping diff --git a/src/main/rx/pwm.c b/src/main/rx/pwm.c index 8969bda057b..252b55d7bb1 100644 --- a/src/main/rx/pwm.c +++ b/src/main/rx/pwm.c @@ -25,6 +25,7 @@ #if defined(USE_RX_PWM) || defined(USE_RX_PPM) +#include "build/debug.h" #include "common/utils.h" #include "config/feature.h" diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index feefc037be8..cc3f1fa3874 100755 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -100,7 +100,7 @@ timeMs_t rcInvalidPulsPeriod[MAX_SUPPORTED_RC_CHANNEL_COUNT]; rxRuntimeConfig_t rxRuntimeConfig; static uint8_t rcSampleIndex = 0; -PG_REGISTER_WITH_RESET_TEMPLATE(rxConfig_t, rxConfig, PG_RX_CONFIG, 6); +PG_REGISTER_WITH_RESET_TEMPLATE(rxConfig_t, rxConfig, PG_RX_CONFIG, 7); #ifndef RX_SPI_DEFAULT_PROTOCOL #define RX_SPI_DEFAULT_PROTOCOL 0 @@ -130,7 +130,7 @@ PG_RESET_TEMPLATE(rxConfig_t, rxConfig, .rssiMin = RSSI_VISIBLE_VALUE_MIN, .rssiMax = RSSI_VISIBLE_VALUE_MAX, .sbusSyncInterval = SBUS_DEFAULT_INTERFRAME_DELAY_US, - .rcSmoothing = 1, + .rcFilterFrequency = 50, ); void resetAllRxChannelRangeConfigurations(void) diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h index e55eca2adb9..7ebb8fa412c 100644 --- a/src/main/rx/rx.h +++ b/src/main/rx/rx.h @@ -129,7 +129,7 @@ typedef struct rxConfig_s { uint16_t maxcheck; // maximum rc end uint16_t rx_min_usec; uint16_t rx_max_usec; - uint8_t rcSmoothing; // Enable/Disable RC filtering + uint8_t rcFilterFrequency; // RC filter cutoff frequency (smoothness vs response sharpness) } rxConfig_t; PG_DECLARE(rxConfig_t, rxConfig); diff --git a/src/main/scheduler/protothreads.h b/src/main/scheduler/protothreads.h index d0904a2eb99..66953d4612f 100644 --- a/src/main/scheduler/protothreads.h +++ b/src/main/scheduler/protothreads.h @@ -39,6 +39,8 @@ #pragma once #include "common/time.h" +#include "common/utils.h" +#include "drivers/time.h" /* Protothreads are a extremely lightweight, stackless threads that provides a blocking context, without the overhead of per-thread stacks. @@ -156,6 +158,18 @@ struct ptState_s { } \ } while (0) +// Suspends protothread for a given amount of time +// Delay is evaluated only once +#define ptDelayUs(delay) \ + do { \ + (currentPt)->startTime = (timeUs_t)micros(); \ + (currentPt)->delayTime = (delay); \ + ptLabel(); \ + if ((timeDelta_t)(micros() - (currentPt)->startTime) <= (currentPt)->delayTime) { \ + return; \ + } \ + } while (0) + // Suspends protothread until it's called again #define ptYield() \ do { \ @@ -188,4 +202,4 @@ typedef bool ptSemaphore_t; #define ptSemaphoreInit(sem) do { sem = false; } while (0) #define ptSemaphoreWait(sem) do { ptWait(sem); sem = false; } while (0) -#define ptSemaphoreSignal(sem) do { sem = true; } while (0) \ No newline at end of file +#define ptSemaphoreSignal(sem) do { sem = true; } while (0) diff --git a/src/main/scheduler/scheduler.c b/src/main/scheduler/scheduler.c index d9c7d10558b..cdf9ffcd488 100755 --- a/src/main/scheduler/scheduler.c +++ b/src/main/scheduler/scheduler.c @@ -306,7 +306,20 @@ void scheduler(void) #endif #if defined(SCHEDULER_DEBUG) DEBUG_SET(DEBUG_SCHEDULER, 2, micros() - currentTimeUs - taskExecutionTime); // time spent in scheduler +#endif } else { + // Execute system real-time callbacks and account for them to SYSTEM account + const timeUs_t currentTimeBeforeTaskCall = micros(); + taskRunRealtimeCallbacks(currentTimeBeforeTaskCall); + +#ifndef SKIP_TASK_STATISTICS + selectedTask = &cfTasks[TASK_SYSTEM]; + const timeUs_t taskExecutionTime = micros() - currentTimeBeforeTaskCall; + selectedTask->movingSumExecutionTime += taskExecutionTime - selectedTask->movingSumExecutionTime / TASK_MOVING_SUM_COUNT; + selectedTask->totalExecutionTime += taskExecutionTime; // time consumed by scheduler + task + selectedTask->maxExecutionTime = MAX(selectedTask->maxExecutionTime, taskExecutionTime); +#endif +#if defined(SCHEDULER_DEBUG) DEBUG_SET(DEBUG_SCHEDULER, 2, micros() - currentTimeUs); #endif } diff --git a/src/main/scheduler/scheduler.h b/src/main/scheduler/scheduler.h index 13a95d69ae3..4ca38bc08dd 100755 --- a/src/main/scheduler/scheduler.h +++ b/src/main/scheduler/scheduler.h @@ -51,14 +51,7 @@ typedef struct { typedef enum { /* Actual tasks */ TASK_SYSTEM = 0, -#ifdef USE_ASYNC_GYRO_PROCESSING - TASK_PID, - TASK_GYRO, - TASK_ACC, - TASK_ATTI, -#else TASK_GYROPID, -#endif TASK_RX, TASK_SERIAL, TASK_BATTERY, @@ -162,6 +155,7 @@ void schedulerResetTaskStatistics(cfTaskId_e taskId); void schedulerInit(void); void scheduler(void); void taskSystem(timeUs_t currentTimeUs); +void taskRunRealtimeCallbacks(timeUs_t currentTimeUs); #define TASK_PERIOD_HZ(hz) (1000000 / (hz)) #define TASK_PERIOD_MS(ms) ((ms) * 1000) diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c index 495ebb62ef7..1b776d362d9 100644 --- a/src/main/sensors/acceleration.c +++ b/src/main/sensors/acceleration.c @@ -27,6 +27,7 @@ #include "common/filter.h" #include "common/maths.h" #include "common/utils.h" +#include "common/calibration.h" #include "config/config_reset.h" #include "config/parameter_group.h" @@ -47,6 +48,7 @@ #include "drivers/accgyro/accgyro_mma845x.h" #include "drivers/accgyro/accgyro_bma280.h" #include "drivers/accgyro/accgyro_bmi160.h" +#include "drivers/accgyro/accgyro_icm20689.h" #include "drivers/accgyro/accgyro_fake.h" #include "drivers/logging.h" #include "drivers/sensor.h" @@ -69,7 +71,7 @@ FASTRAM acc_t acc; // acc access functions -static uint16_t calibratingA = 0; // the calibration is done is the main loop. Calibrating decreases at each cycle down to 0, then we enter in a normal mode. +STATIC_FASTRAM zeroCalibrationVector_t zeroCalibration; STATIC_FASTRAM int32_t accADC[XYZ_AXIS_COUNT]; @@ -266,6 +268,23 @@ static bool accDetect(accDev_t *dev, accelerationSensor_e accHardwareToUse) FALLTHROUGH; #endif +#ifdef USE_ACC_ICM20689 + case ACC_ICM20689: + if (icm20689AccDetect(dev)) { +#ifdef ACC_ICM20689_ALIGN + dev->accAlign = ACC_ICM20689_ALIGN; +#endif + accHardware = ACC_ICM20689; + break; + } + /* If we are asked for a specific sensor - break out, otherwise - fall through and continue */ + if (accHardwareToUse != ACC_AUTODETECT) { + break; + } + FALLTHROUGH; +#endif + + #ifdef USE_FAKE_ACC case ACC_FAKE: if (fakeAccDetect(dev)) { @@ -323,26 +342,6 @@ bool accInit(uint32_t targetLooptime) return true; } -void accSetCalibrationCycles(uint16_t calibrationCyclesRequired) -{ - calibratingA = calibrationCyclesRequired; -} - -bool accIsCalibrationComplete(void) -{ - return calibratingA == 0; -} - -static bool isOnFinalAccelerationCalibrationCycle(void) -{ - return calibratingA == 1; -} - -static bool isOnFirstAccelerationCalibrationCycle(void) -{ - return calibratingA == CALIBRATING_ACC_CYCLES; -} - static bool calibratedPosition[6]; static int32_t accSamples[6][3]; static int calibratedAxisCount = 0; @@ -388,17 +387,21 @@ static int getPrimaryAxisIndex(int32_t accADCData[3]) return -1; } -static void performAcclerationCalibration(void) +bool accIsCalibrationComplete(void) +{ + return zeroCalibrationIsCompleteV(&zeroCalibration); +} + +void accStartCalibration(void) { int positionIndex = getPrimaryAxisIndex(accADC); - // Check if sample is usable if (positionIndex < 0) { return; } - // Top-up and first calibration cycle, reset everything - if (positionIndex == 0 && isOnFirstAccelerationCalibrationCycle()) { + // Top+up and first calibration cycle, reset everything + if (positionIndex == 0) { for (int axis = 0; axis < 6; axis++) { calibratedPosition[axis] = false; accSamples[axis][X] = 0; @@ -410,19 +413,41 @@ static void performAcclerationCalibration(void) DISABLE_STATE(ACCELEROMETER_CALIBRATED); } + // Tolerate 5% variance in accelerometer readings + zeroCalibrationStartV(&zeroCalibration, CALIBRATING_ACC_TIME_MS, acc.dev.acc_1G * 0.05f, true); +} + +static void performAcclerationCalibration(void) +{ + fpVector3_t v; + int positionIndex = getPrimaryAxisIndex(accADC); + + // Check if sample is usable + if (positionIndex < 0) { + return; + } + if (!calibratedPosition[positionIndex]) { - accSamples[positionIndex][X] += accADC[X]; - accSamples[positionIndex][Y] += accADC[Y]; - accSamples[positionIndex][Z] += accADC[Z]; + v.v[0] = accADC[0]; + v.v[1] = accADC[1]; + v.v[2] = accADC[2]; - if (isOnFinalAccelerationCalibrationCycle()) { - calibratedPosition[positionIndex] = true; + zeroCalibrationAddValueV(&zeroCalibration, &v); - accSamples[positionIndex][X] = accSamples[positionIndex][X] / CALIBRATING_ACC_CYCLES; - accSamples[positionIndex][Y] = accSamples[positionIndex][Y] / CALIBRATING_ACC_CYCLES; - accSamples[positionIndex][Z] = accSamples[positionIndex][Z] / CALIBRATING_ACC_CYCLES; + if (zeroCalibrationIsCompleteV(&zeroCalibration)) { + if (zeroCalibrationIsSuccessfulV(&zeroCalibration)) { + zeroCalibrationGetZeroV(&zeroCalibration, &v); - calibratedAxisCount++; + accSamples[positionIndex][X] = v.v[X]; + accSamples[positionIndex][Y] = v.v[Y]; + accSamples[positionIndex][Z] = v.v[Z]; + + calibratedPosition[positionIndex] = true; + calibratedAxisCount++; + } + else { + calibratedPosition[positionIndex] = false; + } beeperConfirmationBeeps(2); } @@ -441,9 +466,9 @@ static void performAcclerationCalibration(void) sensorCalibrationSolveForOffset(&calState, accTmp); - for (int axis = 0; axis < 3; axis++) { - accelerometerConfigMutable()->accZero.raw[axis] = lrintf(accTmp[axis]); - } + accelerometerConfigMutable()->accZero.raw[X] = lrintf(accTmp[X]); + accelerometerConfigMutable()->accZero.raw[Y] = lrintf(accTmp[Y]); + accelerometerConfigMutable()->accZero.raw[Z] = lrintf(accTmp[Z]); /* Not we can offset our accumulated averages samples and calculate scale factors and calculate gains */ sensorCalibrationResetState(&calState); @@ -466,8 +491,6 @@ static void performAcclerationCalibration(void) saveConfigAndNotify(); } - - calibratingA--; } static void applyAccelerationZero(const flightDynamicsTrims_t * accZero, const flightDynamicsTrims_t * accGain) @@ -477,42 +500,14 @@ static void applyAccelerationZero(const flightDynamicsTrims_t * accZero, const f accADC[Z] = (accADC[Z] - accZero->raw[Z]) * accGain->raw[Z] / 4096; } -#ifdef USE_ASYNC_GYRO_PROCESSING -STATIC_FASTRAM float accumulatedMeasurements[XYZ_AXIS_COUNT]; -STATIC_FASTRAM int accumulatedMeasurementCount; - -static void accUpdateAccumulatedMeasurements(void) -{ - for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { - accumulatedMeasurements[axis] += acc.accADCf[axis]; - } - accumulatedMeasurementCount++; -} -#endif - /* * Calculate measured acceleration in body frame in g */ void accGetMeasuredAcceleration(fpVector3_t *measuredAcc) { -#ifdef USE_ASYNC_GYRO_PROCESSING - if (accumulatedMeasurementCount) { - for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { - measuredAcc->v[axis] = accumulatedMeasurements[axis] * GRAVITY_CMSS / accumulatedMeasurementCount; - accumulatedMeasurements[axis] = 0; - } - accumulatedMeasurementCount = 0; - } - else { - for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { - measuredAcc->v[axis] = acc.accADCf[axis] * GRAVITY_CMSS; - } - } -#else for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { measuredAcc->v[axis] = acc.accADCf[axis] * GRAVITY_CMSS; } -#endif } void accUpdate(void) @@ -523,6 +518,7 @@ void accUpdate(void) for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { accADC[axis] = acc.dev.ADCRaw[axis]; + DEBUG_SET(DEBUG_ACC, axis, accADC[axis]); } if (!accIsCalibrationComplete()) { @@ -541,7 +537,7 @@ void accUpdate(void) } // Before filtering check for clipping and vibration levels - if (ABS(acc.accADCf[X]) > ACC_CLIPPING_THRESHOLD_G || ABS(acc.accADCf[Y]) > ACC_CLIPPING_THRESHOLD_G || ABS(acc.accADCf[Z]) > ACC_CLIPPING_THRESHOLD_G) { + if (fabsf(acc.accADCf[X]) > ACC_CLIPPING_THRESHOLD_G || fabsf(acc.accADCf[Y]) > ACC_CLIPPING_THRESHOLD_G || fabsf(acc.accADCf[Z]) > ACC_CLIPPING_THRESHOLD_G) { acc.accClipCount++; } @@ -570,9 +566,6 @@ void accUpdate(void) } #endif -#ifdef USE_ASYNC_GYRO_PROCESSING - accUpdateAccumulatedMeasurements(); -#endif } void accGetVibrationLevels(fpVector3_t *accVibeLevels) diff --git a/src/main/sensors/acceleration.h b/src/main/sensors/acceleration.h index e02e654ba6c..3dc5941f416 100644 --- a/src/main/sensors/acceleration.h +++ b/src/main/sensors/acceleration.h @@ -44,7 +44,8 @@ typedef enum { ACC_MPU6500 = 8, ACC_MPU9250 = 9, ACC_BMI160 = 10, - ACC_FAKE = 11, + ACC_ICM20689 = 11, + ACC_FAKE = 12, ACC_MAX = ACC_FAKE } accelerationSensor_e; @@ -72,7 +73,7 @@ PG_DECLARE(accelerometerConfig_t, accelerometerConfig); bool accInit(uint32_t accTargetLooptime); bool accIsCalibrationComplete(void); -void accSetCalibrationCycles(uint16_t calibrationCyclesRequired); +void accStartCalibration(void); void accGetMeasuredAcceleration(fpVector3_t *measuredAcc); void accGetVibrationLevels(fpVector3_t *accVibeLevels); float accGetVibrationLevel(void); diff --git a/src/main/sensors/barometer.c b/src/main/sensors/barometer.c index ec7bd0ab74d..b37d17113bf 100644 --- a/src/main/sensors/barometer.c +++ b/src/main/sensors/barometer.c @@ -26,6 +26,7 @@ #include "common/maths.h" #include "common/time.h" #include "common/utils.h" +#include "common/calibration.h" #include "config/parameter_group.h" #include "config/parameter_group_ids.h" @@ -52,7 +53,7 @@ baro_t baro; // barometer access functions -PG_REGISTER_WITH_RESET_TEMPLATE(barometerConfig_t, barometerConfig, PG_BAROMETER_CONFIG, 0); +PG_REGISTER_WITH_RESET_TEMPLATE(barometerConfig_t, barometerConfig, PG_BAROMETER_CONFIG, 1); #ifdef USE_BARO #define BARO_HARDWARE_DEFAULT BARO_AUTODETECT @@ -61,13 +62,13 @@ PG_REGISTER_WITH_RESET_TEMPLATE(barometerConfig_t, barometerConfig, PG_BAROMETER #endif PG_RESET_TEMPLATE(barometerConfig_t, barometerConfig, .baro_hardware = BARO_HARDWARE_DEFAULT, - .use_median_filtering = 1 + .use_median_filtering = 1, + .baro_calibration_tolerance = 150 ); #ifdef USE_BARO -static timeMs_t baroCalibrationTimeout = 0; -static bool baroCalibrationFinished = false; +static zeroCalibrationScalar_t zeroCalibration; static float baroGroundAltitude = 0; static float baroGroundPressure = 101325.0f; // 101325 pascal, 1 standard atmosphere @@ -183,17 +184,6 @@ bool baroInit(void) return true; } -bool baroIsCalibrationComplete(void) -{ - return baroCalibrationFinished; -} - -void baroStartCalibration(void) -{ - baroCalibrationTimeout = millis(); - baroCalibrationFinished = false; -} - #define PRESSURE_SAMPLES_MEDIAN 3 /* @@ -275,27 +265,33 @@ static float pressureToAltitude(const float pressure) return (1.0f - powf(pressure / 101325.0f, 0.190295f)) * 4433000.0f; } -static void performBaroCalibrationCycle(void) +static float altitudeToPressure(const float altCm) { - const float baroGroundPressureError = baro.baroPressure - baroGroundPressure; - baroGroundPressure += baroGroundPressureError * 0.15f; + return powf(1.0f - (altCm / 4433000.0f), 5.254999) * 101325.0f; +} - if (ABS(baroGroundPressureError) < (baroGroundPressure * 0.00005f)) { // 0.005% calibration error (should give c. 10cm calibration error) - if ((millis() - baroCalibrationTimeout) > 250) { - baroGroundAltitude = pressureToAltitude(baroGroundPressure); - baroCalibrationFinished = true; - DEBUG_TRACE_SYNC("Barometer calibration complete (%d)", lrintf(baroGroundAltitude)); - } - } - else { - baroCalibrationTimeout = millis(); - } +bool baroIsCalibrationComplete(void) +{ + return zeroCalibrationIsCompleteS(&zeroCalibration) && zeroCalibrationIsSuccessfulS(&zeroCalibration); +} + +void baroStartCalibration(void) +{ + const float acceptedPressureVariance = (101325.0f - altitudeToPressure(barometerConfig()->baro_calibration_tolerance)); // max 30cm deviation during calibration (at sea level) + zeroCalibrationStartS(&zeroCalibration, CALIBRATING_BARO_TIME_MS, acceptedPressureVariance, false); } int32_t baroCalculateAltitude(void) { if (!baroIsCalibrationComplete()) { - performBaroCalibrationCycle(); + zeroCalibrationAddValueS(&zeroCalibration, baro.baroPressure); + + if (zeroCalibrationIsCompleteS(&zeroCalibration)) { + zeroCalibrationGetZeroS(&zeroCalibration, &baroGroundPressure); + baroGroundAltitude = pressureToAltitude(baroGroundPressure); + DEBUG_TRACE_SYNC("Barometer calibration complete (%d)", lrintf(baroGroundAltitude)); + } + baro.BaroAlt = 0; } else { @@ -317,6 +313,11 @@ int32_t baroGetLatestAltitude(void) return baro.BaroAlt; } +int16_t baroGetTemperature(void) +{ + return CENTIDEGREES_TO_DECIDEGREES(baro.baroTemperature); +} + bool baroIsHealthy(void) { return true; diff --git a/src/main/sensors/barometer.h b/src/main/sensors/barometer.h index 30ad0c2a3dc..2ed17cef328 100644 --- a/src/main/sensors/barometer.h +++ b/src/main/sensors/barometer.h @@ -34,8 +34,9 @@ typedef enum { } baroSensor_e; typedef struct barometerConfig_s { - uint8_t baro_hardware; // Barometer hardware to use - uint8_t use_median_filtering; // Use 3-point median filtering + uint8_t baro_hardware; // Barometer hardware to use + uint8_t use_median_filtering; // Use 3-point median filtering + uint16_t baro_calibration_tolerance; // Baro calibration tolerance (cm at sea level) } barometerConfig_t; PG_DECLARE(barometerConfig_t, barometerConfig); @@ -55,4 +56,5 @@ void baroStartCalibration(void); uint32_t baroUpdate(void); int32_t baroCalculateAltitude(void); int32_t baroGetLatestAltitude(void); +int16_t baroGetTemperature(void); bool baroIsHealthy(void); diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index 13cf0591755..d9ef1cb4478 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -552,7 +552,7 @@ uint8_t calculateBatteryPercentage(void) uint32_t capacityDiffBetweenFullAndEmpty = currentBatteryProfile->capacity.value - currentBatteryProfile->capacity.critical; return constrain(batteryRemainingCapacity * 100 / capacityDiffBetweenFullAndEmpty, 0, 100); } else - return constrain((vbat - batteryCriticalVoltage) * 100L / (batteryFullVoltage - batteryCriticalVoltage), 0, 100); + return constrain((getBatteryVoltage() - batteryCriticalVoltage) * 100L / (batteryFullVoltage - batteryCriticalVoltage), 0, 100); } void batteryDisableProfileAutoswitch(void) { diff --git a/src/main/sensors/compass.c b/src/main/sensors/compass.c index d84431843f1..62b1540d243 100644 --- a/src/main/sensors/compass.c +++ b/src/main/sensors/compass.c @@ -95,8 +95,8 @@ bool compassDetect(magDev_t *dev, magSensor_e magHardwareToUse) case MAG_QMC5883: #ifdef USE_MAG_QMC5883 if (qmc5883Detect(dev)) { -#ifdef MAG_QMC5883L_ALIGN - dev->magAlign.onBoard = MAG_QMC5883L_ALIGN; +#ifdef MAG_QMC5883_ALIGN + dev->magAlign.onBoard = MAG_QMC5883_ALIGN; #endif magHardware = MAG_QMC5883; break; diff --git a/src/main/sensors/diagnostics.c b/src/main/sensors/diagnostics.c index fe48ec6028e..360d1d7230b 100644 --- a/src/main/sensors/diagnostics.c +++ b/src/main/sensors/diagnostics.c @@ -176,7 +176,7 @@ hardwareSensorStatus_e getHwGPSStatus(void) } } else { - if (feature(FEATURE_GPS) && gpsStats.timeouts > 3) { + if (feature(FEATURE_GPS) && gpsStats.timeouts > 4) { // Selected but not detected return HW_SENSOR_UNAVAILABLE; } diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index bafd943247a..6c26de7e04e 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -27,6 +27,7 @@ #include "common/axis.h" #include "common/maths.h" +#include "common/calibration.h" #include "common/filter.h" #include "common/utils.h" @@ -48,6 +49,7 @@ #include "drivers/accgyro/accgyro_mma845x.h" #include "drivers/accgyro/accgyro_bma280.h" #include "drivers/accgyro/accgyro_bmi160.h" +#include "drivers/accgyro/accgyro_icm20689.h" #include "drivers/accgyro/accgyro_fake.h" #include "drivers/io.h" #include "drivers/logging.h" @@ -73,13 +75,7 @@ FASTRAM gyro_t gyro; // gyro sensor object STATIC_UNIT_TESTED gyroDev_t gyroDev0; // Not in FASTRAM since it may hold DMA buffers STATIC_FASTRAM int16_t gyroTemperature0; -typedef struct gyroCalibration_s { - int32_t g[XYZ_AXIS_COUNT]; - stdev_t var[XYZ_AXIS_COUNT]; - uint16_t calibratingG; -} gyroCalibration_t; - -STATIC_FASTRAM_UNIT_TESTED gyroCalibration_t gyroCalibration; +STATIC_FASTRAM_UNIT_TESTED zeroCalibrationVector_t gyroCalibration; STATIC_FASTRAM int32_t gyroADC[XYZ_AXIS_COUNT]; STATIC_FASTRAM filterApplyFnPtr softLpfFilterApplyFn; @@ -100,7 +96,7 @@ STATIC_FASTRAM filterApplyFnPtr gyroFilterStage2ApplyFn; STATIC_FASTRAM void *stage2Filter[XYZ_AXIS_COUNT]; #endif -PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 3); +PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 4); PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig, .gyro_lpf = GYRO_LPF_42HZ, // 42HZ value is defined for Invensense/TDK gyros @@ -108,7 +104,7 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig, .gyro_align = ALIGN_DEFAULT, .gyroMovementCalibrationThreshold = 32, .looptime = 1000, - .gyroSync = 0, + .gyroSync = 1, .gyro_to_use = 0, .gyro_soft_notch_hz_1 = 0, .gyro_soft_notch_cutoff_1 = 1, @@ -221,6 +217,18 @@ STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHard FALLTHROUGH; #endif +#ifdef USE_GYRO_ICM20689 + case GYRO_ICM20689: + if (icm20689GyroDetect(dev)) { + gyroHardware = GYRO_ICM20689; +#ifdef GYRO_ICM20689_ALIGN + dev->gyroAlign = GYRO_ICM20689_ALIGN; +#endif + break; + } + FALLTHROUGH; +#endif + #ifdef USE_FAKE_GYRO case GYRO_FAKE: if (fakeGyroDetect(dev)) { @@ -297,7 +305,7 @@ void gyroInitFilters(void) gyroFilterStage2ApplyFn = (filterApplyFnPtr)biquadFilterApply; for (int axis = 0; axis < 3; axis++) { stage2Filter[axis] = &gyroFilterStage2[axis]; - biquadRCFIR2FilterInit(stage2Filter[axis], gyroConfig()->gyro_stage2_lowpass_hz, getGyroUpdateRate()); + biquadRCFIR2FilterInit(stage2Filter[axis], gyroConfig()->gyro_stage2_lowpass_hz, getLooptime()); } } #endif @@ -306,7 +314,7 @@ void gyroInitFilters(void) softLpfFilterApplyFn = (filterApplyFnPtr)biquadFilterApply; for (int axis = 0; axis < 3; axis++) { softLpfFilter[axis] = &gyroFilterLPF[axis]; - biquadFilterInitLPF(softLpfFilter[axis], gyroConfig()->gyro_soft_lpf_hz, getGyroUpdateRate()); + biquadFilterInitLPF(softLpfFilter[axis], gyroConfig()->gyro_soft_lpf_hz, getLooptime()); } } @@ -315,7 +323,7 @@ void gyroInitFilters(void) notchFilter1ApplyFn = (filterApplyFnPtr)biquadFilterApply; for (int axis = 0; axis < 3; axis++) { notchFilter1[axis] = &gyroFilterNotch_1[axis]; - biquadFilterInitNotch(notchFilter1[axis], getGyroUpdateRate(), gyroConfig()->gyro_soft_notch_hz_1, gyroConfig()->gyro_soft_notch_cutoff_1); + biquadFilterInitNotch(notchFilter1[axis], getLooptime(), gyroConfig()->gyro_soft_notch_hz_1, gyroConfig()->gyro_soft_notch_cutoff_1); } } #endif @@ -325,110 +333,65 @@ void gyroInitFilters(void) notchFilter2ApplyFn = (filterApplyFnPtr)biquadFilterApply; for (int axis = 0; axis < 3; axis++) { notchFilter2[axis] = &gyroFilterNotch_2[axis]; - biquadFilterInitNotch(notchFilter2[axis], getGyroUpdateRate(), gyroConfig()->gyro_soft_notch_hz_2, gyroConfig()->gyro_soft_notch_cutoff_2); + biquadFilterInitNotch(notchFilter2[axis], getLooptime(), gyroConfig()->gyro_soft_notch_hz_2, gyroConfig()->gyro_soft_notch_cutoff_2); } } #endif } -void gyroSetCalibrationCycles(uint16_t calibrationCyclesRequired) +void gyroStartCalibration(void) { - gyroCalibration.calibratingG = calibrationCyclesRequired; -} - -STATIC_UNIT_TESTED bool isCalibrationComplete(gyroCalibration_t *gyroCalibration) -{ - return gyroCalibration->calibratingG == 0; + zeroCalibrationStartV(&gyroCalibration, CALIBRATING_GYRO_TIME_MS, gyroConfig()->gyroMovementCalibrationThreshold, false); } bool gyroIsCalibrationComplete(void) { - return gyroCalibration.calibratingG == 0; + return zeroCalibrationIsCompleteV(&gyroCalibration) && zeroCalibrationIsSuccessfulV(&gyroCalibration); } -static bool isOnFinalGyroCalibrationCycle(gyroCalibration_t *gyroCalibration) +STATIC_UNIT_TESTED void performGyroCalibration(gyroDev_t *dev, zeroCalibrationVector_t *gyroCalibration) { - return gyroCalibration->calibratingG == 1; -} + fpVector3_t v; -static bool isOnFirstGyroCalibrationCycle(gyroCalibration_t *gyroCalibration) -{ - return gyroCalibration->calibratingG == CALIBRATING_GYRO_CYCLES; -} + // Consume gyro reading + v.v[0] = dev->gyroADCRaw[0]; + v.v[1] = dev->gyroADCRaw[1]; + v.v[2] = dev->gyroADCRaw[2]; -STATIC_UNIT_TESTED void performGyroCalibration(gyroDev_t *dev, gyroCalibration_t *gyroCalibration, uint8_t gyroMovementCalibrationThreshold) -{ - for (int axis = 0; axis < 3; axis++) { - // Reset g[axis] at start of calibration - if (isOnFirstGyroCalibrationCycle(gyroCalibration)) { - gyroCalibration->g[axis] = 0; - devClear(&gyroCalibration->var[axis]); - } + zeroCalibrationAddValueV(gyroCalibration, &v); - // Sum up CALIBRATING_GYRO_CYCLES readings - gyroCalibration->g[axis] += dev->gyroADCRaw[axis]; - devPush(&gyroCalibration->var[axis], dev->gyroADCRaw[axis]); - - // gyroZero is set to zero until calibration complete - dev->gyroZero[axis] = 0; - - if (isOnFinalGyroCalibrationCycle(gyroCalibration)) { - const float stddev = devStandardDeviation(&gyroCalibration->var[axis]); - // check deviation and start over if the model was moved - if (gyroMovementCalibrationThreshold && stddev > gyroMovementCalibrationThreshold) { - gyroSetCalibrationCycles(CALIBRATING_GYRO_CYCLES); - return; - } - // calibration complete, so set the gyro zero values - dev->gyroZero[axis] = (gyroCalibration->g[axis] + (CALIBRATING_GYRO_CYCLES / 2)) / CALIBRATING_GYRO_CYCLES; - } - } + // Check if calibration is complete after this cycle + if (zeroCalibrationIsCompleteV(gyroCalibration)) { + zeroCalibrationGetZeroV(gyroCalibration, &v); + dev->gyroZero[0] = v.v[0]; + dev->gyroZero[1] = v.v[1]; + dev->gyroZero[2] = v.v[2]; - if (isOnFinalGyroCalibrationCycle(gyroCalibration)) { DEBUG_TRACE_SYNC("Gyro calibration complete (%d, %d, %d)", dev->gyroZero[0], dev->gyroZero[1], dev->gyroZero[2]); schedulerResetTaskStatistics(TASK_SELF); // so calibration cycles do not pollute tasks statistics } - gyroCalibration->calibratingG--; -} - -#ifdef USE_ASYNC_GYRO_PROCESSING -STATIC_FASTRAM float accumulatedRates[XYZ_AXIS_COUNT]; -STATIC_FASTRAM timeUs_t accumulatedRateTimeUs; - -static void gyroUpdateAccumulatedRates(timeDelta_t gyroUpdateDeltaUs) -{ - accumulatedRateTimeUs += gyroUpdateDeltaUs; - const float gyroUpdateDelta = gyroUpdateDeltaUs * 1e-6f; - for (int axis = 0; axis < 3; axis++) { - accumulatedRates[axis] += gyro.gyroADCf[axis] * gyroUpdateDelta; + else { + dev->gyroZero[0] = 0; + dev->gyroZero[1] = 0; + dev->gyroZero[2] = 0; } } -#endif /* * Calculate rotation rate in rad/s in body frame */ void gyroGetMeasuredRotationRate(fpVector3_t *measuredRotationRate) { -#ifdef USE_ASYNC_GYRO_PROCESSING - const float accumulatedRateTime = accumulatedRateTimeUs * 1e-6; - accumulatedRateTimeUs = 0; - for (int axis = 0; axis < 3; axis++) { - measuredRotationRate->v[axis] = DEGREES_TO_RADIANS(accumulatedRates[axis] / accumulatedRateTime); - accumulatedRates[axis] = 0.0f; - } -#else for (int axis = 0; axis < 3; axis++) { measuredRotationRate->v[axis] = DEGREES_TO_RADIANS(gyro.gyroADCf[axis]); } -#endif } -void gyroUpdate(timeDelta_t gyroUpdateDeltaUs) +void gyroUpdate() { // range: +/- 8192; +/- 2000 deg/sec if (gyroDev0.readFn(&gyroDev0)) { - if (isCalibrationComplete(&gyroCalibration)) { + if (zeroCalibrationIsCompleteV(&gyroCalibration)) { // Copy gyro value into int32_t (to prevent overflow) and then apply calibration and alignment gyroADC[X] = (int32_t)gyroDev0.gyroADCRaw[X] - (int32_t)gyroDev0.gyroZero[X]; gyroADC[Y] = (int32_t)gyroDev0.gyroADCRaw[Y] - (int32_t)gyroDev0.gyroZero[Y]; @@ -436,7 +399,7 @@ void gyroUpdate(timeDelta_t gyroUpdateDeltaUs) applySensorAlignment(gyroADC, gyroADC, gyroDev0.gyroAlign); applyBoardAlignment(gyroADC); } else { - performGyroCalibration(&gyroDev0, &gyroCalibration, gyroConfig()->gyroMovementCalibrationThreshold); + performGyroCalibration(&gyroDev0, &gyroCalibration); // Reset gyro values to zero to prevent other code from using uncalibrated data gyro.gyroADCf[X] = 0.0f; gyro.gyroADCf[Y] = 0.0f; @@ -478,11 +441,6 @@ void gyroUpdate(timeDelta_t gyroUpdateDeltaUs) #endif gyro.gyroADCf[axis] = gyroADCf; } - -#ifdef USE_ASYNC_GYRO_PROCESSING - // Accumulate gyro readings for better IMU accuracy - gyroUpdateAccumulatedRates(gyroUpdateDeltaUs); -#endif } bool gyroReadTemperature(void) diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index f145b11c9fc..61d65e62373 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -35,6 +35,7 @@ typedef enum { GYRO_MPU6500, GYRO_MPU9250, GYRO_BMI160, + GYRO_ICM20689, GYRO_FAKE } gyroSensor_e; @@ -66,8 +67,8 @@ PG_DECLARE(gyroConfig_t, gyroConfig); bool gyroInit(void); void gyroInitFilters(void); void gyroGetMeasuredRotationRate(fpVector3_t *imuMeasuredRotationBF); -void gyroUpdate(timeDelta_t gyroUpdateDeltaUs); -void gyroSetCalibrationCycles(uint16_t calibrationCyclesRequired); +void gyroUpdate(); +void gyroStartCalibration(void); bool gyroIsCalibrationComplete(void); bool gyroReadTemperature(void); int16_t gyroGetTemperature(void); diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c index 9389a436450..e4990a14a62 100755 --- a/src/main/sensors/initialisation.c +++ b/src/main/sensors/initialisation.c @@ -37,6 +37,7 @@ #include "sensors/compass.h" #include "sensors/rangefinder.h" #include "sensors/opflow.h" +#include "sensors/temperature.h" #include "sensors/initialisation.h" uint8_t requestedSensors[SENSOR_INDEX_COUNT] = { GYRO_AUTODETECT, ACC_NONE, BARO_NONE, MAG_NONE, RANGEFINDER_NONE, PITOT_NONE, OPFLOW_NONE }; @@ -51,7 +52,7 @@ bool sensorsAutodetect(void) return false; } - accInit(getAccUpdateRate()); + accInit(getLooptime()); #ifdef USE_BARO baroInit(); @@ -65,6 +66,10 @@ bool sensorsAutodetect(void) compassInit(); #endif +#ifdef USE_TEMPERATURE_SENSOR + temperatureInit(); +#endif + #ifdef USE_RANGEFINDER rangefinderInit(); #endif diff --git a/src/main/sensors/pitotmeter.c b/src/main/sensors/pitotmeter.c index 93bbb7781e4..385cac4b088 100644 --- a/src/main/sensors/pitotmeter.c +++ b/src/main/sensors/pitotmeter.c @@ -39,6 +39,8 @@ #include "fc/config.h" #include "fc/runtime_config.h" +#include "scheduler/protothreads.h" + #include "sensors/pitotmeter.h" #include "sensors/sensors.h" @@ -46,14 +48,11 @@ pitot_t pitot; -static timeMs_t pitotCalibrationTimeout = 0; -static bool pitotCalibrationFinished = false; -static float pitotPressureZero = 0; -static float pitotPressure = 0; -static float pitotTemperature = 0; -static float indicatedAirspeed = 0; +PG_REGISTER_WITH_RESET_TEMPLATE(pitotmeterConfig_t, pitotmeterConfig, PG_PITOTMETER_CONFIG, 2); -PG_REGISTER_WITH_RESET_TEMPLATE(pitotmeterConfig_t, pitotmeterConfig, PG_PITOTMETER_CONFIG, 0); +#define PITOT_HARDWARE_TIMEOUT_MS 500 // Accept 500ms of non-responsive sensor, report HW failure otherwise +#define AIR_DENSITY_SEA_LEVEL_15C 1.225f // Air density at sea level and 15 degrees Celsius +#define P0 101325.0f // standard pressure [Pa] #ifdef USE_PITOT #define PITOT_HARDWARE_DEFAULT PITOT_AUTODETECT @@ -62,8 +61,7 @@ PG_REGISTER_WITH_RESET_TEMPLATE(pitotmeterConfig_t, pitotmeterConfig, PG_PITOTME #endif PG_RESET_TEMPLATE(pitotmeterConfig_t, pitotmeterConfig, .pitot_hardware = PITOT_HARDWARE_DEFAULT, - .use_median_filtering = 1, - .pitot_noise_lpf = 0.6f, + .pitot_lpf_milli_hz = 350, .pitot_scale = 1.00f ); @@ -155,112 +153,90 @@ bool pitotInit(void) bool pitotIsCalibrationComplete(void) { - return pitotCalibrationFinished; + return zeroCalibrationIsCompleteS(&pitot.zeroCalibration) && zeroCalibrationIsSuccessfulS(&pitot.zeroCalibration); } void pitotStartCalibration(void) { - pitotCalibrationTimeout = millis(); - pitotCalibrationFinished = false; + zeroCalibrationStartS(&pitot.zeroCalibration, CALIBRATING_PITOT_TIME_MS, P0 * 0.00001f, false); } -#define PRESSURE_SAMPLES_MEDIAN 3 - -static int32_t applyPitotmeterMedianFilter(int32_t newPressureReading) +static void performPitotCalibrationCycle(void) { - static int32_t pitotFilterSamples[PRESSURE_SAMPLES_MEDIAN]; - static int currentFilterSampleIndex = 0; - static bool medianFilterReady = false; - int nextSampleIndex; - - nextSampleIndex = (currentFilterSampleIndex + 1); - if (nextSampleIndex == PRESSURE_SAMPLES_MEDIAN) { - nextSampleIndex = 0; - medianFilterReady = true; + zeroCalibrationAddValueS(&pitot.zeroCalibration, pitot.pressure); + + if (zeroCalibrationIsCompleteS(&pitot.zeroCalibration)) { + zeroCalibrationGetZeroS(&pitot.zeroCalibration, &pitot.pressureZero); + DEBUG_TRACE_SYNC("Pitot calibration complete (%d)", lrintf(pitot.pressureZero)); } +} - pitotFilterSamples[currentFilterSampleIndex] = newPressureReading; - currentFilterSampleIndex = nextSampleIndex; +STATIC_PROTOTHREAD(pitotThread) +{ + ptBegin(pitotThread); - if (medianFilterReady) - return quickMedianFilter3(pitotFilterSamples); - else - return newPressureReading; -} + static float pitotPressureTmp; + timeUs_t currentTimeUs; -typedef enum { - PITOTMETER_NEEDS_SAMPLES = 0, - PITOTMETER_NEEDS_CALCULATION, -} pitotmeterState_e; + // Init filter + pitot.lastMeasurementUs = micros(); + pt1FilterInit(&pitot.lpfState, pitotmeterConfig()->pitot_lpf_milli_hz / 1000.0f, 0); -uint32_t pitotUpdate(void) -{ - static pitotmeterState_e state = PITOTMETER_NEEDS_SAMPLES; - - switch (state) { - default: - case PITOTMETER_NEEDS_SAMPLES: - pitot.dev.start(&pitot.dev); - state = PITOTMETER_NEEDS_CALCULATION; - return pitot.dev.delay; - break; - - case PITOTMETER_NEEDS_CALCULATION: - pitot.dev.get(&pitot.dev); - pitot.dev.calculate(&pitot.dev, &pitotPressure, &pitotTemperature); - if (pitotmeterConfig()->use_median_filtering) { - pitotPressure = applyPitotmeterMedianFilter(pitotPressure); - } - state = PITOTMETER_NEEDS_SAMPLES; - return pitot.dev.delay; - break; - } -} + while(1) { + // Start measurement + if (pitot.dev.start(&pitot.dev)) { + pitot.lastSeenHealthyMs = millis(); + } -#define AIR_DENSITY_SEA_LEVEL_15C 1.225f // Air density at sea level and 15 degrees Celsius -#define AIR_GAS_CONST 287.1f // J / (kg * K) -#define P0 101325.0f // standard pressure + ptDelayUs(pitot.dev.delay); -static void performPitotCalibrationCycle(void) -{ - const float pitotPressureZeroError = pitotPressure - pitotPressureZero; - pitotPressureZero += pitotPressureZeroError * 0.15f; + // Read and calculate data + if (pitot.dev.get(&pitot.dev)) { + pitot.lastSeenHealthyMs = millis(); + } - if (ABS(pitotPressureZeroError) < (P0 * 0.00001f)) { // 0.001% calibration error - if ((millis() - pitotCalibrationTimeout) > 250) { - pitotCalibrationFinished = true; + pitot.dev.calculate(&pitot.dev, &pitotPressureTmp, NULL); + ptYield(); + + // Filter pressure + currentTimeUs = micros(); + pitot.pressure = pt1FilterApply3(&pitot.lpfState, pitotPressureTmp, (currentTimeUs - pitot.lastMeasurementUs) * 1e-6f); + pitot.lastMeasurementUs = currentTimeUs; + ptDelayUs(pitot.dev.delay); + + // Calculate IAS + if (pitotIsCalibrationComplete()) { + // https://en.wikipedia.org/wiki/Indicated_airspeed + // Indicated airspeed (IAS) is the airspeed read directly from the airspeed indicator on an aircraft, driven by the pitot-static system. + // The IAS is an important value for the pilot because it is the indicated speeds which are specified in the aircraft flight manual for + // such important performance values as the stall speed. A typical aircraft will always stall at the same indicated airspeed (for the current configuration) + // regardless of density, altitude or true airspeed. + // + // Therefore we shouldn't care about CAS/TAS and only calculate IAS since it's more indicative to the pilot and more useful in calculations + // It also allows us to use pitot_scale to calibrate the dynamic pressure sensor scale + pitot.airSpeed = pitotmeterConfig()->pitot_scale * sqrtf(2.0f * fabsf(pitot.pressure - pitot.pressureZero) / AIR_DENSITY_SEA_LEVEL_15C) * 100; + } else { + performPitotCalibrationCycle(); + pitot.airSpeed = 0; } } - else { - pitotCalibrationTimeout = millis(); - } + + ptEnd(0); +} + +void pitotUpdate(void) +{ + pitotThread(); } int32_t pitotCalculateAirSpeed(void) { - if (pitotIsCalibrationComplete()) { - // https://en.wikipedia.org/wiki/Indicated_airspeed - // Indicated airspeed (IAS) is the airspeed read directly from the airspeed indicator on an aircraft, driven by the pitot-static system. - // The IAS is an important value for the pilot because it is the indicated speeds which are specified in the aircraft flight manual for - // such important performance values as the stall speed. A typical aircraft will always stall at the same indicated airspeed (for the current configuration) - // regardless of density, altitude or true airspeed. - // - // Therefore we shouldn't care about CAS/TAS and only calculate IAS since it's more indicative to the pilot and more useful in calculations - // It also allows us to use pitot_scale to calibrate the dynamic pressure sensor scale - const float indicatedAirspeed_tmp = pitotmeterConfig()->pitot_scale * sqrtf(2.0f * fabsf(pitotPressure - pitotPressureZero) / AIR_DENSITY_SEA_LEVEL_15C); - indicatedAirspeed += pitotmeterConfig()->pitot_noise_lpf * (indicatedAirspeed_tmp - indicatedAirspeed); - - pitot.airSpeed = indicatedAirspeed * 100; - } else { - performPitotCalibrationCycle(); - pitot.airSpeed = 0; - } return pitot.airSpeed; } bool pitotIsHealthy(void) { - return true; + return (millis() - pitot.lastSeenHealthyMs) < PITOT_HARDWARE_TIMEOUT_MS; } #endif /* PITOT */ diff --git a/src/main/sensors/pitotmeter.h b/src/main/sensors/pitotmeter.h index 8f11fbb1381..548b23be02f 100644 --- a/src/main/sensors/pitotmeter.h +++ b/src/main/sensors/pitotmeter.h @@ -18,6 +18,8 @@ #pragma once #include "config/parameter_group.h" +#include "common/filter.h" +#include "common/calibration.h" #include "drivers/pitotmeter.h" @@ -35,8 +37,7 @@ typedef enum { typedef struct pitotmeterConfig_s { uint8_t pitot_hardware; // Pitotmeter hardware to use - uint8_t use_median_filtering; // Use 3-point median filtering - float pitot_noise_lpf; // additional LPF to reduce pitot noise + uint16_t pitot_lpf_milli_hz; // additional LPF to reduce pitot noise in [0.001Hz] float pitot_scale; // scale value } pitotmeterConfig_t; @@ -44,7 +45,15 @@ PG_DECLARE(pitotmeterConfig_t, pitotmeterConfig); typedef struct pito_s { pitotDev_t dev; - int32_t airSpeed; + float airSpeed; + + zeroCalibrationScalar_t zeroCalibration; + pt1Filter_t lpfState; + timeUs_t lastMeasurementUs; + timeMs_t lastSeenHealthyMs; + + float pressureZero; + float pressure; } pitot_t; #ifdef USE_PITOT @@ -54,7 +63,7 @@ extern pitot_t pitot; bool pitotInit(void); bool pitotIsCalibrationComplete(void); void pitotStartCalibration(void); -uint32_t pitotUpdate(void); +void pitotUpdate(void); int32_t pitotCalculateAirSpeed(void); bool pitotIsHealthy(void); diff --git a/src/main/sensors/rangefinder.c b/src/main/sensors/rangefinder.c index c9bd9238044..4db94d89a32 100644 --- a/src/main/sensors/rangefinder.c +++ b/src/main/sensors/rangefinder.c @@ -152,6 +152,15 @@ static bool rangefinderDetect(rangefinderDev_t * dev, uint8_t rangefinderHardwar #endif break; + case RANGEFINDER_BENEWAKE: +#if defined(USE_RANGEFINDER_BENEWAKE) + if (virtualRangefinderDetect(dev, &rangefinderBenewakeVtable)) { + rangefinderHardware = RANGEFINDER_BENEWAKE; + rescheduleTask(TASK_RANGEFINDER, TASK_PERIOD_MS(RANGEFINDER_VIRTUAL_TASK_PERIOD_MS)); + } +#endif + break; + case RANGEFINDER_NONE: rangefinderHardware = RANGEFINDER_NONE; break; diff --git a/src/main/sensors/rangefinder.h b/src/main/sensors/rangefinder.h index cfb67e71d57..c80d18ac1eb 100644 --- a/src/main/sensors/rangefinder.h +++ b/src/main/sensors/rangefinder.h @@ -29,6 +29,7 @@ typedef enum { RANGEFINDER_VL53L0X = 4, RANGEFINDER_MSP = 5, RANGEFINDER_UIB = 6, + RANGEFINDER_BENEWAKE = 7, } rangefinderType_e; typedef struct rangefinderConfig_s { diff --git a/src/main/sensors/sensors.h b/src/main/sensors/sensors.h index 3764ba0c62c..1a6ab0b8b20 100644 --- a/src/main/sensors/sensors.h +++ b/src/main/sensors/sensors.h @@ -39,8 +39,10 @@ typedef union flightDynamicsTrims_u { flightDynamicsTrims_def_t values; } flightDynamicsTrims_t; -#define CALIBRATING_GYRO_CYCLES 1000 -#define CALIBRATING_ACC_CYCLES 400 +#define CALIBRATING_BARO_TIME_MS 2000 +#define CALIBRATING_PITOT_TIME_MS 2000 +#define CALIBRATING_GYRO_TIME_MS 2000 +#define CALIBRATING_ACC_TIME_MS 500 // These bits have to be aligned with sensorIndex_e typedef enum { @@ -53,6 +55,7 @@ typedef enum { SENSOR_OPFLOW = 1 << 6, SENSOR_GPS = 1 << 7, SENSOR_GPSMAG = 1 << 8, + SENSOR_TEMP = 1 << 9 } sensors_e; extern uint8_t requestedSensors[SENSOR_INDEX_COUNT]; diff --git a/src/main/sensors/temperature.c b/src/main/sensors/temperature.c index 76289004134..3f6b61aab54 100644 --- a/src/main/sensors/temperature.c +++ b/src/main/sensors/temperature.c @@ -17,35 +17,322 @@ #include "stdbool.h" #include "stdint.h" +#include #include "platform.h" +#include "build/debug.h" + #include "common/maths.h" +#include "common/printf.h" #include "config/parameter_group.h" #include "config/parameter_group_ids.h" +#include "drivers/1-wire.h" +#include "drivers/logging.h" +#include "drivers/temperature/temperature.h" +#include "drivers/temperature/lm75.h" +#include "drivers/temperature/ds18b20.h" + +#include "fc/runtime_config.h" + +#include "sensors/sensors.h" #include "sensors/temperature.h" #include "sensors/gyro.h" +#include "sensors/barometer.h" + +#include "scheduler/protothreads.h" + + +PG_REGISTER_ARRAY(tempSensorConfig_t, MAX_TEMP_SENSORS, tempSensorConfig, PG_TEMP_SENSOR_CONFIG, 1); + +#define MPU_TEMP_VALID_BIT 0 +#define BARO_TEMP_VALID_BIT 1 +#define MPU_TEMP_VALID (mpuBaroTempValid & (1 << MPU_TEMP_VALID_BIT)) +#define BARO_TEMP_VALID (mpuBaroTempValid & (1 << BARO_TEMP_VALID_BIT)) + +static uint16_t mpuTemperature, baroTemperature; +static uint8_t mpuBaroTempValid = 0; + +#ifdef USE_TEMPERATURE_SENSOR +static int16_t tempSensorValue[MAX_TEMP_SENSORS]; + +// Each bit corresponds to a sensor LSB = first sensor. 1 = valid value +static uint8_t sensorStatus[MAX_TEMP_SENSORS / 8 + (MAX_TEMP_SENSORS % 8 ? 1 : 0)]; + +#ifdef USE_TEMPERATURE_LM75 +static temperatureDev_t lm75Dev[8]; +#endif + +#ifdef DS18B20_DRIVER_AVAILABLE +static owDev_t *owDev; +#endif + +static void newSensorCheckAndEnter(uint8_t type, uint64_t addr) +{ + int8_t foundConfigIndex = -1, firstFreeConfigSlot = -1; + + // try to find sensor or free config slot + for (uint8_t configIndex = 0; configIndex < MAX_TEMP_SENSORS; ++configIndex) { + + const tempSensorConfig_t *configSlot = tempSensorConfig(configIndex); + + if ((configSlot->type == type) && (configSlot->address == addr)) { + foundConfigIndex = configIndex; + break; + } + + if ((firstFreeConfigSlot == -1) && !configSlot->type) firstFreeConfigSlot = configIndex; + + } -static bool tempSensorValid[TEMP_COUNT]; -static int16_t tempSensorValue[TEMP_COUNT]; + // if new sensor and have free config slot enter new sensor + if ((foundConfigIndex == -1) && (firstFreeConfigSlot != -1)) { + tempSensorConfig_t *configSlot = tempSensorConfigMutable(firstFreeConfigSlot); + configSlot->type = type; + configSlot->address = addr; + configSlot->osdSymbol = 0; + configSlot->label[0] = '\0'; + configSlot->alarm_min = -200; + configSlot->alarm_max = 600; + } +} + +void temperatureInit(void) +{ + memset(sensorStatus, 0, sizeof(sensorStatus) * sizeof(*sensorStatus)); + addBootlogEvent2(BOOT_EVENT_TEMP_SENSOR_DETECTION, BOOT_EVENT_FLAGS_NONE); + + sensorsSet(SENSOR_TEMP); + +#ifdef USE_TEMPERATURE_LM75 + memset(lm75Dev, 0, sizeof(lm75Dev)); + for (uint8_t lm75Addr = 0; lm75Addr < 8; ++lm75Addr) { + if (lm75Detect(lm75Dev + lm75Addr, lm75Addr)) + newSensorCheckAndEnter(TEMP_SENSOR_LM75, lm75Addr); + } +#endif + +#ifdef DS18B20_DRIVER_AVAILABLE + owDev = getOwDev(); + if (owDev) { + uint64_t ds18b20_rom_table[MAX_TEMP_SENSORS]; + uint8_t ds18b20_rom_count = MAX_TEMP_SENSORS; + ds18b20Enumerate(owDev, ds18b20_rom_table, &ds18b20_rom_count); + + for (uint8_t rom_index = 0; rom_index < ds18b20_rom_count; ++rom_index) + newSensorCheckAndEnter(TEMP_SENSOR_DS18B20, ds18b20_rom_table[rom_index]); + } +#endif + + // configure sensors + for (uint8_t configIndex = 0; configIndex < MAX_TEMP_SENSORS; ++configIndex) { + const tempSensorConfig_t *configSlot = tempSensorConfig(configIndex); + + switch (configSlot->type) { +#ifdef DS18B20_DRIVER_AVAILABLE + case TEMP_SENSOR_DS18B20: + if (owDev) { + tempSensorValue[configIndex] = TEMPERATURE_INVALID_VALUE; + ds18b20Configure(owDev, configSlot->address, DS18B20_CONFIG_9BIT); + } + break; +#endif + + default:; + } + } +} + +static bool temperatureSensorValueIsValid(uint8_t temperatureUpdateSensorIndex) +{ + uint8_t mask = 1 << (temperatureUpdateSensorIndex % 8); + uint8_t byteIndex = temperatureUpdateSensorIndex / 8; + return sensorStatus[byteIndex] & mask; +} + +// returns decidegrees centigrade +bool getSensorTemperature(uint8_t temperatureUpdateSensorIndex, int16_t *temperature) +{ + *temperature = tempSensorValue[temperatureUpdateSensorIndex]; + return temperatureSensorValueIsValid(temperatureUpdateSensorIndex); +} -bool isTemperatureSensorValid(tempSensor_e sensor) +// Converts 64bit integer address to hex format +// hex_address must be at least 17 bytes long (16 chars + NULL) +void tempSensorAddressToString(uint64_t address, char *hex_address) { - return tempSensorValid[sensor]; + if (address < 8) + tfp_sprintf(hex_address, "%d", address); + else { + uint32_t *address32 = (uint32_t *)&address; + tfp_sprintf(hex_address, "%08lx%08lx", address32[1], address32[0]); + } } -int16_t getTemperature(tempSensor_e sensor) +// Converts address string in hex format to unsigned integer +// the hex_address parameter must be NULL or space terminated +bool tempSensorStringToAddress(const char *hex_address, uint64_t *address) { - return tempSensorValue[sensor]; + uint16_t char_count = 0; + *address = 0; + while (*hex_address && (*hex_address != ' ')) { + if (++char_count > 16) return false; + char byte = *hex_address++; + if (byte >= '0' && byte <= '9') byte = byte - '0'; + else if (byte >= 'a' && byte <='f') byte = byte - 'a' + 10; + else if (byte >= 'A' && byte <='F') byte = byte - 'A' + 10; + else return false; + *address = (*address << 4) | (byte & 0xF); + } + return true; } +#endif /* USE_TEMPERATURE_SENSOR */ + +#ifdef USE_TEMPERATURE_SENSOR + +static uint8_t temperatureUpdateSensorIndex; +static bool temperatureUpdateValueValid; -void temperatureUpdate(void) +#ifdef DS18B20_DRIVER_AVAILABLE +static uint8_t temperatureUpdateIndex; +static uint8_t temperatureUpdateBuf[9]; +#endif + +#endif /* defined(USE_TEMPERATURE_SENSOR) */ + +PROTOTHREAD(temperatureUpdate) { - // TEMP_GYRO: Update gyro temperature - if (gyroReadTemperature()) { - tempSensorValid[TEMP_GYRO] = true; - tempSensorValue[TEMP_GYRO] = gyroGetTemperature(); + ptBegin(temperatureUpdate); + + while (1) { + + if (gyroReadTemperature()) { + mpuTemperature = gyroGetTemperature(); + mpuBaroTempValid |= (1 << MPU_TEMP_VALID_BIT); + } else + mpuBaroTempValid &= ~(1 << MPU_TEMP_VALID_BIT); + + #ifdef USE_BARO + if (sensors(SENSOR_BARO)) { + baroTemperature = baroGetTemperature(); + mpuBaroTempValid |= (1 << BARO_TEMP_VALID_BIT); + } else + mpuBaroTempValid &= ~(1 << BARO_TEMP_VALID_BIT); + #endif + +#ifdef USE_TEMPERATURE_SENSOR + + temperatureUpdateSensorIndex = 0; + do { + const tempSensorConfig_t *configSlot = tempSensorConfig(temperatureUpdateSensorIndex); + temperatureUpdateValueValid = false; + +#ifdef USE_TEMPERATURE_LM75 + if (configSlot->type == TEMP_SENSOR_LM75) { + if (configSlot->address < 8) { + temperatureDev_t *dev = lm75Dev + configSlot->address; + if (dev->read && dev->read(dev, &tempSensorValue[temperatureUpdateSensorIndex])) temperatureUpdateValueValid = true; + } + } +#endif + +#ifdef DS18B20_DRIVER_AVAILABLE + if ((configSlot->type == TEMP_SENSOR_DS18B20) && owDev) { + bool ack = owDev->owResetCommand(owDev); + if (!ack) goto temperatureUpdateError; + ptWait(owDev->owBusReady(owDev)); + + ack = owDev->owMatchRomCommand(owDev); + if (!ack) goto temperatureUpdateError; + ptWait(owDev->owBusReady(owDev)); + + temperatureUpdateIndex = 0; + do { + ack = owDev->owWriteByteCommand(owDev, ((uint8_t *)&tempSensorConfig(temperatureUpdateSensorIndex)->address)[temperatureUpdateIndex]); + if (!ack) goto temperatureUpdateError; + ptWait(owDev->owBusReady(owDev)); + } while (++temperatureUpdateIndex < 8); + + ack = ds18b20ReadScratchpadCommand(owDev); + if (!ack) goto temperatureUpdateError; + ptWait(owDev->owBusReady(owDev)); + + temperatureUpdateIndex = 0; + do { + ack = owDev->owReadByteCommand(owDev); + if (!ack) goto temperatureUpdateError; + ptWait(owDev->owBusReady(owDev)); + ack = owDev->owReadByteResult(owDev, temperatureUpdateBuf + temperatureUpdateIndex); + if (!ack) goto temperatureUpdateError; + } while (++temperatureUpdateIndex < 9); + + int16_t temperature; + if (ds18b20ReadTemperatureFromScratchPadBuf(temperatureUpdateBuf, &temperature)) { + if (temperatureSensorValueIsValid(temperatureUpdateSensorIndex) || (tempSensorValue[temperatureUpdateSensorIndex] == -1240)) { + tempSensorValue[temperatureUpdateSensorIndex] = temperature; + temperatureUpdateValueValid = true; + } else + tempSensorValue[temperatureUpdateSensorIndex] = -1240; + } else + tempSensorValue[temperatureUpdateSensorIndex] = TEMPERATURE_INVALID_VALUE; + } + +temperatureUpdateError:; +#endif + + uint8_t statusMask = 1 << (temperatureUpdateSensorIndex % 8); + uint8_t byteIndex = temperatureUpdateSensorIndex / 8; + if (temperatureUpdateValueValid) + sensorStatus[byteIndex] |= statusMask; + else + sensorStatus[byteIndex] &= ~statusMask; + + ptYield(); + + } while (++temperatureUpdateSensorIndex < MAX_TEMP_SENSORS); + +#ifdef DS18B20_DRIVER_AVAILABLE + if (owDev) { + bool ack = owDev->owResetCommand(owDev); + if (!ack) goto ds18b20StartConversionError; + ptWait(owDev->owBusReady(owDev)); + + ack = owDev->owSkipRomCommand(owDev); + if (!ack) goto ds18b20StartConversionError; + ptWait(owDev->owBusReady(owDev)); + + ds18b20StartConversionCommand(owDev); + } + +ds18b20StartConversionError:; +#endif + +#endif /* defined(USE_TEMPERATURE_SENSOR) */ + + ptDelayMs(100); // DS18B20 sensors take 94ms for a temperature conversion with 9bit resolution + } + + ptEnd(0); +} + +// returns decidegrees centigrade +bool getIMUTemperature(int16_t *temperature) +{ + *temperature = mpuTemperature; + return MPU_TEMP_VALID; +} + +// returns decidegrees centigrade +bool getBaroTemperature(int16_t *temperature) +{ + *temperature = baroTemperature; + return BARO_TEMP_VALID; +} + +void resetTempSensorConfig(void) +{ + memset(tempSensorConfigMutable(0), 0, sizeof(tempSensorConfig_t) * MAX_TEMP_SENSORS); } diff --git a/src/main/sensors/temperature.h b/src/main/sensors/temperature.h index a0d258174cc..9f6f8be0041 100644 --- a/src/main/sensors/temperature.h +++ b/src/main/sensors/temperature.h @@ -19,13 +19,40 @@ #include "config/parameter_group.h" +#define TEMPERATURE_LABEL_LEN 4 +#define MAX_TEMP_SENSORS 8 + +#define TEMPERATURE_INVALID_VALUE -1250 + typedef enum { - TEMP_GYRO = 0, - TEMP_COUNT -} tempSensor_e; + TEMP_SENSOR_NONE = 0, + TEMP_SENSOR_LM75, + TEMP_SENSOR_DS18B20 +} tempSensorType_e; -// Temperature is returned in degC*10 +typedef struct { + tempSensorType_e type; + uint64_t address; + uint8_t osdSymbol; + char label[TEMPERATURE_LABEL_LEN]; + int16_t alarm_min; + int16_t alarm_max; +} tempSensorConfig_t; -bool isTemperatureSensorValid(tempSensor_e sensor); -int16_t getTemperature(tempSensor_e sensor); +PG_DECLARE_ARRAY(tempSensorConfig_t, MAX_TEMP_SENSORS, tempSensorConfig); + +// Temperature is returned in degC*10 +bool getIMUTemperature(int16_t *temperature); +bool getBaroTemperature(int16_t *temperature); void temperatureUpdate(void); + +#ifdef USE_TEMPERATURE_SENSOR +void temperatureInit(void); + +bool getSensorTemperature(uint8_t sensorIndex, int16_t *temperature); + +void tempSensorAddressToString(uint64_t address, char *hex_address); +bool tempSensorStringToAddress(const char *hex_address, uint64_t *address); +#endif + +void resetTempSensorConfig(void); diff --git a/src/main/target/AIRBOTF4/target.c b/src/main/target/AIRBOTF4/target.c index 8a05e21c1ef..8dd2f4b042c 100644 --- a/src/main/target/AIRBOTF4/target.c +++ b/src/main/target/AIRBOTF4/target.c @@ -24,18 +24,20 @@ #include "drivers/bus.h" const timerHardware_t timerHardware[] = { - { TIM12, IO_TAG(PB14), TIM_Channel_1, 0, IOCFG_AF_PP_PD, GPIO_AF_TIM12, TIM_USE_PPM | TIM_USE_PWM }, // PPM / S.BUS input, above MOTOR1 - { TIM12, IO_TAG(PB15), TIM_Channel_2, 0, IOCFG_AF_PP_PD, GPIO_AF_TIM12, 0 }, // Connected: small CH2 pad, not used as PWM, definition inherited from REVO target - GPIO_PartialRemap_TIM3 - { TIM8, IO_TAG(PC6), TIM_Channel_1, 0, IOCFG_AF_PP_PD, GPIO_AF_TIM8, 0 }, // Connected: UART6 TX, not used as PWM, definition inherited from REVO target - { TIM8, IO_TAG(PC7), TIM_Channel_2, 0, IOCFG_AF_PP_PD, GPIO_AF_TIM8, 0 }, // Connected: UART6 RX, not used as PWM, definition inherited from REVO target - { TIM8, IO_TAG(PC8), TIM_Channel_3, 0, IOCFG_AF_PP_PD, GPIO_AF_TIM8, 0 }, // Connected: small CH5 pad, not used as PWM, definition inherited from REVO target - { TIM8, IO_TAG(PC9), TIM_Channel_4, 0, IOCFG_AF_PP_PD, GPIO_AF_TIM8, 0 }, // Connected: small CH6 pad, not used as PWM, definition inherited from REVO target - { TIM3, IO_TAG(PB0), TIM_Channel_3, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM3, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR }, // MOTOR_1 - { TIM3, IO_TAG(PB1), TIM_Channel_4, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM3, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR }, // MOTOR_2 - { TIM9, IO_TAG(PA3), TIM_Channel_2, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // MOTOR_3 - { TIM2, IO_TAG(PA2), TIM_Channel_3, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // MOTOR_4 - { TIM5, IO_TAG(PA1), TIM_Channel_2, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM5, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO }, // MOTOR_5 - GPIO_PartialRemap_TIM3 - { TIM1, IO_TAG(PA8), TIM_Channel_1, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM1, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO }, // MOTOR_6 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1_OUT D1_ST7 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S2_OUT D1_ST2 + DEF_TIM(TIM2, CH4, PA3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), // S3_OUT D1_ST6 + DEF_TIM(TIM2, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4_OUT D1_ST1 + DEF_TIM(TIM5, CH2, PA1, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO | TIM_USE_ANY, 0, 0), // S5_OUT / LED + DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S6_OUT + DEF_TIM(TIM4, CH1, PB6, TIM_USE_ANY, 0, 0), // LED D1_ST0, n/a on older AIRBOTF4 + + DEF_TIM(TIM12, CH1, PB14, TIM_USE_PWM | TIM_USE_PPM, 0, 0), // PPM (5th pin on FlexiIO port) + DEF_TIM(TIM12, CH2, PB15, TIM_USE_PWM, 0, 0), // S2_IN + DEF_TIM(TIM8, CH1, PC6, TIM_USE_PWM | TIM_USE_FW_SERVO, 0, 0), // S3_IN + DEF_TIM(TIM8, CH2, PC7, TIM_USE_PWM | TIM_USE_FW_SERVO, 0, 0), // S4_IN + DEF_TIM(TIM8, CH3, PC8, TIM_USE_PWM | TIM_USE_FW_SERVO, 0, 0), // S5_IN + DEF_TIM(TIM8, CH4, PC9, TIM_USE_PWM | TIM_USE_FW_SERVO, 0, 0), // S6_IN }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/AIRBOTF4/target.h b/src/main/target/AIRBOTF4/target.h index 8204dad9d81..04a1a421964 100644 --- a/src/main/target/AIRBOTF4/target.h +++ b/src/main/target/AIRBOTF4/target.h @@ -71,9 +71,10 @@ #define USE_BARO_MS5611 #define USE_PITOT_ADC -#define USE_PITOT_MS4525 #define PITOT_I2C_BUS BUS_I2C2 +#define TEMPERATURE_I2C_BUS BUS_I2C2 + #define M25P16_CS_PIN PB3 #define M25P16_SPI_BUS BUS_SPI3 @@ -131,12 +132,7 @@ #define SENSORS_SET (SENSOR_ACC|SENSOR_MAG|SENSOR_BARO) #define USE_LED_STRIP -// LED Strip can run off Pin 5 (PA1) of the MOTOR outputs. -#define WS2811_GPIO_AF GPIO_AF_TIM5 #define WS2811_PIN PA1 -#define WS2811_DMA_STREAM DMA1_Stream4 -#define WS2811_DMA_CHANNEL DMA_Channel_6 -#define WS2811_DMA_HANDLER_IDENTIFER DMA1_ST4_HANDLER #define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT @@ -160,4 +156,6 @@ #define TARGET_IO_PORTC 0xffff #define TARGET_IO_PORTD 0xffff -#define PCA9685_I2C_BUS BUS_I2C2 \ No newline at end of file +#define PCA9685_I2C_BUS BUS_I2C2 + +#define USE_DSHOT \ No newline at end of file diff --git a/src/main/target/AIRBOTF4/target.mk b/src/main/target/AIRBOTF4/target.mk index 4afa232072d..3c0b675157a 100644 --- a/src/main/target/AIRBOTF4/target.mk +++ b/src/main/target/AIRBOTF4/target.mk @@ -15,7 +15,5 @@ TARGET_SRC = \ drivers/compass/compass_ist8310.c \ drivers/compass/compass_ist8308.c \ drivers/compass/compass_lis3mdl.c \ - drivers/pitotmeter_ms4525.c \ drivers/pitotmeter_adc.c \ - drivers/light_ws2811strip.c \ - drivers/light_ws2811strip_stdperiph.c + drivers/light_ws2811strip.c diff --git a/src/main/target/AIRHEROF3/target.c b/src/main/target/AIRHEROF3/target.c index ce6b5a18da6..ae56e77391a 100755 --- a/src/main/target/AIRHEROF3/target.c +++ b/src/main/target/AIRHEROF3/target.c @@ -25,24 +25,28 @@ #include "drivers/bus.h" const timerHardware_t timerHardware[] = { - { TIM2, IO_TAG(PA0), TIM_Channel_1, 0, IOCFG_AF_PP, GPIO_AF_1, TIM_USE_PPM | TIM_USE_PWM }, // PWM1 - RC1 - { TIM2, IO_TAG(PA1), TIM_Channel_2, 0, IOCFG_AF_PP, GPIO_AF_1, TIM_USE_PWM }, // PWM2 - RC2 - { TIM2, IO_TAG(PA2), TIM_Channel_3, 0, IOCFG_AF_PP, GPIO_AF_1, TIM_USE_PWM }, // PWM3 - RC3 - { TIM2, IO_TAG(PA3), TIM_Channel_4, 0, IOCFG_AF_PP, GPIO_AF_1, TIM_USE_PWM }, // PWM4 - RC4 - { TIM3, IO_TAG(PA6), TIM_Channel_1, 0, IOCFG_AF_PP, GPIO_AF_2, TIM_USE_PWM }, // PWM5 - RC5 - { TIM3, IO_TAG(PA7), TIM_Channel_2, 0, IOCFG_AF_PP, GPIO_AF_2, TIM_USE_PWM }, // PWM6 - RC6 + DEF_TIM(TIM2, CH1, PA0, TIM_USE_PPM | TIM_USE_PWM, 0), + DEF_TIM(TIM2, CH2, PA1, TIM_USE_PWM, 0), + DEF_TIM(TIM2, CH3, PA2, TIM_USE_PWM, 0), + DEF_TIM(TIM2, CH4, PA3, TIM_USE_PWM, 0), + DEF_TIM(TIM3, CH1, PA6, TIM_USE_PWM | TIM_USE_ANY, 0), + DEF_TIM(TIM3, CH2, PA7, TIM_USE_PWM, 0), - //{ TIM3, IO_TAG(PB0), TIM_Channel_3, 0, IOCFG_AF_PP, GPIO_AF_2}, // PWM7 - not broken out - //{ TIM3, IO_TAG(PB1), TIM_Channel_4, 0, IOCFG_AF_PP, GPIO_AF_2}, // PWM8 - not broken out - - { TIM1, IO_TAG(PA8), TIM_Channel_1, 1, IOCFG_AF_PP, GPIO_AF_6, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR }, // PWM9 - OUT1 - { TIM1, IO_TAG(PA11), TIM_Channel_4, 1, IOCFG_AF_PP, GPIO_AF_11, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR }, // PWM10 - OUT2 - { TIM8, IO_TAG(PB6), TIM_Channel_1, 1, IOCFG_AF_PP, GPIO_AF_5, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // PWM11 - OUT3 - { TIM4, IO_TAG(PB7), TIM_Channel_2, 1, IOCFG_AF_PP, GPIO_AF_2, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO }, // PWM12 - OUT4 +#if 1 + DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0), // DMA1_CH2 + DEF_TIM(TIM1, CH4, PA11, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0), // DMA1_CH4 + DEF_TIM(TIM8, CH1, PB6, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // DMA2_CH3 + DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0), // DMA1_CH4 - conflict with TIM1_CH4 +#else + DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0), // DMA1_CH2 + DEF_TIM(TIM4, CH1, PA11, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // DMA1_CH1 + DEF_TIM(TIM8, CH1, PB6, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // DMA2_CH3 + DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // DMA1_CH4 +#endif #if !defined(AIRHEROF3_QUAD) - { TIM8, IO_TAG(PB8), TIM_Channel_2, 1, IOCFG_AF_PP, GPIO_AF_10, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // PWM13 - OUT5 - { TIM8, IO_TAG(PB9), TIM_Channel_3, 1, IOCFG_AF_PP, GPIO_AF_10, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO } // PWM14 - OUT6 + DEF_TIM(TIM8, CH2, PB8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // DMA2_CH5 + DEF_TIM(TIM8, CH3, PB9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // DMA2_CH1 #endif }; diff --git a/src/main/target/AIRHEROF3/target.h b/src/main/target/AIRHEROF3/target.h index 2007c0f7f0e..53f1a52acf6 100755 --- a/src/main/target/AIRHEROF3/target.h +++ b/src/main/target/AIRHEROF3/target.h @@ -85,11 +85,7 @@ /* #define USE_LED_STRIP -#define WS2811_TIMER TIM3 #define WS2811_PIN PA6 -#define WS2811_DMA_CHANNEL DMA1_Channel6 -#define WS2811_DMA_TC_FLAG DMA1_FLAG_TC6 -#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH6_HANDLER */ #define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_VBAT | FEATURE_SOFTSERIAL) diff --git a/src/main/target/AIRHEROF3/target.mk b/src/main/target/AIRHEROF3/target.mk index ae84a2c8aab..05892f19d09 100755 --- a/src/main/target/AIRHEROF3/target.mk +++ b/src/main/target/AIRHEROF3/target.mk @@ -13,7 +13,5 @@ TARGET_SRC = \ drivers/compass/compass_lis3mdl.c \ drivers/rangefinder/rangefinder_hcsr04.c \ drivers/light_ws2811strip.c \ - drivers/light_ws2811strip_stdperiph.c \ drivers/serial_softserial.c \ - drivers/pitotmeter_ms4525.c \ drivers/pitotmeter_adc.c diff --git a/src/main/target/ALIENFLIGHTF3/target.c b/src/main/target/ALIENFLIGHTF3/target.c index 05c8843e45a..5964e1318a4 100644 --- a/src/main/target/ALIENFLIGHTF3/target.c +++ b/src/main/target/ALIENFLIGHTF3/target.c @@ -24,19 +24,18 @@ const timerHardware_t timerHardware[] = { // up to 10 Motor Outputs - { TIM15, IO_TAG(PB15), TIM_Channel_2, 1, IOCFG_AF_PP, GPIO_AF_1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR }, // PWM1 - PB15 - TIM1_CH3N, TIM15_CH1N, *TIM15_CH2 - { TIM15, IO_TAG(PB14), TIM_Channel_1, 1, IOCFG_AF_PP, GPIO_AF_1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR }, // PWM2 - PB14 - TIM1_CH2N, *TIM15_CH1 - { TIM1, IO_TAG(PA8), TIM_Channel_1, 1, IOCFG_AF_PP, GPIO_AF_6, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // PWM3 - PA8 - *TIM1_CH1, TIM4_ETR - { TIM3, IO_TAG(PB0), TIM_Channel_3, 0, IOCFG_AF_PP, GPIO_AF_2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // PWM4 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N - { TIM3, IO_TAG(PA6), TIM_Channel_1, 0, IOCFG_AF_PP, GPIO_AF_2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // PWM5 - PA6 - *TIM3_CH1, TIM8_BKIN, TIM1_BKIN, TIM16_CH1 - { TIM2, IO_TAG(PA2), TIM_Channel_3, 0, IOCFG_AF_PP, GPIO_AF_1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // PWM6 - PA2 - *TIM2_CH3, !TIM15_CH1 - { TIM3, IO_TAG(PB1), TIM_Channel_4, 0, IOCFG_AF_PP, GPIO_AF_2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // PWM7 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N - { TIM17, IO_TAG(PA7), TIM_Channel_1, 1, IOCFG_AF_PP, GPIO_AF_1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // PWM8 - PA7 - !TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 - { TIM3, IO_TAG(PA4), TIM_Channel_2, 0, IOCFG_AF_PP, GPIO_AF_2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // PWM9 - PA4 - *TIM3_CH2 - { TIM2, IO_TAG(PA1), TIM_Channel_2, 0, IOCFG_AF_PP, GPIO_AF_1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // PWM10 - PA1 - *TIM2_CH2, TIM15_CH1N + DEF_TIM(TIM1, CH3N, PB15, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0), // PWM1 - PB15 - NONE + DEF_TIM(TIM15, CH1, PB14, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0), // PWM2 - PB14 - DMA1_CH5 + DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // PWM3 - PA8 - DMA1_CH2 + DEF_TIM(TIM8, CH2N, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // PWM4 - PB0 - DMA2_CH5 + DEF_TIM(TIM16, CH1, PA6, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // PWM5 - PA6 - DMA1_CH3 + DEF_TIM(TIM2, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // PWM6 - PA2 - DMA1_CH1 + DEF_TIM(TIM8, CH3N, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // PWM7 - PB1 - DMA2_CH1 + DEF_TIM(TIM17, CH1, PA7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO | TIM_USE_ANY, 0), // PWM8 - PA7 - DMA1_CH7 + DEF_TIM(TIM3, CH2, PA4, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // PWM9 - PA4 - DMA_NONE + DEF_TIM(TIM2, CH2, PA1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // PWM10 - PA1 - DMA1_CH7 - // PPM PORT - Also USART2 RX (AF5) - { TIM2, IO_TAG(PA3), TIM_Channel_4, 0, IOCFG_AF_PP_PD, GPIO_AF_1, TIM_USE_PPM } // PPM - PA3 - TIM2_CH4, TIM15_CH2 - PWM13 + DEF_TIM(TIM2, CH4, PA3, TIM_USE_PPM, 0), // PPM - PA3 - DMA1_CH7 }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/ALIENFLIGHTF3/target.h b/src/main/target/ALIENFLIGHTF3/target.h index ce2e3d7ae26..128364d1ebd 100644 --- a/src/main/target/ALIENFLIGHTF3/target.h +++ b/src/main/target/ALIENFLIGHTF3/target.h @@ -18,7 +18,6 @@ #pragma once #define TARGET_BOARD_IDENTIFIER "AFF3" // ALIENFLIGHT F3 -#define TARGET_CONFIG #define USE_HARDWARE_REVISION_DETECTION #define HW_PIN PB2 diff --git a/src/main/target/ALIENFLIGHTF4/target.c b/src/main/target/ALIENFLIGHTF4/target.c index 938544d2d31..9412799ab07 100644 --- a/src/main/target/ALIENFLIGHTF4/target.c +++ b/src/main/target/ALIENFLIGHTF4/target.c @@ -23,19 +23,19 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[] = { - { TIM1, IO_TAG(PA8), TIM_Channel_1, 0, IOCFG_IPD, GPIO_AF_TIM1, TIM_USE_PPM | TIM_USE_PWM }, // PWM1 - PA8 RC1 - { TIM3, IO_TAG(PB0), TIM_Channel_3, 0, IOCFG_IPD, GPIO_AF_TIM3, TIM_USE_PWM }, // PWM2 - PB0 RC2 - { TIM3, IO_TAG(PB1), TIM_Channel_4, 0, IOCFG_IPD, GPIO_AF_TIM3, TIM_USE_PWM }, // PWM3 - PB1 RC3 - { TIM1, IO_TAG(PB14), TIM_Channel_2, 0, IOCFG_IPD, GPIO_AF_TIM1, TIM_USE_PWM }, // PWM4 - PA14 RC4 - { TIM1, IO_TAG(PB15), TIM_Channel_3, 0, IOCFG_IPD, GPIO_AF_TIM1, TIM_USE_PWM }, // PWM5 - PA15 RC5 - { TIM4, IO_TAG(PB8), TIM_Channel_3, 1, IOCFG_AF_PP, GPIO_AF_TIM4, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR }, // PWM6 - PB8 OUT1 - { TIM4, IO_TAG(PB9), TIM_Channel_4, 1, IOCFG_AF_PP, GPIO_AF_TIM4, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR }, // PWM7 - PB9 OUT2 - { TIM5, IO_TAG(PA0), TIM_Channel_1, 1, IOCFG_AF_PP, GPIO_AF_TIM5, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // PWM8 - PA0 OUT3 - { TIM5, IO_TAG(PA1), TIM_Channel_2, 1, IOCFG_AF_PP, GPIO_AF_TIM5, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // PWM9 - PA1 OUT4 - { TIM8, IO_TAG(PC6), TIM_Channel_1, 1, IOCFG_AF_PP, GPIO_AF_TIM8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // PWM10 - PC6 OUT5 - { TIM8, IO_TAG(PC7), TIM_Channel_2, 1, IOCFG_AF_PP, GPIO_AF_TIM8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // PWM11 - PC7 OUT6 - { TIM8, IO_TAG(PC8), TIM_Channel_3, 1, IOCFG_AF_PP, GPIO_AF_TIM8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // PWM13 - PC8 OUT7 - { TIM8, IO_TAG(PC9), TIM_Channel_4, 1, IOCFG_AF_PP, GPIO_AF_TIM8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // PWM13 - PC9 OUT8 + DEF_TIM(TIM1, CH1, PA8, TIM_USE_PWM | TIM_USE_PPM, 0, 1), // PWM1 - PA8 RC1 - DMA2_ST1 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_PWM, 0, 0), // PWM2 - PB0 RC2 - DMA1_ST5 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_PWM, 0, 0), // PWM3 - PB1 RC3 - DMA1_ST7 + DEF_TIM(TIM1, CH2, PB14, TIM_USE_PWM, 0, 1), // PWM4 - PA14 RC4 - DMA2_ST2 + DEF_TIM(TIM1, CH3, PB15, TIM_USE_PWM | TIM_USE_LED, 0, 0), // PWM5 - PA15 RC5 - DMA2_ST6 + DEF_TIM(TIM4, CH3, PB8, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // PWM6 - PB8 OUT1 - DMA1_ST7 + DEF_TIM(TIM4, CH4, PB9, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // PWM7 - PB9 OUT2 - DMA_NONE + DEF_TIM(TIM5, CH1, PA0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // PWM8 - PA0 OUT3 - DMA1_ST2 + DEF_TIM(TIM5, CH2, PA1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // PWM9 - PA1 OUT4 - DMA1_ST4 + DEF_TIM(TIM8, CH1, PC6, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // PWM10 - PC6 OUT5 - DMA2_ST2 + DEF_TIM(TIM8, CH2, PC7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // PWM11 - PC7 OUT6 - DMA2_ST3 + DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), // PWM13 - PC8 OUT7 - DMA2_ST4 + DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // PWM13 - PC9 OUT8 - DMA2_ST7 }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/ALIENFLIGHTF4/target.h b/src/main/target/ALIENFLIGHTF4/target.h index 92afdbb512b..9bdb5a14393 100644 --- a/src/main/target/ALIENFLIGHTF4/target.h +++ b/src/main/target/ALIENFLIGHTF4/target.h @@ -17,7 +17,6 @@ #pragma once #define TARGET_BOARD_IDENTIFIER "AFF4" -#define TARGET_CONFIG #define USE_HARDWARE_REVISION_DETECTION #define HW_PIN PC13 @@ -70,31 +69,19 @@ #define MAG_MPU9250_ALIGN CW0_DEG +#define TEMPERATURE_I2C_BUS BUS_I2C1 + #define USE_BARO -#define BARO_I2C_BUS BUS_I2C1 +#define BARO_I2C_BUS BUS_I2C1 #define USE_BARO_MS5611 #define USE_BARO_BMP280 #define USE_SDCARD - +#define USE_SDCARD_SPI #define SDCARD_DETECT_INVERTED - -#define SDCARD_DETECT_PIN PB11 -#define SDCARD_DETECT_EXTI_LINE EXTI_Line10 -#define SDCARD_DETECT_EXTI_PIN_SOURCE EXTI_PinSource10 -#define SDCARD_DETECT_EXTI_PORT_SOURCE EXTI_PortSourceGPIOB -#define SDCARD_DETECT_EXTI_IRQn EXTI15_10_IRQn - -#define SDCARD_SPI_INSTANCE SPI2 -#define SDCARD_SPI_CS_PIN PB10 - -#define SDCARD_DMA_CHANNEL_TX DMA1_Stream4 -#define SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG DMA_FLAG_TCIF4 -#define SDCARD_DMA_CLK RCC_AHB1Periph_DMA1 -#define SDCARD_DMA_CHANNEL DMA_Channel_0 - -// Performance logging for SD card operations: -// #define AFATFS_USE_INTROSPECTIVE_LOGGING +#define SDCARD_DETECT_PIN PB11 +#define SDCARD_SPI_BUS BUS_SPI2 +#define SDCARD_CS_PIN PB10 //#define M25P16_CS_PIN SPI2_NSS_PIN //#define M25P16_SPI_BUS BUS_SPI2 @@ -139,6 +126,7 @@ #define SPI2_SCK_PIN PB13 #define SPI2_MISO_PIN PC2 #define SPI2_MOSI_PIN PC3 +#define SPI2_CLOCK_LEADING_EDGE #define SPI3_NSS_PIN PA15 #define SPI3_SCK_PIN PB3 @@ -169,12 +157,6 @@ #define USE_LED_STRIP // LED Strip can run off Pin 41 (PA8) of the ESC outputs. #define WS2811_PIN PA8 -#define WS2811_TIMER TIM1 -#define WS2811_DMA_HANDLER_IDENTIFER DMA2_ST1_HANDLER -#define WS2811_DMA_STREAM DMA2_Stream1 -#define WS2811_DMA_IT DMA_IT_TCIF1 -#define WS2811_DMA_CHANNEL DMA_Channel_6 -#define WS2811_TIMER_CHANNEL TIM_Channel_1 #define USE_SPEKTRUM_BIND // USART2, PA3 diff --git a/src/main/target/ALIENFLIGHTF4/target.mk b/src/main/target/ALIENFLIGHTF4/target.mk index ac039d95fcf..12cfd715478 100644 --- a/src/main/target/ALIENFLIGHTF4/target.mk +++ b/src/main/target/ALIENFLIGHTF4/target.mk @@ -13,5 +13,4 @@ TARGET_SRC = \ drivers/compass/compass_mag3110.c \ drivers/compass/compass_mpu9250.c \ drivers/compass/compass_lis3mdl.c \ - drivers/light_ws2811strip.c \ - drivers/light_ws2811strip_stdperiph.c + drivers/light_ws2811strip.c diff --git a/src/main/target/ALIENFLIGHTNGF7/target.c b/src/main/target/ALIENFLIGHTNGF7/target.c index c71704ddbf7..1a912077924 100644 --- a/src/main/target/ALIENFLIGHTNGF7/target.c +++ b/src/main/target/ALIENFLIGHTNGF7/target.c @@ -22,23 +22,20 @@ #include "drivers/pwm_mapping.h" #include "drivers/timer.h" -#define TIM_EN TIMER_OUTPUT_ENABLED -#define TIM_EN_N TIMER_OUTPUT_ENABLED | TIMER_OUTPUT_N_CHANNEL - const timerHardware_t timerHardware[] = { - { TIM1, IO_TAG(PA8), TIM_CHANNEL_1, 0, IOCFG_AF_PP_PD, GPIO_AF1_TIM1, TIM_USE_PPM | TIM_USE_PWM | TIM_USE_LED }, // PWM1 - PA8 RC1 - { TIM3, IO_TAG(PB1), TIM_CHANNEL_4, 0, IOCFG_AF_PP_PD, GPIO_AF2_TIM3, TIM_USE_PWM | TIM_USE_MC_SERVO }, // PWM2 - PB1 RC2 - { TIM12, IO_TAG(PB15), TIM_CHANNEL_2, 0, IOCFG_AF_PP_PD, GPIO_AF9_TIM12, TIM_USE_PWM | TIM_USE_MC_SERVO }, // PWM3 - PA15 RC3 - { TIM4, IO_TAG(PB8), TIM_CHANNEL_3, 0, IOCFG_AF_PP_PD, GPIO_AF2_TIM4, TIM_USE_PWM | TIM_USE_MC_SERVO }, // PWM4 - PB8 RC4 - { TIM4, IO_TAG(PB9), TIM_CHANNEL_4, 0, IOCFG_AF_PP_PD, GPIO_AF2_TIM4, TIM_USE_PWM | TIM_USE_MC_SERVO }, // PWM5 - PB9 RC5 - { TIM8, IO_TAG(PC6), TIM_CHANNEL_1, TIM_EN, IOCFG_AF_PP_PD, GPIO_AF3_TIM8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // PWM6 - PC6 OUT1 - { TIM8, IO_TAG(PC7), TIM_CHANNEL_2, TIM_EN, IOCFG_AF_PP_PD, GPIO_AF3_TIM8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // PWM7 - PC7 OUT2 - { TIM12, IO_TAG(PB14), TIM_CHANNEL_1, TIM_EN, IOCFG_AF_PP_PD, GPIO_AF9_TIM12, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // PWM9 - PA14 OUT3 - { TIM3, IO_TAG(PB0), TIM_CHANNEL_3, TIM_EN, IOCFG_AF_PP_PD, GPIO_AF2_TIM3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // PWM10 - PB0 OUT4 - { TIM5, IO_TAG(PA0), TIM_CHANNEL_1, TIM_EN, IOCFG_AF_PP_PD, GPIO_AF2_TIM5, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // PWM11 - PA0 OUT5 - { TIM5, IO_TAG(PA1), TIM_CHANNEL_2, TIM_EN, IOCFG_AF_PP_PD, GPIO_AF2_TIM5, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // PWM12 - PA1 OUT6 - { TIM8, IO_TAG(PC8), TIM_CHANNEL_3, TIM_EN, IOCFG_AF_PP_PD, GPIO_AF3_TIM8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // PWM13 - PC8 OUT7 - { TIM8, IO_TAG(PC9), TIM_CHANNEL_4, TIM_EN, IOCFG_AF_PP_PD, GPIO_AF3_TIM8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // PWM14 - PC9 OUT8 + DEF_TIM(TIM1, CH1, PA8, TIM_USE_PPM | TIM_USE_LED, 0, 1), // PPM - DMA2_ST1 + DEF_TIM(TIM8, CH1, PC6, TIM_USE_MC_SERVO, 0, 0), // PWM1 - DMA2_ST2 + DEF_TIM(TIM3, CH2, PC7, TIM_USE_MC_SERVO, 0, 0), // PWM2 - DMA1_ST5 + DEF_TIM(TIM8, CH2N, PB14, TIM_USE_MC_SERVO, 0, 0), // PWM3 - DMA2_ST3 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_SERVO, 0, 0), // PWM4 - DMA1_ST7 + DEF_TIM(TIM5, CH1, PA0, TIM_USE_MC_SERVO, 0, 0), // PWM5 - DMA1_ST2 + DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // PWM6 - DMA2_ST4 + DEF_TIM(TIM5, CH2, PA1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // PWM7 - DMA1_ST4 + DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // PWM8 - DMA2_ST7 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // PWM9 - DMA1_ST2 + DEF_TIM(TIM1, CH3N, PB15, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // PWM10 - DMA2_ST6 + DEF_TIM(TIM4, CH3, PB8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // PWM11 - DMA1_ST7 + DEF_TIM(TIM4, CH4, PB9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // PWM12 - DMA_NONE }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/ALIENFLIGHTNGF7/target.h b/src/main/target/ALIENFLIGHTNGF7/target.h index 918b4971de9..8eef1ed8ebb 100644 --- a/src/main/target/ALIENFLIGHTNGF7/target.h +++ b/src/main/target/ALIENFLIGHTNGF7/target.h @@ -17,7 +17,6 @@ #pragma once #define TARGET_BOARD_IDENTIFIER "AFF7" -#define TARGET_CONFIG #define USE_HARDWARE_REVISION_DETECTION #define HW_PIN PC13 @@ -72,6 +71,8 @@ #define AK8963_CS_PIN PC15 #define AK8963_SPI_BUS BUS_SPI3 +#define TEMPERATURE_I2C_BUS BUS_I2C1 + #define USE_BARO #define BARO_I2C_BUS BUS_I2C1 #define USE_BARO_MS5611 @@ -84,28 +85,14 @@ #define BMP280_SPI_BUS BUS_SPI3 #define USE_SDCARD - +#define USE_SDCARD_SPI #define SDCARD_DETECT_INVERTED +#define SDCARD_DETECT_PIN PB11 +#define SDCARD_SPI_BUS BUS_SPI2 +#define SDCARD_CS_PIN PB10 -#define SDCARD_DETECT_PIN PB11 -#define SDCARD_DETECT_EXTI_LINE EXTI_Line10 -#define SDCARD_DETECT_EXTI_PIN_SOURCE EXTI_PinSource10 -#define SDCARD_DETECT_EXTI_PORT_SOURCE EXTI_PortSourceGPIOB -#define SDCARD_DETECT_EXTI_IRQn EXTI15_10_IRQn - -#define SDCARD_SPI_INSTANCE SPI2 -#define SDCARD_SPI_CS_PIN PB10 - -#define SDCARD_DMA_CHANNEL_TX DMA1_Stream4 -#define SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG DMA_FLAG_TCIF4 -#define SDCARD_DMA_CLK RCC_AHB1Periph_DMA1 -#define SDCARD_DMA_CHANNEL DMA_CHANNEL_0 - -// Performance logging for SD card operations: -// #define AFATFS_USE_INTROSPECTIVE_LOGGING - -#define M25P16_CS_PIN SPI2_NSS_PIN -#define M25P16_SPI_BUS BUS_SPI2 +#define M25P16_CS_PIN SPI2_NSS_PIN +#define M25P16_SPI_BUS BUS_SPI2 #define USE_FLASHFS #define USE_FLASH_M25P16 @@ -147,6 +134,7 @@ #define SPI2_SCK_PIN PB13 #define SPI2_MISO_PIN PC2 #define SPI2_MOSI_PIN PC3 +#define SPI2_CLOCK_LEADING_EDGE #define SPI3_NSS_PIN PA15 #define SPI3_SCK_PIN PB3 @@ -183,13 +171,6 @@ #define USE_LED_STRIP // LED Strip can run off Pin 41 (PA8) of the ESC outputs. #define WS2811_PIN PA8 -#define WS2811_TIMER TIM1 -#define WS2811_DMA_HANDLER_IDENTIFER DMA2_ST1_HANDLER -#define WS2811_DMA_STREAM DMA2_Stream1 -#define WS2811_DMA_IT DMA_IT_TCIF1 -#define WS2811_DMA_CHANNEL DMA_CHANNEL_6 -#define WS2811_TIMER_CHANNEL TIM_CHANNEL_1 -#define WS2811_TIMER_GPIO_AF GPIO_AF1_TIM1 #define USE_SPEKTRUM_BIND // USART2, PA3 diff --git a/src/main/target/ALIENFLIGHTNGF7/target.mk b/src/main/target/ALIENFLIGHTNGF7/target.mk index db6cba5ba96..4beb04ddd82 100644 --- a/src/main/target/ALIENFLIGHTNGF7/target.mk +++ b/src/main/target/ALIENFLIGHTNGF7/target.mk @@ -15,5 +15,4 @@ TARGET_SRC = \ drivers/compass/compass_ist8308.c \ drivers/compass/compass_lis3mdl.c \ drivers/max7456.c \ - drivers/light_ws2811strip.c \ - drivers/light_ws2811strip_hal.c + drivers/light_ws2811strip.c diff --git a/src/main/target/ANYFC/target.c b/src/main/target/ANYFC/target.c index 8bc7c8cc4e6..02ccf460825 100644 --- a/src/main/target/ANYFC/target.c +++ b/src/main/target/ANYFC/target.c @@ -23,23 +23,23 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[] = { - { TIM12, IO_TAG(PB14), TIM_Channel_1, 0, IOCFG_AF_PP_PD, GPIO_AF_TIM12, TIM_USE_PPM | TIM_USE_PWM }, // S1_IN - { TIM12, IO_TAG(PB15), TIM_Channel_2, 0, IOCFG_AF_PP_PD, GPIO_AF_TIM12, TIM_USE_PWM }, // S2_IN - { TIM8, IO_TAG(PC6), TIM_Channel_1, 0, IOCFG_AF_PP_PD, GPIO_AF_TIM8, TIM_USE_PWM }, // S3_IN - { TIM8, IO_TAG(PC7), TIM_Channel_2, 0, IOCFG_AF_PP_PD, GPIO_AF_TIM8, TIM_USE_PWM }, // S4_IN - { TIM8, IO_TAG(PC9), TIM_Channel_4, 0, IOCFG_AF_PP_PD, GPIO_AF_TIM8, TIM_USE_PWM }, // S5_IN - { TIM8, IO_TAG(PC8), TIM_Channel_3, 0, IOCFG_AF_PP_PD, GPIO_AF_TIM8, TIM_USE_PWM }, // S6_IN + DEF_TIM(TIM12, CH1, PB14, TIM_USE_PWM | TIM_USE_PPM, 0, 0), // S1_IN + DEF_TIM(TIM12, CH2, PB15, TIM_USE_PWM, 0, 0), // S2_IN + DEF_TIM(TIM8, CH1, PC6, TIM_USE_PWM, 0, 0), // S3_IN + DEF_TIM(TIM8, CH2, PC7, TIM_USE_PWM, 0, 0), // S4_IN + DEF_TIM(TIM8, CH4, PC9, TIM_USE_PWM, 0, 0), // S6_IN + DEF_TIM(TIM8, CH3, PC8, TIM_USE_PWM, 0, 0), // S5_IN - { TIM2, IO_TAG(PB3), TIM_Channel_2, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // S10_OUT 16 - { TIM5, IO_TAG(PA0), TIM_Channel_1, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM5, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // S6_OUT 12 - { TIM5, IO_TAG(PA1), TIM_Channel_2, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM5, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // S5_OUT 11 - { TIM3, IO_TAG(PB1), TIM_Channel_4, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM3, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR }, // S1_OUT 7 - { TIM3, IO_TAG(PB0), TIM_Channel_3, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM3, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR }, // S2_OUT 8 - { TIM2, IO_TAG(PA3), TIM_Channel_4, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // S3_OUT 9 - { TIM2, IO_TAG(PA2), TIM_Channel_3, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // S4_OUT 10 - { TIM2, IO_TAG(PA15), TIM_Channel_1, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // S7_OUT 13 - { TIM3, IO_TAG(PB5), TIM_Channel_2, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // S8_OUT 14 - { TIM3, IO_TAG(PB4), TIM_Channel_1, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // S9_OUT 15 + DEF_TIM(TIM2, CH2, PB3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S10_OUT 16 + DEF_TIM(TIM5, CH1, PA0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S6_OUT 12 + DEF_TIM(TIM5, CH2, PA1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S5_OUT 11 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1_OUT 7 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S2_OUT 8 + DEF_TIM(TIM2, CH4, PA3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S3_OUT 9 + DEF_TIM(TIM2, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4_OUT 10 + DEF_TIM(TIM2, CH1, PA15, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S7_OUT 13 + DEF_TIM(TIM3, CH2, PB5, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S8_OUT 14 + DEF_TIM(TIM3, CH1, PB4, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S9_OUT 15 }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/ANYFC/target.h b/src/main/target/ANYFC/target.h index f6fc3aa695e..1cf7698c36a 100644 --- a/src/main/target/ANYFC/target.h +++ b/src/main/target/ANYFC/target.h @@ -54,15 +54,15 @@ #define USE_MAG_LIS3MDL #define MAG_HMC5883_ALIGN CW270_DEG_FLIP +#define TEMPERATURE_I2C_BUS BUS_I2C2 + #define USE_RANGEFINDER -#define USE_RANGEFINDER_VL53L0X -#define VL53L0X_I2C_BUS BUS_I2C2 +#define RANGEFINDER_I2C_BUS BUS_I2C2 #define USE_BARO #define BARO_I2C_BUS BUS_I2C1 #define USE_BARO_MS5611 -#define USE_PITOT_MS4525 #define PITOT_I2C_BUS BUS_I2C2 #define USE_VCP @@ -119,16 +119,7 @@ #define RSSI_ADC_CHANNEL ADC_CHN_3 #define USE_LED_STRIP -// LED Strip can run off Pin 6 (PA0) of the ESC outputs. #define WS2811_PIN PA0 -#define WS2811_TIMER TIM5 -#define WS2811_TIMER_CHANNEL TIM_Channel_1 -#define WS2811_DMA_HANDLER_IDENTIFER DMA1_ST2_HANDLER -#define WS2811_DMA_STREAM DMA1_Stream2 -#define WS2811_DMA_FLAG DMA_FLAG_TCIF2 -#define WS2811_DMA_IT DMA_IT_TCIF2 -#define WS2811_DMA_CHANNEL DMA_Channel_6 -#define WS2811_DMA_IRQ DMA1_Stream2_IRQn #define SENSORS_SET (SENSOR_ACC|SENSOR_MAG|SENSOR_BARO) @@ -144,4 +135,4 @@ #define TARGET_IO_PORTC 0xffff #define TARGET_IO_PORTD 0xffff -#define PCA9685_I2C_BUS BUS_I2C2 \ No newline at end of file +#define PCA9685_I2C_BUS BUS_I2C2 diff --git a/src/main/target/ANYFC/target.mk b/src/main/target/ANYFC/target.mk index e9d3377e755..b36cf4f4c5c 100644 --- a/src/main/target/ANYFC/target.mk +++ b/src/main/target/ANYFC/target.mk @@ -11,6 +11,4 @@ TARGET_SRC = \ drivers/compass/compass_ist8308.c \ drivers/compass/compass_lis3mdl.c \ drivers/light_ws2811strip.c \ - drivers/light_ws2811strip_stdperiph.c \ - drivers/pitotmeter_ms4525.c diff --git a/src/main/target/ANYFCF7/target.c b/src/main/target/ANYFCF7/target.c index 07acf8c341f..1a06c3b3eda 100644 --- a/src/main/target/ANYFCF7/target.c +++ b/src/main/target/ANYFCF7/target.c @@ -23,44 +23,23 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[] = { - { TIM12, IO_TAG(PB14), TIM_CHANNEL_1, 0, IOCFG_AF_PP_PD, GPIO_AF9_TIM12, TIM_USE_PPM | TIM_USE_PWM }, // S1_IN 1 - { TIM12, IO_TAG(PB15), TIM_CHANNEL_2, 0, IOCFG_AF_PP_PD, GPIO_AF9_TIM12, TIM_USE_PWM }, // S2_IN 2 - { TIM8, IO_TAG(PC6), TIM_CHANNEL_1, 0, IOCFG_AF_PP_PD, GPIO_AF3_TIM8, TIM_USE_PWM | TIM_USE_MC_SERVO }, // S3_IN 3 - { TIM8, IO_TAG(PC7), TIM_CHANNEL_2, 0, IOCFG_AF_PP_PD, GPIO_AF3_TIM8, TIM_USE_PWM | TIM_USE_MC_SERVO }, // S4_IN 4 - { TIM8, IO_TAG(PC8), TIM_CHANNEL_3, 0, IOCFG_AF_PP_PD, GPIO_AF3_TIM8, TIM_USE_PWM | TIM_USE_MC_SERVO }, // S6_IN 6 - { TIM8, IO_TAG(PC9), TIM_CHANNEL_4, 0, IOCFG_AF_PP_PD, GPIO_AF3_TIM8, TIM_USE_PWM | TIM_USE_MC_SERVO }, // S5_IN 5 + DEF_TIM(TIM12, CH1, PB14, TIM_USE_PWM | TIM_USE_PPM, 0, 0 ), // S1_IN + DEF_TIM(TIM12, CH2, PB15, TIM_USE_PWM, 0, 0 ), // S2_IN + DEF_TIM(TIM8, CH1, PC6, TIM_USE_PWM, 0, 0 ), // S3_IN DMA2_ST2 DMA2_ST2 + DEF_TIM(TIM8, CH2, PC7, TIM_USE_PWM, 0, 0 ), // S4_IN DMA2_ST3 DMA2_ST2 + DEF_TIM(TIM8, CH3, PC8, TIM_USE_PWM, 0, 0 ), // S5_IN DMA2_ST4 DMA2_ST2 + DEF_TIM(TIM8, CH4, PC9, TIM_USE_PWM, 0, 0 ), // S6_IN DMA2_ST7 - { TIM4, IO_TAG(PB8), TIM_CHANNEL_3, 1, IOCFG_AF_PP_PD, GPIO_AF2_TIM4, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // S10_OUT16 - { TIM2, IO_TAG(PA2), TIM_CHANNEL_3, 1, IOCFG_AF_PP_PD, GPIO_AF1_TIM2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // S6_OUT 12 - { TIM4, IO_TAG(PB9), TIM_CHANNEL_4, 1, IOCFG_AF_PP_PD, GPIO_AF2_TIM4, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // S5_OUT 11 - { TIM2, IO_TAG(PA3), TIM_CHANNEL_4, 1, IOCFG_AF_PP_PD, GPIO_AF1_TIM2, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR }, // S1_OUT 7 - { TIM5, IO_TAG(PA1), TIM_CHANNEL_2, 1, IOCFG_AF_PP_PD, GPIO_AF2_TIM5, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR }, // S2_OUT 8 - { TIM9, IO_TAG(PE6), TIM_CHANNEL_2, 1, IOCFG_AF_PP_PD, GPIO_AF3_TIM9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // S3_OUT 9 - { TIM3, IO_TAG(PB5), TIM_CHANNEL_2, 1, IOCFG_AF_PP_PD, GPIO_AF2_TIM3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // S4_OUT 10 - { TIM5, IO_TAG(PA0), TIM_CHANNEL_1, 1, IOCFG_AF_PP_PD, GPIO_AF2_TIM5, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // S7_OUT 13 - { TIM2, IO_TAG(PB3), TIM_CHANNEL_2, 1, IOCFG_AF_PP_PD, GPIO_AF1_TIM2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // S8_OUT 14 - { TIM3, IO_TAG(PB4), TIM_CHANNEL_1, 1, IOCFG_AF_PP_PD, GPIO_AF2_TIM3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // S9_OUT 15 + DEF_TIM(TIM4, CH3, PB8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), // S10_OUT 1 DMA1_ST7 + DEF_TIM(TIM5, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), // S6_OUT 2 DMA1_ST0 + DEF_TIM(TIM4, CH4, PB9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), // S5_OUT + DEF_TIM(TIM5, CH4, PA3, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0 ), // S1_OUT 4 DMA1_ST1 DMA1_ST3 + DEF_TIM(TIM5, CH2, PA1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0 ), // S2_OUT 3 DMA1_ST4 + DEF_TIM(TIM9, CH2, PE6, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), // S3_OUT + DEF_TIM(TIM3, CH2, PB5, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), // S4_OUT DMA1_ST5 + DEF_TIM(TIM5, CH1, PA0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), // S7_OUT DMA1_ST2 + DEF_TIM(TIM2, CH2, PB3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO | TIM_USE_LED, 0, 0 ), // S8_OUT DMA1_ST6 + DEF_TIM(TIM3, CH1, PB4, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), // S9_OUT DMA1_ST4 }; -// ALTERNATE LAYOUT -//const timerHardware_t timerHardware[] = { -// { TIM12, IO_TAG(PB14), TIM_CHANNEL_1, TIM8_BRK_TIM12_IRQn, 0, IOCFG_AF_PP_PD, GPIO_AF9_TIM12 }, // S1_IN -// { TIM12, IO_TAG(PB15), TIM_CHANNEL_2, TIM8_BRK_TIM12_IRQn, 0, IOCFG_AF_PP_PD, GPIO_AF9_TIM12 }, // S2_IN -// { TIM8, IO_TAG(PC6), TIM_CHANNEL_1, TIM8_CC_IRQn, 0, IOCFG_AF_PP_PD, GPIO_AF3_TIM8 }, // S3_IN -// { TIM8, IO_TAG(PC7), TIM_CHANNEL_2, TIM8_CC_IRQn, 0, IOCFG_AF_PP_PD, GPIO_AF3_TIM8 }, // S4_IN -// { TIM8, IO_TAG(PC9), TIM_CHANNEL_4, TIM8_CC_IRQn, 0, IOCFG_AF_PP_PD, GPIO_AF3_TIM8 }, // S5_IN -// { TIM8, IO_TAG(PC8), TIM_CHANNEL_3, TIM8_CC_IRQn, 0, IOCFG_AF_PP_PD, GPIO_AF3_TIM8 }, // S6_IN -// -// { TIM2, IO_TAG(PA3), TIM_CHANNEL_4, TIM2_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF1_TIM2 }, // S1_OUT -// { TIM5, IO_TAG(PA1), TIM_CHANNEL_2, TIM5_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF2_TIM5 }, // S2_OUT -// { TIM9, IO_TAG(PE6), TIM_CHANNEL_2, TIM1_BRK_TIM9_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF3_TIM9 }, // S3_OUT -// { TIM3, IO_TAG(PB5), TIM_CHANNEL_2, TIM3_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF2_TIM3 }, // S4_OUT -// { TIM11, IO_TAG(PB9), TIM_CHANNEL_1, TIM1_TRG_COM_TIM11_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF3_TIM11 }, // S5_OUT -// { TIM9, IO_TAG(PA2), TIM_CHANNEL_1, TIM1_BRK_TIM9_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF3_TIM9 }, // S6_OUT -// { TIM5, IO_TAG(PA0), TIM_CHANNEL_1, TIM5_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF2_TIM5 }, // S7_OUT -// { TIM2, IO_TAG(PB3), TIM_CHANNEL_2, TIM2_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF1_TIM2 }, // S8_OUT -// { TIM3, IO_TAG(PB4), TIM_CHANNEL_1, TIM3_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF2_TIM3 }, // S9_OUT -// { TIM10, IO_TAG(PB8), TIM_CHANNEL_1, TIM1_UP_TIM10_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF3_TIM10 }, // S10_OUT -//}; - const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/ANYFCF7/target.h b/src/main/target/ANYFCF7/target.h index dc981bda7cc..b174105df9c 100644 --- a/src/main/target/ANYFCF7/target.h +++ b/src/main/target/ANYFCF7/target.h @@ -53,6 +53,8 @@ #define USE_MAG_MAG3110 #define USE_MAG_LIS3MDL +#define TEMPERATURE_I2C_BUS BUS_I2C2 + #define USE_BARO #ifdef ANYFCF7_EXTERNAL_BARO @@ -65,7 +67,6 @@ #define USE_BARO_MS5611 #endif -#define USE_PITOT_MS4525 #define PITOT_I2C_BUS BUS_I2C2 #define USE_RANGEFINDER @@ -128,6 +129,7 @@ #define SPI4_SCK_PIN PE12 #define SPI4_MISO_PIN PE13 #define SPI4_MOSI_PIN PE14 +#define SPI4_CLOCK_LEADING_EDGE #define USE_OSD #define USE_MAX7456 @@ -135,30 +137,15 @@ #define MAX7456_CS_PIN SPI3_NSS_PIN #define USE_SDCARD - -// This is needed for BangGood board that used wrong sdcard socket!!! +#define USE_SDCARD_SPI #define SDCARD_DETECT_INVERTED - -#define SDCARD_DETECT_PIN PD3 -#define SDCARD_DETECT_EXTI_LINE EXTI_Line3 -#define SDCARD_DETECT_EXTI_PIN_SOURCE EXTI_PinSource3 -#define SDCARD_DETECT_EXTI_PORT_SOURCE EXTI_PortSourceGPIOD -#define SDCARD_DETECT_EXTI_IRQn EXTI3_IRQn - -#define SDCARD_SPI_INSTANCE SPI4 -#define SDCARD_SPI_CS_PIN SPI4_NSS_PIN - -#define SDCARD_DMA_CHANNEL_TX DMA2_Stream1 -#define SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG DMA_FLAG_TCIF1_5 -#define SDCARD_DMA_CLK RCC_AHB1Periph_DMA2 -#define SDCARD_DMA_CHANNEL DMA_CHANNEL_4 +#define SDCARD_DETECT_PIN PD3 +#define SDCARD_SPI_BUS BUS_SPI4 +#define SDCARD_CS_PIN SPI4_NSS_PIN #define USE_I2C #define USE_I2C_DEVICE_4 #define USE_I2C_DEVICE_2 -//#define USE_I2C_PULLUP - -//#define HIL #define MAG_GPS_ALIGN CW180_DEG_FLIP @@ -177,18 +164,7 @@ #define RSSI_ADC_CHANNEL ADC_CHN_3 #define USE_LED_STRIP - -// LED Strip can run off Pin 6 (PA0) of the ESC outputs. #define WS2811_PIN PA1 -#define WS2811_TIMER TIM5 -#define WS2811_TIMER_CHANNEL TIM_CHANNEL_2 -#define WS2811_DMA_HANDLER_IDENTIFER DMA1_ST4_HANDLER -#define WS2811_DMA_STREAM DMA1_Stream4 -#define WS2811_DMA_FLAG DMA_FLAG_TCIF4 -#define WS2811_DMA_IT DMA_IT_TCIF4 -#define WS2811_DMA_CHANNEL DMA_CHANNEL_6 -#define WS2811_DMA_IRQ DMA1_Stream4_IRQn -#define WS2811_TIMER_GPIO_AF GPIO_AF2_TIM5 #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT @@ -207,4 +183,4 @@ #define TARGET_IO_PORTD 0xffff #define TARGET_IO_PORTE 0xffff -#define PCA9685_I2C_BUS BUS_I2C2 \ No newline at end of file +#define PCA9685_I2C_BUS BUS_I2C2 diff --git a/src/main/target/ANYFCF7/target.mk b/src/main/target/ANYFCF7/target.mk index 203f4ae8164..9d26cbbb2bd 100644 --- a/src/main/target/ANYFCF7/target.mk +++ b/src/main/target/ANYFCF7/target.mk @@ -14,7 +14,5 @@ TARGET_SRC = \ drivers/compass/compass_mag3110.c \ drivers/compass/compass_lis3mdl.c \ drivers/light_ws2811strip.c \ - drivers/light_ws2811strip_hal.c \ - drivers/pitotmeter_ms4525.c \ drivers/max7456.c diff --git a/src/main/target/ANYFCM7/target.c b/src/main/target/ANYFCM7/target.c index 202869bfbd8..d31dedbf18d 100644 --- a/src/main/target/ANYFCM7/target.c +++ b/src/main/target/ANYFCM7/target.c @@ -26,23 +26,23 @@ #define TIM_EN_N TIMER_OUTPUT_ENABLED | TIMER_OUTPUT_N_CHANNEL const timerHardware_t timerHardware[] = { - { TIM12, IO_TAG(PB14), TIM_CHANNEL_1, 0, IOCFG_AF_PP_PD, GPIO_AF9_TIM12, TIM_USE_PWM | TIM_USE_PPM }, // S1_IN - { TIM12, IO_TAG(PB15), TIM_CHANNEL_2, 0, IOCFG_AF_PP_PD, GPIO_AF9_TIM12, TIM_USE_PWM }, // S2_IN - { TIM8, IO_TAG(PC6), TIM_CHANNEL_1, 0, IOCFG_AF_PP_PD, GPIO_AF3_TIM8, TIM_USE_PWM }, // S3_IN - { TIM8, IO_TAG(PC7), TIM_CHANNEL_2, 0, IOCFG_AF_PP_PD, GPIO_AF3_TIM8, TIM_USE_PWM }, // S4_IN - { TIM8, IO_TAG(PC8), TIM_CHANNEL_3, 0, IOCFG_AF_PP_PD, GPIO_AF3_TIM8, TIM_USE_PWM }, // S5_IN - { TIM8, IO_TAG(PC9), TIM_CHANNEL_4, 0, IOCFG_AF_PP_PD, GPIO_AF3_TIM8, TIM_USE_PWM }, // S6_IN + DEF_TIM(TIM12, CH1, PB14, TIM_USE_PWM | TIM_USE_PPM, 0, 0 ), // S1_IN + DEF_TIM(TIM12, CH2, PB15, TIM_USE_PWM, 0, 0 ), // S2_IN + DEF_TIM(TIM8, CH1, PC6, TIM_USE_PWM, 0, 0 ), // S3_IN DMA2_ST2 DMA2_ST2 + DEF_TIM(TIM8, CH2, PC7, TIM_USE_PWM, 0, 0 ), // S4_IN DMA2_ST3 DMA2_ST2 + DEF_TIM(TIM8, CH3, PC8, TIM_USE_PWM, 0, 0 ), // S5_IN DMA2_ST4 DMA2_ST2 + DEF_TIM(TIM8, CH4, PC9, TIM_USE_PWM, 0, 0 ), // S6_IN DMA2_ST7 - { TIM4, IO_TAG(PB8), TIM_CHANNEL_3, TIM_EN, IOCFG_AF_PP_PD, GPIO_AF2_TIM4, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // S10_OUT PWM16 - { TIM2, IO_TAG(PA2), TIM_CHANNEL_3, TIM_EN, IOCFG_AF_PP_PD, GPIO_AF1_TIM2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // S6_OUT PWM12 - { TIM4, IO_TAG(PB7), TIM_CHANNEL_2, TIM_EN, IOCFG_AF_PP_PD, GPIO_AF2_TIM4, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // S5_OUT PWM11 - { TIM2, IO_TAG(PA3), TIM_CHANNEL_4, TIM_EN, IOCFG_AF_PP_PD, GPIO_AF1_TIM2, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR }, // S1_OUT PWM7 - { TIM5, IO_TAG(PA1), TIM_CHANNEL_2, TIM_EN, IOCFG_AF_PP_PD, GPIO_AF2_TIM5, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR }, // S2_OUT PWM8 - { TIM1, IO_TAG(PB0), TIM_CHANNEL_2, TIM_EN_N, IOCFG_AF_PP_PD, GPIO_AF1_TIM1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // S3_OUT PWM9 - { TIM3, IO_TAG(PB5), TIM_CHANNEL_2, TIM_EN, IOCFG_AF_PP_PD, GPIO_AF2_TIM3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // S4_OUT PWM10 - { TIM5, IO_TAG(PA0), TIM_CHANNEL_1, TIM_EN, IOCFG_AF_PP_PD, GPIO_AF2_TIM5, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // S7_OUT PWM13 - { TIM1, IO_TAG(PB1), TIM_CHANNEL_3, TIM_EN_N, IOCFG_AF_PP_PD, GPIO_AF1_TIM1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // S8_OUT PWM14 - { TIM3, IO_TAG(PB4), TIM_CHANNEL_1, TIM_EN, IOCFG_AF_PP_PD, GPIO_AF2_TIM3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // S9_OUT PWM15 + DEF_TIM(TIM4, CH3, PB8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), // S10_OUT 1 DMA1_ST7 + DEF_TIM(TIM2, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), // S6_OUT 2 DMA1_ST1 + DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), // S5_OUT 3 DMA1_ST3 + DEF_TIM(TIM2, CH4, PA3, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 1 ), // S1_OUT 4 DMA1_ST7 DMA1_ST6 + DEF_TIM(TIM5, CH2, PA1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0 ), // S2_OUT DMA1_ST4 + DEF_TIM(TIM1, CH2N, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1 ), // S3_OUT DMA2_ST6 DMA2_ST2 + DEF_TIM(TIM3, CH2, PB5, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO | TIM_USE_LED, 0, 0 ), // S4_OUT DMA1_ST5 + DEF_TIM(TIM5, CH1, PA0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), // S7_OUT DMA1_ST2 + DEF_TIM(TIM1, CH3N, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), // S8_OUT DMA2_ST6 + DEF_TIM(TIM3, CH1, PB4, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), // S9_OUT DMA1_ST4 }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/ANYFCM7/target.h b/src/main/target/ANYFCM7/target.h index 4bcae331aeb..c591f5691b7 100644 --- a/src/main/target/ANYFCM7/target.h +++ b/src/main/target/ANYFCM7/target.h @@ -53,11 +53,12 @@ #define USE_MAG_MAG3110 #define USE_MAG_LIS3MDL +#define TEMPERATURE_I2C_BUS BUS_I2C2 + #define USE_BARO #define BARO_I2C_BUS BUS_I2C2 #define USE_BARO_MS5611 -#define USE_PITOT_MS4525 #define PITOT_I2C_BUS BUS_I2C2 #define USE_VCP diff --git a/src/main/target/ANYFCM7/target.mk b/src/main/target/ANYFCM7/target.mk index b8c1fb61dea..371bcd26da1 100644 --- a/src/main/target/ANYFCM7/target.mk +++ b/src/main/target/ANYFCM7/target.mk @@ -11,6 +11,4 @@ TARGET_SRC = \ drivers/compass/compass_mag3110.c \ drivers/compass/compass_lis3mdl.c \ drivers/light_ws2811strip.c \ - drivers/light_ws2811strip_hal.c \ - drivers/pitotmeter_ms4525.c \ drivers/max7456.c diff --git a/src/main/target/ASGARD32F4/config.c b/src/main/target/ASGARD32F4/config.c index 4094cee1714..8929c9ce7b4 100644 --- a/src/main/target/ASGARD32F4/config.c +++ b/src/main/target/ASGARD32F4/config.c @@ -25,7 +25,6 @@ #include #include -#ifdef TARGET_CONFIG #include "drivers/io.h" #include "drivers/pwm_output.h" #include "flight/mixer.h" @@ -40,4 +39,3 @@ void targetConfiguration(void) motorConfigMutable()->minthrottle = 1075; motorConfigMutable()->maxthrottle = 1950; } -#endif diff --git a/src/main/target/ASGARD32F4/target.c b/src/main/target/ASGARD32F4/target.c index 82733b2a17b..50d209b27df 100644 --- a/src/main/target/ASGARD32F4/target.c +++ b/src/main/target/ASGARD32F4/target.c @@ -30,26 +30,17 @@ #include "drivers/timer.h" #include "drivers/bus.h" -#define DEF_TIM_CHNL_CH1 TIM_Channel_1 -#define DEF_TIM_CHNL_CH2 TIM_Channel_2 -#define DEF_TIM_CHNL_CH3 TIM_Channel_3 -#define DEF_TIM_CHNL_CH4 TIM_Channel_4 - -#define DEF_TIM(_tim, _ch, _pin, _usage, _flags) \ - { _tim, IO_TAG(_pin), DEF_TIM_CHNL_##_ch, _flags, IOCFG_AF_PP, GPIO_AF_##_tim, _usage } - - const timerHardware_t timerHardware[] = { - // DEF_TIM(TIM1, CH1, PA8, TIM_USE_PPM, 0), // PPM - timer clash with SS1_TX + // DEF_TIM(TIM1, CH1, PA8, TIM_USE_PPM, 0, 0), // PPM - timer clash with SS1_TX - DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR, 1), // M1 - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR, 1), // M2 - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR, 1), // M3 - DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR, 1), // M4 + DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR, 1, 1), // M1 - D(2, 4, 7) + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR, 1, 0), // M2 - D(1, 7, 5) + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR, 1, 0), // M3 - D(1, 2, 5) + DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR, 1, 0), // M4 - D(1, 3, 2) // DEF_TIM(TIM4, CH1, PB6, TIM_USE_LED, 0), // LED strip - timer clash with M4 output - DEF_TIM(TIM1, CH2, PA9, TIM_USE_ANY, 0), // SS1 + DEF_TIM(TIM1, CH2, PA9, TIM_USE_ANY, 0, 0), // SS1 - D(2, 6, 0) }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/ASGARD32F4/target.h b/src/main/target/ASGARD32F4/target.h index ab5081f129e..517db8afeab 100644 --- a/src/main/target/ASGARD32F4/target.h +++ b/src/main/target/ASGARD32F4/target.h @@ -152,19 +152,15 @@ #undef USE_RX_PWM #undef USE_RX_PPM -// Pitot not supported -#undef USE_PITOT - // Set default UARTs #define TELEMETRY_UART SERIAL_PORT_SOFTSERIAL1 #define SERIALRX_UART SERIAL_PORT_USART1 #define SMARTAUDIO_UART SERIAL_PORT_USART2 -#define TARGET_CONFIG - #define USE_SERIAL_4WAY_BLHELI_INTERFACE // Number of available PWM outputs +#define USE_DSHOT #define MAX_PWM_OUTPUT_PORTS 4 #define TARGET_MOTOR_COUNT 4 @@ -173,4 +169,6 @@ #define TARGET_IO_PORTC 0xffff #define TARGET_IO_PORTD 0xffff -#define PCA9685_I2C_BUS BUS_I2C2 \ No newline at end of file +#define PITOT_I2C_BUS BUS_I2C2 +#define PCA9685_I2C_BUS BUS_I2C2 +#define TEMPERATURE_I2C_BUS BUS_I2C2 diff --git a/src/main/target/ASGARD32F4/target.mk b/src/main/target/ASGARD32F4/target.mk index 62e119d7ff0..62ba0215c8d 100644 --- a/src/main/target/ASGARD32F4/target.mk +++ b/src/main/target/ASGARD32F4/target.mk @@ -2,8 +2,8 @@ F405_TARGETS += $(TARGET) FEATURES += VCP ONBOARDFLASH TARGET_SRC = \ - drivers/accgyro/accgyro_mpu6000.c \ - drivers/accgyro/accgyro_mpu6500.c \ + drivers/accgyro/accgyro_mpu6000.c \ + drivers/accgyro/accgyro_mpu6500.c \ drivers/barometer/barometer_bmp280.c \ drivers/barometer/barometer_bmp085.c \ drivers/barometer/barometer_ms56xx.c \ @@ -12,8 +12,6 @@ TARGET_SRC = \ drivers/compass/compass_ist8310.c \ drivers/compass/compass_mag3110.c \ drivers/compass/compass_lis3mdl.c \ - drivers/pitotmeter_ms4525.c \ drivers/pitotmeter_adc.c \ drivers/light_ws2811strip.c \ - drivers/light_ws2811strip_stdperiph.c \ drivers/max7456.c diff --git a/src/main/target/ASGARD32F7/config.c b/src/main/target/ASGARD32F7/config.c new file mode 100644 index 00000000000..8929c9ce7b4 --- /dev/null +++ b/src/main/target/ASGARD32F7/config.c @@ -0,0 +1,41 @@ +/* + * This file is part of INAV. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute and/or modify + * it under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or (at your + * option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see http://www.gnu.org/licenses/. + */ + +#include +#include + +#include "drivers/io.h" +#include "drivers/pwm_output.h" +#include "flight/mixer.h" +#include "rx/rx.h" +#include "io/serial.h" + +void targetConfiguration(void) +{ + serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(TELEMETRY_UART)].functionMask = FUNCTION_TELEMETRY_SMARTPORT; + motorConfigMutable()->motorPwmProtocol = PWM_TYPE_MULTISHOT; + motorConfigMutable()->motorPwmRate = 4000; + motorConfigMutable()->minthrottle = 1075; + motorConfigMutable()->maxthrottle = 1950; +} diff --git a/src/main/target/ASGARD32F7/target.c b/src/main/target/ASGARD32F7/target.c new file mode 100644 index 00000000000..a176ef6b545 --- /dev/null +++ b/src/main/target/ASGARD32F7/target.c @@ -0,0 +1,44 @@ +/* + * This file is part of INAV. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute and/or modify + * it under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or (at your + * option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see http://www.gnu.org/licenses/. + */ + +#include + +#include +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" +#include "drivers/timer.h" +#include "drivers/bus.h" + +const timerHardware_t timerHardware[] = { + // DEF_TIM(TIM1, CH1, PA8, TIM_USE_PPM, 0, 0), // PPM - timer clash with SS1_TX + + DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR, 1, 0), // M1 - D(2, 4, 7) + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR, 1, 0), // M2 - D(1, 7, 5) + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR, 1, 0), // M3 - D(1, 2, 5) + DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR, 1, 0), // M4 - D(1, 6, 3) + DEF_TIM(TIM5, CH3, PA2, TIM_USE_LED, 0, 0), // TX2; SA port ---> LED - D(1, 0, 6) + DEF_TIM(TIM1, CH2, PA9, TIM_USE_ANY, 0, 0), // SS1 +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/ASGARD32F7/target.h b/src/main/target/ASGARD32F7/target.h new file mode 100644 index 00000000000..6e1715ae950 --- /dev/null +++ b/src/main/target/ASGARD32F7/target.h @@ -0,0 +1,178 @@ +/* + * This file is part of INAV. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute and/or modify + * it under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or (at your + * option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see http://www.gnu.org/licenses/. + */ + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "ASF4" + +#define USBD_PRODUCT_STRING "Asgard32" + +// Status LED +// #define LED0 PC13 + +// Beeper +#define BEEPER PC13 +#define BEEPER_INVERTED + +// #define USE_EXTI +// #define GYRO_INT_EXTI PC8 +// #define USE_MPU_DATA_READY_SIGNAL // Not connected on FireworksV2 + +#define USE_GYRO +#define USE_ACC + +#define USE_GYRO_MPU6000 +#define USE_ACC_MPU6000 +#define MPU6000_CS_PIN PA4 +#define MPU6000_SPI_BUS BUS_SPI1 +#define GYRO_MPU6000_ALIGN CW90_DEG +#define ACC_MPU6000_ALIGN CW90_DEG + +#define USE_MAG +#define MAG_I2C_BUS BUS_I2C2 +#define USE_MAG_HMC5883 +#define USE_MAG_QMC5883 +#define USE_MAG_IST8310 +#define USE_MAG_MAG3110 +#define USE_MAG_LIS3MDL + +#define TEMPERATURE_I2C_BUS BUS_I2C2 + +#define USE_BARO + +#define USE_BARO_BMP280 +#define BMP280_SPI_BUS BUS_SPI2 +#define BMP280_CS_PIN PB9 + +#define USE_VCP + +#define USE_UART1 +#define UART1_RX_PIN PA10 +#define UART1_TX_PIN PA9 + +#define USE_UART2 +#define UART2_RX_PIN NONE +#define UART2_TX_PIN PA2 + +#define USE_UART3 +#define UART3_RX_PIN PC11 +#define UART3_TX_PIN PC10 + +#define USE_UART4 +#define UART4_RX_PIN PA1 +#define UART4_TX_PIN NONE + +#define USE_UART5 +#define UART5_RX_PIN PD2 +#define UART5_TX_PIN PC12 + +#define USE_UART6 +#define UART6_RX_PIN PC7 +#define UART6_TX_PIN PC6 + +// #define USE_SOFTSERIAL1 +// #define SOFTSERIAL_1_RX_PIN NONE +// #define SOFTSERIAL_1_TX_PIN PA9 // Clash with UART1_TX, needed for S.Port + +#define SERIAL_PORT_COUNT 7 // VCP, USART1, USART2, USART3, USART4, USART5, USART6, SOFTSERIAL1 + +// I2C +#define USE_I2C + +#define USE_I2C_DEVICE_2 +#define I2C2_SCL PB10 +#define I2C2_SDA PB11 + +#define USE_SPI + +#define USE_SPI_DEVICE_1 +#define SPI1_NSS_PIN PA4 +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 + +#define USE_SPI_DEVICE_2 +#define SPI2_NSS_PIN PB12 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PC2 +#define SPI2_MOSI_PIN PC3 + +#define USE_SPI_DEVICE_3 +#define SPI3_NSS_PIN PA15 +#define SPI3_SCK_PIN PB3 +#define SPI3_MISO_PIN PB4 +#define SPI3_MOSI_PIN PB5 + +#define USE_OSD +#define USE_MAX7456 +#define MAX7456_SPI_BUS BUS_SPI3 +#define MAX7456_CS_PIN PA15 + +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT +#define M25P16_CS_PIN PB12 +#define M25P16_SPI_BUS BUS_SPI2 +#define USE_FLASHFS +#define USE_FLASH_M25P16 + +#define USE_ADC +#define ADC_CHANNEL_1_PIN PC0 +#define ADC_CHANNEL_2_PIN PC1 +#define ADC_CHANNEL_3_PIN PC4 + +#define VBAT_ADC_CHANNEL ADC_CHN_1 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 +#define RSSI_ADC_CHANNEL ADC_CHN_3 + +#define USE_LED_STRIP +#define WS2811_PIN PA2 + +#define SENSORS_SET (SENSOR_ACC | SENSOR_BARO) + +#define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_BLACKBOX | FEATURE_VBAT | FEATURE_OSD | FEATURE_SOFTSERIAL | FEATURE_TELEMETRY) +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_SBUS + +// Disable PWM & PPM inputs +#undef USE_RX_PWM +#undef USE_RX_PPM + +// Set default UARTs +#define TELEMETRY_UART SERIAL_PORT_SOFTSERIAL1 +#define SERIALRX_UART SERIAL_PORT_USART1 +#define SMARTAUDIO_UART SERIAL_PORT_USART2 + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +// Number of available PWM outputs +#define USE_DSHOT +#define MAX_PWM_OUTPUT_PORTS 4 +#define TARGET_MOTOR_COUNT 4 + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD 0xffff + +#define PCA9685_I2C_BUS BUS_I2C2 +#define PITOT_I2C_BUS BUS_I2C2 diff --git a/src/main/target/ASGARD32F7/target.mk b/src/main/target/ASGARD32F7/target.mk new file mode 100644 index 00000000000..a7615967053 --- /dev/null +++ b/src/main/target/ASGARD32F7/target.mk @@ -0,0 +1,17 @@ +F7X2RE_TARGETS += $(TARGET) +FEATURES += VCP ONBOARDFLASH + +TARGET_SRC = \ + drivers/accgyro/accgyro_mpu6000.c \ + drivers/accgyro/accgyro_mpu6500.c \ + drivers/barometer/barometer_bmp280.c \ + drivers/barometer/barometer_bmp085.c \ + drivers/barometer/barometer_ms56xx.c \ + drivers/compass/compass_hmc5883l.c \ + drivers/compass/compass_qmc5883l.c \ + drivers/compass/compass_ist8310.c \ + drivers/compass/compass_mag3110.c \ + drivers/compass/compass_lis3mdl.c \ + drivers/pitotmeter_adc.c \ + drivers/light_ws2811strip.c \ + drivers/max7456.c diff --git a/src/main/target/BEEROTORF4/target.c b/src/main/target/BEEROTORF4/target.c index d11c1fa41b3..997c6b3f865 100644 --- a/src/main/target/BEEROTORF4/target.c +++ b/src/main/target/BEEROTORF4/target.c @@ -23,20 +23,19 @@ #include "drivers/pwm_mapping.h" #include "drivers/timer.h" - const timerHardware_t timerHardware[] = { - { TIM9, IO_TAG(PA3), TIM_Channel_2, 0, IOCFG_AF_PP, GPIO_AF_TIM9, TIM_USE_PPM }, // PPM IN - - { TIM1, IO_TAG(PB0), TIM_Channel_2, 1 | TIMER_OUTPUT_N_CHANNEL, IOCFG_AF_PP, GPIO_AF_TIM1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR }, // M1 - { TIM1, IO_TAG(PB1), TIM_Channel_3, 1 | TIMER_OUTPUT_N_CHANNEL, IOCFG_AF_PP, GPIO_AF_TIM1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR }, // M2 - { TIM2, IO_TAG(PA1), TIM_Channel_2, 1 , IOCFG_AF_PP, GPIO_AF_TIM2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // M3 - { TIM5, IO_TAG(PA0), TIM_Channel_1, 1 , IOCFG_AF_PP, GPIO_AF_TIM5, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // M4 - { TIM3, IO_TAG(PC6), TIM_Channel_1, 1 , IOCFG_AF_PP, GPIO_AF_TIM3, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO }, // M5 - { TIM3, IO_TAG(PC7), TIM_Channel_2, 1 , IOCFG_AF_PP, GPIO_AF_TIM3, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO }, // M6 - { TIM3, IO_TAG(PB5), TIM_Channel_2, 1 , IOCFG_AF_PP, GPIO_AF_TIM3, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO }, // M7 - { TIM11, IO_TAG(PB9), TIM_Channel_1, 1 , IOCFG_AF_PP, GPIO_AF_TIM11, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO }, // M8 - - { TIM4, IO_TAG(PB8), TIM_Channel_3, 1, IOCFG_AF_PP, GPIO_AF_TIM4, TIM_USE_LED }, // LED_STRIP / TRANSPONDER + DEF_TIM(TIM9, CH2, PA3, TIM_USE_PPM, 0, 0), // PPM IN + + DEF_TIM(TIM1, CH2N, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // M1 - DMAR: DMA2_ST5 + DEF_TIM(TIM1, CH3N, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // M2 - + DEF_TIM(TIM2, CH2, PA1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // M3 - DMAR: DMA1_ST7 + DEF_TIM(TIM2, CH1, PA0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // M4 - + DEF_TIM(TIM8, CH1, PC6, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // M5 - DMAR: DMA2_ST1 + DEF_TIM(TIM8, CH2, PC7, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // M6 - + DEF_TIM(TIM3, CH2, PB5, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // M7 - DMAR: DMA1_ST2 + DEF_TIM(TIM4, CH4, PB9, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // M8 - DMAR: DMA1_ST6 + + DEF_TIM(TIM4, CH3, PB8, TIM_USE_LED, 0, 0), // LED_STRIP / TRANSPONDER - DMA1_ST7 }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/BEEROTORF4/target.h b/src/main/target/BEEROTORF4/target.h index 2a58651bb4b..20a6963cb01 100644 --- a/src/main/target/BEEROTORF4/target.h +++ b/src/main/target/BEEROTORF4/target.h @@ -60,23 +60,19 @@ #define USE_MAG_MAG3110 #define USE_MAG_LIS3MDL +#define TEMPERATURE_I2C_BUS BUS_I2C1 + #define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_BUS BUS_SPI3 #define MAX7456_CS_PIN SPI3_NSS_PIN #define USE_SDCARD -#define USE_SDCARD_SPI2 - -#define SDCARD_DETECT_PIN PC3 +#define USE_SDCARD_SPI #define SDCARD_DETECT_INVERTED -#define SDCARD_SPI_INSTANCE SPI2 -#define SDCARD_SPI_CS_PIN SPI2_NSS_PIN - -#define SDCARD_DMA_CHANNEL_TX DMA1_Stream4 -#define SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG DMA_FLAG_TCIF4 -#define SDCARD_DMA_CLK RCC_AHB1Periph_DMA1 -#define SDCARD_DMA_CHANNEL DMA_Channel_0 +#define SDCARD_DETECT_PIN PC3 +#define SDCARD_SPI_BUS BUS_SPI2 +#define SDCARD_CS_PIN SPI2_NSS_PIN #define USE_VCP #define VBUS_SENSING_ENABLED @@ -121,6 +117,7 @@ #define SPI2_SCK_PIN PB13 #define SPI2_MISO_PIN PB14 #define SPI2_MOSI_PIN PB15 +#define SPI2_CLOCK_LEADING_EDGE //MAX7456 / SPI RX #define USE_SPI_DEVICE_3 @@ -145,16 +142,7 @@ #define RSSI_ADC_CHANNEL ADC_CHN_3 #define USE_LED_STRIP -#define WS2811_GPIO_AF GPIO_AF_TIM4 #define WS2811_PIN PB8 -#define WS2811_TIMER TIM4 -#define WS2811_TIMER_CHANNEL TIM_Channel_3 -#define WS2811_DMA_HANDLER_IDENTIFER DMA1_ST7_HANDLER -#define WS2811_DMA_STREAM DMA1_Stream7 -#define WS2811_DMA_CHANNEL DMA_Channel_2 -#define WS2811_DMA_IRQ DMA1_Stream7_IRQn -#define WS2811_DMA_FLAG DMA_FLAG_TCIF7 -#define WS2811_DMA_IT DMA_IT_TCIF7 //#define TRANSPONDER //#define TRANSPONDER_GPIO_AF GPIO_AF_TIM4 @@ -168,7 +156,7 @@ //#define TRANSPONDER_DMA_FLAG DMA_FLAG_TCIF7 //#define TRANSPONDER_DMA_IT DMA_IT_TCIF7 -#define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_BLACKBOX | FEATURE_TELEMETRY | FEATURE_OSD | FEATURE_AIRMODE ) +#define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_BLACKBOX | FEATURE_TELEMETRY | FEATURE_OSD ) #define DEFAULT_RX_TYPE RX_TYPE_SERIAL #define DISABLE_RX_PWM_FEATURE #define SERIALRX_PROVIDER SERIALRX_SBUS diff --git a/src/main/target/BEEROTORF4/target.mk b/src/main/target/BEEROTORF4/target.mk index de1088af58c..aaa180a4f8c 100644 --- a/src/main/target/BEEROTORF4/target.mk +++ b/src/main/target/BEEROTORF4/target.mk @@ -15,5 +15,4 @@ TARGET_SRC = \ drivers/compass/compass_mag3110.c \ drivers/compass/compass_lis3mdl.c \ drivers/max7456.c \ - drivers/light_ws2811strip.c \ - drivers/light_ws2811strip_stdperiph.c \ No newline at end of file + drivers/light_ws2811strip.c \ No newline at end of file diff --git a/src/main/target/BETAFLIGHTF3/config.c b/src/main/target/BETAFLIGHTF3/config.c index 479d4259d94..beb2c740901 100755 --- a/src/main/target/BETAFLIGHTF3/config.c +++ b/src/main/target/BETAFLIGHTF3/config.c @@ -17,14 +17,10 @@ #include #include - #include - -#ifdef TARGET_CONFIG #include "sensors/battery.h" void targetConfiguration(void) { batteryMetersConfigMutable()->current.scale = 20; } -#endif diff --git a/src/main/target/BETAFLIGHTF3/target.c b/src/main/target/BETAFLIGHTF3/target.c index 486e51039a2..265c88da5f3 100755 --- a/src/main/target/BETAFLIGHTF3/target.c +++ b/src/main/target/BETAFLIGHTF3/target.c @@ -23,19 +23,25 @@ #include "drivers/timer.h" #include "drivers/pwm_mapping.h" -/*#include "drivers/dma.h"*/ const timerHardware_t timerHardware[] = { - { TIM4, IO_TAG(PB7), TIM_Channel_2, 0, IOCFG_AF_PP, GPIO_AF_2, TIM_USE_PPM }, // Pin PPM - PB7 - { TIM16, IO_TAG(PA6), TIM_Channel_1, 1, IOCFG_AF_PP, GPIO_AF_1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR }, // PWM1 - { TIM8, IO_TAG(PA7), TIM_Channel_1, 1 | TIMER_OUTPUT_N_CHANNEL, IOCFG_AF_PP, GPIO_AF_4, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR }, // PWM2 - { TIM8, IO_TAG(PB8), TIM_Channel_2, 1, IOCFG_AF_PP, GPIO_AF_10, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // PWM3 - { TIM17, IO_TAG(PB9), TIM_Channel_1, 1, IOCFG_AF_PP, GPIO_AF_1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // PWM4 - { TIM1, IO_TAG(PB0), TIM_Channel_2, 1 | TIMER_OUTPUT_N_CHANNEL, IOCFG_AF_PP, GPIO_AF_6, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // PWM5 - { TIM8, IO_TAG(PB1), TIM_Channel_3, 1 | TIMER_OUTPUT_N_CHANNEL, IOCFG_AF_PP, GPIO_AF_4, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // PWM6 - { TIM2, IO_TAG(PA0), TIM_Channel_1, 1, IOCFG_AF_PP, GPIO_AF_1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // PWM7 - { TIM2, IO_TAG(PA2), TIM_Channel_3, 1, IOCFG_AF_PP, GPIO_AF_1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // PWM8 - { TIM1, IO_TAG(PA8), TIM_Channel_1, 1, IOCFG_AF_PP, GPIO_AF_6, TIM_USE_LED } // LED STRIP + DEF_TIM(TIM4, CH2, PB7, TIM_USE_PPM, 0), // PPM DMA(1,4) + + // Motors 1-4 + DEF_TIM(TIM16, CH1, PA6, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0), // PWM1 UP(1,6) + DEF_TIM(TIM8, CH1N, PA7, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0), // PWM2 UP(2,1) + DEF_TIM(TIM8, CH2, PB8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // PWM3 UP(2,1) + DEF_TIM(TIM17, CH1, PB9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // PWM4 UP(1,7) + + // Motors 5-6 or SoftSerial + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // PWM5 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // PWM6 + + // Motors 7-8 or UART2 + DEF_TIM(TIM2, CH4, PA3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // PWM7/UART2_RX + DEF_TIM(TIM2, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // PWM8/UART2_TX + + DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0), // LED DMA(1,2) }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/BETAFLIGHTF3/target.h b/src/main/target/BETAFLIGHTF3/target.h index e27a2468c0c..978ef55dc1a 100755 --- a/src/main/target/BETAFLIGHTF3/target.h +++ b/src/main/target/BETAFLIGHTF3/target.h @@ -20,9 +20,6 @@ #define TARGET_BOARD_IDENTIFIER "BFF3" -#define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT -#define TARGET_CONFIG - #define BEEPER PC15 #define BEEPER_INVERTED @@ -86,25 +83,19 @@ #define SPI2_SCK_PIN PB13 #define SPI2_MISO_PIN PB14 #define SPI2_MOSI_PIN PB15 +#define SPI2_CLOCK_LEADING_EDGE #define USE_OSD #define USE_MAX7456 -#define MAX7456_SPI_BUS BUS_SPI1 -#define MAX7456_CS_PIN PA1 +#define MAX7456_SPI_BUS BUS_SPI1 +#define MAX7456_CS_PIN PA1 #define USE_SDCARD -#define USE_SDCARD_SPI2 +#define USE_SDCARD_SPI #define SDCARD_DETECT_INVERTED - -#define SDCARD_DETECT_PIN PC14 -#define SDCARD_SPI_INSTANCE SPI2 -#define SDCARD_SPI_CS_PIN SPI2_NSS_PIN - -//#define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 128 // XXX -//#define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER 2 // XXX - -#define SDCARD_DMA_CHANNEL_TX DMA1_Channel5 -#define SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG DMA1_FLAG_TC5 +#define SDCARD_DETECT_PIN PC14 +#define SDCARD_SPI_BUS BUS_SPI2 +#define SDCARD_CS_PIN SPI2_NSS_PIN #define BOARD_HAS_VOLTAGE_DIVIDER #define USE_ADC diff --git a/src/main/target/BETAFLIGHTF4/target.c b/src/main/target/BETAFLIGHTF4/target.c index e246b0ad37a..18d56e7e8bc 100755 --- a/src/main/target/BETAFLIGHTF4/target.c +++ b/src/main/target/BETAFLIGHTF4/target.c @@ -24,17 +24,16 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[] = { - { TIM4, IO_TAG(PB8), TIM_Channel_3, 0, IOCFG_AF_PP_PD, GPIO_AF_TIM4, TIM_USE_PPM }, // PPM + DEF_TIM(TIM4, CH3, PB8, TIM_USE_PPM, 0, 0), // PPM // Motors - // { TIM3, IO_TAG(PB0), TIM_Channel_3, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM3, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR }, - { TIM1, IO_TAG(PB0), TIM_Channel_2, 1 | TIMER_OUTPUT_N_CHANNEL, IOCFG_AF_PP_PD, GPIO_AF_TIM1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR }, - { TIM3, IO_TAG(PB1), TIM_Channel_4, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, - { TIM8, IO_TAG(PC9), TIM_Channel_4, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, - { TIM8, IO_TAG(PC8), TIM_Channel_3, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, + DEF_TIM(TIM1, CH2, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1_OUT D2_ST6 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S2_OUT D1_ST2 + DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S3_OUT D1_ST6 + DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4_OUT D1_ST1 // LED strip - { TIM4, IO_TAG(PB6), TIM_Channel_1, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM4, TIM_USE_LED }, // D1_ST0 + DEF_TIM(TIM4, CH1, PB6, TIM_USE_LED, 0, 0), // D1_ST0 }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/BETAFLIGHTF4/target.h b/src/main/target/BETAFLIGHTF4/target.h index 209bfacba3d..4ddd3aaf2c6 100755 --- a/src/main/target/BETAFLIGHTF4/target.h +++ b/src/main/target/BETAFLIGHTF4/target.h @@ -135,6 +135,8 @@ #define USE_MAG_MAG3110 #define USE_MAG_LIS3MDL +#define TEMPERATURE_I2C_BUS BUS_I2C2 + #define USE_BARO #define BARO_I2C_BUS BUS_I2C2 #define USE_BARO_BMP085 @@ -142,7 +144,6 @@ #define USE_BARO_MS5611 #define USE_PITOT_ADC -#define USE_PITOT_MS4525 #define PITOT_I2C_BUS BUS_I2C2 #define USE_RANGEFINDER @@ -157,9 +158,6 @@ #define USE_LED_STRIP #define WS2811_PIN PB6 -#define WS2811_DMA_STREAM DMA1_Stream0 -#define WS2811_DMA_CHANNEL DMA_Channel_2 -#define WS2811_DMA_HANDLER_IDENTIFER DMA1_ST0_HANDLER #define SERIALRX_PROVIDER SERIALRX_SBUS #define DEFAULT_RX_TYPE RX_TYPE_SERIAL @@ -181,4 +179,4 @@ #define MAX_PWM_OUTPUT_PORTS 4 -#define PCA9685_I2C_BUS BUS_I2C2 \ No newline at end of file +#define PCA9685_I2C_BUS BUS_I2C2 diff --git a/src/main/target/BETAFLIGHTF4/target.mk b/src/main/target/BETAFLIGHTF4/target.mk index 04624e14e9d..a5826eef035 100755 --- a/src/main/target/BETAFLIGHTF4/target.mk +++ b/src/main/target/BETAFLIGHTF4/target.mk @@ -17,8 +17,6 @@ TARGET_SRC = \ drivers/compass/compass_ist8308.c \ drivers/compass/compass_mag3110.c \ drivers/compass/compass_lis3mdl.c \ - drivers/pitotmeter_ms4525.c \ drivers/pitotmeter_adc.c \ drivers/max7456.c \ - drivers/light_ws2811strip.c \ - drivers/light_ws2811strip_stdperiph.c \ No newline at end of file + drivers/light_ws2811strip.c diff --git a/src/main/target/BLUEJAYF4/target.c b/src/main/target/BLUEJAYF4/target.c index b82c2e2a201..138fa9b18dd 100644 --- a/src/main/target/BLUEJAYF4/target.c +++ b/src/main/target/BLUEJAYF4/target.c @@ -23,14 +23,15 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[] = { - { TIM8, IO_TAG(PC7), TIM_Channel_2, 0, IOCFG_AF_PP, GPIO_AF_TIM8, TIM_USE_PPM }, // PPM IN + DEF_TIM(TIM8, CH2, PC7, TIM_USE_PPM, 0, 0 ), // PPM IN - { TIM5, IO_TAG(PA0), TIM_Channel_1, 1, IOCFG_AF_PP, GPIO_AF_TIM5, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR}, // S1_OUT - { TIM5, IO_TAG(PA1), TIM_Channel_2, 1, IOCFG_AF_PP, GPIO_AF_TIM5, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR }, // S2_OUT - { TIM2, IO_TAG(PA2), TIM_Channel_3, 1, IOCFG_AF_PP, GPIO_AF_TIM2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // S3_OUT - { TIM9, IO_TAG(PA3), TIM_Channel_2, 1, IOCFG_AF_PP, GPIO_AF_TIM9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // S4_OUT - { TIM3, IO_TAG(PB1), TIM_Channel_4, 1, IOCFG_AF_PP, GPIO_AF_TIM3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // S5_OUT - { TIM3, IO_TAG(PB0), TIM_Channel_3, 1, IOCFG_AF_PP, GPIO_AF_TIM3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // S6_OUT + DEF_TIM(TIM5, CH1, PA0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0 ), // S1_OUT - DMA1_ST2 + DEF_TIM(TIM5, CH2, PA1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0 ), // S2_OUT - DMA1_ST4 + DEF_TIM(TIM2, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), // S3_OUT - DMA1_ST1 + DEF_TIM(TIM9, CH2, PA3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), // S4_OUT - no DMA + // DEF_TIM(TIM5, CH4, PA3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1 ), // S4_OUT - DMA1_ST3 (Could be DMA1_ST1 with dmaopt=0) + DEF_TIM(TIM3, CH4, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO | TIM_USE_LED, 0, 0 ), // S5_OUT - DMA1_ST2 + DEF_TIM(TIM3, CH3, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), // S6_OUT - DMA1_ST7 }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/BLUEJAYF4/target.h b/src/main/target/BLUEJAYF4/target.h index b525ea3414a..bf5dc37144a 100644 --- a/src/main/target/BLUEJAYF4/target.h +++ b/src/main/target/BLUEJAYF4/target.h @@ -17,7 +17,6 @@ #pragma once #define TARGET_BOARD_IDENTIFIER "BJF4" -#define TARGET_CONFIG #define USBD_PRODUCT_STRING "BlueJayF4" @@ -58,6 +57,8 @@ #define USE_MAG_MAG3110 #define USE_MAG_LIS3MDL +#define TEMPERATURE_I2C_BUS BUS_I2C1 + #define USE_BARO #define BARO_I2C_BUS BUS_I2C1 #define USE_BARO_BMP085 @@ -65,20 +66,11 @@ #define USE_BARO_MS5611 #define USE_SDCARD - +#define USE_SDCARD_SPI #define SDCARD_DETECT_INVERTED - -#define SDCARD_DETECT_PIN PD2 -#define SDCARD_SPI_INSTANCE SPI3 -#define SDCARD_SPI_CS_PIN PA15 - -#define SDCARD_DMA_CHANNEL_TX DMA1_Stream5 -#define SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG DMA_FLAG_TCIF5 -#define SDCARD_DMA_CLK RCC_AHB1Periph_DMA1 -#define SDCARD_DMA_CHANNEL DMA_Channel_0 - -// Performance logging for SD card operations: -// #define AFATFS_USE_INTROSPECTIVE_LOGGING +#define SDCARD_DETECT_PIN PD2 +#define SDCARD_SPI_BUS BUS_SPI3 +#define SDCARD_CS_PIN PA15 #define M25P16_CS_PIN PB7 #define M25P16_SPI_BUS BUS_SPI3 @@ -129,6 +121,7 @@ #define SPI3_SCK_PIN PC10 #define SPI3_MISO_PIN PC11 #define SPI3_MOSI_PIN PC12 +#define SPI3_CLOCK_LEADING_EDGE #define USE_I2C #define USE_I2C_DEVICE_1 @@ -139,17 +132,7 @@ #define VBAT_ADC_CHANNEL ADC_CHN_1 #define USE_LED_STRIP -// LED Strip can run off Pin 6 (PB1) of the ESC outputs. #define WS2811_PIN PB1 -#define WS2811_TIMER TIM3 -#define WS2811_TIMER_CHANNEL TIM_Channel_4 -#define WS2811_DMA_HANDLER_IDENTIFER DMA1_ST2_HANDLER -#define WS2811_DMA_STREAM DMA1_Stream2 -#define WS2811_DMA_FLAG DMA_FLAG_TCIF2 -#define WS2811_DMA_IT DMA_IT_TCIF2 -#define WS2811_DMA_CHANNEL DMA_Channel_5 -#define WS2811_DMA_IRQ DMA1_Stream2_IRQn - #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT diff --git a/src/main/target/BLUEJAYF4/target.mk b/src/main/target/BLUEJAYF4/target.mk index 68495696e8c..45a97b49995 100644 --- a/src/main/target/BLUEJAYF4/target.mk +++ b/src/main/target/BLUEJAYF4/target.mk @@ -13,6 +13,4 @@ TARGET_SRC = \ drivers/compass/compass_ist8308.c \ drivers/compass/compass_mag3110.c \ drivers/compass/compass_lis3mdl.c \ - drivers/light_ws2811strip.c \ - drivers/light_ws2811strip_stdperiph.c - + drivers/light_ws2811strip.c \ No newline at end of file diff --git a/src/main/target/CHEBUZZF3/target.c b/src/main/target/CHEBUZZF3/target.c index 8a4f760e9ba..f447c82cc10 100644 --- a/src/main/target/CHEBUZZF3/target.c +++ b/src/main/target/CHEBUZZF3/target.c @@ -24,25 +24,25 @@ const timerHardware_t timerHardware[] = { // INPUTS CH1-8 - { TIM1, IO_TAG(PA8), TIM_Channel_1, 1, IOCFG_AF_PP_PD, GPIO_AF_6, TIM_USE_PPM | TIM_USE_PWM }, // PWM1 - PA8 - { TIM16, IO_TAG(PB8), TIM_Channel_1, 0, IOCFG_AF_PP_PD, GPIO_AF_1, TIM_USE_PWM }, // PWM2 - PB8 - { TIM17, IO_TAG(PB9), TIM_Channel_1, 0, IOCFG_AF_PP_PD, GPIO_AF_1, TIM_USE_PWM }, // PWM3 - PB9 - { TIM8, IO_TAG(PC6), TIM_Channel_1, 1, IOCFG_AF_PP_PD, GPIO_AF_4, TIM_USE_PWM }, // PWM4 - PC6 - { TIM8, IO_TAG(PC7), TIM_Channel_2, 1, IOCFG_AF_PP_PD, GPIO_AF_4, TIM_USE_PWM }, // PWM5 - PC7 - { TIM8, IO_TAG(PC8), TIM_Channel_3, 1, IOCFG_AF_PP_PD, GPIO_AF_4, TIM_USE_PWM }, // PWM6 - PC8 - { TIM15, IO_TAG(PF9), TIM_Channel_1, 0, IOCFG_AF_PP_PD, GPIO_AF_3, TIM_USE_PWM }, // PWM7 - PF9 - { TIM15, IO_TAG(PF10), TIM_Channel_2, 0, IOCFG_AF_PP_PD, GPIO_AF_3, TIM_USE_PWM }, // PWM8 - PF10 + DEF_TIM(TIM1, CH1, PA8, TIM_USE_PPM, 0), // PWM1 - PA8 + DEF_TIM(TIM16, CH1, PB8, TIM_USE_PWM, 0), // PWM2 - PB8 + DEF_TIM(TIM17, CH1, PB9, TIM_USE_PWM, 0), // PWM3 - PB9 + DEF_TIM(TIM8, CH1, PC6, TIM_USE_PWM, 0), // PWM4 - PC6 + DEF_TIM(TIM8, CH2, PC7, TIM_USE_PWM, 0), // PWM5 - PC7 + DEF_TIM(TIM8, CH3, PC8, TIM_USE_PWM, 0), // PWM6 - PC8 + DEF_TIM(TIM15, CH1, PF9, TIM_USE_PWM, 0), // PWM7 - PF9 + DEF_TIM(TIM15, CH2, PF10, TIM_USE_PWM, 0), // PWM8 - PF10 - { TIM4, IO_TAG(PD12), TIM_Channel_1, 0, IOCFG_AF_PP, GPIO_AF_2, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR }, // PWM9 - PD12 - { TIM4, IO_TAG(PD13), TIM_Channel_2, 0, IOCFG_AF_PP, GPIO_AF_2, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR }, // PWM10 - PD13 - { TIM4, IO_TAG(PD14), TIM_Channel_3, 0, IOCFG_AF_PP, GPIO_AF_2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // PWM11 - PD14 - { TIM4, IO_TAG(PD15), TIM_Channel_4, 0, IOCFG_AF_PP, GPIO_AF_2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // PWM12 - PD15 - { TIM2, IO_TAG(PA1), TIM_Channel_2, 0, IOCFG_AF_PP, GPIO_AF_1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // PWM13 - PA1 - { TIM2, IO_TAG(PA2), TIM_Channel_3, 0, IOCFG_AF_PP, GPIO_AF_1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // PWM14 - PA2 - { TIM2, IO_TAG(PA3), TIM_Channel_4, 0, IOCFG_AF_PP, GPIO_AF_1, TIM_USE_ANY }, // PWM15 - PA3 - { TIM3, IO_TAG(PB0), TIM_Channel_3, 0, IOCFG_AF_PP, GPIO_AF_2, TIM_USE_ANY }, // PWM16 - PB0 - { TIM3, IO_TAG(PB1), TIM_Channel_4, 0, IOCFG_AF_PP, GPIO_AF_2, TIM_USE_ANY }, // PWM17 - PB1 - { TIM3, IO_TAG(PA4), TIM_Channel_2, 0, IOCFG_AF_PP, GPIO_AF_2, TIM_USE_ANY } // PWM18 - PA4 + DEF_TIM(TIM4, CH1, PD12, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0), // PWM9 - PD12 + DEF_TIM(TIM4, CH2, PD13, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0), // PWM10 - PD13 + DEF_TIM(TIM4, CH3, PD14, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // PWM11 - PD14 + DEF_TIM(TIM4, CH4, PD15, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // PWM12 - PD15 + DEF_TIM(TIM2, CH2, PA1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // PWM13 - PA1 + DEF_TIM(TIM2, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // PWM14 - PA2 + DEF_TIM(TIM2, CH4, PA3, TIM_USE_ANY, 0), // PWM15 - PA3 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_ANY, 0), // PWM16 - PB0 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_ANY, 0), // PWM17 - PB1 + DEF_TIM(TIM3, CH2, PA4, TIM_USE_ANY, 0), // PWM18 - PA4 }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/CLRACINGF4AIR/target.c b/src/main/target/CLRACINGF4AIR/target.c index 4bb2314deca..5554bc3f2c0 100644 --- a/src/main/target/CLRACINGF4AIR/target.c +++ b/src/main/target/CLRACINGF4AIR/target.c @@ -21,23 +21,23 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[] = { - { TIM11, IO_TAG(PB9), TIM_Channel_1, 0, IOCFG_AF_PP, GPIO_AF_TIM11, TIM_USE_PPM | TIM_USE_PWM }, // PPM - { TIM4, IO_TAG(PB8), TIM_Channel_3, 1, IOCFG_AF_PP, GPIO_AF_TIM4, TIM_USE_LED }, // LED_STRIP + DEF_TIM(TIM11, CH1, PB9, TIM_USE_PPM, 0, 0), + DEF_TIM(TIM4, CH3, PB8, TIM_USE_LED, 0, 0), #if defined(CLRACINGF4AIRV2) || defined(CLRACINGF4AIRV3) - { TIM3, IO_TAG(PB0), TIM_Channel_3, 1, IOCFG_AF_PP, GPIO_AF_TIM3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // MOTOR 3 ELE - { TIM3, IO_TAG(PB1), TIM_Channel_4, 1, IOCFG_AF_PP, GPIO_AF_TIM3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // MOTOR 4 ROLL 1 - { TIM12, IO_TAG(PB15), TIM_Channel_2, 1, IOCFG_AF_PP, GPIO_AF_TIM12, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // MOTOR 5 ROLL 2 - { TIM8, IO_TAG(PC8), TIM_Channel_3, 1, IOCFG_AF_PP, GPIO_AF_TIM8, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR }, // MOTOR 1 ESC - { TIM8, IO_TAG(PC9), TIM_Channel_4, 1, IOCFG_AF_PP, GPIO_AF_TIM8, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR }, // MOTOR 2 ESC - { TIM1, IO_TAG(PA8), TIM_Channel_1, 1, IOCFG_AF_PP, GPIO_AF_TIM1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // MOTOR 6 YAW + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), + DEF_TIM(TIM12, CH2, PB15, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), + DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), + DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), + DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), #else - { TIM3, IO_TAG(PB0), TIM_Channel_3, 1, IOCFG_AF_PP, GPIO_AF_TIM3, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR }, // MOTOR_1 - { TIM3, IO_TAG(PB1), TIM_Channel_4, 1, IOCFG_AF_PP, GPIO_AF_TIM3, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR }, // MOTOR_2 - { TIM9, IO_TAG(PA3), TIM_Channel_2, 1, IOCFG_AF_PP, GPIO_AF_TIM9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // SERVO_1 - { TIM2, IO_TAG(PA2), TIM_Channel_3, 1, IOCFG_AF_PP, GPIO_AF_TIM2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // SERVO_2 - { TIM12, IO_TAG(PB15), TIM_Channel_2, 1, IOCFG_AF_PP, GPIO_AF_TIM12, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // SERVO_3 - { TIM1, IO_TAG(PA8), TIM_Channel_1, 1, IOCFG_AF_PP, GPIO_AF_TIM1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // SERVO_4 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), + DEF_TIM(TIM9, CH2, PA3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), + DEF_TIM(TIM2, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), + DEF_TIM(TIM12, CH2, PB15, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), + DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), #endif }; diff --git a/src/main/target/CLRACINGF4AIR/target.h b/src/main/target/CLRACINGF4AIR/target.h index f69eb59f9f3..afe073190d6 100644 --- a/src/main/target/CLRACINGF4AIR/target.h +++ b/src/main/target/CLRACINGF4AIR/target.h @@ -74,9 +74,6 @@ #define USE_LED_STRIP #define WS2811_PIN PB8 -#define WS2811_DMA_HANDLER_IDENTIFER DMA1_ST7_HANDLER -#define WS2811_DMA_STREAM DMA1_Stream7 -#define WS2811_DMA_CHANNEL DMA_Channel_2 #define USE_VCP @@ -119,6 +116,8 @@ #define I2C_DEVICE (I2CDEV_2) #define I2C2_SCL PB10 #define I2C2_SDA PB11 + +#define TEMPERATURE_I2C_BUS BUS_I2C2 #endif #define USE_ADC diff --git a/src/main/target/CLRACINGF4AIR/target.mk b/src/main/target/CLRACINGF4AIR/target.mk index f88b689f7e1..416569f682c 100644 --- a/src/main/target/CLRACINGF4AIR/target.mk +++ b/src/main/target/CLRACINGF4AIR/target.mk @@ -10,5 +10,4 @@ TARGET_SRC = \ drivers/barometer/barometer_bmp280.c \ drivers/compass/compass_mpu9250.c \ drivers/light_ws2811strip.c \ - drivers/light_ws2811strip_stdperiph.c \ drivers/max7456.c diff --git a/src/main/target/COLIBRI/target.c b/src/main/target/COLIBRI/target.c index d380481f5fd..c418ec9977d 100755 --- a/src/main/target/COLIBRI/target.c +++ b/src/main/target/COLIBRI/target.c @@ -24,23 +24,25 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[] = { - { TIM1, IO_TAG(PA10), TIM_Channel_3, 0, IOCFG_IPD, GPIO_AF_TIM1, TIM_USE_PPM | TIM_USE_PWM }, // S1_IN - { TIM8, IO_TAG(PC6), TIM_Channel_1, 0, IOCFG_IPD, GPIO_AF_TIM8, TIM_USE_PWM }, // S2_IN - { TIM8, IO_TAG(PC7), TIM_Channel_2, 0, IOCFG_IPD, GPIO_AF_TIM8, TIM_USE_PWM }, // S3_IN - { TIM8, IO_TAG(PC8), TIM_Channel_3, 0, IOCFG_IPD, GPIO_AF_TIM8, TIM_USE_PWM }, // S4_IN - { TIM2, IO_TAG(PA15), TIM_Channel_1, 0, IOCFG_IPD, GPIO_AF_TIM2, TIM_USE_PWM }, // S5_IN - { TIM2, IO_TAG(PB3), TIM_Channel_2, 0, IOCFG_IPD, GPIO_AF_TIM2, TIM_USE_PWM }, // S6_IN - { TIM5, IO_TAG(PA0), TIM_Channel_1, 0, IOCFG_IPD, GPIO_AF_TIM5, TIM_USE_PWM }, // S7_IN - { TIM5, IO_TAG(PA1), TIM_Channel_2, 0, IOCFG_IPD, GPIO_AF_TIM5, TIM_USE_PWM }, // S8_IN + DEF_TIM(TIM1, CH3, PA10, TIM_USE_PWM | TIM_USE_PPM, 0, 0), // S1_IN + DEF_TIM(TIM8, CH1, PC6, TIM_USE_PWM, 0, 0), // S2_IN + DEF_TIM(TIM8, CH2, PC7, TIM_USE_PWM, 0, 0), // S3_IN + DEF_TIM(TIM8, CH3, PC8, TIM_USE_PWM, 0, 0), // S4_IN + DEF_TIM(TIM2, CH1, PA15, TIM_USE_PWM, 0, 0), // S5_IN + DEF_TIM(TIM2, CH2, PB3, TIM_USE_PWM, 0, 0), // S6_IN + DEF_TIM(TIM5, CH1, PA0, TIM_USE_PWM, 0, 0), // S7_IN + DEF_TIM(TIM5, CH2, PA1, TIM_USE_PWM, 0, 0), // S8_IN - { TIM3, IO_TAG(PB0), TIM_Channel_3, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // S1_OUT - { TIM3, IO_TAG(PB4), TIM_Channel_1, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // S2_OUT - { TIM3, IO_TAG(PB1), TIM_Channel_4, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // S3_OUT - { TIM12, IO_TAG(PB15), TIM_Channel_2, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM12, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // S4_OUT - { TIM3, IO_TAG(PB5), TIM_Channel_2, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // S5_OUT - { TIM12, IO_TAG(PB14), TIM_Channel_1, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM12, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // S6_OUT - { TIM10, IO_TAG(PB8), TIM_Channel_1, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM10, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR }, // S7_OUT - { TIM11, IO_TAG(PB9), TIM_Channel_1, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM11, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR }, // S8_OUT + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S1_OUT + DEF_TIM(TIM3, CH1, PB4, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S2_OUT + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S3_OUT + DEF_TIM(TIM12, CH2, PB15, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4_OUT + DEF_TIM(TIM3, CH2, PB5, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S5_OUT + DEF_TIM(TIM12, CH1, PB14, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S6_OUT + DEF_TIM(TIM10, CH1, PB8, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR, 0, 0), // S7_OUT + DEF_TIM(TIM11, CH1, PB9, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR, 0, 0), // S8_OUT + + DEF_TIM(TIM4, CH2, PB7, TIM_USE_LED, 0, 0) }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/COLIBRI/target.h b/src/main/target/COLIBRI/target.h index 275a285276f..53d6dd0be5c 100755 --- a/src/main/target/COLIBRI/target.h +++ b/src/main/target/COLIBRI/target.h @@ -59,6 +59,8 @@ #define USE_MAG_MAG3110 #define USE_MAG_LIS3MDL +#define TEMPERATURE_I2C_BUS BUS_I2C3 + #ifdef QUANTON #define ACC_MPU6000_ALIGN CW90_DEG #define GYRO_MPU6000_ALIGN CW90_DEG @@ -128,19 +130,11 @@ #define I2C3_SDA PC9 #define USE_RANGEFINDER - +#define RANGEFINDER_I2C_BUS BUS_I2C3 #define USE_RANGEFINDER_HCSR04 #define RANGEFINDER_HCSR04_TRIGGER_PIN PB8 #define RANGEFINDER_HCSR04_ECHO_PIN PB9 -#define USE_RANGEFINDER_SRF10 -#define SRF10_I2C_BUS BUS_I2C3 - -#define USE_RANGEFINDER_VL53L0X -#define VL53L0X_I2C_BUS BUS_I2C3 - -#define TARGET_CONFIG - #define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT #define DEFAULT_RX_TYPE RX_TYPE_SERIAL @@ -154,5 +148,3 @@ #define TARGET_IO_PORTD 0xffff #define MAX_PWM_OUTPUT_PORTS 15 - -#undef USE_PITOT diff --git a/src/main/target/COLIBRI_RACE/target.c b/src/main/target/COLIBRI_RACE/target.c index 31bcbe2a0d5..f4fc7a58ed7 100644 --- a/src/main/target/COLIBRI_RACE/target.c +++ b/src/main/target/COLIBRI_RACE/target.c @@ -23,18 +23,18 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[] = { - { TIM1, IO_TAG(PA8), TIM_Channel_1, 0, IOCFG_AF_PP_PD, GPIO_AF_6, TIM_USE_PPM }, // PWM1 - PA8 - { TIM3, IO_TAG(PC6), TIM_Channel_1, 1, IOCFG_AF_PP, GPIO_AF_2, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR }, // PWM2 - PC6 - { TIM8, IO_TAG(PC7), TIM_Channel_2, 1, IOCFG_AF_PP, GPIO_AF_4, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // PWM3 - PC7 - { TIM8, IO_TAG(PC8), TIM_Channel_3, 1, IOCFG_AF_PP, GPIO_AF_4, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // PMW4 - PC8 - { TIM8, IO_TAG(PC9), TIM_Channel_4, 1, IOCFG_AF_PP, GPIO_AF_4, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // PWM5 - PC9 - { TIM2, IO_TAG(PA0), TIM_Channel_1, 1, IOCFG_AF_PP, GPIO_AF_1, TIM_USE_MC_MOTOR }, // PWM6 - PA0 - { TIM2, IO_TAG(PA1), TIM_Channel_2, 1, IOCFG_AF_PP, GPIO_AF_1, TIM_USE_MC_MOTOR }, // PWM7 - PA1 - { TIM2, IO_TAG(PA2), TIM_Channel_3, 1, IOCFG_AF_PP, GPIO_AF_1, TIM_USE_MC_MOTOR }, // PWM8 - PA2 - { TIM2, IO_TAG(PA3), TIM_Channel_4, 1, IOCFG_AF_PP, GPIO_AF_1, TIM_USE_MC_MOTOR }, // PWM9 - PA3 - { TIM15, IO_TAG(PB14), TIM_Channel_1, 1, IOCFG_AF_PP_PD, GPIO_AF_1, TIM_USE_MC_MOTOR }, // PWM10 - PB14 - { TIM15, IO_TAG(PB15), TIM_Channel_2, 1, IOCFG_AF_PP_PD, GPIO_AF_1, TIM_USE_MC_MOTOR }, // PWM11 - PB15 - { TIM16, IO_TAG(PA6), TIM_Channel_1, 1, IOCFG_AF_PP_PD, GPIO_AF_1, TIM_USE_LED }, + DEF_TIM(TIM1, CH1, PA8, TIM_USE_PPM, 0), // PWM1 - PA8 + DEF_TIM(TIM3, CH1, PC6, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0), // PWM2 - PC6 + DEF_TIM(TIM8, CH2, PC7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // PWM3 - PC7 + DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // PMW4 - PC8 + DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // PWM5 - PC9 + DEF_TIM(TIM2, CH1, PA0, TIM_USE_MC_MOTOR, 0), // PWM6 - PA0 + DEF_TIM(TIM2, CH2, PA1, TIM_USE_MC_MOTOR, 0), // PWM7 - PA1 + DEF_TIM(TIM2, CH3, PA2, TIM_USE_MC_MOTOR, 0), // PWM8 - PA2 + DEF_TIM(TIM2, CH4, PA3, TIM_USE_MC_MOTOR, 0), // PWM9 - PA3 + DEF_TIM(TIM15, CH1, PB14, TIM_USE_MC_MOTOR, 0), // PWM10 - PB14 + DEF_TIM(TIM15, CH2, PB15, TIM_USE_MC_MOTOR, 0), // PWM11 - PB15 + DEF_TIM(TIM16, CH1, PA6, TIM_USE_LED, 0), // PWM11 - PB15 }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/COLIBRI_RACE/target.h b/src/main/target/COLIBRI_RACE/target.h index fd178eb5cc9..6d4d4b8cd69 100755 --- a/src/main/target/COLIBRI_RACE/target.h +++ b/src/main/target/COLIBRI_RACE/target.h @@ -26,6 +26,8 @@ #define BEEPER PB13 #define BEEPER_INVERTED +#define USE_DSHOT + // MPU6500 interrupt #define USE_EXTI #define GYRO_INT_EXTI PA5 @@ -111,10 +113,7 @@ #define RSSI_ADC_CHANNEL ADC_CHN_3 #define USE_LED_STRIP -#define WS2811_PIN PA6 // TIM16_CH1 -#define WS2811_DMA_STREAM DMA1_Channel3 -#define WS2811_DMA_TC_FLAG DMA1_FLAG_TC3 -#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH3_HANDLER +#define WS2811_PIN PA6 #define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_VBAT) #define DEFAULT_RX_TYPE RX_TYPE_SERIAL @@ -123,8 +122,6 @@ #define USE_SERIAL_4WAY_BLHELI_INTERFACE -#define TARGET_CONFIG - // Number of available PWM outputs #define MAX_PWM_OUTPUT_PORTS 10 diff --git a/src/main/target/COLIBRI_RACE/target.mk b/src/main/target/COLIBRI_RACE/target.mk index 2260fb42c94..d016226a767 100644 --- a/src/main/target/COLIBRI_RACE/target.mk +++ b/src/main/target/COLIBRI_RACE/target.mk @@ -13,6 +13,4 @@ TARGET_SRC = \ drivers/compass/compass_ist8308.c \ drivers/compass/compass_mag3110.c \ drivers/compass/compass_lis3mdl.c \ - drivers/light_ws2811strip.c \ - drivers/light_ws2811strip_stdperiph.c - + drivers/light_ws2811strip.c diff --git a/src/main/target/DALRCF405/target.c b/src/main/target/DALRCF405/target.c index 71727d41575..a8d8deb8f34 100644 --- a/src/main/target/DALRCF405/target.c +++ b/src/main/target/DALRCF405/target.c @@ -22,19 +22,19 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[] = { - { TIM12, IO_TAG(PB15), TIM_Channel_2, 0, IOCFG_AF_PP_PD, GPIO_AF_TIM10, TIM_USE_PPM }, + DEF_TIM(TIM12, CH2, PB15, TIM_USE_PPM, 0, 0), // PPM - { TIM3, IO_TAG(PB0), TIM_Channel_3, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM3, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR }, - { TIM8, IO_TAG(PC6), TIM_Channel_1, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, - { TIM1, IO_TAG(PA10), TIM_Channel_3, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, - { TIM1, IO_TAG(PA8), TIM_Channel_1, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR, 0, 0), // S1 (1,7) + DEF_TIM(TIM8, CH1, PC6, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S2 (2,2) + DEF_TIM(TIM1, CH3, PA10, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S3 (2,6) + DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), // S4 (2,1) (2.3 2.6) + DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S5 (2,4) (2.2) + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S6 (1,2) + DEF_TIM(TIM3, CH2, PC7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S7 (1,5) + DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S8 (2,7) + DEF_TIM(TIM4, CH1, PB6, TIM_USE_LED, 0, 0), // LED STRIP(1,0) - { TIM8, IO_TAG(PC8), TIM_Channel_3, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, - { TIM3, IO_TAG(PB1), TIM_Channel_4, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, - { TIM3, IO_TAG(PC7), TIM_Channel_2, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, - { TIM8, IO_TAG(PC9), TIM_Channel_4, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, - { TIM4, IO_TAG(PB6), TIM_Channel_1, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM4, TIM_USE_LED }, - { TIM2, IO_TAG(PA5), TIM_Channel_1, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM2, TIM_USE_PWM }, + DEF_TIM(TIM2, CH1, PA5, TIM_USE_ANY, 0, 0), // FC CAM }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/DALRCF405/target.h b/src/main/target/DALRCF405/target.h index 8812ced5f01..538aac3394c 100644 --- a/src/main/target/DALRCF405/target.h +++ b/src/main/target/DALRCF405/target.h @@ -102,11 +102,12 @@ #define USE_MAG_MAG3110 #define USE_MAG_LIS3MDL +#define TEMPERATURE_I2C_BUS BUS_I2C1 + #define USE_RANGEFINDER #define USE_RANGEFINDER_HCSR04_I2C #define RANGEFINDER_I2C_BUS DEFAULT_I2C_BUS -#define USE_PITOT_MS4525 #define PITOT_I2C_BUS DEFAULT_I2C_BUS //USART @@ -133,9 +134,7 @@ //LED_STRIP #define USE_LED_STRIP #define WS2811_PIN PB6 -#define WS2811_DMA_STREAM DMA1_Stream0 -#define WS2811_DMA_CHANNEL DMA_Channel_2 -#define WS2811_DMA_HANDLER_IDENTIFER DMA1_ST0_HANDLER + //ADC #define USE_ADC #define ADC_INSTANCE ADC1 diff --git a/src/main/target/DALRCF405/target.mk b/src/main/target/DALRCF405/target.mk index 0c71c508094..69db30708ab 100644 --- a/src/main/target/DALRCF405/target.mk +++ b/src/main/target/DALRCF405/target.mk @@ -14,9 +14,7 @@ TARGET_SRC = \ drivers/compass/compass_ist8310.c \ drivers/compass/compass_mag3110.c \ drivers/compass/compass_lis3mdl.c \ - drivers/pitotmeter_ms4525.c \ drivers/pitotmeter_adc.c \ drivers/light_ws2811strip.c \ - drivers/light_ws2811strip_stdperiph.c \ drivers/max7456.c - + diff --git a/src/main/target/DALRCF722DUAL/hardware_setup.c b/src/main/target/DALRCF722DUAL/hardware_setup.c new file mode 100644 index 00000000000..f7b867bb372 --- /dev/null +++ b/src/main/target/DALRCF722DUAL/hardware_setup.c @@ -0,0 +1,37 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include +#include +#include "platform.h" + +#include "drivers/io_impl.h" + +#ifdef USE_HARDWARE_PREBOOT_SETUP + + +void initialisePreBootHardware(void) +{ + //DALRC F722 have dual GYRO:mpu6000 and icm20602,they use same SPI but different CS pin.Just use mpu6000 for INAV. + //So set icm20602 CS pin high to block icm20602 SPI . + IOInit(DEFIO_IO(PA4), OWNER_SYSTEM, RESOURCE_OUTPUT, 0); + IOConfigGPIO(DEFIO_IO(PA4), IOCFG_OUT_PP); + IOHi(DEFIO_IO(PA4)); +} +#endif + + diff --git a/src/main/target/DALRCF722DUAL/target.c b/src/main/target/DALRCF722DUAL/target.c new file mode 100644 index 00000000000..2eb8a5aa1d0 --- /dev/null +++ b/src/main/target/DALRCF722DUAL/target.c @@ -0,0 +1,44 @@ +/* +* This file is part of Cleanflight. +* +* Cleanflight is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* Cleanflight is distributed in the hope that it will be useful, +* but WITHOUT ANY WARRANTY; without even the implied warranty of +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with Cleanflight. If not, see . +*/ + +#include + +#include "platform.h" + +#include "drivers/bus.h" +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" +#include "drivers/timer.h" + +#define TIM_EN TIMER_OUTPUT_ENABLED +#define TIM_EN_N TIMER_OUTPUT_ENABLED | TIMER_OUTPUT_N_CHANNEL + +const timerHardware_t timerHardware[] = { + DEF_TIM(TIM4, CH2, PB7, TIM_USE_PPM ,0, 0), + + DEF_TIM(TIM8, CH1, PC6, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR,0,0), //S1 DMA2_ST2 T8CH1 + DEF_TIM(TIM8, CH2, PC7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO,0,0), //S2 DMA2_ST3 T8CH2 + DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO,0,0), //S3 DMA2_ST4 T8CH3 + DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO,0,0), //S4 DMA2_ST7 T8CH4 + DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR,0,0), //S5 DMA2_ST1 T1CH1 + DEF_TIM(TIM1, CH2, PA9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO,0,0), //S6 DMA2_ST6 T1CH2 + + DEF_TIM(TIM2, CH1, PA15,TIM_USE_LED,0,0), //2812 STRIP DMA1_ST5 +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); + diff --git a/src/main/target/DALRCF722DUAL/target.h b/src/main/target/DALRCF722DUAL/target.h new file mode 100644 index 00000000000..124069874f0 --- /dev/null +++ b/src/main/target/DALRCF722DUAL/target.h @@ -0,0 +1,152 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "DLF7" +#define USBD_PRODUCT_STRING "DALRCF722DUAL" + +#define USE_HARDWARE_PREBOOT_SETUP + +#define LED0 PC14 +#define BEEPER PC13 +#define BEEPER_INVERTED + +#define USE_GYRO +#define USE_ACC + +// MPU6000 +#define MPU6000_CS_PIN PB0 +#define MPU6000_SPI_BUS BUS_SPI1 +#define USE_GYRO_MPU6000 +#define GYRO_MPU6000_ALIGN CW180_DEG +#define USE_ACC_MPU6000 +#define ACC_MPU6000_ALIGN CW180_DEG + +#define USE_EXTI +#define GYRO_INT_EXTI PB10 +#define USE_MPU_DATA_READY_SIGNAL + +#define USE_VCP + +#define USE_UART1 +#define UART1_TX_PIN PB6 +#define UART1_RX_PIN PB7 + +#define USE_UART2 +#define UART2_TX_PIN PA2 +#define UART2_RX_PIN PA3 + +#define USE_UART3 +#define UART3_TX_PIN PC10 +#define UART3_RX_PIN PC11 + +#define USE_UART4 +#define UART4_TX_PIN PA0 +#define UART4_RX_PIN PA1 + +#define USE_UART5 +#define UART5_TX_PIN PC12 +#define UART5_RX_PIN PD2 +#define SERIAL_PORT_COUNT 6 + +#define USE_I2C +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB8 +#define I2C1_SDA PB9 + + +#define USE_BARO +#define BARO_I2C_BUS BUS_I2C1 +#define USE_BARO_BMP280 +#define USE_BARO_MS5611 +#define USE_BARO_BMP085 + + +#define USE_MAG +#define MAG_I2C_BUS BUS_I2C1 +#define USE_MAG_HMC5883 +#define USE_MAG_QMC5883 +#define USE_MAG_IST8310 +#define USE_MAG_IST8308 +#define USE_MAG_MAG3110 +#define USE_MAG_LIS3MDL + +#define TEMPERATURE_I2C_BUS BUS_I2C1 + +#define PITOT_I2C_BUS BUS_I2C1 + +#define USE_SPI +#define USE_SPI_DEVICE_1 +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 + +#define USE_SPI_DEVICE_2 +#define SPI2_NSS_PIN PB12 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 + +#define USE_SPI_DEVICE_3 +#define SPI3_NSS_PIN NONE +#define SPI3_SCK_PIN PB3 +#define SPI3_MISO_PIN PB4 +#define SPI3_MOSI_PIN PB5 + +#define USE_OSD +#define USE_MAX7456 +#define MAX7456_SPI_BUS BUS_SPI2 +#define MAX7456_CS_PIN SPI2_NSS_PIN + +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT +#define M25P16_CS_PIN PB2 +#define M25P16_SPI_BUS BUS_SPI3 +#define USE_FLASHFS +#define USE_FLASH_M25P16 + +#define USE_ADC +#define ADC_CHANNEL_1_PIN PC0 +#define ADC_CHANNEL_2_PIN PC1 +#define ADC_CHANNEL_3_PIN PA0 + +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_1 +#define VBAT_ADC_CHANNEL ADC_CHN_2 +#define RSSI_ADC_CHANNEL ADC_CHN_3 +#define VBAT_SCALE_DEFAULT 1600 + +#define USE_LED_STRIP +#define WS2811_PIN PA15 +#define WS2811_DMA_HANDLER_IDENTIFER DMA1_ST5_HANDLER +#define WS2811_DMA_STREAM DMA1_Stream5 +#define WS2811_DMA_CHANNEL DMA_CHANNEL_3 + +#define DEFAULT_FEATURES (FEATURE_VBAT | FEATURE_OSD ) +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_SBUS +#define SERIALRX_UART SERIAL_PORT_USART1 + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +#define MAX_PWM_OUTPUT_PORTS 6 + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD (BIT(2)) + + diff --git a/src/main/target/DALRCF722DUAL/target.mk b/src/main/target/DALRCF722DUAL/target.mk new file mode 100644 index 00000000000..d1602929817 --- /dev/null +++ b/src/main/target/DALRCF722DUAL/target.mk @@ -0,0 +1,18 @@ +F7X2RE_TARGETS += $(TARGET) +FEATURES += VCP ONBOARDFLASH + +TARGET_SRC = \ + drivers/accgyro/accgyro_mpu.c \ + drivers/accgyro/accgyro_mpu6000.c \ + drivers/barometer/barometer_bmp085.c \ + drivers/barometer/barometer_bmp280.c \ + drivers/barometer/barometer_ms56xx.c \ + drivers/compass/compass_hmc5883l.c \ + drivers/compass/compass_qmc5883l.c \ + drivers/compass/compass_ist8310.c \ + drivers/compass/compass_ist8308.c \ + drivers/compass/compass_mag3110.c \ + drivers/compass/compass_lis3mdl.c \ + drivers/light_ws2811strip.c \ + drivers/max7456.c \ + drivers/pitotmeter_adc.c \ diff --git a/src/main/target/F4BY/target.c b/src/main/target/F4BY/target.c index 57796e1d8cc..eeaf06b027f 100644 --- a/src/main/target/F4BY/target.c +++ b/src/main/target/F4BY/target.c @@ -6,25 +6,25 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[] = { - { TIM3, IO_TAG(PC9), TIM_Channel_4, 0, IOCFG_AF_PP, GPIO_AF_TIM3, TIM_USE_PPM | TIM_USE_PWM }, // S1_IN - { TIM3, IO_TAG(PC8), TIM_Channel_3, 0, IOCFG_AF_PP, GPIO_AF_TIM3, TIM_USE_PWM }, // S2_IN - { TIM3, IO_TAG(PC6), TIM_Channel_1, 0, IOCFG_AF_PP, GPIO_AF_TIM3, TIM_USE_PWM }, // S3_IN - { TIM3, IO_TAG(PC7), TIM_Channel_2, 0, IOCFG_AF_PP, GPIO_AF_TIM3, TIM_USE_PWM }, // S4_IN - { TIM4, IO_TAG(PD15), TIM_Channel_4, 0, IOCFG_AF_PP, GPIO_AF_TIM4, TIM_USE_PWM }, // S5_IN - { TIM4, IO_TAG(PD14), TIM_Channel_3, 0, IOCFG_AF_PP, GPIO_AF_TIM4, TIM_USE_PWM }, // S6_IN - { TIM4, IO_TAG(PD13), TIM_Channel_2, 0, IOCFG_AF_PP, GPIO_AF_TIM4, TIM_USE_PWM }, // S7_IN - { TIM4, IO_TAG(PD12), TIM_Channel_1, 0, IOCFG_AF_PP, GPIO_AF_TIM4, TIM_USE_PWM }, // S8_IN + DEF_TIM(TIM3, CH4, PC9, TIM_USE_PPM | TIM_USE_PWM, 0, 0), // S1_IN + DEF_TIM(TIM3, CH3, PC8, TIM_USE_PWM, 0, 0), // S2_IN + DEF_TIM(TIM3, CH1, PC6, TIM_USE_PWM, 0, 0), // S3_IN + DEF_TIM(TIM3, CH2, PC7, TIM_USE_PWM, 0, 0), // S4_IN + DEF_TIM(TIM4, CH4, PD15, TIM_USE_PWM, 0, 0), // S5_IN + DEF_TIM(TIM4, CH3, PD14, TIM_USE_PWM, 0, 0), // S6_IN + DEF_TIM(TIM4, CH2, PD13, TIM_USE_PWM, 0, 0), // S7_IN + DEF_TIM(TIM4, CH1, PD12, TIM_USE_PWM, 0, 0), // S8_IN - { TIM2, IO_TAG(PA0), TIM_Channel_1, 1, IOCFG_AF_PP, GPIO_AF_TIM2, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR }, // S1_OUT - { TIM2, IO_TAG(PA1), TIM_Channel_2, 1, IOCFG_AF_PP, GPIO_AF_TIM2, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR }, // S2_OUT - { TIM5, IO_TAG(PA2), TIM_Channel_3, 1, IOCFG_AF_PP, GPIO_AF_TIM5, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO}, // S3_OUT - { TIM5, IO_TAG(PA3), TIM_Channel_4, 1, IOCFG_AF_PP, GPIO_AF_TIM5, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO}, // S4_OUT - { TIM1, IO_TAG(PE9), TIM_Channel_1, 1, IOCFG_AF_PP, GPIO_AF_TIM1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO}, // S5_OUT - { TIM1, IO_TAG(PE11), TIM_Channel_2, 1, IOCFG_AF_PP, GPIO_AF_TIM1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO}, // S6_OUT - { TIM1, IO_TAG(PE13), TIM_Channel_3, 1, IOCFG_AF_PP, GPIO_AF_TIM1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO}, // S7_OUT - { TIM1, IO_TAG(PE14), TIM_Channel_4, 1, IOCFG_AF_PP, GPIO_AF_TIM1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO}, // S8_OUT + DEF_TIM(TIM2, CH1, PA0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1_OUT + DEF_TIM(TIM2, CH2, PA1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S2_OUT + DEF_TIM(TIM5, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S3_OUT + DEF_TIM(TIM5, CH4, PA3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4_OUT + DEF_TIM(TIM1, CH1, PE9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S5_OUT + DEF_TIM(TIM1, CH2, PE11, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S6_OUT + DEF_TIM(TIM1, CH3, PE13, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S7_OUT + DEF_TIM(TIM1, CH4, PE14, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S8_OUT - { TIM9, IO_TAG(PE6), TIM_Channel_2, 0, IOCFG_AF_PP, GPIO_AF_TIM9, TIM_USE_ANY }, // HC-SR04 echo if needed + DEF_TIM(TIM9, CH2, PE6, TIM_USE_ANY, 0, 0), // sonar echo if needed }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/F4BY/target.h b/src/main/target/F4BY/target.h index 1513b781be6..ade4be0ccb8 100644 --- a/src/main/target/F4BY/target.h +++ b/src/main/target/F4BY/target.h @@ -54,20 +54,17 @@ #define USE_MAG_MAG3110 #define USE_MAG_LIS3MDL +#define TEMPERATURE_I2C_BUS BUS_I2C2 + #define USE_BARO #define BARO_I2C_BUS BUS_I2C2 #define USE_BARO_MS5611 #define USE_SDCARD - -#define SDCARD_SPI_INSTANCE SPI2 -#define SDCARD_SPI_CS_PIN PE15 - -#define SDCARD_DMA_CHANNEL_TX DMA1_Stream3 -#define SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG DMA_FLAG_TCIF3 -#define SDCARD_DMA_CLK RCC_AHB1Periph_DMA1 -#define SDCARD_DMA_CHANNEL DMA_Channel_0 +#define USE_SDCARD_SPI +#define SDCARD_SPI_BUS BUS_SPI2 +#define SDCARD_CS_PIN PE15 #define USE_VCP #define VBUS_SENSING_PIN PA9 @@ -111,6 +108,7 @@ #define SPI2_SCK_PIN PB13 #define SPI2_MISO_PIN PB14 #define SPI2_MOSI_PIN PB15 +#define SPI2_CLOCK_LEADING_EDGE #define USE_SPI_DEVICE_3 #define SPI3_NSS_PIN PA15 @@ -151,4 +149,4 @@ #define TARGET_IO_PORTD 0xffff #define TARGET_IO_PORTE 0xffff -#define PCA9685_I2C_BUS BUS_I2C2 \ No newline at end of file +#define PCA9685_I2C_BUS BUS_I2C2 diff --git a/src/main/target/F4BY/target.mk b/src/main/target/F4BY/target.mk index f70cb91b5d0..e5221e515ea 100644 --- a/src/main/target/F4BY/target.mk +++ b/src/main/target/F4BY/target.mk @@ -2,13 +2,13 @@ F405_TARGETS += $(TARGET) FEATURES += SDCARD VCP TARGET_SRC = \ - drivers/accgyro/accgyro_mpu6000.c \ - drivers/barometer/barometer_ms56xx.c \ + drivers/accgyro/accgyro_mpu6000.c \ + drivers/barometer/barometer_ms56xx.c \ drivers/compass/compass_hmc5883l.c \ drivers/compass/compass_qmc5883l.c \ drivers/compass/compass_ist8310.c \ drivers/compass/compass_ist8308.c \ drivers/compass/compass_mag3110.c \ drivers/compass/compass_lis3mdl.c \ - drivers/rangefinder/rangefinder_hcsr04.c \ - drivers/max7456.c \ No newline at end of file + drivers/rangefinder/rangefinder_hcsr04.c \ + drivers/max7456.c \ No newline at end of file diff --git a/src/main/target/FALCORE/config.c b/src/main/target/FALCORE/config.c index ab3ab6c8448..7d4379a9ddc 100755 --- a/src/main/target/FALCORE/config.c +++ b/src/main/target/FALCORE/config.c @@ -42,7 +42,6 @@ void targetConfiguration(void) { - systemConfigMutable()->asyncMode = ASYNC_MODE_NONE; mixerConfigMutable()->platformType = PLATFORM_MULTIROTOR; featureSet(FEATURE_VBAT); @@ -50,7 +49,6 @@ void targetConfiguration(void) featureSet(FEATURE_TELEMETRY); featureSet(FEATURE_LED_STRIP); featureSet(FEATURE_BLACKBOX); - featureSet(FEATURE_AIRMODE); serialConfigMutable()->portConfigs[0].functionMask = FUNCTION_MSP; // VCP serialConfigMutable()->portConfigs[1].functionMask = FUNCTION_GPS; // UART1 diff --git a/src/main/target/FALCORE/target.c b/src/main/target/FALCORE/target.c index 8d6fd3ef893..cc6c8181b6e 100755 --- a/src/main/target/FALCORE/target.c +++ b/src/main/target/FALCORE/target.c @@ -24,23 +24,23 @@ const timerHardware_t timerHardware[] = { // MOTOR outputs - { TIM8, IO_TAG(PC6), TIM_Channel_1, 1, IOCFG_AF_PP, GPIO_AF_4, TIM_USE_MC_MOTOR }, // PWM1 - PC6 - TIM8_CH1 - { TIM8, IO_TAG(PC7), TIM_Channel_2, 1, IOCFG_AF_PP, GPIO_AF_4, TIM_USE_MC_MOTOR }, // PWM2 - PC7 - TIM8_CH2 - { TIM8, IO_TAG(PC8), TIM_Channel_3, 1, IOCFG_AF_PP, GPIO_AF_4, TIM_USE_MC_MOTOR }, // PWM3 - PC8 - TIM8_CH3 - { TIM8, IO_TAG(PC9), TIM_Channel_4, 1, IOCFG_AF_PP, GPIO_AF_4, TIM_USE_MC_MOTOR }, // PWM4 - PC9 - TIM8_CH4 + DEF_TIM(TIM8, CH1, PC6, TIM_USE_MC_MOTOR, 1), + DEF_TIM(TIM8, CH2, PC7, TIM_USE_MC_MOTOR, 1), + DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR, 1), + DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR, 1), // Additional servo outputs - { TIM3, IO_TAG(PA4), TIM_Channel_2, 0, IOCFG_AF_PP, GPIO_AF_2, TIM_USE_MC_SERVO }, // PWM5 - PA4 - TIM3_CH2 - { TIM3, IO_TAG(PB1), TIM_Channel_4, 0, IOCFG_AF_PP, GPIO_AF_2, TIM_USE_MC_SERVO }, // PWM6 - PB1 - TIM3_CH4 + DEF_TIM(TIM3, CH2, PA4, TIM_USE_MC_SERVO, 0), + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_SERVO, 0), // PPM PORT - Also USART2 RX (AF5) - { TIM2, IO_TAG(PA3), TIM_Channel_4, 0, IOCFG_AF_PP, GPIO_AF_1, TIM_USE_PPM }, // PPM - PA3 - TIM2_CH4 + DEF_TIM(TIM2, CH4, PA3, TIM_USE_PPM, 0), // LED_STRIP - { TIM1, IO_TAG(PA8), TIM_Channel_1, 0, IOCFG_AF_PP, GPIO_AF_6, TIM_USE_ANY }, // GPIO_TIMER / LED_STRIP + DEF_TIM(TIM1, CH1, PA8, TIM_USE_ANY, 0), // PWM_BUZZER - { TIM16, IO_TAG(PB4), TIM_Channel_1, 1, IOCFG_AF_PP, GPIO_AF_1, TIM_USE_BEEPER }, // BUZZER - PB4 - TIM16_CH1N + DEF_TIM(TIM16, CH1, PB4, TIM_USE_BEEPER, 0), }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/FALCORE/target.h b/src/main/target/FALCORE/target.h index 5e24a7839d9..8e370c958e6 100755 --- a/src/main/target/FALCORE/target.h +++ b/src/main/target/FALCORE/target.h @@ -19,8 +19,6 @@ #define TARGET_BOARD_IDENTIFIER "FLCR" // FaLCoRre -#define TARGET_CONFIG - #define LED0 PC2 #define LED1 PB11 @@ -110,10 +108,7 @@ #define RSSI_ADC_CHANNEL ADC_CHN_2 #define USE_LED_STRIP -#define USE_LED_STRIP_ON_DMA1_CHANNEL2 #define WS2811_PIN PA8 -#define WS2811_DMA_STREAM DMA1_Channel2 -#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH2_HANDLER #define USE_SERIAL_4WAY_BLHELI_INTERFACE diff --git a/src/main/target/FALCORE/target.mk b/src/main/target/FALCORE/target.mk index b47dc5e2c1a..087ec3e13f4 100755 --- a/src/main/target/FALCORE/target.mk +++ b/src/main/target/FALCORE/target.mk @@ -12,6 +12,4 @@ TARGET_SRC = \ drivers/compass/compass_mag3110.c \ drivers/compass/compass_lis3mdl.c \ drivers/barometer/barometer_ms56xx.c \ - drivers/light_ws2811strip.c \ - drivers/light_ws2811strip_stdperiph.c - + drivers/light_ws2811strip.c \ No newline at end of file diff --git a/src/main/target/FF_F35_LIGHTNING/WINGFC.mk b/src/main/target/FF_F35_LIGHTNING/WINGFC.mk new file mode 100644 index 00000000000..4f5f1f6f0e8 --- /dev/null +++ b/src/main/target/FF_F35_LIGHTNING/WINGFC.mk @@ -0,0 +1 @@ +#WINGFC diff --git a/src/main/target/FF_F35_LIGHTNING/target.c b/src/main/target/FF_F35_LIGHTNING/target.c index fd434cee4eb..0c0bb591e8d 100644 --- a/src/main/target/FF_F35_LIGHTNING/target.c +++ b/src/main/target/FF_F35_LIGHTNING/target.c @@ -22,14 +22,20 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[] = { - { TIM8, IO_TAG(PC8), TIM_Channel_3, 1, IOCFG_AF_PP, GPIO_AF_TIM8, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR }, // S1_OUT - { TIM8, IO_TAG(PC9), TIM_Channel_4, 1, IOCFG_AF_PP, GPIO_AF_TIM8, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR }, // S2_OUT - { TIM4, IO_TAG(PB8), TIM_Channel_3, 1, IOCFG_AF_PP, GPIO_AF_TIM4, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // S3_OUT - { TIM4, IO_TAG(PB9), TIM_Channel_4, 1, IOCFG_AF_PP, GPIO_AF_TIM4, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // S4_OUT - { TIM3, IO_TAG(PB0), TIM_Channel_3, 1, IOCFG_AF_PP, GPIO_AF_TIM3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // S5_OUT - { TIM3, IO_TAG(PB1), TIM_Channel_4, 1, IOCFG_AF_PP, GPIO_AF_TIM3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // S6_OUT - { TIM2, IO_TAG(PB10), TIM_Channel_3, 0, IOCFG_AF_PP, GPIO_AF_TIM2, TIM_USE_PPM }, // PPM IN + DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 1, 0), + DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 1, 0), + DEF_TIM(TIM4, CH3, PB8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 1, 0), + DEF_TIM(TIM4, CH4, PB9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 1, 0), + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 1, 0), + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 1, 0), + +#ifdef WINGFC + DEF_TIM(TIM2, CH4, PB11, TIM_USE_PPM, 0, 0), // UART3 RX +#else + DEF_TIM(TIM2, CH3, PB10, TIM_USE_PPM, 0, 0), // UART3 TX +#endif + }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/FF_F35_LIGHTNING/target.h b/src/main/target/FF_F35_LIGHTNING/target.h index 7a82ab82441..d7364d68af7 100644 --- a/src/main/target/FF_F35_LIGHTNING/target.h +++ b/src/main/target/FF_F35_LIGHTNING/target.h @@ -17,7 +17,6 @@ #pragma once #define TARGET_BOARD_IDENTIFIER "FF35" -#define TARGET_CONFIG #define USBD_PRODUCT_STRING "FURIOUS F35-LIGHTNING" @@ -126,6 +125,8 @@ #define USE_PITOT_MS4525 #define PITOT_I2C_BUS BUS_I2C1 +#define TEMPERATURE_I2C_BUS BUS_I2C1 + #define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_VBAT | FEATURE_CURRENT_METER | FEATURE_OSD | FEATURE_GPS | FEATURE_TELEMETRY) #define CURRENT_METER_SCALE 250 diff --git a/src/main/target/FF_F35_LIGHTNING/target.mk b/src/main/target/FF_F35_LIGHTNING/target.mk index 0a37c1a2511..167cd1789d3 100644 --- a/src/main/target/FF_F35_LIGHTNING/target.mk +++ b/src/main/target/FF_F35_LIGHTNING/target.mk @@ -11,7 +11,6 @@ TARGET_SRC = \ drivers/compass/compass_ist8310.c \ drivers/compass/compass_mag3110.c \ drivers/rangefinder/rangefinder_hcsr04.c \ - drivers/pitotmeter_ms4525.c \ drivers/serial_usb_vcp.c \ drivers/max7456.c - \ No newline at end of file + diff --git a/src/main/target/FF_FORTINIF4/config.c b/src/main/target/FF_FORTINIF4/config.c index 439e2cf1eb5..6acec9f9b4b 100644 --- a/src/main/target/FF_FORTINIF4/config.c +++ b/src/main/target/FF_FORTINIF4/config.c @@ -20,13 +20,9 @@ #include -#ifdef TARGET_CONFIG #include "fc/config.h" - #include "config/feature.h" - #include "rx/rx.h" - #include "hardware_revision.h" void targetConfiguration(void) @@ -37,4 +33,3 @@ void targetConfiguration(void) rxConfigMutable()->halfDuplex = false; } -#endif diff --git a/src/main/target/FF_FORTINIF4/target.c b/src/main/target/FF_FORTINIF4/target.c index a05b6ccd0c2..98ea019571c 100644 --- a/src/main/target/FF_FORTINIF4/target.c +++ b/src/main/target/FF_FORTINIF4/target.c @@ -22,21 +22,13 @@ #include "drivers/pwm_mapping.h" #include "drivers/timer.h" -#define TIM_EN TIMER_OUTPUT_ENABLED - const timerHardware_t timerHardware[] = { - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM_EN, IOCFG_AF_PP, GPIO_AF_TIM3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // S1_OUT - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM_EN, IOCFG_AF_PP, GPIO_AF_TIM3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // S2_OUT - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM_EN, IOCFG_AF_PP, GPIO_AF_TIM2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // S3_OUT - { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM_EN, IOCFG_AF_PP, GPIO_AF_TIM2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // S4_OUT - { TIM4, IO_TAG(PB9), TIM_Channel_4, 0, IOCFG_AF_PP, GPIO_AF_TIM4, TIM_USE_PPM | TIM_USE_MC_SERVO }, // PPM IN - { TIM4, IO_TAG(PB7), TIM_Channel_2, 0, IOCFG_AF_PP, GPIO_AF_TIM4, TIM_USE_LED | TIM_USE_MC_SERVO }, // LED -// DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR, TIMER_OUTPUT_STANDARD, 0 ), // S1_OUT - DMA1_ST7 -// DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, TIMER_OUTPUT_STANDARD, 0 ), // S2_OUT - DMA1_ST2 -// DEF_TIM(TIM2, CH4, PA3, TIM_USE_MOTOR, TIMER_OUTPUT_STANDARD, 1 ), // S3_OUT - DMA1_ST6 -// DEF_TIM(TIM2, CH3, PA2, TIM_USE_MOTOR, TIMER_OUTPUT_STANDARD, 0 ), // S4_OUT - DMA1_ST1 -// DEF_TIM(TIM4, CH4, PB9, TIM_USE_PPM, TIMER_INPUT_ENABLED, 0 ), // PPM IN -// DEF_TIM(TIM4, CH2, PB7, TIM_USE_LED, TIMER_OUTPUT_STANDARD, 0), // LED - DMA1_ST3 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 1, 0 ), // S1_OUT - DMA1_ST7 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 1, 0 ), // S2_OUT - DMA1_ST2 + DEF_TIM(TIM2, CH4, PA3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 1, 1 ), // S3_OUT - DMA1_ST6 + DEF_TIM(TIM2, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 1, 0 ), // S4_OUT - DMA1_ST1 + DEF_TIM(TIM4, CH4, PB9, TIM_USE_PPM, 0, 0 ), // PPM IN + DEF_TIM(TIM4, CH2, PB7, TIM_USE_LED, 0, 0), // LED - DMA1_ST3 }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/FF_FORTINIF4/target.h b/src/main/target/FF_FORTINIF4/target.h index 79e6de2ba5b..44a87b5e692 100644 --- a/src/main/target/FF_FORTINIF4/target.h +++ b/src/main/target/FF_FORTINIF4/target.h @@ -18,7 +18,6 @@ #pragma once #define TARGET_BOARD_IDENTIFIER "FORT" #define USBD_PRODUCT_STRING "FortiniF4" -#define TARGET_CONFIG #define USE_HARDWARE_REVISION_DETECTION #define HW_PIN PC14 /*--------------LED----------------*/ diff --git a/src/main/target/FF_FORTINIF4/target.mk b/src/main/target/FF_FORTINIF4/target.mk index a6b838a95e4..581916350df 100644 --- a/src/main/target/FF_FORTINIF4/target.mk +++ b/src/main/target/FF_FORTINIF4/target.mk @@ -4,5 +4,4 @@ FEATURES += VCP ONBOARDFLASH TARGET_SRC = \ drivers/accgyro/accgyro_mpu6500.c \ drivers/max7456.c \ - drivers/light_ws2811strip.c \ - drivers/light_ws2811strip_stdperiph.c + drivers/light_ws2811strip.c \ No newline at end of file diff --git a/src/main/target/FF_PIKOF4/config.c b/src/main/target/FF_PIKOF4/config.c index 513db9a5976..14e365618ab 100644 --- a/src/main/target/FF_PIKOF4/config.c +++ b/src/main/target/FF_PIKOF4/config.c @@ -20,13 +20,9 @@ #include -#ifdef USE_TARGET_CONFIG #include "rx/rx.h" - #include "telemetry/telemetry.h" - #include "sensors/battery.h" - #define CURRENTSCALE 250 void targetConfiguration(void) @@ -35,4 +31,3 @@ void targetConfiguration(void) telemetryConfigMutable()->smartportUartUnidirectional = true; batteryMetersConfigMutable()->current.scale = CURRENTSCALE; } -#endif diff --git a/src/main/target/FF_PIKOF4/target.c b/src/main/target/FF_PIKOF4/target.c index 34083d8dc6a..14a5ab95d5b 100644 --- a/src/main/target/FF_PIKOF4/target.c +++ b/src/main/target/FF_PIKOF4/target.c @@ -26,30 +26,19 @@ const timerHardware_t timerHardware[] = { #if defined(FF_PIKOF4OSD) - { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM_EN, IOCFG_AF_PP, GPIO_AF_TIM2, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR }, // S1_OUT // TIM2_CH4 | TIM5_CH4 | TIM9_CH2 - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM_EN, IOCFG_AF_PP, GPIO_AF_TIM3, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR }, // S2_OUT // TIM1_CH3N | TIM3_CH4 | TIM8_CH3N - { TIM5, IO_TAG(PA2), TIM_Channel_3, TIM_EN, IOCFG_AF_PP, GPIO_AF_TIM5, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // S3_OUT // TIM2_CH3 | TIM5_CH3 | TIM9_CH1 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM_EN, IOCFG_AF_PP, GPIO_AF_TIM3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // S4_OUT // TIM1_CH2N | TIM3_CH3 | TIM8_CH2N - { TIM12, IO_TAG(PB14), TIM_Channel_1, TIM_EN, IOCFG_AF_PP, GPIO_AF_TIM12, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // RC4 // TIM1_CH2N | TIM8_CH2N | TIM12_CH1 - { TIM12, IO_TAG(PB15), TIM_Channel_2, TIM_EN, IOCFG_AF_PP, GPIO_AF_TIM12, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // RC5 // TIM1_CH3N | TIM8_CH3N | TIM12_CH2 -// DEF_TIM(TIM5, CH4, PA3, TIM_USE_MOTOR, 0, 0 ), // S1_OUT - DMA1_ST7 -// DEF_TIM(TIM3, CH3, PB1, TIM_USE_MOTOR, 0, 0 ), // S2_OUT - DMA1_ST1 -// DEF_TIM(TIM5, CH3, PA2, TIM_USE_MOTOR, 0, 0 ), // S3_OUT - DMA1_ST6 -// DEF_TIM(TIM3, CH4, PB0, TIM_USE_MOTOR, 0, 0 ), // S4_OUT - DMA1_ST2 -// DEF_TIM(TIM8, CH2N, PB14, TIM_USE_MOTOR, 0, 0 ), // PA14 RC4 - DMA2_ST6, *DMA2_ST2 -// DEF_TIM(TIM1, CH3N, PB15, TIM_USE_MOTOR, 0, 0 ), // PA15 RC5 - DMA2_ST6, DMA2_ST6 + DEF_TIM(TIM2, CH4, PA3, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0 ), + DEF_TIM(TIM3, CH3, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0 ), + DEF_TIM(TIM5, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), + DEF_TIM(TIM3, CH4, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), + DEF_TIM(TIM12, CH1, PB14, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), + DEF_TIM(TIM12, CH2, PB15, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), #else - { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM_EN, IOCFG_AF_PP, GPIO_AF_TIM2, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR }, // S1_OUT // TIM2_CH4 | TIM5_CH4 | TIM9_CH2 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM_EN, IOCFG_AF_PP, GPIO_AF_TIM3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // S2_OUT // TIM1_CH2N | TIM3_CH3 | TIM8_CH2N - { TIM5, IO_TAG(PA2), TIM_Channel_3, TIM_EN, IOCFG_AF_PP, GPIO_AF_TIM5, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // S3_OUT // TIM2_CH3 | TIM5_CH3 | TIM9_CH1 - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM_EN, IOCFG_AF_PP, GPIO_AF_TIM3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // S4_OUT // TIM1_CH3N | TIM3_CH4 | TIM8_CH3N -// DEF_TIM(TIM2, CH4, PA3, TIM_USE_MOTOR, 0, 1 ), // S1_OUT - DMA1_ST6 -// DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR, 0, 0 ), // S2_OUT - DMA1_ST7 -// DEF_TIM(TIM2, CH3, PA2, TIM_USE_MOTOR, 0, 0 ), // S3_OUT - DMA1_ST1 -// DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, 0, 0 ), // S4_OUT - DMA1_ST2 + DEF_TIM(TIM2, CH4, PA3, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0 ), + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), + DEF_TIM(TIM5, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), #endif - { TIM4, IO_TAG(PB7), TIM_Channel_2, 0, IOCFG_AF_PP, GPIO_AF_TIM4, TIM_USE_LED | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO }, // LED -// DEF_TIM(TIM4, CH2, PB7, TIM_USE_LED, 0, 0 ), // LED - DMA1_ST3 + DEF_TIM(TIM4, CH2, PB7, TIM_USE_LED, 0, 0 ), }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/FF_PIKOF4/target.h b/src/main/target/FF_PIKOF4/target.h index 0d4a48d7f7b..5ec9a8f634c 100644 --- a/src/main/target/FF_PIKOF4/target.h +++ b/src/main/target/FF_PIKOF4/target.h @@ -23,7 +23,6 @@ #define TARGET_BOARD_IDENTIFIER "PIK4" #define USBD_PRODUCT_STRING "PikoF4" #endif -#define USE_TARGET_CONFIG /*--------------LED----------------*/ #if defined(FF_PIKOF4OSD) #define LED0 PB5 diff --git a/src/main/target/FF_PIKOF4/target.mk b/src/main/target/FF_PIKOF4/target.mk index f6b7d8dd416..234c6ad4458 100644 --- a/src/main/target/FF_PIKOF4/target.mk +++ b/src/main/target/FF_PIKOF4/target.mk @@ -5,6 +5,5 @@ TARGET_SRC = \ drivers/accgyro/accgyro_mpu6000.c \ drivers/accgyro/accgyro_mpu6500.c \ drivers/max7456.c \ - drivers/light_ws2811strip.c \ - drivers/light_ws2811strip_stdperiph.c + drivers/light_ws2811strip.c \ No newline at end of file diff --git a/src/main/target/FIREWORKSV2/OMNIBUSF4V6.mk b/src/main/target/FIREWORKSV2/OMNIBUSF4V6.mk new file mode 100644 index 00000000000..b47e4817c22 --- /dev/null +++ b/src/main/target/FIREWORKSV2/OMNIBUSF4V6.mk @@ -0,0 +1 @@ +#OMNIBUSF4V6 diff --git a/src/main/target/FIREWORKSV2/config.c b/src/main/target/FIREWORKSV2/config.c index a00a96f555b..ed075a2a9bc 100644 --- a/src/main/target/FIREWORKSV2/config.c +++ b/src/main/target/FIREWORKSV2/config.c @@ -25,7 +25,6 @@ #include #include -#ifdef TARGET_CONFIG #include "drivers/io.h" #include "rx/rx.h" #include "io/serial.h" @@ -34,4 +33,3 @@ void targetConfiguration(void) { serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(TELEMETRY_UART)].functionMask = FUNCTION_TELEMETRY_SMARTPORT; } -#endif diff --git a/src/main/target/FIREWORKSV2/target.c b/src/main/target/FIREWORKSV2/target.c index 51fa5fcde46..a232dd5881d 100644 --- a/src/main/target/FIREWORKSV2/target.c +++ b/src/main/target/FIREWORKSV2/target.c @@ -30,26 +30,41 @@ #include "drivers/timer.h" #include "drivers/bus.h" -#define DEF_TIM_CHNL_CH1 TIM_Channel_1 -#define DEF_TIM_CHNL_CH2 TIM_Channel_2 -#define DEF_TIM_CHNL_CH3 TIM_Channel_3 -#define DEF_TIM_CHNL_CH4 TIM_Channel_4 - -#define DEF_TIM(_tim, _ch, _pin, _usage, _flags) \ - { _tim, IO_TAG(_pin), DEF_TIM_CHNL_##_ch, _flags, IOCFG_AF_PP, GPIO_AF_##_tim, _usage } +#include "drivers/pwm_output.h" +#include "common/maths.h" +#include "fc/config.h" const timerHardware_t timerHardware[] = { - DEF_TIM(TIM10, CH1, PB8, TIM_USE_PPM, 0), // PPM + DEF_TIM(TIM10, CH1, PB8, TIM_USE_PPM, 0, 0), // PPM + + // Motor output 1: use different set of timers for MC and FW + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR, 1, 0), // S1_OUT D(1,7) + DEF_TIM(TIM8, CH2N, PB0, TIM_USE_FW_MOTOR, 1, 1), // S1_OUT D(2,2,0),D(2,3,7) - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 1), // S1_OUT - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 1), // S2_OUT - DEF_TIM(TIM9, CH2, PA3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 1), // S3_OUT - DEF_TIM(TIM3, CH2, PB5, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 1), // S4_OUT + // Motor output 2: use different set of timers for MC and FW + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR, 1, 0), // S2_OUT D(1,2) + DEF_TIM(TIM8, CH3N, PB1, TIM_USE_FW_MOTOR, 1, 1), // S2_OUT D(2,2,0),D(2,4,7) - DEF_TIM(TIM4, CH1, PB6, TIM_USE_LED, 0), // LED strip + DEF_TIM(TIM2, CH4, PA3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 1, 1), // S3_OUT D(1,6) + DEF_TIM(TIM3, CH2, PB5, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 1, 0), // S4_OUT D(1,5) - DEF_TIM(TIM1, CH2, PA9, TIM_USE_ANY, 0), // SS1 + DEF_TIM(TIM4, CH1, PB6, TIM_USE_LED, 0, 0), // LED strip D(1,0) + + DEF_TIM(TIM1, CH2, PA9, TIM_USE_ANY, 0, 0), // SS1 }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); + +#ifdef USE_DSHOT +void validateAndFixTargetConfig(void) +{ + // On airplanes DSHOT is not supported on this target + if (mixerConfig()->platformType != PLATFORM_MULTIROTOR && mixerConfig()->platformType != PLATFORM_TRICOPTER) { + if (motorConfig()->motorPwmProtocol >= PWM_TYPE_DSHOT150) { + motorConfigMutable()->motorPwmProtocol = PWM_TYPE_STANDARD; + motorConfigMutable()->motorPwmRate = MIN(motorConfig()->motorPwmRate, 490); + } + } +} +#endif \ No newline at end of file diff --git a/src/main/target/FIREWORKSV2/target.h b/src/main/target/FIREWORKSV2/target.h index aded631dfad..f5aab377b24 100644 --- a/src/main/target/FIREWORKSV2/target.h +++ b/src/main/target/FIREWORKSV2/target.h @@ -24,9 +24,19 @@ #pragma once +#if defined(OMNIBUSF4V6) +#define TARGET_BOARD_IDENTIFIER "OBV6" +#else #define TARGET_BOARD_IDENTIFIER "FWX2" +#endif -#define USBD_PRODUCT_STRING "OMNIBUS F4 FWX V2" +#if defined(OMNIBUSF4V6) +#define USBD_PRODUCT_STRING "OmnibusF4 V6" +#else +#define USBD_PRODUCT_STRING "OMNIBUS F4 FWX V2" +#endif + +#define USE_DSHOT // Status LED #define LED0 PA8 @@ -37,8 +47,15 @@ // I2C #define USE_I2C +#if defined(OMNIBUSF4V6) +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB8 // SCL PIN,alt MST8 +#define I2C1_SDA PB9 // SDA PIN,alt MST7 +#define DEFAULT_I2C_BUS BUS_I2C1 +#else #define USE_I2C_DEVICE_2 #define I2C_DEVICE_2_SHARES_UART3 +#endif #define USE_EXTI #define GYRO_INT_EXTI PC8 @@ -49,12 +66,20 @@ #define USE_GYRO_MPU6500 #define USE_ACC_MPU6500 + +#if defined(OMNIBUSF4V6) +#define MPU6500_CS_PIN PC14 +#define MPU6500_SPI_BUS BUS_SPI1 +#define GYRO_MPU6500_ALIGN CW0_DEG +#define ACC_MPU6500_ALIGN CW0_DEG +#else #define MPU6500_CS_PIN PD2 #define MPU6500_SPI_BUS BUS_SPI3 #define GYRO_MPU6500_ALIGN CW180_DEG #define ACC_MPU6500_ALIGN CW180_DEG +#endif -// OmnibusF4 Nano v6 has a MPU6000 +// OmnibusF4 Nano v6 and OmnibusF4 V6 has a MPU6000 #define USE_GYRO_MPU6000 #define USE_ACC_MPU6000 #define MPU6000_CS_PIN PA4 @@ -63,24 +88,43 @@ #define ACC_MPU6000_ALIGN CW180_DEG #define USE_MAG +#if defined(OMNIBUSF4V6) +#define MAG_I2C_BUS BUS_I2C1 +#else #define MAG_I2C_BUS BUS_I2C2 +#endif #define USE_MAG_HMC5883 #define USE_MAG_QMC5883 #define USE_MAG_IST8310 #define USE_MAG_MAG3110 #define USE_MAG_LIS3MDL -#define USE_BARO +#if defined(OMNIBUSF4V6) +#define TEMPERATURE_I2C_BUS BUS_I2C1 +#else +#define TEMPERATURE_I2C_BUS BUS_I2C2 +#endif +#define USE_BARO #define USE_BARO_BMP280 -#define BMP280_SPI_BUS BUS_SPI3 -#define BMP280_CS_PIN PB3 - -#define USE_PITOT_MS4525 +#define BMP280_SPI_BUS BUS_SPI3 +#define BMP280_CS_PIN PB3 +#if defined(OMNIBUSF4V6) +#define BARO_I2C_BUS BUS_I2C1 +#endif + +#if defined(OMNIBUSF4V6) +#define PITOT_I2C_BUS BUS_I2C1 +#else #define PITOT_I2C_BUS BUS_I2C2 +#endif #define USE_RANGEFINDER +#if defined(OMNIBUSF4V6) +#define RANGEFINDER_I2C_BUS BUS_I2C1 +#else #define RANGEFINDER_I2C_BUS BUS_I2C2 +#endif #define USE_RANGEFINDER_HCSR04_I2C #define USE_VCP @@ -170,9 +214,11 @@ #define SERIALRX_UART SERIAL_PORT_USART1 #define SMARTAUDIO_UART SERIAL_PORT_USART4 -#define TARGET_CONFIG +//Default values for OmnibusF4V6,calib values for FireworksV2 +#if !defined(OMNIBUSF4V6) #define CURRENT_METER_SCALE 175 #define CURRENT_METER_OFFSET 326 +#endif #define USE_SERIAL_4WAY_BLHELI_INTERFACE @@ -185,4 +231,8 @@ #define TARGET_IO_PORTC 0xffff #define TARGET_IO_PORTD 0xffff -#define PCA9685_I2C_BUS BUS_I2C2 \ No newline at end of file +#if defined(OMNIBUSF4V6) +#define PCA9685_I2C_BUS BUS_I2C1 +#else +#define PCA9685_I2C_BUS BUS_I2C2 +#endif diff --git a/src/main/target/FIREWORKSV2/target.mk b/src/main/target/FIREWORKSV2/target.mk index 62e119d7ff0..b7423b68ca7 100644 --- a/src/main/target/FIREWORKSV2/target.mk +++ b/src/main/target/FIREWORKSV2/target.mk @@ -12,8 +12,6 @@ TARGET_SRC = \ drivers/compass/compass_ist8310.c \ drivers/compass/compass_mag3110.c \ drivers/compass/compass_lis3mdl.c \ - drivers/pitotmeter_ms4525.c \ drivers/pitotmeter_adc.c \ drivers/light_ws2811strip.c \ - drivers/light_ws2811strip_stdperiph.c \ drivers/max7456.c diff --git a/src/main/target/FISHDRONEF4/target.c b/src/main/target/FISHDRONEF4/target.c index 31805ed2f9e..5273c1d426a 100644 --- a/src/main/target/FISHDRONEF4/target.c +++ b/src/main/target/FISHDRONEF4/target.c @@ -23,21 +23,21 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[] = { - { TIM5, IO_TAG(PA0), TIM_Channel_1, 0, IOCFG_AF_PP , GPIO_AF_TIM5, TIM_USE_PPM | TIM_USE_PWM }, // S1_IN(PPM) - { TIM5, IO_TAG(PA1), TIM_Channel_2, 0, IOCFG_AF_PP , GPIO_AF_TIM5, TIM_USE_PWM }, // S2_IN - { TIM3, IO_TAG(PB0), TIM_Channel_3, 0, IOCFG_AF_PP , GPIO_AF_TIM3, TIM_USE_PWM }, // S3_IN - { TIM2, IO_TAG(PB10), TIM_Channel_3, 0, IOCFG_AF_PP , GPIO_AF_TIM2, TIM_USE_PWM }, // S4_IN - { TIM2, IO_TAG(PB11), TIM_Channel_4, 0, IOCFG_AF_PP , GPIO_AF_TIM2, TIM_USE_PWM }, // S5_IN - { TIM1, IO_TAG(PA8), TIM_Channel_1, 0, IOCFG_AF_PP , GPIO_AF_TIM1, TIM_USE_PWM }, // S6_IN + DEF_TIM(TIM5, CH1, PA0, TIM_USE_PWM | TIM_USE_PPM, 0, 0 ), // PPM IN + DEF_TIM(TIM5, CH2, PA1, TIM_USE_PWM, 0, 0 ), + DEF_TIM(TIM3, CH3, PB0, TIM_USE_PWM, 0, 0 ), + DEF_TIM(TIM2, CH3, PB10, TIM_USE_PWM, 0, 0 ), + DEF_TIM(TIM2, CH4, PB11, TIM_USE_PWM, 0, 0 ), + DEF_TIM(TIM1, CH1, PA8, TIM_USE_PWM, 0, 0 ), - { TIM12, IO_TAG(PB14), TIM_Channel_1, 1, IOCFG_AF_PP, GPIO_AF_TIM12, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR },// S1_OUT - { TIM12, IO_TAG(PB15), TIM_Channel_2, 1, IOCFG_AF_PP, GPIO_AF_TIM12, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR },// S2_OUT - { TIM8, IO_TAG(PC6), TIM_Channel_1, 1, IOCFG_AF_PP, GPIO_AF_TIM8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // S3_OUT - { TIM8, IO_TAG(PC7), TIM_Channel_2, 1, IOCFG_AF_PP, GPIO_AF_TIM8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // S4_OUT - { TIM8, IO_TAG(PC8), TIM_Channel_3, 1, IOCFG_AF_PP, GPIO_AF_TIM8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // S5_OUT - { TIM8, IO_TAG(PC9), TIM_Channel_4, 1, IOCFG_AF_PP, GPIO_AF_TIM8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // S6_OUT + DEF_TIM(TIM12, CH1, PB14, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0 ), + DEF_TIM(TIM12, CH2, PB15, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0 ), + DEF_TIM(TIM8, CH1, PC6, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), + DEF_TIM(TIM8, CH2, PC7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), + DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), + DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), - { TIM3, IO_TAG(PB1), TIM_Channel_4, 1, IOCFG_AF_PP , GPIO_AF_TIM3, TIM_USE_LED }, // LED_STRIP + DEF_TIM(TIM3, CH4, PB1, TIM_USE_LED, 0, 0 ), }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/FISHDRONEF4/target.h b/src/main/target/FISHDRONEF4/target.h index f9f1a95c4c5..9e8a1921cfc 100644 --- a/src/main/target/FISHDRONEF4/target.h +++ b/src/main/target/FISHDRONEF4/target.h @@ -68,6 +68,9 @@ #define USE_MAG_LIS3MDL #define MAG_IST8310_ALIGN CW270_DEG +// *************** Temperature sensor ***************** +#define TEMPERATURE_I2C_BUS BUS_I2C1 + // *************** BARO ***************************** #define USE_BARO #define BARO_I2C_BUS BUS_I2C1 @@ -91,17 +94,14 @@ #define SPI3_MISO_PIN PB4 #define SPI3_MOSI_PIN PB5 #define SPI3_NSS_PIN PB6 +#define SPI3_CLOCK_LEADING_EDGE #define USE_SDCARD +#define USE_SDCARD_SPI #define SDCARD_DETECT_INVERTED -#define SDCARD_DETECT_PIN PB7 -#define SDCARD_SPI_INSTANCE SPI3 -#define SDCARD_SPI_CS_PIN PB6 - -#define SDCARD_DMA_CHANNEL_TX DMA1_Stream5 -#define SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG DMA_FLAG_TCIF5 -#define SDCARD_DMA_CLK RCC_AHB1Periph_DMA1 -#define SDCARD_DMA_CHANNEL DMA_Channel_0 +#define SDCARD_DETECT_PIN PB7 +#define SDCARD_SPI_BUS BUS_SPI3 +#define SDCARD_CS_PIN PB6 // *************** Flash ***************************** #define M25P16_CS_PIN PA15 @@ -135,14 +135,6 @@ // *************** WS2811 ***************************** #define USE_LED_STRIP #define WS2811_PIN PB1 -#define WS2811_TIMER TIM3 -#define WS2811_TIMER_CHANNEL TIM_Channel_4 -#define WS2811_DMA_HANDLER_IDENTIFER DMA1_ST2_HANDLER -#define WS2811_DMA_STREAM DMA1_Stream2 -#define WS2811_DMA_FLAG DMA_FLAG_TCIF2 -#define WS2811_DMA_IT DMA_IT_TCIF2 -#define WS2811_DMA_CHANNEL DMA_Channel_5 -#define WS2811_DMA_IRQ DMA1_Stream2_IRQn // *************** IIC ***************************** #define USE_I2C diff --git a/src/main/target/FISHDRONEF4/target.mk b/src/main/target/FISHDRONEF4/target.mk index 2efb86bbe2b..ab900e2a149 100644 --- a/src/main/target/FISHDRONEF4/target.mk +++ b/src/main/target/FISHDRONEF4/target.mk @@ -14,5 +14,4 @@ TARGET_SRC = \ drivers/compass/compass_mag3110.c \ drivers/compass/compass_lis3mdl.c \ drivers/light_ws2811strip.c \ - drivers/light_ws2811strip_stdperiph.c\ drivers/max7456.c diff --git a/src/main/target/FOXEERF405/target.c b/src/main/target/FOXEERF405/target.c new file mode 100644 index 00000000000..2a2edd874b3 --- /dev/null +++ b/src/main/target/FOXEERF405/target.c @@ -0,0 +1,39 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + */ + +#include +#include +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" +#include "drivers/timer.h" + +const timerHardware_t timerHardware[] = { + DEF_TIM(TIM12, CH2, PB15, TIM_USE_PPM, 0, 0), // PPM + + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1 (1,7) + DEF_TIM(TIM3, CH1, PB4, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S2 (1,4) + DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S3 (2,4) + DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4 (2,7) + DEF_TIM(TIM8, CH1, PC6, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S5 (2,2) + DEF_TIM(TIM8, CH2, PC7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S6 (2,3) + + DEF_TIM(TIM1, CH3, PA10, TIM_USE_LED, 0, 0), // LED STRIP(2,6) + + //DEF_TIM(TIM2, CH2, PB3, TIM_USE_CAMERA_CONTROL, 0, 0), // Reserved (Cam Control for BF) +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/FOXEERF405/target.h b/src/main/target/FOXEERF405/target.h new file mode 100644 index 00000000000..625088b4f63 --- /dev/null +++ b/src/main/target/FOXEERF405/target.h @@ -0,0 +1,153 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + */ + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "FXF4" +#define USBD_PRODUCT_STRING "FOXEERF405" + +/*** Indicators ***/ +#define LED0 PC15 +#define BEEPER PA4 +#define BEEPER_INVERTED + +/*** IMU sensors ***/ +#define USE_EXTI +#define USE_GYRO +#define USE_ACC + +#define GYRO_INT_EXTI PC4 +#define USE_MPU_DATA_READY_SIGNAL + +// MPU6000 +#define USE_GYRO_MPU6000 +#define USE_ACC_MPU6000 +#define MPU6000_CS_PIN PB2 +#define MPU6000_SPI_BUS BUS_SPI1 +#define GYRO_MPU6000_ALIGN CW90_DEG +#define ACC_MPU6000_ALIGN CW90_DEG + +// ICM20689 - handled by MPU6500 driver +#define USE_GYRO_MPU6500 +#define USE_ACC_MPU6500 +#define MPU6500_CS_PIN PB2 +#define MPU6500_SPI_BUS BUS_SPI1 +#define GYRO_MPU6500_ALIGN CW90_DEG +#define ACC_MPU6500_ALIGN CW90_DEG + +/*** SPI/I2C bus ***/ +#define USE_SPI +#define USE_SPI_DEVICE_1 +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 + +#define USE_SPI_DEVICE_2 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PC3 + +#define USE_SPI_DEVICE_3 +#define SPI3_SCK_PIN PC10 +#define SPI3_MISO_PIN PC11 +#define SPI3_MOSI_PIN PB5 + +#define USE_I2C +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB8 +#define I2C1_SDA PB9 + +/*** Onboard flash ***/ +#define USE_FLASHFS +#define USE_FLASH_M25P16 +#define M25P16_CS_PIN PB12 +#define M25P16_SPI_BUS BUS_SPI2 + +/*** OSD ***/ +#define USE_OSD +#define USE_MAX7456 +#define MAX7456_SPI_BUS BUS_SPI3 +#define MAX7456_CS_PIN PA15 + +/*** Serial ports ***/ +#define USE_VCP + +#define USE_UART1 +#define UART1_RX_PIN PB7 +#define UART1_TX_PIN PA9 + +#define USE_UART2 +#define UART2_RX_PIN PA3 +#define UART2_TX_PIN PA2 + +#define USE_UART3 +#define UART3_RX_PIN PB11 +#define UART3_TX_PIN PB10 + +#define USE_UART4 +#define UART4_RX_PIN PA1 +#define UART4_TX_PIN PA0 + +#define USE_UART5 +#define UART5_RX_PIN PD2 +#define UART5_TX_PIN PC12 + +#define SERIAL_PORT_COUNT 6 + +/*** BARO & MAG ***/ +#define USE_BARO +#define BARO_I2C_BUS BUS_I2C1 +#define USE_BARO_BMP280 +#define USE_BARO_MS5611 +#define USE_BARO_BMP085 + +#define USE_MAG +#define MAG_I2C_BUS BUS_I2C2 +#define USE_MAG_HMC5883 +#define USE_MAG_QMC5883 +#define USE_MAG_IST8310 +#define USE_MAG_MAG3110 +#define USE_MAG_LIS3MDL + +/*** ADC ***/ +#define USE_ADC +#define ADC_CHANNEL_1_PIN PC0 +#define ADC_CHANNEL_2_PIN PC1 + +#define VBAT_ADC_CHANNEL ADC_CHN_1 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 + +/*** LED STRIP ***/ +#define USE_LED_STRIP +#define WS2811_PIN PA10 + +/*** Default settings ***/ +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_SBUS +#define CURRENT_METER_SCALE 166 + +/*** Timer/PWM output ***/ +#define USE_SERIAL_4WAY_BLHELI_INTERFACE +#define MAX_PWM_OUTPUT_PORTS 6 +#define USE_DSHOT + +/*** Used pins ***/ +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD (BIT(2)) \ No newline at end of file diff --git a/src/main/target/FOXEERF405/target.mk b/src/main/target/FOXEERF405/target.mk new file mode 100644 index 00000000000..23c3774fdfe --- /dev/null +++ b/src/main/target/FOXEERF405/target.mk @@ -0,0 +1,15 @@ +F405_TARGETS += $(TARGET) +FEATURES += VCP ONBOARDFLASH +TARGET_SRC = \ + drivers/accgyro/accgyro_mpu6000.c \ + drivers/accgyro/accgyro_mpu6500.c \ + drivers/barometer/barometer_bmp085.c \ + drivers/barometer/barometer_bmp280.c \ + drivers/barometer/barometer_ms56xx.c \ + drivers/compass/compass_hmc5883l.c \ + drivers/compass/compass_qmc5883l.c \ + drivers/compass/compass_ist8310.c \ + drivers/compass/compass_mag3110.c \ + drivers/compass/compass_lis3mdl.c \ + drivers/light_ws2811strip.c \ + drivers/max7456.c diff --git a/src/main/target/FOXEERF722DUAL/target.c b/src/main/target/FOXEERF722DUAL/target.c new file mode 100644 index 00000000000..27d080d6637 --- /dev/null +++ b/src/main/target/FOXEERF722DUAL/target.c @@ -0,0 +1,43 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + */ + +#include +#include +#include "drivers/io.h" +#include "drivers/bus.h" +#include "drivers/timer.h" +#include "drivers/pwm_mapping.h" + +BUSDEV_REGISTER_SPI_TAG(busdev_mpu6000, DEVHW_MPU6000, MPU6000_SPI_BUS, MPU6000_CS_PIN, MPU6000_EXTI_PIN, 0, DEVFLAGS_NONE); +BUSDEV_REGISTER_SPI_TAG(busdev_mpu6500, DEVHW_MPU6500, MPU6500_SPI_BUS, MPU6500_CS_PIN, MPU6500_EXTI_PIN, 1, DEVFLAGS_NONE); + +const timerHardware_t timerHardware[] = { + DEF_TIM(TIM4, CH2, PB7, TIM_USE_PPM, 0, 0), // PPM&SBUS + + DEF_TIM(TIM1, CH2, PA9, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 1), // S1 - D(2, 1, 6) + DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S2 - D(2, 6, 0) + DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S3 - D(2, 7, 7) + DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4 - D(2, 4, 7) + DEF_TIM(TIM3, CH1, PC6, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S5 - D(1, 4, 5) + DEF_TIM(TIM3, CH2, PC7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S6 - D(1, 5, 5) + + DEF_TIM(TIM2, CH1, PA15, TIM_USE_LED, 0, 0), // LED STRIP(1,5) + + //DEF_TIM(TIM2, CH2, PB3, TIM_USE_CAMERA_CONTROL, 0, 0), // Reserved (Cam Control for BF) +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/FOXEERF722DUAL/target.h b/src/main/target/FOXEERF722DUAL/target.h new file mode 100644 index 00000000000..c8a07d2fcd3 --- /dev/null +++ b/src/main/target/FOXEERF722DUAL/target.h @@ -0,0 +1,161 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + */ + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "FXF7" +#define USBD_PRODUCT_STRING "FOXEER722DUAL" + +/*** Indicators ***/ +#define LED0 PC15 +#define BEEPER PA4 +#define BEEPER_INVERTED + +/*** IMU sensors ***/ +#define USE_EXTI +#define USE_ACC +#define USE_GYRO + +// We use dual IMU sensors, they have to be described in the target file +#define USE_TARGET_IMU_HARDWARE_DESCRIPTORS +#define USE_MPU_DATA_READY_SIGNAL +#define USE_DUAL_GYRO + +// MPU6000 +#define USE_GYRO_MPU6000 +#define USE_ACC_MPU6000 + +#define MPU6000_CS_PIN PB2 +#define MPU6000_SPI_BUS BUS_SPI1 +#define MPU6000_EXTI_PIN PC4 +#define GYRO_MPU6000_ALIGN CW270_DEG +#define ACC_MPU6000_ALIGN CW270_DEG + +// ICM20602 - handled by MPU6500 driver +#define USE_GYRO_MPU6500 +#define USE_ACC_MPU6500 + +#define MPU6500_CS_PIN PB1 +#define MPU6500_SPI_BUS BUS_SPI1 +#define MPU6500_EXTI_PIN PB0 +#define GYRO_MPU6500_ALIGN CW180_DEG +#define ACC_MPU6500_ALIGN CW180_DEG + +/*** SPI/I2C bus ***/ +#define USE_SPI +#define USE_SPI_DEVICE_1 +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 + +#define USE_SPI_DEVICE_2 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 + +#define USE_SPI_DEVICE_3 +#define SPI3_SCK_PIN PC10 +#define SPI3_MISO_PIN PC11 +#define SPI3_MOSI_PIN PB5 + +#define USE_I2C +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB8 +#define I2C1_SDA PB9 + +/*** Onboard flash ***/ +#define USE_FLASHFS +#define USE_FLASH_M25P16 +#define M25P16_CS_PIN PB12 +#define M25P16_SPI_BUS BUS_SPI2 + +/*** OSD ***/ +#define USE_OSD +#define USE_MAX7456 +#define MAX7456_SPI_BUS BUS_SPI3 +#define MAX7456_CS_PIN PC3 + +/*** Serial ports ***/ +#define USE_VCP + +#define USE_UART1 +#define UART1_TX_PIN PB6 +#define UART1_RX_PIN PB7 + +#define USE_UART2 +#define UART2_TX_PIN PA2 +#define UART2_RX_PIN PA3 + +#define USE_UART3 +#define UART3_TX_PIN PB10 +#define UART3_RX_PIN PB11 + +#define USE_UART4 +#define UART4_TX_PIN PA0 +#define UART4_RX_PIN PA1 + +#define USE_UART5 +#define UART5_TX_PIN PC12 +#define UART5_RX_PIN PD2 + +#define SERIAL_PORT_COUNT 6 + +/*** BARO & MAG ***/ +#define USE_BARO +#define BARO_I2C_BUS BUS_I2C1 +#define USE_BARO_BMP280 +#define USE_BARO_MS5611 + +#define USE_MAG +#define MAG_I2C_BUS BUS_I2C2 +#define USE_MAG_HMC5883 +#define USE_MAG_QMC5883 +#define USE_MAG_IST8310 +#define USE_MAG_MAG3110 +#define USE_MAG_LIS3MDL + +/*** ADC ***/ +#define USE_ADC +#define ADC_CHANNEL_1_PIN PC0 +#define ADC_CHANNEL_2_PIN PC2 +#define ADC_CHANNEL_3_PIN PA0 + +#define VBAT_ADC_CHANNEL ADC_CHN_1 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 +#define RSSI_ADC_CHANNEL ADC_CHN_3 + +/*** LED STRIP ***/ +#define USE_LED_STRIP +#define WS2811_PIN PA15 + +/*** Default settings ***/ +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT +#define SERIALRX_UART SERIAL_PORT_USART1 +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_SBUS +//#define CURRENT_METER_SCALE 166 + +/*** Timer/PWM output ***/ +#define USE_SERIAL_4WAY_BLHELI_INTERFACE +#define MAX_PWM_OUTPUT_PORTS 6 +#define USE_DSHOT + +/*** Used pins ***/ +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD (BIT(2)) \ No newline at end of file diff --git a/src/main/target/FOXEERF722DUAL/target.mk b/src/main/target/FOXEERF722DUAL/target.mk new file mode 100644 index 00000000000..ea8a3a9209f --- /dev/null +++ b/src/main/target/FOXEERF722DUAL/target.mk @@ -0,0 +1,14 @@ +F7X2RE_TARGETS += $(TARGET) +FEATURES += VCP ONBOARDFLASH +TARGET_SRC = \ + drivers/accgyro/accgyro_mpu6000.c \ + drivers/accgyro/accgyro_mpu6500.c \ + drivers/barometer/barometer_bmp280.c \ + drivers/barometer/barometer_ms56xx.c \ + drivers/compass/compass_hmc5883l.c \ + drivers/compass/compass_qmc5883l.c \ + drivers/compass/compass_ist8310.c \ + drivers/compass/compass_mag3110.c \ + drivers/compass/compass_lis3mdl.c \ + drivers/light_ws2811strip.c \ + drivers/max7456.c diff --git a/src/main/target/FRSKYF3/config.c b/src/main/target/FRSKYF3/config.c index d39e3c48af8..09bf9765939 100644 --- a/src/main/target/FRSKYF3/config.c +++ b/src/main/target/FRSKYF3/config.c @@ -18,7 +18,6 @@ #include #include -#ifdef TARGET_CONFIG #include "drivers/io.h" #include "rx/rx.h" #include "io/serial.h" @@ -28,4 +27,3 @@ void targetConfiguration(void) serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(TELEMETRY_UART)].functionMask = FUNCTION_TELEMETRY_SMARTPORT; rxConfigMutable()->rssi_channel = 8; } -#endif diff --git a/src/main/target/FRSKYF3/target.c b/src/main/target/FRSKYF3/target.c index 707a7aa0b3c..1a7c406e954 100644 --- a/src/main/target/FRSKYF3/target.c +++ b/src/main/target/FRSKYF3/target.c @@ -23,17 +23,16 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[] = { + DEF_TIM(TIM8, CH3, PB9, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0), // PWM1 + DEF_TIM(TIM8, CH2, PB8, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0), // PWM2 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // PWM3 + DEF_TIM(TIM2, CH2, PA1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // PWM4 + DEF_TIM(TIM2, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // PWM5 + DEF_TIM(TIM2, CH4, PA3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // PWM6 + DEF_TIM(TIM2, CH1, PA0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // PWM7 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // PWM8 - { TIM4, IO_TAG(PB9), TIM_Channel_4, 1, IOCFG_AF_PP, GPIO_AF_2, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR }, // PWM1 - { TIM4, IO_TAG(PB8), TIM_Channel_3, 1, IOCFG_AF_PP, GPIO_AF_2, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR }, // PWM2 - { TIM3, IO_TAG(PB1), TIM_Channel_4, 1, IOCFG_AF_PP, GPIO_AF_2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // PWM3 - { TIM2, IO_TAG(PA1), TIM_Channel_2, 1, IOCFG_AF_PP, GPIO_AF_1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // PWM4 - { TIM2, IO_TAG(PA2), TIM_Channel_3, 1, IOCFG_AF_PP, GPIO_AF_1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // PWM5 - { TIM2, IO_TAG(PA3), TIM_Channel_4, 1, IOCFG_AF_PP, GPIO_AF_1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // PWM6 - { TIM2, IO_TAG(PA0), TIM_Channel_1, 1, IOCFG_AF_PP, GPIO_AF_1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // PWM7 - { TIM3, IO_TAG(PB0), TIM_Channel_3, 1, IOCFG_AF_PP, GPIO_AF_2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // PWM8 - - // DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED|TIM_USE_TRANSPONDER, TIMER_OUTPUT_ENABLED), // LED + DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0), // LED }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/FRSKYF3/target.h b/src/main/target/FRSKYF3/target.h index d948bca2836..13a2bfad993 100644 --- a/src/main/target/FRSKYF3/target.h +++ b/src/main/target/FRSKYF3/target.h @@ -18,7 +18,6 @@ #pragma once #define TARGET_BOARD_IDENTIFIER "FRF3" -#define TARGET_CONFIG #define LED0_PIN PB3 #define BEEPER PC15 @@ -83,13 +82,14 @@ #define SPI1_SCK_PIN PA5 #define SPI1_MISO_PIN PA6 #define SPI1_MOSI_PIN PA7 +#define SPI1_CLOCK_LEADING_EDGE #define USE_SDCARD - +#define USE_SDCARD_SPI #define SDCARD_DETECT_INVERTED -#define SDCARD_DETECT_PIN PB5 -#define SDCARD_SPI_INSTANCE SPI1 -#define SDCARD_SPI_CS_PIN SPI1_NSS_PIN +#define SDCARD_DETECT_PIN PB5 +#define SDCARD_SPI_BUS BUS_SPI1 +#define SDCARD_CS_PIN SPI1_NSS_PIN #define USE_ADC #define ADC_INSTANCE ADC2 diff --git a/src/main/target/FRSKYF4/config.c b/src/main/target/FRSKYF4/config.c index d39e3c48af8..09bf9765939 100755 --- a/src/main/target/FRSKYF4/config.c +++ b/src/main/target/FRSKYF4/config.c @@ -18,7 +18,6 @@ #include #include -#ifdef TARGET_CONFIG #include "drivers/io.h" #include "rx/rx.h" #include "io/serial.h" @@ -28,4 +27,3 @@ void targetConfiguration(void) serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(TELEMETRY_UART)].functionMask = FUNCTION_TELEMETRY_SMARTPORT; rxConfigMutable()->rssi_channel = 8; } -#endif diff --git a/src/main/target/FRSKYF4/target.c b/src/main/target/FRSKYF4/target.c index 043e19c675c..11d40beb500 100755 --- a/src/main/target/FRSKYF4/target.c +++ b/src/main/target/FRSKYF4/target.c @@ -22,30 +22,15 @@ #include "drivers/timer.h" #include "drivers/pwm_mapping.h" -#define DEF_TIM_CHNL_CH1 TIM_Channel_1 -#define DEF_TIM_CHNL_CH2 TIM_Channel_2 -#define DEF_TIM_CHNL_CH3 TIM_Channel_3 -#define DEF_TIM_CHNL_CH4 TIM_Channel_4 - -#define DEF_TIM(_tim, _ch, _pin, _usage, _flags) \ - { _tim, IO_TAG(_pin), DEF_TIM_CHNL_##_ch, _flags, IOCFG_AF_PP, GPIO_AF_##_tim, _usage } - const timerHardware_t timerHardware[] = { - // DEF_TIM(TIM4, CH3, PB8, TIM_USE_PWM | TIM_USE_PPM, 0), // PPM - // DEF_TIM(TIM4, CH4, PB9, TIM_USE_PWM, 0), // S2_IN - // DEF_TIM(TIM8, CH1, PC6, TIM_USE_PWM, 0), // S3_IN, UART6_TX - // DEF_TIM(TIM8, CH2, PC7, TIM_USE_PWM, 0), // S4_IN, UART6_RX - // DEF_TIM(TIM8, CH3, PC8, TIM_USE_PWM, 0), // S5_IN - // DEF_TIM(TIM8, CH4, PC9, TIM_USE_PWM, 0), // S6_IN - - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 1), // S1_OUT - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 1), // S2_OUT - DEF_TIM(TIM2, CH4, PA3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 1), // S3_OUT - DEF_TIM(TIM2, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 1), // S4_OUT - DEF_TIM(TIM5, CH2, PA1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 1), // S5_OUT - DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 1), // S6_OUT - - DEF_TIM(TIM4, CH1, PB6, TIM_USE_LED, 0), // LED strip + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 1, 0), // S1_OUT + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 1, 0), // S2_OUT + DEF_TIM(TIM2, CH4, PA3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 1, 0), // S3_OUT + DEF_TIM(TIM2, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 1, 0), // S4_OUT + DEF_TIM(TIM5, CH2, PA1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 1, 0), // S5_OUT + DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 1, 0), // S6_OUT + + DEF_TIM(TIM4, CH1, PB6, TIM_USE_LED, 0, 0), // LED strip }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/FRSKYF4/target.h b/src/main/target/FRSKYF4/target.h index d5862d9b698..cefdc61f3ad 100755 --- a/src/main/target/FRSKYF4/target.h +++ b/src/main/target/FRSKYF4/target.h @@ -17,7 +17,6 @@ #define TARGET_BOARD_IDENTIFIER "FRF4" #define USBD_PRODUCT_STRING "FRSKYF4" -#define TARGET_CONFIG #define LED0 PB5 #define BEEPER PB4 @@ -51,10 +50,11 @@ #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT #define USE_SDCARD +#define USE_SDCARD_SPI #define SDCARD_DETECT_INVERTED -#define SDCARD_DETECT_PIN PB7 -#define SDCARD_SPI_INSTANCE SPI2 -#define SDCARD_SPI_CS_PIN SPI2_NSS_PIN +#define SDCARD_DETECT_PIN PB7 +#define SDCARD_SPI_BUS BUS_SPI2 +#define SDCARD_CS_PIN SPI2_NSS_PIN #define USE_VCP #define VBUS_SENSING_PIN PC5 @@ -84,6 +84,7 @@ #define SPI2_SCK_PIN PB13 #define SPI2_MISO_PIN PB14 #define SPI2_MOSI_PIN PB15 +#define SPI2_CLOCK_LEADING_EDGE #define USE_SPI_DEVICE_3 #define SPI3_NSS_PIN PA15 diff --git a/src/main/target/FURYF3/target.c b/src/main/target/FURYF3/target.c index 3de9ca129ff..eaeefa0bfa6 100755 --- a/src/main/target/FURYF3/target.c +++ b/src/main/target/FURYF3/target.c @@ -8,18 +8,17 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[] = { - { TIM2, IO_TAG(PB3), TIM_Channel_2, 0, IOCFG_AF_PP, GPIO_AF_1, TIM_USE_PPM | TIM_USE_PWM }, // PPM IN + DEF_TIM(TIM2, CH2, PB3, TIM_USE_PPM, 0), // PPM IN + + DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR, 0), + DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_MOTOR, 0), + DEF_TIM(TIM17, CH1, PB5, TIM_USE_MC_MOTOR, 0), + DEF_TIM(TIM16, CH1, PB4, TIM_USE_MC_MOTOR, 0), - { TIM4, IO_TAG(PB7), TIM_Channel_2, 1, IOCFG_AF_PP, GPIO_AF_2, TIM_USE_MC_MOTOR }, // PWM4 - S1 - { TIM4, IO_TAG(PB6), TIM_Channel_1, 1, IOCFG_AF_PP, GPIO_AF_2, TIM_USE_MC_MOTOR }, // PWM5 - S2 - { TIM17, IO_TAG(PB5), TIM_Channel_1, 1, IOCFG_AF_PP, GPIO_AF_10, TIM_USE_MC_MOTOR }, // PWM6 - S3 - { TIM16, IO_TAG(PB4), TIM_Channel_1, 1, IOCFG_AF_PP, GPIO_AF_1, TIM_USE_MC_MOTOR }, // PWM7 - S4 - - { TIM3, IO_TAG(PB0), TIM_Channel_3, 1, IOCFG_AF_PP, GPIO_AF_2, TIM_USE_PWM | TIM_USE_MC_MOTOR }, // SS1 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N - { TIM3, IO_TAG(PB1), TIM_Channel_4, 1, IOCFG_AF_PP, GPIO_AF_2, TIM_USE_PWM | TIM_USE_MC_MOTOR }, // SS1 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N - - { TIM1, IO_TAG(PA8), TIM_Channel_1, 1, IOCFG_AF_PP, GPIO_AF_6, TIM_USE_ANY }, // GPIO TIMER - LED_STRIP + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR, 0), + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR, 0), + DEF_TIM(TIM1, CH1, PA8, TIM_USE_ANY, 0), }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/FURYF3/target.h b/src/main/target/FURYF3/target.h index dbe4aef801e..41e0a224854 100755 --- a/src/main/target/FURYF3/target.h +++ b/src/main/target/FURYF3/target.h @@ -91,23 +91,13 @@ #endif #ifdef USE_SDCARD -#define USE_SDCARD_SPI2 +#define USE_SDCARD_SPI +#define SPI2_CLOCK_LEADING_EDGE #define SDCARD_DETECT_INVERTED - -#define SDCARD_DETECT_PIN PB2 -#define SDCARD_DETECT_EXTI_LINE EXTI_Line2 -#define SDCARD_DETECT_EXTI_PIN_SOURCE EXTI_PinSource2 -#define SDCARD_DETECT_EXTI_PORT_SOURCE EXTI_PortSourceGPIOB -#define SDCARD_DETECT_EXTI_IRQn EXTI15_10_IRQn - -#define SDCARD_SPI_INSTANCE SPI2 -#define SDCARD_SPI_CS_GPIO SPI2_GPIO -#define SDCARD_SPI_CS_PIN SPI2_NSS_PIN - -// Note, this is the same DMA channel as UART1_RX. Luckily we don't use DMA for USART Rx. -#define SDCARD_DMA_CHANNEL_TX DMA1_Channel5 -#define SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG DMA1_FLAG_TC5 +#define SDCARD_DETECT_PIN PB2 +#define SDCARD_SPI_BUS BUS_SPI2 +#define SDCARD_CS_PIN SPI2_NSS_PIN #endif #define USE_VCP @@ -146,13 +136,7 @@ #define CURRENT_METER_ADC_CHANNEL ADC_CHN_3 #define USE_LED_STRIP -#define USE_LED_STRIP_ON_DMA1_CHANNEL2 #define WS2811_PIN PA8 -#define WS2811_TIMER TIM1 -#define WS2811_DMA_STREAM DMA1_Channel2 -#define WS2811_IRQ DMA1_Channel2_IRQn -#define WS2811_DMA_TC_FLAG DMA1_FLAG_TC2 -#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH2_HANDLER #define USE_RANGEFINDER #define USE_RANGEFINDER_HCSR04 diff --git a/src/main/target/FURYF3/target.mk b/src/main/target/FURYF3/target.mk index 62126108842..758082e8986 100755 --- a/src/main/target/FURYF3/target.mk +++ b/src/main/target/FURYF3/target.mk @@ -16,6 +16,5 @@ TARGET_SRC = \ drivers/compass/compass_mag3110.c \ drivers/compass/compass_lis3mdl.c \ drivers/light_ws2811strip.c \ - drivers/light_ws2811strip_stdperiph.c \ drivers/serial_softserial.c diff --git a/src/main/target/KAKUTEF4/config.c b/src/main/target/KAKUTEF4/config.c index 1da3155b091..23969d5b7b0 100755 --- a/src/main/target/KAKUTEF4/config.c +++ b/src/main/target/KAKUTEF4/config.c @@ -27,8 +27,6 @@ #include -#ifdef USE_TARGET_CONFIG - #include "io/serial.h" #include "rx/rx.h" #include "telemetry/telemetry.h" @@ -37,4 +35,3 @@ void targetConfiguration(void) { serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(TELEMETRY_UART)].functionMask = FUNCTION_TELEMETRY_SMARTPORT; } -#endif diff --git a/src/main/target/KAKUTEF4/target.c b/src/main/target/KAKUTEF4/target.c index f381a0c57f5..48082d288a0 100755 --- a/src/main/target/KAKUTEF4/target.c +++ b/src/main/target/KAKUTEF4/target.c @@ -29,20 +29,20 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[] = { - { TIM8, IO_TAG(PC7), TIM_Channel_2, 0, IOCFG_AF_PP_PD, GPIO_AF_TIM8, TIM_USE_PPM }, // PPM + DEF_TIM(TIM8, CH2, PC7, TIM_USE_PPM, 0, 0), // PPM IN - { TIM3, IO_TAG(PB0), TIM_Channel_3, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM3, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR }, // MOTOR_1 - { TIM3, IO_TAG(PB1), TIM_Channel_4, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM3, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR }, // MOTOR_2 - { TIM2, IO_TAG(PA3), TIM_Channel_4, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // MOTOR_3 - { TIM2, IO_TAG(PA2), TIM_Channel_3, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // MOTOR_4 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1_OUT - DMA1_ST7 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S2_OUT - DMA1_ST2 + DEF_TIM(TIM2, CH4, PA3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), // S3_OUT - DMA1_ST6 + DEF_TIM(TIM2, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4_OUT - DMA1_ST1 - #if defined(KAKUTEF4V2) - { TIM8, IO_TAG(PC8), TIM_Channel_3, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM8, TIM_USE_LED } - #else - { TIM5, IO_TAG(PA0), TIM_Channel_1, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM5, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // MOTOR_5 - { TIM8, IO_TAG(PC8), TIM_Channel_3, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // MOTOR_6 - { TIM5, IO_TAG(PA1), TIM_Channel_2, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM5, TIM_USE_LED } - #endif +#if defined(KAKUTEF4V2) + DEF_TIM(TIM8, CH3, PC8, TIM_USE_LED, 0, 0), // LED_STRIP - DMA2_ST2 +#else + DEF_TIM(TIM5, CH1, PA0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S5_OUT - DMA1_ST2 + DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), // S6_OUT - DMA2_ST4 + DEF_TIM(TIM5, CH2, PA1, TIM_USE_LED, 0, 0), // LED_STRIP - DMA1_ST4 +#endif }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/KAKUTEF4/target.h b/src/main/target/KAKUTEF4/target.h index 68ffd16c613..ed79614f08e 100755 --- a/src/main/target/KAKUTEF4/target.h +++ b/src/main/target/KAKUTEF4/target.h @@ -31,8 +31,6 @@ # define USBD_PRODUCT_STRING "KakuteF4-V1" #endif -#define USE_TARGET_CONFIG - #define LED0 PB5 #define LED1 PB4 #define LED2 PB6 @@ -71,6 +69,8 @@ # define USE_MAG_IST8308 # define USE_MAG_LIS3MDL +# define TEMPERATURE_I2C_BUS BUS_I2C1 + # define USE_BARO # define BARO_I2C_BUS BUS_I2C1 # define USE_BARO_MS5611 diff --git a/src/main/target/KAKUTEF4/target.mk b/src/main/target/KAKUTEF4/target.mk index 99264125b89..74fab0fb57f 100755 --- a/src/main/target/KAKUTEF4/target.mk +++ b/src/main/target/KAKUTEF4/target.mk @@ -13,5 +13,4 @@ TARGET_SRC = \ drivers/compass/compass_ist8308.c \ drivers/compass/compass_lis3mdl.c \ drivers/light_ws2811strip.c \ - drivers/light_ws2811strip_stdperiph.c \ drivers/max7456.c diff --git a/src/main/target/KAKUTEF7/target.c b/src/main/target/KAKUTEF7/target.c new file mode 100755 index 00000000000..f0451514875 --- /dev/null +++ b/src/main/target/KAKUTEF7/target.c @@ -0,0 +1,46 @@ +/* + * This file is part of INAV. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute and/or modify + * it under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or (at your + * option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see http://www.gnu.org/licenses/. + */ + +#include +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" +#include "drivers/timer.h" +#include "drivers/bus.h" + +BUSDEV_REGISTER_SPI_TAG(busdev_icm20689, DEVHW_ICM20689, ICM20689_SPI_BUS, ICM20689_CS_PIN, ICM20689_EXTI_PIN, 0, DEVFLAGS_NONE); + +const timerHardware_t timerHardware[] = { + DEF_TIM(TIM1, CH3, PE13, TIM_USE_PPM, 0, 1), // PPM, DMA2_ST6 + + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // M1 , DMA1_ST7 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // M2 , DMA1_ST2 + DEF_TIM(TIM1, CH1, PE9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 2), // M3 , DMA2_ST2 + DEF_TIM(TIM1, CH2, PE11, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), // M4 , DMA2_ST4 + DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // M5 , DMA2_ST7 + DEF_TIM(TIM5, CH4, PA3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // M6 , DMA1_ST1 + + DEF_TIM(TIM4, CH1, PD12, TIM_USE_LED, 0, 0), // LED_TRIP, DMA1_ST0 +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/KAKUTEF7/target.h b/src/main/target/KAKUTEF7/target.h new file mode 100644 index 00000000000..9d0b44791e4 --- /dev/null +++ b/src/main/target/KAKUTEF7/target.h @@ -0,0 +1,156 @@ +/* + * This file is part of INAV. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software under the terms of the + * GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) + * any later version. + * + * Cleanflight and Betaflight are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "KTF7" +#define USBD_PRODUCT_STRING "KakuteF7" + +#define LED0 PA2 + +#define BEEPER PD15 +#define BEEPER_INVERTED + +#define USE_ACC +#define USE_GYRO + +#define USE_MPU_DATA_READY_SIGNAL +#define USE_EXTI + +// ICM-20689 +#define USE_ACC_ICM20689 +#define USE_GYRO_ICM20689 +#define GYRO_ICM20689_ALIGN CW270_DEG +#define ACC_ICM20689_ALIGN CW270_DEG + +#define ICM20689_EXTI_PIN PE1 +#define ICM20689_CS_PIN SPI4_NSS_PIN +#define ICM20689_SPI_BUS BUS_SPI4 + +#define USB_IO +#define USE_VCP +#define VBUS_SENSING_ENABLED +#define VBUS_SENSING_PIN PA8 + +#define USE_UART1 +#define UART1_TX_PIN PA9 +#define UART1_RX_PIN PA10 + +#define USE_UART2 +#define UART2_TX_PIN PD5 +#define UART2_RX_PIN PD6 + +#define USE_UART3 +#define UART3_TX_PIN PB10 +#define UART3_RX_PIN PB11 + +#define USE_UART4 +#define UART4_TX_PIN PA0 +#define UART4_RX_PIN PA1 + +#define USE_UART6 +#define UART6_TX_PIN PC6 +#define UART6_RX_PIN PC7 + +#define USE_UART7 +#define UART7_TX_PIN NONE +#define UART7_RX_PIN PE7 + +#define SERIAL_PORT_COUNT 7 //VCP,UART1,UART2,UART3,UART4,UART6,UART7 + +#define USE_SPI +#define USE_SPI_DEVICE_1 //SD Card +#define USE_SPI_DEVICE_2 //OSD +#define USE_SPI_DEVICE_4 //ICM20689 + +#define SPI1_NSS_PIN PA4 +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 +#define SPI1_CLOCK_LEADING_EDGE + +#define SPI2_NSS_PIN PB12 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 + +#define SPI4_NSS_PIN PE4 +#define SPI4_SCK_PIN PE2 +#define SPI4_MISO_PIN PE5 +#define SPI4_MOSI_PIN PE6 + +#define USE_OSD +#define USE_MAX7456 +#define MAX7456_SPI_BUS BUS_SPI2 +#define MAX7456_CS_PIN SPI2_NSS_PIN + +#define USE_SDCARD +#define USE_SDCARD_SPI +#define SDCARD_SPI_BUS BUS_SPI1 +#define SDCARD_CS_PIN SPI1_NSS_PIN +#define SDCARD_DETECT_INVERTED +#define SDCARD_DETECT_PIN PD8 + +#define USE_I2C +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB6 +#define I2C1_SDA PB7 + +#define USE_BARO +#define USE_BARO_BMP280 +#define BARO_I2C_BUS BUS_I2C1 + +#define USE_MAG +#define MAG_I2C_BUS BUS_I2C1 +#define USE_MAG_HMC5883 +#define MAG_HMC5883_ALIGN CW180_DEG +#define USE_MAG_QMC5883 +#define USE_MAG_MAG3110 +#define USE_MAG_IST8310 +#define USE_MAG_IST8308 +#define USE_MAG_LIS3MDL + +#define TEMPERATURE_I2C_BUS BUS_I2C1 + +#define USE_ADC +#define ADC_CHANNEL_1_PIN PC2 +#define ADC_CHANNEL_2_PIN PC3 +#define ADC_CHANNEL_3_PIN PC5 + +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_1 +#define VBAT_ADC_CHANNEL ADC_CHN_2 +#define RSSI_ADC_CHANNEL ADC_CHN_3 + +#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT + +#define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_TELEMETRY | FEATURE_OSD | FEATURE_VBAT | FEATURE_CURRENT_METER | FEATURE_BLACKBOX) +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_UART SERIAL_PORT_USART6 +#define SERIALRX_PROVIDER SERIALRX_SBUS + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD 0xffff +#define TARGET_IO_PORTE 0xffff + +#define MAX_PWM_OUTPUT_PORTS 6 diff --git a/src/main/target/KAKUTEF7/target.mk b/src/main/target/KAKUTEF7/target.mk new file mode 100755 index 00000000000..f22a7f7d6fa --- /dev/null +++ b/src/main/target/KAKUTEF7/target.mk @@ -0,0 +1,15 @@ +F7X5XG_TARGETS += $(TARGET) +FEATURES += SDCARD VCP + +TARGET_SRC = \ + drivers/accgyro/accgyro_icm20689.c \ + drivers/barometer/barometer_bmp280.c \ + drivers/compass/compass_ak8963.c \ + drivers/compass/compass_hmc5883l.c \ + drivers/compass/compass_qmc5883l.c \ + drivers/compass/compass_mag3110.c \ + drivers/compass/compass_ist8310.c \ + drivers/compass/compass_ist8308.c \ + drivers/compass/compass_lis3mdl.c \ + drivers/light_ws2811strip.c \ + drivers/max7456.c diff --git a/src/main/target/KFC32F3_INAV/target.c b/src/main/target/KFC32F3_INAV/target.c index 7f1edfe84be..6c6abf12dab 100755 --- a/src/main/target/KFC32F3_INAV/target.c +++ b/src/main/target/KFC32F3_INAV/target.c @@ -24,17 +24,19 @@ const timerHardware_t timerHardware[] = { - { TIM2, IO_TAG(PA0), TIM_Channel_1, 0, IOCFG_AF_PP, GPIO_AF_1, TIM_USE_PPM }, // // PPM - PA0 - *TIM2_CH1 - { TIM4, IO_TAG(PB9), TIM_Channel_4, 1, IOCFG_IPD, GPIO_AF_2, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR }, // PWM1 - { TIM3, IO_TAG(PB0), TIM_Channel_3, 1, IOCFG_IPD, GPIO_AF_2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // PWM2 - { TIM3, IO_TAG(PB1), TIM_Channel_4, 1, IOCFG_IPD, GPIO_AF_2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // PWM3 - { TIM4, IO_TAG(PB7), TIM_Channel_2, 1, IOCFG_IPD, GPIO_AF_2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // PWM4 - { TIM4, IO_TAG(PB8), TIM_Channel_3, 1, IOCFG_IPD, GPIO_AF_2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // PWM5 - { TIM4, IO_TAG(PB6), TIM_Channel_1, 1, IOCFG_IPD, GPIO_AF_2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // PWM6 - { TIM2, IO_TAG(PB10), TIM_Channel_3, 1, IOCFG_AF_PP, GPIO_AF_1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // PWM7 - { TIM2, IO_TAG(PB11), TIM_Channel_4, 1, IOCFG_AF_PP, GPIO_AF_1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // PWM8 - { TIM1, IO_TAG(PA8), TIM_Channel_1, 1, IOCFG_AF_PP, GPIO_AF_6, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO | TIM_USE_ANY | TIM_USE_LED }, // S1_out - { TIM2, IO_TAG(PA1), TIM_Channel_2, 1, IOCFG_IPD, GPIO_AF_1, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO |TIM_USE_ANY }, // S2_out + DEF_TIM(TIM2, CH1, PA0, TIM_USE_PPM, 0), // PPM + + DEF_TIM(TIM4, CH4, PB9, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0), // PWM1 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0), // PWM2 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0), // PWM3 + DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0), // PWM4 + DEF_TIM(TIM4, CH3, PB8, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0), // PWM5 + DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0), // PWM6 + DEF_TIM(TIM2, CH3, PB10, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0), // PWM7 + DEF_TIM(TIM2, CH4, PB11, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0), // PWM8 + + DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR | TIM_USE_LED, 0), // S1_out + DEF_TIM(TIM2, CH2, PA1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0), // S2_out }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/KFC32F3_INAV/target.mk b/src/main/target/KFC32F3_INAV/target.mk index aeb7043a5e5..99d179b6877 100644 --- a/src/main/target/KFC32F3_INAV/target.mk +++ b/src/main/target/KFC32F3_INAV/target.mk @@ -16,6 +16,4 @@ TARGET_SRC = \ drivers/flash_m25p16.c \ drivers/max7456.c \ drivers/light_ws2811strip.c \ - drivers/light_ws2811strip_stdperiph.c \ - drivers/pitotmeter_ms4525.c \ drivers/pitotmeter_adc.c diff --git a/src/main/target/KISSFC/config.c b/src/main/target/KISSFC/config.c new file mode 100644 index 00000000000..a6c6802d429 --- /dev/null +++ b/src/main/target/KISSFC/config.c @@ -0,0 +1,33 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software under the terms of the + * GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) + * any later version. + * + * Cleanflight and Betaflight are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +#include +#include + +#include "platform.h" + +#include "fc/config.h" +#include "sensors/boardalignment.h" + +void targetConfiguration(void) +{ + +} + diff --git a/src/main/target/KISSFC/target.c b/src/main/target/KISSFC/target.c new file mode 100644 index 00000000000..9e3b854b9b4 --- /dev/null +++ b/src/main/target/KISSFC/target.c @@ -0,0 +1,44 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software under the terms of the + * GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) + * any later version. + * + * Cleanflight and Betaflight are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +#include +#include + +#include +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" +#include "drivers/timer.h" + +const timerHardware_t timerHardware[] = { + DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0), + DEF_TIM(TIM1, CH2N, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0), + DEF_TIM(TIM15, CH1, PB14, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), + DEF_TIM(TIM15, CH2, PB15, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), + DEF_TIM(TIM3, CH1, PA6, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), + DEF_TIM(TIM3, CH2, PA7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), + + DEF_TIM(TIM4, CH3, PA13, TIM_USE_PWM, 0), + DEF_TIM(TIM2, CH2, PB3, TIM_USE_PWM | TIM_USE_PPM, 0), + DEF_TIM(TIM8, CH1, PA15, TIM_USE_PWM, 0), + DEF_TIM(TIM2, CH3, PA2, TIM_USE_PWM, 0), + DEF_TIM(TIM2, CH4, PB11, TIM_USE_PWM, 0), +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/KISSFC/target.h b/src/main/target/KISSFC/target.h new file mode 100644 index 00000000000..bf37520717a --- /dev/null +++ b/src/main/target/KISSFC/target.h @@ -0,0 +1,90 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software under the terms of the + * GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) + * any later version. + * + * Cleanflight and Betaflight are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +#pragma once + +#define SBUS_PORT_OPTIONS (SERIAL_STOPBITS_2 | SERIAL_PARITY_EVEN | SERIAL_INVERTED | SERIAL_BIDIR) + +#define TARGET_BOARD_IDENTIFIER "KISSFC" + +#define LED0 PB1 + +#define BEEPER PB13 +#define BEEPER_INVERTED + +#define USE_ACC +#define USE_ACC_MPU6050 +#define ACC_MPU6050_ALIGN CW180_DEG +#define MPU6050_I2C_BUS BUS_I2C1 + +#define USE_GYRO +#define USE_GYRO_MPU6050 +#define GYRO_MPU6050_ALIGN CW180_DEG + +#define USE_EXTI +#define MPU_INT_EXTI PB2 +#define USE_MPU_DATA_READY_SIGNAL +#define ENSURE_MPU_DATA_READY_IS_LOW + +#define USE_VCP +#define USE_UART1 +#define USE_UART2 +#define USE_UART3 +//#define USE_SOFTSERIAL1 +//#define USE_SOFTSERIAL2 + +#define SERIAL_PORT_COUNT 4 + +#define UART1_TX_PIN PA9 +#define UART1_RX_PIN PA10 + +#define UART2_TX_PIN PB3 +#define UART2_RX_PIN PB4 + +#define UART3_TX_PIN PB10 // PB10 (AF7) +#define UART3_RX_PIN PB11 // PB11 (AF7) + +//#define SOFTSERIAL1_TX_PIN PA13 // AUX1 +//#define SOFTSERIAL2_TX_PIN PA15 // ROLL + +#define USE_I2C +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB6 +#define I2C1_SDA PB7 + +#define USE_ADC +#define VBAT_SCALE_DEFAULT 160 +#define ADC_INSTANCE ADC1 +#define VBAT_ADC_PIN PA0 +#define CURRENT_METER_ADC_PIN PA2 + +#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL +#define SERIALRX_PROVIDER SERIALRX_SBUS +#define SERIALRX_UART SERIAL_PORT_USART2 + +#define MAX_PWM_OUTPUT_PORTS 12 + +#define AVOID_UART2_FOR_PWM_PPM + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD 0xffff +#define TARGET_IO_PORTF (BIT(4)) diff --git a/src/main/target/KISSFC/target.mk b/src/main/target/KISSFC/target.mk new file mode 100644 index 00000000000..a07094b5267 --- /dev/null +++ b/src/main/target/KISSFC/target.mk @@ -0,0 +1,9 @@ +F3_TARGETS += $(TARGET) +FEATURES = VCP + +TARGET_SRC = \ + drivers/accgyro/accgyro_mpu.c \ + drivers/accgyro/accgyro_mpu6050.c \ + drivers/display_ug2864hsweg01.c \ + drivers/serial_usb_vcp.c + diff --git a/src/main/target/KROOZX/target.c b/src/main/target/KROOZX/target.c index 331f590d2ca..4dcba204f35 100755 --- a/src/main/target/KROOZX/target.c +++ b/src/main/target/KROOZX/target.c @@ -23,17 +23,20 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[] = { - {TIM8, IO_TAG(PC7), TIM_Channel_2, 0, IOCFG_AF_PP_PD, GPIO_AF_TIM8, TIM_USE_PPM }, // PPM IN - {TIM5, IO_TAG(PA1), TIM_Channel_2, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM5, TIM_USE_MC_MOTOR }, // PWM2 - {TIM5, IO_TAG(PA3), TIM_Channel_4, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM5, TIM_USE_MC_MOTOR }, // PWM3 - {TIM5, IO_TAG(PA0), TIM_Channel_1, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM5, TIM_USE_MC_MOTOR }, // PWM4 - {TIM5, IO_TAG(PA2), TIM_Channel_3, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM5, TIM_USE_MC_MOTOR }, // PWM5 - {TIM3, IO_TAG(PB1), TIM_Channel_4, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM3, TIM_USE_MC_MOTOR }, // PWM6 - {TIM3, IO_TAG(PB0), TIM_Channel_3, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM3, TIM_USE_MC_MOTOR }, // PWM7 - {TIM4, IO_TAG(PB8), TIM_Channel_3, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM4, TIM_USE_MC_MOTOR }, // PWM8 - {TIM4, IO_TAG(PB9), TIM_Channel_4, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM4, TIM_USE_MC_MOTOR }, // PWM9 - {TIM12, IO_TAG(PB14), TIM_Channel_1, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM12, TIM_USE_MC_MOTOR }, // PWM10 - {TIM12, IO_TAG(PB15), TIM_Channel_2, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM12, TIM_USE_MC_MOTOR }, // PWM11 + DEF_TIM(TIM8, CH2, PC7, TIM_USE_PPM, 0, 0), // PPM IN + + DEF_TIM(TIM5, CH2, PA1, TIM_USE_MC_MOTOR, 0, 0), // PWM4 + DEF_TIM(TIM5, CH4, PA3, TIM_USE_MC_MOTOR, 0, 0), // PWM2 + DEF_TIM(TIM5, CH1, PA0, TIM_USE_MC_MOTOR, 0, 0), // PWM3 + DEF_TIM(TIM5, CH3, PA2, TIM_USE_MC_MOTOR, 0, 0), // PWM1 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR, 0, 0), // PWM5 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR, 0, 0), // PWM6 + DEF_TIM(TIM4, CH3, PB8, TIM_USE_MC_MOTOR, 0, 0), // PWM7 + DEF_TIM(TIM4, CH4, PB9, TIM_USE_MC_MOTOR, 0, 0), // PWM8 + DEF_TIM(TIM4, CH1, PB14, TIM_USE_MC_MOTOR, 0, 0), // PWM9 + DEF_TIM(TIM4, CH2, PB15, TIM_USE_MC_MOTOR, 0, 0), // PWM10 + + DEF_TIM(TIM8, CH1, PC6, TIM_USE_LED, 0, 0), // LED_STRIP }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/KROOZX/target.h b/src/main/target/KROOZX/target.h index 91ebd7e91fe..acbbb08865c 100755 --- a/src/main/target/KROOZX/target.h +++ b/src/main/target/KROOZX/target.h @@ -54,20 +54,18 @@ #define USE_MAG_MAG3110 #define USE_MAG_LIS3MDL +#define TEMPERATURE_I2C_BUS BUS_I2C1 + #define USE_BARO #define BARO_I2C_BUS BUS_I2C3 #define USE_BARO_MS5611 #define USE_SDCARD +#define USE_SDCARD_SPI +#define SDCARD_SPI_BUS BUS_SPI3 +#define SDCARD_CS_PIN PA15 #define SDCARD_DETECT_INVERTED -#define SDCARD_DETECT_PIN PC13 -#define SDCARD_SPI_INSTANCE SPI3 -#define SDCARD_SPI_CS_PIN PA15 - -#define SDCARD_DMA_CHANNEL_TX DMA1_Stream5 -#define SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG DMA_FLAG_TCIF5 -#define SDCARD_DMA_CLK RCC_AHB1Periph_DMA1 -#define SDCARD_DMA_CHANNEL DMA_Channel_0 +#define SDCARD_DETECT_PIN PC13 #define USE_OSD #ifdef USE_MSP_DISPLAYPORT @@ -138,6 +136,7 @@ #define SPI3_SCK_PIN PB3 #define SPI3_MISO_PIN PB4 #define SPI3_MOSI_PIN PB5 +#define SPI3_CLOCK_LEADING_EDGE #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT @@ -148,16 +147,7 @@ #define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_VBAT | FEATURE_CURRENT_METER | FEATURE_OSD) #define USE_LED_STRIP -#define WS2811_GPIO_AF GPIO_AF_TIM3 #define WS2811_PIN PC6 -#define WS2811_TIMER TIM3 -#define WS2811_TIMER_CHANNEL TIM_Channel_1 -#define WS2811_DMA_HANDLER_IDENTIFER DMA1_ST4_HANDLER -#define WS2811_DMA_STREAM DMA1_Stream4 -#define WS2811_DMA_CHANNEL DMA_Channel_5 -#define WS2811_DMA_IRQ DMA1_Stream4_IRQn -#define WS2811_DMA_FLAG DMA_FLAG_TCIF4 -#define WS2811_DMA_IT DMA_IT_TCIF4 #define USE_SERIAL_4WAY_BLHELI_INTERFACE diff --git a/src/main/target/KROOZX/target.mk b/src/main/target/KROOZX/target.mk index 9fc70c0a69f..529d41babbf 100755 --- a/src/main/target/KROOZX/target.mk +++ b/src/main/target/KROOZX/target.mk @@ -12,5 +12,4 @@ TARGET_SRC = \ drivers/compass/compass_mag3110.c \ drivers/compass/compass_lis3mdl.c \ drivers/max7456.c \ - drivers/light_ws2811strip.c \ - drivers/light_ws2811strip_stdperiph.c \ No newline at end of file + drivers/light_ws2811strip.c \ No newline at end of file diff --git a/src/main/target/LUX_RACE/target.c b/src/main/target/LUX_RACE/target.c index 31bcbe2a0d5..95b2b654283 100644 --- a/src/main/target/LUX_RACE/target.c +++ b/src/main/target/LUX_RACE/target.c @@ -23,18 +23,21 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[] = { - { TIM1, IO_TAG(PA8), TIM_Channel_1, 0, IOCFG_AF_PP_PD, GPIO_AF_6, TIM_USE_PPM }, // PWM1 - PA8 - { TIM3, IO_TAG(PC6), TIM_Channel_1, 1, IOCFG_AF_PP, GPIO_AF_2, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR }, // PWM2 - PC6 - { TIM8, IO_TAG(PC7), TIM_Channel_2, 1, IOCFG_AF_PP, GPIO_AF_4, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // PWM3 - PC7 - { TIM8, IO_TAG(PC8), TIM_Channel_3, 1, IOCFG_AF_PP, GPIO_AF_4, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // PMW4 - PC8 - { TIM8, IO_TAG(PC9), TIM_Channel_4, 1, IOCFG_AF_PP, GPIO_AF_4, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // PWM5 - PC9 - { TIM2, IO_TAG(PA0), TIM_Channel_1, 1, IOCFG_AF_PP, GPIO_AF_1, TIM_USE_MC_MOTOR }, // PWM6 - PA0 - { TIM2, IO_TAG(PA1), TIM_Channel_2, 1, IOCFG_AF_PP, GPIO_AF_1, TIM_USE_MC_MOTOR }, // PWM7 - PA1 - { TIM2, IO_TAG(PA2), TIM_Channel_3, 1, IOCFG_AF_PP, GPIO_AF_1, TIM_USE_MC_MOTOR }, // PWM8 - PA2 - { TIM2, IO_TAG(PA3), TIM_Channel_4, 1, IOCFG_AF_PP, GPIO_AF_1, TIM_USE_MC_MOTOR }, // PWM9 - PA3 - { TIM15, IO_TAG(PB14), TIM_Channel_1, 1, IOCFG_AF_PP_PD, GPIO_AF_1, TIM_USE_MC_MOTOR }, // PWM10 - PB14 - { TIM15, IO_TAG(PB15), TIM_Channel_2, 1, IOCFG_AF_PP_PD, GPIO_AF_1, TIM_USE_MC_MOTOR }, // PWM11 - PB15 - { TIM16, IO_TAG(PA6), TIM_Channel_1, 1, IOCFG_AF_PP_PD, GPIO_AF_1, TIM_USE_LED }, + DEF_TIM(TIM1, CH1, PA8, TIM_USE_PPM, 0), // PWM1 - PA8 + + DEF_TIM(TIM3, CH1, PC6, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0), // PWM2 - PC6 + DEF_TIM(TIM8, CH2, PC7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // PWM3 - PC7 + DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // PMW4 - PC8 + DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // PWM5 - PC9 + + DEF_TIM(TIM2, CH1, PA0, TIM_USE_MC_MOTOR, 0), // PWM6 - PA0 + DEF_TIM(TIM2, CH2, PA1, TIM_USE_MC_MOTOR, 0), // PWM7 - PA1 + DEF_TIM(TIM2, CH3, PA2, TIM_USE_MC_MOTOR, 0), // PWM8 - PA2 + DEF_TIM(TIM2, CH4, PA3, TIM_USE_MC_MOTOR, 0), // PWM9 - PA3 + DEF_TIM(TIM15, CH1, PB14, TIM_USE_MC_MOTOR, 0), // PWM10 - PB14 + DEF_TIM(TIM15, CH2, PB15, TIM_USE_MC_MOTOR, 0), // PWM11 - PB15 + + DEF_TIM(TIM16, CH1, PA6, TIM_USE_LED, 0), }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/LUX_RACE/target.mk b/src/main/target/LUX_RACE/target.mk index 81adcd66c61..67886273cb8 100644 --- a/src/main/target/LUX_RACE/target.mk +++ b/src/main/target/LUX_RACE/target.mk @@ -6,6 +6,5 @@ TARGET_SRC = \ drivers/accgyro/accgyro_mpu6500.c \ drivers/accgyro/accgyro_mpu9250.c \ drivers/light_ws2811strip.c \ - drivers/light_ws2811strip_stdperiph.c \ drivers/serial_usb_vcp.c \ diff --git a/src/main/target/MATEKF405/target.c b/src/main/target/MATEKF405/target.c index bb140f936e3..1b442177567 100644 --- a/src/main/target/MATEKF405/target.c +++ b/src/main/target/MATEKF405/target.c @@ -22,31 +22,30 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[] = { - { TIM9, IO_TAG(PA3), TIM_Channel_2, 0, IOCFG_AF_PP_PD, GPIO_AF_TIM9, TIM_USE_PPM }, + DEF_TIM(TIM9, CH2, PA3, TIM_USE_PPM, 0, 0), // PPM #ifdef MATEKF405_SERVOS6 - { TIM3, IO_TAG(PC6), TIM_Channel_1, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM3, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR }, //S1 DMA1_ST4 MT1 + DEF_TIM(TIM3, CH1, PC6, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1 #else - { TIM3, IO_TAG(PC6), TIM_Channel_1, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM3, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR }, + DEF_TIM(TIM3, CH1, PC6, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR, 0, 0), // S1 #endif - { TIM8, IO_TAG(PC7), TIM_Channel_2, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, //S2 DMA2_ST3 SV3 - { TIM8, IO_TAG(PC8), TIM_Channel_3, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, //S3 DMA2_ST4 SV4 - { TIM8, IO_TAG(PC9), TIM_Channel_4, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, //S4 DMA3_ST7 SV5 + DEF_TIM(TIM8, CH2, PC7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), // S2 UP(2,1) + DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), // S3 UP(2,1) + DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4 UP(2,1) - { TIM2, IO_TAG(PA15), TIM_Channel_1, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM2, TIM_USE_MC_MOTOR | TIM_USE_LED }, //S5 DMA1_ST5 2812LED + DEF_TIM(TIM2, CH1, PA15, TIM_USE_MC_MOTOR | TIM_USE_LED , 0, 0), // S5 UP(1,7) #ifdef MATEKF405_SERVOS6 - { TIM1, IO_TAG(PA8), TIM_Channel_1, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM1, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO }, //S6 DMA2_ST6 SV6 + DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S6 UP(2,5) #else - { TIM1, IO_TAG(PA8), TIM_Channel_1, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, + DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S6 UP(2,5) #endif - { TIM4, IO_TAG(PB8), TIM_Channel_3, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM4, TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR }, //S7 DMA1_ST7 MT2 + DEF_TIM(TIM4, CH3, PB8, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S7 D(1,7)!S5 UP(2,6) - { TIM5, IO_TAG(PA2), TIM_Channel_3, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM5, TIM_USE_PWM }, //TX2 - - { TIM5, IO_TAG(PA0), TIM_Channel_1, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM5, TIM_USE_PWM }, //TX4 softserial1_tx - { TIM5, IO_TAG(PA1), TIM_Channel_2, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM5, TIM_USE_PWM }, //RX4 softserial1_rx + DEF_TIM(TIM5, CH3, PA2, TIM_USE_PWM, 0, 0), // TX2 + DEF_TIM(TIM5, CH1, PA0, TIM_USE_PWM, 0, 0), // TX4 UP(1,6) D(1,2)!S1 + DEF_TIM(TIM5, CH2, PA1, TIM_USE_PWM, 0, 0), // RX4 UP(1,6) D(1,4) }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/MATEKF405/target.h b/src/main/target/MATEKF405/target.h index 940b91c62fa..6d635a92fd3 100644 --- a/src/main/target/MATEKF405/target.h +++ b/src/main/target/MATEKF405/target.h @@ -61,20 +61,17 @@ // *************** SD Card ************************** #define USE_SDCARD -#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT +#define USE_SDCARD_SPI +#define SDCARD_SPI_BUS BUS_SPI3 +#define SDCARD_CS_PIN PC1 #define USE_SPI_DEVICE_3 #define SPI3_SCK_PIN PB3 #define SPI3_MISO_PIN PB4 #define SPI3_MOSI_PIN PB5 +#define SPI3_CLOCK_LEADING_EDGE -#define SDCARD_SPI_INSTANCE SPI3 -#define SDCARD_SPI_CS_PIN PC1 - -#define SDCARD_DMA_CHANNEL_TX DMA1_Stream7 -#define SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG DMA_FLAG_TCIF7 -#define SDCARD_DMA_CLK RCC_AHB1Periph_DMA1 -#define SDCARD_DMA_CHANNEL DMA_Channel_0 +#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT // *************** M25P256 flash ******************** #define USE_FLASHFS @@ -171,6 +168,8 @@ #define USE_MAG_MAG3110 #define USE_MAG_LIS3MDL +#define TEMPERATURE_I2C_BUS DEFAULT_I2C_BUS + #define USE_OPTICAL_FLOW #define USE_OPFLOW_MSP @@ -179,7 +178,6 @@ #define USE_RANGEFINDER_HCSR04_I2C #define RANGEFINDER_I2C_BUS DEFAULT_I2C_BUS -#define USE_PITOT_MS4525 #define PITOT_I2C_BUS DEFAULT_I2C_BUS // *************** ADC ***************************** @@ -198,9 +196,6 @@ #define USE_LED_STRIP #define WS2811_PIN PA15 // S5 pad for iNav -#define WS2811_DMA_HANDLER_IDENTIFER DMA1_ST5_HANDLER -#define WS2811_DMA_STREAM DMA1_Stream5 -#define WS2811_DMA_CHANNEL DMA_Channel_3 #define USE_SPEKTRUM_BIND #define BIND_PIN PA3 // RX2 @@ -212,6 +207,8 @@ #define TARGET_IO_PORTC 0xffff #define TARGET_IO_PORTD (BIT(2)) +#define USE_DSHOT + #define MAX_PWM_OUTPUT_PORTS 6 -#define PCA9685_I2C_BUS DEFAULT_I2C_BUS \ No newline at end of file +#define PCA9685_I2C_BUS DEFAULT_I2C_BUS diff --git a/src/main/target/MATEKF405/target.mk b/src/main/target/MATEKF405/target.mk index c728eab7ad4..1d0fa6dad92 100755 --- a/src/main/target/MATEKF405/target.mk +++ b/src/main/target/MATEKF405/target.mk @@ -15,8 +15,6 @@ TARGET_SRC = \ drivers/compass/compass_ist8308.c \ drivers/compass/compass_mag3110.c \ drivers/compass/compass_lis3mdl.c \ - drivers/pitotmeter_ms4525.c \ drivers/pitotmeter_adc.c \ drivers/light_ws2811strip.c \ - drivers/light_ws2811strip_stdperiph.c \ drivers/max7456.c diff --git a/src/main/target/MATEKF405SE/target.c b/src/main/target/MATEKF405SE/target.c index 945179e5cc7..6e77baa43f1 100644 --- a/src/main/target/MATEKF405SE/target.c +++ b/src/main/target/MATEKF405SE/target.c @@ -22,20 +22,21 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[] = { - { TIM4, IO_TAG(PB7), TIM_Channel_2, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM4, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR }, //S1 - { TIM4, IO_TAG(PB6), TIM_Channel_1, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM4, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR }, //S2 - { TIM3, IO_TAG(PB0), TIM_Channel_3, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, //S3 - { TIM3, IO_TAG(PB1), TIM_Channel_4, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, //S4 - { TIM8, IO_TAG(PC8), TIM_Channel_3, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, //S5 - { TIM8, IO_TAG(PC9), TIM_Channel_4, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, //S6 - { TIM12, IO_TAG(PB14), TIM_Channel_1, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM12, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO }, //S7 - { TIM12, IO_TAG(PB15), TIM_Channel_2, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM12, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO }, //S8 - { TIM1, IO_TAG(PA8), TIM_Channel_1, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM1, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO }, //S9 + DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 1, 0), // S1 + DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 1, 0), // S2 - { TIM2, IO_TAG(PA15), TIM_Channel_1, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM2, TIM_USE_LED }, //2812LED - { TIM9, IO_TAG(PA3), TIM_Channel_2, 0, IOCFG_AF_PP_PD, GPIO_AF_TIM9, TIM_USE_PPM }, //RX2 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 1, 0), // S3 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 1, 0), // S4 + DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 1, 0), // S5 + DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 1, 0), // S6 + DEF_TIM(TIM12, CH1, PB14, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 1, 0), // S7 + DEF_TIM(TIM12, CH2, PB15, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 1, 0), // S8 + DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 1, 0), // S9 - { TIM5, IO_TAG(PA2), TIM_Channel_3, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM5, TIM_USE_PWM }, //TX2 softserial1_Tx + DEF_TIM(TIM2, CH1, PA15, TIM_USE_LED, 0, 0), //2812LED + + DEF_TIM(TIM9, CH2, PA3, TIM_USE_PPM, 0, 0), //RX2 + DEF_TIM(TIM5, CH3, PA2, TIM_USE_ANY, 0, 0), //TX2 softserial1_Tx }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/MATEKF405SE/target.h b/src/main/target/MATEKF405SE/target.h index af3b56c6874..39ec76c8e65 100644 --- a/src/main/target/MATEKF405SE/target.h +++ b/src/main/target/MATEKF405SE/target.h @@ -19,7 +19,6 @@ #define TARGET_BOARD_IDENTIFIER "MF4S" #define USBD_PRODUCT_STRING "Matek_F405SE" -#define TARGET_CONFIG #define LED0 PA14 //Blue #define LED1 PA13 //Green @@ -75,9 +74,15 @@ #define USE_MAG_MAG3110 #define USE_MAG_LIS3MDL -#define USE_PITOT_MS4525 +#define USE_RANGEFINDER +#define USE_RANGEFINDER_HCSR04_I2C +#define RANGEFINDER_I2C_BUS BUS_I2C2 + #define PITOT_I2C_BUS BUS_I2C2 +#define TEMPERATURE_I2C_BUS BUS_I2C2 + + // *************** SPI2 OSD *************************** #define USE_SPI_DEVICE_2 #define SPI2_SCK_PIN PB13 @@ -90,20 +95,18 @@ #define MAX7456_SPI_BUS BUS_SPI2 // *************** SPI3 SD Card ******************** +#define USE_SDCARD +#define USE_SDCARD_SPI +#define SDCARD_SPI_BUS BUS_SPI3 +#define SDCARD_CS_PIN PC14 + #define USE_SPI_DEVICE_3 #define SPI3_SCK_PIN PB3 #define SPI3_MISO_PIN PB4 #define SPI3_MOSI_PIN PB5 +#define SPI3_CLOCK_LEADING_EDGE -#define USE_SDCARD #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT -#define SDCARD_SPI_INSTANCE SPI3 -#define SDCARD_SPI_CS_PIN PC14 - -#define SDCARD_DMA_CHANNEL_TX DMA1_Stream7 -#define SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG DMA_FLAG_TCIF7 -#define SDCARD_DMA_CLK RCC_AHB1Periph_DMA1 -#define SDCARD_DMA_CHANNEL DMA_Channel_0 // *************** UART ***************************** #define USE_VCP @@ -178,4 +181,4 @@ #define MAX_PWM_OUTPUT_PORTS 9 -#define PCA9685_I2C_BUS BUS_I2C2 \ No newline at end of file +#define PCA9685_I2C_BUS BUS_I2C2 diff --git a/src/main/target/MATEKF405SE/target.mk b/src/main/target/MATEKF405SE/target.mk index 49849d517cc..3a860b32837 100644 --- a/src/main/target/MATEKF405SE/target.mk +++ b/src/main/target/MATEKF405SE/target.mk @@ -13,8 +13,6 @@ TARGET_SRC = \ drivers/compass/compass_ist8310.c \ drivers/compass/compass_mag3110.c \ drivers/compass/compass_lis3mdl.c \ - drivers/pitotmeter_ms4525.c \ drivers/pitotmeter_adc.c \ drivers/light_ws2811strip.c \ - drivers/light_ws2811strip_stdperiph.c \ drivers/max7456.c diff --git a/src/main/target/MATEKF411/MATEKF411_RSSI.mk b/src/main/target/MATEKF411/MATEKF411_RSSI.mk new file mode 100644 index 00000000000..e69de29bb2d diff --git a/src/main/target/MATEKF411/MATEKF411_SFTSRL2.mk b/src/main/target/MATEKF411/MATEKF411_SFTSRL2.mk new file mode 100644 index 00000000000..e69de29bb2d diff --git a/src/main/target/MATEKF411/target.c b/src/main/target/MATEKF411/target.c index d85fec5bbac..553718340ac 100755 --- a/src/main/target/MATEKF411/target.c +++ b/src/main/target/MATEKF411/target.c @@ -23,16 +23,21 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[] = { - { TIM3, IO_TAG(PB4), TIM_Channel_1, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM3, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR }, //S1 - { TIM3, IO_TAG(PB5), TIM_Channel_2, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM3, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR }, //S2 - { TIM4, IO_TAG(PB6), TIM_Channel_1, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM4, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, //S3 - { TIM4, IO_TAG(PB7), TIM_Channel_2, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM4, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, //S4 - { TIM2, IO_TAG(PB3), TIM_Channel_2, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM2, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO }, //S5 - { TIM2, IO_TAG(PB10), TIM_Channel_3, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM2, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO }, //S6 - { TIM2, IO_TAG(PA15), TIM_Channel_1, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM2, TIM_USE_MC_CHNFW | TIM_USE_FW_SERVO }, //S7 + // DEF_TIM(TIM9, CH2, PA3, TIM_USE_PPM, 0, 0), // PPM IN - { TIM1, IO_TAG(PA8), TIM_Channel_1, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM1, TIM_USE_LED}, //2812LED - { TIM5, IO_TAG(PA0), TIM_Channel_1, 0, IOCFG_AF_PP_PD, GPIO_AF_TIM5, TIM_USE_PPM }, //use rssi pad for PPM/softserial_tx1 + DEF_TIM(TIM3, CH1, PB4, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1_OUT + DEF_TIM(TIM3, CH2, PB5, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S2_OUT + DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S3_OUT + DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4_OUT + + DEF_TIM(TIM2, CH2, PB3, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S5 + DEF_TIM(TIM2, CH3, PB10, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S6 + DEF_TIM(TIM2, CH1, PA15, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S7 + + DEF_TIM(TIM1, CH1, PA8, TIM_USE_ANY, 0, 0), //softserial_tx2 - 2812LED TIM_USE_LED + DEF_TIM(TIM5, CH1, PA0, TIM_USE_PPM, 0, 0), //use rssi pad for PPM/softserial_tx1 + + //DEF_TIM(TIM5, CH3, PA2, TIM_USE_ANY, 0, 0), // TX2 }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/MATEKF411/target.h b/src/main/target/MATEKF411/target.h index 6b6a87d2517..79bd5d2b72d 100755 --- a/src/main/target/MATEKF411/target.h +++ b/src/main/target/MATEKF411/target.h @@ -74,11 +74,27 @@ #define UART2_TX_PIN PA2 #define UART2_RX_PIN PA3 -#define USE_SOFTSERIAL1 //Frsky SmartPort on rssi pad -#define SOFTSERIAL_1_TX_PIN PA0 -#define SOFTSERIAL_1_RX_PIN PA0 +#if defined(MATEKF411_SFTSRL2) +#define USE_SOFTSERIAL1 +#define SOFTSERIAL_1_TX_PIN PA0 // ST1 pad +#define SOFTSERIAL_1_RX_PIN PA0 +#define USE_SOFTSERIAL2 +#define SOFTSERIAL_2_TX_PIN PA8 // LED pad +#define SOFTSERIAL_2_RX_PIN PA8 +#define SERIAL_PORT_COUNT 5 + +#elif defined(MATEKF411_RSSI) +#define USE_SOFTSERIAL1 +#define SOFTSERIAL_1_TX_PIN PA8 // LED pad +#define SOFTSERIAL_1_RX_PIN PA8 +#define SERIAL_PORT_COUNT 4 +#else +#define USE_SOFTSERIAL1 +#define SOFTSERIAL_1_TX_PIN PA0 // ST1 pad +#define SOFTSERIAL_1_RX_PIN PA0 #define SERIAL_PORT_COUNT 4 +#endif #define DEFAULT_RX_TYPE RX_TYPE_SERIAL #define SERIALRX_PROVIDER SERIALRX_SBUS @@ -98,6 +114,8 @@ #define USE_BARO_MS5611 #define USE_BARO_BMP085 +#define TEMPERATURE_I2C_BUS BUS_I2C1 + #define USE_MAG #define MAG_I2C_BUS BUS_I2C1 #define USE_MAG_HMC5883 @@ -106,7 +124,6 @@ #define USE_MAG_MAG3110 #define USE_MAG_LIS3MDL -#define USE_PITOT_MS4525 #define PITOT_I2C_BUS BUS_I2C1 // *************** ADC ***************************** @@ -115,18 +132,19 @@ #define ADC1_DMA_STREAM DMA2_Stream0 #define ADC_CHANNEL_1_PIN PB0 #define ADC_CHANNEL_2_PIN PB1 -//#define ADC_CHANNEL_3_PIN PA0 #define VBAT_ADC_CHANNEL ADC_CHN_1 #define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 -//#define RSSI_ADC_CHANNEL ADC_CHN_3 + +#if defined(MATEKF411_RSSI) +#define ADC_CHANNEL_3_PIN PA0 +#define RSSI_ADC_CHANNEL ADC_CHN_3 +#endif // *************** LED2812 ************************ +#if !defined(MATEKF411_SFTSRL2) && !defined(MATEKF411_RSSI) #define USE_LED_STRIP #define WS2811_PIN PA8 -#define WS2811_DMA_HANDLER_IDENTIFER DMA2_ST1_HANDLER -#define WS2811_DMA_STREAM DMA2_Stream1 -#define WS2811_DMA_CHANNEL DMA_Channel_6 - +#endif // *************** OTHERS ************************* #define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_OSD | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TELEMETRY | FEATURE_SOFTSERIAL ) diff --git a/src/main/target/MATEKF411/target.mk b/src/main/target/MATEKF411/target.mk index e025819ce19..43139bcc712 100755 --- a/src/main/target/MATEKF411/target.mk +++ b/src/main/target/MATEKF411/target.mk @@ -2,7 +2,7 @@ F411_TARGETS += $(TARGET) FEATURES += VCP TARGET_SRC = \ - drivers/accgyro/accgyro_mpu6000.c \ + drivers/accgyro/accgyro_mpu6000.c \ drivers/barometer/barometer_bmp085.c \ drivers/barometer/barometer_bmp280.c \ drivers/barometer/barometer_ms56xx.c \ @@ -11,8 +11,6 @@ TARGET_SRC = \ drivers/compass/compass_qmc5883l.c \ drivers/compass/compass_ist8310.c \ drivers/compass/compass_lis3mdl.c \ - drivers/pitotmeter_ms4525.c \ drivers/pitotmeter_adc.c \ drivers/light_ws2811strip.c \ - drivers/light_ws2811strip_stdperiph.c \ drivers/max7456.c diff --git a/src/main/target/MATEKF722/MATEKF722_HEXSERVO.mk b/src/main/target/MATEKF722/MATEKF722_HEXSERVO.mk new file mode 100644 index 00000000000..e69de29bb2d diff --git a/src/main/target/MATEKF722/target.c b/src/main/target/MATEKF722/target.c index 41c2b57bb3a..20155c09de8 100755 --- a/src/main/target/MATEKF722/target.c +++ b/src/main/target/MATEKF722/target.c @@ -24,23 +24,25 @@ #include "drivers/pwm_mapping.h" #include "drivers/timer.h" -#define TIM_EN TIMER_OUTPUT_ENABLED -#define TIM_EN_N TIMER_OUTPUT_ENABLED | TIMER_OUTPUT_N_CHANNEL - const timerHardware_t timerHardware[] = { - { TIM5, IO_TAG(PA3), TIM_CHANNEL_4, 0, IOCFG_AF_PP_PD, GPIO_AF2_TIM5, TIM_USE_PPM }, + DEF_TIM(TIM5, CH4, PA3, TIM_USE_PPM, 0, 0), // PPM + + DEF_TIM(TIM3, CH1, PC6, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1 D(1, 4, 5) + DEF_TIM(TIM8, CH2, PC7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S2 D(2, 3, 7) + DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S3 D(2, 4, 7) + DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4 D(2, 7, 7) - { TIM3, IO_TAG(PC6), TIM_CHANNEL_1, TIM_EN, IOCFG_AF_PP_PD, GPIO_AF2_TIM3, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR }, //S1 DMA1_ST4 MT1 - { TIM8, IO_TAG(PC7), TIM_CHANNEL_2, TIM_EN, IOCFG_AF_PP_PD, GPIO_AF3_TIM8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, //S2 DMA2_ST3 SV3 - { TIM8, IO_TAG(PC8), TIM_CHANNEL_3, TIM_EN, IOCFG_AF_PP_PD, GPIO_AF3_TIM8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, //S3 DMA2_ST4 SV4 - { TIM8, IO_TAG(PC9), TIM_CHANNEL_4, TIM_EN, IOCFG_AF_PP_PD, GPIO_AF3_TIM8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, //S4 DMA2_ST7 SV5 - { TIM3, IO_TAG(PB1), TIM_CHANNEL_4, TIM_EN, IOCFG_AF_PP_PD, GPIO_AF2_TIM3, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR }, //S5 DMA1_ST2 MT2 - { TIM1, IO_TAG(PA8), TIM_CHANNEL_1, TIM_EN, IOCFG_AF_PP_PD, GPIO_AF1_TIM1, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO }, //S6 DMA2_ST6 SV6 - { TIM4, IO_TAG(PB8), TIM_CHANNEL_3, TIM_EN, IOCFG_AF_PP_PD, GPIO_AF2_TIM4, TIM_USE_MC_CHNFW | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO }, //S7 DMA1_ST7 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S5 DMA1_ST2 +#ifdef MATEKF722_HEXSERVO + DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S6 DMA2_ST6 +#else + DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S6 DMA2_ST6 +#endif + DEF_TIM(TIM4, CH3, PB8, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S7 DMA1_ST7 - //{ TIM5, IO_TAG(PA2), TIM_CHANNEL_3, TIM_EN, IOCFG_AF_PP_PD, GPIO_AF2_TIM5, TIM_USE_ANY }, + // DEF_TIM(TIM5, CH3, PA2, TIM_USE_ANY, 0, 0), // TX2/S8 DMA1_ST0 - { TIM2, IO_TAG(PA15), TIM_CHANNEL_1, TIM_EN, IOCFG_AF_PP_PD, GPIO_AF1_TIM2, TIM_USE_LED}, //2812 STRIP DMA1_ST5 + DEF_TIM(TIM2, CH1, PA15, TIM_USE_LED, 0, 0), // LED STRIP D(1, 5, 3) }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/MATEKF722/target.h b/src/main/target/MATEKF722/target.h index bcf30f4fbf0..6318091d00a 100755 --- a/src/main/target/MATEKF722/target.h +++ b/src/main/target/MATEKF722/target.h @@ -71,25 +71,23 @@ #define USE_MAG_MAG3110 #define USE_MAG_LIS3MDL -#define USE_PITOT_MS4525 +#define TEMPERATURE_I2C_BUS BUS_I2C1 + #define PITOT_I2C_BUS BUS_I2C1 // *************** SD Card ************************** #define USE_SDCARD -#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT +#define USE_SDCARD_SPI +#define SDCARD_SPI_BUS BUS_SPI3 +#define SDCARD_CS_PIN PC1 #define USE_SPI_DEVICE_3 #define SPI3_SCK_PIN PB3 #define SPI3_MISO_PIN PB4 #define SPI3_MOSI_PIN PB5 +#define SPI3_CLOCK_LEADING_EDGE -#define SDCARD_SPI_INSTANCE SPI3 -#define SDCARD_SPI_CS_PIN PC1 - -#define SDCARD_DMA_CHANNEL_TX DMA1_Stream7 -#define SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG DMA_FLAG_TCIF7 -#define SDCARD_DMA_CLK RCC_AHB1Periph_DMA1 -#define SDCARD_DMA_CHANNEL DMA_CHANNEL_0 +#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT // *************** OSD ***************************** #define USE_SPI_DEVICE_2 @@ -145,18 +143,12 @@ #define RSSI_ADC_CHANNEL ADC_CHN_3 #define USE_RANGEFINDER -#define USE_RANGEFINDER_VL53L0X -#define VL53L0X_I2C_BUS BUS_I2C1 #define RANGEFINDER_I2C_BUS BUS_I2C1 -#define USE_RANGEFINDER_HCSR04_I2C #define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_CURRENT_METER | FEATURE_TELEMETRY| FEATURE_VBAT | FEATURE_OSD ) #define USE_LED_STRIP #define WS2811_PIN PA15 //TIM2_CH1 -#define WS2811_DMA_HANDLER_IDENTIFER DMA1_ST5_HANDLER -#define WS2811_DMA_STREAM DMA1_Stream5 -#define WS2811_DMA_CHANNEL DMA_CHANNEL_3 #define USE_SPEKTRUM_BIND #define BIND_PIN PA3 //USART2_RX @@ -169,3 +161,4 @@ #define TARGET_IO_PORTD (BIT(2)) #define MAX_PWM_OUTPUT_PORTS 7 +#define USE_DSHOT diff --git a/src/main/target/MATEKF722/target.mk b/src/main/target/MATEKF722/target.mk index 7af4a300ce8..730cfa99000 100755 --- a/src/main/target/MATEKF722/target.mk +++ b/src/main/target/MATEKF722/target.mk @@ -13,8 +13,6 @@ TARGET_SRC = \ drivers/compass/compass_mag3110.c \ drivers/compass/compass_lis3mdl.c \ drivers/light_ws2811strip.c \ - drivers/light_ws2811strip_hal.c \ drivers/max7456.c \ - drivers/pitotmeter_ms4525.c \ drivers/pitotmeter_adc.c \ diff --git a/src/main/target/MATEKF722SE/MATEKF722MINI.mk b/src/main/target/MATEKF722SE/MATEKF722MINI.mk new file mode 100644 index 00000000000..e69de29bb2d diff --git a/src/main/target/MATEKF722SE/config.c b/src/main/target/MATEKF722SE/config.c new file mode 100644 index 00000000000..c7d6689cddd --- /dev/null +++ b/src/main/target/MATEKF722SE/config.c @@ -0,0 +1,26 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include +#include "platform.h" +#include "io/piniobox.h" + +void targetConfiguration(void) +{ + pinioBoxConfigMutable()->permanentId[0] = 47; + pinioBoxConfigMutable()->permanentId[1] = 48; +} diff --git a/src/main/target/MATEKF722SE/target.c b/src/main/target/MATEKF722SE/target.c new file mode 100644 index 00000000000..d792cec4bf2 --- /dev/null +++ b/src/main/target/MATEKF722SE/target.c @@ -0,0 +1,50 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + */ + +#include + +#include "platform.h" + +#include "drivers/bus.h" +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" +#include "drivers/timer.h" +#include "drivers/pinio.h" + +BUSDEV_REGISTER_SPI_TAG(busdev_mpu6000, DEVHW_MPU6000, MPU6000_SPI_BUS, MPU6000_CS_PIN, MPU6000_EXTI_PIN, 0, DEVFLAGS_NONE); +BUSDEV_REGISTER_SPI_TAG(busdev_mpu6500, DEVHW_MPU6500, MPU6500_SPI_BUS, MPU6500_CS_PIN, MPU6500_EXTI_PIN, 1, DEVFLAGS_NONE); + +const timerHardware_t timerHardware[] = { + DEF_TIM(TIM3, CH1, PB4, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S1 UP1-2 D(1, 4, 5) + DEF_TIM(TIM3, CH2, PB5, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S2 UP1-2 D(1, 5, 5) + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S3 UP1-2 D(1, 7, 5) + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4 UP1-2 D(1, 2, 5) + + DEF_TIM(TIM2, CH1, PA15, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S5 UP1-7 D(1, 5, 3) - clash with S2 + DEF_TIM(TIM2, CH2, PB3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S6 UP1-7 D(1, 6, 3) + + DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR, 0, 0), // S7 UP1-6 D(1, 0, 2) + DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR, 0, 0), // S8 UP1-6 D(1, 3, 2) + + DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 2), // LED D(2, 6, 0) + + DEF_TIM(TIM9, CH2, PA3, TIM_USE_PPM, 0, 0), // PPM, RX2 + + DEF_TIM(TIM5, CH3, PA2, TIM_USE_PWM, 0, 0), // TX2 & softserial1 +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/MATEKF722SE/target.h b/src/main/target/MATEKF722SE/target.h new file mode 100644 index 00000000000..7d98e23a3b5 --- /dev/null +++ b/src/main/target/MATEKF722SE/target.h @@ -0,0 +1,199 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + */ + + +#pragma once + +#if defined(MATEKF722MINI) +# define TARGET_BOARD_IDENTIFIER "MF7M" +# define USBD_PRODUCT_STRING "MatekF722Mini" +#else +# define TARGET_BOARD_IDENTIFIER "MF7S" +# define USBD_PRODUCT_STRING "MatekF722SE" +#endif + +#define LED0 PA14 //Blue SWCLK +#define LED1 PA13 //Green SWDIO + +#define BEEPER PC13 +#define BEEPER_INVERTED + +// *************** SPI1 Gyro & ACC ******************* +#define USE_TARGET_IMU_HARDWARE_DESCRIPTORS + +#define USE_SPI +#define USE_SPI_DEVICE_1 +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 + +#define USE_ACC +#define USE_GYRO +#define USE_DUAL_GYRO + +#define USE_ACC_MPU6000 +#define USE_GYRO_MPU6000 +#define MPU6000_CS_PIN PB2 +#define MPU6000_SPI_BUS BUS_SPI1 +#define MPU6000_EXTI_PIN PC4 + +#define USE_ACC_MPU6500 +#define USE_GYRO_MPU6500 +#define MPU6500_CS_PIN PC15 +#define MPU6500_SPI_BUS BUS_SPI1 +#define MPU6500_EXTI_PIN PC3 + +#define GYRO_MPU6000_ALIGN CW180_DEG_FLIP +#define ACC_MPU6000_ALIGN CW180_DEG_FLIP + +#define GYRO_MPU6500_ALIGN CW90_DEG +#define ACC_MPU6500_ALIGN CW90_DEG + +#define USE_EXTI +#define USE_MPU_DATA_READY_SIGNAL + +// *************** I2C /Baro/Mag ********************* +#define USE_I2C +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB8 +#define I2C1_SDA PB9 + +#define USE_BARO +#define BARO_I2C_BUS BUS_I2C1 +#define USE_BARO_BMP280 +#define USE_BARO_MS5611 + +#define USE_MAG +#define MAG_I2C_BUS BUS_I2C1 +#define USE_MAG_HMC5883 +#define USE_MAG_QMC5883 +#define USE_MAG_IST8310 +#define USE_MAG_IST8308 +#define USE_MAG_MAG3110 +#define USE_MAG_LIS3MDL + +#define TEMPERATURE_I2C_BUS BUS_I2C1 + +#define PITOT_I2C_BUS BUS_I2C1 + +#define USE_RANGEFINDER +#define RANGEFINDER_I2C_BUS BUS_I2C1 + +// *************** SPI2 OSD *********************** +#define USE_SPI_DEVICE_2 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 + +#define USE_OSD +#define USE_MAX7456 +#define MAX7456_SPI_BUS BUS_SPI2 +#define MAX7456_CS_PIN PB12 + +// *************** SPI3 SD BLACKBOX******************* +#define USE_SPI_DEVICE_3 +#define SPI3_SCK_PIN PC10 +#define SPI3_MISO_PIN PC11 +#define SPI3_MOSI_PIN PC12 + +#if defined(MATEKF722MINI) +# define USE_FLASHFS +# define USE_FLASH_M25P16 +# define M25P16_SPI_BUS BUS_SPI3 +# define M25P16_CS_PIN PD2 +# define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT +#else +# define USE_SDCARD +# define USE_SDCARD_SPI +# define SDCARD_SPI_BUS BUS_SPI3 +# define SDCARD_CS_PIN PD2 +# define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT +# define SPI3_CLOCK_LEADING_EDGE // TODO(digitalentity): implement DEVFLAGS_SPI_MODE_0 flag in SPI drivers +#endif + +// *************** UART ***************************** +#define USE_VCP +#define USB_DETECT_PIN PC14 +#define USE_USB_DETECT + +#define USE_UART1 +#define UART1_TX_PIN PA9 +#define UART1_RX_PIN PA10 + +#define USE_UART2 +#define UART2_TX_PIN PA2 +#define UART2_RX_PIN PA3 + +#define USE_UART3 +#define UART3_TX_PIN PB10 +#define UART3_RX_PIN PB11 + +#define USE_UART4 +#define UART4_TX_PIN PA0 +#define UART4_RX_PIN PA1 + +#define USE_UART6 +#define UART6_TX_PIN PC6 +#define UART6_RX_PIN PC7 + +#define USE_SOFTSERIAL1 +#define SOFTSERIAL_1_TX_PIN PA2 +#define SOFTSERIAL_1_RX_PIN PA2 + +#define SERIAL_PORT_COUNT 7 + +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_SBUS +#define SERIALRX_UART SERIAL_PORT_USART2 + +// *************** ADC ***************************** +#define USE_ADC +#define ADC_INSTANCE ADC1 +#define ADC1_DMA_STREAM DMA2_Stream0 + +#define ADC_CHANNEL_1_PIN PC2 +#define ADC_CHANNEL_2_PIN PC1 +#define ADC_CHANNEL_3_PIN PC0 +#define ADC_CHANNEL_4_PIN PA4 + +#define VBAT_ADC_CHANNEL ADC_CHN_1 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 +#define RSSI_ADC_CHANNEL ADC_CHN_3 +#define AIRSPEED_ADC_CHANNEL ADC_CHN_4 + +// *************** PINIO *************************** +#define USE_PINIO +#define USE_PINIOBOX +#define PINIO1_PIN PC8 // VTX power switcher +#define PINIO2_PIN PC9 // 2xCamera switcher + +// *************** LEDSTRIP ************************ +#define USE_LED_STRIP +#define WS2811_PIN PA8 + +#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_TELEMETRY | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TX_PROF_SEL | FEATURE_BLACKBOX) +#define CURRENT_METER_SCALE 179 + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD 0xffff + +#define MAX_PWM_OUTPUT_PORTS 8 +#define USE_DSHOT diff --git a/src/main/target/MATEKF722SE/target.mk b/src/main/target/MATEKF722SE/target.mk new file mode 100644 index 00000000000..db59b6711f8 --- /dev/null +++ b/src/main/target/MATEKF722SE/target.mk @@ -0,0 +1,19 @@ +F7X2RE_TARGETS += $(TARGET) +FEATURES += SDCARD VCP ONBOARDFLASH + +TARGET_SRC = \ + drivers/accgyro/accgyro_mpu6000.c \ + drivers/accgyro/accgyro_mpu6500.c \ + drivers/barometer/barometer_bmp085.c \ + drivers/barometer/barometer_bmp280.c \ + drivers/barometer/barometer_ms56xx.c \ + drivers/compass/compass_hmc5883l.c \ + drivers/compass/compass_qmc5883l.c \ + drivers/compass/compass_ist8310.c \ + drivers/compass/compass_ist8308.c \ + drivers/compass/compass_mag3110.c \ + drivers/compass/compass_lis3mdl.c \ + drivers/light_ws2811strip.c \ + drivers/max7456.c \ + drivers/pitotmeter_adc.c \ + diff --git a/src/main/target/MOTOLAB/target.c b/src/main/target/MOTOLAB/target.c index 82e228ec175..d6abc0ab093 100644 --- a/src/main/target/MOTOLAB/target.c +++ b/src/main/target/MOTOLAB/target.c @@ -23,16 +23,16 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[] = { - { TIM3, IO_TAG(PA4), TIM_Channel_2, 1, IOCFG_AF_PP, GPIO_AF_2, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR }, // PWM1 - PA4 - *TIM3_CH2 - { TIM3, IO_TAG(PA6), TIM_Channel_1, 1, IOCFG_AF_PP, GPIO_AF_2, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR }, // PWM2 - PA6 - *TIM3_CH1, TIM8_BKIN, TIM1_BKIN, TIM16_CH1 - { TIM8, IO_TAG(PB0), TIM_Channel_2, 1 | TIMER_OUTPUT_N_CHANNEL, IOCFG_AF_PP, GPIO_AF_2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // PWM3 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N - { TIM8, IO_TAG(PB1), TIM_Channel_3, 1 | TIMER_OUTPUT_N_CHANNEL, IOCFG_AF_PP, GPIO_AF_2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // PWM4 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N - { TIM2, IO_TAG(PA1), TIM_Channel_2, 1, IOCFG_AF_PP, GPIO_AF_1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // PWM5 - PA1 - *TIM2_CH2, TIM15_CH1N - { TIM2, IO_TAG(PA2), TIM_Channel_3, 1, IOCFG_AF_PP, GPIO_AF_1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // PWM6 - PA2 - *TIM2_CH3, !TIM15_CH1 - { TIM15, IO_TAG(PA3), TIM_Channel_2, 1, IOCFG_AF_PP, GPIO_AF_9, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO }, // PWM7 - PA3 - *TIM15_CH2, TIM2_CH4 - { TIM1, IO_TAG(PA8), TIM_Channel_1, 1, IOCFG_AF_PP, GPIO_AF_6, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO }, // PWM8 - PA8 - *TIM1_CH1, TIM4_ETR - { TIM17, IO_TAG(PA7), TIM_Channel_1, 0, IOCFG_AF_PP_PD, GPIO_AF_1, TIM_USE_PPM }, // PPM - PA7 - *TIM17_CH1, TIM1_CH1N, TIM8_CH1 - { TIM16, IO_TAG(PB8), TIM_Channel_1, 0, IOCFG_AF_PP_PD, GPIO_AF_1, TIM_USE_ANY }, + DEF_TIM(TIM3, CH2, PA4, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0), // PWM1 - PA4 - *TIM3_CH2 + DEF_TIM(TIM3, CH1, PA6, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0), // PWM2 - PA6 - *TIM3_CH1, TIM8_BKIN, TIM1_BKIN, TIM16_CH1 + DEF_TIM(TIM8, CH2N, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // PWM3 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N + DEF_TIM(TIM8, CH3N, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // PWM4 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N + DEF_TIM(TIM2, CH2, PA1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // PWM5 - PA1 - *TIM2_CH2, TIM15_CH1N + DEF_TIM(TIM2, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // PWM6 - PA2 - *TIM2_CH3, !TIM15_CH1 + DEF_TIM(TIM15, CH2, PA3, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0), // PWM7 - PA3 - *TIM15_CH2, TIM2_CH4 + DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0), // PWM8 - PA8 - *TIM1_CH1, TIM4_ETR + DEF_TIM(TIM17, CH1, PA7, TIM_USE_PPM, 0), // PPM - PA7 - *TIM17_CH1, TIM1_CH1N, TIM8_CH1 + DEF_TIM(TIM16, CH1, PB8, TIM_USE_LED, 0), // LED - PB8 - *TIM16_CH1, TIM4_CH3, TIM8_CH2 }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/MOTOLAB/target.h b/src/main/target/MOTOLAB/target.h index 3ab28a06606..278bd9654dc 100644 --- a/src/main/target/MOTOLAB/target.h +++ b/src/main/target/MOTOLAB/target.h @@ -102,11 +102,6 @@ #define USE_LED_STRIP_ON_DMA1_CHANNEL3 #define WS2811_PIN PB8 // TIM16_CH1 -#define WS2811_TIMER TIM16 -#define WS2811_DMA_STREAM DMA1_Channel3 -#define WS2811_IRQ DMA1_Channel3_IRQn -#define WS2811_DMA_TC_FLAG DMA1_FLAG_TC3 -#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH3_HANDLER #define USE_SPEKTRUM_BIND diff --git a/src/main/target/MOTOLAB/target.mk b/src/main/target/MOTOLAB/target.mk index 2108fd450e1..e9d744c1420 100644 --- a/src/main/target/MOTOLAB/target.mk +++ b/src/main/target/MOTOLAB/target.mk @@ -13,6 +13,5 @@ TARGET_SRC = \ drivers/compass/compass_mag3110.c \ drivers/compass/compass_lis3mdl.c \ drivers/light_ws2811strip.c \ - drivers/light_ws2811strip_stdperiph.c \ drivers/serial_softserial.c diff --git a/src/main/target/NOX/target.c b/src/main/target/NOX/target.c new file mode 100755 index 00000000000..adf62c34ce8 --- /dev/null +++ b/src/main/target/NOX/target.c @@ -0,0 +1,39 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include +#include + +#include "drivers/io.h" + +#include "drivers/dma.h" +#include "drivers/timer.h" + +const timerHardware_t timerHardware[] = { + DEF_TIM(TIM2, CH3, PB10, TIM_USE_PPM, 0, 0), //PPM + DEF_TIM(TIM2, CH1, PA0, TIM_USE_LED, 0, 0), //2812LED + + DEF_TIM(TIM5, CH2, PA1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1_OUT + DEF_TIM(TIM1, CH1N, PA7, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S2_OUT + DEF_TIM(TIM4, CH3, PB8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S3_OUT + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4_OUT + + DEF_TIM(TIM9, CH1, PA2, TIM_USE_ANY, 0, 0), //UART2 TX + DEF_TIM(TIM9, CH2, PA3, TIM_USE_ANY, 0, 0), //UART2 RX +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/NOX/target.h b/src/main/target/NOX/target.h new file mode 100755 index 00000000000..7951a76183c --- /dev/null +++ b/src/main/target/NOX/target.h @@ -0,0 +1,147 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once + + +#define TARGET_BOARD_IDENTIFIER "NOX1" +#define USBD_PRODUCT_STRING "NoxF4V1" + +#define LED0 PA4 + +#define BEEPER PC13 +#define BEEPER_INVERTED + +#define I2C2_SCL NONE +#define I2C2_SDA NONE + +// *************** SPI ********************** +#define USE_SPI +#define USE_SPI_DEVICE_1 +#define SPI1_SCK_PIN PB3 +#define SPI1_MISO_PIN PB4 +#define SPI1_MOSI_PIN PB5 + +#define USE_SPI_DEVICE_2 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 + + +// *************** SPI Gyro & ACC ********************** +#define MPU6000_CS_PIN PB12 +#define MPU6000_SPI_BUS BUS_SPI2 + +#define USE_EXTI +#define GYRO_INT_EXTI PA8 +#define USE_MPU_DATA_READY_SIGNAL + +#define USE_GYRO +#define USE_GYRO_MPU6000 + +#define USE_ACC +#define USE_ACC_MPU6000 + +// *************** SPI BARO ***************************** +#define USE_BARO +#define USE_BARO_BMP280 +#define USE_BARO_SPI_BMP280 +#define BMP280_SPI_BUS BUS_SPI2 +#define BMP280_CS_PIN PA9 + +// *************** SPI OSD ***************************** +#define USE_OSD +#define USE_MAX7456 +#define MAX7456_SPI_BUS BUS_SPI2 +#define MAX7456_CS_PIN PA10 +//#define MAX7456_SPI_CLK (SPI_CLOCK_STANDARD*2) +//#define MAX7456_RESTORE_CLK (SPI_CLOCK_FAST) + +// *************** SPI FLASH ************************** +#define USE_FLASHFS +#define USE_FLASH_M25P16 +#define M25P16_CS_PIN PA15 +#define M25P16_SPI_BUS BUS_SPI1 +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT + +// *************** UART ***************************** +#define USE_UART_INVERTER + +#define USE_VCP + +#define USE_UART1 +#define UART1_TX_PIN PB6 +#define UART1_RX_PIN PB7 + +#define USE_UART2 +#define UART2_TX_PIN PA2 +#define UART2_RX_PIN PA3 + +// #define INVERTER_PIN_UART2 PC14 +#define INVERTER_PIN_UART2_RX PC14 // PC14 used as inverter select GPIO + +#define USE_SOFTSERIAL1 +#define SOFTSERIAL_1_TX_PIN PA2 // Workaround for softserial not initializing with only RX +#define SOFTSERIAL_1_RX_PIN PA2 // Backdoor timer on UART2_TX, used for ESC telemetry + +#define SERIAL_PORT_COUNT 4 //VCP, USART1, USART2, SOFTSERIAL1 + +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_SBUS +#define SERIALRX_UART SERIAL_PORT_USART2 + +// *************** ADC ***************************** +#define USE_ADC +#define ADC_INSTANCE ADC1 +#define ADC_CHANNEL_1_PIN NONE +#define ADC_CHANNEL_2_PIN PA5 +#define ADC_CHANNEL_3_PIN NONE + +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_1 +#define VBAT_ADC_CHANNEL ADC_CHN_2 +#define RSSI_ADC_CHANNEL ADC_CHN_3 +/* +#define ADC_INSTANCE ADC1 +#define ADC1_DMA_STREAM DMA2_Stream0 +#define ADC_CHANNEL_1_PIN PA5 +#define ADC_CHANNEL_2_PIN NONE +//#define ADC_CHANNEL_3_PIN PA0 +#define VBAT_ADC_CHANNEL ADC_CHN_1 +//#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 +//#define RSSI_ADC_CHANNEL ADC_CHN_3 +*/ +// *************** LED2812 ************************ +#define USE_LED_STRIP +#define WS2811_PIN PA0 +#define WS2811_DMA_HANDLER_IDENTIFER DMA2_ST1_HANDLER +#define WS2811_DMA_STREAM DMA2_Stream1 +#define WS2811_DMA_CHANNEL DMA_Channel_6 + +// *************** OTHERS ************************* +#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_SOFTSERIAL | FEATURE_VBAT) + + +// #define USE_SPEKTRUM_BIND +// #define BIND_PIN PA10 // RX1 + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +#define TARGET_IO_PORTA (0xffff & ~(BIT(14)|BIT(13))) +#define TARGET_IO_PORTB (0xffff & ~(BIT(2)|BIT(11))) +#define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) + +#define MAX_PWM_OUTPUT_PORTS 4 diff --git a/src/main/target/NOX/target.mk b/src/main/target/NOX/target.mk new file mode 100755 index 00000000000..d0c27801a35 --- /dev/null +++ b/src/main/target/NOX/target.mk @@ -0,0 +1,10 @@ +F411_TARGETS += $(TARGET) +FEATURES += VCP ONBOARDFLASH + +TARGET_SRC = \ + drivers/accgyro/accgyro_mpu6000.c \ + drivers/barometer/barometer_bmp280.c \ + drivers/light_ws2811strip.c \ + drivers/flash_m25p16.c \ + drivers/max7456.c + diff --git a/src/main/target/OMNIBUS/target.c b/src/main/target/OMNIBUS/target.c index 2af6170fc34..acddb3ca638 100644 --- a/src/main/target/OMNIBUS/target.c +++ b/src/main/target/OMNIBUS/target.c @@ -24,20 +24,22 @@ const timerHardware_t timerHardware[] = { // PWM1 PPM Pad - { TIM3, IO_TAG(PB4), TIM_Channel_1, 0, IOCFG_AF_PP, GPIO_AF_2, TIM_USE_PPM }, // Pin PPM - PB4 + DEF_TIM(TIM3, CH1, PB4, TIM_USE_PPM, 0), // PPM - PB4 + // PB5 / TIM3 CH2 is connected to USBPresent // PWM2-PWM5 - { TIM8, IO_TAG(PB8), TIM_Channel_2, 1, IOCFG_AF_PP, GPIO_AF_10, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR },// Pin PWM1 - PB8 - { TIM4, IO_TAG(PB9), TIM_Channel_4, 1, IOCFG_AF_PP, GPIO_AF_2, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR }, // Pin PWM2 - PB9 - { TIM15, IO_TAG(PA3), TIM_Channel_2, 1, IOCFG_AF_PP, GPIO_AF_9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // Pin PWM3 - PA3 - { TIM15, IO_TAG(PA2), TIM_Channel_1, 1, IOCFG_AF_PP, GPIO_AF_9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // Pin PWM4 - PA2 + DEF_TIM(TIM8, CH2, PB8, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR, 0), // PWM1 - PB8 + DEF_TIM(TIM4, CH4, PB9, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0), // PWM2 - PB9 + DEF_TIM(TIM15, CH2, PA3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // PWM3 - PA3 + DEF_TIM(TIM15, CH1, PA2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // PWM4 - PA2 + // For iNav, PWM6&7 (PWM pins 5&6) are shared with UART3 - { TIM2, IO_TAG(PB10), TIM_Channel_3, 1, IOCFG_AF_PP, GPIO_AF_1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // Pin PWM5 - PB10 - TIM2_CH3 / UART3_TX (AF7) - { TIM2, IO_TAG(PB11), TIM_Channel_4, 1, IOCFG_AF_PP, GPIO_AF_1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // Pin PWM6 - PB11 - TIM2_CH4 / UART3_RX (AF7) + DEF_TIM(TIM2, CH3, PB10, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // PWM4 - PA2 + DEF_TIM(TIM2, CH4, PB11, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // PWM4 - PA2 - { TIM1, IO_TAG(PA8), TIM_Channel_1, 1, IOCFG_AF_PP, GPIO_AF_6, TIM_USE_LED }, + DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0), }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/OMNIBUS/target.h b/src/main/target/OMNIBUS/target.h index c6989a2c453..cccbce0120d 100644 --- a/src/main/target/OMNIBUS/target.h +++ b/src/main/target/OMNIBUS/target.h @@ -87,7 +87,6 @@ #define USE_I2C_DEVICE_1 // PB6/SCL(PWM8), PB7/SDA(PWM7) #define USE_I2C_PULLUP -#define USE_PITOT_MS4525 #define PITOT_I2C_BUS BUS_I2C1 #define USE_SPI @@ -97,26 +96,18 @@ #define SPI2_SCK_PIN PB13 #define SPI2_MISO_PIN PB14 #define SPI2_MOSI_PIN PB15 +#define SPI2_CLOCK_LEADING_EDGE //#define USE_RX_SPI #define RX_SPI_INSTANCE SPI2 #define RX_NSS_PIN PB3 #define USE_SDCARD -#define USE_SDCARD_SPI2 - +#define USE_SDCARD_SPI #define SDCARD_DETECT_INVERTED - -#define SDCARD_DETECT_PIN PC14 -#define SDCARD_SPI_INSTANCE SPI2 -#define SDCARD_SPI_CS_PIN SPI2_NSS_PIN - -// Note, this is the same DMA channel as UART1_RX. Luckily we don't use DMA for USART Rx. -#define SDCARD_DMA_CHANNEL_TX DMA1_Channel5 -#define SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG DMA1_FLAG_TC5 - -// Performance logging for SD card operations: -// #define AFATFS_USE_INTROSPECTIVE_LOGGING +#define SDCARD_DETECT_PIN PC14 +#define SDCARD_SPI_BUS BUS_SPI2 +#define SDCARD_CS_PIN SPI2_NSS_PIN #define USE_OSD #define USE_MAX7456 @@ -136,8 +127,6 @@ #define USE_LED_STRIP #define WS2811_PIN PA8 -#define WS2811_DMA_STREAM DMA1_Channel2 -#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH2_HANDLER //#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT diff --git a/src/main/target/OMNIBUS/target.mk b/src/main/target/OMNIBUS/target.mk index 4fc8af5c40a..163577e3f4b 100644 --- a/src/main/target/OMNIBUS/target.mk +++ b/src/main/target/OMNIBUS/target.mk @@ -13,9 +13,8 @@ TARGET_SRC = \ drivers/compass/compass_ist8308.c \ drivers/compass/compass_mag3110.c \ drivers/compass/compass_lis3mdl.c \ - drivers/pitotmeter_ms4525.c \ + drivers/compass/compass_ak8975.c \ drivers/pitotmeter_adc.c \ drivers/light_ws2811strip.c \ - drivers/light_ws2811strip_stdperiph.c \ drivers/serial_usb_vcp.c \ drivers/max7456.c diff --git a/src/main/target/OMNIBUSF4/DYSF4PROV2.mk b/src/main/target/OMNIBUSF4/DYSF4PROV2.mk new file mode 100755 index 00000000000..f1b5bb0faf4 --- /dev/null +++ b/src/main/target/OMNIBUSF4/DYSF4PROV2.mk @@ -0,0 +1 @@ +#DYSF4PROV2 diff --git a/src/main/target/OMNIBUSF4/target.c b/src/main/target/OMNIBUSF4/target.c index 6d97277868b..c32af130e93 100644 --- a/src/main/target/OMNIBUSF4/target.c +++ b/src/main/target/OMNIBUSF4/target.c @@ -25,27 +25,32 @@ const timerHardware_t timerHardware[] = { #if defined(OMNIBUSF4PRO) || defined(OMNIBUSF4V3) - { TIM10, IO_TAG(PB8), TIM_Channel_1, 0, IOCFG_AF_PP_PD, GPIO_AF_TIM10, TIM_USE_PPM }, // PPM - { TIM4, IO_TAG(PB9), TIM_Channel_4, 0, IOCFG_AF_PP_PD, GPIO_AF_TIM4, TIM_USE_ANY }, // S2_IN + DEF_TIM(TIM10, CH1, PB8, TIM_USE_PPM, 0, 0), // PPM + DEF_TIM(TIM4, CH4, PB9, TIM_USE_ANY, 0, 0), // S2_IN #else - { TIM12, IO_TAG(PB14), TIM_Channel_1, 0, IOCFG_AF_PP_PD, GPIO_AF_TIM12, TIM_USE_PPM }, // PPM / S.BUS input, above MOTOR1 - { TIM12, IO_TAG(PB15), TIM_Channel_2, 0, IOCFG_AF_PP_PD, GPIO_AF_TIM12, TIM_USE_ANY }, // Connected: small CH2 pad, not used as PWM, definition inherited from REVO target - GPIO_PartialRemap_TIM3 + DEF_TIM(TIM12, CH1, PB14, TIM_USE_PPM, 0, 0), // PPM + DEF_TIM(TIM12, CH2, PB15, TIM_USE_ANY, 0, 0), // S2_IN #endif - { TIM8, IO_TAG(PC6), TIM_Channel_1, 0, IOCFG_AF_PP_PD, GPIO_AF_TIM8, TIM_USE_ANY }, // Connected: UART6 TX, not used as PWM, definition inherited from REVO target - { TIM8, IO_TAG(PC7), TIM_Channel_2, 0, IOCFG_AF_PP_PD, GPIO_AF_TIM8, TIM_USE_ANY }, // Connected: UART6 RX, not used as PWM, definition inherited from REVO target - { TIM8, IO_TAG(PC8), TIM_Channel_3, 0, IOCFG_AF_PP_PD, GPIO_AF_TIM8, TIM_USE_ANY }, // Connected: small CH5 pad, not used as PWM, definition inherited from REVO target - { TIM8, IO_TAG(PC9), TIM_Channel_4, 0, IOCFG_AF_PP_PD, GPIO_AF_TIM8, TIM_USE_ANY }, // Connected: small CH6 pad, not used as PWM, definition inherited from REVO target - { TIM3, IO_TAG(PB0), TIM_Channel_3, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM3, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR }, // MOTOR_1 - { TIM3, IO_TAG(PB1), TIM_Channel_4, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM3, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR }, // MOTOR_2 - { TIM9, IO_TAG(PA3), TIM_Channel_2, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // MOTOR_3 - { TIM2, IO_TAG(PA2), TIM_Channel_3, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // MOTOR_4 + DEF_TIM(TIM8, CH1, PC6, TIM_USE_ANY, 0, 0), // S3_IN, UART6_TX + DEF_TIM(TIM8, CH2, PC7, TIM_USE_ANY, 0, 0), // S4_IN, UART6_RX + DEF_TIM(TIM8, CH3, PC8, TIM_USE_ANY, 0, 0), // S5_IN + DEF_TIM(TIM8, CH4, PC9, TIM_USE_ANY, 0, 0), // S6_IN + + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1_OUT D1_ST7 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S2_OUT D1_ST2 + DEF_TIM(TIM2, CH4, PA3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), // S3_OUT D1_ST6 + DEF_TIM(TIM2, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4_OUT D1_ST1 + + // { TIM9, IO_TAG(PA3), TIM_Channel_2, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // MOTOR_3 #if (defined(OMNIBUSF4PRO) || defined(OMNIBUSF4V3)) && !defined(OMNIBUSF4PRO_LEDSTRIPM5) - { TIM5, IO_TAG(PA1), TIM_Channel_2, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM5, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO }, // MOTOR_5 - { TIM4, IO_TAG(PB6), TIM_Channel_1, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM4, TIM_USE_LED }, // LED strip for F4 V2 / F4-Pro-0X and later + DEF_TIM(TIM5, CH2, PA1, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S5_OUT #else - { TIM5, IO_TAG(PA1), TIM_Channel_2, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM5, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO }, // MOTOR_5 + DEF_TIM(TIM5, CH2, PA1, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S5_OUT +#endif + DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S6_OUT +#if (defined(OMNIBUSF4PRO) || defined(OMNIBUSF4V3)) && !defined(OMNIBUSF4PRO_LEDSTRIPM5) + DEF_TIM(TIM4, CH1, PB6, TIM_USE_LED, 0, 0), // LED strip for F4 V2 / F4-Pro-0X and later (RCD_CS for F4) #endif - { TIM1, IO_TAG(PA8), TIM_Channel_1, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM1, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO }, // MOTOR_6 }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/OMNIBUSF4/target.h b/src/main/target/OMNIBUSF4/target.h index dd420f158ec..6ab62f25500 100644 --- a/src/main/target/OMNIBUSF4/target.h +++ b/src/main/target/OMNIBUSF4/target.h @@ -26,6 +26,8 @@ #define TARGET_BOARD_IDENTIFIER "OB43" #elif defined(DYSF4PRO) #define TARGET_BOARD_IDENTIFIER "DYS4" +#elif defined(DYSF4PROV2) +#define TARGET_BOARD_IDENTIFIER "DY42" #else #define TARGET_BOARD_IDENTIFIER "OBF4" #endif @@ -41,11 +43,20 @@ #define BEEPER PB4 #define BEEPER_INVERTED +#if defined(DYSF4PROV2) +#define USE_I2C +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB8 +#define I2C1_SDA PB9 +#define I2C_EXT_BUS BUS_I2C1 +#else #define USE_I2C #define USE_I2C_DEVICE_2 #define I2C_DEVICE_2_SHARES_UART3 +#define I2C_EXT_BUS BUS_I2C2 +#endif -#define UG2864_I2C_BUS BUS_I2C2 +#define UG2864_I2C_BUS I2C_EXT_BUS // MPU6000 interrupts #define USE_EXTI @@ -85,7 +96,7 @@ #endif #define USE_MAG -#define MAG_I2C_BUS BUS_I2C2 +#define MAG_I2C_BUS I2C_EXT_BUS #define MAG_HMC5883_ALIGN CW90_DEG #define USE_MAG_HMC5883 #define USE_MAG_QMC5883 @@ -93,6 +104,9 @@ #define USE_MAG_IST8308 #define USE_MAG_MAG3110 #define USE_MAG_LIS3MDL +#define USE_MAG_AK8975 + +#define TEMPERATURE_I2C_BUS I2C_EXT_BUS #define USE_BARO @@ -102,23 +116,20 @@ #define BMP280_CS_PIN PB3 // v1 // Support external barometers - #define BARO_I2C_BUS BUS_I2C2 + #define BARO_I2C_BUS I2C_EXT_BUS #define USE_BARO_BMP085 #define USE_BARO_MS5611 #else - #define BARO_I2C_BUS BUS_I2C2 + #define BARO_I2C_BUS I2C_EXT_BUS #define USE_BARO_BMP085 #define USE_BARO_BMP280 #define USE_BARO_MS5611 #endif -#define USE_PITOT_MS4525 -#define PITOT_I2C_BUS BUS_I2C2 +#define PITOT_I2C_BUS I2C_EXT_BUS #define USE_RANGEFINDER -#define RANGEFINDER_I2C_BUS BUS_I2C2 -#define USE_RANGEFINDER_HCSR04_I2C -#define USE_RANGEFINDER_VL53L0X +#define RANGEFINDER_I2C_BUS I2C_EXT_BUS #define USE_OPTICAL_FLOW #define USE_OPFLOW_CXOF @@ -169,6 +180,7 @@ #if defined(OMNIBUSF4PRO) || defined(OMNIBUSF4V3) #define USE_SPI_DEVICE_2 + #define SPI2_CLOCK_LEADING_EDGE #define SPI2_NSS_PIN PB12 #define SPI2_SCK_PIN PB13 #define SPI2_MISO_PIN PB14 @@ -193,17 +205,13 @@ #if defined(OMNIBUSF4PRO) || defined(OMNIBUSF4V3) #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT #define USE_SDCARD - #define USE_SDCARD_SPI2 + #define USE_SDCARD_SPI + + #define SDCARD_SPI_BUS BUS_SPI2 + #define SDCARD_CS_PIN SPI2_NSS_PIN + #define SDCARD_DETECT_PIN PB7 #define SDCARD_DETECT_INVERTED - #define SDCARD_DETECT_PIN PB7 - #define SDCARD_SPI_INSTANCE SPI2 - #define SDCARD_SPI_CS_PIN SPI2_NSS_PIN - - #define SDCARD_DMA_CHANNEL_TX DMA1_Stream4 - #define SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG DMA_FLAG_TCIF4 - #define SDCARD_DMA_CLK RCC_AHB1Periph_DMA1 - #define SDCARD_DMA_CHANNEL DMA_Channel_0 #else #define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT #define M25P16_CS_PIN SPI3_NSS_PIN @@ -230,15 +238,9 @@ #define USE_LED_STRIP #if (defined(OMNIBUSF4PRO) || defined(OMNIBUSF4V3)) && !defined(OMNIBUSF4PRO_LEDSTRIPM5) -# define WS2811_PIN PB6 -# define WS2811_DMA_HANDLER_IDENTIFER DMA1_ST0_HANDLER -# define WS2811_DMA_STREAM DMA1_Stream0 -# define WS2811_DMA_CHANNEL DMA_Channel_2 +# define WS2811_PIN PB6 #else -# define WS2811_PIN PA1 -# define WS2811_DMA_HANDLER_IDENTIFER DMA1_ST4_HANDLER -# define WS2811_DMA_STREAM DMA1_Stream4 -# define WS2811_DMA_CHANNEL DMA_Channel_6 +# define WS2811_PIN PA1 #endif #define DEFAULT_RX_TYPE RX_TYPE_PPM @@ -253,6 +255,7 @@ // Number of available PWM outputs #define MAX_PWM_OUTPUT_PORTS 6 #define TARGET_MOTOR_COUNT 6 +#define USE_DSHOT #define TARGET_IO_PORTA 0xffff #define TARGET_IO_PORTB 0xffff @@ -263,4 +266,4 @@ #define CURRENT_METER_SCALE 265 #endif -#define PCA9685_I2C_BUS BUS_I2C2 \ No newline at end of file +#define PCA9685_I2C_BUS I2C_EXT_BUS diff --git a/src/main/target/OMNIBUSF4/target.mk b/src/main/target/OMNIBUSF4/target.mk index e89dfcf8f38..1dbfc623fac 100644 --- a/src/main/target/OMNIBUSF4/target.mk +++ b/src/main/target/OMNIBUSF4/target.mk @@ -2,8 +2,8 @@ F405_TARGETS += $(TARGET) FEATURES += VCP ONBOARDFLASH TARGET_SRC = \ - drivers/accgyro/accgyro_mpu6000.c \ - drivers/accgyro/accgyro_mpu6500.c \ + drivers/accgyro/accgyro_mpu6000.c \ + drivers/accgyro/accgyro_mpu6500.c \ drivers/barometer/barometer_bmp280.c \ drivers/barometer/barometer_bmp085.c \ drivers/barometer/barometer_ms56xx.c \ @@ -13,8 +13,7 @@ TARGET_SRC = \ drivers/compass/compass_ist8308.c \ drivers/compass/compass_mag3110.c \ drivers/compass/compass_lis3mdl.c \ - drivers/pitotmeter_ms4525.c \ + drivers/compass/compass_ak8975.c \ drivers/pitotmeter_adc.c \ drivers/light_ws2811strip.c \ - drivers/light_ws2811strip_stdperiph.c \ drivers/max7456.c diff --git a/src/main/target/OMNIBUSF7/target.c b/src/main/target/OMNIBUSF7/target.c index 0d56fb85ece..3132b03ac68 100644 --- a/src/main/target/OMNIBUSF7/target.c +++ b/src/main/target/OMNIBUSF7/target.c @@ -35,13 +35,15 @@ BUSDEV_REGISTER_SPI( busdev_max7456, DEVHW_MAX7456, MAX7456_SPI_BUS, const timerHardware_t timerHardware[] = { - { TIM2, IO_TAG(PA3), TIM_CHANNEL_4, 0, IOCFG_AF_PP_PD, GPIO_AF1_TIM2, TIM_USE_PPM | TIM_USE_PWM }, //PPM Input on UART2 RX - { TIM3, IO_TAG(PB0), TIM_CHANNEL_3, 1, IOCFG_AF_PP_PD, GPIO_AF2_TIM3, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR }, //Motor 1 - { TIM3, IO_TAG(PB1), TIM_CHANNEL_4, 1, IOCFG_AF_PP_PD, GPIO_AF2_TIM3, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR }, //Motor 2 - { TIM1, IO_TAG(PE9), TIM_CHANNEL_1, 1, IOCFG_AF_PP_PD, GPIO_AF1_TIM1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, //Motor 3 - { TIM1, IO_TAG(PE11), TIM_CHANNEL_2, 1, IOCFG_AF_PP_PD, GPIO_AF1_TIM1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, //Motor 4 + DEF_TIM(TIM2, CH4, PA3, TIM_USE_PPM, 0, 0 ), // UART2_RX, joined with PE13 + // DEF_TIM(TIM1, CH3, PE13, TIM_USE_NONE, 0, 0 ), // RC1 / PPM, unusable - // DEF_TIM(TIM4, CH1, PD12, TIM_USE_LED, 1, 0 ), // LED + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0 ), // M1 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0 ), // M2 + DEF_TIM(TIM1, CH1, PE9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 2 ), // M3 + DEF_TIM(TIM1, CH2, PE11, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1 ), // M4 + + DEF_TIM(TIM4, CH1, PD12, TIM_USE_LED, 0, 0 ), // LED }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/OMNIBUSF7/target.h b/src/main/target/OMNIBUSF7/target.h index ecff774e878..9492dd2bd6b 100644 --- a/src/main/target/OMNIBUSF7/target.h +++ b/src/main/target/OMNIBUSF7/target.h @@ -136,6 +136,7 @@ #define SPI4_SCK_PIN PE2 #define SPI4_MISO_PIN PE5 #define SPI4_MOSI_PIN PE6 +#define SPI4_CLOCK_LEADING_EDGE #define USE_OSD @@ -144,20 +145,11 @@ #define MAX7456_CS_PIN SPI2_NSS_PIN #define USE_SDCARD +#define USE_SDCARD_SPI #define SDCARD_DETECT_INVERTED -#define SDCARD_DETECT_PIN PE3 -#define SDCARD_DETECT_EXTI_LINE EXTI_Line3 -#define SDCARD_DETECT_EXTI_PIN_SOURCE EXTI_PinSource3 -#define SDCARD_DETECT_EXTI_PORT_SOURCE EXTI_PortSourceGPIOE -#define SDCARD_DETECT_EXTI_IRQn EXTI3_IRQn - -#define SDCARD_SPI_INSTANCE SPI4 -#define SDCARD_SPI_CS_PIN SPI4_NSS_PIN - -#define SDCARD_DMA_CHANNEL_TX DMA2_Stream1 -#define SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG DMA_FLAG_TCIF1_5 -#define SDCARD_DMA_CLK RCC_AHB1Periph_DMA2 -#define SDCARD_DMA_CHANNEL DMA_CHANNEL_4 +#define SDCARD_DETECT_PIN PE3 +#define SDCARD_SPI_BUS BUS_SPI4 +#define SDCARD_CS_PIN SPI4_NSS_PIN #define USE_I2C #define USE_I2C_DEVICE_2 @@ -178,6 +170,8 @@ #define USE_MAG_MAG3110 #define USE_MAG_LIS3MDL +#define TEMPERATURE_I2C_BUS BUS_I2C2 + #define USE_RANGEFINDER #define USE_RANGEFINDER_HCSR04_I2C #define RANGEFINDER_I2C_BUS BUS_I2C2 @@ -197,13 +191,6 @@ //Following configuration needs to be reviewed, when LED is enabled, VCP stops to work //Until someone with deeper knowledge od DMA fixes it, LED are disabled in target #define WS2811_PIN PD12 -#define WS2811_TIMER TIM4 -#define WS2811_TIMER_CHANNEL TIM_CHANNEL_1 -#define WS2811_DMA_HANDLER_IDENTIFER DMA2_ST4_HANDLER -#define WS2811_DMA_STREAM DMA2_Stream4 -#define WS2811_DMA_IT DMA_IT_TCIF2 -#define WS2811_DMA_CHANNEL DMA_CHANNEL_6 -#define WS2811_TIMER_GPIO_AF GPIO_AF2_TIM4 #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT @@ -224,4 +211,4 @@ #define TARGET_IO_PORTD 0xffff #define TARGET_IO_PORTE 0xffff -#define PCA9685_I2C_BUS BUS_I2C2 \ No newline at end of file +#define PCA9685_I2C_BUS BUS_I2C2 diff --git a/src/main/target/OMNIBUSF7/target.mk b/src/main/target/OMNIBUSF7/target.mk index 2aedbd6fc54..1fc43f1f964 100644 --- a/src/main/target/OMNIBUSF7/target.mk +++ b/src/main/target/OMNIBUSF7/target.mk @@ -15,6 +15,4 @@ TARGET_SRC = \ drivers/compass/compass_mag3110.c \ drivers/compass/compass_lis3mdl.c \ drivers/light_ws2811strip.c \ - drivers/light_ws2811strip_hal.c \ - drivers/max7456.c - + drivers/max7456.c \ No newline at end of file diff --git a/src/main/target/OMNIBUSF7NXT/config.c b/src/main/target/OMNIBUSF7NXT/config.c index 786e65167ca..a42affa59aa 100644 --- a/src/main/target/OMNIBUSF7NXT/config.c +++ b/src/main/target/OMNIBUSF7NXT/config.c @@ -25,7 +25,6 @@ #include #include -#ifdef TARGET_CONFIG #include "drivers/io.h" #include "rx/rx.h" #include "io/serial.h" @@ -36,4 +35,3 @@ void targetConfiguration(void) ledStripConfigMutable()->ledConfigs[0] = DEFINE_LED(0, 0, COLOR_GREEN, 0, LED_FUNCTION_ARM_STATE, LED_FLAG_OVERLAY(LED_OVERLAY_WARNING), 0); // serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(TELEMETRY_UART)].functionMask = FUNCTION_TELEMETRY_SMARTPORT; } -#endif diff --git a/src/main/target/OMNIBUSF7NXT/target.c b/src/main/target/OMNIBUSF7NXT/target.c index e3f725b0b4a..e6439d4ac1e 100644 --- a/src/main/target/OMNIBUSF7NXT/target.c +++ b/src/main/target/OMNIBUSF7NXT/target.c @@ -30,45 +30,26 @@ #include "drivers/timer.h" #include "drivers/bus.h" -#define DEF_TIM_CHNL_CH1 TIM_CHANNEL_1 -#define DEF_TIM_CHNL_CH2 TIM_CHANNEL_2 -#define DEF_TIM_CHNL_CH3 TIM_CHANNEL_3 -#define DEF_TIM_CHNL_CH4 TIM_CHANNEL_4 - -#define GPIO_AF_PA9_TIM1 GPIO_AF1_TIM1 -#define GPIO_AF_PB0_TIM3 GPIO_AF2_TIM3 -#define GPIO_AF_PB1_TIM3 GPIO_AF2_TIM3 -#define GPIO_AF_PB4_TIM3 GPIO_AF2_TIM3 -#define GPIO_AF_PB5_TIM3 GPIO_AF2_TIM3 -#define GPIO_AF_PB6_TIM4 GPIO_AF2_TIM4 -#define GPIO_AF_PC8_TIM8 GPIO_AF3_TIM8 -#define GPIO_AF_PC9_TIM8 GPIO_AF3_TIM8 - -#define DEF_TIM(_tim, _ch, _pin, _usage, _flags) \ - { _tim, IO_TAG(_pin), DEF_TIM_CHNL_##_ch, _flags, IOCFG_AF_PP, GPIO_AF_##_pin##_##_tim, _usage } - // Board hardware definitions BUSDEV_REGISTER_SPI_TAG(busdev_mpu6000, DEVHW_MPU6000, MPU6000_SPI_BUS, MPU6000_CS_PIN, NONE, 0, DEVFLAGS_NONE); BUSDEV_REGISTER_SPI_TAG(busdev_mpu6500, DEVHW_MPU6500, MPU6500_SPI_BUS, MPU6500_CS_PIN, NONE, 1, DEVFLAGS_NONE); -//BUSDEV_REGISTER_SPI(busdev_lps25h, DEVHW_LPS25H, LPS25H_SPI_BUS, LPS25H_CS_PIN, NONE, DEVFLAGS_NONE); - const timerHardware_t timerHardware[] = { - // DEF_TIM(TIM10, CH1, PB8, TIM_USE_PPM, 0), // PPM + DEF_TIM(TIM4, CH2, PB7, TIM_USE_PPM, 0, 0), // PPM / UART1_RX // OUTPUT 1-4 - DEF_TIM(TIM3, CH2, PB5, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 1), - DEF_TIM(TIM3, CH1, PB4, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 1), - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 1), - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 1), + DEF_TIM(TIM3, CH2, PB5, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 1, 0), // D(1, 5, 5) + DEF_TIM(TIM3, CH1, PB4, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 1, 0), // D(1, 4, 5) + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 1, 0), // D(1, 7, 5) + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 1, 0), // D(1, 2, 5) // OUTPUT 5-6 - DEF_TIM(TIM8, CH3, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 1), - DEF_TIM(TIM8, CH4, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 1), + DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 1, 0), // D(2, 7, 7) + DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 1, 1), // D(2, 2, 0) // AUXILARY pins - DEF_TIM(TIM1, CH2, PA9, TIM_USE_LED, 1), // LED - DEF_TIM(TIM4, CH1, PB6, TIM_USE_ANY, 0) // SS1 TX + DEF_TIM(TIM1, CH2, PA9, TIM_USE_LED, 1, 0), // LED + DEF_TIM(TIM4, CH1, PB6, TIM_USE_ANY, 0, 0) // SS1 TX / UART1_TX }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/OMNIBUSF7NXT/target.h b/src/main/target/OMNIBUSF7NXT/target.h index bf0798adaa2..fab05054a83 100644 --- a/src/main/target/OMNIBUSF7NXT/target.h +++ b/src/main/target/OMNIBUSF7NXT/target.h @@ -29,7 +29,6 @@ #define USBD_PRODUCT_STRING "OMNIBUS NEXT" #define USE_TARGET_IMU_HARDWARE_DESCRIPTORS // Don't use common busdev descriptors for IMU -#define TARGET_CONFIG // Status LED #define LED0 PB2 @@ -65,18 +64,17 @@ #define USE_MAG_MAG3110 #define USE_MAG_LIS3MDL +#define TEMPERATURE_I2C_BUS BUS_I2C1 + #define USE_BARO #define USE_BARO_LPS25H #define LPS25H_SPI_BUS BUS_SPI2 #define LPS25H_CS_PIN PA10 -#define USE_PITOT_MS4525 #define PITOT_I2C_BUS BUS_I2C1 #define USE_RANGEFINDER #define RANGEFINDER_I2C_BUS BUS_I2C1 -#define USE_RANGEFINDER_HCSR04_I2C -#define USE_RANGEFINDER_VL53L0X #define USE_VCP #define VBUS_SENSING_PIN PC5 @@ -137,7 +135,7 @@ #define USE_OSD #define USE_MAX7456 -#define MAX7456_SPI_BUS BUS_SPI3 +#define MAX7456_SPI_BUS BUS_SPI2 #define MAX7456_CS_PIN PA15 #define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT @@ -161,9 +159,6 @@ #define USE_LED_STRIP #define WS2811_PIN PA9 -#define WS2811_DMA_HANDLER_IDENTIFER DMA2_ST2_HANDLER -#define WS2811_DMA_STREAM DMA2_Stream2 -#define WS2811_DMA_CHANNEL DMA_CHANNEL_6 #define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_BLACKBOX | FEATURE_VBAT | FEATURE_OSD | FEATURE_CURRENT_METER | FEATURE_SOFTSERIAL | FEATURE_TELEMETRY) #define DEFAULT_RX_TYPE RX_TYPE_SERIAL @@ -178,6 +173,8 @@ #define USE_SERIAL_4WAY_BLHELI_INTERFACE +#define USE_DSHOT + // Number of available PWM outputs #define MAX_PWM_OUTPUT_PORTS 6 #define TARGET_MOTOR_COUNT 6 diff --git a/src/main/target/OMNIBUSF7NXT/target.mk b/src/main/target/OMNIBUSF7NXT/target.mk index a8bd54ada9e..ff0b3579f3d 100644 --- a/src/main/target/OMNIBUSF7NXT/target.mk +++ b/src/main/target/OMNIBUSF7NXT/target.mk @@ -1,16 +1,14 @@ F7X2RE_TARGETS += $(TARGET) FEATURES += VCP ONBOARDFLASH TARGET_SRC = \ - drivers/accgyro/accgyro_mpu6000.c \ - drivers/accgyro/accgyro_mpu6500.c \ + drivers/accgyro/accgyro_mpu6000.c \ + drivers/accgyro/accgyro_mpu6500.c \ drivers/barometer/barometer_lps25h.c \ drivers/compass/compass_hmc5883l.c \ drivers/compass/compass_qmc5883l.c \ drivers/compass/compass_ist8310.c \ drivers/compass/compass_mag3110.c \ drivers/compass/compass_lis3mdl.c \ - drivers/pitotmeter_ms4525.c \ drivers/pitotmeter_adc.c \ drivers/light_ws2811strip.c \ - drivers/light_ws2811strip_hal.c \ drivers/max7456.c diff --git a/src/main/target/PIKOBLX/target.c b/src/main/target/PIKOBLX/target.c index 6989507d3e5..42133dfc25f 100644 --- a/src/main/target/PIKOBLX/target.c +++ b/src/main/target/PIKOBLX/target.c @@ -23,16 +23,15 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[] = { - { TIM3, IO_TAG(PA4), TIM_Channel_2, 1, IOCFG_AF_PP, GPIO_AF_2, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR }, // PWM1 - PA4 - *TIM3_CH2 - { TIM3, IO_TAG(PA6), TIM_Channel_1, 1, IOCFG_AF_PP, GPIO_AF_2, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR }, // PWM2 - PA6 - *TIM3_CH1, TIM8_BKIN, TIM1_BKIN, TIM16_CH1 - { TIM8, IO_TAG(PB0), TIM_Channel_2, 1 | TIMER_OUTPUT_N_CHANNEL, IOCFG_AF_PP, GPIO_AF_4, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // PWM3 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N - { TIM8, IO_TAG(PB1), TIM_Channel_3, 1 | TIMER_OUTPUT_N_CHANNEL, IOCFG_AF_PP, GPIO_AF_4, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // PWM4 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N - /*{ TIM3, IO_TAG(PB0), TIM_Channel_3, 1, IOCFG_AF_PP, GPIO_AF_2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // PWM3 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N*/ - /*{ TIM3, IO_TAG(PB1), TIM_Channel_4, 1, IOCFG_AF_PP, GPIO_AF_2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // PWM4 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N*/ - { TIM2, IO_TAG(PA1), TIM_Channel_2, 1, IOCFG_AF_PP, GPIO_AF_1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // PWM5 - PA1 - *TIM2_CH2, TIM15_CH1N - { TIM2, IO_TAG(PA2), TIM_Channel_3, 1, IOCFG_AF_PP, GPIO_AF_1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // PWM6 - PA2 - *TIM2_CH3, !TIM15_CH1 - { TIM17, IO_TAG(PA7), TIM_Channel_1, 0, IOCFG_AF_PP, GPIO_AF_1, TIM_USE_PPM }//, // PPM - PA7 - *TIM17_CH1, TIM1_CH1N, TIM8_CH1 - /*{ TIM16, IO_TAG(PB8), TIM_Channel_1, 0, IOCFG_AF_PP_PD, GPIO_AF_1, TIM_USE_LED }*/ + DEF_TIM(TIM3, CH2, PA4, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0), + DEF_TIM(TIM3, CH1, PA6, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0), + DEF_TIM(TIM8, CH2N, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), + DEF_TIM(TIM8, CH3N, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), + DEF_TIM(TIM2, CH2, PA1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), + DEF_TIM(TIM2, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), + + DEF_TIM(TIM17, CH1, PA7, TIM_USE_PPM, 0), + DEF_TIM(TIM16, CH1, PB8, TIM_USE_LED, 0), }; diff --git a/src/main/target/PIKOBLX/target.h b/src/main/target/PIKOBLX/target.h index 7a2560ba983..2d167eaeacf 100644 --- a/src/main/target/PIKOBLX/target.h +++ b/src/main/target/PIKOBLX/target.h @@ -77,27 +77,11 @@ #define VBAT_ADC_CHANNEL ADC_CHN_2 #define RSSI_ADC_CHANNEL ADC_CHN_3 -//#define USE_LED_STRIP -#define USE_LED_STRIP_ON_DMA1_CHANNEL3 +#define USE_LED_STRIP #define WS2811_PIN PB8 // TIM16_CH1 -#define WS2811_TIMER TIM16 -#define WS2811_DMA_STREAM DMA1_Channel3 -#define WS2811_IRQ DMA1_Channel3_IRQn -#define WS2811_DMA_TC_FLAG DMA1_FLAG_TC3 -#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH3_HANDLER - -#define TRANSPONDER -#define TRANSPONDER_GPIO GPIOA -#define TRANSPONDER_GPIO_AHB_PERIPHERAL RCC_AHBPeriph_GPIOA -#define TRANSPONDER_GPIO_AF GPIO_AF_6 -#define TRANSPONDER_PIN GPIO_Pin_8 -#define TRANSPONDER_PIN_SOURCE GPIO_PinSource8 -#define TRANSPONDER_TIMER TIM1 -#define TRANSPONDER_TIMER_APB2_PERIPHERAL RCC_APB2Periph_TIM1 -#define TRANSPONDER_DMA_CHANNEL DMA1_Channel2 -#define TRANSPONDER_IRQ DMA1_Channel2_IRQn -#define TRANSPONDER_DMA_TC_FLAG DMA1_FLAG_TC2 -#define TRANSPONDER_DMA_HANDLER_IDENTIFER DMA1_CH2_HANDLER + +// #define USE_TRANSPONDER +// #define TRANSPONDER_PIN PA8 #define USE_SPEKTRUM_BIND // USART3, PB11 diff --git a/src/main/target/PIKOBLX/target.mk b/src/main/target/PIKOBLX/target.mk index a3e3ce0c822..d53a4c76d1f 100644 --- a/src/main/target/PIKOBLX/target.mk +++ b/src/main/target/PIKOBLX/target.mk @@ -5,7 +5,6 @@ TARGET_SRC = \ drivers/accgyro/accgyro_mpu.c \ drivers/accgyro/accgyro_mpu6000.c \ drivers/light_ws2811strip.c \ - drivers/light_ws2811strip_stdperiph.c \ drivers/serial_softserial.c # seth neiman dec06.16 porting inav to PIKO BLX using betaflight target related files from before timer.h/timer_def.h changes diff --git a/src/main/target/PIXRACER/target.c b/src/main/target/PIXRACER/target.c index 3ccb3a4dd29..fcd15cde95d 100755 --- a/src/main/target/PIXRACER/target.c +++ b/src/main/target/PIXRACER/target.c @@ -36,13 +36,15 @@ BUSDEV_REGISTER_I2C_TAG(busdev_mag3110, DEVHW_MAG3110, MAG_I2C_BUS, BUSDEV_REGISTER_SPI_TAG(busdev_hmc5983_spi, DEVHW_HMC5883, MPU9250_SPI_BUS, PE15, NONE, 1, DEVFLAGS_NONE); const timerHardware_t timerHardware[] = { - { TIM3, IO_TAG(PB0), TIM_Channel_3, 0, IOCFG_AF_PP_PD, GPIO_AF_TIM3, TIM_USE_PPM }, // PPM shared uart6 pc7 - { TIM1, IO_TAG(PE14), TIM_Channel_4, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // S1_OUT - { TIM1, IO_TAG(PE13), TIM_Channel_3, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // S2_OUT - { TIM1, IO_TAG(PE11), TIM_Channel_2, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // S3_OUT - { TIM1, IO_TAG(PE9), TIM_Channel_1, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // S4_OUT - { TIM4, IO_TAG(PD13), TIM_Channel_2, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM4, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR }, // S5_OUT - { TIM4, IO_TAG(PD14), TIM_Channel_3, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM4, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR }, // S6_OUT + DEF_TIM(TIM3, CH3, PB0, TIM_USE_PPM, 0, 0 ), // PPM shared uart6 pc7 + + DEF_TIM(TIM1, CH4, PE14, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), // S1_OUT + DEF_TIM(TIM1, CH3, PE13, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), // S2_OUT + DEF_TIM(TIM1, CH2, PE11, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), // S3_OUT + DEF_TIM(TIM1, CH1, PE9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), // S4_OUT + + DEF_TIM(TIM4, CH2, PD13, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0 ), // S5_OUT + DEF_TIM(TIM4, CH3, PD14, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0 ), // S6_OUT }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/PIXRACER/target.h b/src/main/target/PIXRACER/target.h index 7e36a1794df..f2552c26efc 100755 --- a/src/main/target/PIXRACER/target.h +++ b/src/main/target/PIXRACER/target.h @@ -75,6 +75,8 @@ #define USE_MAG_MAG3110 #define USE_MAG_LIS3MDL +#define TEMPERATURE_I2C_BUS BUS_I2C1 + #define USE_BARO #define USE_BARO_MS5611 #define MS5611_CS_PIN PD7 diff --git a/src/main/target/RADIX/target.c b/src/main/target/RADIX/target.c index 7433461eee1..88d6f5aea40 100644 --- a/src/main/target/RADIX/target.c +++ b/src/main/target/RADIX/target.c @@ -25,13 +25,14 @@ /* TIMERS */ const timerHardware_t timerHardware[] = { - { TIM12, IO_TAG(PB14), TIM_Channel_1, 0, IOCFG_AF_PP_PD, GPIO_AF_TIM12, TIM_USE_PPM }, - { TIM5, IO_TAG(PA2), TIM_Channel_3, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM5, TIM_USE_MC_MOTOR }, // S3_OUT - { TIM5, IO_TAG(PA3), TIM_Channel_4, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM5, TIM_USE_MC_MOTOR }, // S4_OUT - { TIM1, IO_TAG(PA10), TIM_Channel_3, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM1, TIM_USE_MC_MOTOR }, // S5_OUT - { TIM2, IO_TAG(PA15), TIM_Channel_1, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM2, TIM_USE_MC_MOTOR }, // S6_OUT - { TIM8, IO_TAG(PC8), TIM_Channel_3, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM8, TIM_USE_MC_MOTOR }, // S5_OUT - { TIM3, IO_TAG(PB0), TIM_Channel_3, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM3, TIM_USE_MC_MOTOR }, // S6_OUT + DEF_TIM(TIM12, CH1, PB14, TIM_USE_PPM, 0, 0), // PPM In + DEF_TIM(TIM5, CH3, PA2, TIM_USE_MC_MOTOR, 0, 0), // S1 + DEF_TIM(TIM5, CH4, PA3, TIM_USE_MC_MOTOR, 0, 0), // S2 + DEF_TIM(TIM1, CH3, PA10, TIM_USE_MC_MOTOR, 0, 0), // S3 + DEF_TIM(TIM2, CH1, PA15, TIM_USE_MC_MOTOR, 0, 0), // S4 + DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR, 0, 0), // S5 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR, 0, 0), // S6 + DEF_TIM(TIM11, CH1, PB9, TIM_USE_ANY, 0, 0), // Camera Control }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/RADIX/target.h b/src/main/target/RADIX/target.h index dfb7e3a73d0..15f9cfe5228 100644 --- a/src/main/target/RADIX/target.h +++ b/src/main/target/RADIX/target.h @@ -60,9 +60,7 @@ // #define USE_OPFLOW_CXOF // #define USE_RANGEFINDER -// #define USE_RANGEFINDER_UIB -// #define USE_RANGEFINDER_VL53L0X -// #define VL53L0X_I2C_BUS BUS_I2C2 +// #define RANGEFINDER_I2C_BUS BUS_I2C2 #define USE_VCP #define VBUS_SENSING_PIN PA9 @@ -111,18 +109,14 @@ #define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 #define RSSI_ADC_CHANNEL ADC_CHN_3 -#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT -#define USE_SDCARD +// #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT +// #define USE_SDCARD +// #define USE_SDCARD_SPI -#define SDCARD_DETECT_INVERTED -#define SDCARD_SPI_INSTANCE SPI2 -#define SDCARD_DETECT_PIN PB13 -#define SDCARD_SPI_CS_PIN PB15 - -#define SDCARD_DMA_CHANNEL_TX DMA2_Stream3 -#define SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG DMA_FLAG_TCIF3 -#define SDCARD_DMA_CLK RCC_AHB1Periph_DMA2 -#define SDCARD_DMA_CHANNEL DMA_Channel_3 +// #define SDCARD_DETECT_INVERTED +// #define SDCARD_DETECT_PIN PB13 +// #define SDCARD_SPI_BUS BUS_SPI2 +// #define SDCARD_CS_PIN PB15 #define SENSORS_SET (SENSOR_ACC|SENSOR_BARO) diff --git a/src/main/target/RCEXPLORERF3/target.c b/src/main/target/RCEXPLORERF3/target.c index aa997b65fd9..1a0e3596bcc 100644 --- a/src/main/target/RCEXPLORERF3/target.c +++ b/src/main/target/RCEXPLORERF3/target.c @@ -24,13 +24,13 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[] = { - { TIM1, IO_TAG(PA8), TIM_Channel_1, 1, IOCFG_AF_PP, GPIO_AF_6, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // PWM3 - PA8 - { TIM17, IO_TAG(PA7), TIM_Channel_1, 1, IOCFG_AF_PP, GPIO_AF_1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR | TIM_USE_MC_SERVO }, // PWM2 - PA7 - { TIM3, IO_TAG(PB0), TIM_Channel_3, 1, IOCFG_AF_PP, GPIO_AF_2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // PWM4 - PB0 - { TIM3, IO_TAG(PA4), TIM_Channel_2, 1, IOCFG_AF_PP, GPIO_AF_2, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR }, // PWM1 - PA4 - { TIM3, IO_TAG(PB1), TIM_Channel_4, 1, IOCFG_AF_PP, GPIO_AF_2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // PWM5 - PB1 - { TIM2, IO_TAG(PA1), TIM_Channel_2, 0, IOCFG_AF_PP, GPIO_AF_1, TIM_USE_PPM }, // PWM6 - PPM - { TIM16, IO_TAG(PB8), TIM_Channel_1, 0, IOCFG_AF_PP, GPIO_AF_1, TIM_USE_ANY } + DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // PWM3 - PA8 + DEF_TIM(TIM17, CH1, PA7, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR | TIM_USE_MC_SERVO, 0), // PWM2 - PA7 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // PWM4 - PB0 + DEF_TIM(TIM3, CH2, PA4, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0), // PWM1 - PA4 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // PWM5 - PB1 + DEF_TIM(TIM2, CH2, PA1, TIM_USE_PPM, 0), // PWM6 - PPM + DEF_TIM(TIM16, CH1, PB8, TIM_USE_LED, 0), }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/RCEXPLORERF3/target.h b/src/main/target/RCEXPLORERF3/target.h index 76a9233c8e9..53cc0371dd0 100644 --- a/src/main/target/RCEXPLORERF3/target.h +++ b/src/main/target/RCEXPLORERF3/target.h @@ -92,10 +92,7 @@ #define RSSI_ADC_CHANNEL ADC_CHN_3 #define USE_LED_STRIP // LED strip configuration using PWM motor output pin 5. -#define USE_LED_STRIP_ON_DMA1_CHANNEL3 #define WS2811_PIN PB8 // TIM16_CH1 -#define WS2811_DMA_STREAM DMA1_Channel3 -#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH3_HANDLER #define USE_RANGEFINDER #define USE_RANGEFINDER_HCSR04 diff --git a/src/main/target/RCEXPLORERF3/target.mk b/src/main/target/RCEXPLORERF3/target.mk index ddcf6783a3a..e7a04881a54 100644 --- a/src/main/target/RCEXPLORERF3/target.mk +++ b/src/main/target/RCEXPLORERF3/target.mk @@ -14,5 +14,4 @@ TARGET_SRC = \ drivers/display_ug2864hsweg01.c \ drivers/serial_usb_vcp.c \ drivers/flash_m25p16.c \ - drivers/light_ws2811strip.c \ - drivers/light_ws2811strip_stdperiph.c + drivers/light_ws2811strip.c diff --git a/src/main/target/REVO/target.c b/src/main/target/REVO/target.c index aa427710c89..3f7431af59a 100644 --- a/src/main/target/REVO/target.c +++ b/src/main/target/REVO/target.c @@ -20,37 +20,33 @@ #include #include "drivers/io.h" #include "drivers/bus.h" -#include "drivers/pwm_mapping.h" #include "drivers/timer.h" +#include "drivers/timer_def.h" +#include "drivers/pwm_mapping.h" /* GYRO */ -BUSDEV_REGISTER_SPI_TAG(busdev_mpu6000, DEVHW_MPU6000, MPU6000_SPI_BUS, MPU6000_CS_PIN, MPU6000_EXTI_PIN, 0, DEVFLAGS_NONE); - -BUSDEV_REGISTER_I2C( busdev_ms5611, DEVHW_MS5611, BARO_I2C_BUS, 0x77, NONE, DEVFLAGS_USE_RAW_REGISTERS); - BUSDEV_REGISTER_I2C_TAG(busdev_hmc5883_int, DEVHW_HMC5883, MAG_I2C_BUS_INT, 0x1E, NONE, 0, DEVFLAGS_NONE); BUSDEV_REGISTER_I2C_TAG(busdev_hmc5883, DEVHW_HMC5883, MAG_I2C_BUS_EXT, 0x1E, NONE, 1, DEVFLAGS_NONE); BUSDEV_REGISTER_I2C_TAG(busdev_qmc5883, DEVHW_QMC5883, MAG_I2C_BUS_EXT, 0x0D, NONE, 1, DEVFLAGS_NONE); BUSDEV_REGISTER_I2C_TAG(busdev_mag3110, DEVHW_MAG3110, MAG_I2C_BUS_EXT, 0x0E, NONE, 1, DEVFLAGS_NONE); -BUSDEV_REGISTER_SPI (busdev_m25p16, DEVHW_M25P16, M25P16_SPI_BUS, M25P16_CS_PIN, NONE, DEVFLAGS_NONE); /* TIMERS */ const timerHardware_t timerHardware[] = { - { TIM3, IO_TAG(PB0), TIM_Channel_3, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM3, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR }, // S1_OUT - { TIM3, IO_TAG(PB1), TIM_Channel_4, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM3, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR }, // S2_OUT - { TIM9, IO_TAG(PA3), TIM_Channel_2, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // S3_OUT - { TIM2, IO_TAG(PA2), TIM_Channel_3, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // S4_OUT - { TIM5, IO_TAG(PA1), TIM_Channel_2, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM5, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO | TIM_USE_ANY }, // S5_OUT - GPIO_PartialRemap_TIM3 - { TIM5, IO_TAG(PA0), TIM_Channel_1, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM5, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO }, // S6_OUT - - { TIM12, IO_TAG(PB14), TIM_Channel_1, 0, IOCFG_AF_PP_PD, GPIO_AF_TIM12, TIM_USE_PWM | TIM_USE_PPM }, // PWM1 - PPM (5th pin on FlexiIO port) - { TIM12, IO_TAG(PB15), TIM_Channel_2, 0, IOCFG_AF_PP_PD, GPIO_AF_TIM12, TIM_USE_PWM }, // PWM2 - S2_IN - GPIO_PartialRemap_TIM3 - { TIM8, IO_TAG(PC6), TIM_Channel_1, 0, IOCFG_AF_PP_PD, GPIO_AF_TIM8, TIM_USE_PWM | TIM_USE_FW_SERVO }, // PWM3 - S3_IN - { TIM8, IO_TAG(PC7), TIM_Channel_2, 0, IOCFG_AF_PP_PD, GPIO_AF_TIM8, TIM_USE_PWM | TIM_USE_FW_SERVO }, // PWM4 - S4_IN - { TIM8, IO_TAG(PC8), TIM_Channel_3, 0, IOCFG_AF_PP_PD, GPIO_AF_TIM8, TIM_USE_PWM | TIM_USE_FW_SERVO }, // PWM5 - S5_IN - { TIM8, IO_TAG(PC9), TIM_Channel_4, 0, IOCFG_AF_PP_PD, GPIO_AF_TIM8, TIM_USE_PWM | TIM_USE_FW_SERVO }, // PWM6 - S6_IN + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1_OUT D1_ST7 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S2_OUT D1_ST2 + DEF_TIM(TIM2, CH4, PA3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), // S3_OUT D1_ST6 + DEF_TIM(TIM2, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4_OUT D1_ST1 + DEF_TIM(TIM5, CH2, PA1, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO | TIM_USE_ANY, 0, 0), // S5_OUT / LED + DEF_TIM(TIM5, CH1, PA0, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S6_OUT D1_ST2 + + DEF_TIM(TIM12, CH1, PB14, TIM_USE_PWM | TIM_USE_PPM, 0, 0), // PPM (5th pin on FlexiIO port) + DEF_TIM(TIM12, CH2, PB15, TIM_USE_PWM, 0, 0), // S2_IN + DEF_TIM(TIM8, CH1, PC6, TIM_USE_PWM | TIM_USE_FW_SERVO, 0, 0), // S3_IN + DEF_TIM(TIM8, CH2, PC7, TIM_USE_PWM | TIM_USE_FW_SERVO, 0, 0), // S4_IN + DEF_TIM(TIM8, CH3, PC8, TIM_USE_PWM | TIM_USE_FW_SERVO, 0, 0), // S5_IN + DEF_TIM(TIM8, CH4, PC9, TIM_USE_PWM | TIM_USE_FW_SERVO, 0, 0), // S6_IN }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/REVO/target.h b/src/main/target/REVO/target.h index d0400ac33c1..c1fb22d5d29 100644 --- a/src/main/target/REVO/target.h +++ b/src/main/target/REVO/target.h @@ -24,14 +24,16 @@ #define USBD_SERIALNUMBER_STRING "0x8020000" #endif -// Use target-specific hardware descriptors (don't use common_hardware.h) -#define USE_TARGET_HARDWARE_DESCRIPTORS +// Use target-specific MAG hardware descriptors (don't use common_hardware.h) +#define USE_TARGET_MAG_HARDWARE_DESCRIPTORS #define LED0 PB5 #define LED1 PB4 #define BEEPER PB4 +#define USE_DSHOT + // MPU6000 interrupts #define USE_EXTI #define MPU6000_EXTI_PIN PC4 @@ -60,22 +62,18 @@ #define USE_MAG_MAG3110 #define USE_MAG_LIS3MDL +#define TEMPERATURE_I2C_BUS BUS_I2C2 + #define USE_BARO #define BARO_I2C_BUS BUS_I2C1 #define USE_BARO_MS5611 -//#define USE_PITOT_MS4525 -//#define PITOT_I2C_BUS BUS_I2C2 +#define PITOT_I2C_BUS BUS_I2C2 #define USE_OPTICAL_FLOW -#define USE_OPFLOW_CXOF -#define USE_OPFLOW_MSP #define USE_RANGEFINDER -#define USE_RANGEFINDER_MSP -#define USE_RANGEFINDER_VL53L0X -#define VL53L0X_I2C_BUS BUS_I2C2 - +#define RANGEFINDER_I2C_BUS BUS_I2C2 #define M25P16_CS_PIN PB3 #define M25P16_SPI_BUS BUS_SPI3 @@ -105,6 +103,10 @@ #define UART6_RX_PIN PC7 #define UART6_TX_PIN PC6 +// #define USE_SOFTSERIAL1 +// #define SOFTSERIAL_1_RX_PIN PC8 +// #define SOFTSERIAL_1_TX_PIN PC9 + #define SERIAL_PORT_COUNT 4 //VCP, USART1, USART3, USART6 #define USE_SPI @@ -133,11 +135,7 @@ #define SENSORS_SET (SENSOR_ACC|SENSOR_MAG|SENSOR_BARO) #define USE_LED_STRIP -// LED Strip can run off Pin 5 (PA1) of the MOTOR outputs. #define WS2811_PIN PA1 -#define WS2811_DMA_HANDLER_IDENTIFER DMA1_ST4_HANDLER -#define WS2811_DMA_STREAM DMA1_Stream4 -#define WS2811_DMA_CHANNEL DMA_Channel_6 #define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT @@ -159,4 +157,4 @@ #define TARGET_IO_PORTC 0xffff #define TARGET_IO_PORTD 0xffff -#define PCA9685_I2C_BUS BUS_I2C2 \ No newline at end of file +#define PCA9685_I2C_BUS BUS_I2C2 diff --git a/src/main/target/REVO/target.mk b/src/main/target/REVO/target.mk index ca4f1d64e65..b30794eff9a 100644 --- a/src/main/target/REVO/target.mk +++ b/src/main/target/REVO/target.mk @@ -13,6 +13,5 @@ TARGET_SRC = \ drivers/compass/compass_mag3110.c \ drivers/compass/compass_lis3mdl.c \ drivers/pitotmeter_adc.c \ - drivers/light_ws2811strip.c \ - drivers/light_ws2811strip_stdperiph.c - + drivers/pitotmeter_ms4525.c \ + drivers/light_ws2811strip.c diff --git a/src/main/target/RMDO/target.c b/src/main/target/RMDO/target.c index ca8c94c1f34..59769ba5055 100644 --- a/src/main/target/RMDO/target.c +++ b/src/main/target/RMDO/target.c @@ -23,25 +23,25 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[] = { - { TIM16, IO_TAG(PA6), TIM_Channel_1, 1, IOCFG_AF_PP, GPIO_AF_1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR }, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1 - { TIM17, IO_TAG(PA7), TIM_Channel_1, 1, IOCFG_AF_PP, GPIO_AF_1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR }, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 - { TIM4, IO_TAG(PA11), TIM_Channel_1, 1, IOCFG_AF_PP, GPIO_AF_10, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // PWM3 - PA11 - { TIM4, IO_TAG(PA12), TIM_Channel_2, 1, IOCFG_AF_PP, GPIO_AF_10, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // PWM4 - PA12 - { TIM4, IO_TAG(PB8), TIM_Channel_3, 1, IOCFG_AF_PP, GPIO_AF_2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // PWM5 - PB8 - { TIM4, IO_TAG(PB9), TIM_Channel_4, 1, IOCFG_AF_PP, GPIO_AF_2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // PWM6 - PB9 - { TIM15, IO_TAG(PA2), TIM_Channel_1, 1, IOCFG_AF_PP, GPIO_AF_9, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO }, // PWM7 - PA2 - { TIM15, IO_TAG(PA3), TIM_Channel_2, 1, IOCFG_AF_PP, GPIO_AF_9, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO }, // PWM8 - PA3 + DEF_TIM(TIM16, CH1, PA6, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0), // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1 + DEF_TIM(TIM17, CH1, PA7, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0), // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 + DEF_TIM(TIM4, CH1, PA11, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // PWM3 - PA11 + DEF_TIM(TIM4, CH2, PA12, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // PWM4 - PA12 + DEF_TIM(TIM4, CH3, PB8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // PWM5 - PB8 + DEF_TIM(TIM4, CH4, PB9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // PWM6 - PB9 + DEF_TIM(TIM15, CH1, PA2, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0), // PWM7 - PA2 + DEF_TIM(TIM15, CH2, PA3, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0), // PWM8 - PA3 - { TIM1, IO_TAG(PA8), TIM_Channel_1, 1, IOCFG_AF_PP, GPIO_AF_6, TIM_USE_LED }, // GPIO_TIMER / LED_STRIP + DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0), // GPIO_TIMER / LED_STRIP - { TIM2, IO_TAG(PA0), TIM_Channel_1, 0, IOCFG_AF_PP, GPIO_AF_1, TIM_USE_PWM | TIM_USE_PPM }, // RC_CH1 - PA0 - *TIM2_CH1 - { TIM2, IO_TAG(PA1), TIM_Channel_2, 0, IOCFG_AF_PP, GPIO_AF_1, TIM_USE_PWM }, // RC_CH2 - PA1 - *TIM2_CH2, TIM15_CH1N - { TIM2, IO_TAG(PB11), TIM_Channel_4, 0, IOCFG_AF_PP, GPIO_AF_1, TIM_USE_PWM }, // RC_CH3 - PB11 - *TIM2_CH4, USART3_RX (AF7) - { TIM2, IO_TAG(PB10), TIM_Channel_3, 0, IOCFG_AF_PP, GPIO_AF_1, TIM_USE_PWM }, // RC_CH4 - PB10 - *TIM2_CH3, USART3_TX (AF7) - { TIM3, IO_TAG(PB4), TIM_Channel_1, 0, IOCFG_AF_PP, GPIO_AF_2, TIM_USE_PWM | TIM_USE_MC_SERVO }, // RC_CH5 - PB4 - *TIM3_CH1 - { TIM3, IO_TAG(PB5), TIM_Channel_2, 0, IOCFG_AF_PP, GPIO_AF_2, TIM_USE_PWM | TIM_USE_MC_SERVO }, // RC_CH6 - PB5 - *TIM3_CH2 - { TIM3, IO_TAG(PB0), TIM_Channel_3, 0, IOCFG_AF_PP, GPIO_AF_2, TIM_USE_PWM | TIM_USE_MC_SERVO }, // RC_CH7 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N - { TIM3, IO_TAG(PB1), TIM_Channel_4, 0, IOCFG_AF_PP, GPIO_AF_2, TIM_USE_PWM | TIM_USE_MC_SERVO }, // RC_CH8 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N + DEF_TIM(TIM2, CH1, PA0, TIM_USE_PWM | TIM_USE_PPM, 0), // RC_CH1 - PA0 - *TIM2_CH1 + DEF_TIM(TIM2, CH2, PA1, TIM_USE_PWM, 0), // RC_CH2 - PA1 - *TIM2_CH2, TIM15_CH1N + DEF_TIM(TIM2, CH4, PB11, TIM_USE_PWM, 0), // RC_CH3 - PB11 - *TIM2_CH4, UART3_RX (AF7) + DEF_TIM(TIM2, CH3, PB10, TIM_USE_PWM, 0), // RC_CH4 - PB10 - *TIM2_CH3, UART3_TX (AF7) + DEF_TIM(TIM3, CH1, PB4, TIM_USE_PWM, 0), // RC_CH5 - PB4 - *TIM3_CH1 + DEF_TIM(TIM3, CH2, PB5, TIM_USE_PWM, 0), // RC_CH6 - PB5 - *TIM3_CH2 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_PWM, 0), // RC_CH7 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N + DEF_TIM(TIM3, CH4, PB1, TIM_USE_PWM, 0), // RC_CH8 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N }; diff --git a/src/main/target/RMDO/target.h b/src/main/target/RMDO/target.h index a589850ec6a..1e78f6df93b 100644 --- a/src/main/target/RMDO/target.h +++ b/src/main/target/RMDO/target.h @@ -105,8 +105,6 @@ #define USE_LED_STRIP_ON_DMA1_CHANNEL2 #define WS2811_PIN PA8 -#define WS2811_DMA_STREAM DMA1_Channel2 -#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH2_HANDLER #undef NAV_MAX_WAYPOINTS #define NAV_MAX_WAYPOINTS 30 diff --git a/src/main/target/RMDO/target.mk b/src/main/target/RMDO/target.mk index 35e6269bac5..ced7c059419 100644 --- a/src/main/target/RMDO/target.mk +++ b/src/main/target/RMDO/target.mk @@ -17,6 +17,5 @@ TARGET_SRC = \ drivers/compass/compass_lis3mdl.c \ drivers/flash_m25p16.c \ drivers/light_ws2811strip.c \ - drivers/light_ws2811strip_stdperiph.c \ drivers/serial_softserial.c diff --git a/src/main/target/SPARKY/target.c b/src/main/target/SPARKY/target.c index ead259c2445..9a7829f2533 100644 --- a/src/main/target/SPARKY/target.c +++ b/src/main/target/SPARKY/target.c @@ -24,21 +24,21 @@ const timerHardware_t timerHardware[] = { // 6 3-pin headers - { TIM15, IO_TAG(PB15), TIM_Channel_2, 1, IOCFG_AF_PP, GPIO_AF_1, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR }, // PWM1 - PB15 - TIM1_CH3N, TIM15_CH1N, *TIM15_CH2 - { TIM15, IO_TAG(PB14), TIM_Channel_1, 1, IOCFG_AF_PP, GPIO_AF_1, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR }, // PWM2 - PB14 - TIM1_CH2N, *TIM15_CH1 - { TIM1, IO_TAG(PA8), TIM_Channel_1, 1, IOCFG_AF_PP, GPIO_AF_6, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // PWM3 - PA8 - *TIM1_CH1, TIM4_ETR - { TIM3, IO_TAG(PB0), TIM_Channel_3, 0, IOCFG_AF_PP, GPIO_AF_2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // PWM4 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N - { TIM16, IO_TAG(PA6), TIM_Channel_1, 0, IOCFG_AF_PP, GPIO_AF_1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO | TIM_USE_LED }, // PWM5 - PA6 - *TIM3_CH1, TIM8_BKIN, TIM1_BKIN, TIM16_CH1 - { TIM2, IO_TAG(PA2), TIM_Channel_3, 0, IOCFG_AF_PP, GPIO_AF_1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // PWM6 - PA2 - *TIM2_CH3, !TIM15_CH1 + DEF_TIM(TIM15, CH2, PB15, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR, 0), // PWM1 - PB15 - TIM1_CH3N, TIM15_CH1N, *TIM15_CH2 + DEF_TIM(TIM15, CH1, PB14, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR, 0), // PWM2 - PB14 - TIM1_CH2N, *TIM15_CH1 + DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // PWM3 - PA8 - *TIM1_CH1, TIM4_ETR + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // PWM4 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N + DEF_TIM(TIM16, CH1, PA6, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO | TIM_USE_LED, 0), // PWM5 - PA6 - *TIM3_CH1, TIM8_BKIN, TIM1_BKIN, TIM16_CH1 + DEF_TIM(TIM2, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // PWM6 - PA2 - *TIM2_CH3, !TIM15_CH1 // PWM7 - PMW10 - { TIM3, IO_TAG(PB1), TIM_Channel_4, 0, IOCFG_AF_PP, GPIO_AF_2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // PWM7 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N - { TIM17, IO_TAG(PA7), TIM_Channel_1, 1, IOCFG_AF_PP, GPIO_AF_1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // PWM8 - PA7 - !TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 - { TIM3, IO_TAG(PA4), TIM_Channel_2, 0, IOCFG_AF_PP, GPIO_AF_2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // PWM9 - PA4 - *TIM3_CH2 - { TIM2, IO_TAG(PA1), TIM_Channel_2, 0, IOCFG_AF_PP, GPIO_AF_1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // PWM10 - PA1 - *TIM2_CH2, TIM15_CH1N + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // PWM7 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N + DEF_TIM(TIM17, CH1, PA7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // PWM8 - PA7 - !TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 + DEF_TIM(TIM3, CH2, PA4, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // PWM9 - PA4 - *TIM3_CH2 + DEF_TIM(TIM2, CH2, PA1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // PWM10 - PA1 - *TIM2_CH2, TIM15_CH1N // PPM PORT - Also USART2 RX (AF5) - { TIM2, IO_TAG(PA3), TIM_Channel_4, 0, IOCFG_AF_PP_PD, GPIO_AF_1, TIM_USE_PPM } // PPM - PA3 - TIM2_CH4, TIM15_CH2 - PWM13 + DEF_TIM(TIM2, CH4, PA3, TIM_USE_PPM, 0), // PPM - PA3 - TIM2_CH4, TIM15_CH2 - PWM13 }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/SPARKY/target.h b/src/main/target/SPARKY/target.h index ede88c4095d..6b028cb0d6f 100644 --- a/src/main/target/SPARKY/target.h +++ b/src/main/target/SPARKY/target.h @@ -91,8 +91,6 @@ #define USE_LED_STRIP #define USE_LED_STRIP_ON_DMA1_CHANNEL3 #define WS2811_PIN PA6 // TIM16_CH1 -#define WS2811_DMA_STREAM DMA1_Channel3 -#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH3_HANDLER #define USE_SERIAL_4WAY_BLHELI_INTERFACE diff --git a/src/main/target/SPARKY/target.mk b/src/main/target/SPARKY/target.mk index c8615f89d75..eb23cc1bbd7 100644 --- a/src/main/target/SPARKY/target.mk +++ b/src/main/target/SPARKY/target.mk @@ -14,6 +14,4 @@ TARGET_SRC = \ drivers/compass/compass_ist8308.c \ drivers/compass/compass_mag3110.c \ drivers/compass/compass_lis3mdl.c \ - drivers/light_ws2811strip.c \ - drivers/light_ws2811strip_stdperiph.c - + drivers/light_ws2811strip.c diff --git a/src/main/target/SPARKY2/target.c b/src/main/target/SPARKY2/target.c index d78e9c37fa7..1de5d4c598b 100644 --- a/src/main/target/SPARKY2/target.c +++ b/src/main/target/SPARKY2/target.c @@ -24,18 +24,18 @@ #include "drivers/bus.h" const timerHardware_t timerHardware[] = { - { TIM8, IO_TAG(PC7), TIM_Channel_2, 0, IOCFG_AF_PP, GPIO_AF_TIM8, TIM_USE_PPM | TIM_USE_PWM}, // PPM IN - { TIM12, IO_TAG(PB14), TIM_Channel_1, 0, IOCFG_AF_PP, GPIO_AF_TIM12, TIM_USE_PWM}, // S2_IN - { TIM12, IO_TAG(PB15), TIM_Channel_2, 0, IOCFG_AF_PP, GPIO_AF_TIM12, TIM_USE_PWM}, // S3_IN - GPIO_PartialRemap_TIM3 - { TIM8, IO_TAG(PC8), TIM_Channel_3, 0, IOCFG_AF_PP, GPIO_AF_TIM8, TIM_USE_PWM}, // S4_IN - { TIM8, IO_TAG(PC9), TIM_Channel_4, 0, IOCFG_AF_PP, GPIO_AF_TIM8, TIM_USE_PWM}, // S5_IN + DEF_TIM(TIM8, CH2, PC7, TIM_USE_PWM | TIM_USE_PPM, 0, 0 ), // PPM IN + DEF_TIM(TIM12, CH1, PB14, TIM_USE_PWM, 0, 0 ), // S2_IN + DEF_TIM(TIM12, CH2, PB15, TIM_USE_PWM, 0, 0 ), // S3_IN - GPIO_PartialRemap_TIM3 + DEF_TIM(TIM8, CH3, PC8, TIM_USE_PWM, 0, 0 ), // S4_IN + DEF_TIM(TIM8, CH4, PC9, TIM_USE_PWM, 0, 0 ), // S5_IN - { TIM3, IO_TAG(PB0), TIM_Channel_3, 1, IOCFG_AF_PP, GPIO_AF_TIM3, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR}, // S1_OUT - { TIM3, IO_TAG(PB1), TIM_Channel_4, 1, IOCFG_AF_PP, GPIO_AF_TIM3, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR}, // S2_OUT - { TIM9, IO_TAG(PA3), TIM_Channel_2, 1, IOCFG_AF_PP, GPIO_AF_TIM9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO}, // S3_OUT - { TIM2, IO_TAG(PA2), TIM_Channel_3, 1, IOCFG_AF_PP, GPIO_AF_TIM2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO}, // S4_OUT - { TIM5, IO_TAG(PA1), TIM_Channel_2, 1, IOCFG_AF_PP, GPIO_AF_TIM5, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO}, // S5_OUT - GPIO_PartialRemap_TIM3 - { TIM5, IO_TAG(PA0), TIM_Channel_1, 1, IOCFG_AF_PP, GPIO_AF_TIM5, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO}, // S6_OUT + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0 ), // S1_OUT + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0 ), // S2_OUT + DEF_TIM(TIM9, CH2, PA3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), // S3_OUT + DEF_TIM(TIM2, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), // S4_OUT + DEF_TIM(TIM5, CH2, PA1, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0 ), // S5_OUT - GPIO_PartialRemap_TIM3 + DEF_TIM(TIM5, CH1, PA0, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0 ), // S6_OUT }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/SPARKY2/target.h b/src/main/target/SPARKY2/target.h index 081e9563d34..8ed846eb7f4 100644 --- a/src/main/target/SPARKY2/target.h +++ b/src/main/target/SPARKY2/target.h @@ -57,6 +57,8 @@ #define USE_MAG_MAG3110 #define USE_MAG_LIS3MDL +#define TEMPERATURE_I2C_BUS BUS_I2C1 + #define USE_BARO #define BARO_I2C_BUS BUS_I2C1 #define USE_BARO_MS5611 @@ -114,16 +116,15 @@ #define USE_ADC #define ADC_CHANNEL_1_PIN PC2 -#define ADC_CHANNEL_2_PIN PC1 +#define ADC_CHANNEL_2_PIN PC3 #define VBAT_ADC_CHANNEL ADC_CHN_2 #define CURRENT_METER_ADC_CHANNEL ADC_CHN_1 -#define USE_LED_STRIP -#define LED_STRIP_TIMER TIM5 +// #define USE_LED_STRIP +// #define WS2811_PIN PA1 #define USE_RANGEFINDER -#define USE_RANGEFINDER_VL53L0X -#define VL53L0X_I2C_BUS BUS_I2C2 +#define RANGEFINDER_I2C_BUS BUS_I2C2 #define USE_OPTICAL_FLOW #define USE_OPFLOW_CXOF @@ -141,4 +142,4 @@ #define TARGET_IO_PORTB 0xffff #define TARGET_IO_PORTC 0xffff -#define PCA9685_I2C_BUS BUS_I2C2 \ No newline at end of file +#define PCA9685_I2C_BUS BUS_I2C2 diff --git a/src/main/target/SPARKY2/target.mk b/src/main/target/SPARKY2/target.mk index dfdc5f99e8d..1487b00b103 100644 --- a/src/main/target/SPARKY2/target.mk +++ b/src/main/target/SPARKY2/target.mk @@ -14,6 +14,4 @@ TARGET_SRC = \ drivers/compass/compass_ist8308.c \ drivers/compass/compass_mag3110.c \ drivers/compass/compass_lis3mdl.c \ - drivers/light_ws2811strip.c \ - drivers/light_ws2811strip_stdperiph.c - + drivers/light_ws2811strip.c diff --git a/src/main/target/SPEEDYBEEF4/target.c b/src/main/target/SPEEDYBEEF4/target.c index f050f9a7517..3c5b3162c8a 100644 --- a/src/main/target/SPEEDYBEEF4/target.c +++ b/src/main/target/SPEEDYBEEF4/target.c @@ -24,16 +24,17 @@ #include "drivers/bus.h" const timerHardware_t timerHardware[] = { - { TIM5, IO_TAG(PA3), TIM_Channel_4, 0, IOCFG_AF_PP_UP, GPIO_AF_TIM5, TIM_USE_PPM}, // PPM - { TIM8, IO_TAG(PC6), TIM_Channel_1, 1, IOCFG_AF_PP_UP, GPIO_AF_TIM8, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR }, // S1 UP(1,2) - { TIM8, IO_TAG(PC7), TIM_Channel_2, 1, IOCFG_AF_PP_UP, GPIO_AF_TIM8, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR }, // S2 UP(2,1) - { TIM8, IO_TAG(PC8), TIM_Channel_3, 1, IOCFG_AF_PP_UP, GPIO_AF_TIM8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // S3 UP(2,1) - { TIM8, IO_TAG(PC9), TIM_Channel_4, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // S4 UP(2,1) - { TIM2, IO_TAG(PA15), TIM_Channel_1, 1, IOCFG_AF_PP_UP, GPIO_AF_TIM2, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO}, //S5 UP(1,7) - { TIM1, IO_TAG(PA8), TIM_Channel_1, 1, IOCFG_AF_PP_UP, GPIO_AF_TIM1, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO}, // S6 UP(2,5) + DEF_TIM(TIM5, CH4, PA3, TIM_USE_PPM, 0, 0), // PPM - { TIM4, IO_TAG(PB8), TIM_Channel_3, 1, IOCFG_AF_PP_UP, GPIO_AF_TIM5, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO}, //S7 UP(1,7) - { TIM4, IO_TAG(PB6), TIM_Channel_1, 1, IOCFG_AF_PP_UP, GPIO_AF_TIM5, TIM_USE_LED}, // S6 UP(2,5) + DEF_TIM(TIM8, CH1, PC6, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1 UP(1,2) + DEF_TIM(TIM8, CH2, PC7, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 1), // S2 UP(2,1) + DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), // S3 UP(2,1) + DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4 UP(2,1) + DEF_TIM(TIM2, CH1, PA15, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S5 UP(1,7) + DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S6 UP(2,5) + + DEF_TIM(TIM4, CH3, PB8, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S7 D(1,7)!S5 UP(2,6) + DEF_TIM(TIM4, CH1, PB6, TIM_USE_LED, 0, 0), // LED D(1,0) UP(2,6) }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/SPEEDYBEEF4/target.h b/src/main/target/SPEEDYBEEF4/target.h index 93c8f8def64..96ce695a5c8 100644 --- a/src/main/target/SPEEDYBEEF4/target.h +++ b/src/main/target/SPEEDYBEEF4/target.h @@ -17,7 +17,6 @@ #pragma once -#define TARGET_CONFIG #define TARGET_BOARD_IDENTIFIER "SBF4" #define USBD_PRODUCT_STRING "SpeedyBeeF4" @@ -57,6 +56,8 @@ #define USE_MAG_MAG3110 #define USE_MAG_LIS3MDL +#define TEMPERATURE_I2C_BUS BUS_I2C1 + #define USE_BARO #define BARO_I2C_BUS BUS_I2C1 @@ -64,7 +65,6 @@ #define USE_BARO_BMP280 #define USE_BARO_MS5611 -#define USE_PITOT_MS4525 #define PITOT_I2C_BUS BUS_I2C1 #define USE_RANGEFINDER diff --git a/src/main/target/SPEEDYBEEF4/target.mk b/src/main/target/SPEEDYBEEF4/target.mk index e4d4cb7b6f6..62ba0215c8d 100644 --- a/src/main/target/SPEEDYBEEF4/target.mk +++ b/src/main/target/SPEEDYBEEF4/target.mk @@ -12,8 +12,6 @@ TARGET_SRC = \ drivers/compass/compass_ist8310.c \ drivers/compass/compass_mag3110.c \ drivers/compass/compass_lis3mdl.c \ - drivers/pitotmeter_ms4525.c \ drivers/pitotmeter_adc.c \ drivers/light_ws2811strip.c \ - drivers/light_ws2811strip_stdperiph.c \ drivers/max7456.c diff --git a/src/main/target/SPRACINGF3/target.c b/src/main/target/SPRACINGF3/target.c index eefdfc561c3..d5cc9bcd6ae 100644 --- a/src/main/target/SPRACINGF3/target.c +++ b/src/main/target/SPRACINGF3/target.c @@ -21,28 +21,29 @@ #include "drivers/io.h" #include "drivers/pwm_mapping.h" #include "drivers/timer.h" +#include "drivers/timer_def.h" #include "drivers/bus.h" -const timerHardware_t timerHardware[] = { - { TIM16, IO_TAG(PA6), TIM_Channel_1, 1, IOCFG_AF_PP, GPIO_AF_1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR }, // PWM1 - { TIM17, IO_TAG(PA7), TIM_Channel_1, 1, IOCFG_AF_PP, GPIO_AF_1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR }, // PWM2 - { TIM4, IO_TAG(PA11), TIM_Channel_1, 1, IOCFG_AF_PP, GPIO_AF_10, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // PWM3 - { TIM4, IO_TAG(PA12), TIM_Channel_2, 1, IOCFG_AF_PP, GPIO_AF_10, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // PWM4 - { TIM4, IO_TAG(PB8), TIM_Channel_3, 1, IOCFG_AF_PP, GPIO_AF_2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // PWM5 - { TIM4, IO_TAG(PB9), TIM_Channel_4, 1, IOCFG_AF_PP, GPIO_AF_2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // PWM6 - { TIM15, IO_TAG(PA2), TIM_Channel_1, 1, IOCFG_AF_PP, GPIO_AF_9, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO }, // PWM7 - { TIM15, IO_TAG(PA3), TIM_Channel_2, 1, IOCFG_AF_PP, GPIO_AF_9, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO }, // PWM8 +const timerHardware_t timerHardware[] = { + DEF_TIM(TIM16, CH1, PA6, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0), // PWM1 + DEF_TIM(TIM17, CH1, PA7, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0), // PWM2 + DEF_TIM(TIM4, CH1, PA11, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // PWM3 + DEF_TIM(TIM4, CH2, PA12, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // PWM4 + DEF_TIM(TIM4, CH3, PB8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // PWM5 + DEF_TIM(TIM4, CH4, PB9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // PWM6 + DEF_TIM(TIM15, CH1, PA2, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0), // PWM7 + DEF_TIM(TIM15, CH2, PA3, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0), // PWM8 - { TIM1, IO_TAG(PA8), TIM_Channel_1, 1, IOCFG_AF_PP, GPIO_AF_6, TIM_USE_LED }, // GPIO_TIMER / LED_STRIP + DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0), // GPIO_TIMER / LED_STRIP - { TIM2, IO_TAG(PA0), TIM_Channel_1, 0, IOCFG_AF_PP, GPIO_AF_1, TIM_USE_PPM | TIM_USE_PWM }, // RC_CH1 - { TIM2, IO_TAG(PA1), TIM_Channel_2, 0, IOCFG_AF_PP, GPIO_AF_1, TIM_USE_PWM}, // RC_CH2 - { TIM2, IO_TAG(PB11), TIM_Channel_4, 0, IOCFG_AF_PP, GPIO_AF_1, TIM_USE_PWM }, // RC_CH3 - { TIM2, IO_TAG(PB10), TIM_Channel_3, 0, IOCFG_AF_PP, GPIO_AF_1, TIM_USE_PWM }, // RC_CH4 - { TIM3, IO_TAG(PB4), TIM_Channel_1, 0, IOCFG_AF_PP, GPIO_AF_2, TIM_USE_PWM | TIM_USE_MC_SERVO }, // RC_CH5 - { TIM3, IO_TAG(PB5), TIM_Channel_2, 0, IOCFG_AF_PP, GPIO_AF_2, TIM_USE_PWM | TIM_USE_MC_SERVO }, // RC_CH6 - { TIM3, IO_TAG(PB0), TIM_Channel_3, 0, IOCFG_AF_PP, GPIO_AF_2, TIM_USE_PWM | TIM_USE_MC_SERVO }, // RC_CH7 - { TIM3, IO_TAG(PB1), TIM_Channel_4, 0, IOCFG_AF_PP, GPIO_AF_2, TIM_USE_PWM | TIM_USE_MC_SERVO }, // RC_CH8 + DEF_TIM(TIM2, CH1, PA0, TIM_USE_PWM | TIM_USE_PPM, 0), // RC_CH1 + DEF_TIM(TIM2, CH2, PA1, TIM_USE_PWM, 0), // RC_CH2 + DEF_TIM(TIM2, CH4, PB11, TIM_USE_PWM, 0), // RC_CH3 + DEF_TIM(TIM2, CH3, PB10, TIM_USE_PWM, 0), // RC_CH4 + DEF_TIM(TIM3, CH1, PB4, TIM_USE_PWM | TIM_USE_MC_SERVO, 0), // RC_CH5 + DEF_TIM(TIM3, CH2, PB5, TIM_USE_PWM | TIM_USE_MC_SERVO, 0), // RC_CH6 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_PWM | TIM_USE_MC_SERVO, 0), // RC_CH7 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_PWM | TIM_USE_MC_SERVO, 0), // RC_CH8 }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/SPRACINGF3/target.h b/src/main/target/SPRACINGF3/target.h index 468d716ab22..4717440c16b 100644 --- a/src/main/target/SPRACINGF3/target.h +++ b/src/main/target/SPRACINGF3/target.h @@ -101,8 +101,6 @@ #define USE_LED_STRIP #define WS2811_PIN PA8 -#define WS2811_DMA_STREAM DMA1_Channel2 -#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH2_HANDLER #define USE_RANGEFINDER #define USE_RANGEFINDER_HCSR04 diff --git a/src/main/target/SPRACINGF3/target.mk b/src/main/target/SPRACINGF3/target.mk index b2508f0e397..c8549b88c4d 100644 --- a/src/main/target/SPRACINGF3/target.mk +++ b/src/main/target/SPRACINGF3/target.mk @@ -14,5 +14,4 @@ TARGET_SRC = \ drivers/compass/compass_mag3110.c \ drivers/compass/compass_lis3mdl.c \ drivers/light_ws2811strip.c \ - drivers/light_ws2811strip_stdperiph.c \ drivers/serial_softserial.c diff --git a/src/main/target/SPRACINGF3EVO/target.c b/src/main/target/SPRACINGF3EVO/target.c index 24665c4c252..640a139bf0c 100755 --- a/src/main/target/SPRACINGF3EVO/target.c +++ b/src/main/target/SPRACINGF3EVO/target.c @@ -24,28 +24,29 @@ const timerHardware_t timerHardware[] = { // PPM / UART2 RX - { TIM8, IO_TAG(PA15), TIM_Channel_1, 0, IOCFG_AF_PP, GPIO_AF_2, TIM_USE_PPM }, // PPM + DEF_TIM(TIM8, CH1, PA15, TIM_USE_PPM, 0), // PPM + + DEF_TIM(TIM2, CH1, PA0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0), // PWM1 [TIM2_CH1 (D1_CH5)] + DEF_TIM(TIM2, CH2, PA1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0), // PWM2 [TIM2_CH2 (D1_CH7)] [TIM15_CH1N (D1_CH5)] + DEF_TIM(TIM15, CH1, PA2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // PWM3 [TIM2_CH3 (D1_CH1)] [TIM15_CH1 (D1_CH5)] + DEF_TIM(TIM15, CH2, PA3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // PWM4 [TIM2_CH4 (D1_CH7)] - { TIM2, IO_TAG(PA0), TIM_Channel_1, 1, IOCFG_AF_PP, GPIO_AF_1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR }, // PWM1 - { TIM2, IO_TAG(PA1), TIM_Channel_2, 1, IOCFG_AF_PP, GPIO_AF_1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR }, // PWM2 - { TIM15, IO_TAG(PA2), TIM_Channel_1, 1, IOCFG_AF_PP, GPIO_AF_9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // PWM3 - { TIM15, IO_TAG(PA3), TIM_Channel_2, 1, IOCFG_AF_PP, GPIO_AF_9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // PWM4 #ifdef SPRACINGF3EVO_1SS - { TIM16, IO_TAG(PA6), TIM_Channel_1, 1, IOCFG_AF_PP, GPIO_AF_1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // PWM5 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1 - { TIM17, IO_TAG(PA7), TIM_Channel_1, 1, IOCFG_AF_PP, GPIO_AF_1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // PWM6 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 + DEF_TIM(TIM16, CH1, PA6, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // PWM5 + DEF_TIM(TIM17, CH1, PA7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // PWM6 #else - { TIM3, IO_TAG(PA6), TIM_Channel_1, 1, IOCFG_AF_PP, GPIO_AF_2, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO }, // PWM5 - { TIM3, IO_TAG(PA7), TIM_Channel_2, 1, IOCFG_AF_PP, GPIO_AF_2, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO }, // PWM6 + DEF_TIM(TIM3, CH1, PA6, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0), // PWM5 + DEF_TIM(TIM3, CH2, PA7, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0), // PWM6 #endif - { TIM3, IO_TAG(PB0), TIM_Channel_3, 1, IOCFG_AF_PP, GPIO_AF_2, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO }, // PWM7 - { TIM3, IO_TAG(PB1), TIM_Channel_4, 1, IOCFG_AF_PP, GPIO_AF_2, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO }, // PWM8 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0), // PWM7 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0), // PWM8 // UART3 RX/TX - { TIM2, IO_TAG(PB10), TIM_Channel_3, 1, IOCFG_AF_PP, GPIO_AF_1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // RC_CH4 - PB10 - *TIM2_CH3, USART3_TX (AF7) - { TIM2, IO_TAG(PB11), TIM_Channel_4, 1, IOCFG_AF_PP, GPIO_AF_1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // RC_CH3 - PB11 - *TIM2_CH4, USART3_RX (AF7) + DEF_TIM(TIM2, CH3, PB10, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // RC_CH4 - PB10 - *TIM2_CH3, UART3_TX (AF7) + DEF_TIM(TIM2, CH4, PB11, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // RC_CH3 - PB11 - *TIM2_CH4, UART3_RX (AF7) // IR / LED Strip Pad - { TIM1, IO_TAG(PA8), TIM_Channel_1, 1, IOCFG_AF_PP, GPIO_AF_6, TIM_USE_LED }, // GPIO_TIMER / LED_STRIP + DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0), // LED_STRIP / TRANSPONDER }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/SPRACINGF3EVO/target.h b/src/main/target/SPRACINGF3EVO/target.h index 031d42aec8e..0259638054c 100755 --- a/src/main/target/SPRACINGF3EVO/target.h +++ b/src/main/target/SPRACINGF3EVO/target.h @@ -109,16 +109,14 @@ #define SPI2_SCK_PIN PB13 #define SPI2_MISO_PIN PB14 #define SPI2_MOSI_PIN PB15 +#define SPI2_CLOCK_LEADING_EDGE #define USE_SDCARD -#define USE_SDCARD_SPI2 - +#define USE_SDCARD_SPI #define SDCARD_DETECT_INVERTED #define SDCARD_DETECT_PIN PC14 -#define SDCARD_SPI_INSTANCE SPI2 -#define SDCARD_SPI_CS_PIN SPI2_NSS_PIN -#define SDCARD_DMA_CHANNEL_TX DMA1_Channel5 -#define SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG DMA1_FLAG_TC5 +#define SDCARD_SPI_BUS BUS_SPI2 +#define SDCARD_CS_PIN SPI2_NSS_PIN #define USE_ADC #define ADC_INSTANCE ADC2 @@ -130,10 +128,7 @@ #define RSSI_ADC_CHANNEL ADC_CHN_3 #define USE_LED_STRIP -#define USE_LED_STRIP_ON_DMA1_CHANNEL2 #define WS2811_PIN PA8 -#define WS2811_DMA_STREAM DMA1_Channel2 -#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH2_HANDLER #define USE_RANGEFINDER #define RANGEFINDER_I2C_BUS BUS_I2C1 diff --git a/src/main/target/SPRACINGF3EVO/target.mk b/src/main/target/SPRACINGF3EVO/target.mk index 3ebfe9a6b05..bc26cae3272 100755 --- a/src/main/target/SPRACINGF3EVO/target.mk +++ b/src/main/target/SPRACINGF3EVO/target.mk @@ -15,6 +15,5 @@ TARGET_SRC = \ drivers/compass/compass_mag3110.c \ drivers/compass/compass_lis3mdl.c \ drivers/light_ws2811strip.c \ - drivers/light_ws2811strip_stdperiph.c \ drivers/serial_usb_vcp.c \ drivers/serial_softserial.c diff --git a/src/main/target/SPRACINGF3MINI/target.c b/src/main/target/SPRACINGF3MINI/target.c index 5b88b0c3265..acf31a0384e 100644 --- a/src/main/target/SPRACINGF3MINI/target.c +++ b/src/main/target/SPRACINGF3MINI/target.c @@ -25,28 +25,28 @@ const timerHardware_t timerHardware[] = { // PPM Pad #ifdef SPRACINGF3MINI_MKII_REVA - { TIM3, IO_TAG(PB5), TIM_Channel_2, 0, IOCFG_AF_PP, GPIO_AF_2, TIM_USE_PPM }, // PPM - PB5 + DEF_TIM(TIM3, CH2, PB5, TIM_USE_PPM, 0), // PPM - PB5 // PB4 / TIM3 CH1 is connected to USBPresent #else - { TIM3, IO_TAG(PB4), TIM_Channel_1, 0, IOCFG_AF_PP, GPIO_AF_2, TIM_USE_PPM }, // PPM - PB4 + DEF_TIM(TIM3, CH1, PB4, TIM_USE_PPM, 0), // PPM - PB4 // PB5 / TIM3 CH2 is connected to USBPresent #endif - { TIM16, IO_TAG(PA6), TIM_Channel_1, 1, IOCFG_AF_PP, GPIO_AF_1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR }, // PWM1 - PA6 - { TIM17, IO_TAG(PA7), TIM_Channel_1, 1, IOCFG_AF_PP, GPIO_AF_1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR }, // PWM2 - PA7 - { TIM4, IO_TAG(PB8), TIM_Channel_3, 1, IOCFG_AF_PP, GPIO_AF_2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // PWM3 - PB8 - { TIM4, IO_TAG(PB9), TIM_Channel_4, 1, IOCFG_AF_PP, GPIO_AF_2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // PWM4 - PB9 - { TIM15, IO_TAG(PA2), TIM_Channel_1, 1, IOCFG_AF_PP, GPIO_AF_9, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO }, // PWM5 - PA2 - { TIM15, IO_TAG(PA3), TIM_Channel_2, 1, IOCFG_AF_PP, GPIO_AF_9, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO }, // PWM6 - PA3 - { TIM2, IO_TAG(PA0), TIM_Channel_1, 1, IOCFG_AF_PP, GPIO_AF_1, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO }, // PWM7 - PA0 - { TIM2, IO_TAG(PA1), TIM_Channel_2, 1, IOCFG_AF_PP, GPIO_AF_1, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO }, // PWM8 - PA1 + DEF_TIM(TIM16, CH1, PA6, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0), // PWM1 - PA6 + DEF_TIM(TIM17, CH1, PA7, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0), // PWM2 - PA7 + DEF_TIM(TIM4, CH3, PB8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // PWM3 - PB8 + DEF_TIM(TIM4, CH4, PB9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // PWM4 - PB9 + DEF_TIM(TIM15, CH1, PA2, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0), // PWM5 - PA2 + DEF_TIM(TIM15, CH2, PA3, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0), // PWM6 - PA3 + DEF_TIM(TIM2, CH1, PA0, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0), // PWM7 - PA0 + DEF_TIM(TIM2, CH2, PA1, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0), // PWM8 - PA1 // UART3 RX/TX - { TIM2, IO_TAG(PB10), TIM_Channel_3, 1, IOCFG_AF_PP, GPIO_AF_1, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO }, // PWM9 - PB10 - TIM2_CH3 / UART3_TX (AF7) - { TIM2, IO_TAG(PB11), TIM_Channel_4, 1, IOCFG_AF_PP, GPIO_AF_1, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO }, // PWM10 - PB11 - TIM2_CH4 / UART3_RX (AF7) + DEF_TIM(TIM2, CH3, PB10, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0), // PWM9 - PB10 - TIM2_CH3 / UART3_TX (AF7) + DEF_TIM(TIM2, CH4, PB11, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0), // PWM10 - PB11 - TIM2_CH4 / UART3_RX (AF7) // LED Strip Pad - { TIM1, IO_TAG(PA8), TIM_Channel_1, 1, IOCFG_AF_PP, GPIO_AF_6, TIM_USE_LED }, // GPIO_TIMER / LED_STRIP + DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0), // LED_STRIP / TRANSPONDER }; diff --git a/src/main/target/SPRACINGF3MINI/target.h b/src/main/target/SPRACINGF3MINI/target.h index e6ac0b50b4a..6cd241afd9d 100644 --- a/src/main/target/SPRACINGF3MINI/target.h +++ b/src/main/target/SPRACINGF3MINI/target.h @@ -101,22 +101,14 @@ #define SPI2_SCK_PIN PB13 #define SPI2_MISO_PIN PB14 #define SPI2_MOSI_PIN PB15 +#define SPI2_CLOCK_LEADING_EDGE #define USE_SDCARD -#define USE_SDCARD_SPI2 - +#define USE_SDCARD_SPI #define SDCARD_DETECT_INVERTED - -#define SDCARD_DETECT_PIN PC14 -#define SDCARD_SPI_INSTANCE SPI2 -#define SDCARD_SPI_CS_PIN SPI2_NSS_PIN - -// Note, this is the same DMA channel as UART1_RX. Luckily we don't use DMA for USART Rx. -#define SDCARD_DMA_CHANNEL_TX DMA1_Channel5 -#define SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG DMA1_FLAG_TC5 - -// Performance logging for SD card operations: -// #define AFATFS_USE_INTROSPECTIVE_LOGGING +#define SDCARD_DETECT_PIN PC14 +#define SDCARD_SPI_BUS BUS_SPI2 +#define SDCARD_CS_PIN SPI2_NSS_PIN #define BOARD_HAS_VOLTAGE_DIVIDER #define USE_ADC @@ -130,8 +122,6 @@ #define USE_LED_STRIP #define WS2811_PIN PA8 -#define WS2811_DMA_STREAM DMA1_Channel2 -#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH2_HANDLER #define TRANSPONDER #define TRANSPONDER_GPIO GPIOA diff --git a/src/main/target/SPRACINGF3MINI/target.mk b/src/main/target/SPRACINGF3MINI/target.mk index f33eee9011f..aa9a87bdd19 100644 --- a/src/main/target/SPRACINGF3MINI/target.mk +++ b/src/main/target/SPRACINGF3MINI/target.mk @@ -15,7 +15,6 @@ TARGET_SRC = \ drivers/compass/compass_lis3mdl.c \ drivers/flash_m25p16.c \ drivers/light_ws2811strip.c \ - drivers/light_ws2811strip_stdperiph.c \ drivers/serial_softserial.c \ drivers/serial_usb_vcp.c \ drivers/rangefinder/rangefinder_hcsr04.c \ diff --git a/src/main/target/SPRACINGF3NEO/config.c b/src/main/target/SPRACINGF3NEO/config.c index 4a6f585e0e2..922e391a275 100644 --- a/src/main/target/SPRACINGF3NEO/config.c +++ b/src/main/target/SPRACINGF3NEO/config.c @@ -20,8 +20,6 @@ #include "platform.h" -#ifdef TARGET_CONFIG - #include "io/serial.h" #include "sensors/sensors.h" @@ -39,4 +37,3 @@ void targetConfiguration(void) serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(GPS_UART)].functionMask = FUNCTION_GPS; //telemetryConfigMutable()->halfDuplex = 1; } -#endif diff --git a/src/main/target/SPRACINGF3NEO/target.c b/src/main/target/SPRACINGF3NEO/target.c index 6b2a2a4d75e..6a932ead1bc 100755 --- a/src/main/target/SPRACINGF3NEO/target.c +++ b/src/main/target/SPRACINGF3NEO/target.c @@ -24,24 +24,23 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[] = { - { TIM15, IO_TAG(PA3), TIM_Channel_2, 1, IOCFG_AF_PP, GPIO_AF_9, TIM_USE_PWM | TIM_USE_PPM }, // PWM1 / PPM / UART2 RX - { TIM15, IO_TAG(PA2), TIM_Channel_1, 1, IOCFG_AF_PP, GPIO_AF_9, TIM_USE_PWM }, // PWM2 + DEF_TIM(TIM15, CH2, PA3, TIM_USE_PWM | TIM_USE_PPM, 0), // PWM1 / PPM / UART2 RX + DEF_TIM(TIM15, CH1, PA2, TIM_USE_PWM, 0), // PWM2 - { TIM3, IO_TAG(PB1), TIM_Channel_4, 1, IOCFG_AF_PP, GPIO_AF_2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // ESC1 - { TIM3, IO_TAG(PC7), TIM_Channel_2, 1, IOCFG_AF_PP, GPIO_AF_2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // ESC2 - { TIM3, IO_TAG(PB0), TIM_Channel_3, 1, IOCFG_AF_PP, GPIO_AF_2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // ESC3 - { TIM3, IO_TAG(PC6), TIM_Channel_1, 1, IOCFG_AF_PP, GPIO_AF_2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // ESC4 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // ESC1 + DEF_TIM(TIM3, CH2, PC7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // ESC2 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // ESC3 + DEF_TIM(TIM3, CH1, PC6, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // ESC4 - { TIM8, IO_TAG(PC8), TIM_Channel_3, 1, IOCFG_AF_PP, GPIO_AF_4, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR }, // ESC5 (FW motor) - { TIM8, IO_TAG(PC9), TIM_Channel_4, 1, IOCFG_AF_PP, GPIO_AF_4, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR }, // ESC6 (FW motor) + DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0), // ESC5 (FW motor) + DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0), // ESC6 (FW motor) - { TIM2, IO_TAG(PB10), TIM_Channel_3, 1, IOCFG_AF_PP, GPIO_AF_1, TIM_USE_PWM }, // PWM3 - PB10 - *TIM2_CH3, UART3_TX (AF7) - { TIM2, IO_TAG(PB11), TIM_Channel_4, 1, IOCFG_AF_PP, GPIO_AF_1, TIM_USE_PWM }, // PWM4 - PB11 - *TIM2_CH4, UART3_RX (AF7) - // with DSHOT DMA1-CH3 conflicts with TIM3_CH4 / ESC1. - //{ TIM16, IO_TAG(PB8), TIM_Channel_1, 1, IOCFG_AF_PP, GPIO_AF_1 }, // What's PB8 ??? + DEF_TIM(TIM2, CH3, PB10, TIM_USE_PWM, 0), // PWM3 - PB10 - *TIM2_CH3, UART3_TX (AF7) + DEF_TIM(TIM2, CH4, PB11, TIM_USE_PWM, 0), // PWM4 - PB11 - *TIM2_CH4, UART3_RX (AF7) - { TIM1, IO_TAG(PA8), TIM_Channel_1, 1, IOCFG_AF_PP, GPIO_AF_6, TIM_USE_LED } + DEF_TIM(TIM16, CH1, PB8, TIM_USE_ANY, 0), + DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0), }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/SPRACINGF3NEO/target.h b/src/main/target/SPRACINGF3NEO/target.h index cd8c8410cfe..6df547c8f02 100755 --- a/src/main/target/SPRACINGF3NEO/target.h +++ b/src/main/target/SPRACINGF3NEO/target.h @@ -18,9 +18,6 @@ #pragma once #define TARGET_BOARD_IDENTIFIER "SP3N" -#define TARGET_CONFIG - -#define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT #define LED0 PB9 #define LED1 PB2 @@ -87,9 +84,6 @@ #define UART3_TX_PIN PB10 #define UART3_RX_PIN PB11 -#define USE_ESCSERIAL -#define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1 - #define USE_I2C #define USE_I2C_DEVICE_1 @@ -107,6 +101,7 @@ #define SPI2_SCK_PIN PB13 #define SPI2_MISO_PIN PB14 #define SPI2_MOSI_PIN PB15 +#define SPI2_CLOCK_LEADING_EDGE #define SPI3_NSS_PIN PA15 #define SPI3_SCK_PIN PB3 @@ -116,8 +111,13 @@ #define USE_VTX_RTC6705 #define VTX_RTC6705_OPTIONAL // VTX/OSD board is OPTIONAL +#undef USE_VTX_FFPV #undef USE_VTX_SMARTAUDIO // Disabled due to flash size #undef USE_VTX_TRAMP // Disabled due to flash size +#undef USE_PWM_SERVO_DRIVER // Disabled due to RAM size + +#undef USE_PITOT // Disabled due to RAM size +#undef USE_PITOT_ADC // Disabled due to RAM size #define RTC6705_CS_PIN PF4 #define RTC6705_SPI_INSTANCE SPI3 @@ -133,17 +133,11 @@ #define SPI_SHARED_MAX7456_AND_RTC6705 #define USE_SDCARD -#define USE_SDCARD_SPI2 - +#define USE_SDCARD_SPI #define SDCARD_DETECT_INVERTED - #define SDCARD_DETECT_PIN PC14 -#define SDCARD_SPI_INSTANCE SPI2 -#define SDCARD_SPI_CS_PIN SPI2_NSS_PIN - -// Note, this is the same DMA channel as UART1_RX. Luckily we don't use DMA for USART Rx. -#define SDCARD_DMA_CHANNEL_TX DMA1_Channel5 -#define SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG DMA1_FLAG_TC5 +#define SDCARD_SPI_BUS BUS_SPI2 +#define SDCARD_CS_PIN SPI2_NSS_PIN #define USE_ADC #define ADC_INSTANCE ADC1 @@ -156,8 +150,6 @@ #define USE_LED_STRIP #define WS2811_PIN PA8 -#define WS2811_DMA_STREAM DMA1_Channel2 -#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH2_HANDLER #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT diff --git a/src/main/target/SPRACINGF3NEO/target.mk b/src/main/target/SPRACINGF3NEO/target.mk old mode 100755 new mode 100644 index 5d73c98f896..4e1c1016b8f --- a/src/main/target/SPRACINGF3NEO/target.mk +++ b/src/main/target/SPRACINGF3NEO/target.mk @@ -15,6 +15,5 @@ TARGET_SRC = \ drivers/compass/compass_mag3110.c \ drivers/compass/compass_lis3mdl.c \ drivers/light_ws2811strip.c \ - drivers/light_ws2811strip_stdperiph.c \ drivers/max7456.c \ drivers/vtx_rtc6705.c diff --git a/src/main/target/SPRACINGF4EVO/config.c b/src/main/target/SPRACINGF4EVO/config.c index 96b52172066..fa1e8022978 100755 --- a/src/main/target/SPRACINGF4EVO/config.c +++ b/src/main/target/SPRACINGF4EVO/config.c @@ -20,17 +20,13 @@ #include -#ifdef TARGET_CONFIG - #include "io/serial.h" #include "sensors/barometer.h" #include "telemetry/telemetry.h" - void targetConfiguration(void) { barometerConfigMutable()->baro_hardware = BARO_BMP280; serialConfigMutable()->portConfigs[1].functionMask = FUNCTION_MSP; // So SPRacingF3OSD users don't have to change anything. serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(TELEMETRY_UART)].functionMask = FUNCTION_TELEMETRY_SMARTPORT; } -#endif diff --git a/src/main/target/SPRACINGF4EVO/target.c b/src/main/target/SPRACINGF4EVO/target.c index 89b5ff0dd9b..aebbb346984 100755 --- a/src/main/target/SPRACINGF4EVO/target.c +++ b/src/main/target/SPRACINGF4EVO/target.c @@ -23,26 +23,26 @@ const timerHardware_t timerHardware[] = { - { TIM9, IO_TAG(PA3), TIM_Channel_2, 0, IOCFG_AF_PP_PD, GPIO_AF_TIM9, TIM_USE_PPM | TIM_USE_PWM }, // PPM - { TIM9, IO_TAG(PA2), TIM_Channel_1, 0, IOCFG_AF_PP_PD, GPIO_AF_TIM9, TIM_USE_PWM }, + DEF_TIM(TIM9, CH2, PA3, TIM_USE_PPM | TIM_USE_PWM, 0, 0), // PPM / PWM1 / UART2 RX + DEF_TIM(TIM9, CH1, PA2, TIM_USE_PWM, 0, 0), // PPM / PWM2 / UART2 TX - { TIM8, IO_TAG(PC6), TIM_Channel_1, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, - { TIM8, IO_TAG(PC7), TIM_Channel_2, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, - { TIM8, IO_TAG(PC9), TIM_Channel_4, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, - { TIM8, IO_TAG(PC8), TIM_Channel_3, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, + DEF_TIM(TIM8, CH1, PC6, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), // ESC 1 + DEF_TIM(TIM8, CH2, PC7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), // ESC 2 + DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // ESC 3 + DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), // ESC 4 #if defined(SPRACINGF4EVO_REV) && (SPRACINGF4EVO_REV >= 2) - { TIM4, IO_TAG(PB6), TIM_Channel_1, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM4, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, - { TIM4, IO_TAG(PB7), TIM_Channel_2, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM4, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, + DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // ESC 5 / Conflicts with USART5_RX / SPI3_RX - SPI3_RX can be mapped to DMA1_ST3_CH0 + DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // ESC 6 / Conflicts with USART3_RX #else - { TIM4, IO_TAG(PB8), TIM_Channel_3, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM4, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, - { TIM4, IO_TAG(PB9), TIM_Channel_4, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM4, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, + DEF_TIM(TIM10, CH1, PB8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // ESC 5 + DEF_TIM(TIM11, CH1, PB9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // ESC 6 #endif - { TIM3, IO_TAG(PB1), TIM_Channel_4, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM3, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR }, - { TIM3, IO_TAG(PB0), TIM_Channel_3, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM3, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR }, + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR, 0, 0), // ESC 7 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR, 0, 0), // ESC 8 - { TIM2, IO_TAG(PA1), TIM_Channel_2, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM2, TIM_USE_ANY }, + DEF_TIM(TIM2, CH2, PA1, TIM_USE_LED, 0, 0), // LED Strip }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/SPRACINGF4EVO/target.h b/src/main/target/SPRACINGF4EVO/target.h index 6030c1acb2a..f48b9a24a48 100755 --- a/src/main/target/SPRACINGF4EVO/target.h +++ b/src/main/target/SPRACINGF4EVO/target.h @@ -18,7 +18,6 @@ #pragma once #define TARGET_BOARD_IDENTIFIER "SP4E" -#define TARGET_CONFIG #ifndef SPRACINGF4EVO_REV #define SPRACINGF4EVO_REV 2 @@ -67,6 +66,8 @@ #define USE_MAG_MAG3110 #define USE_MAG_LIS3MDL +#define TEMPERATURE_I2C_BUS BUS_I2C1 + #define USE_VCP #define USE_UART_INVERTER @@ -119,6 +120,7 @@ #define SPI2_SCK_PIN PB13 #define SPI2_MISO_PIN PB14 #define SPI2_MOSI_PIN PB15 +#define SPI2_CLOCK_LEADING_EDGE #define SPI3_NSS_PIN PA15 // NC #define SPI3_SCK_PIN PB3 // NC @@ -132,16 +134,11 @@ #define RTC6705_SPI_INSTANCE SPI3 #define USE_SDCARD +#define USE_SDCARD_SPI #define SDCARD_DETECT_INVERTED -#define SDCARD_DETECT_PIN PC14 - -#define SDCARD_SPI_INSTANCE SPI2 -#define SDCARD_SPI_CS_PIN SPI2_NSS_PIN - -#define SDCARD_DMA_CHANNEL_TX DMA1_Stream4 -#define SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG DMA_FLAG_TCIF4 -#define SDCARD_DMA_CLK RCC_AHB1Periph_DMA1 -#define SDCARD_DMA_CHANNEL DMA_Channel_0 +#define SDCARD_DETECT_PIN PC14 +#define SDCARD_SPI_BUS BUS_SPI2 +#define SDCARD_CS_PIN SPI2_NSS_PIN #define MPU6500_CS_PIN SPI1_NSS_PIN #define MPU6500_SPI_BUS BUS_SPI1 @@ -166,9 +163,6 @@ #define USE_LED_STRIP #define WS2811_PIN PA1 -#define WS2811_DMA_HANDLER_IDENTIFER DMA1_ST6_HANDLER -#define WS2811_DMA_STREAM DMA1_Stream6 -#define WS2811_DMA_CHANNEL DMA_Channel_3 // ??? #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT diff --git a/src/main/target/SPRACINGF4EVO/target.mk b/src/main/target/SPRACINGF4EVO/target.mk index 8c6d1ac05f2..179fd78bc76 100755 --- a/src/main/target/SPRACINGF4EVO/target.mk +++ b/src/main/target/SPRACINGF4EVO/target.mk @@ -13,7 +13,6 @@ TARGET_SRC = \ drivers/compass/compass_mag3110.c \ drivers/compass/compass_lis3mdl.c \ drivers/barometer/barometer_ms56xx.c \ - drivers/light_ws2811strip_stdperiph.c \ drivers/light_ws2811strip.c \ drivers/max7456.c # drivers/vtx_rtc6705.c diff --git a/src/main/target/SPRACINGF7DUAL/config.c b/src/main/target/SPRACINGF7DUAL/config.c new file mode 100644 index 00000000000..332f792d921 --- /dev/null +++ b/src/main/target/SPRACINGF7DUAL/config.c @@ -0,0 +1,67 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software under the terms of the + * GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) + * any later version. + * + * Cleanflight and Betaflight are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +#include +#include + +#include "platform.h" + +#include "common/axis.h" + +#include "drivers/sensor.h" +#include "drivers/compass/compass.h" +#include "drivers/serial.h" + +#include "fc/rc_controls.h" + +#include "flight/failsafe.h" +#include "flight/mixer.h" +#include "flight/pid.h" + +#include "rx/rx.h" + +#include "io/serial.h" + +#include "telemetry/telemetry.h" + +#include "sensors/sensors.h" +#include "sensors/compass.h" +#include "sensors/barometer.h" + +#include "config/feature.h" + +#include "fc/config.h" + +#ifdef USE_TARGET_CONFIG + +#define TELEMETRY_UART SERIAL_PORT_USART5 + +void targetConfiguration(void) +{ + barometerConfigMutable()->baro_hardware = BARO_BMP280; + +#ifdef USE_TELEMETRY + // change telemetry settings + serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(TELEMETRY_UART)].functionMask = FUNCTION_TELEMETRY_SMARTPORT; + telemetryConfigMutable()->telemetry_inverted = 1; + //telemetryConfigMutable()->halfDuplex = 1; +#endif +} +#endif diff --git a/src/main/target/SPRACINGF7DUAL/target.c b/src/main/target/SPRACINGF7DUAL/target.c new file mode 100644 index 00000000000..d9c1c21b9a2 --- /dev/null +++ b/src/main/target/SPRACINGF7DUAL/target.c @@ -0,0 +1,85 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software under the terms of the + * GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) + * any later version. + * + * Cleanflight and Betaflight are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +#include + +#include "platform.h" +#include "drivers/io.h" + +#include "drivers/timer.h" +#include "drivers/timer_def.h" +#include "drivers/dma.h" +#include "drivers/bus.h" + +// Register both MPU6500 +BUSDEV_REGISTER_SPI(busdev_mpu6500_1, DEVHW_MPU6500, MPU6500_1_SPI_BUS, MPU6500_1_CS_PIN, GYRO_1_EXTI_PIN, DEVFLAGS_NONE); +BUSDEV_REGISTER_SPI(busdev_mpu6500_2, DEVHW_MPU6500, MPU6500_2_SPI_BUS, MPU6500_2_CS_PIN, GYRO_2_EXTI_PIN, DEVFLAGS_NONE); + +const timerHardware_t timerHardware[] = { + + DEF_TIM(TIM9, CH2, PA3, TIM_USE_PPM | TIM_USE_PWM, 0, 0), // PPM / PWM1 / UART2 RX + DEF_TIM(TIM9, CH1, PA2, TIM_USE_PWM, 0, 0), // PPM / PWM2 / UART2 TX + +#if (SPRACINGF7DUAL_REV <= 1) + DEF_TIM(TIM8, CH2, PC7, TIM_USE_MC_MOTOR, 0, 0), // ESC 1 +#else + DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR, 0, 0), // ESC 1 +#endif + DEF_TIM(TIM8, CH1, PC6, TIM_USE_MC_MOTOR, 0, 0), // ESC 2 + DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR, 0, 0), // ESC 3 +#if (SPRACINGF7DUAL_REV <= 1) + DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR, 0, 0), // ESC 4 +#else + DEF_TIM(TIM8, CH2, PC7, TIM_USE_MC_MOTOR, 0, 0), // ESC 4 +#endif + + DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_MOTOR, 0, 0), // ESC 5 / Conflicts with USART5_RX / SPI3_RX - SPI3_RX can be mapped to DMA1_ST3_CH0 + DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR, 0, 0), // ESC 6 / Conflicts with USART3_RX + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR, 0, 0), // ESC 7 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR, 0, 0), // ESC 8 + + DEF_TIM(TIM2, CH2, PA1, TIM_USE_LED, 0, 0), // LED Strip + // Additional 2 PWM channels available on UART3 RX/TX pins + // However, when using led strip the timer cannot be used, but no code appears to prevent that right now + DEF_TIM(TIM2, CH3, PB10, TIM_USE_MC_MOTOR, 0, 0), // Shared with UART3 TX PIN and SPI3 TX (OSD) + DEF_TIM(TIM2, CH4, PB11, TIM_USE_MC_MOTOR, 0, 0), // Shared with UART3 RX PIN + + //DEF_TIM(TIM1, CH1, PA8, USE_TRANSPONDER, 0, 0), // Transponder + // Additional 2 PWM channels available on UART1 RX/TX pins + // However, when using transponder the timer cannot be used, but no code appears to prevent that right now + DEF_TIM(TIM1, CH2, PA9, TIM_USE_FW_SERVO | TIM_USE_PWM, 0, 1), // PWM 3 + DEF_TIM(TIM1, CH3, PA10, TIM_USE_FW_SERVO | TIM_USE_PWM, 0, 1), // PWM 4 +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); + +#if (SPRACINGF7DUAL_REV <= 1) + +#include "drivers/serial.h" +#include "drivers/serial_uart.h" + +void usartTargetConfigure(uartPort_t *uartPort) +{ + if (uartPort->USARTx == USART3) { + uartPort->Handle.AdvancedInit.AdvFeatureInit |= UART_ADVFEATURE_SWAP_INIT; + uartPort->Handle.AdvancedInit.Swap = UART_ADVFEATURE_SWAP_ENABLE; + } +} +#endif diff --git a/src/main/target/SPRACINGF7DUAL/target.h b/src/main/target/SPRACINGF7DUAL/target.h new file mode 100644 index 00000000000..d174add3097 --- /dev/null +++ b/src/main/target/SPRACINGF7DUAL/target.h @@ -0,0 +1,220 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software under the terms of the + * GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) + * any later version. + * + * Cleanflight and Betaflight are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "SP7D" +#define USE_TARGET_CONFIG + +#ifndef SPRACINGF7DUAL_REV +#define SPRACINGF7DUAL_REV 2 +#endif + +#define USBD_PRODUCT_STRING "SP Racing F7 DUAL" + +#define USE_DUAL_GYRO + +#define LED0_PIN PC4 + +#define BEEPER PC15 +#define BEEPER_INVERTED + +#define USE_EXTI +#define GYRO_1_EXTI_PIN PC13 +#define GYRO_2_EXTI_PIN PC14 +#define GYRO_INT_EXTI GYRO_1_EXTI_PIN +#define MPU_INT_EXTI + +#define USE_MPU_DATA_READY_SIGNAL +#define ENSURE_MPU_DATA_READY_IS_LOW + +#define USE_TARGET_IMU_HARDWARE_DESCRIPTORS +#define USE_GYRO +#define USE_GYRO_MPU6500 + +#define USE_ACC +#define USE_ACC_MPU6500 + +#if (SPRACINGF7DUAL_REV >= 2) +#define ACC_MPU6500_1_ALIGN CW0_DEG +#define GYRO_MPU6500_1_ALIGN CW0_DEG + +#define ACC_MPU6500_2_ALIGN CW270_DEG +#define GYRO_MPU6500_2_ALIGN CW270_DEG +#else +#define ACC_MPU6500_1_ALIGN CW180_DEG +#define GYRO_MPU6500_1_ALIGN CW180_DEG + +#define ACC_MPU6500_2_ALIGN CW270_DEG +#define GYRO_MPU6500_2_ALIGN CW270_DEG +#endif + + +#define GYRO_1_ALIGN GYRO_MPU6500_1_ALIGN +#define GYRO_2_ALIGN GYRO_MPU6500_2_ALIGN +#define ACC_1_ALIGN ACC_MPU6500_1_ALIGN +#define ACC_2_ALIGN ACC_MPU6500_2_ALIGN + +#define ACC_MPU6500_ALIGN ACC_1_ALIGN +#define GYRO_MPU6500_ALIGN GYRO_1_ALIGN + +#define USE_BARO +#define BARO_I2C_BUS BUS_I2C1 +#define USE_BARO_BMP280 +#define USE_BARO_MS5611 + +#define MAG_I2C_BUS BUS_I2C1 +#define USE_MAG +#define USE_MAG_HMC5883 +#define USE_MAG_QMC5883 +#define USE_MAG_IST8310 +#define USE_MAG_IST8308 +#define USE_MAG_MAG3110 +#define USE_MAG_LIS3MDL + +#define TEMPERATURE_I2C_BUS BUS_I2C1 + +#define USE_VCP +#define USE_UART1 +#define USE_UART2 +#define USE_UART3 +#define USE_UART4 +#define USE_UART5 +#define SERIAL_PORT_COUNT 6 + +#define UART1_TX_PIN PA9 +#define UART1_RX_PIN PA10 + +#define UART2_TX_PIN PA2 +#define UART2_RX_PIN PA3 + +#define UART3_TX_PIN PB10 +#define UART3_RX_PIN PB11 + +#define UART4_TX_PIN PC10 +#define UART4_RX_PIN PC11 + +#define UART5_TX_PIN PC12 +#define UART5_RX_PIN PD2 + +#if (SPRACINGF7DUAL_REV <= 1) + #define TARGET_USART_CONFIG +#endif + +// TODO +// #define USE_ESCSERIAL +// #define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1 + +#define USE_I2C +#define USE_I2C_DEVICE_1 +#define I2C_DEVICE (I2CDEV_1) +#define I2C1_SCL PB8 +#define I2C1_SDA PB9 + +#define USE_SPI + +#define USE_SPI_DEVICE_1 // 2xMPU +#define SPI1_NSS_PIN PA15 +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 + +#define USE_SPI_DEVICE_2 // MAX7456 +#define SPI2_NSS_PIN PB12 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 + +#define USE_MAX7456 +#define MAX7456_SPI_BUS BUS_SPI2 +#define MAX7456_CS_PIN SPI2_NSS_PIN + +#define USE_SDCARD +#define USE_SDCARD_SPI + +#define USE_SPI_DEVICE_3 // SDCARD +#define SPI3_SCK_PIN PB3 +#define SPI3_MISO_PIN PB4 +#define SPI3_MOSI_PIN PB5 +#define SPI3_CLOCK_LEADING_EDGE +#define SDCARD_SPI_BUS BUS_SPI3 +#define SDCARD_CS_PIN PC3 + +// disabled for motor outputs 5-8: +//#define USE_VTX_RTC6705 +//#define USE_VTX_RTC6705_SOFTSPI +//#define VTX_RTC6705_OPTIONAL // VTX/OSD board is OPTIONAL + +//#define RTC6705_SPI_MOSI_PIN PB0 // Shared with PWM8 +//#define RTC6705_CS_PIN PB6 // Shared with PWM5 +//#define RTC6705_SPICLK_PIN PB1 // Shared with PWM7 +//#define RTC6705_POWER_PIN PB7 // Shared with PWM6 + +#define GYRO_1_CS_PIN SPI1_NSS_PIN +#define GYRO_1_SPI_INSTANCE BUS_SPI1 +#define GYRO_2_CS_PIN PB2 +#define GYRO_2_SPI_INSTANCE BUS_SPI1 +#define MPU6500_1_CS_PIN GYRO_1_CS_PIN +#define MPU6500_1_SPI_BUS GYRO_1_SPI_INSTANCE +#define MPU6500_2_CS_PIN GYRO_2_CS_PIN +#define MPU6500_2_SPI_BUS GYRO_2_SPI_INSTANCE + +#define USE_ADC +// It's possible to use ADC1 or ADC3 on this target, same pins. +//#define ADC_INSTANCE ADC1 +//#define ADC1_DMA_STREAM DMA2_Stream0 + +// Using ADC3 frees up DMA2_Stream0 for SPI1_RX (not necessarily, SPI1_RX has DMA2_Stream2 as well) +#define ADC_CHANNEL_1_PIN PC1 +#define ADC_CHANNEL_2_PIN PC2 +#define ADC_CHANNEL_3_PIN PC0 + + +//#define ADC_INSTANCE ADC3 +//#define ADC3_DMA_STREAM DMA2_Stream0 +#define VBAT_ADC_CHANNEL ADC_CHN_1 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 +#define RSSI_ADC_CHANNEL ADC_CHN_3 + +#define CURRENT_METER_SCALE 300 + +#define USE_OSD + +#define USE_LED_STRIP +#define WS2811_PIN PA1 + +//#define RX_CHANNELS_TAER +#define DEFAULT_FEATURES (FEATURE_TRANSPONDER | FEATURE_RSSI_ADC | FEATURE_TELEMETRY | FEATURE_OSD | FEATURE_LED_STRIP| FEATURE_VBAT | FEATURE_CURRENT_METER| FEATURE_BLACKBOX) + +#define GPS_UART SERIAL_PORT_USART3 + +#define SERIALRX_UART SERIAL_PORT_USART2 +#define SERIALRX_PROVIDER SERIALRX_SBUS + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD (BIT(2)) + +#define MAX_PWM_OUTPUT_PORTS 8 + +#define USE_DSHOT diff --git a/src/main/target/SPRACINGF7DUAL/target.mk b/src/main/target/SPRACINGF7DUAL/target.mk new file mode 100644 index 00000000000..47302073242 --- /dev/null +++ b/src/main/target/SPRACINGF7DUAL/target.mk @@ -0,0 +1,21 @@ +F7X2RE_TARGETS += $(TARGET) +FEATURES = VCP SDCARD + +TARGET_SRC = \ + drivers/accgyro/accgyro_fake.c \ + drivers/barometer/barometer_fake.c \ + drivers/compass/compass_fake.c \ + drivers/accgyro/accgyro_mpu6500.c \ + drivers/barometer/barometer_bmp280.c \ + drivers/barometer/barometer_ms56xx.c \ + drivers/compass/compass_hmc5883l.c \ + drivers/compass/compass_qmc5883l.c \ + drivers/compass/compass_ist8310.c \ + drivers/compass/compass_ist8308.c \ + drivers/compass/compass_mag3110.c \ + drivers/compass/compass_lis3mdl.c \ + drivers/light_ws2811strip.c \ + drivers/max7456.c \ + drivers/vtx_rtc6705_soft_spi.c + + diff --git a/src/main/target/STM32F3DISCOVERY/target.c b/src/main/target/STM32F3DISCOVERY/target.c index d34236157a1..66450827ff9 100644 --- a/src/main/target/STM32F3DISCOVERY/target.c +++ b/src/main/target/STM32F3DISCOVERY/target.c @@ -23,20 +23,20 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[] = { - { TIM1, IO_TAG(PA8), TIM_Channel_1, 1, IOCFG_AF_PP_PD, GPIO_AF_6, TIM_USE_PWM | TIM_USE_PPM }, // PWM1 - PA8 - { TIM16, IO_TAG(PB8), TIM_Channel_1, 0, IOCFG_AF_PP_PD, GPIO_AF_1, TIM_USE_PWM | TIM_USE_LED }, // PWM2 - PB8 - { TIM17, IO_TAG(PB9), TIM_Channel_1, 0, IOCFG_AF_PP_PD, GPIO_AF_1, TIM_USE_PWM }, // PWM3 - PB9 - { TIM8, IO_TAG(PC6), TIM_Channel_1, 1, IOCFG_AF_PP_PD, GPIO_AF_4, TIM_USE_PWM }, // PWM4 - PC6 - { TIM8, IO_TAG(PC7), TIM_Channel_2, 1, IOCFG_AF_PP_PD, GPIO_AF_4, TIM_USE_PWM }, // PWM5 - PC7 - { TIM8, IO_TAG(PC8), TIM_Channel_3, 1, IOCFG_AF_PP_PD, GPIO_AF_4, TIM_USE_PWM }, // PWM6 - PC8 - { TIM3, IO_TAG(PB1), TIM_Channel_4, 0, IOCFG_AF_PP_PD, GPIO_AF_2, TIM_USE_PWM }, // PWM7 - PB1 - { TIM3, IO_TAG(PA4), TIM_Channel_2, 0, IOCFG_AF_PP_PD, GPIO_AF_2, TIM_USE_PWM }, // PWM8 - PA2 - { TIM4, IO_TAG(PD12), TIM_Channel_1, 0, IOCFG_AF_PP, GPIO_AF_2, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR }, // PWM9 - PD12 - { TIM4, IO_TAG(PD13), TIM_Channel_2, 0, IOCFG_AF_PP, GPIO_AF_2, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR }, // PWM10 - PD13 - { TIM4, IO_TAG(PD14), TIM_Channel_3, 0, IOCFG_AF_PP, GPIO_AF_2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // PWM11 - PD14 - { TIM4, IO_TAG(PD15), TIM_Channel_4, 0, IOCFG_AF_PP, GPIO_AF_2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // PWM12 - PD15 - { TIM2, IO_TAG(PA1), TIM_Channel_2, 0, IOCFG_AF_PP, GPIO_AF_1, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO }, // PWM13 - PA1 - { TIM2, IO_TAG(PA2), TIM_Channel_3, 0, IOCFG_AF_PP, GPIO_AF_1, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO } // PWM14 - PA2 + DEF_TIM(TIM1, CH1, PA8, TIM_USE_PWM | TIM_USE_PPM, 0), + DEF_TIM(TIM16, CH1, PB8, TIM_USE_PWM | TIM_USE_LED, 0), + DEF_TIM(TIM17, CH1, PB9, TIM_USE_PWM, 0), + DEF_TIM(TIM8, CH1, PC6, TIM_USE_PWM, 0), + DEF_TIM(TIM8, CH2, PC7, TIM_USE_PWM, 0), + DEF_TIM(TIM8, CH3, PC8, TIM_USE_PWM, 0), + DEF_TIM(TIM3, CH4, PB1, TIM_USE_PWM, 0), + DEF_TIM(TIM3, CH2, PA4, TIM_USE_PWM, 0), + DEF_TIM(TIM4, CH1, PD12, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0), + DEF_TIM(TIM4, CH2, PD13, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0), + DEF_TIM(TIM4, CH3, PD14, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), + DEF_TIM(TIM4, CH4, PD15, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), + DEF_TIM(TIM2, CH2, PA1, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0), + DEF_TIM(TIM2, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0), }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/STM32F3DISCOVERY/target.h b/src/main/target/STM32F3DISCOVERY/target.h index b476c4c21a7..c0d97b45006 100644 --- a/src/main/target/STM32F3DISCOVERY/target.h +++ b/src/main/target/STM32F3DISCOVERY/target.h @@ -135,9 +135,6 @@ #define USE_LED_STRIP #define WS2811_PIN PB8 // TIM16_CH1 -#define WS2811_TIMER TIM16 -#define WS2811_DMA_STREAM DMA1_Channel3 -#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH3_HANDLER #define USE_SPEKTRUM_BIND #define BIND_PIN PA3 // USART2, PA3 diff --git a/src/main/target/STM32F3DISCOVERY/target.mk b/src/main/target/STM32F3DISCOVERY/target.mk index 1db7bb0536c..ebb458b8383 100644 --- a/src/main/target/STM32F3DISCOVERY/target.mk +++ b/src/main/target/STM32F3DISCOVERY/target.mk @@ -30,6 +30,4 @@ TARGET_SRC = \ drivers/compass/compass_ist8308.c \ drivers/flash_m25p16.c \ drivers/light_ws2811strip.c \ - drivers/light_ws2811strip_stdperiph.c \ - drivers/pitotmeter_ms4525.c diff --git a/src/main/target/YUPIF4/config.c b/src/main/target/YUPIF4/config.c index 2aca5e6d4a1..5ac92a83589 100644 --- a/src/main/target/YUPIF4/config.c +++ b/src/main/target/YUPIF4/config.c @@ -18,7 +18,7 @@ #include #include #include -#ifdef TARGET_CONFIG + #include "config/feature.h" #include "drivers/pwm_output.h" #include "blackbox/blackbox.h" @@ -60,4 +60,3 @@ void targetConfiguration(void) pidProfileMutable()->bank_mc.pid[PID_LEVEL].D = 75; } -#endif diff --git a/src/main/target/YUPIF4/target.c b/src/main/target/YUPIF4/target.c index 3b49e281d12..81385400cdc 100644 --- a/src/main/target/YUPIF4/target.c +++ b/src/main/target/YUPIF4/target.c @@ -24,21 +24,23 @@ #include "drivers/bus.h" const timerHardware_t timerHardware[] = { - { TIM8, IO_TAG(PC8), TIM_Channel_3, 0, IOCFG_AF_PP, GPIO_AF_TIM8, TIM_USE_PPM }, - { TIM2, IO_TAG(PA0), TIM_Channel_1, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM2, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR }, - { TIM2, IO_TAG(PA1), TIM_Channel_2, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM2, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR }, - { TIM5, IO_TAG(PA2), TIM_Channel_3, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM5, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, - { TIM5, IO_TAG(PA3), TIM_Channel_4, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM5, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, - { TIM3, IO_TAG(PB0), TIM_Channel_3, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM3, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO }, + DEF_TIM(TIM8, CH3, PC8, TIM_USE_PPM, 0, 0 ), // PPM IN + + DEF_TIM(TIM5, CH1, PA0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0 ), // S1_OUT - DMA1_ST2 + DEF_TIM(TIM5, CH2, PA1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0 ), // S2_OUT - DMA1_ST4 + DEF_TIM(TIM2, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), // S3_OUT - DMA1_ST1 + DEF_TIM(TIM2, CH4, PA3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1 ), // S4_OUT - DMA1_ST6 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0 ), // S5_OUT - DMA1_ST7 + #if defined (YUPIF4R2) - { TIM3, IO_TAG(PB1), TIM_Channel_4, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM3, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO | TIM_USE_LED }, - { TIM12, IO_TAG(PB14), TIM_Channel_1, 1, IOCFG_AF_PP, GPIO_AF_TIM12, TIM_USE_BEEPER }, + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO | TIM_USE_LED, 0, 0 ), + DEF_TIM(TIM12, CH1, PB14, TIM_USE_BEEPER, 0, 0 ), // BEEPER PWM OPT #elif (YUPIF4MINI) - { TIM4, IO_TAG(PB7), TIM_Channel_2, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM4, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO }, - { TIM3, IO_TAG(PC9), TIM_Channel_4, 1, IOCFG_AF_PP, GPIO_AF_TIM3, TIM_USE_BEEPER }, + DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0 ), + DEF_TIM(TIM3, CH4, PC9, TIM_USE_BEEPER, 0, 0 ), // BEEPER PWM #else - { TIM3, IO_TAG(PB1), TIM_Channel_4, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM3, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO | TIM_USE_LED }, - { TIM3, IO_TAG(PC9), TIM_Channel_4, 1, IOCFG_AF_PP, GPIO_AF_TIM3, TIM_USE_BEEPER }, + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO | TIM_USE_LED, 0, 0 ), + DEF_TIM(TIM3, CH4, PC9, TIM_USE_BEEPER, 0, 0 ), // BEEPER PWM #endif }; diff --git a/src/main/target/YUPIF4/target.h b/src/main/target/YUPIF4/target.h index a8b5db4f606..45202fd71d0 100644 --- a/src/main/target/YUPIF4/target.h +++ b/src/main/target/YUPIF4/target.h @@ -28,8 +28,6 @@ #define USBD_PRODUCT_STRING "YUPIF4" #endif -#define TARGET_CONFIG - #define LED0 PB6 #define LED1 PB4 @@ -80,6 +78,8 @@ #define MAG_HMC5883_ALIGN CW270_DEG_FLIP #define USE_MAG_QMC5883 +#define TEMPERATURE_I2C_BUS BUS_I2C2 + #define USE_BARO #define BARO_I2C_BUS BUS_I2C2 #define USE_BARO_MS5611 @@ -93,15 +93,11 @@ #if defined(YUPIF4MINI) #else #define USE_SDCARD -#define USE_SDCARD_SPI3 +#define USE_SDCARD_SPI #define SDCARD_DETECT_INVERTED -#define SDCARD_DETECT_PIN PD2 -#define SDCARD_SPI_INSTANCE SPI3 -#define SDCARD_SPI_CS_PIN SPI3_NSS_PIN -#define SDCARD_DMA_CHANNEL_TX DMA1_Stream5 -#define SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG DMA_FLAG_TCIF5 -#define SDCARD_DMA_CLK RCC_AHB1Periph_DMA1 -#define SDCARD_DMA_CHANNEL DMA_Channel_0 +#define SDCARD_DETECT_PIN PD2 +#define SDCARD_SPI_BUS BUS_SPI3 +#define SDCARD_CS_PIN SPI3_NSS_PIN #endif #define USB_IO @@ -143,6 +139,7 @@ #define SPI3_SCK_PIN PC10 #define SPI3_MISO_PIN PC11 #define SPI3_MOSI_PIN PC12 +#define SPI3_CLOCK_LEADING_EDGE // ADC inputs #define BOARD_HAS_VOLTAGE_DIVIDER @@ -155,10 +152,6 @@ // LED Strip can run off Pin 5 (PB1) of the motor outputs #define USE_LED_STRIP #define WS2811_PIN PB1 -#define WS2811_TIMER TIM8 -#define WS2811_DMA_HANDLER_IDENTIFER DMA1_ST2_HANDLER -#define WS2811_DMA_STREAM DMA1_Stream2 -#define WS2811_DMA_CHANNEL DMA_Channel_5 // Features // #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT @@ -178,4 +171,4 @@ #define TARGET_IO_PORTC 0xffff #define TARGET_IO_PORTD (BIT(2)) -#define PCA9685_I2C_BUS BUS_I2C2 \ No newline at end of file +#define PCA9685_I2C_BUS BUS_I2C2 diff --git a/src/main/target/YUPIF4/target.mk b/src/main/target/YUPIF4/target.mk index 6a8de1be2a4..d1329a43f80 100644 --- a/src/main/target/YUPIF4/target.mk +++ b/src/main/target/YUPIF4/target.mk @@ -12,5 +12,4 @@ TARGET_SRC = \ drivers/compass/compass_ist8310.c \ drivers/compass/compass_mag3110.c \ drivers/max7456.c \ - drivers/light_ws2811strip.c \ - drivers/light_ws2811strip_stdperiph.c + drivers/light_ws2811strip.c diff --git a/src/main/target/YUPIF7/config.c b/src/main/target/YUPIF7/config.c index adbd9f97b0e..e61e532d560 100644 --- a/src/main/target/YUPIF7/config.c +++ b/src/main/target/YUPIF7/config.c @@ -18,7 +18,7 @@ #include #include #include -#ifdef TARGET_CONFIG + #include "config/feature.h" #include "drivers/pwm_output.h" #include "blackbox/blackbox.h" @@ -60,4 +60,3 @@ void targetConfiguration(void) pidProfileMutable()->bank_mc.pid[PID_LEVEL].D = 75; } -#endif diff --git a/src/main/target/YUPIF7/target.c b/src/main/target/YUPIF7/target.c index e58c8dbe48a..c01c2c41bae 100644 --- a/src/main/target/YUPIF7/target.c +++ b/src/main/target/YUPIF7/target.c @@ -24,15 +24,15 @@ #include "drivers/bus.h" const timerHardware_t timerHardware[] = { - { TIM8, IO_TAG(PC8), TIM_CHANNEL_3, 0, IOCFG_AF_PP, GPIO_AF3_TIM8, TIM_USE_PPM }, // PPM IN - { TIM2, IO_TAG(PA0), TIM_CHANNEL_1, 1, IOCFG_AF_PP_PD, GPIO_AF1_TIM2, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR }, // S1_OUT - { TIM5, IO_TAG(PA1), TIM_CHANNEL_2, 1, IOCFG_AF_PP_PD, GPIO_AF2_TIM5, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR }, // S2_OUT - { TIM2, IO_TAG(PA2), TIM_CHANNEL_3, 1, IOCFG_AF_PP_PD, GPIO_AF1_TIM2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // S3_OUT - { TIM2, IO_TAG(PA3), TIM_CHANNEL_4, 1, IOCFG_AF_PP_PD, GPIO_AF1_TIM2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // S4_OUT - { TIM3, IO_TAG(PB0), TIM_CHANNEL_3, 1, IOCFG_AF_PP_PD, GPIO_AF2_TIM3, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO }, // S5_OUT - { TIM3, IO_TAG(PB1), TIM_CHANNEL_4, 1, IOCFG_AF_PP_PD, GPIO_AF2_TIM3, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO | TIM_USE_LED }, // S6_OUT - { TIM2, IO_TAG(PB7), TIM_CHANNEL_2, 1, IOCFG_AF_PP_PD, GPIO_AF2_TIM4, TIM_USE_ANY }, // ANY - { TIM12, IO_TAG(PB14), TIM_CHANNEL_1, 1, IOCFG_AF_PP_PD, GPIO_AF9_TIM12, TIM_USE_BEEPER }, // BEEPER PWM + DEF_TIM(TIM8, CH3, PC8, TIM_USE_PPM, 0, 1 ), // PPM IN + DEF_TIM(TIM2, CH1, PA0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0 ), // S1_OUT - DMA1_ST2 + DEF_TIM(TIM5, CH2, PA1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), // S2_OUT - DMA1_ST4 + DEF_TIM(TIM5, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), // S3_OUT - DMA1_ST1 + DEF_TIM(TIM5, CH4, PA3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1 ), // S4_OUT - DMA1_ST6 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0 ), // S5_OUT - DMA1_ST7 - LED Control + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO | TIM_USE_LED, 0, 0 ), // S6_OUT + DEF_TIM(TIM4, CH2, PB7, TIM_USE_ANY, 0, 0 ), // ANY + DEF_TIM(TIM12, CH1, PB14, TIM_USE_BEEPER, 0, 0 ), // BEEPER PWM }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/YUPIF7/target.h b/src/main/target/YUPIF7/target.h index 2a9802db211..5b7516522a2 100644 --- a/src/main/target/YUPIF7/target.h +++ b/src/main/target/YUPIF7/target.h @@ -19,7 +19,6 @@ #pragma once #define TARGET_BOARD_IDENTIFIER "YPF7" #define USBD_PRODUCT_STRING "YUPIF7" -#define TARGET_CONFIG #define LED0 PB4 @@ -60,6 +59,8 @@ #define MAG_HMC5883_ALIGN CW270_DEG_FLIP #define USE_MAG_QMC5883 +#define TEMPERATURE_I2C_BUS BUS_I2C1 + #define USE_BARO #define BARO_I2C_BUS BUS_I2C1 #define USE_BARO_MS5611 @@ -142,12 +143,6 @@ // LED Strip can run off Pin 5 (PB1) of the motor outputs #define USE_LED_STRIP #define WS2811_PIN PB1 -#define WS2811_TIMER TIM8 -#define WS2811_TIMER_CHANNEL TIM_CHANNEL_3 -#define WS2811_DMA_HANDLER_IDENTIFER DMA1_ST2_HANDLER -#define WS2811_DMA_STREAM DMA1_Stream2 -#define WS2811_DMA_CHANNEL DMA_CHANNEL_5 -#define WS2811_TIMER_GPIO_AF GPIO_AF3_TIM8 // Default configuration #define SERIALRX_PROVIDER SERIALRX_SBUS diff --git a/src/main/target/YUPIF7/target.mk b/src/main/target/YUPIF7/target.mk index 76833e976ba..daa1a78f0fa 100644 --- a/src/main/target/YUPIF7/target.mk +++ b/src/main/target/YUPIF7/target.mk @@ -12,5 +12,4 @@ TARGET_SRC = \ drivers/compass/compass_mag3110.c \ drivers/max7456.c \ drivers/light_ws2811strip.c \ - drivers/light_ws2811strip_hal.c \ drivers/serial_softserial.c diff --git a/src/main/target/common.h b/src/main/target/common.h index c1d7ab9af1b..047cf767275 100755 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -17,6 +17,12 @@ #pragma once +#if defined(STM32F3) +#define DYNAMIC_HEAP_SIZE 1024 +#else +#define DYNAMIC_HEAP_SIZE 2048 +#endif + #define I2C1_OVERCLOCK false #define I2C2_OVERCLOCK false #define USE_I2C_PULLUP // Enable built-in pullups on all boards in case external ones are too week @@ -33,13 +39,13 @@ #if defined(STM32F3) #define USE_UNDERCLOCK +//save flash for F3 targets +#define CLI_MINIMAL_VERBOSITY +#define SKIP_CLI_COMMAND_HELP +#define SKIP_CLI_RESOURCES #endif -#if defined(STM32F3) || defined(STM32F4) #define USE_ADC_AVERAGING -#define ADC_AVERAGE_N_SAMPLES 20 -#endif - #define USE_64BIT_TIME #define USE_BLACKBOX #define USE_GPS @@ -50,6 +56,7 @@ #define USE_TELEMETRY_FRSKY #define USE_GYRO_BIQUAD_RC_FIR2 +#define USE_MR_BRAKING_MODE #if defined(STM_FAST_TARGET) #define SCHEDULER_DELAY_LIMIT 10 @@ -61,15 +68,43 @@ #define USE_EXTENDED_CMS_MENUS #define USE_UAV_INTERCONNECT #define USE_RX_UIB + +// Allow default rangefinders +#define USE_RANGEFINDER +#define USE_RANGEFINDER_MSP +#define USE_RANGEFINDER_BENEWAKE +#define USE_RANGEFINDER_VL53L0X + +// Allow default optic flow boards +#define USE_OPFLOW +#define USE_OPFLOW_CXOF +#define USE_OPFLOW_MSP + +#define USE_PITOT +#define USE_PITOT_MS4525 + +#define USE_1WIRE +#define USE_1WIRE_DS2482 + +#define USE_TEMPERATURE_SENSOR +#define USE_TEMPERATURE_LM75 +#define USE_TEMPERATURE_DS18B20 + +#define USE_MSP_DISPLAYPORT +#define USE_DASHBOARD +#define DASHBOARD_ARMED_BITMAP +#define USE_OLED_UG2864 + +#define USE_PWM_DRIVER_PCA9685 + +#define USE_BOOTLOG +#define BOOTLOG_DESCRIPTIONS #endif #if (FLASH_SIZE > 128) #define NAV_FIXED_WING_LANDING #define USE_AUTOTUNE_FIXED_WING -#define USE_ASYNC_GYRO_PROCESSING #define USE_DEBUG_TRACE -#define USE_BOOTLOG -#define BOOTLOG_DESCRIPTIONS #define USE_STATS #define USE_GYRO_NOTCH_1 #define USE_GYRO_NOTCH_2 @@ -77,10 +112,6 @@ #define USE_ACC_NOTCH #define USE_CMS #define CMS_MENU_OSD -#define USE_DASHBOARD -#define USE_OLED_UG2864 -#define USE_MSP_DISPLAYPORT -#define DASHBOARD_ARMED_BITMAP #define USE_GPS_PROTO_NMEA #define USE_GPS_PROTO_NAZA #define USE_GPS_PROTO_MTK @@ -102,18 +133,17 @@ #define USE_SERIALRX_CRSF #define USE_PWM_SERVO_DRIVER #define USE_SERIAL_PASSTHROUGH -#define USE_PWM_DRIVER_PCA9685 #define NAV_MAX_WAYPOINTS 60 #define MAX_BOOTLOG_ENTRIES 64 #define USE_RCDEVICE #define USE_PITOT #define USE_PITOT_ADC -//Enable VTX controll -#define USE_VTX_COMMON +//Enable VTX control #define USE_VTX_CONTROL #define USE_VTX_SMARTAUDIO #define USE_VTX_TRAMP +#define USE_VTX_FFPV //Enable DST calculations #define RTC_AUTOMATIC_DST @@ -121,10 +151,7 @@ #define USE_WIND_ESTIMATOR #else // FLASH_SIZE < 128 -#define CLI_MINIMAL_VERBOSITY + #define SKIP_TASK_STATISTICS -#define SKIP_CLI_COMMAND_HELP -#define SKIP_CLI_RESOURCES -#define NAV_MAX_WAYPOINTS 30 -#define MAX_BOOTLOG_ENTRIES 32 + #endif diff --git a/src/main/target/common_hardware.c b/src/main/target/common_hardware.c index fc968b3f240..b338074bf85 100755 --- a/src/main/target/common_hardware.c +++ b/src/main/target/common_hardware.c @@ -118,7 +118,7 @@ #endif /** COMPASS SENSORS **/ - +#if !defined(USE_TARGET_MAG_HARDWARE_DESCRIPTORS) #if defined(USE_MAG_HMC5883) #if !defined(HMC5883_I2C_BUS) #define HMC5883_I2C_BUS MAG_I2C_BUS @@ -159,7 +159,7 @@ #if !defined(MAG3110_I2C_BUS) #define MAG3110_I2C_BUS MAG_I2C_BUS #endif - BUSDEV_REGISTER_I2C(busdev_mag3110, DEVHW_MAG3110, MAG3110_I2C_BUS, 0x0E, NONE, DEVFLAGS_NONE); + BUSDEV_REGISTER_I2C(busdev_mag3110, DEVHW_MAG3110, MAG3110_I2C_BUS, 0x0C, NONE, DEVFLAGS_NONE); #endif #if defined(USE_MAG_LIS3MDL) @@ -175,7 +175,7 @@ #if !defined(IST8310_I2C_BUS) #define IST8310_I2C_BUS MAG_I2C_BUS #endif - BUSDEV_REGISTER_I2C(busdev_ist8310, DEVHW_IST8310, IST8310_I2C_BUS, 0x0C, NONE, DEVFLAGS_NONE); + BUSDEV_REGISTER_I2C(busdev_ist8310, DEVHW_IST8310, IST8310_I2C_BUS, 0x0E, NONE, DEVFLAGS_NONE); #endif #if defined(USE_MAG_IST8308) @@ -184,6 +184,40 @@ #endif BUSDEV_REGISTER_I2C(busdev_ist8308, DEVHW_IST8308, IST8308_I2C_BUS, 0x0C, NONE, DEVFLAGS_NONE); #endif +#endif + + +/** 1-Wire IF **/ + +#ifdef USE_1WIRE + +#if defined(TEMPERATURE_I2C_BUS) && !defined(DS2482_I2C_BUS) + #define DS2482_I2C_BUS TEMPERATURE_I2C_BUS +#endif + +#if defined(USE_1WIRE_DS2482) && defined(DS2482_I2C_BUS) + BUSDEV_REGISTER_I2C(busdev_ds2482, DEVHW_DS2482, DS2482_I2C_BUS, 0x18, NONE, DEVFLAGS_USE_RAW_REGISTERS); +#endif + +#endif + + +/** TEMP SENSORS **/ + +#if defined(TEMPERATURE_I2C_BUS) && !defined(LM75_I2C_BUS) + #define LM75_I2C_BUS TEMPERATURE_I2C_BUS +#endif + +#if defined(USE_TEMPERATURE_LM75) && defined(LM75_I2C_BUS) + BUSDEV_REGISTER_I2C(busdev_lm75_0, DEVHW_LM75_0, LM75_I2C_BUS, 0x48, NONE, DEVFLAGS_NONE); + BUSDEV_REGISTER_I2C(busdev_lm75_1, DEVHW_LM75_1, LM75_I2C_BUS, 0x49, NONE, DEVFLAGS_NONE); + BUSDEV_REGISTER_I2C(busdev_lm75_2, DEVHW_LM75_2, LM75_I2C_BUS, 0x4A, NONE, DEVFLAGS_NONE); + BUSDEV_REGISTER_I2C(busdev_lm75_3, DEVHW_LM75_3, LM75_I2C_BUS, 0x4B, NONE, DEVFLAGS_NONE); + BUSDEV_REGISTER_I2C(busdev_lm75_4, DEVHW_LM75_4, LM75_I2C_BUS, 0x4C, NONE, DEVFLAGS_NONE); + BUSDEV_REGISTER_I2C(busdev_lm75_5, DEVHW_LM75_5, LM75_I2C_BUS, 0x4D, NONE, DEVFLAGS_NONE); + BUSDEV_REGISTER_I2C(busdev_lm75_6, DEVHW_LM75_6, LM75_I2C_BUS, 0x4E, NONE, DEVFLAGS_NONE); + BUSDEV_REGISTER_I2C(busdev_lm75_7, DEVHW_LM75_7, LM75_I2C_BUS, 0x4F, NONE, DEVFLAGS_NONE); +#endif /** RANGEFINDER SENSORS **/ @@ -192,31 +226,39 @@ #if !defined(SRF10_I2C_BUS) #define SRF10_I2C_BUS RANGEFINDER_I2C_BUS #endif + #if defined(SRF10_I2C_BUS) BUSDEV_REGISTER_I2C(busdev_srf10, DEVHW_SRF10, SRF10_I2C_BUS, 0x70, NONE, DEVFLAGS_NONE); + #endif #endif #if defined(USE_RANGEFINDER_HCSR04_I2C) #if !defined(HCSR04_I2C_BUS) #define HCSR04_I2C_BUS RANGEFINDER_I2C_BUS #endif + #if defined(HCSR04_I2C_BUS) BUSDEV_REGISTER_I2C(busdev_hcsr04, DEVHW_HCSR04_I2C, HCSR04_I2C_BUS, 0x14, NONE, DEVFLAGS_NONE); + #endif #endif #if defined(USE_RANGEFINDER_VL53L0X) - #if !defined(VL53L0X_I2C_BUS) + #if !defined(VL53L0X_I2C_BUS) && defined(RANGEFINDER_I2C_BUS) #define VL53L0X_I2C_BUS RANGEFINDER_I2C_BUS #endif + + #if defined(VL53L0X_I2C_BUS) BUSDEV_REGISTER_I2C(busdev_vl53l0x, DEVHW_VL53L0X, VL53L0X_I2C_BUS, 0x29, NONE, DEVFLAGS_NONE); + #endif #endif /** AIRSPEED SENSORS **/ -#if defined(USE_PITOT_MS4525) - #if !defined(MS4525_I2C_BUS) - #define MS4525_I2C_BUS PITOT_I2C_BUS - #endif - BUSDEV_REGISTER_I2C(busdev_ms5425, DEVHW_MS4525, MS4525_I2C_BUS, 0x28, NONE, DEVFLAGS_NONE); +#if defined(PITOT_I2C_BUS) && !defined(MS4525_I2C_BUS) + #define MS4525_I2C_BUS PITOT_I2C_BUS +#endif + +#if defined(USE_PITOT_MS4525) && defined(MS4525_I2C_BUS) + BUSDEV_REGISTER_I2C(busdev_ms5425, DEVHW_MS4525, MS4525_I2C_BUS, 0x28, NONE, DEVFLAGS_USE_RAW_REGISTERS); // Requires 0xFF to passthrough #endif @@ -230,6 +272,14 @@ BUSDEV_REGISTER_SPI(busdev_m25p16, DEVHW_M25P16, M25P16_SPI_BUS, M25P16_CS_PIN, NONE, DEVFLAGS_NONE); #endif +#if defined(USE_SDCARD) && defined(USE_SDCARD_SPI) + BUSDEV_REGISTER_SPI(busdev_sdcard_spi, DEVHW_SDCARD, SDCARD_SPI_BUS, SDCARD_CS_PIN, NONE, DEVFLAGS_USE_MANUAL_DEVICE_SELECT | DEVFLAGS_SPI_MODE_0); +#endif + +#if defined(USE_SDCARD) && defined(USE_SDCARD_SDIO) + BUSDEV_REGISTER_SDIO(busdev_sdcard_sdio,DEVHW_SDCARD, SDCARD_SDIO_BUS, SDCARD_CS_PIN, NONE, DEVFLAGS_USE_MANUAL_DEVICE_SELECT); +#endif + #if defined(USE_OLED_UG2864) #if !defined(UG2864_I2C_BUS) #define UG2864_I2C_BUS BUS_I2C1 diff --git a/src/main/target/common_post.h b/src/main/target/common_post.h index 58a1f4fc4f8..6cef361b998 100644 --- a/src/main/target/common_post.h +++ b/src/main/target/common_post.h @@ -29,3 +29,9 @@ # undef USE_DASHBOARD # undef USE_OLED_UG2864 #endif + +// Enable MSP_DISPLAYPORT for F3 targets without builtin OSD, +// since it's used to display CMS on MWOSD +#if !defined(USE_MSP_DISPLAYPORT) && (FLASH_SIZE > 128) && !defined(USE_OSD) +#define USE_MSP_DISPLAYPORT +#endif diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c index fc369e8ac10..f4188e19a7f 100755 --- a/src/main/telemetry/crsf.c +++ b/src/main/telemetry/crsf.c @@ -23,6 +23,7 @@ #if defined(USE_TELEMETRY) && defined(USE_SERIALRX_CRSF) && defined(USE_TELEMETRY_CRSF) +#include "build/atomic.h" #include "build/build_config.h" #include "build/version.h" @@ -31,14 +32,17 @@ #include "common/streambuf.h" #include "common/time.h" #include "common/utils.h" +#include "common/printf.h" #include "config/feature.h" #include "drivers/serial.h" #include "drivers/time.h" +#include "drivers/nvic.h" #include "fc/config.h" #include "fc/rc_controls.h" +#include "fc/rc_modes.h" #include "fc/runtime_config.h" #include "flight/imu.h" @@ -55,20 +59,75 @@ #include "telemetry/crsf.h" #include "telemetry/telemetry.h" +#include "telemetry/msp_shared.h" #define CRSF_CYCLETIME_US 100000 // 100ms, 10 Hz - +#define CRSF_DEVICEINFO_VERSION 0x01 // According to TBS: "CRSF over serial should always use a sync byte at the beginning of each frame. // To get better performance it's recommended to use the sync byte 0xC8 to get better performance" // // Digitalentity: Using frame address byte as a sync field looks somewhat hacky to me, but seems it's needed to get CRSF working properly -#define CRSF_TELEMETRY_SYNC_BYTE 0xC8 +#define CRSF_DEVICEINFO_PARAMETER_COUNT 0 + +#define CRSF_MSP_BUFFER_SIZE 96 +#define CRSF_MSP_LENGTH_OFFSET 1 -static bool crsfTelemetryEnabled; static uint8_t crsfCrc; +static bool crsfTelemetryEnabled; +static bool deviceInfoReplyPending; static uint8_t crsfFrame[CRSF_FRAME_SIZE_MAX]; +#if defined(USE_MSP_OVER_TELEMETRY) +typedef struct mspBuffer_s { + uint8_t bytes[CRSF_MSP_BUFFER_SIZE]; + int len; +} mspBuffer_t; + +static mspBuffer_t mspRxBuffer; + +void initCrsfMspBuffer(void) +{ + mspRxBuffer.len = 0; +} + +bool bufferCrsfMspFrame(uint8_t *frameStart, int frameLength) +{ + if (mspRxBuffer.len + CRSF_MSP_LENGTH_OFFSET + frameLength > CRSF_MSP_BUFFER_SIZE) { + return false; + } else { + uint8_t *p = mspRxBuffer.bytes + mspRxBuffer.len; + *p++ = frameLength; + memcpy(p, frameStart, frameLength); + mspRxBuffer.len += CRSF_MSP_LENGTH_OFFSET + frameLength; + return true; + } +} + +bool handleCrsfMspFrameBuffer(uint8_t payloadSize, mspResponseFnPtr responseFn) +{ + bool requestHandled = false; + if (!mspRxBuffer.len) { + return false; + } + int pos = 0; + while (true) { + const int mspFrameLength = mspRxBuffer.bytes[pos]; + if (handleMspFrame(&mspRxBuffer.bytes[CRSF_MSP_LENGTH_OFFSET + pos], mspFrameLength)) { + requestHandled |= sendMspReply(payloadSize, responseFn); + } + pos += CRSF_MSP_LENGTH_OFFSET + mspFrameLength; + ATOMIC_BLOCK(NVIC_PRIO_SERIALUART1) { + if (pos >= mspRxBuffer.len) { + mspRxBuffer.len = 0; + return requestHandled; + } + } + } + return requestHandled; +} +#endif + static void crsfInitializeFrame(sbuf_t *dst) { crsfCrc = 0; @@ -132,7 +191,7 @@ CRSF frame has the structure: Device address: (uint8_t) Frame length: length in bytes including Type (uint8_t) Type: (uint8_t) -CRC: (uint8_t) +CRC: (uint8_t), crc of and */ /* @@ -142,7 +201,7 @@ int32_t Latitude ( degree / 10`000`000 ) int32_t Longitude (degree / 10`000`000 ) uint16_t Groundspeed ( km/h / 10 ) uint16_t GPS heading ( degree / 100 ) -uint16 Altitude ( meter ­ 1000m offset ) +uint16 Altitude ( meter ­1000m offset ) uint8_t Satellites in use ( counter ) */ void crsfFrameGps(sbuf_t *dst) @@ -228,34 +287,127 @@ char[] Flight mode ( Null­terminated string ) */ void crsfFrameFlightMode(sbuf_t *dst) { - // just do Angle for the moment as a placeholder + // just do "OK" for the moment as a placeholder // write zero for frame length, since we don't know it yet uint8_t *lengthPtr = sbufPtr(dst); sbufWriteU8(dst, 0); crsfSerialize8(dst, CRSF_FRAMETYPE_FLIGHT_MODE); - // use same logic as OSD, so telemetry displays same flight text as OSD - const char *flightMode = "ACRO"; - if (FLIGHT_MODE(FAILSAFE_MODE)) { - flightMode = "!FS"; - } else if (FLIGHT_MODE(ANGLE_MODE)) { - flightMode = "ANGL"; - } else if (FLIGHT_MODE(HORIZON_MODE)) { - flightMode = "HOR"; + // use same logic as OSD, so telemetry displays same flight text as OSD when armed + const char *flightMode = "OK"; + if (ARMING_FLAG(ARMED)) { + if (isAirmodeActive()) { + flightMode = "AIR"; + } else { + flightMode = "ACRO"; + } + if (FLIGHT_MODE(FAILSAFE_MODE)) { + flightMode = "!FS!"; + } else if (ARMING_FLAG(ARMED) && IS_RC_MODE_ACTIVE(BOXHOMERESET) && !FLIGHT_MODE(NAV_RTH_MODE) && !FLIGHT_MODE(NAV_WP_MODE)) { + flightMode = "HRST"; + } else if (FLIGHT_MODE(MANUAL_MODE)) { + flightMode = "MANU"; + } else if (FLIGHT_MODE(NAV_RTH_MODE)) { + flightMode = "RTH"; + } else if (FLIGHT_MODE(NAV_POSHOLD_MODE)) { + flightMode = "HOLD"; + } else if (FLIGHT_MODE(NAV_CRUISE_MODE) && FLIGHT_MODE(NAV_ALTHOLD_MODE)) { + flightMode = "3CRS"; + } else if (FLIGHT_MODE(NAV_CRUISE_MODE)) { + flightMode = "CRS"; + } else if (FLIGHT_MODE(NAV_ALTHOLD_MODE)) { + flightMode = "AH"; + } else if (FLIGHT_MODE(NAV_WP_MODE)) { + flightMode = "WP"; + } else if (FLIGHT_MODE(ANGLE_MODE)) { + flightMode = "ANGL"; + } else if (FLIGHT_MODE(HORIZON_MODE)) { + flightMode = "HOR"; + } +#ifdef USE_GPS + } else if (feature(FEATURE_GPS) && navConfig()->general.flags.extra_arming_safety && (!STATE(GPS_FIX) || !STATE(GPS_FIX_HOME))) { + flightMode = "WAIT"; // Waiting for GPS lock +#endif + } else if (isArmingDisabled()) { + flightMode = "!ERR"; } + crsfSerializeData(dst, (const uint8_t*)flightMode, strlen(flightMode)); crsfSerialize8(dst, 0); // zero terminator for string // write in the length *lengthPtr = sbufPtr(dst) - lengthPtr; } +/* +0x29 Device Info +Payload: +uint8_t Destination +uint8_t Origin +char[] Device Name ( Null terminated string ) +uint32_t Null Bytes +uint32_t Null Bytes +uint32_t Null Bytes +uint8_t 255 (Max MSP Parameter) +uint8_t 0x01 (Parameter version 1) +*/ +void crsfFrameDeviceInfo(sbuf_t *dst) { + + char buff[30]; + tfp_sprintf(buff, "%s %s: %s", FC_FIRMWARE_NAME, FC_VERSION_STRING, TARGET_BOARD_IDENTIFIER); + + uint8_t *lengthPtr = sbufPtr(dst); + sbufWriteU8(dst, 0); + crsfSerialize8(dst, CRSF_FRAMETYPE_DEVICE_INFO); + crsfSerialize8(dst, CRSF_ADDRESS_RADIO_TRANSMITTER); + crsfSerialize8(dst, CRSF_ADDRESS_FLIGHT_CONTROLLER); + crsfSerializeData(dst, (const uint8_t*)buff, strlen(buff)); + crsfSerialize8(dst, 0); // zero terminator for string + for (unsigned int ii=0; ii<12; ii++) { + crsfSerialize8(dst, 0x00); + } + crsfSerialize8(dst, CRSF_DEVICEINFO_PARAMETER_COUNT); + crsfSerialize8(dst, CRSF_DEVICEINFO_VERSION); + *lengthPtr = sbufPtr(dst) - lengthPtr; +} + #define BV(x) (1 << (x)) // bit value // schedule array to decide how often each type of frame is sent -#define CRSF_SCHEDULE_COUNT_MAX 5 +typedef enum { + CRSF_FRAME_START_INDEX = 0, + CRSF_FRAME_ATTITUDE_INDEX = CRSF_FRAME_START_INDEX, + CRSF_FRAME_BATTERY_SENSOR_INDEX, + CRSF_FRAME_FLIGHT_MODE_INDEX, + CRSF_FRAME_GPS_INDEX, + CRSF_SCHEDULE_COUNT_MAX +} crsfFrameTypeIndex_e; + static uint8_t crsfScheduleCount; static uint8_t crsfSchedule[CRSF_SCHEDULE_COUNT_MAX]; +#if defined(USE_MSP_OVER_TELEMETRY) + +static bool mspReplyPending; + +void crsfScheduleMspResponse(void) +{ + mspReplyPending = true; +} + +void crsfSendMspResponse(uint8_t *payload) +{ + sbuf_t crsfPayloadBuf; + sbuf_t *dst = &crsfPayloadBuf; + + crsfInitializeFrame(dst); + sbufWriteU8(dst, CRSF_FRAME_TX_MSP_FRAME_SIZE + CRSF_FRAME_LENGTH_EXT_TYPE_CRC); + crsfSerialize8(dst, CRSF_FRAMETYPE_MSP_RESP); + crsfSerialize8(dst, CRSF_ADDRESS_RADIO_TRANSMITTER); + crsfSerialize8(dst, CRSF_ADDRESS_FLIGHT_CONTROLLER); + crsfSerializeData(dst, (const uint8_t*)payload, CRSF_FRAME_TX_MSP_FRAME_SIZE); + crsfFinalize(dst); +} +#endif static void processCrsf(void) { @@ -265,23 +417,23 @@ static void processCrsf(void) sbuf_t crsfPayloadBuf; sbuf_t *dst = &crsfPayloadBuf; - if (currentSchedule & BV(CRSF_FRAME_ATTITUDE)) { + if (currentSchedule & BV(CRSF_FRAME_ATTITUDE_INDEX)) { crsfInitializeFrame(dst); crsfFrameAttitude(dst); crsfFinalize(dst); } - if (currentSchedule & BV(CRSF_FRAME_BATTERY_SENSOR)) { + if (currentSchedule & BV(CRSF_FRAME_BATTERY_SENSOR_INDEX)) { crsfInitializeFrame(dst); crsfFrameBatterySensor(dst); crsfFinalize(dst); } - if (currentSchedule & BV(CRSF_FRAME_FLIGHT_MODE)) { + if (currentSchedule & BV(CRSF_FRAME_FLIGHT_MODE_INDEX)) { crsfInitializeFrame(dst); crsfFrameFlightMode(dst); crsfFinalize(dst); } #ifdef USE_GPS - if (currentSchedule & BV(CRSF_FRAME_GPS)) { + if (currentSchedule & BV(CRSF_FRAME_GPS_INDEX)) { crsfInitializeFrame(dst); crsfFrameGps(dst); crsfFinalize(dst); @@ -290,17 +442,28 @@ static void processCrsf(void) crsfScheduleIndex = (crsfScheduleIndex + 1) % crsfScheduleCount; } +void crsfScheduleDeviceInfoResponse(void) +{ + deviceInfoReplyPending = true; +} + void initCrsfTelemetry(void) { // check if there is a serial port open for CRSF telemetry (ie opened by the CRSF RX) // and feature is enabled, if so, set CRSF telemetry enabled crsfTelemetryEnabled = crsfRxIsActive(); + + deviceInfoReplyPending = false; +#if defined(USE_MSP_OVER_TELEMETRY) + mspReplyPending = false; +#endif + int index = 0; - crsfSchedule[index++] = BV(CRSF_FRAME_ATTITUDE); - crsfSchedule[index++] = BV(CRSF_FRAME_BATTERY_SENSOR); - crsfSchedule[index++] = BV(CRSF_FRAME_FLIGHT_MODE); + crsfSchedule[index++] = BV(CRSF_FRAME_ATTITUDE_INDEX); + crsfSchedule[index++] = BV(CRSF_FRAME_BATTERY_SENSOR_INDEX); + crsfSchedule[index++] = BV(CRSF_FRAME_FLIGHT_MODE_INDEX); if (feature(FEATURE_GPS)) { - crsfSchedule[index++] = BV(CRSF_FRAME_GPS); + crsfSchedule[index++] = BV(CRSF_FRAME_GPS_INDEX); } crsfScheduleCount = (uint8_t)index; @@ -326,8 +489,29 @@ void handleCrsfTelemetry(timeUs_t currentTimeUs) // in between the RX frames. crsfRxSendTelemetryData(); + // Send ad-hoc response frames as soon as possible +#if defined(USE_MSP_OVER_TELEMETRY) + if (mspReplyPending) { + mspReplyPending = handleCrsfMspFrameBuffer(CRSF_FRAME_TX_MSP_FRAME_SIZE, &crsfSendMspResponse); + crsfLastCycleTime = currentTimeUs; // reset telemetry timing due to ad-hoc request + return; + } +#endif + + if (deviceInfoReplyPending) { + sbuf_t crsfPayloadBuf; + sbuf_t *dst = &crsfPayloadBuf; + crsfInitializeFrame(dst); + crsfFrameDeviceInfo(dst); + crsfFinalize(dst); + deviceInfoReplyPending = false; + crsfLastCycleTime = currentTimeUs; // reset telemetry timing due to ad-hoc request + return; + } + // Actual telemetry data only needs to be sent at a low frequency, ie 10Hz - if (currentTimeUs >= crsfLastCycleTime + CRSF_CYCLETIME_US) { + // Spread out scheduled frames evenly so each frame is sent at the same frequency. + if (currentTimeUs >= crsfLastCycleTime + (CRSF_CYCLETIME_US / crsfScheduleCount)) { crsfLastCycleTime = currentTimeUs; processCrsf(); } @@ -341,17 +525,17 @@ int getCrsfFrame(uint8_t *frame, crsfFrameType_e frameType) crsfInitializeFrame(sbuf); switch (frameType) { default: - case CRSF_FRAME_ATTITUDE: + case CRSF_FRAMETYPE_ATTITUDE: crsfFrameAttitude(sbuf); break; - case CRSF_FRAME_BATTERY_SENSOR: + case CRSF_FRAMETYPE_BATTERY_SENSOR: crsfFrameBatterySensor(sbuf); break; - case CRSF_FRAME_FLIGHT_MODE: + case CRSF_FRAMETYPE_FLIGHT_MODE: crsfFrameFlightMode(sbuf); break; #if defined(USE_GPS) - case CRSF_FRAME_GPS: + case CRSF_FRAMETYPE_GPS: crsfFrameGps(sbuf); break; #endif diff --git a/src/main/telemetry/crsf.h b/src/main/telemetry/crsf.h index 980467f517b..6ffa4df3588 100755 --- a/src/main/telemetry/crsf.h +++ b/src/main/telemetry/crsf.h @@ -18,17 +18,18 @@ #pragma once #include "common/time.h" +#include "rx/crsf.h" -typedef enum { - CRSF_FRAME_START = 0, - CRSF_FRAME_ATTITUDE = CRSF_FRAME_START, - CRSF_FRAME_BATTERY_SENSOR, - CRSF_FRAME_FLIGHT_MODE, - CRSF_FRAME_GPS -} crsfFrameType_e; +#define CRSF_MSP_RX_BUF_SIZE 128 +#define CRSF_MSP_TX_BUF_SIZE 128 void initCrsfTelemetry(void); bool checkCrsfTelemetryState(void); void handleCrsfTelemetry(timeUs_t currentTimeUs); - +void crsfScheduleDeviceInfoResponse(void); +void crsfScheduleMspResponse(void); int getCrsfFrame(uint8_t *frame, crsfFrameType_e frameType); +#if defined(USE_MSP_OVER_TELEMETRY) +void initCrsfMspBuffer(void); +bool bufferCrsfMspFrame(uint8_t *frameStart, int frameLength); +#endif \ No newline at end of file diff --git a/src/main/telemetry/ibus_shared.c b/src/main/telemetry/ibus_shared.c index aefbd676701..644ea8e46a9 100644 --- a/src/main/telemetry/ibus_shared.c +++ b/src/main/telemetry/ibus_shared.c @@ -36,6 +36,7 @@ #include "io/serial.h" #include "sensors/barometer.h" +#include "sensors/temperature.h" #include "sensors/acceleration.h" #include "sensors/battery.h" #include "sensors/sensors.h" @@ -140,15 +141,10 @@ static uint8_t dispatchMeasurementRequest(ibusAddress_t address) { } #endif if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_TEMPERATURE) { //BARO_TEMP\GYRO_TEMP - if (sensors(SENSOR_BARO)) return sendIbusMeasurement2(address, (uint16_t) ((baro.baroTemperature + 50) / 10 + IBUS_TEMPERATURE_OFFSET)); //int32_t - else { - /* - * There is no temperature data - * assuming (baro.baroTemperature + 50) / 10 - * 0 degrees (no sensor) equals 50 / 10 = 5 - */ - return sendIbusMeasurement2(address, (uint16_t) (5 + IBUS_TEMPERATURE_OFFSET)); //int16_t - } + int16_t temperature; + const bool temp_valid = sensors(SENSOR_BARO) ? getBaroTemperature(&temperature) : getIMUTemperature(&temperature); + if (!temp_valid || (temperature < -400)) temperature = -400; // Minimum reported temperature is -40°C + return sendIbusMeasurement2(address, (uint16_t)((temperature + 50) / 10 + IBUS_TEMPERATURE_OFFSET)); } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_RPM) { return sendIbusMeasurement2(address, (uint16_t) (rcCommand[THROTTLE])); } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_EXTERNAL_VOLTAGE) { //VBAT diff --git a/src/main/telemetry/msp_shared.h b/src/main/telemetry/msp_shared.h index b8aa69d4168..fad6b912d39 100644 --- a/src/main/telemetry/msp_shared.h +++ b/src/main/telemetry/msp_shared.h @@ -17,16 +17,12 @@ typedef struct mspPackage_s { typedef union mspRxBuffer_u { uint8_t smartPortMspRxBuffer[SMARTPORT_MSP_RX_BUF_SIZE]; -#if 0 uint8_t crsfMspRxBuffer[CRSF_MSP_RX_BUF_SIZE]; -#endif } mspRxBuffer_t; typedef union mspTxBuffer_u { uint8_t smartPortMspTxBuffer[SMARTPORT_MSP_TX_BUF_SIZE]; -#if 0 uint8_t crsfMspTxBuffer[CRSF_MSP_TX_BUF_SIZE]; -#endif } mspTxBuffer_t; void initSharedMsp(void); diff --git a/src/main/vcp_hal/usbd_cdc_interface.c b/src/main/vcp_hal/usbd_cdc_interface.c index fd9388f7d27..26cf9e9c881 100644 --- a/src/main/vcp_hal/usbd_cdc_interface.c +++ b/src/main/vcp_hal/usbd_cdc_interface.c @@ -56,8 +56,8 @@ /* Private typedef -----------------------------------------------------------*/ /* Private define ------------------------------------------------------------*/ -#define APP_RX_DATA_SIZE 2048 -#define APP_TX_DATA_SIZE 2048 +#define APP_RX_DATA_SIZE 4096 +#define APP_TX_DATA_SIZE 4096 /* Private macro -------------------------------------------------------------*/ /* Private variables ---------------------------------------------------------*/ diff --git a/src/main/vcpf4/usbd_cdc_vcp.c b/src/main/vcpf4/usbd_cdc_vcp.c index b9cdab30e6b..2fc2da8b1de 100644 --- a/src/main/vcpf4/usbd_cdc_vcp.c +++ b/src/main/vcpf4/usbd_cdc_vcp.c @@ -63,6 +63,7 @@ static void *ctrlLineStateCbContext; static void (*baudRateCb)(void *context, uint32_t baud); static void *baudRateCbContext; +__ALIGN_BEGIN USB_OTG_CORE_HANDLE USB_OTG_dev __ALIGN_END; CDC_IF_Prop_TypeDef VCP_fops = {VCP_Init, VCP_DeInit, VCP_Ctrl, VCP_DataTx, VCP_DataRx }; diff --git a/src/main/vcpf4/usbd_cdc_vcp.h b/src/main/vcpf4/usbd_cdc_vcp.h index 0a046ad1b70..fc27a6d98ee 100644 --- a/src/main/vcpf4/usbd_cdc_vcp.h +++ b/src/main/vcpf4/usbd_cdc_vcp.h @@ -34,7 +34,7 @@ #include "usbd_usr.h" #include "usbd_desc.h" -__ALIGN_BEGIN USB_OTG_CORE_HANDLE USB_OTG_dev __ALIGN_END; +extern USB_OTG_CORE_HANDLE USB_OTG_dev; uint32_t CDC_Send_DATA(const uint8_t *ptrBuffer, uint32_t sendLength); uint32_t CDC_Send_FreeBytes(void); diff --git a/src/test/Makefile b/src/test/Makefile index 405cb814c93..6ce38c4ae61 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -105,6 +105,15 @@ $(OBJECT_DIR)/common/maths.o : \ $(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/common/maths.c -o $@ +$(OBJECT_DIR)/common/calibration.o : \ + $(USER_DIR)/common/calibration.c \ + $(USER_DIR)/common/calibration.h \ + $(GTEST_HEADERS) + + @mkdir -p $(dir $@) + $(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/common/calibration.c -o $@ + + $(OBJECT_DIR)/common/bitarray.o : \ $(USER_DIR)/common/bitarray.c \ $(USER_DIR)/common/bitarray.h \ @@ -639,6 +648,7 @@ $(OBJECT_DIR)/sensor_gyro_unittest.o : \ $(OBJECT_DIR)/sensor_gyro_unittest : \ $(OBJECT_DIR)/build/debug.o \ $(OBJECT_DIR)/common/maths.o \ + $(OBJECT_DIR)/common/calibration.o \ $(OBJECT_DIR)/common/filter.o \ $(OBJECT_DIR)/drivers/accgyro/accgyro_fake.o \ $(OBJECT_DIR)/sensors/gyro.o \ @@ -730,6 +740,26 @@ $(OBJECT_DIR)/bitarray_unittest : \ $(CXX) $(CXX_FLAGS) $^ -o $(OBJECT_DIR)/$@ +$(OBJECT_DIR)/common/olc.o : $(USER_DIR)/common/olc.c $(USER_DIR)/common/olc.h $(GTEST_HEADERS) + @mkdir -p $(dir $@) + $(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/common/olc.c -o $@ + +$(OBJECT_DIR)/olc_unittest.o : \ + $(TEST_DIR)/olc_unittest.cc \ + $(USER_DIR)/common/olc.h \ + $(GTEST_HEADERS) + + @mkdir -p $(dir $@) + $(CXX) $(CXX_FLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/olc_unittest.cc -o $@ + +$(OBJECT_DIR)/olc_unittest : \ + $(OBJECT_DIR)/common/olc.o \ + $(OBJECT_DIR)/olc_unittest.o \ + $(OBJECT_DIR)/gtest_main.a + + $(CXX) $(CXX_FLAGS) $^ -o $(OBJECT_DIR)/$@ + + test: $(TESTS:%=test-%) diff --git a/src/test/unit/bitarray_unittest.cc b/src/test/unit/bitarray_unittest.cc index eca539e800c..6375d828ae9 100644 --- a/src/test/unit/bitarray_unittest.cc +++ b/src/test/unit/bitarray_unittest.cc @@ -59,9 +59,27 @@ TEST(BitArrayTest, TestFind) EXPECT_EQ(bitArrayFindFirstSet(p, 16, sizeof(p)), 44); EXPECT_EQ(bitArrayFindFirstSet(p, 17, sizeof(p)), 44); EXPECT_EQ(bitArrayFindFirstSet(p, 18, sizeof(p)), 44); + EXPECT_EQ(bitArrayFindFirstSet(p, 43, sizeof(p)), 44); + EXPECT_EQ(bitArrayFindFirstSet(p, 44, sizeof(p)), 44); EXPECT_EQ(bitArrayFindFirstSet(p, 45, sizeof(p)), -1); bitArrayClr(p, 44); EXPECT_EQ(bitArrayFindFirstSet(p, 0, sizeof(p)), -1); EXPECT_EQ(bitArrayFindFirstSet(p, 64, sizeof(p)), -1); } + +TEST(BitArrayTest, TestSetClrAll) +{ + const int bits = 32 * 4; + + BITARRAY_DECLARE(p, bits); + BITARRAY_CLR_ALL(p); + + EXPECT_EQ(-1, BITARRAY_FIND_FIRST_SET(p, 0)); + + BITARRAY_SET_ALL(p); + + for (int ii = 0; ii < bits; ii++) { + EXPECT_EQ(ii, BITARRAY_FIND_FIRST_SET(p, ii)); + } +} diff --git a/src/test/unit/flight_failsafe_unittest.cc.txt b/src/test/unit/flight_failsafe_unittest.cc.txt index cee78f66536..21a97172611 100644 --- a/src/test/unit/flight_failsafe_unittest.cc.txt +++ b/src/test/unit/flight_failsafe_unittest.cc.txt @@ -410,7 +410,7 @@ int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; uint8_t armingFlags; int16_t rcCommand[4]; uint32_t rcModeActivationMask; -int16_t debug[DEBUG16_VALUE_COUNT]; +int32_t debug[DEBUG32_VALUE_COUNT]; bool isUsingSticksToArm = true; diff --git a/src/test/unit/flight_imu_unittest.cc.txt b/src/test/unit/flight_imu_unittest.cc.txt index df913ebdb5e..28886c89ff9 100644 --- a/src/test/unit/flight_imu_unittest.cc.txt +++ b/src/test/unit/flight_imu_unittest.cc.txt @@ -135,7 +135,7 @@ int16_t heading; gyro_t gyro; int32_t magADC[XYZ_AXIS_COUNT]; int32_t BaroAlt; -int16_t debug[DEBUG16_VALUE_COUNT]; +int32_t debug[DEBUG32_VALUE_COUNT]; uint8_t stateFlags; uint16_t flightModeFlags; @@ -179,4 +179,4 @@ bool isBaroCalibrationComplete(void) { return true; } void performBaroCalibrationCycle(void) {} int32_t baroCalculateAltitude(void) { return 0; } -} \ No newline at end of file +} diff --git a/src/test/unit/flight_mixer_unittest.cc.txt b/src/test/unit/flight_mixer_unittest.cc.txt index 6909f4f95bd..6b33b6c50c5 100644 --- a/src/test/unit/flight_mixer_unittest.cc.txt +++ b/src/test/unit/flight_mixer_unittest.cc.txt @@ -373,7 +373,7 @@ int16_t rcCommand[4]; int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; uint32_t rcModeActivationMask; -int16_t debug[DEBUG16_VALUE_COUNT]; +int32_t debug[DEBUG32_VALUE_COUNT]; uint8_t stateFlags; uint16_t flightModeFlags; diff --git a/src/test/unit/maths_unittest.cc b/src/test/unit/maths_unittest.cc index 77779d366e2..538f28086af 100644 --- a/src/test/unit/maths_unittest.cc +++ b/src/test/unit/maths_unittest.cc @@ -128,28 +128,28 @@ void expectVectorsAreEqual(fpVector3_t *a, fpVector3_t *b) TEST(MathsUnittest, TestRotateVectorWithNoAngle) { - fpVector3_t vector = { .x = 1.0f, .y = 0.0f, .z = 0.0f}; + fpVector3_t vector = { 1.0f, 0.0f, 0.0f}; fp_angles_t euler_angles = {.raw={0.0f, 0.0f, 0.0f}}; fpMat3_t rmat; rotationMatrixFromAngles(&rmat, &euler_angles); rotationMatrixRotateVector(&vector, &vector, &rmat); - fpVector3_t expected_result = { .x = 1.0f, .y = 0.0f, .z = 0.0f}; + fpVector3_t expected_result = { 1.0f, 0.0f, 0.0f}; expectVectorsAreEqual(&vector, &expected_result); } TEST(MathsUnittest, TestRotateVectorAroundAxis) { // Rotate a vector <1, 0, 0> around an each axis x y and z. - fpVector3_t vector = { .x = 1.0f, .y = 0.0f, .z = 0.0f}; + fpVector3_t vector = { 1.0f, 0.0f, 0.0f}; fp_angles_t euler_angles = {.raw={90.0f, 0.0f, 0.0f}}; fpMat3_t rmat; rotationMatrixFromAngles(&rmat, &euler_angles); rotationMatrixRotateVector(&vector, &vector, &rmat); - fpVector3_t expected_result = { .x = 1.0f, .y = 0.0f, .z = 0.0f}; + fpVector3_t expected_result = { 1.0f, 0.0f, 0.0f}; expectVectorsAreEqual(&vector, &expected_result); } diff --git a/src/test/unit/navigation_unittest.cc.txt b/src/test/unit/navigation_unittest.cc.txt index e6f0b6dd89f..10472283f48 100755 --- a/src/test/unit/navigation_unittest.cc.txt +++ b/src/test/unit/navigation_unittest.cc.txt @@ -134,7 +134,7 @@ int32_t accSum[XYZ_AXIS_COUNT]; int16_t accADC[XYZ_AXIS_COUNT]; int16_t gyroADC[XYZ_AXIS_COUNT]; int16_t magADC[XYZ_AXIS_COUNT]; -int16_t debug[DEBUG16_VALUE_COUNT]; +int32_t debug[DEBUG32_VALUE_COUNT]; uint8_t stateFlags; uint16_t flightModeFlags; diff --git a/src/test/unit/olc_unittest.cc b/src/test/unit/olc_unittest.cc new file mode 100644 index 00000000000..d930daad621 --- /dev/null +++ b/src/test/unit/olc_unittest.cc @@ -0,0 +1,69 @@ +#include + +extern "C" { +#include "common/olc.h" +#include "common/utils.h" +} + +#include "gtest/gtest.h" + +using std::string; + +struct EncodeCase { + string result; + double lat; + double lon; + + int length(); +}; + +int EncodeCase::length() +{ + int n = 0; + for (size_t ii = 0; ii < this->result.length(); ii++) { + if (result[ii] == '0') { + break; + } + if (result[ii] != '+') { + n++; + } + } + return n; +} + +// Tests cases from https://github.com/google/open-location-code/blob/master/test_data/encodingTests.csv +struct EncodeCase encodeCases[] = { + {"7FG49Q00+", 20.375, 2.775}, + {"7FG49QCJ+2V", 20.3700625, 2.7821875}, + {"7FG49QCJ+2VX", 20.3701125, 2.782234375}, + {"7FG49QCJ+2VXGJ", 20.3701135, 2.78223535156}, + {"8FVC2222+22", 47.0000625, 8.0000625}, + {"4VCPPQGP+Q9", -41.2730625, 174.7859375}, + {"62G20000+", 0.5, -179.5}, + {"22220000+", -89.5, -179.5}, + {"7FG40000+", 20.5, 2.5}, + {"22222222+22", -89.9999375, -179.9999375}, + {"6VGX0000+", 0.5, 179.5}, + {"6FH32222+222", 1, 1}, + // Special cases over 90 latitude and 180 longitude + {"CFX30000+", 90, 1}, + {"CFX30000+", 92, 1}, + {"62H20000+", 1, 180}, + {"62H30000+", 1, 181}, + {"CFX3X2X2+X2", 90, 1}, + // Test non-precise latitude/longitude value + {"6FH56C22+22", 1.2, 3.4}, +}; + +TEST(OLCTest, TestEncode) +{ + char buf[20]; + + for (unsigned ii = 0; ii < ARRAYLEN(encodeCases); ii++) { + struct EncodeCase c = encodeCases[ii]; + int32_t lat = c.lat * OLC_DEG_MULTIPLIER; + int32_t lon = c.lon * OLC_DEG_MULTIPLIER; + EXPECT_GT(olc_encode(lat, lon, c.length(), buf, sizeof(buf)), 0); + EXPECT_EQ(c.result, (string)buf); + } +} diff --git a/src/test/unit/sensor_gyro_unittest.cc b/src/test/unit/sensor_gyro_unittest.cc index d86448bde99..52168e14cd3 100644 --- a/src/test/unit/sensor_gyro_unittest.cc +++ b/src/test/unit/sensor_gyro_unittest.cc @@ -28,6 +28,7 @@ extern "C" { #include "build/debug.h" #include "common/axis.h" #include "common/maths.h" + #include "common/calibration.h" #include "common/utils.h" #include "drivers/accgyro/accgyro_fake.h" #include "drivers/logging_codes.h" @@ -37,17 +38,11 @@ extern "C" { #include "sensors/acceleration.h" #include "sensors/sensors.h" - typedef struct gyroCalibration_s { - int32_t g[XYZ_AXIS_COUNT]; - stdev_t var[XYZ_AXIS_COUNT]; - uint16_t calibratingG; - } gyroCalibration_t; - extern gyroCalibration_t gyroCalibration; + extern zeroCalibrationVector_t gyroCalibration; extern gyroDev_t gyroDev0; STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHardware); - STATIC_UNIT_TESTED void performGyroCalibration(gyroDev_t *dev, gyroCalibration_t *gyroCalibration, uint8_t gyroMovementCalibrationThreshold); - STATIC_UNIT_TESTED bool isCalibrationComplete(gyroCalibration_t *gyroCalibration); + STATIC_UNIT_TESTED void performGyroCalibration(gyroDev_t *dev, zeroCalibrationVector_t *gyroCalibration); } #include "unittest_macros.h" @@ -81,7 +76,7 @@ TEST(SensorGyro, Read) TEST(SensorGyro, Calibrate) { - gyroSetCalibrationCycles(CALIBRATING_GYRO_CYCLES); + gyroStartCalibration(); gyroInit(); fakeGyroSet(5, 6, 7); const bool read = gyroDev0.readFn(&gyroDev0); @@ -89,17 +84,16 @@ TEST(SensorGyro, Calibrate) EXPECT_EQ(5, gyroDev0.gyroADCRaw[X]); EXPECT_EQ(6, gyroDev0.gyroADCRaw[Y]); EXPECT_EQ(7, gyroDev0.gyroADCRaw[Z]); - static const int gyroMovementCalibrationThreshold = 32; gyroDev0.gyroZero[X] = 8; gyroDev0.gyroZero[Y] = 9; gyroDev0.gyroZero[Z] = 10; - performGyroCalibration(&gyroDev0, &gyroCalibration, gyroMovementCalibrationThreshold); + performGyroCalibration(&gyroDev0, &gyroCalibration); EXPECT_EQ(0, gyroDev0.gyroZero[X]); EXPECT_EQ(0, gyroDev0.gyroZero[Y]); EXPECT_EQ(0, gyroDev0.gyroZero[Z]); - EXPECT_EQ(false, isCalibrationComplete(&gyroCalibration)); - while (!isCalibrationComplete(&gyroCalibration)) { - performGyroCalibration(&gyroDev0, &gyroCalibration, gyroMovementCalibrationThreshold); + EXPECT_EQ(false, gyroIsCalibrationComplete()); + while (!gyroIsCalibrationComplete()) { + performGyroCalibration(&gyroDev0, &gyroCalibration); } EXPECT_EQ(5, gyroDev0.gyroZero[X]); EXPECT_EQ(6, gyroDev0.gyroZero[Y]); @@ -108,29 +102,29 @@ TEST(SensorGyro, Calibrate) TEST(SensorGyro, Update) { - gyroSetCalibrationCycles(CALIBRATING_GYRO_CYCLES); - EXPECT_EQ(false, isCalibrationComplete(&gyroCalibration)); + gyroStartCalibration(); + EXPECT_EQ(false, gyroIsCalibrationComplete()); gyroInit(); fakeGyroSet(5, 6, 7); - gyroUpdate(0); - EXPECT_EQ(false, isCalibrationComplete(&gyroCalibration)); - while (!isCalibrationComplete(&gyroCalibration)) { - gyroUpdate(0); + gyroUpdate(); + EXPECT_EQ(false, gyroIsCalibrationComplete()); + while (!gyroIsCalibrationComplete()) { + gyroUpdate(); } - EXPECT_EQ(true, isCalibrationComplete(&gyroCalibration)); + EXPECT_EQ(true, gyroIsCalibrationComplete()); EXPECT_EQ(5, gyroDev0.gyroZero[X]); EXPECT_EQ(6, gyroDev0.gyroZero[Y]); EXPECT_EQ(7, gyroDev0.gyroZero[Z]); EXPECT_FLOAT_EQ(0, gyro.gyroADCf[X]); EXPECT_FLOAT_EQ(0, gyro.gyroADCf[Y]); EXPECT_FLOAT_EQ(0, gyro.gyroADCf[Z]); - gyroUpdate(0); + gyroUpdate(); // expect zero values since gyro is calibrated EXPECT_FLOAT_EQ(0, gyro.gyroADCf[X]); EXPECT_FLOAT_EQ(0, gyro.gyroADCf[Y]); EXPECT_FLOAT_EQ(0, gyro.gyroADCf[Z]); fakeGyroSet(15, 26, 97); - gyroUpdate(0); + gyroUpdate(); EXPECT_FLOAT_EQ(10 * gyroDev0.scale, gyro.gyroADCf[X]); // gyroADCf values are scaled EXPECT_FLOAT_EQ(20 * gyroDev0.scale, gyro.gyroADCf[Y]); EXPECT_FLOAT_EQ(90 * gyroDev0.scale, gyro.gyroADCf[Z]); @@ -140,13 +134,15 @@ TEST(SensorGyro, Update) // STUBS extern "C" { +static timeMs_t milliTime = 0; +timeMs_t millis(void) {return milliTime++;} uint32_t micros(void) {return 0;} void beeper(beeperMode_e) {} uint8_t detectedSensors[] = { GYRO_NONE, ACC_NONE }; void addBootlogEvent6(bootLogEventCode_e eventCode, uint16_t eventFlags, uint16_t param1, uint16_t param2, uint16_t param3, uint16_t param4) {UNUSED(eventCode);UNUSED(eventFlags);UNUSED(param1);UNUSED(param2);UNUSED(param3);UNUSED(param4);} -timeDelta_t getGyroUpdateRate(void) {return gyro.targetLooptime;} +timeDelta_t getLooptime(void) {return gyro.targetLooptime;} void sensorsSet(uint32_t) {} void schedulerResetTaskStatistics(cfTaskId_e) {} } diff --git a/src/test/unit/serial_msp_unittest.cc.txt b/src/test/unit/serial_msp_unittest.cc.txt index 2ffb7e558c8..14d75c9c179 100644 --- a/src/test/unit/serial_msp_unittest.cc.txt +++ b/src/test/unit/serial_msp_unittest.cc.txt @@ -511,7 +511,7 @@ void featureSet(uint32_t mask) {UNUSED(mask);} void featureClearAll() {} uint32_t featureMask(void) {return 0;} // from debug.c -int16_t debug[DEBUG16_VALUE_COUNT]; +int32_t debug[DEBUG32_VALUE_COUNT]; // from gps.c #define GPS_SV_MAXSATS 16 int32_t GPS_coord[2]; // LAT/LON diff --git a/src/test/unit/telemetry_hott_unittest.cc b/src/test/unit/telemetry_hott_unittest.cc index fadbee50c97..3e643b5f6fb 100644 --- a/src/test/unit/telemetry_hott_unittest.cc +++ b/src/test/unit/telemetry_hott_unittest.cc @@ -163,7 +163,7 @@ TEST(TelemetryHottTest, PrepareGPSMessage_Altitude1m) extern "C" { -int16_t debug[DEBUG16_VALUE_COUNT]; +int32_t debug[DEBUG32_VALUE_COUNT]; uint32_t stateFlags; diff --git a/src/utils/scan_target_headers.py b/src/utils/scan_target_headers.py new file mode 100755 index 00000000000..4f8473fa51d --- /dev/null +++ b/src/utils/scan_target_headers.py @@ -0,0 +1,48 @@ +#!/usr/bin/env python3 + +""" +This file is part of Cleanflight. + +Cleanflight is free software: you can redistribute it and/or modify +it under the terms of the GNU General Public License as published by +the Free Software Foundation, either version 3 of the License, or +(at your option) any later version. + +Cleanflight is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +GNU General Public License for more details. + +You should have received a copy of the GNU General Public License +along with Cleanflight. If not, see . +""" + +import os +import operator +import glob + +if __name__ == '__main__': + path = os.path.join(os.path.dirname(__file__), '..', + 'main', 'target', '*', 'target.h') + files = glob.glob(path) + defines = {} + + for file in files: + target = os.path.basename(os.path.dirname(file)) + with open(file, 'r') as handle: + for line in handle: + line = line.strip() + if line.startswith('#define'): + try: + line = line.replace('\t', ' ') + define = line.split(' ')[1].strip() + if define not in defines: + defines[define] = [] + defines[define].append(target) + except IndexError: + pass + + counts = {k: len(v) for k, v in defines.items()} + + for define, count in sorted(counts.items(), key=operator.itemgetter(1)): + print("{}\t{}\t{}".format(define, count, ', '.join(sorted(defines[define])))) diff --git a/src/utils/settings.rb b/src/utils/settings.rb index a3c30483ee5..77955523088 100644 --- a/src/utils/settings.rb +++ b/src/utils/settings.rb @@ -37,6 +37,8 @@ DEBUG = false INFO = false +SETTINGS_WORDS_BITS_PER_CHAR = 5 + def dputs(s) puts s if DEBUG end @@ -77,6 +79,7 @@ def to_carr class NameEncoder attr_reader :max_length + attr_reader :max_word_length def initialize(names, max_length) @names = names @@ -91,6 +94,8 @@ def initialize(names, max_length) # Key is the name, value is its encoding @encoded = Hash.new + @max_word_length = 0; + update_words encode_names end @@ -105,10 +110,14 @@ def words def estimated_size(settings_count) size = 0 + @max_word_length = 0 @words.each do |word, count| - size += word.length + 1 + size += (word.length + 1) * (5/8.0) + if word.length > @max_word_length + @max_word_length = word.length + end end - return size + @max_length * settings_count + return size.to_i + @max_length * settings_count end def format_encoded_name(name) @@ -329,11 +338,11 @@ def print_stats puts "each setting name uses #{@name_encoder.max_length} bytes" puts "#{@name_encoder.estimated_size(@count)} bytes estimated for setting name storage" values_size = @value_encoder.values.length * 4 - puts "value storage uses #{values_size} bytes" + puts "min/max value storage uses #{values_size} bytes" value_idx_size = @value_encoder.index_bytes * 2 value_idx_total = value_idx_size * @count puts "value indexing uses #{value_idx_size} per setting, #{value_idx_total} bytes total" - puts "#{value_idx_size+value_idx_total} bytes estimated for value storage" + puts "#{value_idx_size+value_idx_total} bytes estimated for value+indexes storage" buf = StringIO.new buf << "#include \"fc/settings.h\"\n" @@ -370,10 +379,12 @@ def write_header_file(file) buf << "#pragma once\n" # Write setting_t size constants buf << "#define SETTING_MAX_NAME_LENGTH #{@max_name_length+1}\n" # +1 for the terminating '\0' + buf << "#define SETTING_MAX_WORD_LENGTH #{@name_encoder.max_word_length+1}\n" # +1 for the terminating '\0' buf << "#define SETTING_ENCODED_NAME_MAX_BYTES #{@name_encoder.max_length}\n" if @name_encoder.uses_byte_indexing buf << "#define SETTING_ENCODED_NAME_USES_BYTE_INDEXING\n" end + buf << "#define SETTINGS_WORDS_BITS_PER_CHAR #{SETTINGS_WORDS_BITS_PER_CHAR}\n" buf << "#define SETTINGS_TABLE_COUNT #{@count}\n" offset_type = "uint16_t" if can_use_byte_offsetof @@ -406,7 +417,11 @@ def write_header_file(file) ii = 0 foreach_enabled_member do |group, member| name = member["name"] - buf << "#define SETTING_#{name.upcase} #{ii}\n" + min, max = resolve_range(member) + setting_name = "SETTING_#{name.upcase}" + buf << "#define #{setting_name} #{ii}\n" + buf << "#define #{setting_name}_MIN #{min}\n" + buf << "#define #{setting_name}_MAX #{max}\n" ii += 1 end @@ -455,13 +470,63 @@ def write_impl_file(file) buf << "};\n" # Write word list - buf << "static const char *settingNamesWords[] = {\n" - buf << "\tNULL,\n" + buf << "static const uint8_t settingNamesWords[] = {\n" + word_bits = SETTINGS_WORDS_BITS_PER_CHAR + # We need 27 symbols for a-z + null + rem_symbols = 2 ** word_bits - 27 + symbols = Array.new + acc = 0 + acc_bits = 0 + encode_byte = lambda do |c| + if c == 0 + chr = 0 # XXX: Remove this if we go for explicit lengths + elsif c >= 'a'.ord && c <= 'z'.ord + chr = 1 + (c - 'a'.ord) + elsif c >= 'A'.ord && c <= 'Z'.ord + raise "Cannot encode uppercase character #{c.ord} (#{c})" + else + idx = symbols.index(c) + if idx.nil? + if rem_symbols == 0 + raise "Cannot encode character #{c.ord} (#{c}), no symbols remaining" + end + rem_symbols -= 1 + idx = symbols.length + symbols.push(c) + end + chr = 1 + ('z'.ord - 'a'.ord + 1) + idx + end + if acc_bits >= (8 - word_bits) + # Write + remaining = 8 - acc_bits + acc |= chr << (remaining - word_bits) + buf << "0x#{acc.to_s(16)}," + acc = (chr << (8 - (word_bits - remaining))) & 0xff + else + # Accumulate for next byte + acc |= chr << (3 - acc_bits) + end + acc_bits = (acc_bits + word_bits) % 8 + end @name_encoder.words.each do |w| - buf << "\t#{w.inspect},\n" + buf << "\t" + w.each_byte {|c| encode_byte.call(c)} + encode_byte.call(0) + buf << " /* #{w.inspect} */ \n" + end + if acc_bits > 0 + buf << "\t0x#{acc.to_s(16)}," + if acc_bits > (8 - word_bits) + buf << "0x00" + end + buf << "\n" end buf << "};\n" + # Output symbol array + buf << "static const char wordSymbols[] = {" + symbols.each { |s| buf << "'#{s.chr}'," } + buf << "};\n" # Write the tables table_names = ordered_table_names() table_names.each do |name| @@ -514,8 +579,7 @@ def write_impl_file(file) buf << " | MODE_LOOKUP" buf << ", .config.lookup = { #{table_constant_name(tbl)} }" else - min = @value_encoder.resolve_value(member["min"]) - max = @value_encoder.resolve_value(member["max"]) + min, max = resolve_range(member) if min > max raise "Error encoding #{name}: min (#{min}) > max (#{max})" end @@ -554,6 +618,12 @@ def value_type(group) return group["value_type"] || "MASTER_VALUE" end + def resolve_range(member) + min = @value_encoder.resolve_value(member["min"]) + max = @value_encoder.resolve_value(member["max"]) + return min, max + end + def is_condition_enabled(cond) return !cond || @true_conditions.include?(cond) end