diff --git a/.travis.sh b/.travis.sh new file mode 100644 index 0000000000..77a0820a4c --- /dev/null +++ b/.travis.sh @@ -0,0 +1,7 @@ +#!/bin/bash +# A hacky way of running the unit tests at the same time as the normal builds. +if [ $RUNTESTS ] ; then + cd ./src/test && make test +else + make -j2 +fi diff --git a/.travis.yml b/.travis.yml index a456e51de4..7db4e3f0ca 100644 --- a/.travis.yml +++ b/.travis.yml @@ -1,9 +1,11 @@ env: + - RUNTESTS=True - TARGET=CC3D + - TARGET=CC3D OPBL=yes - TARGET=CHEBUZZF3 - TARGET=CJMCU - TARGET=EUSTM32F103RC - - TARGET=MASSIVEF3 + - TARGET=SPRACINGF3 - TARGET=NAZE - TARGET=NAZE32PRO - TARGET=OLIMEXINO @@ -11,14 +13,15 @@ env: - TARGET=SPARKY - TARGET=STM32F3DISCOVERY - TARGET=ALIENWIIF1 -language: c -compiler: arm-none-eabi-gcc +# We use cpp for unit tests, and c for the main project. +language: cpp +compiler: clang before_install: sudo add-apt-repository -y ppa:terry.guo/gcc-arm-embedded && sudo apt-get update install: sudo apt-get install build-essential gcc-arm-none-eabi git -before_script: $CC --version -script: make -j2 +before_script: arm-none-eabi-gcc --version +script: ./.travis.sh notifications: irc: "chat.freenode.net#cleanflight" use_notice: true - skip_join: true \ No newline at end of file + skip_join: true diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md new file mode 100644 index 0000000000..532e8d4d01 --- /dev/null +++ b/CONTRIBUTING.md @@ -0,0 +1,50 @@ +# Issues and Support. + +Please remember the issue tracker on github is _not_ for user support. Please also do not email developers directly for support. Instead please use IRC or the forums first, then if the problem is confirmed create an issue that details how to repeat the problem so it can be investigated. + +Issued created without steps to repeat are likely to be closed. E-mail requests for support will go un-answered. + +Remember that issues that are due to mis-configuration, wiring or failure to read documentation just takes time away from the developers and can often be solved without developer interaction by other users. + +Please search for existing issues *before* creating new ones. + +# Developers + +Please see the Contributing section of the README.md + +Please see the docs/developers folder for other notes. + +Ensure you understand the github workflow: https://guides.github.com/introduction/flow/index.html + +Please keep pull requests focused on one thing only, since this makes it easier to merge and test in a timely manner. + +If you need help with pull requests there are guides on github here: + +https://help.github.com/articles/creating-a-pull-request/ + +The main flow for a contributing is as follows: + +1. Login to github, goto the cleanflight repository and press `fork`. +2. Then using the command line/terminal on your computer: `git clone ` +3. `cd cleanflight` +4. `git checkout master` +5. `git checkout -b my-new-code` +6. Make changes +7. `git add ` +8. `git commit` +9. `git push origin my-new-code` +10. Create pull request using github UI to merge your changes from your new branch into `cleanflight/master` +11. Repeat from step 4 for new other changes. + +The primary thing to remember is that separate pull requests should be created for separate branches. Never create a pull request from your `master` branch. + +Later, you can get the changes from the cleanflight repo into your `master` branch by adding cleanflight as a git remote and merging from it as follows: + +1. `git add remote cleanflight https://github.com/cleanflight/cleanflight.git` +2. `git checkout master` +3. `git fetch cleanflight` +4. `git merge cleanflight/master` + + +You can also perform the git commands using the git client inside Eclipse. Refer to the Eclipse git manual. + \ No newline at end of file diff --git a/Makefile b/Makefile index cda4d0421b..c4197a7729 100644 --- a/Makefile +++ b/Makefile @@ -35,7 +35,7 @@ SERIAL_DEVICE ?= /dev/ttyUSB0 FORKNAME = cleanflight -VALID_TARGETS = NAZE NAZE32PRO OLIMEXINO STM32F3DISCOVERY CHEBUZZF3 CC3D CJMCU EUSTM32F103RC MASSIVEF3 PORT103R SPARKY ALIENWIIF1 +VALID_TARGETS = NAZE NAZE32PRO OLIMEXINO STM32F3DISCOVERY CHEBUZZF3 CC3D CJMCU EUSTM32F103RC SPRACINGF3 PORT103R SPARKY ALIENWIIF1 # Valid targets for OP BootLoader support OPBL_VALID_TARGETS = CC3D @@ -54,7 +54,7 @@ LINKER_DIR = $(ROOT)/src/main/target # Search path for sources VPATH := $(SRC_DIR):$(SRC_DIR)/startup -ifeq ($(TARGET),$(filter $(TARGET),STM32F3DISCOVERY CHEBUZZF3 NAZE32PRO MASSIVEF3 SPARKY)) +ifeq ($(TARGET),$(filter $(TARGET),STM32F3DISCOVERY CHEBUZZF3 NAZE32PRO SPRACINGF3 SPARKY)) STDPERIPH_DIR = $(ROOT)/lib/main/STM32F30x_StdPeriph_Driver USBFS_DIR = $(ROOT)/lib/main/STM32_USB-FS-Device_Driver @@ -67,21 +67,31 @@ EXCLUDES = stm32f30x_crc.c \ STDPERIPH_SRC := $(filter-out ${EXCLUDES}, $(STDPERIPH_SRC)) -DEVICE_STDPERIPH_SRC = $(USBPERIPH_SRC) \ +DEVICE_STDPERIPH_SRC = \ $(STDPERIPH_SRC) -VPATH := $(VPATH):$(CMSIS_DIR)/CM1/CoreSupport:$(CMSIS_DIR)/CM1/DeviceSupport/ST/STM32F30x:$(USBFS_DIR)/src +VPATH := $(VPATH):$(CMSIS_DIR)/CM1/CoreSupport:$(CMSIS_DIR)/CM1/DeviceSupport/ST/STM32F30x CMSIS_SRC = $(notdir $(wildcard $(CMSIS_DIR)/CM1/CoreSupport/*.c \ $(CMSIS_DIR)/CM1/DeviceSupport/ST/STM32F30x/*.c)) INCLUDE_DIRS := $(INCLUDE_DIRS) \ $(STDPERIPH_DIR)/inc \ - $(USBFS_DIR)/inc \ $(CMSIS_DIR)/CM1/CoreSupport \ - $(CMSIS_DIR)/CM1/DeviceSupport/ST/STM32F30x \ + $(CMSIS_DIR)/CM1/DeviceSupport/ST/STM32F30x + +ifneq ($(TARGET),SPRACINGF3) +INCLUDE_DIRS := $(INCLUDE_DIRS) \ + $(USBFS_DIR)/inc \ $(ROOT)/src/main/vcp +VPATH := $(VPATH):$(USBFS_DIR)/src + +DEVICE_STDPERIPH_SRC := $(DEVICE_STDPERIPH_SRC)\ + $(USBPERIPH_SRC) + +endif + LD_SCRIPT = $(LINKER_DIR)/stm32_flash_f303_256k.ld ARCH_FLAGS = -mthumb -mcpu=cortex-m4 -mfloat-abi=hard -mfpu=fpv4-sp-d16 -mfpu=fpv4-sp-d16 -fsingle-precision-constant -Wdouble-promotion @@ -92,12 +102,6 @@ ifeq ($(TARGET),CHEBUZZF3) TARGET_FLAGS := $(TARGET_FLAGS) -DSTM32F3DISCOVERY endif -ifeq ($(TARGET),MASSIVEF3) -# MASSIVEF3 is a VARIANT of STM32F3DISCOVERY -TARGET_FLAGS := $(TARGET_FLAGS) -DSTM32F3DISCOVERY -endif - - else ifeq ($(TARGET),$(filter $(TARGET),EUSTM32F103RC PORT103R)) @@ -350,7 +354,8 @@ $(error OPBL specified with a unsupported target) endif endif -CJMCU_SRC = startup_stm32f10x_md_gcc.S \ +CJMCU_SRC = \ + startup_stm32f10x_md_gcc.S \ drivers/adc.c \ drivers/adc_stm32f10x.c \ drivers/accgyro_mpu6050.c \ @@ -371,11 +376,17 @@ CJMCU_SRC = startup_stm32f10x_md_gcc.S \ hardware_revision.c \ $(COMMON_SRC) -CC3D_SRC = startup_stm32f10x_md_gcc.S \ +CC3D_SRC = \ + startup_stm32f10x_md_gcc.S \ drivers/accgyro_spi_mpu6000.c \ drivers/adc.c \ drivers/adc_stm32f10x.c \ + drivers/barometer_bmp085.c \ + drivers/barometer_ms5611.c \ drivers/bus_spi.c \ + drivers/bus_i2c_stm32f10x.c \ + drivers/compass_hmc5883l.c \ + drivers/display_ug2864hsweg01.c \ drivers/gpio_stm32f10x.c \ drivers/inverter.c \ drivers/light_led_stm32f10x.c \ @@ -394,7 +405,8 @@ CC3D_SRC = startup_stm32f10x_md_gcc.S \ $(HIGHEND_SRC) \ $(COMMON_SRC) -STM32F30x_COMMON_SRC = startup_stm32f30x_md_gcc.S \ +STM32F30x_COMMON_SRC = \ + startup_stm32f30x_md_gcc.S \ drivers/adc.c \ drivers/adc_stm32f30x.c \ drivers/bus_i2c_stm32f30x.c \ @@ -408,29 +420,37 @@ STM32F30x_COMMON_SRC = startup_stm32f30x_md_gcc.S \ drivers/pwm_rx.c \ drivers/serial_uart.c \ drivers/serial_uart_stm32f30x.c \ - drivers/serial_usb_vcp.c \ drivers/sound_beeper_stm32f30x.c \ drivers/system_stm32f30x.c \ drivers/timer.c \ - drivers/timer_stm32f30x.c \ + drivers/timer_stm32f30x.c + +VCP_SRC = \ vcp/hw_config.c \ vcp/stm32_it.c \ vcp/usb_desc.c \ vcp/usb_endp.c \ vcp/usb_istr.c \ vcp/usb_prop.c \ - vcp/usb_pwr.c + vcp/usb_pwr.c \ + drivers/serial_usb_vcp.c -NAZE32PRO_SRC = $(STM32F30x_COMMON_SRC) \ +NAZE32PRO_SRC = \ + $(STM32F30x_COMMON_SRC) \ $(HIGHEND_SRC) \ - $(COMMON_SRC) + $(COMMON_SRC) \ + $(VCP_SRC) -STM32F3DISCOVERY_COMMON_SRC = $(STM32F30x_COMMON_SRC) \ +STM32F3DISCOVERY_COMMON_SRC = \ + $(STM32F30x_COMMON_SRC) \ drivers/accgyro_l3gd20.c \ drivers/accgyro_l3gd20.c \ - drivers/accgyro_lsm303dlhc.c + drivers/accgyro_lsm303dlhc.c \ + drivers/compass_hmc5883l.c \ + $(VCP_SRC) -STM32F3DISCOVERY_SRC = $(STM32F3DISCOVERY_COMMON_SRC) \ +STM32F3DISCOVERY_SRC = \ + $(STM32F3DISCOVERY_COMMON_SRC) \ drivers/accgyro_adxl345.c \ drivers/accgyro_bma280.c \ drivers/accgyro_mma845x.c \ @@ -442,23 +462,31 @@ STM32F3DISCOVERY_SRC = $(STM32F3DISCOVERY_COMMON_SRC) \ $(HIGHEND_SRC) \ $(COMMON_SRC) -CHEBUZZF3_SRC = $(STM32F3DISCOVERY_SRC) \ +CHEBUZZF3_SRC = \ + $(STM32F3DISCOVERY_SRC) \ $(HIGHEND_SRC) \ $(COMMON_SRC) -MASSIVEF3_SRC = $(STM32F3DISCOVERY_SRC) \ - $(HIGHEND_SRC) \ - $(COMMON_SRC) - -SPARKY_SRC = $(STM32F30x_COMMON_SRC) \ +SPARKY_SRC = \ + $(STM32F30x_COMMON_SRC) \ drivers/display_ug2864hsweg01.c \ drivers/accgyro_mpu6050.c \ drivers/barometer_ms5611.c \ drivers/compass_ak8975.c \ + drivers/serial_usb_vcp.c \ + $(HIGHEND_SRC) \ + $(COMMON_SRC) \ + $(VCP_SRC) + +SPRACINGF3_SRC = \ + $(STM32F30x_COMMON_SRC) \ + drivers/accgyro_mpu6050.c \ + drivers/barometer_ms5611.c \ + drivers/compass_hmc5883l.c \ $(HIGHEND_SRC) \ $(COMMON_SRC) -ifeq ($(TARGET),MASSIVEF3) +ifeq ($(TARGET),SPRACINGF3) LD_SCRIPT = $(LINKER_DIR)/stm32_flash_f303_128k.ld endif diff --git a/README.md b/README.md index 7f36dd25da..9ad931ae0a 100644 --- a/README.md +++ b/README.md @@ -83,7 +83,7 @@ https://github.com/cleanflight/cleanflight-configurator ## Contributing -Contributions are welcome and encouraged. You can contibute in many ways: +Contributions are welcome and encouraged. You can contribute in many ways: * Documentation updates and corrections. * How-To guides - received help? help others! diff --git a/build_docs.sh b/build_docs.sh index 396e276d12..464dbe5f6e 100755 --- a/build_docs.sh +++ b/build_docs.sh @@ -2,6 +2,7 @@ filename=Manual doc_files=( + 'Introduction.md' 'Installation.md' 'Configuration.md' 'Cli.md' @@ -24,6 +25,7 @@ doc_files=( 'Autotune.md' 'Blackbox.md' 'Migrating from baseflight.md' + 'Boards.md' 'Board - AlienWii32.md' 'Board - CC3D.md' 'Board - CJMCU.md' diff --git a/docs/Board - CC3D.md b/docs/Board - CC3D.md index c1194ba5ec..580bd4c353 100644 --- a/docs/Board - CC3D.md +++ b/docs/Board - CC3D.md @@ -79,6 +79,22 @@ The SoftSerial port is not available when RX_PARALLEL_PWM is used. The transmiss To connect the GUI to the flight controller you need additional hardware attached to the USART1 serial port (by default). +# Flex Port + +The flex port will be enabled in I2C mode unless USART3 is used. You can connect external I2C sensors and displays to this port. + +You cannot use USART3 and I2C at the same time. + +## Flex port pinout + +| Pin | Signal | Notes | +| --- | ------------------ | ----------------------- | +| 1 | GND | | +| 2 | VCC unregulated | | +| 3 | I2C SCL / UART3 TX | 3.3v level | +| 4 | I2C SDA / UART3 RX | 3.3v level (5v tolerant | + + # Flashing There are two primary ways to get Cleanflight onto a CC3D board. diff --git a/docs/Boards.md b/docs/Boards.md new file mode 100644 index 0000000000..e86484a8df --- /dev/null +++ b/docs/Boards.md @@ -0,0 +1,32 @@ +# Flight controller hardware + +The current focus is geared towards flight controller hardware that use the STM32F103 and STM32F303 series processors. The core logic is separated from the hardware drivers, porting to other processors is possible. + +The core set of supported flyable boards are: + +* CC3D +* CJMCU +* Flip32+ +* Naze32 +* Sparky +* AlienWii32 + +Cleanflight also runs on the following developer boards: + +* STM32F3Discovery +* Port103R +* EUSTM32F103RB + +There is also limited support for the following boards which may be removed due to lack of users or commercial availability. + +* Olimexino +* Naze32Pro +* STM32F3Discovery with Chebuzz F3 shield. + +Each board has it's pros and cons, before purchasing hardware the main thing to check is if the board offers enough serial ports and input/output pins for the hardware you want to use with it and that you can use them at the same time. On some boards some features are mutually exclusive. + +Please see the board-specific chapters in the manual for wiring details. + +There are off-shoots (forks) of the project that support the STM32F4 processors as found on the Revo and Quanton boards. + +Where applicable the chapters also provide links to other hardware that is known to work with Cleanflight, such as receivers, buzzers, etc. diff --git a/docs/Failsafe.md b/docs/Failsafe.md index 358bbcbb38..2f930b269a 100644 --- a/docs/Failsafe.md +++ b/docs/Failsafe.md @@ -28,7 +28,7 @@ a) no valid channel data from the RX is received via Serial RX. b) the first 4 Parallel PWM/PPM channels do not have valid signals. -And: +And when: c) the failsafe guard time specified by `failsafe_delay` has elapsed. diff --git a/docs/Gps.md b/docs/Gps.md index 247df2427a..d2772f696d 100644 --- a/docs/Gps.md +++ b/docs/Gps.md @@ -58,7 +58,7 @@ UBlox GPS units can either be configured using the FC or manually. ### UBlox GPS manual configuration -Use UBox U-Center and connect your GPS to your computer. The CLI `gpspassthough` command may be of use if you do not have a spare USART to USB adapter. +Use UBox U-Center and connect your GPS to your computer. The CLI `gpspassthrough` command may be of use if you do not have a spare USART to USB adapter. Display the Packet Console (so you can see what messages your receiver is sending to your computer). diff --git a/docs/Introduction.md b/docs/Introduction.md new file mode 100644 index 0000000000..ad1a9c623a --- /dev/null +++ b/docs/Introduction.md @@ -0,0 +1,33 @@ +# Cleanflight + +Welcome to CleanFlight! + +Cleanflight is an community project which attempts to deliver flight controller firmware and related tools. + +## Primary Goals + +* Community driven. +* Friendly project atmosphere. +* Focus on the needs of users. +* Great flight performance. +* Understandable and maintainable code. + +## Hardware + +See the flight controller hardware chapter for details. + +## Software + +There are two primary components, the firmware and the configuration tool. The firmware is the code that runs on the flight controller board. The GUI configuration tool (configurator) is used to configure the flight controller, it runs on Windows, OSX and Linux. + +## Feedback & Contributing + +We welcome all feedback. If you love it we want to hear from you, if you have problems please tell us how we could improve things so we can make it better for everyone. + +If you want to contribute please see the notes here: + +https://github.com/cleanflight/cleanflight#contributing + +Developers should read this: + +https://github.com/cleanflight/cleanflight/blob/master/CONTRIBUTING.md diff --git a/docs/LedStrip.md b/docs/LedStrip.md index 86c2076dc1..4305c99075 100644 --- a/docs/LedStrip.md +++ b/docs/LedStrip.md @@ -12,6 +12,7 @@ supports the following: * Heading/Orientation lights. * Flight mode specific color schemes. * Low battery warning. +* AUX operated on/off switch The function and orientation configuration is fixed for now but later it should be able to be set via the UI or CLI.. @@ -110,6 +111,7 @@ Note: It is perfectly possible to configure an LED to have all directions `NESWU * `I` - `I`ndicator. * `A` - `A`rmed state. * `T` - `T`hrust state. +* `R` - `R`ing thrust state. Example: @@ -170,6 +172,24 @@ This mode fades the LED current LED color to the previous/next color in the HSB throttle is in the middle position the color is unaffected, thus it can be mixed with orientation colors to indicate orientation and throttle at the same time. +#### Thrust ring state + +This mode is allows you to use a 12, 16 or 24 leds ring (e.g. NeoPixel ring) for an afterburner effect. When armed the leds use the following sequences: 2 On, 4 Off, 2 On, 4 Off, and so on. The light pattern rotates clockwise as throttle increases. + +A better effect is acheived when LEDs configured for thrust ring have no other functions. + +LED direction and X/Y positions are irrelevant for thrust ring LED state. The order of the LEDs that have the state determines how the LED behaves. + +Each LED of the ring can be a different color. The color can be selected between the 15 colors availables. + +For example, led 0 is set as a `R`ing thrust state led in color 13 as follow. + +``` +led 0 2,2::R:13 +``` + +LED strips and rings can be combined. + ## Positioning Cut the strip into sections as per diagrams below. When the strips are cut ensure you reconnect each output to each input with cable where the break is made. diff --git a/docs/PID tuning.md b/docs/PID tuning.md new file mode 100644 index 0000000000..99c770d782 --- /dev/null +++ b/docs/PID tuning.md @@ -0,0 +1,107 @@ +# PID tuning + +Every aspect of flight dynamics is controlled by the selected "PID controller". This is an algorithm which is +responsible for reacting to your stick inputs and keeping the craft stable in the air by using the gyroscopes and/or +accelerometers (depending on your flight mode). + +The "PIDs" are a set of tuning parameters which control the operation of the PID controller. The optimal PID settings +to use are different on every craft, so if you can't find someone with your exact setup who will share their settings +with you, some trial and error is required to find the best performing PID settings. + +A video on how to recognise and correct different flight problems caused by PID settings is available here: + +https://www.youtube.com/watch?v=YNzqTGEl2xQ + +Basically, the goal of the PID controller is to bring the craft's rotation rate in all three axes to the rate that +you're commanding with your sticks. An error is computed which is the difference between your target rotation rate and +the actual one measured by the gyroscopes, and the controller tries to bring this error to zero. + +The P term controls the strength of the correction that is applied to bring the craft toward the target angle or +rotation rate. If the P term is too low, the craft will be difficult to control as it won't respond quickly enough to +keep itself stable. If it is set too high, the craft will rapidly oscillate/shake as it continually overshoots its +target. + +The I term corrects small, long term errors. If it is set too low, the craft's attitude will slowly drift. If it is +set too high, the craft will oscillate (but with slower oscillations than with P being set too high). + +The D term attempts to increase system stability by monitoring the rate of change in the error. If the error is +changing slowly (so the P and I terms aren't having enough impact on reaching the target) the D term causes an increase +in the correction in order to reach the target sooner. If the error is rapidly converging to zero, the D term causes the +strength of the correction to be backed off in order to avoid overshooting the target. + +## PID controllers + +Cleanflight has three built-in PID controllers which each have different flight behavior. Each controller requires +different PID settings for best performance, so if you tune your craft using one PID controller, those settings will +likely not work well on any of the other controllers. + +You can change between PID controllers by running `set pid_controller = X` on the CLI tab of the Cleanflight +Configurator, where X is the number of the controller you want to use. Please read these notes first before trying one +out! + +### PID controller 0, "MultiWii" (default) + +PID Controller 0 is the default controller in Cleanflight, and Cleanflight's default PID settings are tuned to be +middle-of-the-road settings for this controller. It originates from the old MultiWii PID controller from MultiWii 2.2 +and earlier. + +One of the quirks with this controller is that if you increase the P value for an axis, the maximum rotation rates for +that axis are lowered. Hence you need to crank up the pitch or roll rates if you have higher and higher P values. + +In Horizon and Angle modes, this controller uses both the LEVEL "P" and "I" settings in order to tune the +auto-leveling corrections in a similar way to the way that P and I settings are applied to roll and yaw axes in the acro +flight modes. The LEVEL "D" term is used as a limiter to constrain the maximum correction applied by the LEVEL "P" term. + +### PID controller 1, "Rewrite" + +PID Controller 1 is a newer PID controller that is derived from the one in MultiWii 2.3 and later. It works better from +all accounts, and fixes some inherent problems in the way the old one worked. From reports, tuning is apparently easier +on controller 1, and it tolerates a wider range of PID values well. + +Unlike controller 0, controller 1 allows the user to manipulate PID values to tune reaction and stability without +affecting yaw, roll or pitch rotation rates (which are tuned by the dedicated roll & pitch and yaw rate +settings). + +In Angle mode, this controller uses the LEVEL "P" PID setting to decide how strong the auto-level correction should +be. + +In Horizon mode, this controller uses the LEVEL "I" PID setting to decide how much auto-level correction should be +applied. The default Cleanflight setting for "I" will result in virtually no auto-leveling being applied, so that will +need to be increased in order to perform like PID controller 0. + +The LEVEL "D" setting is not used by this controller. + +### PID controller 2, "Baseflight" + +PID Controller 2 is Lux's new floating point PID controller. Both controller 0 and 1 use integer arithmetic, which was +faster in the days of the slower 8-bit MultiWii controllers, but is less precise. + +This controller has code that attempts to compensate for variations in the looptime, which should mean that the PIDs +don't have to be retuned when the looptime setting changes. + +There were initially some problems with horizon mode, and sluggishness in acro mode, that were recently fixed by +nebbian in v1.6.0. The autotune feature does not work on this controller, so don't try to autotune it. + +Even though PC2 is called "pidBaseflight" in the code, it was never in Baseflight or MultiWii. A better name might have +been pidFloatingPoint, or pidCleanflight. It is the first PID Controller designed for 32-bit processors and not derived +from MultiWii. I believe it was named pidBaseflight because it was to be the first true 32-bit processor native PID +controller, and thus the native Baseflight PC, but Timecop never accepted the code into Baseflight. + +The strength of the auto-leveling correction applied during Angle mode is set by the parameter "level_angle" which +is labeled "LEVEL Integral" in the GUI. This can be used to tune the auto-leveling strength in Angle mode compared to +Horizon mode. The default is 5.0. + +The strength of the auto-leveling correction applied during Horizon mode is set by the parameter "level_horizon" which +is labeled "LEVEL Integral" in the GUI. The default is 3.0, which makes the Horizon mode apply weaker self-leveling than +the Angle mode. Note: There is currently a bug in the Configurator which shows this parameter divided by 100 (so it +shows as 0.03 rather than 3.0). + +The transition between self-leveling and acro behavior in Horizon mode is controlled by the "sensitivity_horizon" +parameter which is labeled "LEVEL Derivative" in the Cleanflight Configurator GUI. This sets the percentage of your +stick travel that should have self-leveling applied to it, so smaller values cause more of the stick area to fly using +only the gyros. The default is 75% + +For example, at a setting of "100" for "sensitivity_horizon", 100% self-leveling strength will be applied at center +stick, 50% self-leveling will be applied at 50% stick, and no self-leveling will be applied at 100% stick. If +sensitivity is decreased to 75, 100% self-leveling will be applied at center stick, 50% will be applied at 63% +stick, and no self-leveling will be applied at 75% stick and onwards. \ No newline at end of file diff --git a/docs/Rx.md b/docs/Rx.md index 98b2cee6bf..add5907afd 100644 --- a/docs/Rx.md +++ b/docs/Rx.md @@ -4,9 +4,9 @@ A receiver is used to receive radio control signals from your transmitter and co There are 3 basic types of receivers: -Parallel PWM Receivers -PPM Receivers -Serial Receivers +1. Parallel PWM Receivers +2. PPM Receivers +3. Serial Receivers ## Parallel PWM Receivers diff --git a/docs/Telemetry.md b/docs/Telemetry.md index 1358f25a06..27e5010555 100644 --- a/docs/Telemetry.md +++ b/docs/Telemetry.md @@ -64,17 +64,18 @@ Only Electric Air Modules and GPS Modules are emulated, remember to enable them Serial ports use two wires but HoTT uses a single wire so some electronics are required so that the signals don't get mixed up. Connect as follows: -``` -HoTT TX/RX -> Serial RX (connect directly) -Serial TX -> 1N4148 Diode -(| )-> HoTT TX/RX (connect via diode) -``` + +* HoTT TX/RX `T` -> Serial RX (connect directly) +* HoTT TX/RX `T` -> Diode `-( |)-` > Serial TX (connect via diode) The diode should be arranged to allow the data signals to flow the right way ``` --(| )- == Diode, | indicates cathode marker. +-( |)- == Diode, | indicates cathode marker. ``` +1N4148 diodes have been tested and work with the GR-24. + As noticed by Skrebber the GR-12 (and probably GR-16/24, too) are based on a PIC 24FJ64GA-002, which has 5V tolerant digital pins. Note: The SoftSerial ports are not listed as 5V tolerant in the STM32F103xx data sheet pinouts and pin description section. Verify if you require a 5v/3.3v level shifters. diff --git a/docs/development/Building in Windows.md b/docs/development/Building in Windows.md index 99b5bcd478..bb0f144f8e 100755 --- a/docs/development/Building in Windows.md +++ b/docs/development/Building in Windows.md @@ -38,7 +38,7 @@ Continue with the Installation and accept all autodetected dependencies. ---------- -versions do matter, 4.8-2014-q2 is known to work well. Download this version from htps://launchpad.net/gcc-arm-embedded/+download - preferrebly as a ZIP-File. +versions do matter, 4.8-2014-q2 is known to work well. Download this version from https://launchpad.net/gcc-arm-embedded/+download - preferrebly as a ZIP-File. Extract the contents of this archive to any folder of your choice, for instance ```C:\dev\gcc-arm-none-eabi-4_8-2014q2```. diff --git a/docs/development/Development.md b/docs/development/Development.md index a450c35789..1ae4d9dd24 100755 --- a/docs/development/Development.md +++ b/docs/development/Development.md @@ -28,7 +28,7 @@ Note: Tests are written in C++ and linked with with firmware's C code. 6. Keep methods short - it makes it easier to test. 7. Don't be afraid of moving code to a new file - it helps to reduce test dependencies. 8. Avoid noise-words in variable names, like 'data' or 'info'. Think about what you're naming and name it well. Don't be afraid to rename anything. -9. Avoid comments taht describe what the code is doing, the code should describe itself. Comments are useful however for big-picture purposes and to document content of variables. +9. Avoid comments that describe what the code is doing, the code should describe itself. Comments are useful however for big-picture purposes and to document content of variables. 10. If you need to document a variable do it at the declarion, don't copy the comment to the `extern` usage since it will lead to comment rot. 11. Seek advice from other developers - know you can always learn more. 12. Be professional - attempts at humor or slating existing code in the codebase itself is not helpful when you have to change/fix it. diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 447f791f19..211e25911c 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -150,6 +150,7 @@ typedef struct blackboxGPSFieldDefinition_t { uint8_t isSigned; uint8_t predict; uint8_t encode; + uint8_t condition; // Decide whether this field should appear in the log } blackboxGPSFieldDefinition_t; /** @@ -181,6 +182,7 @@ static const blackboxMainFieldDefinition_t blackboxMainFields[] = { {"rcCommand[3]", UNSIGNED, .Ipredict = PREDICT(MINTHROTTLE), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_4S16), CONDITION(ALWAYS)}, {"vbatLatest", UNSIGNED, .Ipredict = PREDICT(VBATREF), .Iencode = ENCODING(NEG_14BIT), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_8SVB), FLIGHT_LOG_FIELD_CONDITION_VBAT}, + {"amperageLatest",UNSIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_8SVB), FLIGHT_LOG_FIELD_CONDITION_AMPERAGE}, #ifdef MAG {"magADC[0]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_8SVB), FLIGHT_LOG_FIELD_CONDITION_MAG}, {"magADC[1]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_8SVB), FLIGHT_LOG_FIELD_CONDITION_MAG}, @@ -213,18 +215,19 @@ static const blackboxMainFieldDefinition_t blackboxMainFields[] = { #ifdef GPS // GPS position/vel frame static const blackboxGPSFieldDefinition_t blackboxGpsGFields[] = { - {"GPS_numSat", UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)}, - {"GPS_coord[0]", SIGNED, PREDICT(HOME_COORD), ENCODING(SIGNED_VB)}, - {"GPS_coord[1]", SIGNED, PREDICT(HOME_COORD), ENCODING(SIGNED_VB)}, - {"GPS_altitude", UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)}, - {"GPS_speed", UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)}, - {"GPS_ground_course",UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)} + {"time", UNSIGNED, PREDICT(LAST_MAIN_FRAME_TIME), ENCODING(UNSIGNED_VB), CONDITION(NOT_LOGGING_EVERY_FRAME)}, + {"GPS_numSat", UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB), CONDITION(ALWAYS)}, + {"GPS_coord[0]", SIGNED, PREDICT(HOME_COORD), ENCODING(SIGNED_VB), CONDITION(ALWAYS)}, + {"GPS_coord[1]", SIGNED, PREDICT(HOME_COORD), ENCODING(SIGNED_VB), CONDITION(ALWAYS)}, + {"GPS_altitude", UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB), CONDITION(ALWAYS)}, + {"GPS_speed", UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB), CONDITION(ALWAYS)}, + {"GPS_ground_course",UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB), CONDITION(ALWAYS)} }; // GPS home frame static const blackboxGPSFieldDefinition_t blackboxGpsHFields[] = { - {"GPS_home[0]", SIGNED, PREDICT(0), ENCODING(SIGNED_VB)}, - {"GPS_home[1]", SIGNED, PREDICT(0), ENCODING(SIGNED_VB)} + {"GPS_home[0]", SIGNED, PREDICT(0), ENCODING(SIGNED_VB), CONDITION(ALWAYS)}, + {"GPS_home[1]", SIGNED, PREDICT(0), ENCODING(SIGNED_VB), CONDITION(ALWAYS)} }; #endif @@ -631,6 +634,12 @@ static bool testBlackboxConditionUncached(FlightLogFieldCondition condition) case FLIGHT_LOG_FIELD_CONDITION_VBAT: return feature(FEATURE_VBAT); + case FLIGHT_LOG_FIELD_CONDITION_AMPERAGE: + return feature(FEATURE_CURRENT_METER); + + case FLIGHT_LOG_FIELD_CONDITION_NOT_LOGGING_EVERY_FRAME: + return masterConfig.blackbox_rate_num < masterConfig.blackbox_rate_denom; + case FLIGHT_LOG_FIELD_CONDITION_NEVER: return false; default: @@ -721,6 +730,11 @@ static void writeIntraframe(void) writeUnsignedVB((vbatReference - blackboxCurrent->vbatLatest) & 0x3FFF); } + if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_AMPERAGE)) { + // 12bit value directly from ADC + writeUnsignedVB(blackboxCurrent->amperageLatest); + } + #ifdef MAG if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_MAG)) { for (x = 0; x < XYZ_AXIS_COUNT; x++) @@ -772,8 +786,8 @@ static void writeInterframe(void) //No need to store iteration count since its delta is always 1 /* - * Since the difference between the difference between successive times will be nearly zero, use - * second-order differences. + * Since the difference between the difference between successive times will be nearly zero (due to consistent + * looptime spacing), use second-order differences. */ writeSignedVB((int32_t) (blackboxHistory[0]->time - 2 * blackboxHistory[1]->time + blackboxHistory[2]->time)); @@ -806,13 +820,17 @@ static void writeInterframe(void) writeTag8_4S16(deltas); - //Check for sensors that are updated periodically (so deltas are normally zero) VBAT, MAG, BARO + //Check for sensors that are updated periodically (so deltas are normally zero) VBAT, Amperage, MAG, BARO int optionalFieldCount = 0; if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_VBAT)) { deltas[optionalFieldCount++] = (int32_t) blackboxCurrent->vbatLatest - blackboxLast->vbatLatest; } + if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_AMPERAGE)) { + deltas[optionalFieldCount++] = (int32_t) blackboxCurrent->amperageLatest - blackboxLast->amperageLatest; + } + #ifdef MAG if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_MAG)) { for (x = 0; x < XYZ_AXIS_COUNT; x++) @@ -894,7 +912,7 @@ static void configureBlackboxPort(void) * * 9 / 1250 = 7200 / 1000000 */ - serialChunkSize = max((masterConfig.looptime * 9) / 1250, 4); + serialChunkSize = MAX((masterConfig.looptime * 9) / 1250, 4); } static void releaseBlackboxPort(void) @@ -931,7 +949,7 @@ void startBlackbox(void) blackboxHistory[1] = &blackboxHistoryRing[1]; blackboxHistory[2] = &blackboxHistoryRing[2]; - vbatReference = vbatLatest; + vbatReference = vbatLatestADC; //No need to clear the content of blackboxHistoryRing since our first frame will be an intra which overwrites it @@ -980,6 +998,17 @@ static void writeGPSFrame() { blackboxWrite('G'); + /* + * If we're logging every frame, then a GPS frame always appears just after a frame with the + * currentTime timestamp in the log, so the reader can just use that timestamp for the GPS frame. + * + * If we're not logging every frame, we need to store the time of this GPS frame. + */ + if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_NOT_LOGGING_EVERY_FRAME)) { + // Predict the time of the last frame in the main log + writeUnsignedVB(currentTime - blackboxHistory[1]->time); + } + writeUnsignedVB(GPS_numSat); writeSignedVB(GPS_coord[0] - gpsHistory.GPS_home[0]); writeSignedVB(GPS_coord[1] - gpsHistory.GPS_home[1]); @@ -1022,7 +1051,8 @@ static void loadBlackboxState(void) for (i = 0; i < motorCount; i++) blackboxCurrent->motor[i] = motor[i]; - blackboxCurrent->vbatLatest = vbatLatest; + blackboxCurrent->vbatLatest = vbatLatestADC; + blackboxCurrent->amperageLatest = amperageLatestADC; #ifdef MAG for (i = 0; i < XYZ_AXIS_COUNT; i++) @@ -1133,7 +1163,7 @@ static bool blackboxWriteSysinfo() } // How many bytes can we afford to transmit this loop? - xmitState.u.serialBudget = min(xmitState.u.serialBudget + serialChunkSize, 64); + xmitState.u.serialBudget = MIN(xmitState.u.serialBudget + serialChunkSize, 64); // Most headers will consume at least 20 bytes so wait until we've built up that much link budget if (xmitState.u.serialBudget < 20) { @@ -1209,6 +1239,10 @@ static bool blackboxWriteSysinfo() xmitState.u.serialBudget -= strlen("H vbatref:%u\n"); break; + case 13: + blackboxPrintf("H currentMeter:%i,%i\n", masterConfig.batteryConfig.currentMeterOffset, masterConfig.batteryConfig.currentMeterScale); + + xmitState.u.serialBudget -= strlen("H currentMeter:%i,%i\n"); default: return true; } @@ -1309,14 +1343,14 @@ void handleBlackbox(void) case BLACKBOX_STATE_SEND_GPS_H_HEADERS: //On entry of this state, xmitState.headerIndex is 0 and xmitState.u.fieldIndex is -1 if (!sendFieldDefinition(blackboxGPSHHeaderNames, ARRAY_LENGTH(blackboxGPSHHeaderNames), blackboxGpsHFields, blackboxGpsHFields + 1, - ARRAY_LENGTH(blackboxGpsHFields), NULL, NULL)) { + ARRAY_LENGTH(blackboxGpsHFields), &blackboxGpsHFields[0].condition, &blackboxGpsHFields[1].condition)) { blackboxSetState(BLACKBOX_STATE_SEND_GPS_G_HEADERS); } break; case BLACKBOX_STATE_SEND_GPS_G_HEADERS: //On entry of this state, xmitState.headerIndex is 0 and xmitState.u.fieldIndex is -1 if (!sendFieldDefinition(blackboxGPSGHeaderNames, ARRAY_LENGTH(blackboxGPSGHeaderNames), blackboxGpsGFields, blackboxGpsGFields + 1, - ARRAY_LENGTH(blackboxGpsGFields), NULL, NULL)) { + ARRAY_LENGTH(blackboxGpsGFields), &blackboxGpsGFields[0].condition, &blackboxGpsGFields[1].condition)) { blackboxSetState(BLACKBOX_STATE_SEND_SYSINFO); } break; diff --git a/src/main/blackbox/blackbox.h b/src/main/blackbox/blackbox.h index b971feec59..59085249e9 100644 --- a/src/main/blackbox/blackbox.h +++ b/src/main/blackbox/blackbox.h @@ -35,6 +35,7 @@ typedef struct blackboxValues_t { int16_t servo[MAX_SUPPORTED_SERVOS]; uint16_t vbatLatest; + uint16_t amperageLatest; #ifdef BARO int32_t BaroAlt; diff --git a/src/main/blackbox/blackbox_fielddefs.h b/src/main/blackbox/blackbox_fielddefs.h index 84f0ee1195..26d862268f 100644 --- a/src/main/blackbox/blackbox_fielddefs.h +++ b/src/main/blackbox/blackbox_fielddefs.h @@ -32,11 +32,14 @@ typedef enum FlightLogFieldCondition { FLIGHT_LOG_FIELD_CONDITION_MAG, FLIGHT_LOG_FIELD_CONDITION_BARO, FLIGHT_LOG_FIELD_CONDITION_VBAT, + FLIGHT_LOG_FIELD_CONDITION_AMPERAGE, FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0, FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_1, FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_2, + FLIGHT_LOG_FIELD_CONDITION_NOT_LOGGING_EVERY_FRAME, + FLIGHT_LOG_FIELD_CONDITION_NEVER, FLIGHT_LOG_FIELD_CONDITION_FIRST = FLIGHT_LOG_FIELD_CONDITION_ALWAYS, @@ -72,7 +75,10 @@ typedef enum FlightLogFieldPredictor { FLIGHT_LOG_FIELD_PREDICTOR_1500 = 8, //Predict vbatref, the reference ADC level stored in the header - FLIGHT_LOG_FIELD_PREDICTOR_VBATREF = 9 + FLIGHT_LOG_FIELD_PREDICTOR_VBATREF = 9, + + //Predict the last time value written in the main stream + FLIGHT_LOG_FIELD_PREDICTOR_LAST_MAIN_FRAME_TIME = 10 } FlightLogFieldPredictor; diff --git a/src/main/common/maths.c b/src/main/common/maths.c index 716d83e491..15d02f8f66 100644 --- a/src/main/common/maths.c +++ b/src/main/common/maths.c @@ -22,7 +22,7 @@ int32_t applyDeadband(int32_t value, int32_t deadband) { - if (abs(value) < deadband) { + if (ABS(value) < deadband) { value = 0; } else if (value > 0) { value -= deadband; @@ -92,3 +92,52 @@ int scaleRange(int x, int srcMin, int srcMax, int destMin, int destMax) { return ((a / b) - (destMax - destMin)) + destMax; } +// Normalize a vector +void normalizeV(struct fp_vector *src, struct fp_vector *dest) +{ + float length; + + length = sqrtf(src->X * src->X + src->Y * src->Y + src->Z * src->Z); + if (length != 0) { + dest->X = src->X / length; + dest->Y = src->Y / length; + dest->Z = src->Z / length; + } +} + +// Rotate a vector *v by the euler angles defined by the 3-vector *delta. +void rotateV(struct fp_vector *v, fp_angles_t *delta) +{ + struct fp_vector v_tmp = *v; + + float mat[3][3]; + float cosx, sinx, cosy, siny, cosz, sinz; + float coszcosx, sinzcosx, coszsinx, sinzsinx; + + cosx = cosf(delta->angles.roll); + sinx = sinf(delta->angles.roll); + cosy = cosf(delta->angles.pitch); + siny = sinf(delta->angles.pitch); + cosz = cosf(delta->angles.yaw); + sinz = sinf(delta->angles.yaw); + + coszcosx = cosz * cosx; + sinzcosx = sinz * cosx; + coszsinx = sinx * cosz; + sinzsinx = sinx * sinz; + + mat[0][0] = cosz * cosy; + mat[0][1] = -cosy * sinz; + mat[0][2] = siny; + mat[1][0] = sinzcosx + (coszsinx * siny); + mat[1][1] = coszcosx - (sinzsinx * siny); + mat[1][2] = -sinx * cosy; + mat[2][0] = (sinzsinx) - (coszcosx * siny); + mat[2][1] = (coszsinx) + (sinzcosx * siny); + mat[2][2] = cosy * cosx; + + v->X = v_tmp.X * mat[0][0] + v_tmp.Y * mat[1][0] + v_tmp.Z * mat[2][0]; + v->Y = v_tmp.X * mat[0][1] + v_tmp.Y * mat[1][1] + v_tmp.Z * mat[2][1]; + v->Z = v_tmp.X * mat[0][2] + v_tmp.Y * mat[1][2] + v_tmp.Z * mat[2][2]; +} + diff --git a/src/main/common/maths.h b/src/main/common/maths.h index 85b08c6410..8dc281fee3 100644 --- a/src/main/common/maths.h +++ b/src/main/common/maths.h @@ -21,20 +21,14 @@ #define sq(x) ((x)*(x)) #endif -#ifdef M_PI -// M_PI should be float, but previous definition may be double -# undef M_PI -#endif -#define M_PI 3.14159265358979323846f +// Use floating point M_PI instead explicitly. +#define M_PIf 3.14159265358979323846f -#define RADX10 (M_PI / 1800.0f) // 0.001745329252f -#define RAD (M_PI / 180.0f) +#define RAD (M_PIf / 180.0f) -#define DEG2RAD(degrees) (degrees * RAD) - -#define min(a, b) ((a) < (b) ? (a) : (b)) -#define max(a, b) ((a) > (b) ? (a) : (b)) -#define abs(x) ((x) > 0 ? (x) : -(x)) +#define MIN(a, b) ((a) < (b) ? (a) : (b)) +#define MAX(a, b) ((a) > (b) ? (a) : (b)) +#define ABS(x) ((x) > 0 ? (x) : -(x)) typedef struct stdev_t { @@ -42,6 +36,31 @@ typedef struct stdev_t int m_n; } stdev_t; +// Floating point 3 vector. +typedef struct fp_vector { + float X; + float Y; + float Z; +} t_fp_vector_def; + +typedef union { + float A[3]; + t_fp_vector_def V; +} t_fp_vector; + +// Floating point Euler angles. +// Be carefull, could be either of degrees or radians. +typedef struct fp_angles { + float roll; + float pitch; + float yaw; +} fp_angles_def; + +typedef union { + float raw[3]; + fp_angles_def angles; +} fp_angles_t; + int32_t applyDeadband(int32_t value, int32_t deadband); int constrain(int amt, int low, int high); @@ -54,3 +73,7 @@ float devStandardDeviation(stdev_t *dev); float degreesToRadians(int16_t degrees); int scaleRange(int x, int srcMin, int srcMax, int destMin, int destMax); + +void normalizeV(struct fp_vector *src, struct fp_vector *dest); + +void rotateV(struct fp_vector *v, fp_angles_t *delta); diff --git a/src/main/common/typeconversion.c b/src/main/common/typeconversion.c index 7df5bb1370..4e3f8b4c2e 100644 --- a/src/main/common/typeconversion.c +++ b/src/main/common/typeconversion.c @@ -159,7 +159,7 @@ char *ftoa(float x, char *floatString) value = (int32_t)(x * 1000.0f); // Convert float * 1000 to an integer - itoa(abs(value), intString1, 10); // Create string from abs of integer value + itoa(ABS(value), intString1, 10); // Create string from abs of integer value if (value >= 0) intString2[0] = ' '; // Positive number, add a pad space diff --git a/src/main/config/config.c b/src/main/config/config.c index ea8d512673..6ec55de3d2 100644 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -25,6 +25,8 @@ #include "common/color.h" #include "common/axis.h" +#include "common/maths.h" + #include "flight/flight.h" #include "drivers/sensor.h" @@ -613,10 +615,12 @@ void activateConfig(void) imuRuntimeConfig.acc_unarmedcal = currentProfile->acc_unarmedcal;; imuRuntimeConfig.small_angle = masterConfig.small_angle; - configureImu( + configureIMU( &imuRuntimeConfig, ¤tProfile->pidProfile, - ¤tProfile->accDeadband + ¤tProfile->accDeadband, + currentProfile->accz_lpf_cutoff, + currentProfile->throttle_correction_angle ); configureAltitudeHold( @@ -626,9 +630,6 @@ void activateConfig(void) &masterConfig.escAndServoConfig ); - calculateThrottleAngleScale(currentProfile->throttle_correction_angle); - calculateAccZLowPassFilterRCTimeConstant(currentProfile->accz_lpf_cutoff); - #ifdef BARO useBarometerConfig(¤tProfile->barometerConfig); #endif diff --git a/src/main/drivers/accgyro_l3gd20.c b/src/main/drivers/accgyro_l3gd20.c index ebc223f681..8671ce8be2 100644 --- a/src/main/drivers/accgyro_l3gd20.c +++ b/src/main/drivers/accgyro_l3gd20.c @@ -20,6 +20,8 @@ #include "platform.h" +#include "build_config.h" + #include "common/maths.h" #include "system.h" @@ -110,7 +112,7 @@ static void l3gd20SpiInit(SPI_TypeDef *SPIx) GPIO_SetBits(L3GD20_CS_GPIO, L3GD20_CS_PIN); - SPI_I2S_DeInit(SPI1); + SPI_I2S_DeInit(SPIx); SPI_InitStructure.SPI_Direction = SPI_Direction_2Lines_FullDuplex; SPI_InitStructure.SPI_Mode = SPI_Mode_Master; @@ -122,11 +124,11 @@ static void l3gd20SpiInit(SPI_TypeDef *SPIx) SPI_InitStructure.SPI_FirstBit = SPI_FirstBit_MSB; SPI_InitStructure.SPI_CRCPolynomial = 7; - SPI_Init(SPI1, &SPI_InitStructure); + SPI_Init(SPIx, &SPI_InitStructure); - SPI_RxFIFOThresholdConfig(L3GD20_SPI, SPI_RxFIFOThreshold_QF); + SPI_RxFIFOThresholdConfig(SPIx, SPI_RxFIFOThreshold_QF); - SPI_Cmd(SPI1, ENABLE); + SPI_Cmd(SPIx, ENABLE); } void l3gd20GyroInit(void) @@ -194,6 +196,8 @@ static void l3gd20GyroRead(int16_t *gyroData) bool l3gd20Detect(gyro_t *gyro, uint16_t lpf) { + UNUSED(lpf); + gyro->init = l3gd20GyroInit; gyro->read = l3gd20GyroRead; diff --git a/src/main/drivers/accgyro_mpu6050.c b/src/main/drivers/accgyro_mpu6050.c index e5044afc25..eacea46b0b 100644 --- a/src/main/drivers/accgyro_mpu6050.c +++ b/src/main/drivers/accgyro_mpu6050.c @@ -178,9 +178,17 @@ static const mpu6050Config_t *mpu6050Config = NULL; void mpu6050GpioInit(void) { gpio_config_t gpio; - if (mpu6050Config->gpioAPB2Peripherals) { - RCC_APB2PeriphClockCmd(mpu6050Config->gpioAPB2Peripherals, ENABLE); - } +#ifdef STM32F303 + if (mpu6050Config->gpioAHBPeripherals) { + RCC_AHBPeriphClockCmd(mpu6050Config->gpioAHBPeripherals, ENABLE); + } +#endif +#ifdef STM32F10X + if (mpu6050Config->gpioAPB2Peripherals) { + RCC_APB2PeriphClockCmd(mpu6050Config->gpioAPB2Peripherals, ENABLE); + } +#endif + gpio.pin = mpu6050Config->gpioPin; gpio.speed = Speed_2MHz; diff --git a/src/main/drivers/accgyro_mpu6050.h b/src/main/drivers/accgyro_mpu6050.h index b05a611563..e52c013fff 100644 --- a/src/main/drivers/accgyro_mpu6050.h +++ b/src/main/drivers/accgyro_mpu6050.h @@ -18,7 +18,12 @@ #pragma once typedef struct mpu6050Config_s { +#ifdef STM32F303 + uint32_t gpioAHBPeripherals; +#endif +#ifdef STM32F10X uint32_t gpioAPB2Peripherals; +#endif uint16_t gpioPin; GPIO_TypeDef *gpioPort; } mpu6050Config_t; diff --git a/src/main/drivers/accgyro_spi_mpu6000.c b/src/main/drivers/accgyro_spi_mpu6000.c index 590e6e922a..8795bc54da 100644 --- a/src/main/drivers/accgyro_spi_mpu6000.c +++ b/src/main/drivers/accgyro_spi_mpu6000.c @@ -319,7 +319,7 @@ bool mpu6000SpiGyroDetect(gyro_t *gyro, uint16_t lpf) gyro->read = mpu6000SpiGyroRead; // 16.4 dps/lsb scalefactor gyro->scale = 1.0f / 16.4f; - //gyro->scale = (4.0f / 16.4f) * (M_PI / 180.0f) * 0.000001f; + //gyro->scale = (4.0f / 16.4f) * (M_PIf / 180.0f) * 0.000001f; delay(100); return true; } diff --git a/src/main/drivers/accgyro_spi_mpu6500.c b/src/main/drivers/accgyro_spi_mpu6500.c index 123ac08c35..7b3893cc1d 100644 --- a/src/main/drivers/accgyro_spi_mpu6500.c +++ b/src/main/drivers/accgyro_spi_mpu6500.c @@ -128,7 +128,7 @@ bool mpu6500SpiGyroDetect(gyro_t *gyro, uint16_t lpf) // 16.4 dps/lsb scalefactor gyro->scale = 1.0f / 16.4f; - //gyro->scale = (4.0f / 16.4f) * (M_PI / 180.0f) * 0.000001f; + //gyro->scale = (4.0f / 16.4f) * (M_PIf / 180.0f) * 0.000001f; // default lpf is 42Hz if (lpf >= 188) diff --git a/src/main/drivers/adc_stm32f10x.c b/src/main/drivers/adc_stm32f10x.c index ae6680c4ef..5f591ed0a7 100644 --- a/src/main/drivers/adc_stm32f10x.c +++ b/src/main/drivers/adc_stm32f10x.c @@ -48,7 +48,7 @@ extern volatile uint16_t adcValues[ADC_CHANNEL_COUNT]; void adcInit(drv_adc_config_t *init) { -#ifdef CC3D +#if defined(CJMCU) || defined(CC3D) UNUSED(init); #endif @@ -64,58 +64,49 @@ void adcInit(drv_adc_config_t *init) GPIO_StructInit(&GPIO_InitStructure); GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AIN; -#ifdef CC3D - GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0; - adcConfig[ADC_BATTERY].adcChannel = ADC_Channel_0; - adcConfig[ADC_BATTERY].dmaIndex = configuredAdcChannels++; - adcConfig[ADC_BATTERY].enabled = true; - adcConfig[ADC_BATTERY].sampleTime = ADC_SampleTime_239Cycles5; - -#else - // configure always-present battery index (ADC4) - GPIO_InitStructure.GPIO_Pin = GPIO_Pin_4; - adcConfig[ADC_BATTERY].adcChannel = ADC_Channel_4; +#ifdef VBAT_ADC_GPIO_PIN + GPIO_InitStructure.GPIO_Pin = VBAT_ADC_GPIO_PIN; + GPIO_Init(VBAT_ADC_GPIO, &GPIO_InitStructure); + adcConfig[ADC_BATTERY].adcChannel = VBAT_ADC_CHANNEL; adcConfig[ADC_BATTERY].dmaIndex = configuredAdcChannels++; adcConfig[ADC_BATTERY].enabled = true; adcConfig[ADC_BATTERY].sampleTime = ADC_SampleTime_239Cycles5; +#endif +#ifdef EXTERNAL1_ADC_GPIO if (init->enableExternal1) { -#ifdef OLIMEXINO - GPIO_InitStructure.GPIO_Pin |= GPIO_Pin_5; - adcConfig[ADC_EXTERNAL1].adcChannel = ADC_Channel_5; + GPIO_InitStructure.GPIO_Pin = EXTERNAL1_ADC_GPIO_PIN; + GPIO_Init(EXTERNAL1_ADC_GPIO, &GPIO_InitStructure); + adcConfig[ADC_EXTERNAL1].adcChannel = EXTERNAL1_ADC_CHANNEL; adcConfig[ADC_EXTERNAL1].dmaIndex = configuredAdcChannels++; adcConfig[ADC_EXTERNAL1].enabled = true; adcConfig[ADC_EXTERNAL1].sampleTime = ADC_SampleTime_239Cycles5; -#endif - -#ifdef NAZE - GPIO_InitStructure.GPIO_Pin |= GPIO_Pin_5; - adcConfig[ADC_EXTERNAL1].adcChannel = ADC_Channel_5; - adcConfig[ADC_EXTERNAL1].dmaIndex = configuredAdcChannels++; - adcConfig[ADC_EXTERNAL1].enabled = true; - adcConfig[ADC_EXTERNAL1].sampleTime = ADC_SampleTime_239Cycles5; -#endif } -#endif // !CC3D +#endif +#ifdef RSSI_ADC_GPIO if (init->enableRSSI) { - GPIO_InitStructure.GPIO_Pin |= GPIO_Pin_1; - adcConfig[ADC_RSSI].adcChannel = ADC_Channel_1; + GPIO_InitStructure.GPIO_Pin = RSSI_ADC_GPIO_PIN; + GPIO_Init(RSSI_ADC_GPIO, &GPIO_InitStructure); + adcConfig[ADC_RSSI].adcChannel = RSSI_ADC_CHANNEL; adcConfig[ADC_RSSI].dmaIndex = configuredAdcChannels++; adcConfig[ADC_RSSI].enabled = true; adcConfig[ADC_RSSI].sampleTime = ADC_SampleTime_239Cycles5; } +#endif - GPIO_Init(GPIOA, &GPIO_InitStructure); - +#ifdef CURRENT_METER_ADC_GPIO if (init->enableCurrentMeter) { - GPIO_InitStructure.GPIO_Pin = GPIO_Pin_1; - GPIO_Init(GPIOB, &GPIO_InitStructure); - adcConfig[ADC_CURRENT].adcChannel = ADC_Channel_9; + GPIO_InitStructure.GPIO_Pin = CURRENT_METER_ADC_GPIO_PIN; + GPIO_Init(CURRENT_METER_ADC_GPIO, &GPIO_InitStructure); + adcConfig[ADC_CURRENT].adcChannel = CURRENT_METER_ADC_CHANNEL; adcConfig[ADC_CURRENT].dmaIndex = configuredAdcChannels++; adcConfig[ADC_CURRENT].enabled = true; adcConfig[ADC_CURRENT].sampleTime = ADC_SampleTime_239Cycles5; } +#endif + + RCC_ADCCLKConfig(RCC_PCLK2_Div8); // 9MHz from 72MHz APB2 clock(HSE), 8MHz from 64MHz (HSI) RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1, ENABLE); RCC_APB2PeriphClockCmd(RCC_APB2Periph_ADC1, ENABLE); diff --git a/src/main/drivers/compass_hmc5883l.c b/src/main/drivers/compass_hmc5883l.c index 98788f18b6..afa40d6c45 100644 --- a/src/main/drivers/compass_hmc5883l.c +++ b/src/main/drivers/compass_hmc5883l.c @@ -142,10 +142,16 @@ void hmc5883lInit(void) gpio_config_t gpio; if (hmc5883Config) { +#ifdef STM32F303 + if (hmc5883Config->gpioAHBPeripherals) { + RCC_AHBPeriphClockCmd(hmc5883Config->gpioAHBPeripherals, ENABLE); + } +#endif +#ifdef STM32F10X if (hmc5883Config->gpioAPB2Peripherals) { RCC_APB2PeriphClockCmd(hmc5883Config->gpioAPB2Peripherals, ENABLE); } - +#endif gpio.pin = hmc5883Config->gpioPin; gpio.speed = Speed_2MHz; gpio.mode = Mode_IN_FLOATING; @@ -171,7 +177,7 @@ void hmc5883lInit(void) xyz_total[Z] += magADC[Z]; // Detect saturation. - if (-4096 >= min(magADC[X], min(magADC[Y], magADC[Z]))) { + if (-4096 >= MIN(magADC[X], MIN(magADC[Y], magADC[Z]))) { bret = false; break; // Breaks out of the for loop. No sense in continuing if we saturated. } @@ -191,7 +197,7 @@ void hmc5883lInit(void) xyz_total[Z] -= magADC[Z]; // Detect saturation. - if (-4096 >= min(magADC[X], min(magADC[Y], magADC[Z]))) { + if (-4096 >= MIN(magADC[X], MIN(magADC[Y], magADC[Z]))) { bret = false; break; // Breaks out of the for loop. No sense in continuing if we saturated. } diff --git a/src/main/drivers/compass_hmc5883l.h b/src/main/drivers/compass_hmc5883l.h index df07d21adf..bc6aabc311 100644 --- a/src/main/drivers/compass_hmc5883l.h +++ b/src/main/drivers/compass_hmc5883l.h @@ -18,7 +18,12 @@ #pragma once typedef struct hmc5883Config_s { +#ifdef STM32F303 + uint32_t gpioAHBPeripherals; +#endif +#ifdef STM32F10X uint32_t gpioAPB2Peripherals; +#endif uint16_t gpioPin; GPIO_TypeDef *gpioPort; } hmc5883Config_t; diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c index 9130d14841..a09f6ec10f 100644 --- a/src/main/drivers/pwm_mapping.c +++ b/src/main/drivers/pwm_mapping.c @@ -69,7 +69,7 @@ enum { MAP_TO_SERVO_OUTPUT, }; -#if defined(NAZE) || defined(OLIMEXINO) || defined(NAZE32PRO) || defined(STM32F3DISCOVERY) || defined(EUSTM32F103RC) || defined(MASSIVEF3) || defined(PORT103R) +#if defined(NAZE) || defined(OLIMEXINO) || defined(NAZE32PRO) || defined(STM32F3DISCOVERY) || defined(EUSTM32F103RC) || defined(PORT103R) static const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed @@ -267,15 +267,64 @@ static const uint16_t multiPWM[] = { }; static const uint16_t airPPM[] = { + // TODO 0xFFFF }; static const uint16_t airPWM[] = { + // TODO 0xFFFF }; #endif + +#ifdef SPRACINGF3 +static const uint16_t multiPPM[] = { + PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input + + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM12 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM13 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM14 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM15 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM16 | (MAP_TO_MOTOR_OUTPUT << 8), + 0xFFFF +}; + +static const uint16_t multiPWM[] = { + PWM1 | (MAP_TO_PWM_INPUT << 8), + PWM2 | (MAP_TO_PWM_INPUT << 8), + PWM3 | (MAP_TO_PWM_INPUT << 8), + PWM4 | (MAP_TO_PWM_INPUT << 8), + PWM5 | (MAP_TO_PWM_INPUT << 8), + PWM6 | (MAP_TO_PWM_INPUT << 8), + PWM7 | (MAP_TO_PWM_INPUT << 8), + PWM8 | (MAP_TO_PWM_INPUT << 8), + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM12 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM13 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM14 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM15 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM16 | (MAP_TO_MOTOR_OUTPUT << 8), + 0xFFFF +}; + +static const uint16_t airPPM[] = { + // TODO + 0xFFFF +}; + +static const uint16_t airPWM[] = { + // TODO + 0xFFFF +}; +#endif + static const uint16_t * const hardwareMaps[] = { multiPWM, multiPPM, @@ -345,24 +394,20 @@ pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init) #endif -#ifdef STM32F10X - // skip ADC for RSSI - if (init->useRSSIADC && timerIndex == PWM2) +#ifdef VBAT_ADC_GPIO + if (init->useVbat && timerHardwarePtr->gpio == VBAT_ADC_GPIO && timerHardwarePtr->pin == VBAT_ADC_GPIO_PIN) { continue; + } #endif -#ifdef CC3D - if (init->useVbat && timerIndex == Vbat_TIMER) { +#ifdef RSSI_ADC_GPIO + if (init->useRSSIADC && timerHardwarePtr->gpio == RSSI_ADC_GPIO && timerHardwarePtr->pin == RSSI_ADC_GPIO_PIN) { continue; } #endif -#ifdef CC3D - if (init->useCurrentMeterADC && timerIndex == CurrentMeter_TIMER) { - continue; - } -#endif -#ifdef CC3D - if (init->useRSSIADC && timerIndex == RSSI_TIMER) { + +#ifdef CURRENT_METER_ADC_GPIO + if (init->useCurrentMeterADC && timerHardwarePtr->gpio == CURRENT_METER_ADC_GPIO && timerHardwarePtr->pin == CURRENT_METER_ADC_GPIO_PIN) { continue; } #endif @@ -387,6 +432,12 @@ pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init) type = MAP_TO_SERVO_OUTPUT; #endif +#if defined(SPRACINGF3) + // remap PWM15+16 as servos + if ((timerIndex == PWM15 || timerIndex == PWM16) && timerHardwarePtr->tim == TIM15) + type = MAP_TO_SERVO_OUTPUT; +#endif + #if defined(NAZE32PRO) || (defined(STM32F3DISCOVERY) && !defined(CHEBUZZF3)) // remap PWM 5+6 or 9+10 as servos - softserial pin pairs require timer ports that use the same timer if (init->useSoftSerial) { diff --git a/src/main/drivers/pwm_mapping.h b/src/main/drivers/pwm_mapping.h index b11ac21b47..a78a6e4fa1 100644 --- a/src/main/drivers/pwm_mapping.h +++ b/src/main/drivers/pwm_mapping.h @@ -65,7 +65,7 @@ typedef struct pwmOutputConfiguration_s { uint8_t motorCount; } pwmOutputConfiguration_t; -// This indexes into the read-only hardware definition structure, timerHardware_t, as well as into pwmPorts structure with dynamic data. +// This indexes into the read-only hardware definition structure, timerHardware_t enum { PWM1 = 0, PWM2, @@ -80,5 +80,7 @@ enum { PWM11, PWM12, PWM13, - PWM14 + PWM14, + PWM15, + PWM16 }; diff --git a/src/main/drivers/serial_usb_vcp.c b/src/main/drivers/serial_usb_vcp.c index 6fbfd723e9..2aab5ad9a7 100644 --- a/src/main/drivers/serial_usb_vcp.c +++ b/src/main/drivers/serial_usb_vcp.c @@ -17,17 +17,17 @@ #include #include +#include +#include #include "platform.h" +#include "build_config.h" + #include "usb_core.h" #include "usb_init.h" #include "hw_config.h" -#include -#include - - #include "drivers/system.h" #include "serial.h" @@ -40,26 +40,37 @@ static vcpPort_t vcpPort; void usbVcpSetBaudRate(serialPort_t *instance, uint32_t baudRate) { + UNUSED(instance); + UNUSED(baudRate); + // TODO implement } void usbVcpSetMode(serialPort_t *instance, portMode_t mode) { + UNUSED(instance); + UNUSED(mode); + // TODO implement } bool isUsbVcpTransmitBufferEmpty(serialPort_t *instance) { + UNUSED(instance); return true; } uint8_t usbVcpAvailable(serialPort_t *instance) { + UNUSED(instance); + return receiveLength & 0xFF; // FIXME use uint32_t return type everywhere } uint8_t usbVcpRead(serialPort_t *instance) { + UNUSED(instance); + uint8_t buf[1]; uint32_t rxed = 0; @@ -73,6 +84,8 @@ uint8_t usbVcpRead(serialPort_t *instance) void usbVcpWrite(serialPort_t *instance, uint8_t c) { + UNUSED(instance); + uint32_t txed; uint32_t start = millis(); diff --git a/src/main/drivers/sound_beeper_stm32f30x.c b/src/main/drivers/sound_beeper_stm32f30x.c index 34f6536c90..d1c81b6d62 100644 --- a/src/main/drivers/sound_beeper_stm32f30x.c +++ b/src/main/drivers/sound_beeper_stm32f30x.c @@ -21,13 +21,17 @@ #include "platform.h" +#include "build_config.h" + #include "gpio.h" #include "sound_beeper.h" void initBeeperHardware(beeperConfig_t *config) { -#ifdef BEEPER +#ifndef BEEPER + UNUSED(config); +#else gpio_config_t gpioConfig = { config->gpioPin, config->gpioMode, diff --git a/src/main/drivers/timer.c b/src/main/drivers/timer.c index d3a86778d2..5c10e1674c 100644 --- a/src/main/drivers/timer.c +++ b/src/main/drivers/timer.c @@ -220,6 +220,37 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { #endif +#ifdef SPRACINGF3 +const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { + { TIM2, GPIOA, Pin_0, TIM_Channel_1, TIM2_IRQn, 0, Mode_AF_PP, GPIO_PinSource0, GPIO_AF_1}, // RC_CH1 - PA0 - *TIM2_CH1 + { TIM2, GPIOA, Pin_1, TIM_Channel_2, TIM2_IRQn, 0, Mode_AF_PP, GPIO_PinSource1, GPIO_AF_1}, // RC_CH2 - PA1 - *TIM2_CH2, TIM15_CH1N + { TIM2, GPIOB, Pin_10, TIM_Channel_3, TIM3_IRQn, 0, Mode_AF_PP, GPIO_PinSource10, GPIO_AF_1}, // RC_CH3 - PB10 - *TIM2_CH3, USART3_TX (AF7) + { TIM2, GPIOB, Pin_11, TIM_Channel_4, TIM3_IRQn, 0, Mode_AF_PP, GPIO_PinSource11, GPIO_AF_1}, // RC_CH4 - PB11 - *TIM2_CH4, USART3_RX (AF7) + { TIM3, GPIOB, Pin_4, TIM_Channel_1, TIM3_IRQn, 0, Mode_AF_PP, GPIO_PinSource4, GPIO_AF_2}, // RC_CH5 - PB4 - *TIM3_CH1 + { TIM3, GPIOB, Pin_5, TIM_Channel_2, TIM3_IRQn, 0, Mode_AF_PP, GPIO_PinSource5, GPIO_AF_2}, // RC_CH5 - PB4 - *TIM3_CH2 + { TIM3, GPIOB, Pin_0, TIM_Channel_3, TIM3_IRQn, 0, Mode_AF_PP, GPIO_PinSource0, GPIO_AF_2}, // RC_CH6 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N + { TIM3, GPIOB, Pin_1, TIM_Channel_4, TIM3_IRQn, 0, Mode_AF_PP, GPIO_PinSource1, GPIO_AF_2}, // RC_CH7 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N + + { TIM16, GPIOA, Pin_6, TIM_Channel_1, TIM1_UP_TIM16_IRQn, 1, Mode_AF_PP, GPIO_PinSource6, GPIO_AF_1}, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1 + { TIM17, GPIOA, Pin_7, TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, Mode_AF_PP, GPIO_PinSource7, GPIO_AF_1}, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 + { TIM4, GPIOA, Pin_11, TIM_Channel_1, TIM4_IRQn, 1, Mode_AF_PP, GPIO_PinSource11, GPIO_AF_10}, // PWM3 - PA11 + { TIM4, GPIOA, Pin_12, TIM_Channel_2, TIM4_IRQn, 1, Mode_AF_PP, GPIO_PinSource12, GPIO_AF_10}, // PWM4 - PA12 + { TIM4, GPIOB, Pin_8, TIM_Channel_3, TIM4_IRQn, 1, Mode_AF_PP, GPIO_PinSource8, GPIO_AF_2}, // PWM5 - PB8 + { TIM4, GPIOB, Pin_9, TIM_Channel_4, TIM4_IRQn, 1, Mode_AF_PP, GPIO_PinSource9, GPIO_AF_2}, // PWM6 - PB9 + { TIM15, GPIOA, Pin_2, TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, Mode_AF_PP, GPIO_PinSource2, GPIO_AF_9}, // PWM7 - PA2 + { TIM15, GPIOA, Pin_3, TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, Mode_AF_PP, GPIO_PinSource3, GPIO_AF_9}, // PWM8 - PA3 + + { TIM1, GPIOA, Pin_8, TIM_Channel_1, TIM1_CC_IRQn, 1, Mode_AF_PP, GPIO_PinSource8, GPIO_AF_6}, // GPIO_TIMER / LED_STRIP +}; + +#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(15) | TIM_N(16) |TIM_N(17)) + +#define TIMER_APB1_PERIPHERALS (RCC_APB1Periph_TIM2 | RCC_APB1Periph_TIM3 | RCC_APB1Periph_TIM4) +#define TIMER_APB2_PERIPHERALS (RCC_APB2Periph_TIM1 | RCC_APB2Periph_TIM15 | RCC_APB2Periph_TIM16 | RCC_APB2Periph_TIM17) +#define TIMER_AHB_PERIPHERALS (RCC_AHBPeriph_GPIOA | RCC_AHBPeriph_GPIOB) + +#endif + #define USED_TIMER_COUNT BITCOUNT(USED_TIMERS) #define CC_CHANNELS_PER_TIMER 4 // TIM_Channel_1..4 diff --git a/src/main/drivers/timer.h b/src/main/drivers/timer.h index 1a32699c53..d8d78a4471 100644 --- a/src/main/drivers/timer.h +++ b/src/main/drivers/timer.h @@ -17,18 +17,6 @@ #pragma once -#ifdef SPARKY -#define USABLE_TIMER_CHANNEL_COUNT 11 -#endif - -#ifdef CHEBUZZF3 -#define USABLE_TIMER_CHANNEL_COUNT 18 -#endif - -#ifdef CC3D -#define USABLE_TIMER_CHANNEL_COUNT 12 -#endif - #if !defined(USABLE_TIMER_CHANNEL_COUNT) #define USABLE_TIMER_CHANNEL_COUNT 14 #endif diff --git a/src/main/flight/altitudehold.c b/src/main/flight/altitudehold.c index d2bb2b146c..f74a32c73e 100644 --- a/src/main/flight/altitudehold.c +++ b/src/main/flight/altitudehold.c @@ -91,7 +91,7 @@ static void applyMultirotorAltHold(void) // multirotor alt hold if (rcControlsConfig->alt_hold_fast_change) { // rapid alt changes - if (abs(rcCommand[THROTTLE] - initialThrottleHold) > rcControlsConfig->alt_hold_deadband) { + if (ABS(rcCommand[THROTTLE] - initialThrottleHold) > rcControlsConfig->alt_hold_deadband) { errorVelocityI = 0; isAltHoldChanged = 1; rcCommand[THROTTLE] += (rcCommand[THROTTLE] > initialThrottleHold) ? -rcControlsConfig->alt_hold_deadband : rcControlsConfig->alt_hold_deadband; @@ -104,7 +104,7 @@ static void applyMultirotorAltHold(void) } } else { // slow alt changes, mostly used for aerial photography - if (abs(rcCommand[THROTTLE] - initialThrottleHold) > rcControlsConfig->alt_hold_deadband) { + if (ABS(rcCommand[THROTTLE] - initialThrottleHold) > rcControlsConfig->alt_hold_deadband) { // set velocity proportional to stick movement +100 throttle gives ~ +50 cm/s setVelocity = (rcCommand[THROTTLE] - initialThrottleHold) / 2; velocityControl = 1; @@ -172,12 +172,12 @@ void updateSonarAltHoldState(void) bool isThrustFacingDownwards(rollAndPitchInclination_t *inclination) { - return abs(inclination->values.rollDeciDegrees) < DEGREES_80_IN_DECIDEGREES && abs(inclination->values.pitchDeciDegrees) < DEGREES_80_IN_DECIDEGREES; + return ABS(inclination->values.rollDeciDegrees) < DEGREES_80_IN_DECIDEGREES && ABS(inclination->values.pitchDeciDegrees) < DEGREES_80_IN_DECIDEGREES; } int16_t calculateTiltAngle(rollAndPitchInclination_t *inclination) { - return max(abs(inclination->values.rollDeciDegrees), abs(inclination->values.pitchDeciDegrees)); + return MAX(ABS(inclination->values.rollDeciDegrees), ABS(inclination->values.pitchDeciDegrees)); } diff --git a/src/main/flight/flight.c b/src/main/flight/flight.c index dbc049b3d3..7b994e5f6d 100644 --- a/src/main/flight/flight.c +++ b/src/main/flight/flight.c @@ -117,11 +117,11 @@ static void pidBaseflight(pidProfile_t *pidProfile, controlRateConfig_t *control stickPosAil = getRcStickDeflection(FD_ROLL, rxConfig->midrc); stickPosEle = getRcStickDeflection(FD_PITCH, rxConfig->midrc); - if(abs(stickPosAil) > abs(stickPosEle)){ - mostDeflectedPos = abs(stickPosAil); + if(ABS(stickPosAil) > ABS(stickPosEle)){ + mostDeflectedPos = ABS(stickPosAil); } else { - mostDeflectedPos = abs(stickPosEle); + mostDeflectedPos = ABS(stickPosEle); } // Progressively turn off the horizon self level strength as the stick is banged over @@ -220,7 +220,7 @@ static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRa UNUSED(controlRateConfig); // **** PITCH & ROLL & YAW PID **** - prop = min(max(abs(rcCommand[PITCH]), abs(rcCommand[ROLL])), 500); // range [0;500] + prop = MIN(MAX(ABS(rcCommand[PITCH]), ABS(rcCommand[ROLL])), 500); // range [0;500] for (axis = 0; axis < 3; axis++) { if ((FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) && (axis == FD_ROLL || axis == FD_PITCH)) { // MODE relying on ACC @@ -252,7 +252,7 @@ static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRa PTermGYRO = rcCommand[axis]; errorGyroI[axis] = constrain(errorGyroI[axis] + error, -16000, +16000); // WindUp - if ((abs(gyroData[axis]) > (640 * 4)) || (axis == FD_YAW && abs(rcCommand[axis]) > 100)) + if ((ABS(gyroData[axis]) > (640 * 4)) || (axis == FD_YAW && ABS(rcCommand[axis]) > 100)) errorGyroI[axis] = 0; ITermGYRO = (errorGyroI[axis] / 125 * pidProfile->I8[axis]) / 64; diff --git a/src/main/flight/flight.h b/src/main/flight/flight.h index ed96b43b1b..95877badf2 100644 --- a/src/main/flight/flight.h +++ b/src/main/flight/flight.h @@ -61,28 +61,6 @@ typedef enum { #define FLIGHT_DYNAMICS_INDEX_COUNT 3 -typedef struct fp_vector { - float X; - float Y; - float Z; -} t_fp_vector_def; - -typedef union { - float A[3]; - t_fp_vector_def V; -} t_fp_vector; - -typedef struct fp_angles { - float roll; - float pitch; - float yaw; -} fp_angles_def; - -typedef union { - float raw[3]; - fp_angles_def angles; -} fp_angles_t; - typedef struct int16_flightDynamicsTrims_s { int16_t roll; int16_t pitch; diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index a9eb8b0de5..afea1727e9 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -73,59 +73,46 @@ int16_t gyroZero[FLIGHT_DYNAMICS_INDEX_COUNT] = { 0, 0, 0 }; rollAndPitchInclination_t inclination = { { 0, 0 } }; // absolute angle inclination in multiple of 0.1 degree 180 deg = 1800 float anglerad[2] = { 0.0f, 0.0f }; // absolute angle inclination in radians -static void getEstimatedAttitude(void); - imuRuntimeConfig_t *imuRuntimeConfig; pidProfile_t *pidProfile; accDeadband_t *accDeadband; -void configureImu(imuRuntimeConfig_t *initialImuRuntimeConfig, pidProfile_t *initialPidProfile, accDeadband_t *initialAccDeadband) +void configureIMU( + imuRuntimeConfig_t *initialImuRuntimeConfig, + pidProfile_t *initialPidProfile, + accDeadband_t *initialAccDeadband, + float accz_lpf_cutoff, + //TODO: Move throttle angle correction code into flight/throttle_angle_correction.c + uint16_t throttle_correction_angle +) { imuRuntimeConfig = initialImuRuntimeConfig; pidProfile = initialPidProfile; accDeadband = initialAccDeadband; + fc_acc = calculateAccZLowPassFilterRCTimeConstant(accz_lpf_cutoff); + //TODO: Move throttle angle correction code into flight/throttle_angle_correction.c + throttleAngleScale = calculateThrottleAngleScale(throttle_correction_angle); } -void imuInit() +void initIMU() { - smallAngle = lrintf(acc_1G * cosf(RAD * imuRuntimeConfig->small_angle)); + smallAngle = lrintf(acc_1G * cosf(degreesToRadians(imuRuntimeConfig->small_angle))); accVelScale = 9.80665f / acc_1G / 10000.0f; - gyroScaleRad = gyro.scale * (M_PI / 180.0f) * 0.000001f; + gyroScaleRad = gyro.scale * (M_PIf / 180.0f) * 0.000001f; } -void calculateThrottleAngleScale(uint16_t throttle_correction_angle) +//TODO: Move throttle angle correction code into flight/throttle_angle_correction.c +float calculateThrottleAngleScale(uint16_t throttle_correction_angle) { - throttleAngleScale = (1800.0f / M_PI) * (900.0f / throttle_correction_angle); + return (1800.0f / M_PIf) * (900.0f / throttle_correction_angle); } -void calculateAccZLowPassFilterRCTimeConstant(float accz_lpf_cutoff) +/* +* Calculate RC time constant used in the accZ lpf. +*/ +float calculateAccZLowPassFilterRCTimeConstant(float accz_lpf_cutoff) { - fc_acc = 0.5f / (M_PI * accz_lpf_cutoff); // calculate RC time constant used in the accZ lpf -} - -void computeIMU(rollAndPitchTrims_t *accelerometerTrims, uint8_t mixerMode) -{ - static int16_t gyroYawSmooth = 0; - - gyroGetADC(); - if (sensors(SENSOR_ACC)) { - updateAccelerationReadings(accelerometerTrims); - getEstimatedAttitude(); - } else { - accADC[X] = 0; - accADC[Y] = 0; - accADC[Z] = 0; - } - - gyroData[FD_ROLL] = gyroADC[FD_ROLL]; - gyroData[FD_PITCH] = gyroADC[FD_PITCH]; - - if (mixerMode == MIXER_TRI) { - gyroData[FD_YAW] = (gyroYawSmooth * 2 + gyroADC[FD_YAW]) / 3; - gyroYawSmooth = gyroData[FD_YAW]; - } else { - gyroData[FD_YAW] = gyroADC[FD_YAW]; - } + return 0.5f / (M_PIf * accz_lpf_cutoff); } // ************************************************** @@ -145,56 +132,6 @@ void computeIMU(rollAndPitchTrims_t *accelerometerTrims, uint8_t mixerMode) t_fp_vector EstG; -// Normalize a vector -void normalizeV(struct fp_vector *src, struct fp_vector *dest) -{ - float length; - - length = sqrtf(src->X * src->X + src->Y * src->Y + src->Z * src->Z); - if (length != 0) { - dest->X = src->X / length; - dest->Y = src->Y / length; - dest->Z = src->Z / length; - } -} - -// Rotate Estimated vector(s) with small angle approximation, according to the gyro data -void rotateV(struct fp_vector *v, fp_angles_t *delta) -{ - struct fp_vector v_tmp = *v; - - // This does a "proper" matrix rotation using gyro deltas without small-angle approximation - float mat[3][3]; - float cosx, sinx, cosy, siny, cosz, sinz; - float coszcosx, sinzcosx, coszsinx, sinzsinx; - - cosx = cosf(delta->angles.roll); - sinx = sinf(delta->angles.roll); - cosy = cosf(delta->angles.pitch); - siny = sinf(delta->angles.pitch); - cosz = cosf(delta->angles.yaw); - sinz = sinf(delta->angles.yaw); - - coszcosx = cosz * cosx; - sinzcosx = sinz * cosx; - coszsinx = sinx * cosz; - sinzsinx = sinx * sinz; - - mat[0][0] = cosz * cosy; - mat[0][1] = -cosy * sinz; - mat[0][2] = siny; - mat[1][0] = sinzcosx + (coszsinx * siny); - mat[1][1] = coszcosx - (sinzsinx * siny); - mat[1][2] = -sinx * cosy; - mat[2][0] = (sinzsinx) - (coszcosx * siny); - mat[2][1] = (coszsinx) + (sinzcosx * siny); - mat[2][2] = cosy * cosx; - - v->X = v_tmp.X * mat[0][0] + v_tmp.Y * mat[1][0] + v_tmp.Z * mat[2][0]; - v->Y = v_tmp.X * mat[0][1] + v_tmp.Y * mat[1][1] + v_tmp.Z * mat[2][1]; - v->Z = v_tmp.X * mat[0][2] + v_tmp.Y * mat[1][2] + v_tmp.Z * mat[2][2]; -} - void accSum_reset(void) { accSum[0] = 0; @@ -248,8 +185,37 @@ void acc_calc(uint32_t deltaT) accSumCount++; } -// baseflight calculation by Luggi09 originates from arducopter -static int16_t calculateHeading(t_fp_vector *vec) +/* +* Baseflight calculation by Luggi09 originates from arducopter +* ============================================================ +* This function rotates magnetic vector to cancel actual yaw and +* pitch of craft. Then it computes it's direction in X/Y plane. +* This value is returned as compass heading, value is 0-360 degrees. +* +* Note that Earth's magnetic field is not parallel with ground unless +* you are near equator. Its inclination is considerable, >60 degrees +* towards ground in most of Europe. +* +* First we consider it in 2D: +* +* An example, the vector <1, 1> would be turned into the heading +* 45 degrees, representing it's angle clockwise from north. +* +* ***************** * +* * | <1,1> * +* * | / * +* * | / * +* * |/ * +* * * * +* * * +* * * +* * * +* * * +* ******************* +* +* //TODO: Add explanation for how it uses the Z dimension. +*/ +int16_t calculateHeading(t_fp_vector *vec) { int16_t head; @@ -259,8 +225,12 @@ static int16_t calculateHeading(t_fp_vector *vec) float sinePitch = sinf(anglerad[AI_PITCH]); float Xh = vec->A[X] * cosinePitch + vec->A[Y] * sineRoll * sinePitch + vec->A[Z] * sinePitch * cosineRoll; float Yh = vec->A[Y] * cosineRoll - vec->A[Z] * sineRoll; - float hd = (atan2f(Yh, Xh) * 1800.0f / M_PI + magneticDeclination) / 10.0f; + //TODO: Replace this comment with an explanation of why Yh and Xh can never simultanoeusly be zero, + // or handle the case in which they are and (atan2f(0, 0) is undefined. + float hd = (atan2f(Yh, Xh) * 1800.0f / M_PIf + magneticDeclination) / 10.0f; head = lrintf(hd); + + // Arctan returns a value in the range -180 to 180 degrees. We 'normalize' negative angles to be positive. if (head < 0) head += 360; @@ -318,8 +288,8 @@ static void getEstimatedAttitude(void) // Attitude of the estimated vector anglerad[AI_ROLL] = atan2f(EstG.V.Y, EstG.V.Z); anglerad[AI_PITCH] = atan2f(-EstG.V.X, sqrtf(EstG.V.Y * EstG.V.Y + EstG.V.Z * EstG.V.Z)); - inclination.values.rollDeciDegrees = lrintf(anglerad[AI_ROLL] * (1800.0f / M_PI)); - inclination.values.pitchDeciDegrees = lrintf(anglerad[AI_PITCH] * (1800.0f / M_PI)); + inclination.values.rollDeciDegrees = lrintf(anglerad[AI_ROLL] * (1800.0f / M_PIf)); + inclination.values.pitchDeciDegrees = lrintf(anglerad[AI_PITCH] * (1800.0f / M_PIf)); if (sensors(SENSOR_MAG)) { rotateV(&EstM.V, &deltaGyroAngle); @@ -338,16 +308,46 @@ static void getEstimatedAttitude(void) acc_calc(deltaT); // rotate acc vector into earth frame } -// correction of throttle in lateral wind, +void computeIMU(rollAndPitchTrims_t *accelerometerTrims, uint8_t mixerMode) +{ + static int16_t gyroYawSmooth = 0; + + gyroGetADC(); + if (sensors(SENSOR_ACC)) { + updateAccelerationReadings(accelerometerTrims); + getEstimatedAttitude(); + } else { + accADC[X] = 0; + accADC[Y] = 0; + accADC[Z] = 0; + } + + gyroData[FD_ROLL] = gyroADC[FD_ROLL]; + gyroData[FD_PITCH] = gyroADC[FD_PITCH]; + + if (mixerMode == MIXER_TRI) { + gyroData[FD_YAW] = (gyroYawSmooth * 2 + gyroADC[FD_YAW]) / 3; + gyroYawSmooth = gyroData[FD_YAW]; + } else { + gyroData[FD_YAW] = gyroADC[FD_YAW]; + } +} + +//TODO: Move throttle angle correction code into flight/throttle_angle_correction.c int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value) { float cosZ = EstG.V.Z / sqrtf(EstG.V.X * EstG.V.X + EstG.V.Y * EstG.V.Y + EstG.V.Z * EstG.V.Z); - if (cosZ <= 0.015f) { // we are inverted, vertical or with a small angle < 0.86 deg + /* + * Use 0 as the throttle angle correction if we are inverted, vertical or with a + * small angle < 0.86 deg + * TODO: Define this small angle in config. + */ + if (cosZ <= 0.015f) { return 0; } int angle = lrintf(acosf(cosZ) * throttleAngleScale); if (angle > 900) angle = 900; - return lrintf(throttle_correction_value * sinf(angle / (900.0f * M_PI / 2.0f))); + return lrintf(throttle_correction_value * sinf(angle / (900.0f * M_PIf / 2.0f))); } diff --git a/src/main/flight/imu.h b/src/main/flight/imu.h index de5ac89f01..45949c4e1e 100644 --- a/src/main/flight/imu.h +++ b/src/main/flight/imu.h @@ -30,12 +30,20 @@ typedef struct imuRuntimeConfig_s { int8_t small_angle; } imuRuntimeConfig_t; -void configureImu(imuRuntimeConfig_t *initialImuRuntimeConfig, pidProfile_t *initialPidProfile, accDeadband_t *initialAccDeadband); +void configureIMU( + imuRuntimeConfig_t *initialImuRuntimeConfig, + pidProfile_t *initialPidProfile, + accDeadband_t *initialAccDeadband, + float accz_lpf_cutoff, + uint16_t throttle_correction_angle +); void calculateEstimatedAltitude(uint32_t currentTime); void computeIMU(rollAndPitchTrims_t *accelerometerTrims, uint8_t mixerMode); -void calculateThrottleAngleScale(uint16_t throttle_correction_angle); +float calculateThrottleAngleScale(uint16_t throttle_correction_angle); int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value); -void calculateAccZLowPassFilterRCTimeConstant(float accz_lpf_cutoff); +float calculateAccZLowPassFilterRCTimeConstant(float accz_lpf_cutoff); + +int16_t calculateHeading(t_fp_vector *vec); void accSum_reset(void); diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 324bdf12fc..18c9e380fc 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -509,7 +509,7 @@ void mixTable(void) if (motorCount > 3) { // prevent "yaw jump" during yaw correction - axisPID[YAW] = constrain(axisPID[YAW], -100 - abs(rcCommand[YAW]), +100 + abs(rcCommand[YAW])); + axisPID[YAW] = constrain(axisPID[YAW], -100 - ABS(rcCommand[YAW]), +100 + ABS(rcCommand[YAW])); } // motors for non-servo mixes diff --git a/src/main/flight/navigation.c b/src/main/flight/navigation.c index 9eb5627e79..9c1e919cc8 100644 --- a/src/main/flight/navigation.c +++ b/src/main/flight/navigation.c @@ -163,7 +163,7 @@ static int32_t get_D(int32_t input, float *dt, PID *pid, PID_PARAM *pid_param) // Low pass filter cut frequency for derivative calculation // Set to "1 / ( 2 * PI * gps_lpf ) - float pidFilter = (1.0f / (2.0f * M_PI * (float)gpsProfile->gps_lpf)); + float pidFilter = (1.0f / (2.0f * M_PIf * (float)gpsProfile->gps_lpf)); // discrete low pass filter, cuts out the // high frequency noise that can drive the controller crazy pid->derivative = pid->last_derivative + (*dt / (pidFilter + *dt)) * (pid->derivative - pid->last_derivative); @@ -306,7 +306,7 @@ void onGpsNewData(void) dTnav = (float)(millis() - nav_loopTimer) / 1000.0f; nav_loopTimer = millis(); // prevent runup from bad GPS - dTnav = min(dTnav, 1.0f); + dTnav = MIN(dTnav, 1.0f); GPS_calculateDistanceAndDirectionToHome(); @@ -413,7 +413,7 @@ void gpsUsePIDs(pidProfile_t *pidProfile) // static void GPS_calc_longitude_scaling(int32_t lat) { - float rads = (abs((float)lat) / 10000000.0f) * 0.0174532925f; + float rads = (ABS((float)lat) / 10000000.0f) * 0.0174532925f; GPS_scaleLonDown = cosf(rads); } @@ -442,7 +442,7 @@ static bool check_missed_wp(void) int32_t temp; temp = target_bearing - original_target_bearing; temp = wrap_18000(temp); - return (abs(temp) > 10000); // we passed the waypoint by 100 degrees + return (ABS(temp) > 10000); // we passed the waypoint by 100 degrees } #define DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR_IN_HUNDREDS_OF_KILOMETERS 1.113195f @@ -536,7 +536,7 @@ static void GPS_calc_poshold(void) // get rid of noise #if defined(GPS_LOW_SPEED_D_FILTER) - if (abs(actual_speed[axis]) < 50) + if (ABS(actual_speed[axis]) < 50) d = 0; #endif @@ -582,7 +582,7 @@ static void GPS_calc_nav_rate(uint16_t max_speed) // static void GPS_update_crosstrack(void) { - if (abs(wrap_18000(target_bearing - original_target_bearing)) < 4500) { // If we are too far off or too close we don't do track following + if (ABS(wrap_18000(target_bearing - original_target_bearing)) < 4500) { // If we are too far off or too close we don't do track following float temp = (target_bearing - original_target_bearing) * RADX100; crosstrack_error = sinf(temp) * (wp_distance * CROSSTRACK_GAIN); // Meters we are off track line nav_bearing = target_bearing + constrain(crosstrack_error, -3000, 3000); @@ -607,10 +607,10 @@ static uint16_t GPS_calc_desired_speed(uint16_t max_speed, bool _slow) { // max_speed is default 400 or 4m/s if (_slow) { - max_speed = min(max_speed, wp_distance / 2); + max_speed = MIN(max_speed, wp_distance / 2); } else { - max_speed = min(max_speed, wp_distance); - max_speed = max(max_speed, gpsProfile->nav_speed_min); // go at least 100cm/s + max_speed = MIN(max_speed, wp_distance); + max_speed = MAX(max_speed, gpsProfile->nav_speed_min); // go at least 100cm/s } // limit the ramp up of the speed diff --git a/src/main/io/display.c b/src/main/io/display.c index 001615e874..ef103fd44e 100644 --- a/src/main/io/display.c +++ b/src/main/io/display.c @@ -292,12 +292,12 @@ void showProfilePage(void) void showGpsPage() { uint8_t rowIndex = PAGE_TITLE_LINE_COUNT; - i2c_OLED_set_xy(max(0, SATELLITE_GRAPH_LEFT_OFFSET), rowIndex++); + i2c_OLED_set_xy(MAX(0, SATELLITE_GRAPH_LEFT_OFFSET), rowIndex++); uint32_t index; for (index = 0; index < SATELLITE_COUNT && index < SCREEN_CHARACTER_COLUMN_COUNT; index++) { uint8_t bargraphValue = ((uint16_t) GPS_svinfo_cno[index] * VERTICAL_BARGRAPH_CHARACTER_COUNT) / (GPS_DBHZ_MAX - 1); - bargraphValue = min(bargraphValue, VERTICAL_BARGRAPH_CHARACTER_COUNT - 1); + bargraphValue = MIN(bargraphValue, VERTICAL_BARGRAPH_CHARACTER_COUNT - 1); i2c_OLED_send_char(VERTICAL_BARGRAPH_ZERO_CHARACTER + bargraphValue); } diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index 75eadc25da..53a95118cc 100644 --- a/src/main/io/ledstrip.c +++ b/src/main/io/ledstrip.c @@ -48,9 +48,14 @@ #include "io/ledstrip.h" static bool ledStripInitialised = false; +static bool ledStripEnabled = true; + static failsafe_t* failsafe; +static void ledStripDisable(void); + //#define USE_LED_ANIMATION +//#define USE_LED_RING_DEFAULT_CONFIG // timers #ifdef USE_LED_ANIMATION @@ -59,6 +64,7 @@ static uint32_t nextAnimationUpdateAt = 0; static uint32_t nextIndicatorFlashAt = 0; static uint32_t nextWarningFlashAt = 0; +static uint32_t nextRotationUpdateAt = 0; #define LED_STRIP_20HZ ((1000 * 1000) / 20) #define LED_STRIP_10HZ ((1000 * 1000) / 10) @@ -221,23 +227,44 @@ static const modeColorIndexes_t baroModeColors = { uint8_t ledGridWidth; uint8_t ledGridHeight; uint8_t ledCount; +uint8_t ledsInRingCount; ledConfig_t *ledConfigs; +hsvColor_t *colors; + +#ifdef USE_LED_RING_DEFAULT_CONFIG const ledConfig_t defaultLedStripConfig[] = { - { CALCULATE_LED_XY( 2, 2), LED_DIRECTION_SOUTH | LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, - { CALCULATE_LED_XY( 2, 1), LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, - { CALCULATE_LED_XY( 2, 0), LED_DIRECTION_NORTH | LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, - { CALCULATE_LED_XY( 1, 0), LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE }, - { CALCULATE_LED_XY( 0, 0), LED_DIRECTION_NORTH | LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, - { CALCULATE_LED_XY( 0, 1), LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, - { CALCULATE_LED_XY( 0, 2), LED_DIRECTION_SOUTH | LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, - { CALCULATE_LED_XY( 1, 2), LED_DIRECTION_SOUTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, - { CALCULATE_LED_XY( 1, 1), LED_DIRECTION_UP | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, - { CALCULATE_LED_XY( 1, 1), LED_DIRECTION_UP | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, - { CALCULATE_LED_XY( 1, 1), LED_DIRECTION_DOWN | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, - { CALCULATE_LED_XY( 1, 1), LED_DIRECTION_DOWN | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, + { CALCULATE_LED_XY( 2, 2), 3, LED_FUNCTION_THRUST_RING}, + { CALCULATE_LED_XY( 2, 1), 3, LED_FUNCTION_THRUST_RING}, + { CALCULATE_LED_XY( 2, 0), 3, LED_FUNCTION_THRUST_RING}, + { CALCULATE_LED_XY( 1, 0), 3, LED_FUNCTION_THRUST_RING}, + { CALCULATE_LED_XY( 0, 0), 3, LED_FUNCTION_THRUST_RING}, + { CALCULATE_LED_XY( 0, 1), 3, LED_FUNCTION_THRUST_RING}, + { CALCULATE_LED_XY( 0, 2), 3, LED_FUNCTION_THRUST_RING}, + { CALCULATE_LED_XY( 1, 2), 3, LED_FUNCTION_THRUST_RING}, + { CALCULATE_LED_XY( 1, 1), 3, LED_FUNCTION_THRUST_RING}, + { CALCULATE_LED_XY( 1, 1), 3, LED_FUNCTION_THRUST_RING}, + { CALCULATE_LED_XY( 1, 1), 3, LED_FUNCTION_THRUST_RING}, + { CALCULATE_LED_XY( 1, 1), 3, LED_FUNCTION_THRUST_RING}, }; +#else +const ledConfig_t defaultLedStripConfig[] = { + { CALCULATE_LED_XY( 2, 2), 0, LED_DIRECTION_SOUTH | LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, + { CALCULATE_LED_XY( 2, 1), 0, LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, + { CALCULATE_LED_XY( 2, 0), 0, LED_DIRECTION_NORTH | LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, + { CALCULATE_LED_XY( 1, 0), 0, LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE }, + { CALCULATE_LED_XY( 0, 0), 0, LED_DIRECTION_NORTH | LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, + { CALCULATE_LED_XY( 0, 1), 0, LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, + { CALCULATE_LED_XY( 0, 2), 0, LED_DIRECTION_SOUTH | LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, + { CALCULATE_LED_XY( 1, 2), 0, LED_DIRECTION_SOUTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, + { CALCULATE_LED_XY( 1, 1), 0, LED_DIRECTION_UP | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, + { CALCULATE_LED_XY( 1, 1), 0, LED_DIRECTION_UP | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, + { CALCULATE_LED_XY( 1, 1), 0, LED_DIRECTION_DOWN | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, + { CALCULATE_LED_XY( 1, 1), 0, LED_DIRECTION_DOWN | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, +}; +#endif + /* @@ -254,12 +281,13 @@ typedef enum { X_COORDINATE, Y_COORDINATE, DIRECTIONS, - FUNCTIONS + FUNCTIONS, + RING_COLORS } parseState_e; -#define PARSE_STATE_COUNT 4 +#define PARSE_STATE_COUNT 5 -static const char chunkSeparators[PARSE_STATE_COUNT] = {',', ':', ':', '\0' }; +static const char chunkSeparators[PARSE_STATE_COUNT] = {',', ':', ':',':', '\0' }; static const char directionCodes[] = { 'N', 'E', 'S', 'W', 'U', 'D' }; #define DIRECTION_COUNT (sizeof(directionCodes) / sizeof(directionCodes[0])) @@ -272,14 +300,15 @@ static const uint8_t directionMappings[DIRECTION_COUNT] = { LED_DIRECTION_DOWN }; -static const char functionCodes[] = { 'I', 'W', 'F', 'A', 'T' }; +static const char functionCodes[] = { 'I', 'W', 'F', 'A', 'T', 'R' }; #define FUNCTION_COUNT (sizeof(functionCodes) / sizeof(functionCodes[0])) static const uint16_t functionMappings[FUNCTION_COUNT] = { LED_FUNCTION_INDICATOR, LED_FUNCTION_WARNING, LED_FUNCTION_FLIGHT_MODE, LED_FUNCTION_ARM_STATE, - LED_FUNCTION_THROTTLE + LED_FUNCTION_THROTTLE, + LED_FUNCTION_THRUST_RING }; // grid offsets @@ -323,17 +352,28 @@ void determineOrientationLimits(void) void updateLedCount(void) { + const ledConfig_t *ledConfig; uint8_t ledIndex; ledCount = 0; + ledsInRingCount = 0; + for (ledIndex = 0; ledIndex < MAX_LED_STRIP_LENGTH; ledIndex++) { - if (ledConfigs[ledIndex].flags == 0 && ledConfigs[ledIndex].xy == 0) { + + ledConfig = &ledConfigs[ledIndex]; + + if (ledConfig->flags == 0 && ledConfig->xy == 0) { break; } + ledCount++; + + if ((ledConfig->flags & LED_FUNCTION_THRUST_RING)) { + ledsInRingCount++; + } } } -static void reevalulateLedConfig(void) +void reevalulateLedConfig(void) { updateLedCount(); determineLedStripDimensions(); @@ -406,6 +446,15 @@ bool parseLedStripConfig(uint8_t ledIndex, const char *config) } } break; + case RING_COLORS: + if (atoi(chunk) < CONFIGURABLE_COLOR_COUNT) { + ledConfig->color = atoi(chunk); + } else { + ledConfig->color = 0; + } + break; + default : + break; } parseState++; @@ -429,6 +478,7 @@ void generateLedConfig(uint8_t ledIndex, char *ledConfigBuffer, size_t bufferSiz char directions[DIRECTION_COUNT]; uint8_t index; uint8_t mappingIndex; + ledConfig_t *ledConfig = &ledConfigs[ledIndex]; memset(ledConfigBuffer, 0, bufferSize); @@ -447,7 +497,7 @@ void generateLedConfig(uint8_t ledIndex, char *ledConfigBuffer, size_t bufferSiz } } - sprintf(ledConfigBuffer, "%u,%u:%s:%s", GET_LED_X(ledConfig), GET_LED_Y(ledConfig), directions, functions); + sprintf(ledConfigBuffer, "%u,%u:%s:%s:%u", GET_LED_X(ledConfig), GET_LED_Y(ledConfig), directions, functions, ledConfig->color); } void applyDirectionalModeColor(const uint8_t ledIndex, const ledConfig_t *ledConfig, const modeColorIndexes_t *modeColors) @@ -525,7 +575,9 @@ void applyLedModeLayer(void) ledConfig = &ledConfigs[ledIndex]; - setLedHsv(ledIndex, &hsv_black); + if (!(ledConfig->flags & LED_FUNCTION_THRUST_RING)) { + setLedHsv(ledIndex, &hsv_black); + } if (!(ledConfig->flags & LED_FUNCTION_FLIGHT_MODE)) { if (ledConfig->flags & LED_FUNCTION_ARM_STATE) { @@ -678,7 +730,7 @@ void applyLedThrottleLayer() uint8_t ledIndex; for (ledIndex = 0; ledIndex < ledCount; ledIndex++) { ledConfig = &ledConfigs[ledIndex]; - if(!(ledConfig->flags & LED_FUNCTION_THROTTLE)) { + if (!(ledConfig->flags & LED_FUNCTION_THROTTLE)) { continue; } @@ -691,15 +743,63 @@ void applyLedThrottleLayer() } } -#ifdef USE_LED_ANIMATION -static uint8_t frameCounter = 0; +#define ROTATION_SEQUENCE_LED_COUNT 6 // 2 on, 4 off +#define ROTATION_SEQUENCE_LED_WIDTH 2 -static uint8_t previousRow; -static uint8_t currentRow; -static uint8_t nextRow; - -static void updateLedAnimationState(void) +void applyLedThrustRingLayer(void) { + uint8_t ledIndex; + static uint8_t rotationPhase = ROTATION_SEQUENCE_LED_COUNT; + bool nextLedOn = false; + hsvColor_t ringColor; + const ledConfig_t *ledConfig; + + uint8_t ledRingIndex = 0; + for (ledIndex = 0; ledIndex < ledCount; ledIndex++) { + + ledConfig = &ledConfigs[ledIndex]; + + if ((ledConfig->flags & LED_FUNCTION_THRUST_RING) == 0) { + continue; + } + + bool applyColor = false; + if (ARMING_FLAG(ARMED)) { + if ((ledRingIndex + rotationPhase) % ROTATION_SEQUENCE_LED_COUNT < ROTATION_SEQUENCE_LED_WIDTH) { + applyColor = true; + } + } else { + if (nextLedOn == false) { + applyColor = true; + } + nextLedOn = !nextLedOn; + } + + if (applyColor) { + ringColor = colors[ledConfig->color]; + } else { + ringColor = hsv_black; + } + + setLedHsv(ledIndex, &ringColor); + + ledRingIndex++; + } + + rotationPhase--; + if (rotationPhase == 0) { + rotationPhase = ROTATION_SEQUENCE_LED_COUNT; + } +} + +#ifdef USE_LED_ANIMATION +void updateLedAnimationState(void) +{ + static uint8_t frameCounter = 0; + + static uint8_t previousRow; + static uint8_t currentRow; + static uint8_t nextRow; uint8_t animationFrames = ledGridHeight; previousRow = (frameCounter + animationFrames - 1) % animationFrames; @@ -737,19 +837,36 @@ static void applyLedAnimationLayer(void) void updateLedStrip(void) { - if (!(ledStripInitialised && isWS2811LedStripReady())) { + + if (!(ledStripInitialised && isWS2811LedStripReady())) { return; } + if (IS_RC_MODE_ACTIVE(BOXLEDLOW)) { + if (ledStripEnabled) { + ledStripDisable(); + ledStripEnabled = false; + } + } else { + ledStripEnabled = true; + } + + if (!ledStripEnabled){ + return; + } + + uint32_t now = micros(); bool indicatorFlashNow = indicatorFlashNow = (int32_t)(now - nextIndicatorFlashAt) >= 0L; - bool warningFlashNow =warningFlashNow = (int32_t)(now - nextWarningFlashAt) >= 0L; + bool warningFlashNow = warningFlashNow = (int32_t)(now - nextWarningFlashAt) >= 0L; + bool rotationUpdateNow = (int32_t)(now - nextRotationUpdateAt) >= 0L; #ifdef USE_LED_ANIMATION bool animationUpdateNow = animationUpdateNow = (int32_t)(now - nextAnimationUpdateAt) >= 0L; #endif if (!( indicatorFlashNow || + rotationUpdateNow || warningFlashNow #ifdef USE_LED_ANIMATION || animationUpdateNow @@ -775,10 +892,10 @@ void updateLedStrip(void) if (indicatorFlashNow) { - uint8_t rollScale = abs(rcCommand[ROLL]) / 50; - uint8_t pitchScale = abs(rcCommand[PITCH]) / 50; - uint8_t scale = max(rollScale, pitchScale); - nextIndicatorFlashAt = now + (LED_STRIP_5HZ / max(1, scale)); + uint8_t rollScale = ABS(rcCommand[ROLL]) / 50; + uint8_t pitchScale = ABS(rcCommand[PITCH]) / 50; + uint8_t scale = MAX(rollScale, pitchScale); + nextIndicatorFlashAt = now + (LED_STRIP_5HZ / MAX(1, scale)); if (indicatorFlashState == 0) { indicatorFlashState = 1; @@ -794,10 +911,22 @@ void updateLedStrip(void) nextAnimationUpdateAt = now + LED_STRIP_20HZ; updateLedAnimationState(); } - applyLedAnimationLayer(); #endif + if (rotationUpdateNow) { + + applyLedThrustRingLayer(); + + uint8_t animationSpeedScale = 1; + + if (ARMING_FLAG(ARMED)) { + animationSpeedScale = scaleRange(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX, 1, 10); + } + + nextRotationUpdateAt = now + LED_STRIP_5HZ/animationSpeedScale; + } + ws2811UpdateStrip(); } @@ -817,6 +946,7 @@ bool parseColor(uint8_t index, const char *colorConfig) if (val > HSV_HUE_MAX) { ok = false; continue; + } colors[index].h = val; break; @@ -883,4 +1013,11 @@ void ledStripEnable(void) ws2811LedStripInit(); } + +static void ledStripDisable(void) +{ + setStripColor(&hsv_black); + + ws2811UpdateStrip(); +} #endif diff --git a/src/main/io/ledstrip.h b/src/main/io/ledstrip.h index 83776b7c21..c62dd4c4ce 100644 --- a/src/main/io/ledstrip.h +++ b/src/main/io/ledstrip.h @@ -18,6 +18,7 @@ #pragma once #define MAX_LED_STRIP_LENGTH 32 +#define CONFIGURABLE_COLOR_COUNT 16 #define LED_X_BIT_OFFSET 4 #define LED_Y_BIT_OFFSET 0 @@ -30,6 +31,7 @@ #define CALCULATE_LED_X(x) ((x & LED_XY_MASK) << LED_X_BIT_OFFSET) #define CALCULATE_LED_Y(y) ((y & LED_XY_MASK) << LED_Y_BIT_OFFSET) + #define CALCULATE_LED_XY(x,y) (CALCULATE_LED_X(x) | CALCULATE_LED_Y(y)) typedef enum { @@ -44,27 +46,43 @@ typedef enum { LED_FUNCTION_WARNING = (1 << 7), LED_FUNCTION_FLIGHT_MODE = (1 << 8), LED_FUNCTION_ARM_STATE = (1 << 9), - LED_FUNCTION_THROTTLE = (1 << 10) + LED_FUNCTION_THROTTLE = (1 << 10), + LED_FUNCTION_THRUST_RING = (1 << 11), } ledFlag_e; #define LED_DIRECTION_BIT_OFFSET 0 -#define LED_DIRECTION_MASK 0x3F +#define LED_DIRECTION_MASK ( \ + LED_DIRECTION_NORTH | \ + LED_DIRECTION_EAST | \ + LED_DIRECTION_SOUTH | \ + LED_DIRECTION_WEST | \ + LED_DIRECTION_UP | \ + LED_DIRECTION_DOWN \ +) #define LED_FUNCTION_BIT_OFFSET 6 -#define LED_FUNCTION_MASK 0x7C0 +#define LED_FUNCTION_MASK ( \ + LED_FUNCTION_INDICATOR | \ + LED_FUNCTION_WARNING | \ + LED_FUNCTION_FLIGHT_MODE | \ + LED_FUNCTION_ARM_STATE | \ + LED_FUNCTION_THROTTLE | \ + LED_FUNCTION_THRUST_RING \ +) typedef struct ledConfig_s { - uint8_t xy; // see LED_X/Y_MASK defines + uint8_t xy; // see LED_X/Y_MASK defines + uint8_t color; // see colors (config_master) uint16_t flags; // see ledFlag_e } ledConfig_t; extern uint8_t ledCount; -#define CONFIGURABLE_COLOR_COUNT 16 bool parseLedStripConfig(uint8_t ledIndex, const char *config); void updateLedStrip(void); +void updateLedRing(void); void applyDefaultLedStripConfig(ledConfig_t *ledConfig); void generateLedConfig(uint8_t ledIndex, char *ledConfigBuffer, size_t bufferSize); @@ -73,4 +91,5 @@ bool parseColor(uint8_t index, const char *colorConfig); void applyDefaultColors(hsvColor_t *colors, uint8_t colorCount); void ledStripEnable(void); +void reevalulateLedConfig(void); diff --git a/src/main/io/rc_controls.c b/src/main/io/rc_controls.c index 9bd4560085..64a70ba40e 100644 --- a/src/main/io/rc_controls.c +++ b/src/main/io/rc_controls.c @@ -64,7 +64,7 @@ bool isUsingSticksForArming(void) bool areSticksInApModePosition(uint16_t ap_mode) { - return abs(rcCommand[ROLL]) < ap_mode && abs(rcCommand[PITCH]) < ap_mode; + return ABS(rcCommand[ROLL]) < ap_mode && ABS(rcCommand[PITCH]) < ap_mode; } throttleStatus_e calculateThrottleStatus(rxConfig_t *rxConfig, uint16_t deadband3d_throttle) @@ -518,7 +518,7 @@ void updateAdjustmentStates(adjustmentRange_t *adjustmentRanges) } int32_t getRcStickDeflection(int32_t axis, uint16_t midrc) { - return min(abs(rcData[axis] - midrc), 500); + return MIN(ABS(rcData[axis] - midrc), 500); } void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, escAndServoConfig_t *escAndServoConfigToUse, pidProfile_t *pidProfileToUse) diff --git a/src/main/io/serial.c b/src/main/io/serial.c index 185ecf51c5..e9e9a47122 100644 --- a/src/main/io/serial.c +++ b/src/main/io/serial.c @@ -73,26 +73,32 @@ static serialPort_t *serialPorts[SERIAL_PORT_COUNT]; #ifdef STM32F303xC static serialPortFunction_t serialPortFunctions[SERIAL_PORT_COUNT] = { +#ifdef USE_VCP {SERIAL_PORT_USB_VCP, NULL, SCENARIO_UNUSED, FUNCTION_NONE}, - {SERIAL_PORT_USART1, NULL, SCENARIO_UNUSED, FUNCTION_NONE}, - {SERIAL_PORT_USART2, NULL, SCENARIO_UNUSED, FUNCTION_NONE}, -#if (SERIAL_PORT_COUNT > 3) - {SERIAL_PORT_USART3, NULL, SCENARIO_UNUSED, FUNCTION_NONE}, -#if (SERIAL_PORT_COUNT > 4) - {SERIAL_PORT_USART4, NULL, SCENARIO_UNUSED, FUNCTION_NONE} #endif +#ifdef USE_USART1 + {SERIAL_PORT_USART1, NULL, SCENARIO_UNUSED, FUNCTION_NONE}, +#endif +#ifdef USE_USART2 + {SERIAL_PORT_USART2, NULL, SCENARIO_UNUSED, FUNCTION_NONE}, +#endif +#ifdef USE_USART3 + {SERIAL_PORT_USART3, NULL, SCENARIO_UNUSED, FUNCTION_NONE}, #endif }; const serialPortConstraint_t serialPortConstraints[SERIAL_PORT_COUNT] = { +#ifdef USE_VCP {SERIAL_PORT_USB_VCP, 9600, 115200, SPF_NONE }, - {SERIAL_PORT_USART1, 9600, 115200, SPF_NONE | SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_BIDIR_MODE}, - {SERIAL_PORT_USART2, 9600, 115200, SPF_SUPPORTS_CALLBACK | SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_BIDIR_MODE}, -#if (SERIAL_PORT_COUNT > 3) - {SERIAL_PORT_USART3, 9600, 115200, SPF_SUPPORTS_CALLBACK | SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_BIDIR_MODE}, -#if (SERIAL_PORT_COUNT > 4) - {SERIAL_PORT_USART4, 9600, 115200, SPF_SUPPORTS_CALLBACK} #endif +#ifdef USE_USART1 + {SERIAL_PORT_USART1, 9600, 115200, SPF_NONE | SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_BIDIR_MODE}, +#endif +#ifdef USE_USART2 + {SERIAL_PORT_USART2, 9600, 115200, SPF_SUPPORTS_CALLBACK | SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_BIDIR_MODE}, +#endif +#ifdef USE_USART3 + {SERIAL_PORT_USART3, 9600, 115200, SPF_SUPPORTS_CALLBACK | SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_BIDIR_MODE}, #endif }; diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index b422f393d1..2b8ca9f64e 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -121,7 +121,7 @@ void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, es #define MSP_PROTOCOL_VERSION 0 #define API_VERSION_MAJOR 1 // 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 4 // increment when any change is made, reset to zero when major changes are released after changing API_VERSION_MAJOR #define API_VERSION_LENGTH 2 @@ -617,6 +617,12 @@ void mspInit(serialConfig_t *serialConfig) activeBoxIds[activeBoxIdCount++] = BOXBEEPERON; +#ifdef LED_STRIP + if (feature(FEATURE_LED_STRIP)) { + activeBoxIds[activeBoxIdCount++] = BOXLEDLOW; + } +#endif + if (feature(FEATURE_INFLIGHT_ACC_CAL)) activeBoxIds[activeBoxIdCount++] = BOXCALIB; @@ -746,6 +752,7 @@ static bool processOutCommand(uint8_t cmdMSP) IS_ENABLED(FLIGHT_MODE(PASSTHRU_MODE)) << BOXPASSTHRU | IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBEEPERON)) << BOXBEEPERON | IS_ENABLED(IS_RC_MODE_ACTIVE(BOXLEDMAX)) << BOXLEDMAX | + IS_ENABLED(IS_RC_MODE_ACTIVE(BOXLEDLOW)) << BOXLEDLOW | IS_ENABLED(IS_RC_MODE_ACTIVE(BOXLLIGHTS)) << BOXLLIGHTS | IS_ENABLED(IS_RC_MODE_ACTIVE(BOXCALIB)) << BOXCALIB | IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGOV)) << BOXGOV | @@ -820,9 +827,9 @@ static bool processOutCommand(uint8_t cmdMSP) serialize16((uint16_t)constrain(mAhDrawn, 0, 0xFFFF)); // milliamphours drawn from battery serialize16(rssi); if(masterConfig.batteryConfig.multiwiiCurrentMeterOutput) { - serialize16((uint16_t)constrain((abs(amperage) * 10), 0, 0xFFFF)); // send amperage in 0.001 A steps + serialize16((uint16_t)constrain((ABS(amperage) * 10), 0, 0xFFFF)); // send amperage in 0.001 A steps } else - serialize16((uint16_t)constrain(abs(amperage), 0, 0xFFFF)); // send amperage in 0.01 A steps + serialize16((uint16_t)constrain(ABS(amperage), 0, 0xFFFF)); // send amperage in 0.01 A steps break; case MSP_RC_TUNING: headSerialReply(7); @@ -1097,13 +1104,14 @@ static bool processOutCommand(uint8_t cmdMSP) break; case MSP_LED_STRIP_CONFIG: - headSerialReply(MAX_LED_STRIP_LENGTH * 6); + headSerialReply(MAX_LED_STRIP_LENGTH * 7); for (i = 0; i < MAX_LED_STRIP_LENGTH; i++) { ledConfig_t *ledConfig = &masterConfig.ledConfigs[i]; serialize16((ledConfig->flags & LED_DIRECTION_MASK) >> LED_DIRECTION_BIT_OFFSET); serialize16((ledConfig->flags & LED_FUNCTION_MASK) >> LED_FUNCTION_BIT_OFFSET); serialize8(GET_LED_X(ledConfig)); serialize8(GET_LED_Y(ledConfig)); + serialize8(ledConfig->color); } break; #endif @@ -1443,7 +1451,7 @@ static bool processInCommand(void) case MSP_SET_LED_STRIP_CONFIG: { i = read8(); - if (i >= MAX_LED_STRIP_LENGTH || currentPort->dataSize != 7) { + if (i >= MAX_LED_STRIP_LENGTH || currentPort->dataSize != (1 + 7)) { headSerialError(0); break; } @@ -1462,6 +1470,10 @@ static bool processInCommand(void) mask = read8(); ledConfig->xy |= CALCULATE_LED_Y(mask); + + ledConfig->color = read8(); + + reevalulateLedConfig(); } break; #endif diff --git a/src/main/main.c b/src/main/main.c index bcbd18412e..3e300b8156 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -104,7 +104,7 @@ void beepcodeInit(failsafe_t *initialFailsafe); void gpsInit(serialConfig_t *serialConfig, gpsConfig_t *initialGpsConfig); void navigationInit(gpsProfile_t *initialGpsProfile, pidProfile_t *pidProfile); bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint16_t gyroLpf, uint8_t accHardwareToUse, int8_t magHardwareToUse, int16_t magDeclinationFromConfig); -void imuInit(void); +void initIMU(void); void displayInit(rxConfig_t *intialRxConfig); void ledStripInit(ledConfig_t *ledConfigsToUse, hsvColor_t *colorsToUse, failsafe_t* failsafeToUse); void loop(void); @@ -206,17 +206,20 @@ void init(void) #endif #ifdef USE_I2C -#ifdef NAZE +#if defined(NAZE) if (hardwareRevision != NAZE32_SP) { i2cInit(I2C_DEVICE); } +#elif defined(CC3D) + if (!doesConfigurationUsePort(SERIAL_PORT_USART3)) { + i2cInit(I2C_DEVICE); + } #else - // Configure the rest of the stuff i2cInit(I2C_DEVICE); #endif #endif -#if !defined(SPARKY) +#ifdef USE_ADC drv_adc_config_t adc_params; adc_params.enableRSSI = feature(FEATURE_RSSI_ADC); @@ -264,7 +267,7 @@ void init(void) LED0_OFF; LED1_OFF; - imuInit(); + initIMU(); mixerInit(masterConfig.mixerMode, masterConfig.customMixer); #ifdef MAG diff --git a/src/main/mw.c b/src/main/mw.c index 9b2a45e0a6..f7081090b1 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -182,7 +182,7 @@ void annexCode(void) } for (axis = 0; axis < 3; axis++) { - tmp = min(abs(rcData[axis] - masterConfig.rxConfig.midrc), 500); + tmp = MIN(ABS(rcData[axis] - masterConfig.rxConfig.midrc), 500); if (axis == ROLL || axis == PITCH) { if (currentProfile->rcControlsConfig.deadband) { if (tmp > currentProfile->rcControlsConfig.deadband) { @@ -205,7 +205,7 @@ void annexCode(void) } } rcCommand[axis] = tmp * -masterConfig.yaw_control_direction; - prop1 = 100 - (uint16_t)currentControlRateProfile->yawRate * abs(tmp) / 500; + prop1 = 100 - (uint16_t)currentControlRateProfile->yawRate * ABS(tmp) / 500; } // FIXME axis indexes into pids. use something like lookupPidIndex(rc_alias_e alias) to reduce coupling. dynP8[axis] = (uint16_t)currentProfile->pidProfile.P8[axis] * prop1 / 100; @@ -397,7 +397,7 @@ void updateInflightCalibrationState(void) void updateMagHold(void) { - if (abs(rcCommand[YAW]) < 70 && FLIGHT_MODE(MAG_MODE)) { + if (ABS(rcCommand[YAW]) < 70 && FLIGHT_MODE(MAG_MODE)) { int16_t dif = heading - magHold; if (dif <= -180) dif += 360; diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index 8e4bda1e85..8ab48f534b 100644 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -289,7 +289,7 @@ void processRxChannels(void) uint16_t sample = rcReadRawFunc(&rxRuntimeConfig, rawChannel); if (feature(FEATURE_FAILSAFE) && shouldCheckPulse) { - failsafe->vTable->checkPulse(rawChannel, sample); + failsafe->vTable->checkPulse(chan, sample); } // validate the range diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index f3c3e80288..a367cb8f85 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -36,7 +36,8 @@ uint16_t batteryWarningVoltage; uint16_t batteryCriticalVoltage; uint8_t vbat = 0; // battery voltage in 0.1V steps -uint16_t vbatLatest = 0; // most recent unsmoothed raw reading from vbat adc +uint16_t vbatLatestADC = 0; // most recent unsmoothed raw reading from vbat ADC +uint16_t amperageLatestADC = 0; // most recent raw reading from current ADC int32_t amperage = 0; // amperage read by current sensor in centiampere (1/100th A) int32_t mAhDrawn = 0; // milliampere hours drawn from the battery since start @@ -60,7 +61,7 @@ void updateBatteryVoltage(void) uint16_t vbatSampleTotal = 0; // store the battery voltage with some other recent battery voltage readings - vbatSamples[(currentSampleIndex++) % BATTERY_SAMPLE_COUNT] = vbatLatest = adcGetChannel(ADC_BATTERY); + vbatSamples[(currentSampleIndex++) % BATTERY_SAMPLE_COUNT] = vbatLatestADC = adcGetChannel(ADC_BATTERY); // calculate vbat based on the average of recent readings for (index = 0; index < BATTERY_SAMPLE_COUNT; index++) { @@ -123,7 +124,7 @@ void updateCurrentMeter(int32_t lastUpdateAt) switch(batteryConfig->currentMeterType) { case CURRENT_SENSOR_ADC: amperageRaw -= amperageRaw / 8; - amperageRaw += adcGetChannel(ADC_CURRENT); + amperageRaw += (amperageLatestADC = adcGetChannel(ADC_CURRENT)); amperage = currentSensorToCentiamps(amperageRaw / 8); break; case CURRENT_SENSOR_VIRTUAL: diff --git a/src/main/sensors/battery.h b/src/main/sensors/battery.h index 38d2dd17d7..1b255a0b84 100644 --- a/src/main/sensors/battery.h +++ b/src/main/sensors/battery.h @@ -50,9 +50,10 @@ typedef enum { } batteryState_e; extern uint8_t vbat; -extern uint16_t vbatLatest; +extern uint16_t vbatLatestADC; extern uint8_t batteryCellCount; extern uint16_t batteryWarningVoltage; +extern uint16_t amperageLatestADC; extern int32_t amperage; extern int32_t mAhDrawn; diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c index d13711ec66..099e6fe000 100644 --- a/src/main/sensors/initialisation.c +++ b/src/main/sensors/initialisation.c @@ -94,6 +94,15 @@ const mpu6050Config_t *selectMPU6050Config(void) return &nazeRev5MPU6050Config; } #endif + +#ifdef SPRACINGF3 + static const mpu6050Config_t spRacingF3MPU6050Config = { + .gpioAHBPeripherals = RCC_AHBPeriph_GPIOC, + .gpioPort = GPIOC, + .gpioPin = Pin_13 + }; + return &spRacingF3MPU6050Config; +#endif return NULL; } @@ -164,8 +173,8 @@ bool detectGyro(uint16_t gyroLpf) #ifdef USE_GYRO_L3GD20 if (l3gd20Detect(&gyro, gyroLpf)) { -#ifdef GYRO_GYRO_L3GD20_ALIGN - gyroAlign = GYRO_GYRO_L3GD20_ALIGN; +#ifdef GYRO_L3GD20_ALIGN + gyroAlign = GYRO_L3GD20_ALIGN; #endif return true; } @@ -406,6 +415,17 @@ static void detectMag(uint8_t magHardwareToUse) hmc5883Config = &nazeHmc5883Config; #endif + +#ifdef SPRACINGF3 + hmc5883Config_t spRacingF3Hmc5883Config = { + .gpioAHBPeripherals = RCC_AHBPeriph_GPIOC, + .gpioPin = Pin_14, + .gpioPort = GPIOC + }; + + hmc5883Config = &spRacingF3Hmc5883Config; +#endif + #endif retry: diff --git a/src/main/startup/startup_stm32f30x_md_gcc.S b/src/main/startup/startup_stm32f30x_md_gcc.S index 9c2af124cb..e1387cbc0e 100644 --- a/src/main/startup/startup_stm32f30x_md_gcc.S +++ b/src/main/startup/startup_stm32f30x_md_gcc.S @@ -70,6 +70,12 @@ defined in linker script */ .weak Reset_Handler .type Reset_Handler, %function Reset_Handler: + ldr r0, =0x20009FFC // HJI 11/9/2012 + ldr r1, =0xDEADBEEF // HJI 11/9/2012 + ldr r2, [r0, #0] // HJI 11/9/2012 + str r0, [r0, #0] // HJI 11/9/2012 + cmp r2, r1 // HJI 11/9/2012 + beq Reboot_Loader // HJI 11/9/2012 /* Copy the data segment initializers from flash to SRAM */ movs r1, #0 @@ -104,6 +110,18 @@ LoopFillZerobss: /* Call the application's entry point.*/ bl main bx lr + +LoopForever: + b LoopForever + +Reboot_Loader: // HJI 11/9/2012 + + // Reboot to ROM // HJI 11/9/2012 + ldr r0, =0x1FFFD800 // HJI 4/26/2013 + ldr sp,[r0, #0] // HJI 11/9/2012 + ldr r0,[r0, #4] // HJI 11/9/2012 + bx r0 // HJI 11/9/2012 + .size Reset_Handler, .-Reset_Handler /** diff --git a/src/main/target/CC3D/target.h b/src/main/target/CC3D/target.h index 3ae21223b3..26a1358c61 100644 --- a/src/main/target/CC3D/target.h +++ b/src/main/target/CC3D/target.h @@ -35,6 +35,8 @@ #define MPU6000_CS_PIN GPIO_Pin_4 #define MPU6000_SPI_INSTANCE SPI1 +#define USABLE_TIMER_CHANNEL_COUNT 12 + #define GYRO #define USE_GYRO_SPI_MPU6000 @@ -45,8 +47,18 @@ #define ACC_SPI_MPU6000_ALIGN CW270_DEG +// External I2C BARO +#define BARO +#define USE_BARO_MS5611 +#define USE_BARO_BMP085 + +// External I2C MAG +#define MAG +#define USE_MAG_HMC5883 + #define INVERTER #define BEEPER +#define DISPLAY #define USE_USART1 #define USE_USART3 @@ -57,10 +69,6 @@ #define SOFTSERIAL_1_TIMER_TX_HARDWARE 1 // PWM 2 #define SOFTSERIAL_1_TIMER_RX_HARDWARE 2 // PWM 3 -#define CurrentMeter_TIMER 3 // PWM4 -#define Vbat_TIMER 4 // PWM5 -#define RSSI_TIMER 5 // PWM6 - #define USART3_RX_PIN Pin_11 #define USART3_TX_PIN Pin_10 #define USART3_GPIO GPIOB @@ -71,6 +79,24 @@ #define USE_SPI_DEVICE_1 #define USE_SPI_DEVICE_2 +#define USE_I2C +#define I2C_DEVICE (I2CDEV_2) // Flex port - SCL/PB10, SDA/PB11 + +#define USE_ADC + +#define CURRENT_METER_ADC_GPIO GPIOB +#define CURRENT_METER_ADC_GPIO_PIN GPIO_Pin_1 +#define CURRENT_METER_ADC_CHANNEL ADC_Channel_9 + +#define VBAT_ADC_GPIO GPIOA +#define VBAT_ADC_GPIO_PIN GPIO_Pin_0 +#define VBAT_ADC_CHANNEL ADC_Channel_0 + +#define RSSI_ADC_GPIO GPIOA +#define RSSI_ADC_GPIO_PIN GPIO_Pin_1 +#define RSSI_ADC_CHANNEL ADC_Channel_1 + + #define SENSORS_SET (SENSOR_ACC) #define GPS diff --git a/src/main/target/CHEBUZZF3/target.h b/src/main/target/CHEBUZZF3/target.h index de560a7fc5..cc699343cf 100644 --- a/src/main/target/CHEBUZZF3/target.h +++ b/src/main/target/CHEBUZZF3/target.h @@ -33,10 +33,13 @@ #define BEEP_PERIPHERAL RCC_AHBPeriph_GPIOE #define BEEPER_INVERTED +#define USABLE_TIMER_CHANNEL_COUNT 18 + #define GYRO #define USE_GYRO_L3GD20 #define USE_GYRO_MPU6050 +#define GYRO_L3GD20_ALIGN CW90_DEG #define GYRO_MPU6050_ALIGN CW0_DEG #define ACC @@ -65,6 +68,8 @@ #define USE_I2C #define I2C_DEVICE (I2CDEV_1) +#define USE_ADC + #define SENSORS_SET (SENSOR_ACC | SENSOR_BARO | SENSOR_MAG) #define GPS diff --git a/src/main/target/EUSTM32F103RC/target.h b/src/main/target/EUSTM32F103RC/target.h index 8ef66c2ccc..ccc9bebaa3 100644 --- a/src/main/target/EUSTM32F103RC/target.h +++ b/src/main/target/EUSTM32F103RC/target.h @@ -98,6 +98,24 @@ // #define SOFT_I2C_PB1011 // If SOFT_I2C is enabled above, need to define pinout as well (I2C1 = PB67, I2C2 = PB1011) // #define SOFT_I2C_PB67 +#define USE_ADC + +#define CURRENT_METER_ADC_GPIO GPIOB +#define CURRENT_METER_ADC_GPIO_PIN GPIO_Pin_1 +#define CURRENT_METER_ADC_CHANNEL ADC_Channel_9 + +#define VBAT_ADC_GPIO GPIOA +#define VBAT_ADC_GPIO_PIN GPIO_Pin_4 +#define VBAT_ADC_CHANNEL ADC_Channel_4 + +#define RSSI_ADC_GPIO GPIOA +#define RSSI_ADC_GPIO_PIN GPIO_Pin_1 +#define RSSI_ADC_CHANNEL ADC_Channel_1 + +#define EXTERNAL1_ADC_GPIO GPIOA +#define EXTERNAL1_ADC_GPIO_PIN GPIO_Pin_5 +#define EXTERNAL1_ADC_CHANNEL ADC_Channel_5 + #define SENSORS_SET (SENSOR_ACC | SENSOR_BARO | SENSOR_MAG) #define GPS diff --git a/src/main/target/MASSIVEF3/target.h b/src/main/target/MASSIVEF3/target.h deleted file mode 100644 index 1484ec94a9..0000000000 --- a/src/main/target/MASSIVEF3/target.h +++ /dev/null @@ -1,63 +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 . - */ - -#pragma once - -#define TARGET_BOARD_IDENTIFIER "MF3A" - -#define LED0_GPIO GPIOE -#define LED0_PIN Pin_8|Pin_12 // Blue LEDs - PE8/PE12 -#define LED0_PERIPHERAL RCC_AHBPeriph_GPIOE -#define LED0_INVERTED -#define LED1_GPIO GPIOE -#define LED1_PIN Pin_10|Pin_14 // Orange LEDs - PE10/PE14 -#define LED1_PERIPHERAL RCC_AHBPeriph_GPIOE -#define LED1_INVERTED - -#define BEEP_GPIO GPIOE -#define BEEP_PIN Pin_9|Pin_13 // Red LEDs - PE9/PE13 -#define BEEP_PERIPHERAL RCC_AHBPeriph_GPIOE -#define BEEPER_INVERTED - - -#define BEEPER_INVERTED - -#define GYRO -#define ACC - -#define BARO -#define USE_BARO_MS5611 -#define USE_BARO_BMP085 - -#define BEEPER -#define LED0 -#define LED1 - -#define USE_VCP -#define USE_USART1 -#define USE_USART2 -#define SERIAL_PORT_COUNT 3 - -#define USE_I2C -#define I2C_DEVICE (I2CDEV_1) - -#define SENSORS_SET (SENSOR_ACC) - -#define GPS -#define TELEMETRY -#define SERIAL_RX -#define AUTOTUNE diff --git a/src/main/target/NAZE/target.h b/src/main/target/NAZE/target.h index 1686d523c9..ffd179baaa 100644 --- a/src/main/target/NAZE/target.h +++ b/src/main/target/NAZE/target.h @@ -117,6 +117,23 @@ // #define SOFT_I2C_PB1011 // If SOFT_I2C is enabled above, need to define pinout as well (I2C1 = PB67, I2C2 = PB1011) // #define SOFT_I2C_PB67 +#define USE_ADC + +#define CURRENT_METER_ADC_GPIO GPIOB +#define CURRENT_METER_ADC_GPIO_PIN GPIO_Pin_1 +#define CURRENT_METER_ADC_CHANNEL ADC_Channel_9 + +#define VBAT_ADC_GPIO GPIOA +#define VBAT_ADC_GPIO_PIN GPIO_Pin_4 +#define VBAT_ADC_CHANNEL ADC_Channel_4 + +#define RSSI_ADC_GPIO GPIOA +#define RSSI_ADC_GPIO_PIN GPIO_Pin_1 +#define RSSI_ADC_CHANNEL ADC_Channel_1 + +#define EXTERNAL1_ADC_GPIO GPIOA +#define EXTERNAL1_ADC_GPIO_PIN GPIO_Pin_5 +#define EXTERNAL1_ADC_CHANNEL ADC_Channel_5 #define SENSORS_SET (SENSOR_ACC | SENSOR_BARO | SENSOR_MAG) diff --git a/src/main/target/OLIMEXINO/target.h b/src/main/target/OLIMEXINO/target.h index 935ff691c5..34ec32a7dc 100644 --- a/src/main/target/OLIMEXINO/target.h +++ b/src/main/target/OLIMEXINO/target.h @@ -85,6 +85,25 @@ // #define SOFT_I2C_PB1011 // If SOFT_I2C is enabled above, need to define pinout as well (I2C1 = PB67, I2C2 = PB1011) // #define SOFT_I2C_PB67 +#define USE_ADC + +#define CURRENT_METER_ADC_GPIO GPIOB +#define CURRENT_METER_ADC_GPIO_PIN GPIO_Pin_1 +#define CURRENT_METER_ADC_CHANNEL ADC_Channel_9 + +#define VBAT_ADC_GPIO GPIOA +#define VBAT_ADC_GPIO_PIN GPIO_Pin_4 +#define VBAT_ADC_CHANNEL ADC_Channel_4 + +#define RSSI_ADC_GPIO GPIOA +#define RSSI_ADC_GPIO_PIN GPIO_Pin_1 +#define RSSI_ADC_CHANNEL ADC_Channel_1 + +#define EXTERNAL1_ADC_GPIO GPIOA +#define EXTERNAL1_ADC_GPIO_PIN GPIO_Pin_5 +#define EXTERNAL1_ADC_CHANNEL ADC_Channel_5 + + #define SENSORS_SET (SENSOR_ACC | SENSOR_BARO | SENSOR_MAG) #define GPS diff --git a/src/main/target/PORT103R/target.h b/src/main/target/PORT103R/target.h index 0f7805213f..d371f69727 100644 --- a/src/main/target/PORT103R/target.h +++ b/src/main/target/PORT103R/target.h @@ -95,6 +95,24 @@ // #define SOFT_I2C_PB1011 // If SOFT_I2C is enabled above, need to define pinout as well (I2C1 = PB67, I2C2 = PB1011) // #define SOFT_I2C_PB67 +#define USE_ADC + +#define CURRENT_METER_ADC_GPIO GPIOB +#define CURRENT_METER_ADC_GPIO_PIN GPIO_Pin_1 +#define CURRENT_METER_ADC_CHANNEL ADC_Channel_9 + +#define VBAT_ADC_GPIO GPIOA +#define VBAT_ADC_GPIO_PIN GPIO_Pin_4 +#define VBAT_ADC_CHANNEL ADC_Channel_4 + +#define RSSI_ADC_GPIO GPIOA +#define RSSI_ADC_GPIO_PIN GPIO_Pin_1 +#define RSSI_ADC_CHANNEL ADC_Channel_1 + +#define EXTERNAL1_ADC_GPIO GPIOA +#define EXTERNAL1_ADC_GPIO_PIN GPIO_Pin_5 +#define EXTERNAL1_ADC_CHANNEL ADC_Channel_5 + #define SENSORS_SET (SENSOR_ACC | SENSOR_BARO | SENSOR_MAG) #define LED0 diff --git a/src/main/target/SPARKY/target.h b/src/main/target/SPARKY/target.h index d020296653..aee9257cd1 100644 --- a/src/main/target/SPARKY/target.h +++ b/src/main/target/SPARKY/target.h @@ -26,6 +26,8 @@ #define LED1_PIN Pin_5 // Green LEDs - PB5 #define LED1_PERIPHERAL RCC_AHBPeriph_GPIOB +#define USABLE_TIMER_CHANNEL_COUNT 11 + // MPU 9150 INT connected to PA15, pulled up to VCC by 10K Resistor, contains MPU6050 and AK8975 in single component. #define GYRO #define USE_GYRO_MPU6050 diff --git a/src/main/target/MASSIVEF3/system_stm32f30x.c b/src/main/target/SPRACINGF3/system_stm32f30x.c similarity index 99% rename from src/main/target/MASSIVEF3/system_stm32f30x.c rename to src/main/target/SPRACINGF3/system_stm32f30x.c index 8166961185..fca6969825 100644 --- a/src/main/target/MASSIVEF3/system_stm32f30x.c +++ b/src/main/target/SPRACINGF3/system_stm32f30x.c @@ -55,9 +55,9 @@ *----------------------------------------------------------------------------- * APB1 Prescaler | 2 *----------------------------------------------------------------------------- - * HSE Frequency(Hz) | 12000000 + * HSE Frequency(Hz) | 8000000 *---------------------------------------------------------------------------- - * PLLMUL | 6 + * PLLMUL | 9 *----------------------------------------------------------------------------- * PREDIV | 1 *----------------------------------------------------------------------------- diff --git a/src/main/target/MASSIVEF3/system_stm32f30x.h b/src/main/target/SPRACINGF3/system_stm32f30x.h similarity index 100% rename from src/main/target/MASSIVEF3/system_stm32f30x.h rename to src/main/target/SPRACINGF3/system_stm32f30x.h diff --git a/src/main/target/SPRACINGF3/target.h b/src/main/target/SPRACINGF3/target.h new file mode 100644 index 0000000000..af35c8a182 --- /dev/null +++ b/src/main/target/SPRACINGF3/target.h @@ -0,0 +1,86 @@ +/* + * 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 "SRF3" + +#define LED0_GPIO GPIOB +#define LED0_PIN Pin_3 +#define LED0_PERIPHERAL RCC_AHBPeriph_GPIOB + +#define BEEP_GPIO GPIOC +#define BEEP_PIN Pin_15 +#define BEEP_PERIPHERAL RCC_AHBPeriph_GPIOC + +#define USABLE_TIMER_CHANNEL_COUNT 17 + +#define GYRO +#define USE_GYRO_MPU6050 + +#define ACC +#define USE_ACC_MPU6050 + +#define BARO +#define USE_BARO_MS5611 + +#define MAG +#define USE_MAG_HMC5883 + +#define BEEPER +#define LED0 + +#define USE_USART1 +#define USE_USART2 +#define USE_USART3 +#define SERIAL_PORT_COUNT 3 + +#define UART2_TX_PIN GPIO_Pin_14 // PA14 / SWCLK +#define UART2_RX_PIN GPIO_Pin_15 // PA15 +#define UART2_GPIO GPIOA +#define UART2_GPIO_AF GPIO_AF_7 +#define UART2_TX_PINSOURCE GPIO_PinSource14 +#define UART2_RX_PINSOURCE GPIO_PinSource15 + +#define USE_I2C +#define I2C_DEVICE (I2CDEV_1) // PB6/SCL, PB7/SDA + +//#define USE_SPI +//#define USE_SPI_DEVICE_2 // PB12,13,14,15 on AF5 + +#define LED_STRIP +#define LED_STRIP_TIMER TIM1 + +#define USE_LED_STRIP_ON_DMA1_CHANNEL2 +#define WS2811_GPIO GPIOA +#define WS2811_GPIO_AHB_PERIPHERAL RCC_AHBPeriph_GPIOA +#define WS2811_GPIO_AF GPIO_AF_6 +#define WS2811_PIN GPIO_Pin_8 +#define WS2811_PIN_SOURCE GPIO_PinSource8 +#define WS2811_TIMER TIM1 +#define WS2811_TIMER_APB2_PERIPHERAL RCC_APB2Periph_TIM1 +#define WS2811_DMA_CHANNEL DMA1_Channel2 +#define WS2811_IRQ DMA1_Channel2_IRQn + +#define SENSORS_SET (SENSOR_ACC | SENSOR_BARO | SENSOR_MAG) + +#define GPS +#define BLACKBOX +#define TELEMETRY +#define SERIAL_RX +#define AUTOTUNE + diff --git a/src/main/target/STM32F3DISCOVERY/target.h b/src/main/target/STM32F3DISCOVERY/target.h index 561a2f0581..ab840213fb 100644 --- a/src/main/target/STM32F3DISCOVERY/target.h +++ b/src/main/target/STM32F3DISCOVERY/target.h @@ -39,9 +39,14 @@ #define GYRO #define USE_GYRO_L3GD20 +#define GYRO_L3GD20_ALIGN CW90_DEG + #define ACC #define USE_ACC_LSM303DLHC +#define MAG +#define USE_MAG_HMC5883 + #define BEEPER #define LED0 #define LED1 @@ -54,7 +59,9 @@ #define USE_I2C #define I2C_DEVICE (I2CDEV_1) -#define SENSORS_SET (SENSOR_ACC) +#define USE_ADC + +#define SENSORS_SET (SENSOR_ACC | SENSOR_MAG) #define BLACKBOX #define GPS diff --git a/src/main/target/stm32_flash_f303_128k.ld b/src/main/target/stm32_flash_f303_128k.ld index da6cbda9d4..3fe0aa8d52 100644 --- a/src/main/target/stm32_flash_f303_128k.ld +++ b/src/main/target/stm32_flash_f303_128k.ld @@ -22,7 +22,7 @@ _Min_Stack_Size = 0x400; /* required amount of stack */ /* Specify the memory areas */ MEMORY { - FLASH (rx) : ORIGIN = 0x04000000, LENGTH = 126K /* last 2kb used for config storage */ + FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 126K /* last 2kb used for config storage */ RAM (xrw) : ORIGIN = 0x20000000, LENGTH = 40K MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K } diff --git a/src/main/telemetry/frsky.c b/src/main/telemetry/frsky.c index be9e0b830e..f30f961eb3 100644 --- a/src/main/telemetry/frsky.c +++ b/src/main/telemetry/frsky.c @@ -161,7 +161,7 @@ static void sendBaro(void) sendDataHead(ID_ALTITUDE_BP); serialize16(BaroAlt / 100); sendDataHead(ID_ALTITUDE_AP); - serialize16(abs(BaroAlt % 100)); + serialize16(ABS(BaroAlt % 100)); } static void sendGpsAltitude(void) @@ -247,7 +247,7 @@ static void sendTime(void) static void GPStoDDDMM_MMMM(int32_t mwiigps, gpsCoordinateDDDMMmmmm_t *result) { int32_t absgps, deg, min; - absgps = abs(mwiigps); + absgps = ABS(mwiigps); deg = absgps / GPS_DEGREES_DIVIDER; absgps = (absgps - deg * GPS_DEGREES_DIVIDER) * 60; // absgps = Minutes left * 10^7 min = absgps / GPS_DEGREES_DIVIDER; // minutes left diff --git a/src/main/version.h b/src/main/version.h index 59a5afa312..a31fe051fc 100644 --- a/src/main/version.h +++ b/src/main/version.h @@ -16,7 +16,7 @@ */ #define FC_VERSION_MAJOR 1 // increment when a major release is made (big new feature, etc) -#define FC_VERSION_MINOR 5 // increment when a minor release is made (small new feature, change etc) +#define FC_VERSION_MINOR 6 // 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 MW_VERSION 231 diff --git a/src/test/Makefile b/src/test/Makefile index 8e376ea51b..6642d98b8f 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -47,6 +47,7 @@ TESTS = \ battery_unittest \ flight_imu_unittest \ altitude_hold_unittest \ + maths_unittest \ gps_conversion_unittest \ telemetry_hott_unittest \ rc_controls_unittest \ @@ -149,6 +150,21 @@ flight_imu_unittest : \ $(OBJECT_DIR)/flight/imu.o \ $(OBJECT_DIR)/flight/altitudehold.o \ $(OBJECT_DIR)/flight_imu_unittest.o \ + $(OBJECT_DIR)/common/maths.o \ + $(OBJECT_DIR)/gtest_main.a + + $(CXX) $(CXX_FLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@ + +$(OBJECT_DIR)/maths_unittest.o : \ + $(TEST_DIR)/maths_unittest.cc \ + $(GTEST_HEADERS) + + @mkdir -p $(dir $@) + $(CXX) $(CXX_FLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/maths_unittest.cc -o $@ + +maths_unittest : \ + $(OBJECT_DIR)/maths_unittest.o \ + $(OBJECT_DIR)/common/maths.o \ $(OBJECT_DIR)/gtest_main.a $(CXX) $(CXX_FLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@ @@ -302,4 +318,8 @@ ws2811_unittest : \ $(CXX) $(CXX_FLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@ +test: $(TESTS) + set -e && for test in $(TESTS) ; do \ + $(OBJECT_DIR)/$$test; \ + done diff --git a/src/test/unit/altitude_hold_unittest.cc b/src/test/unit/altitude_hold_unittest.cc index b9df220b11..b2e1e328cc 100644 --- a/src/test/unit/altitude_hold_unittest.cc +++ b/src/test/unit/altitude_hold_unittest.cc @@ -27,6 +27,7 @@ extern "C" { #include "flight/flight.h" #include "sensors/sensors.h" + #include "drivers/sensor.h" #include "drivers/accgyro.h" #include "sensors/acceleration.h" #include "sensors/barometer.h" diff --git a/src/test/unit/flight_imu_unittest.cc b/src/test/unit/flight_imu_unittest.cc index 67425aa63d..d82d12fb1d 100644 --- a/src/test/unit/flight_imu_unittest.cc +++ b/src/test/unit/flight_imu_unittest.cc @@ -24,6 +24,7 @@ extern "C" { #include "common/axis.h" + #include "common/maths.h" #include "flight/flight.h" #include "sensors/sensors.h" @@ -48,10 +49,23 @@ extern "C" { #define UPWARDS_THRUST false -TEST(FlightImuTest, Placeholder) +TEST(FlightImuTest, TestCalculateHeading) { - // TODO test things - EXPECT_EQ(true, true); + //TODO: Add test cases using the Z dimension. + t_fp_vector north = {.A={1.0f, 0.0f, 0.0f}}; + EXPECT_EQ(calculateHeading(&north), 0); + + t_fp_vector east = {.A={0.0f, 1.0f, 0.0f}}; + EXPECT_EQ(calculateHeading(&east), 90); + + t_fp_vector south = {.A={-1.0f, 0.0f, 0.0f}}; + EXPECT_EQ(calculateHeading(&south), 180); + + t_fp_vector west = {.A={0.0f, -1.0f, 0.0f}}; + EXPECT_EQ(calculateHeading(&west), 270); + + t_fp_vector north_east = {.A={1.0f, 1.0f, 0.0f}}; + EXPECT_EQ(calculateHeading(&north_east), 45); } // STUBS @@ -85,18 +99,8 @@ void updateAccelerationReadings(rollAndPitchTrims_t *rollAndPitchTrims) UNUSED(rollAndPitchTrims); } -int32_t applyDeadband(int32_t, int32_t) { return 0; } - uint32_t micros(void) { return 0; } bool isBaroCalibrationComplete(void) { return true; } void performBaroCalibrationCycle(void) {} int32_t baroCalculateAltitude(void) { return 0; } -int constrain(int amt, int low, int high) -{ - UNUSED(amt); - UNUSED(low); - UNUSED(high); - return 0; -} - } diff --git a/src/test/unit/ledstrip_unittest.cc b/src/test/unit/ledstrip_unittest.cc index 04c2a53869..a1e86d20d7 100644 --- a/src/test/unit/ledstrip_unittest.cc +++ b/src/test/unit/ledstrip_unittest.cc @@ -32,6 +32,8 @@ extern "C" { #include "rx/rx.h" + #include "io/rc_controls.h" + #include "drivers/light_ws2811strip.h" #include "io/ledstrip.h" } @@ -66,38 +68,38 @@ TEST(LedStripTest, parseLedStripConfig) // given static const ledConfig_t expectedLedStripConfig[WS2811_LED_STRIP_LENGTH] = { - { CALCULATE_LED_XY( 9, 9), LED_DIRECTION_SOUTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, - { CALCULATE_LED_XY(10, 10), LED_DIRECTION_SOUTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, - { CALCULATE_LED_XY(11, 11), LED_DIRECTION_SOUTH | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, - { CALCULATE_LED_XY(11, 11), LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, - { CALCULATE_LED_XY(10, 10), LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE }, - { CALCULATE_LED_XY( 9, 9), LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE }, + { CALCULATE_LED_XY( 9, 9), 0, LED_DIRECTION_SOUTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, + { CALCULATE_LED_XY(10, 10), 0, LED_DIRECTION_SOUTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, + { CALCULATE_LED_XY(11, 11), 0, LED_DIRECTION_SOUTH | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, + { CALCULATE_LED_XY(11, 11), 0, LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, + { CALCULATE_LED_XY(10, 10), 0, LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE }, + { CALCULATE_LED_XY( 9, 9), 0, LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE }, - { CALCULATE_LED_XY(10, 5), LED_DIRECTION_SOUTH | LED_FUNCTION_FLIGHT_MODE }, - { CALCULATE_LED_XY(11, 4), LED_DIRECTION_SOUTH | LED_FUNCTION_FLIGHT_MODE }, - { CALCULATE_LED_XY(12, 3), LED_DIRECTION_SOUTH | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, - { CALCULATE_LED_XY(12, 2), LED_DIRECTION_NORTH | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, - { CALCULATE_LED_XY(11, 1), LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE }, - { CALCULATE_LED_XY(10, 0), LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE }, + { CALCULATE_LED_XY(10, 5), 0, LED_DIRECTION_SOUTH | LED_FUNCTION_FLIGHT_MODE }, + { CALCULATE_LED_XY(11, 4), 0, LED_DIRECTION_SOUTH | LED_FUNCTION_FLIGHT_MODE }, + { CALCULATE_LED_XY(12, 3), 0, LED_DIRECTION_SOUTH | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, + { CALCULATE_LED_XY(12, 2), 0, LED_DIRECTION_NORTH | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, + { CALCULATE_LED_XY(11, 1), 0, LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE }, + { CALCULATE_LED_XY(10, 0), 0, LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE }, - { CALCULATE_LED_XY( 7, 0), LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, - { CALCULATE_LED_XY( 6, 0), LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, - { CALCULATE_LED_XY( 5, 0), LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, - { CALCULATE_LED_XY( 4, 0), LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, + { CALCULATE_LED_XY( 7, 0), 0, LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, + { CALCULATE_LED_XY( 6, 0), 0, LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, + { CALCULATE_LED_XY( 5, 0), 0, LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, + { CALCULATE_LED_XY( 4, 0), 0, LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, - { CALCULATE_LED_XY( 2, 0), LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE }, - { CALCULATE_LED_XY( 1, 1), LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE }, - { CALCULATE_LED_XY( 0, 2), LED_DIRECTION_NORTH | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, - { CALCULATE_LED_XY( 0, 3), LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, - { CALCULATE_LED_XY( 1, 4), LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE }, - { CALCULATE_LED_XY( 2, 5), LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE }, + { CALCULATE_LED_XY( 2, 0), 0, LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE }, + { CALCULATE_LED_XY( 1, 1), 0, LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE }, + { CALCULATE_LED_XY( 0, 2), 0, LED_DIRECTION_NORTH | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, + { CALCULATE_LED_XY( 0, 3), 0, LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, + { CALCULATE_LED_XY( 1, 4), 0, LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE }, + { CALCULATE_LED_XY( 2, 5), 0, LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE }, - { CALCULATE_LED_XY( 2, 9), LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE }, - { CALCULATE_LED_XY( 1, 10), LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE }, - { CALCULATE_LED_XY( 0, 11), LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, - { CALCULATE_LED_XY( 0, 11), LED_DIRECTION_SOUTH | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, - { CALCULATE_LED_XY( 1, 10), LED_DIRECTION_SOUTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, - { CALCULATE_LED_XY( 2, 9), LED_DIRECTION_SOUTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, + { CALCULATE_LED_XY( 2, 9), 0, LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE }, + { CALCULATE_LED_XY( 1, 10), 0, LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE }, + { CALCULATE_LED_XY( 0, 11), 0, LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, + { CALCULATE_LED_XY( 0, 11), 0, LED_DIRECTION_SOUTH | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, + { CALCULATE_LED_XY( 1, 10), 0, LED_DIRECTION_SOUTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, + { CALCULATE_LED_XY( 2, 9), 0, LED_DIRECTION_SOUTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, { 0, 0 }, { 0, 0 }, @@ -110,42 +112,42 @@ TEST(LedStripTest, parseLedStripConfig) // Spider quad // right rear cluster - "9,9:S:FW", - "10,10:S:FW", - "11,11:S:IA", - "11,11:E:IA", - "10,10:E:F", - "9,9:E:F", + "9,9:S:FW:0", + "10,10:S:FW:0", + "11,11:S:IA:0", + "11,11:E:IA:0", + "10,10:E:F:0", + "9,9:E:F:0", // right front cluster - "10,5:S:F", - "11,4:S:F", - "12,3:S:IA", - "12,2:N:IA", - "11,1:N:F", - "10,0:N:F", + "10,5:S:F:0", + "11,4:S:F:0", + "12,3:S:IA:0", + "12,2:N:IA:0", + "11,1:N:F:0", + "10,0:N:F:0", // center front cluster - "7,0:N:FW", - "6,0:N:FW", - "5,0:N:FW", - "4,0:N:FW", + "7,0:N:FW:0", + "6,0:N:FW:0", + "5,0:N:FW:0", + "4,0:N:FW:0", // left front cluster - "2,0:N:F", - "1,1:N:F", - "0,2:N:IA", - "0,3:W:IA", - "1,4:W:F", - "2,5:W:F", + "2,0:N:F:0", + "1,1:N:F:0", + "0,2:N:IA:0", + "0,3:W:IA:0", + "1,4:W:F:0", + "2,5:W:F:0", // left rear cluster - "2,9:W:F", - "1,10:W:F", - "0,11:W:IA", - "0,11:S:IA", - "1,10:S:FW", - "2,9:S:FW" + "2,9:W:F:0", + "1,10:W:F:0", + "0,11:W:IA:0", + "0,11:S:IA:0", + "1,10:S:FW:0", + "2,9:S:FW:0" }; // and memset(&systemLedConfigs, 0, sizeof(systemLedConfigs)); @@ -191,14 +193,14 @@ TEST(LedStripTest, smallestGridWithCenter) // and static const ledConfig_t testLedConfigs[] = { - { CALCULATE_LED_XY( 2, 2), LED_DIRECTION_SOUTH | LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, - { CALCULATE_LED_XY( 2, 1), LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, - { CALCULATE_LED_XY( 2, 0), LED_DIRECTION_NORTH | LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, - { CALCULATE_LED_XY( 1, 0), LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, - { CALCULATE_LED_XY( 0, 0), LED_DIRECTION_NORTH | LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, - { CALCULATE_LED_XY( 0, 1), LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, - { CALCULATE_LED_XY( 0, 2), LED_DIRECTION_SOUTH | LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, - { CALCULATE_LED_XY( 1, 2), LED_DIRECTION_SOUTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING } + { CALCULATE_LED_XY( 2, 2), 0, LED_DIRECTION_SOUTH | LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, + { CALCULATE_LED_XY( 2, 1), 0, LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, + { CALCULATE_LED_XY( 2, 0), 0, LED_DIRECTION_NORTH | LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, + { CALCULATE_LED_XY( 1, 0), 0, LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, + { CALCULATE_LED_XY( 0, 0), 0, LED_DIRECTION_NORTH | LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, + { CALCULATE_LED_XY( 0, 1), 0, LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, + { CALCULATE_LED_XY( 0, 2), 0, LED_DIRECTION_SOUTH | LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, + { CALCULATE_LED_XY( 1, 2), 0, LED_DIRECTION_SOUTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING } }; memcpy(&systemLedConfigs, &testLedConfigs, sizeof(testLedConfigs)); @@ -227,10 +229,10 @@ TEST(LedStripTest, smallestGrid) // and static const ledConfig_t testLedConfigs[] = { - { CALCULATE_LED_XY( 1, 1), LED_DIRECTION_SOUTH | LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_FLIGHT_MODE }, - { CALCULATE_LED_XY( 1, 0), LED_DIRECTION_NORTH | LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_FLIGHT_MODE }, - { CALCULATE_LED_XY( 0, 0), LED_DIRECTION_NORTH | LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_FLIGHT_MODE }, - { CALCULATE_LED_XY( 0, 1), LED_DIRECTION_SOUTH | LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_FLIGHT_MODE }, + { CALCULATE_LED_XY( 1, 1), 0, LED_DIRECTION_SOUTH | LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_FLIGHT_MODE }, + { CALCULATE_LED_XY( 1, 0), 0, LED_DIRECTION_NORTH | LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_FLIGHT_MODE }, + { CALCULATE_LED_XY( 0, 0), 0, LED_DIRECTION_NORTH | LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_FLIGHT_MODE }, + { CALCULATE_LED_XY( 0, 1), 0, LED_DIRECTION_SOUTH | LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_FLIGHT_MODE }, }; memcpy(&systemLedConfigs, &testLedConfigs, sizeof(testLedConfigs)); @@ -252,45 +254,45 @@ TEST(LedStripTest, smallestGrid) } /* - { CALCULATE_LED_XY( 1, 14), LED_DIRECTION_SOUTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_INDICATOR | LED_FUNCTION_FLIGHT_MODE }, + { CALCULATE_LED_XY( 1, 14), 0, LED_DIRECTION_SOUTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_INDICATOR | LED_FUNCTION_FLIGHT_MODE }, - { CALCULATE_LED_XY( 0, 13), LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, - { CALCULATE_LED_XY( 0, 12), LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, + { CALCULATE_LED_XY( 0, 13), 0, LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, + { CALCULATE_LED_XY( 0, 12), 0, LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, - { CALCULATE_LED_XY( 0, 11), LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE }, - { CALCULATE_LED_XY( 0, 10), LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE }, - { CALCULATE_LED_XY( 0, 9), LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE }, - { CALCULATE_LED_XY( 0, 8), LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, - { CALCULATE_LED_XY( 0, 7), LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, - { CALCULATE_LED_XY( 0, 6), LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, - { CALCULATE_LED_XY( 0, 5), LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE }, - { CALCULATE_LED_XY( 0, 4), LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE }, - { CALCULATE_LED_XY( 0, 3), LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE }, + { CALCULATE_LED_XY( 0, 11), 0, LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE }, + { CALCULATE_LED_XY( 0, 10), 0, LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE }, + { CALCULATE_LED_XY( 0, 9), 0, LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE }, + { CALCULATE_LED_XY( 0, 8), 0, LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, + { CALCULATE_LED_XY( 0, 7), 0, LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, + { CALCULATE_LED_XY( 0, 6), 0, LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, + { CALCULATE_LED_XY( 0, 5), 0, LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE }, + { CALCULATE_LED_XY( 0, 4), 0, LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE }, + { CALCULATE_LED_XY( 0, 3), 0, LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE }, - { CALCULATE_LED_XY( 0, 2), LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, - { CALCULATE_LED_XY( 0, 1), LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, + { CALCULATE_LED_XY( 0, 2), 0, LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, + { CALCULATE_LED_XY( 0, 1), 0, LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, - { CALCULATE_LED_XY( 1, 0), LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, - { CALCULATE_LED_XY( 2, 0), LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_ARM_STATE }, - { CALCULATE_LED_XY( 3, 0), LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, + { CALCULATE_LED_XY( 1, 0), 0, LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, + { CALCULATE_LED_XY( 2, 0), 0, LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_ARM_STATE }, + { CALCULATE_LED_XY( 3, 0), 0, LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, - { CALCULATE_LED_XY( 4, 1), LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, - { CALCULATE_LED_XY( 4, 2), LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, + { CALCULATE_LED_XY( 4, 1), 0, LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, + { CALCULATE_LED_XY( 4, 2), 0, LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, - { CALCULATE_LED_XY( 4, 3), LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE }, - { CALCULATE_LED_XY( 4, 4), LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE }, - { CALCULATE_LED_XY( 4, 5), LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE }, - { CALCULATE_LED_XY( 4, 6), LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, - { CALCULATE_LED_XY( 4, 7), LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, - { CALCULATE_LED_XY( 4, 8), LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, - { CALCULATE_LED_XY( 4, 9), LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE }, - { CALCULATE_LED_XY( 4, 10), LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE }, - { CALCULATE_LED_XY( 4, 11), LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE }, + { CALCULATE_LED_XY( 4, 3), 0, LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE }, + { CALCULATE_LED_XY( 4, 4), 0, LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE }, + { CALCULATE_LED_XY( 4, 5), 0, LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE }, + { CALCULATE_LED_XY( 4, 6), 0, LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, + { CALCULATE_LED_XY( 4, 7), 0, LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, + { CALCULATE_LED_XY( 4, 8), 0, LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, + { CALCULATE_LED_XY( 4, 9), 0, LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE }, + { CALCULATE_LED_XY( 4, 10), 0, LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE }, + { CALCULATE_LED_XY( 4, 11), 0, LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE }, - { CALCULATE_LED_XY( 4, 12), LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, - { CALCULATE_LED_XY( 4, 13), LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, + { CALCULATE_LED_XY( 4, 12), 0, LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, + { CALCULATE_LED_XY( 4, 13), 0, LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, - { CALCULATE_LED_XY( 3, 14), LED_DIRECTION_SOUTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, + { CALCULATE_LED_XY( 3, 14), 0, LED_DIRECTION_SOUTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, */ @@ -345,6 +347,7 @@ uint8_t armingFlags = 0; uint16_t flightModeFlags = 0; int16_t rcCommand[4]; int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; +uint32_t rcModeActivationMask; batteryState_e calculateBatteryState(void) { return BATTERY_OK; diff --git a/src/test/unit/maths_unittest.cc b/src/test/unit/maths_unittest.cc new file mode 100644 index 0000000000..429f2396d6 --- /dev/null +++ b/src/test/unit/maths_unittest.cc @@ -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 . + */ + +#include +#include + +#include + +#define BARO + +extern "C" { + #include "common/maths.h" +} + +#include "unittest_macros.h" +#include "gtest/gtest.h" + +TEST(MathsUnittest, TestConstrain) +{ + // Within bounds + EXPECT_EQ(constrain(0, 0, 0), 0); + EXPECT_EQ(constrain(1, 1, 1), 1); + EXPECT_EQ(constrain(1, 0, 2), 1); + + // Equal to bottom bound. + EXPECT_EQ(constrain(1, 1, 2), 1); + // Equal to top bound. + EXPECT_EQ(constrain(1, 0, 1), 1); + + // Equal to both bottom and top bound. + EXPECT_EQ(constrain(1, 1, 1), 1); + + // Above top bound. + EXPECT_EQ(constrain(2, 0, 1), 1); + // Below bottom bound. + EXPECT_EQ(constrain(0, 1, 2), 1); +} + +TEST(MathsUnittest, TestConstrainNegatives) +{ + // Within bounds. + EXPECT_EQ(constrain(-1, -1, -1), -1); + EXPECT_EQ(constrain(-1, -2, 0), -1); + + // Equal to bottom bound. + EXPECT_EQ(constrain(-1, -1, 0), -1); + // Equal to top bound. + EXPECT_EQ(constrain(-1, -2, -1), -1); + + // Equal to both bottom and top bound. + EXPECT_EQ(constrain(-1, -1, -1), -1); + + // Above top bound. + EXPECT_EQ(constrain(-1, -3, -2), -2); + // Below bottom bound. + EXPECT_EQ(constrain(-3, -2, -1), -2); +} + +TEST(MathsUnittest, TestConstrainf) +{ + // Within bounds. + EXPECT_EQ(constrainf(1.0f, 0.0f, 2.0f), 1.0f); + + // Equal to bottom bound. + EXPECT_EQ(constrainf(1.0f, 1.0f, 2.0f), 1.0f); + // Equal to top bound. + EXPECT_EQ(constrainf(1.0f, 0.0f, 1.0f), 1.0f); + + // Equal to both bottom and top bound. + EXPECT_EQ(constrainf(1.0f, 1.0f, 1.0f), 1.0f); + + // Above top bound. + EXPECT_EQ(constrainf(2.0f, 0.0f, 1.0f), 1.0f); + // Below bottom bound. + EXPECT_EQ(constrainf(0, 1.0f, 2.0f), 1.0f); + + // Above bouth bounds. + EXPECT_EQ(constrainf(2.0f, 0.0f, 1.0f), 1.0f); + // Below bouth bounds. + EXPECT_EQ(constrainf(0, 1.0f, 2.0f), 1.0f); +} + +TEST(MathsUnittest, TestDegreesToRadians) +{ + EXPECT_EQ(degreesToRadians(0), 0.0f); + EXPECT_EQ(degreesToRadians(90), 0.5f * M_PIf); + EXPECT_EQ(degreesToRadians(180), M_PIf); + EXPECT_EQ(degreesToRadians(-180), - M_PIf); +} + +TEST(MathsUnittest, TestApplyDeadband) +{ + EXPECT_EQ(applyDeadband(0, 0), 0); + EXPECT_EQ(applyDeadband(1, 0), 1); + EXPECT_EQ(applyDeadband(-1, 0), -1); + + EXPECT_EQ(applyDeadband(0, 10), 0); + EXPECT_EQ(applyDeadband(1, 10), 0); + EXPECT_EQ(applyDeadband(10, 10), 0); + + EXPECT_EQ(applyDeadband(11, 10), 1); + EXPECT_EQ(applyDeadband(-11, 10), -1); +} + +void expectVectorsAreEqual(struct fp_vector *a, struct fp_vector *b) +{ + EXPECT_EQ(a->X, b->X); + EXPECT_EQ(a->Y, b->Y); + EXPECT_EQ(a->Z, b->Z); +} + +TEST(MathsUnittest, TestRotateVectorWithNoAngle) +{ + fp_vector vector = {1.0f, 0.0f, 0.0f}; + fp_angles_t euler_angles = {.raw={0.0f, 0.0f, 0.0f}}; + + rotateV(&vector, &euler_angles); + fp_vector 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. + fp_vector vector = {1.0f, 0.0f, 0.0f}; + fp_angles_t euler_angles = {.raw={90.0f, 0.0f, 0.0f}}; + + rotateV(&vector, &euler_angles); + fp_vector expected_result = {1.0f, 0.0f, 0.0f}; + + expectVectorsAreEqual(&vector, &expected_result); +}