diff --git a/.travis.yml b/.travis.yml index a456e51de4..a4457c2c02 100644 --- a/.travis.yml +++ b/.travis.yml @@ -1,9 +1,10 @@ env: - TARGET=CC3D + - TARGET=CC3D OPBL=yes - TARGET=CHEBUZZF3 - TARGET=CJMCU - TARGET=EUSTM32F103RC - - TARGET=MASSIVEF3 + - TARGET=SPRACINGF3 - TARGET=NAZE - TARGET=NAZE32PRO - TARGET=OLIMEXINO 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 0a4285e075..85c7a06f01 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 028f54c903..e0c82c4795 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -74,7 +74,6 @@ #include "config/config_profile.h" #include "config/config_master.h" -#include "blackbox_fielddefs.h" #include "blackbox.h" #define BLACKBOX_BAUDRATE 115200 @@ -83,6 +82,9 @@ #define ARRAY_LENGTH(x) (sizeof((x))/sizeof((x)[0])) +#define STATIC_ASSERT(condition, name ) \ + typedef char assert_failed_ ## name [(condition) ? 1 : -1 ] + // Some macros to make writing FLIGHT_LOG_FIELD_* constants shorter: #define STR_HELPER(x) #x #define STR(x) STR_HELPER(x) @@ -148,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; /** @@ -179,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}, @@ -211,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 @@ -235,7 +240,8 @@ typedef enum BlackboxState { BLACKBOX_STATE_SEND_GPS_G_HEADERS, BLACKBOX_STATE_SEND_SYSINFO, BLACKBOX_STATE_PRERUN, - BLACKBOX_STATE_RUNNING + BLACKBOX_STATE_RUNNING, + BLACKBOX_STATE_SHUTTING_DOWN } BlackboxState; typedef struct gpsState_t { @@ -267,8 +273,11 @@ static struct { } u; } xmitState; +// Cache for FLIGHT_LOG_FIELD_CONDITION_* test results: static uint32_t blackboxConditionCache; +STATIC_ASSERT((sizeof(blackboxConditionCache) * 8) >= FLIGHT_LOG_FIELD_CONDITION_NEVER, too_many_flight_log_conditions); + static uint32_t blackboxIteration; static uint32_t blackboxPFrameIndex, blackboxIFrameIndex; @@ -625,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: @@ -671,6 +686,9 @@ static void blackboxSetState(BlackboxState newState) blackboxPFrameIndex = 0; blackboxIFrameIndex = 0; break; + case BLACKBOX_STATE_SHUTTING_DOWN: + xmitState.u.startTime = millis(); + break; default: ; } @@ -712,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++) @@ -763,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)); @@ -797,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,6 +921,14 @@ static void releaseBlackboxPort(void) serialSetBaudRate(blackboxPort, previousBaudRate); endSerialPortFunction(blackboxPort, FUNCTION_BLACKBOX); + + /* + * Normally this would be handled by mw.c, but since we take an unknown amount + * of time to shut down asynchronously, we're the only ones that know when to call it. + */ + if (isSerialPortFunctionShared(FUNCTION_BLACKBOX, FUNCTION_MSP)) { + mspAllocateSerialPorts(&masterConfig.serialConfig); + } } void startBlackbox(void) @@ -914,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 @@ -931,10 +966,18 @@ void startBlackbox(void) void finishBlackbox(void) { - if (blackboxState != BLACKBOX_STATE_DISABLED && blackboxState != BLACKBOX_STATE_STOPPED) { - blackboxSetState(BLACKBOX_STATE_STOPPED); - + if (blackboxState == BLACKBOX_STATE_RUNNING) { + blackboxLogEvent(FLIGHT_LOG_EVENT_LOG_END, NULL); + + blackboxSetState(BLACKBOX_STATE_SHUTTING_DOWN); + } else if (blackboxState != BLACKBOX_STATE_DISABLED && blackboxState != BLACKBOX_STATE_STOPPED + && blackboxState != BLACKBOX_STATE_SHUTTING_DOWN) { + /* + * We're shutting down in the middle of transmitting headers, so we can't log a "log completed" event. + * Just give the port back and stop immediately. + */ releaseBlackboxPort(); + blackboxSetState(BLACKBOX_STATE_STOPPED); } } @@ -955,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]); @@ -997,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++) @@ -1141,7 +1196,6 @@ static bool blackboxWriteSysinfo() blackboxPrintf("H P interval:%d/%d\n", masterConfig.blackbox_rate_num, masterConfig.blackbox_rate_denom); xmitState.u.serialBudget -= strlen("H P interval:%d/%d\n"); - break; case 5: blackboxPrintf("H rcRate:%d\n", masterConfig.controlRateProfiles[masterConfig.current_profile_index].rcRate8); @@ -1163,10 +1217,10 @@ static bool blackboxWriteSysinfo() blackboxPrintf("H gyro.scale:0x%x\n", floatConvert.u); xmitState.u.serialBudget -= strlen("H gyro.scale:0x%x\n") + 6; - break; + break; case 9: blackboxPrintf("H acc_1G:%u\n", acc_1G); - + xmitState.u.serialBudget -= strlen("H acc_1G:%u\n"); break; case 10: @@ -1185,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; } @@ -1193,10 +1251,49 @@ static bool blackboxWriteSysinfo() return false; } +/** + * Write the given event to the log immediately + */ +void blackboxLogEvent(FlightLogEvent event, flightLogEventData_t *data) +{ + if (blackboxState != BLACKBOX_STATE_RUNNING) + return; + + //Shared header for event frames + blackboxWrite('E'); + blackboxWrite(event); + + //Now serialize the data for this specific frame type + switch (event) { + case FLIGHT_LOG_EVENT_SYNC_BEEP: + writeUnsignedVB(data->syncBeep.time); + break; + case FLIGHT_LOG_EVENT_AUTOTUNE_CYCLE_START: + blackboxWrite(data->autotuneCycleStart.phase); + blackboxWrite(data->autotuneCycleStart.cycle); + blackboxWrite(data->autotuneCycleStart.p); + blackboxWrite(data->autotuneCycleStart.i); + blackboxWrite(data->autotuneCycleStart.d); + break; + case FLIGHT_LOG_EVENT_AUTOTUNE_CYCLE_RESULT: + blackboxWrite(data->autotuneCycleResult.overshot); + blackboxWrite(data->autotuneCycleStart.p); + blackboxWrite(data->autotuneCycleStart.i); + blackboxWrite(data->autotuneCycleStart.d); + break; + case FLIGHT_LOG_EVENT_LOG_END: + blackboxPrint("End of log"); + blackboxWrite(0); + break; + } +} + // Beep the buzzer and write the current time to the log as a synchronization point static void blackboxPlaySyncBeep() { - uint32_t now = micros(); + flightLogEvent_syncBeep_t eventData; + + eventData.time = micros(); /* * The regular beep routines aren't going to work for us, because they queue up the beep to be executed later. @@ -1207,10 +1304,7 @@ static void blackboxPlaySyncBeep() // Have the regular beeper code turn off the beep for us eventually, since that's not timing-sensitive queueConfirmationBeep(1); - blackboxWrite('E'); - blackboxWrite(FLIGHT_LOG_EVENT_SYNC_BEEP); - - writeUnsignedVB(now); + blackboxLogEvent(FLIGHT_LOG_EVENT_SYNC_BEEP, (flightLogEventData_t *) &eventData); } void handleBlackbox(void) @@ -1249,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; @@ -1269,9 +1363,9 @@ void handleBlackbox(void) blackboxSetState(BLACKBOX_STATE_PRERUN); break; case BLACKBOX_STATE_PRERUN: - blackboxPlaySyncBeep(); - blackboxSetState(BLACKBOX_STATE_RUNNING); + + blackboxPlaySyncBeep(); break; case BLACKBOX_STATE_RUNNING: // On entry to this state, blackboxIteration, blackboxPFrameIndex and blackboxIFrameIndex are reset to 0 @@ -1317,6 +1411,20 @@ void handleBlackbox(void) blackboxIFrameIndex++; } break; + case BLACKBOX_STATE_SHUTTING_DOWN: + //On entry of this state, startTime is set + + /* + * Wait for the log we've transmitted to make its way to the logger before we release the serial port, + * since releasing the port clears the Tx buffer. + * + * Don't wait longer than it could possibly take if something funky happens. + */ + if (millis() > xmitState.u.startTime + 200 || isSerialTransmitBufferEmpty(blackboxPort)) { + releaseBlackboxPort(); + blackboxSetState(BLACKBOX_STATE_STOPPED); + } + break; default: break; } diff --git a/src/main/blackbox/blackbox.h b/src/main/blackbox/blackbox.h index 5d30671bbf..59085249e9 100644 --- a/src/main/blackbox/blackbox.h +++ b/src/main/blackbox/blackbox.h @@ -17,9 +17,12 @@ #pragma once -#include "common/axis.h" #include +#include "common/axis.h" +#include "flight/mixer.h" +#include "blackbox/blackbox_fielddefs.h" + typedef struct blackboxValues_t { uint32_t time; @@ -32,6 +35,7 @@ typedef struct blackboxValues_t { int16_t servo[MAX_SUPPORTED_SERVOS]; uint16_t vbatLatest; + uint16_t amperageLatest; #ifdef BARO int32_t BaroAlt; @@ -41,6 +45,8 @@ typedef struct blackboxValues_t { #endif } blackboxValues_t; +void blackboxLogEvent(FlightLogEvent event, flightLogEventData_t *data); + void initBlackbox(void); void handleBlackbox(void); void startBlackbox(void); diff --git a/src/main/blackbox/blackbox_fielddefs.h b/src/main/blackbox/blackbox_fielddefs.h index 1763561092..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; @@ -93,5 +99,40 @@ typedef enum FlightLogFieldSign { typedef enum FlightLogEvent { FLIGHT_LOG_EVENT_SYNC_BEEP = 0, + FLIGHT_LOG_EVENT_AUTOTUNE_CYCLE_START = 10, + FLIGHT_LOG_EVENT_AUTOTUNE_CYCLE_RESULT = 11, FLIGHT_LOG_EVENT_LOG_END = 255 } FlightLogEvent; + +typedef struct flightLogEvent_syncBeep_t { + uint32_t time; +} flightLogEvent_syncBeep_t; + +typedef struct flightLogEvent_autotuneCycleStart_t { + uint8_t phase; + uint8_t cycle; + uint8_t p; + uint8_t i; + uint8_t d; +} flightLogEvent_autotuneCycleStart_t; + +typedef struct flightLogEvent_autotuneCycleResult_t { + uint8_t overshot; + uint8_t p; + uint8_t i; + uint8_t d; +} flightLogEvent_autotuneCycleResult_t; + +typedef union flightLogEventData_t +{ + flightLogEvent_syncBeep_t syncBeep; + flightLogEvent_autotuneCycleStart_t autotuneCycleStart; + flightLogEvent_autotuneCycleResult_t autotuneCycleResult; + +} flightLogEventData_t; + +typedef struct flightLogEvent_t +{ + FlightLogEvent event; + flightLogEventData_t data; +} flightLogEvent_t; diff --git a/src/main/common/maths.c b/src/main/common/maths.c index 716d83e491..5ed16f916d 100644 --- a/src/main/common/maths.c +++ b/src/main/common/maths.c @@ -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..d65b87df22 100644 --- a/src/main/common/maths.h +++ b/src/main/common/maths.h @@ -21,16 +21,10 @@ #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 DEG2RAD(degrees) (degrees * RAD) +#define RAD (M_PIf / 180.0f) #define min(a, b) ((a) < (b) ? (a) : (b)) #define max(a, b) ((a) > (b) ? (a) : (b)) @@ -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 e3e788d35d..7df5bb1370 100644 --- a/src/main/common/typeconversion.c +++ b/src/main/common/typeconversion.c @@ -92,7 +92,7 @@ int a2d(char ch) char a2i(char ch, const char **src, int base, int *nump) { - char *p = *src; + const char *p = *src; int num = 0; int digit; while ((digit = a2d(ch)) >= 0) { diff --git a/src/main/config/config.c b/src/main/config/config.c index f4b0dca420..d8659bd83e 100644 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -108,7 +108,7 @@ profile_t *currentProfile; static uint8_t currentControlRateProfileIndex = 0; controlRateConfig_t *currentControlRateProfile; -static const uint8_t EEPROM_CONF_VERSION = 88; +static const uint8_t EEPROM_CONF_VERSION = 89; static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims) { @@ -159,6 +159,7 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->D_f[YAW] = 0.05f; pidProfile->A_level = 5.0f; pidProfile->H_level = 3.0f; + pidProfile->H_sensitivity = 75; } #ifdef GPS @@ -612,7 +613,7 @@ void activateConfig(void) imuRuntimeConfig.acc_unarmedcal = currentProfile->acc_unarmedcal;; imuRuntimeConfig.small_angle = masterConfig.small_angle; - configureImu( + configureIMU( &imuRuntimeConfig, ¤tProfile->pidProfile, ¤tProfile->accDeadband 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..63d0566c90 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; 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/autotune.c b/src/main/flight/autotune.c index 5ba9664f66..7ecf5a80bf 100644 --- a/src/main/flight/autotune.c +++ b/src/main/flight/autotune.c @@ -31,6 +31,9 @@ #include "flight/flight.h" +#include "config/config.h" +#include "blackbox/blackbox.h" + extern int16_t debug[4]; /* @@ -97,6 +100,12 @@ typedef enum { PHASE_SAVE_OR_RESTORE_PIDS, } autotunePhase_e; +typedef enum { + CYCLE_TUNE_I = 0, + CYCLE_TUNE_PD, + CYCLE_TUNE_PD2 +} autotuneCycle_e; + static const pidIndex_e angleIndexToPidIndexMap[] = { PIDROLL, PIDPITCH @@ -112,7 +121,7 @@ static pidProfile_t pidBackup; static uint8_t pidController; static uint8_t pidIndex; static bool rising; -static uint8_t cycleCount; // TODO can we replace this with an enum to improve readability. +static autotuneCycle_e cycle; static uint32_t timeoutAt; static angle_index_t autoTuneAngleIndex; static autotunePhase_e phase = PHASE_IDLE; @@ -140,10 +149,33 @@ bool isAutotuneIdle(void) return phase == PHASE_IDLE; } +#ifdef BLACKBOX + +static void autotuneLogCycleStart() +{ + if (feature(FEATURE_BLACKBOX)) { + flightLogEvent_autotuneCycleStart_t eventData; + + eventData.phase = phase; + eventData.cycle = cycle; + eventData.p = pid.p * MULTIWII_P_MULTIPLIER; + eventData.i = pid.i * MULTIWII_I_MULTIPLIER; + eventData.d = pid.d; + + blackboxLogEvent(FLIGHT_LOG_EVENT_AUTOTUNE_CYCLE_START, (flightLogEventData_t*)&eventData); + } +} + +#endif + static void startNewCycle(void) { rising = !rising; secondPeakAngle = firstPeakAngle = 0; + +#ifdef BLACKBOX + autotuneLogCycleStart(); +#endif } static void updatePidIndex(void) @@ -155,8 +187,7 @@ static void updateTargetAngle(void) { if (rising) { targetAngle = AUTOTUNE_TARGET_ANGLE; - } - else { + } else { targetAngle = -AUTOTUNE_TARGET_ANGLE; } @@ -210,30 +241,48 @@ float autotune(angle_index_t angleIndex, const rollAndPitchInclination_t *inclin debug[3] = DEGREES_TO_DECIDEGREES(secondPeakAngle); } else if (secondPeakAngle > 0) { - if (cycleCount == 0) { - // when checking the I value, we would like to overshoot the target position by half of the max oscillation. - if (currentAngle - targetAngle < AUTOTUNE_MAX_OSCILLATION_ANGLE / 2) { - pid.i *= AUTOTUNE_INCREASE_MULTIPLIER; - } else { - pid.i *= AUTOTUNE_DECREASE_MULTIPLIER; - if (pid.i < AUTOTUNE_MINIMUM_I_VALUE) { - pid.i = AUTOTUNE_MINIMUM_I_VALUE; + switch (cycle) { + case CYCLE_TUNE_I: + // when checking the I value, we would like to overshoot the target position by half of the max oscillation. + if (currentAngle - targetAngle < AUTOTUNE_MAX_OSCILLATION_ANGLE / 2) { + pid.i *= AUTOTUNE_INCREASE_MULTIPLIER; + } else { + pid.i *= AUTOTUNE_DECREASE_MULTIPLIER; + if (pid.i < AUTOTUNE_MINIMUM_I_VALUE) { + pid.i = AUTOTUNE_MINIMUM_I_VALUE; + } } - } - // go back to checking P and D - cycleCount = 1; - pidProfile->I8[pidIndex] = 0; - startNewCycle(); - } else { - // we are checking P and D values - // set up to look for the 2nd peak - firstPeakAngle = currentAngle; - timeoutAt = millis() + AUTOTUNE_SETTLING_DELAY_MS; +#ifdef BLACKBOX + if (feature(FEATURE_BLACKBOX)) { + flightLogEvent_autotuneCycleResult_t eventData; + + eventData.overshot = currentAngle - targetAngle < AUTOTUNE_MAX_OSCILLATION_ANGLE / 2 ? 0 : 1; + eventData.p = pidProfile->P8[pidIndex]; + eventData.i = pidProfile->I8[pidIndex]; + eventData.d = pidProfile->D8[pidIndex]; + + blackboxLogEvent(FLIGHT_LOG_EVENT_AUTOTUNE_CYCLE_RESULT, (flightLogEventData_t*)&eventData); + } +#endif + + // go back to checking P and D + cycle = CYCLE_TUNE_PD; + pidProfile->I8[pidIndex] = 0; + startNewCycle(); + break; + + case CYCLE_TUNE_PD: + case CYCLE_TUNE_PD2: + // we are checking P and D values + // set up to look for the 2nd peak + firstPeakAngle = currentAngle; + timeoutAt = millis() + AUTOTUNE_SETTLING_DELAY_MS; + break; } } } else { - // we saw the first peak. looking for the second + // We saw the first peak while tuning PD, looking for the second if (currentAngle < firstPeakAngle) { firstPeakAngle = currentAngle; @@ -266,8 +315,8 @@ float autotune(angle_index_t angleIndex, const rollAndPitchInclination_t *inclin pid.d *= AUTOTUNE_INCREASE_MULTIPLIER; } #else - pid.p *= AUTOTUNE_DECREASE_MULTIPLIER; - pid.d *= AUTOTUNE_INCREASE_MULTIPLIER; + pid.p *= AUTOTUNE_DECREASE_MULTIPLIER; + pid.d *= AUTOTUNE_INCREASE_MULTIPLIER; #endif } else { // undershot @@ -286,15 +335,30 @@ float autotune(angle_index_t angleIndex, const rollAndPitchInclination_t *inclin pidProfile->P8[pidIndex] = pid.p * MULTIWII_P_MULTIPLIER; pidProfile->D8[pidIndex] = pid.d; - // switch to the other direction and start a new cycle - startNewCycle(); +#ifdef BLACKBOX + if (feature(FEATURE_BLACKBOX)) { + flightLogEvent_autotuneCycleResult_t eventData; - if (++cycleCount == 3) { - // switch to testing I value - cycleCount = 0; + eventData.overshot = secondPeakAngle > targetAngleAtPeak ? 1 : 0; + eventData.p = pidProfile->P8[pidIndex]; + eventData.i = pidProfile->I8[pidIndex]; + eventData.d = pidProfile->D8[pidIndex]; - pidProfile->I8[pidIndex] = pid.i * MULTIWII_I_MULTIPLIER; + blackboxLogEvent(FLIGHT_LOG_EVENT_AUTOTUNE_CYCLE_RESULT, (flightLogEventData_t*)&eventData); } +#endif + + if (cycle == CYCLE_TUNE_PD2) { + // switch to testing I value + cycle = CYCLE_TUNE_I; + + pidProfile->I8[pidIndex] = pid.i * MULTIWII_I_MULTIPLIER; + } else { + cycle = CYCLE_TUNE_PD2; + } + + // switch to the other direction for the new cycle + startNewCycle(); } } @@ -344,7 +408,7 @@ void autotuneBeginNextPhase(pidProfile_t *pidProfileToTune, uint8_t pidControlle } rising = true; - cycleCount = 1; + cycle = CYCLE_TUNE_PD; secondPeakAngle = firstPeakAngle = 0; pidProfile = pidProfileToTune; @@ -360,6 +424,10 @@ void autotuneBeginNextPhase(pidProfile_t *pidProfileToTune, uint8_t pidControlle pidProfile->D8[pidIndex] = pid.d; pidProfile->I8[pidIndex] = 0; + +#ifdef BLACKBOX + autotuneLogCycleStart(); +#endif } void autotuneEndPhase(void) diff --git a/src/main/flight/flight.c b/src/main/flight/flight.c index c5942b05ca..dbc049b3d3 100644 --- a/src/main/flight/flight.c +++ b/src/main/flight/flight.c @@ -57,10 +57,10 @@ static float errorGyroIf[3] = { 0.0f, 0.0f, 0.0f }; static int32_t errorAngleI[2] = { 0, 0 }; static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, - uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim); + uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig); typedef void (*pidControllerFuncPtr)(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, - uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim); // pid controller function prototype + uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig); // pid controller function prototype pidControllerFuncPtr pid_controller = pidMultiWii; // which pid controller are we using, defaultMultiWii @@ -97,18 +97,42 @@ bool shouldAutotune(void) #endif static void pidBaseflight(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, - uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim) + uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) { float RateError, errorAngle, AngleRate, gyroRate; float ITerm,PTerm,DTerm; + int32_t stickPosAil, stickPosEle, mostDeflectedPos; static float lastGyroRate[3]; static float delta1[3], delta2[3]; float delta, deltaSum; float dT; int axis; + float horizonLevelStrength = 1; dT = (float)cycleTime * 0.000001f; + if (FLIGHT_MODE(HORIZON_MODE)) { + + // Figure out the raw stick positions + stickPosAil = getRcStickDeflection(FD_ROLL, rxConfig->midrc); + stickPosEle = getRcStickDeflection(FD_PITCH, rxConfig->midrc); + + if(abs(stickPosAil) > abs(stickPosEle)){ + mostDeflectedPos = abs(stickPosAil); + } + else { + mostDeflectedPos = abs(stickPosEle); + } + + // Progressively turn off the horizon self level strength as the stick is banged over + horizonLevelStrength = (float)(500 - mostDeflectedPos) / 500; // 1 at centre stick, 0 = max stick deflection + if(pidProfile->H_sensitivity == 0){ + horizonLevelStrength = 0; + } else { + horizonLevelStrength = constrainf(((horizonLevelStrength - 1) * (100 / pidProfile->H_sensitivity)) + 1, 0, 1); + } + } + // ----------PID controller---------- for (axis = 0; axis < 3; axis++) { // -----Get the desired angle rate depending on flight mode @@ -139,7 +163,7 @@ static void pidBaseflight(pidProfile_t *pidProfile, controlRateConfig_t *control AngleRate = (float)((controlRateConfig->rollPitchRate + 20) * rcCommand[axis]) / 50.0f; // 200dps to 1200dps max yaw rate if (FLIGHT_MODE(HORIZON_MODE)) { // mix up angle error to desired AngleRate to add a little auto-level feel - AngleRate += errorAngle * pidProfile->H_level; + AngleRate += errorAngle * pidProfile->H_level * horizonLevelStrength; } } } @@ -181,8 +205,10 @@ static void pidBaseflight(pidProfile_t *pidProfile, controlRateConfig_t *control } static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, - uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim) + uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) { + UNUSED(rxConfig); + int axis, prop; int32_t error, errorAngle; int32_t PTerm, ITerm, PTermACC = 0, ITermACC = 0, PTermGYRO = 0, ITermGYRO = 0, DTerm; @@ -264,8 +290,10 @@ static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRa #define GYRO_I_MAX 256 static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, - rollAndPitchTrims_t *angleTrim) + rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) { + UNUSED(rxConfig); + int32_t errorAngle; int axis; int32_t delta, deltaSum; diff --git a/src/main/flight/flight.h b/src/main/flight/flight.h index 158ac23a79..95877badf2 100644 --- a/src/main/flight/flight.h +++ b/src/main/flight/flight.h @@ -42,6 +42,7 @@ typedef struct pidProfile_s { float D_f[3]; float A_level; float H_level; + uint8_t H_sensitivity; } pidProfile_t; typedef enum { @@ -60,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..f0a4535a00 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -79,28 +79,28 @@ 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) { imuRuntimeConfig = initialImuRuntimeConfig; pidProfile = initialPidProfile; accDeadband = initialAccDeadband; } -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) { - throttleAngleScale = (1800.0f / M_PI) * (900.0f / throttle_correction_angle); + throttleAngleScale = (1800.0f / M_PIf) * (900.0f / throttle_correction_angle); } void calculateAccZLowPassFilterRCTimeConstant(float accz_lpf_cutoff) { - fc_acc = 0.5f / (M_PI * accz_lpf_cutoff); // calculate RC time constant used in the accZ lpf + fc_acc = 0.5f / (M_PIf * accz_lpf_cutoff); // calculate RC time constant used in the accZ lpf } void computeIMU(rollAndPitchTrims_t *accelerometerTrims, uint8_t mixerMode) @@ -145,56 +145,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,7 +198,13 @@ void acc_calc(uint32_t deltaT) accSumCount++; } -// baseflight calculation by Luggi09 originates from arducopter +/* +* Baseflight calculation by Luggi09 originates from arducopter +* ============================================================ +* +* Calculate the heading of the craft (in degrees clockwise from North) +* when given a 3-vector representing the direction of North. +*/ static int16_t calculateHeading(t_fp_vector *vec) { int16_t head; @@ -259,8 +215,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 +278,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 +298,21 @@ static void getEstimatedAttitude(void) acc_calc(deltaT); // rotate acc vector into earth frame } -// correction of throttle in lateral wind, +// Correction of throttle in lateral wind. 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..552093508d 100644 --- a/src/main/flight/imu.h +++ b/src/main/flight/imu.h @@ -30,7 +30,7 @@ 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); void calculateEstimatedAltitude(uint32_t currentTime); void computeIMU(rollAndPitchTrims_t *accelerometerTrims, uint8_t mixerMode); diff --git a/src/main/flight/navigation.c b/src/main/flight/navigation.c index 9eb5627e79..dd09430562 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); diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index 75eadc25da..a2402f1960 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 @@ -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 66db651275..9bd4560085 100644 --- a/src/main/io/rc_controls.c +++ b/src/main/io/rc_controls.c @@ -517,6 +517,10 @@ void updateAdjustmentStates(adjustmentRange_t *adjustmentRanges) } } +int32_t getRcStickDeflection(int32_t axis, uint16_t midrc) { + return min(abs(rcData[axis] - midrc), 500); +} + void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, escAndServoConfig_t *escAndServoConfigToUse, pidProfile_t *pidProfileToUse) { uint8_t index; diff --git a/src/main/io/rc_controls.h b/src/main/io/rc_controls.h index 745acd4682..8441ea90f1 100644 --- a/src/main/io/rc_controls.h +++ b/src/main/io/rc_controls.h @@ -214,3 +214,5 @@ void updateAdjustmentStates(adjustmentRange_t *adjustmentRanges); void processRcAdjustments(controlRateConfig_t *controlRateConfig, rxConfig_t *rxConfig); bool isUsingSticksForArming(void); + +int32_t getRcStickDeflection(int32_t axis, uint16_t midrc); diff --git a/src/main/io/serial.c b/src/main/io/serial.c index 1c975e70ea..dbd2843ad2 100644 --- a/src/main/io/serial.c +++ b/src/main/io/serial.c @@ -73,7 +73,9 @@ 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}, +#endif {SERIAL_PORT_USART1, NULL, SCENARIO_UNUSED, FUNCTION_NONE}, {SERIAL_PORT_USART2, NULL, SCENARIO_UNUSED, FUNCTION_NONE}, #if (SERIAL_PORT_COUNT > 3) @@ -85,7 +87,9 @@ static serialPortFunction_t serialPortFunctions[SERIAL_PORT_COUNT] = { }; const serialPortConstraint_t serialPortConstraints[SERIAL_PORT_COUNT] = { +#ifdef USE_VCP {SERIAL_PORT_USB_VCP, 9600, 115200, SPF_NONE }, +#endif {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) diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 428496c159..adf96d7f57 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -393,6 +393,7 @@ const clivalue_t valueTable[] = { { "level_horizon", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.H_level, 0, 10 }, { "level_angle", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.A_level, 0, 10 }, + { "sensitivity_horizon", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.H_sensitivity, 0, 250 }, { "p_alt", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.P8[PIDALT], 0, 200 }, { "i_alt", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I8[PIDALT], 0, 200 }, diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 8943c4453d..c70e19fe87 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 | @@ -846,7 +853,7 @@ static bool processOutCommand(uint8_t cmdMSP) if (i == PIDLEVEL) { serialize8(constrain(lrintf(currentProfile->pidProfile.A_level * 10.0f), 0, 250)); serialize8(constrain(lrintf(currentProfile->pidProfile.H_level * 10.0f), 0, 250)); - serialize8(0); + serialize8(constrain(lrintf(currentProfile->pidProfile.H_sensitivity), 0, 250)); } else { serialize8(currentProfile->pidProfile.P8[i]); serialize8(currentProfile->pidProfile.I8[i]); @@ -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 @@ -1171,7 +1179,7 @@ static bool processInCommand(void) if (i == PIDLEVEL) { currentProfile->pidProfile.A_level = (float)read8() / 10.0f; currentProfile->pidProfile.H_level = (float)read8() / 10.0f; - read8(); + currentProfile->pidProfile.H_sensitivity = read8(); } else { currentProfile->pidProfile.P8[i] = read8(); currentProfile->pidProfile.I8[i] = read8(); @@ -1442,28 +1450,30 @@ static bool processInCommand(void) case MSP_SET_LED_STRIP_CONFIG: { - uint8_t ledCount = currentPort->dataSize / 6; - if (ledCount != MAX_LED_STRIP_LENGTH) { + i = read8(); + if (i >= MAX_LED_STRIP_LENGTH || currentPort->dataSize != (1 + 7)) { headSerialError(0); break; } - for (i = 0; i < MAX_LED_STRIP_LENGTH; i++) { - ledConfig_t *ledConfig = &masterConfig.ledConfigs[i]; - uint16_t mask; - // currently we're storing directions and functions in a uint16 (flags) - // the msp uses 2 x uint16_t to cater for future expansion - mask = read16(); - ledConfig->flags = (mask << LED_DIRECTION_BIT_OFFSET) & LED_DIRECTION_MASK; + ledConfig_t *ledConfig = &masterConfig.ledConfigs[i]; + uint16_t mask; + // currently we're storing directions and functions in a uint16 (flags) + // the msp uses 2 x uint16_t to cater for future expansion + mask = read16(); + ledConfig->flags = (mask << LED_DIRECTION_BIT_OFFSET) & LED_DIRECTION_MASK; - mask = read16(); - ledConfig->flags |= (mask << LED_FUNCTION_BIT_OFFSET) & LED_FUNCTION_MASK; + mask = read16(); + ledConfig->flags |= (mask << LED_FUNCTION_BIT_OFFSET) & LED_FUNCTION_MASK; - mask = read8(); - ledConfig->xy = CALCULATE_LED_X(mask); + mask = read8(); + ledConfig->xy = CALCULATE_LED_X(mask); - mask = read8(); - ledConfig->xy |= CALCULATE_LED_Y(mask); - } + 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 4a9656b8df..08c1e64694 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 fff9470b8d..9b2a45e0a6 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -97,7 +97,8 @@ static uint32_t disarmAt; // Time of automatic disarm when "Don't spin the m extern uint8_t dynP8[3], dynI8[3], dynD8[3]; extern failsafe_t *failsafe; -typedef void (*pidControllerFuncPtr)(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, rollAndPitchTrims_t *accelerometerTrims); +typedef void (*pidControllerFuncPtr)(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, + uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig); // pid controller function prototype extern pidControllerFuncPtr pid_controller; @@ -311,9 +312,6 @@ void mwDisarm(void) #ifdef BLACKBOX if (feature(FEATURE_BLACKBOX)) { finishBlackbox(); - if (isSerialPortFunctionShared(FUNCTION_BLACKBOX, FUNCTION_MSP)) { - mspAllocateSerialPorts(&masterConfig.serialConfig); - } } #endif } @@ -712,7 +710,8 @@ void loop(void) ¤tProfile->pidProfile, currentControlRateProfile, masterConfig.max_angle_inclination, - ¤tProfile->accelerometerTrims + ¤tProfile->accelerometerTrims, + &masterConfig.rxConfig ); mixTable(); diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index 0e1182c29e..0fdca6d09a 100644 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -292,7 +292,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/rx/sbus.c b/src/main/rx/sbus.c index 71c543c4ab..6df723db07 100644 --- a/src/main/rx/sbus.c +++ b/src/main/rx/sbus.c @@ -121,7 +121,7 @@ static void sbusDataReceive(uint16_t c) static uint8_t sbusFramePosition = 0; static uint32_t sbusTimeoutAt = 0; - uint32_t now = millis(); + uint32_t now = micros(); if ((int32_t)(sbusTimeoutAt - now) < 0) { sbusFramePosition = 0; diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index 9766d40130..86539cb3ff 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -31,7 +31,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 @@ -55,7 +56,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++) { @@ -114,7 +115,7 @@ void updateCurrentMeter(int32_t lastUpdateAt) static int64_t mAhdrawnRaw = 0; amperageRaw -= amperageRaw / 8; - amperageRaw += adcGetChannel(ADC_CURRENT); + amperageRaw += (amperageLatestADC = adcGetChannel(ADC_CURRENT)); amperage = currentSensorToCentiamps(amperageRaw / 8); mAhdrawnRaw += (amperage * lastUpdateAt) / 1000; diff --git a/src/main/sensors/battery.h b/src/main/sensors/battery.h index f90395f5fc..23f918c7c3 100644 --- a/src/main/sensors/battery.h +++ b/src/main/sensors/battery.h @@ -42,9 +42,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/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/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..5001c57230 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -149,6 +149,7 @@ 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)/$@ diff --git a/src/test/unit/flight_imu_unittest.cc b/src/test/unit/flight_imu_unittest.cc index 67425aa63d..44aada0687 100644 --- a/src/test/unit/flight_imu_unittest.cc +++ b/src/test/unit/flight_imu_unittest.cc @@ -85,18 +85,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; -} - }