Merge remote-tracking branch 'upstream/master' into lowpass

.gitignore

@@ -11,3 +11,8 @@
 obj/
 patches/
 startup_stm32f10x_md_gcc.s
+
+# script-generated files
+docs/Manual.pdf
+README.pdf
+

.travis.sh

@@ -0,0 +1,7 @@
+#!/bin/bash
+# A hacky way of running the unit tests at the same time as the normal builds.
+if [ $RUNTESTS ] ; then
+	cd ./src/test && make test
+else
+	make -j2
+fi

.travis.yml

@@ -1,9 +1,11 @@
 env:
+  - RUNTESTS=True
   - TARGET=CC3D
+  - TARGET=CC3D OPBL=yes
   - TARGET=CHEBUZZF3
   - TARGET=CJMCU
   - TARGET=EUSTM32F103RC
-  - TARGET=MASSIVEF3
+  - TARGET=SPRACINGF3
   - TARGET=NAZE
   - TARGET=NAZE32PRO
   - TARGET=OLIMEXINO
@@ -11,12 +13,14 @@ env:
   - TARGET=SPARKY
   - TARGET=STM32F3DISCOVERY
   - TARGET=ALIENWIIF1
-language: c
+  - TARGET=ALIENWIIF3
-compiler: arm-none-eabi-gcc
+# We use cpp for unit tests, and c for the main project.
+language: cpp
+compiler: clang
 before_install: sudo add-apt-repository -y ppa:terry.guo/gcc-arm-embedded && sudo apt-get update
 install: sudo apt-get install build-essential gcc-arm-none-eabi git
-before_script: $CC --version
+before_script: arm-none-eabi-gcc --version
-script: make -j2
+script: ./.travis.sh
 
 notifications:
   irc: "chat.freenode.net#cleanflight"

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 <url to YOUR fork>`
+3. `cd cleanflight`
+4. `git checkout master`
+5. `git checkout -b my-new-code`
+6. Make changes
+7. `git add <files that have changed>`
+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.
+ 

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 ALIENWIIF3
 
 # Valid targets for OP BootLoader support
 OPBL_VALID_TARGETS = CC3D
@@ -53,13 +53,13 @@ LINKER_DIR	 = $(ROOT)/src/main/target
 
 # Search path for sources
 VPATH		:= $(SRC_DIR):$(SRC_DIR)/startup
+USBFS_DIR	= $(ROOT)/lib/main/STM32_USB-FS-Device_Driver
+USBPERIPH_SRC = $(notdir $(wildcard $(USBFS_DIR)/src/*.c))
 
-ifeq ($(TARGET),$(filter $(TARGET),STM32F3DISCOVERY CHEBUZZF3 NAZE32PRO MASSIVEF3 SPARKY))
+ifeq ($(TARGET),$(filter $(TARGET),STM32F3DISCOVERY CHEBUZZF3 NAZE32PRO SPRACINGF3 SPARKY ALIENWIIF3))
 
 STDPERIPH_DIR	= $(ROOT)/lib/main/STM32F30x_StdPeriph_Driver
-USBFS_DIR	= $(ROOT)/lib/main/STM32_USB-FS-Device_Driver
 
-USBPERIPH_SRC = $(notdir $(wildcard $(USBFS_DIR)/src/*.c))
 STDPERIPH_SRC = $(notdir $(wildcard $(STDPERIPH_DIR)/src/*.c))
 
 EXCLUDES	= stm32f30x_crc.c \
@@ -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))
 
 
@@ -151,14 +155,26 @@ INCLUDE_DIRS := $(INCLUDE_DIRS) \
 		   $(CMSIS_DIR)/CM3/CoreSupport \
 		   $(CMSIS_DIR)/CM3/DeviceSupport/ST/STM32F10x \
 
+DEVICE_STDPERIPH_SRC = $(STDPERIPH_SRC)
+
+ifeq ($(TARGET),CC3D)
+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_f103_128k.ld
 
 ARCH_FLAGS	 = -mthumb -mcpu=cortex-m3
 TARGET_FLAGS = -D$(TARGET) -pedantic
 DEVICE_FLAGS = -DSTM32F10X_MD -DSTM32F10X
 
-DEVICE_STDPERIPH_SRC = $(STDPERIPH_SRC)
-
 endif
 
 TARGET_DIR = $(ROOT)/src/main/target/$(TARGET)
@@ -229,10 +245,21 @@ HIGHEND_SRC  = flight/autotune.c \
 		   telemetry/frsky.c \
 		   telemetry/hott.c \
 		   telemetry/msp.c \
-           telemetry/smartport.c \
+		   telemetry/smartport.c \
 		   sensors/sonar.c \
 		   sensors/barometer.c \
-		   blackbox/blackbox.c
+		   blackbox/blackbox.c \
+		   blackbox/blackbox_io.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 \
+		   drivers/serial_usb_vcp.c
 
 NAZE_SRC	 = startup_stm32f10x_md_gcc.S \
 		   drivers/accgyro_adxl345.c \
@@ -270,7 +297,7 @@ NAZE_SRC	 = startup_stm32f10x_md_gcc.S \
 		   $(HIGHEND_SRC) \
 		   $(COMMON_SRC)
 
-ALIENWIIF1_SRC	= $(NAZE_SRC)
+ALIENWIIF1_SRC	 = $(NAZE_SRC)
 
 EUSTM32F103RC_SRC	 = startup_stm32f10x_hd_gcc.S \
 		   drivers/accgyro_adxl345.c \
@@ -350,7 +377,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 \
@@ -367,15 +395,20 @@ CJMCU_SRC	 = startup_stm32f10x_md_gcc.S \
 		   drivers/system_stm32f10x.c \
 		   drivers/timer.c \
 		   drivers/timer_stm32f10x.c \
-		   blackbox/blackbox.c \
 		   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 \
@@ -392,9 +425,11 @@ CC3D_SRC	 = startup_stm32f10x_md_gcc.S \
 		   drivers/timer.c \
 		   drivers/timer_stm32f10x.c \
 		   $(HIGHEND_SRC) \
-		   $(COMMON_SRC)
+		   $(COMMON_SRC) \
+		   $(VCP_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 +443,27 @@ 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/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
 
-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 +475,33 @@ 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) \
+SPARKY_SRC	 = \
-		   $(HIGHEND_SRC) \
+		   $(STM32F30x_COMMON_SRC) \
-		   $(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)
+
+ALIENWIIF3_SRC	 = $(SPARKY_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
 

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!

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'

docs/Battery.md

@@ -74,8 +74,19 @@ Enable current monitoring using the CLI command
 feature CURRENT_METER
 ```
 
+Configure the current meter type using the `current_meter_type` settings as per the following table.
+
+| Value | Sensor Type            |
+| ----- | ---------------------- | 
+| 0     | None                   |
+| 1     | ADC/hardware sensor    |
+| 2     | Virtual sensor         |
+
 Configure capacity using the `battery_capacity` setting, which takes a value in mAh.
 
+If you're using an OSD that expects the multiwii current meter output value, then set `multiwii_current_meter_output` to `1` (this multiplies amperage sent to MSP by 10).
+
+### ADC Sensor
 The current meter may need to be configured so that the value read at the ADC input matches actual current draw.  Just like you need a voltmeter to correctly calibrate your voltage reading you also need an ammeter to calibrate your current sensor.
 
 Use the following settings to adjust calibration. 
@@ -83,4 +94,23 @@ Use the following settings to adjust calibration.
 `current_meter_scale`
 `current_meter_offset`
 
-If you're using an OSD that expects the multiwii current meter output value, then set `multiwii_current_meter_output` to `1`.
+### Virtual Sensor
+The virtual sensor uses the throttle position to calculate as estimated current value. This is useful when a real sensor is not available. The following settings adjust the calibration.
+
+| Setting                       | Description                                              |
+| ----------------------------- | -------------------------------------------------------- | 
+| `current_meter_scale`      | The throttle scaling factor [centiamps, i.e. 1/100th A]  |
+| `current_meter_offset`     | The current at zero throttle [centiamps, i.e. 1/100th A] |
+
+If you know your current at zero throttle (Imin) and maximum throttle(Imax) you can calculate the scaling factors using the following formulas where Tmax is maximum throttle offset (i.e. for `max_throttle` = 1850, Tmax = 1850 - 1000 = 850):
+```
+current_meter_scale = (Imax - Imin) * 100000 / (Tmax + (Tmax * Tmax / 50))
+current_meter_offset = Imin * 100
+```
+e.g. For a maximum current of 34.2 A and minimum current of 2.8 A with `max_throttle` = 1850
+```
+current_meter_scale = (Imax - Imin) * 100000 / (Tmax + (Tmax * Tmax / 50))
+                    = (34.2 - 2.8) * 100000 / (850 + (850 * 850 / 50))
+                    = 205
+current_meter_offset = Imin * 100 = 280
+```

docs/Board - AlienWii32.md

@@ -1,10 +1,12 @@
-# Board - AlienWii32
+# Board - AlienWii32 (ALIENWIIF1 and ALIENWIIF3 target)
 
-The AlienWii32 is actually in prototype stage and only a few samples exist. There is some more field testing ongoing. The information below is preliminary and will be updated as needed.
+The AlienWii32 is actually in prototype stage and only a few samples exist. There are two different variants and some more field testing with some users is ongoing. The information below is preliminary and will be updated as needed.
 
 Here are the hardware specifications:
 
-- STM32F103CBT6 MCU
+- STM32F103CBT6 MCU (ALIENWIIF1)
+- STM32F303CCT6 MCU (ALIENWIIF3)
+- optional integrated serial/ppm receiver (ALIENWIIF3 only, future enhancement)
 - MPU6050 accelerometer/gyro sensor unit
 - 8x 4.2A brushed ESCs, integrated, to run the strongest micro motors
 - extra-wide traces on the PCB, for maximum power throughput
@@ -21,4 +23,4 @@ Here are the hardware specifications:
 
 Deltang receivers in serial mode will work like any other Spektrum satellite receiver (10bit, 22ms) only the bind process will be different. 
 
-The pin layout is very similar as the NAZE32 or the related clones (MW32, Flip32, etc.). The hardware bind pin is connected to pin 41 (PB5). The AlienWii32 firmware will be built with TARGET=NAZE and OPTIONS="AlienWii32". The firmware image will come with alternative default settings which will give the user a plug and play experience. There is no computer needed to get this into the air with an small Quadcopter. An preconfigured custom mixer for an Octocopter is part of the default settings to allow clean straight wiring with the AlienWii32. The mixer can be activated with "mixer custom" in the CLI. To use the AlienWii32 in an Hexa- or Octocopter or to do some more tuning additional configuration changes can be done as usual in the CLI or the Cleanflight configurator. 
+The pin layout for the ALIENWIIF1 is very similar to NAZE32 or the related clones (MW32, Flip32, etc.). The hardware bind pin is connected to pin 41 (PB5). The pin layout for the ALIENWIIF3 is similar to Sparky. The hardware bind pin is connected to pin 25 (PB12). The AlienWii32 firmware will be built as target ALIENWIIF1 or ALIENWIIF3. The firmware image will come with alternative default settings which will give the user a plug and play experience. There is no computer needed to get this into the air with an small Quadcopter. An preconfigured custom mixer for an Octocopter is part of the default settings to allow clean straight wiring with the AlienWii32. The mixer can be activated with "mixer custom" in the CLI. To use the AlienWii32 in an Hexa- or Octocopter or to do some more tuning additional configuration changes can be done as usual in the CLI or the Cleanflight configurator. 

docs/Board - CC3D.md

@@ -10,9 +10,6 @@ If issues are found with this board please report via the [github issue tracker]
 The board has a USB port directly connected to the processor.  Other boards like the Naze and Flip32
 have an on-board USB to uart adapter which connect to the processor's serial port instead.
 
-Currently there is no support for virtual com port functionality on the CC3D which means that cleanflight
-does not currently use the USB socket at all. Therefore, the communication with the board is achieved through a USB-UART adaptater connected to the Main port.
-
 The board cannot currently be used for hexacopters/octocopters.
 
 Tricopter & Airplane support is untested, please report success or failure if you try it. 
@@ -71,13 +68,30 @@ The 6 pin RC_Output connector has the following pinouts when used in RX_PARALLEL
 
 | Value | Identifier   | Board Markings | Notes                                    |
 | ----- | ------------ | -------------- | -----------------------------------------|
-| 1     | USART1       | MAIN PORT      | Has a hardware inverter for S.BUS        |
+| 1     | VCP          | USB PORT       |                                          |
-| 2     | USART3       | FLEX PORT      |                                          |
+| 2     | USART1       | MAIN PORT      | Has a hardware inverter for S.BUS        |
-| 3     | SoftSerial   | RC connector   | Pins 4 and 5 (Tx and Rx respectively)    |
+| 3     | USART3       | FLEX PORT      |                                          |
+| 4     | SoftSerial   | RC connector   | Pins 4 and 5 (Tx and Rx respectively)    |
 
 The SoftSerial port is not available when RX_PARALLEL_PWM is used. The transmission data rate is limited to 19200 baud.
 
-To connect the GUI to the flight controller you need additional hardware attached to the USART1 serial port (by default).
+To connect the GUI to the flight controller you just need a USB cable to use the Virtual Com Port (VCP).
+
+# 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
 
@@ -90,6 +104,8 @@ There are two primary ways to get Cleanflight onto a CC3D board.
 
 The entire flash ram on the target processor is flashed with a single image.
 
+The image can be flashed by using a USB to UART adapter connected to the main port when the CC3D is put into the STM32 bootloader mode, achieved by powering on the CC3D with the SBL/3.3v pads bridged.  
+
 ## OpenPilot Bootloader compatible image mode.
 
 The initial section of flash ram on the target process is flashed with a bootloader which can then run the code in the
@@ -98,4 +114,3 @@ remaining area of flash ram.
 The OpenPilot bootloader code also allows the remaining section of flash to be reconfigured and re-flashed by the
 OpenPilot Ground Station (GCS) via USB without requiring a USB to uart adapter.
 
-In this mode a USB to uart adapter is still required to connect to via the GUI or CLI.

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.

docs/Buzzer.md

@@ -35,4 +35,4 @@ Buzzer support on the CC3D requires that a buzzer circuit be created to which th
 PA15 is unused and not connected according to the CC3D Revision A schematic.
 Connecting to PA15 requires careful soldering.
 
-See the [CC3D - buzzer circuir.pdf](Wiring/CC3D - buzzer circuir.pdf) for details.
+See the [CC3D - buzzer circuit.pdf](Wiring/CC3D - buzzer circuit.pdf) for details.

docs/Configuration.md

@@ -1,6 +1,6 @@
 # Configuration
 
-Cleanflight is configured primarilty using the Cleanflight Configurator GUI.
+Cleanflight is configured primarily using the Cleanflight Configurator GUI.
 
 Both the command line interface and GUI are accessible by connecting to a serial port on the target,
 be it a USB virtual serial port, physical hardware UART port or a SoftSerial port.
@@ -15,7 +15,7 @@ __Due to ongoing development, the fact that the GUI cannot yet backup all your s
 
 ## GUI
 
-![Cleanflight Gui](cleanflight-gui.png)
+![Cleanflight Gui](Screenshots/cleanflight-gui.png)
 
 The GUI tool is the preferred way of configuration.  The GUI tool also includes a terminal which
 can be used to interact with the CLI.

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. 
 

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).
 

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

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..
 
@@ -51,12 +52,12 @@ uses.  e.g. ESC1/BEC1 -> FC, ESC2/BEC2 -> LED strip.   It's also possible to pow
 from another BEC.  Just ensure that the GROUND is the same for all BEC outputs and LEDs.
 
 
-| Target                | Pin | LED Strip | Signal |
+| Target                | Pin  | LED Strip | Signal |
-| --------------------- | --- | --------- | -------|
+| --------------------- | ---- | --------- | -------|
-| Naze/Olimexino        | RC5 | Data In   | PA6    |
+| Naze/Olimexino        | RC5  | Data In   | PA6    |
-| CC3D                  | ??? | Data In   | PB4    |
+| CC3D                  | RCO5 | Data In   | PB4    |
-| ChebuzzF3/F3Discovery | PB8 | Data In   | PB8    |
+| ChebuzzF3/F3Discovery | PB8  | Data In   | PB8    |
-
+| Sparky                | PWM5 | Data In   | PA6    |
 
 Since RC5 is also used for SoftSerial on the Naze/Olimexino it means that you cannot use SoftSerial and led strips at the same time.
 Additionally, since RC5 is also used for Parallel PWM RC input on both the Naze, Chebuzz and STM32F3Discovery targets, led strips
@@ -83,11 +84,11 @@ If you enable LED_STRIP feature and the feature is turned off again after a rebo
 
 Configure the LEDs using the `led` command.
 
-The `led` command takes either zero or two arguments - an zero-based led number and a pair of coordinates, direction flags and mode flags.
+The `led` command takes either zero or two arguments - an zero-based led number and a sequence which indicates pair of coordinates, direction flags and mode flags and a color.
 
 If used with zero arguments it prints out the led configuration which can be copied for future reference.
 
-Each led is configured using the following template: `x,y:ddd:mmm`
+Each led is configured using the following template: `x,y:ddd:mmm:cc`
 
 `x` and `y` are grid coordinates of a 0 based 16x16 grid, north west is 0,0, south east is 15,15
 `ddd` specifies the directions, since an led can face in any direction it can have multiple directions.  Directions are:
@@ -110,22 +111,29 @@ 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.
+* `C` - `C`olor.
+
+`cc` specifies the color number (0 based index).
 
 Example:
 
 ```
-led 0 0,15:SD:IAW
+led 0 0,15:SD:IAW:0
-led 1 15,0:ND:IAW
+led 1 15,0:ND:IAW:0
-led 2 0,0:ND:IAW
+led 2 0,0:ND:IAW:0
-led 3 0,15:SD:IAW
+led 3 0,15:SD:IAW:0
+led 4 7,7::C:1
+led 5 8,8::C:2
 ```
 
-to erase an led, and to mark the end of the chain, use `0,0::` as the second argument, like this:
+To erase an led, and to mark the end of the chain, use `0,0::` as the second argument, like this:
 
 ```
-led 4 0,0::
+led 4 0,0:::
 ```
 
+It is best to erase all LEDs that you do not have connected.
 
 ### Modes
 
@@ -154,6 +162,8 @@ LEDs are set in a specific order:
 
 That is, south facing LEDs have priority.
 
+The mapping between modes led placement and colors is currently fixed and cannot be changed.
+
 #### Indicator
 
 This mode flashes LEDs that correspond to roll and pitch stick positions.  i.e.  they indicate the direction the craft is going to turn.
@@ -170,6 +180,88 @@ 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 16 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.
+
+#### Solid Color
+
+The mode allows you to set an LED to be permanently on and set to a specific color.
+
+x,y position and directions are ignored when using this mode.
+
+Other modes will override or combine with the color mode.
+
+For example, to set led 0 to always use color 10 you would issue this command. 
+
+```
+led 0 0,0::C:10
+```
+
+### Colors
+
+Colors can be configured using the cli `color` command.
+
+The `color` command takes either zero or two arguments - an zero-based color number and a sequence which indicates pair of hue, saturation and value (HSV).
+
+See http://en.wikipedia.org/wiki/HSL_and_HSV
+
+If used with zero arguments it prints out the color configuration which can be copied for future reference.
+
+The default color configuration is as follows:
+
+| Index | Color       |
+| ----- | ----------- |
+|     0 | black       |
+|     1 | white       |
+|     2 | red         |
+|     3 | orange      |
+|     4 | yellow      |
+|     5 | lime green  |
+|     6 | green       |
+|     7 | mint green  |
+|     8 | cyan        |
+|     9 | light blue  |
+|    10 | blue        |
+|    11 | dark violet |
+|    12 | magenta     |
+|    13 | deep pink   |
+|    14 | black       |
+|    15 | black       |
+
+```
+color 0 0,0,0
+color 1 0,255,255
+color 2 0,0,255
+color 3 30,0,255
+color 4 60,0,255
+color 5 90,0,255
+color 6 120,0,255
+color 7 150,0,255
+color 8 180,0,255
+color 9 210,0,255
+color 10 240,0,255
+color 11 270,0,255
+color 12 300,0,255
+color 13 330,0,255
+color 14 0,0,0
+color 15 0,0,0
+```
+
 ## 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.
@@ -181,128 +273,150 @@ Orientation is when viewed with the front of the aircraft facing away from you a
 
 The default configuration is as follows
 ```
-led 0 2,2:ES:IA
+led 0 15,15:ES:IA:0
-led 1 2,1:E:WF
+led 1 15,8:E:WF:0
-led 2 2,0:NE:IA
+led 2 15,7:E:WF:0
-led 3 1,0:N:F
+led 3 15,0:NE:IA:0
-led 4 0,0:NW:IA
+led 4 8,0:N:F:0
-led 5 0,1:W:WF
+led 5 7,0:N:F:0
-led 6 0,2:SW:IA
+led 6 0,0:NW:IA:0
-led 7 1,2:S:WF
+led 7 0,7:W:WF:0
-led 8 1,1:U:WF
+led 8 0,8:W:WF:0
-led 9 1,1:U:WF
+led 9 0,15:SW:IA:0
-led 10 1,1:D:WF
+led 10 7,15:S:WF:0
-led 11 1,1:D:WF
+led 11 8,15:S:WF:0
+led 12 7,7:U:WF:0
+led 13 8,7:U:WF:0
+led 14 7,8:D:WF:0
+led 15 8,8:D:WF:0
+led 16 8,9::R:3
+led 17 9,10::R:3
+led 18 10,11::R:3
+led 19 10,12::R:3
+led 20 9,13::R:3
+led 21 8,14::R:3
+led 22 7,14::R:3
+led 23 6,13::R:3
+led 24 5,12::R:3
+led 25 5,11::R:3
+led 26 6,10::R:3
+led 27 7,9::R:3
+led 28 0,0:::0
+led 29 0,0:::0
+led 30 0,0:::0
+led 31 0,0:::0
 ```
 
 Which translates into the following positions:
 
 ```
-     5             3
+     6             3
       \           /
-       \    4    / 
+       \   5-4   /
         \ FRONT /
-      6 | 9-12  | 2
+    7,8 | 12-15 | 1,2
         /  BACK \
-       /    8    \
+       /  10,11  \
       /           \
-     7             1  
+     9             0
+       RING 16-27
 ```
 
-LEDs 1,3,5 and 7 should be placed underneath the quad, facing downwards.
+LEDs 0,3,6 and 9 should be placed underneath the quad, facing downwards.
-LEDs 2, 4, 6 and 8 should be positioned so the face east/north/west/south, respectively.
+LEDs 1-2, 4-5, 7-8 and 10-11 should be positioned so the face east/north/west/south, respectively.
-LEDs 9-10 should be placed facing down, in the middle
+LEDs 12-13 should be placed facing down, in the middle
-LEDs 11-12 should be placed facing up, in the middle
+LEDs 14-15 should be placed facing up, in the middle
+LEDs 16-17 should be placed in a ring and positioned at the rear facing south.
 
-This is the default so that if you don't want to place LEDs top and bottom in the middle just connect the first 8 LEDs.
+This is the default so that if you don't want to place LEDs top and bottom in the middle just connect the first 12 LEDs.
 
 ### Example 16 LED config
 
 ```
-led 0 15,15:SD:IA
+led 0 15,15:SD:IA:0
-led 1 8,8:E:FW
+led 1 8,8:E:FW:0
-led 2 8,7:E:FW
+led 2 8,7:E:FW:0
-led 3 15,0:ND:IA
+led 3 15,0:ND:IA:0
-led 4 7,7:N:FW
+led 4 7,7:N:FW:0
-led 5 8,7:N:FW
+led 5 8,7:N:FW:0
-led 6 0,0:ND:IA
+led 6 0,0:ND:IA:0
-led 7 7,7:W:FW
+led 7 7,7:W:FW:0
-led 8 7,8:W:FW
+led 8 7,8:W:FW:0
-led 9 0,15:SD:IA
+led 9 0,15:SD:IA:0
-led 10 7,8:S:FW
+led 10 7,8:S:FW:0
-led 11 8,8:S:FW
+led 11 8,8:S:FW:0
-led 12 7,7:D:FW
+led 12 7,7:D:FW:0
-led 13 8,7:D:FW
+led 13 8,7:D:FW:0
-led 14 7,7:U:FW
+led 14 7,7:U:FW:0
-led 15 8,7:U:FW
+led 15 8,7:U:FW:0
 ```
 
 Which translates into the following positions:
 
 ```
-     7             4
+     6             3
       \           / 
-       \   6-5   / 
+       \   5-4   / 
-      8 \ FRONT / 3
+      7 \ FRONT / 2
-        | 13-16 | 
+        | 12-15 | 
-      9 /  BACK \ 2
+      8 /  BACK \ 1
-       /  11-12  \
+       /  10-11  \
       /           \ 
-    10             1  
+     9             0
 ```
 
-LEDs 1,4,7 and 10 should be placed underneath the quad, facing downwards.
+LEDs 0,3,6 and 9 should be placed underneath the quad, facing downwards.
-LEDs 2-3, 6-5, 8-9 and 11-12 should be positioned so the face east/north/west/south, respectively.
+LEDs 1-2, 4-5, 7-8 and 10-11 should be positioned so the face east/north/west/south, respectively.
-LEDs 13-14 should be placed facing down, in the middle
+LEDs 12-13 should be placed facing down, in the middle
-LEDs 15-16 should be placed facing up, in the middle
+LEDs 14-15 should be placed facing up, in the middle
  
 ### Exmple 28 LED config
 
 ```
 #right rear cluster
-led 0 9,9:S:FWT
+led 0 9,9:S:FWT:0
-led 1 10,10:S:FWT
+led 1 10,10:S:FWT:0
-led 2 11,11:S:IA
+led 2 11,11:S:IA:0
-led 3 11,11:E:IA
+led 3 11,11:E:IA:0
-led 4 10,10:E:AT
+led 4 10,10:E:AT:0
-led 5 9,9:E:AT
+led 5 9,9:E:AT:0
 # right front cluster
-led 6 10,5:S:F
+led 6 10,5:S:F:0
-led 7 11,4:S:F
+led 7 11,4:S:F:0
-led 8 12,3:S:IA
+led 8 12,3:S:IA:0
-led 9 12,2:N:IA
+led 9 12,2:N:IA:0
-led 10 11,1:N:F
+led 10 11,1:N:F:0
-led 11 10,0:N:F
+led 11 10,0:N:F:0
 # center front cluster
-led 12 7,0:N:FW
+led 12 7,0:N:FW:0
-led 13 6,0:N:FW
+led 13 6,0:N:FW:0
-led 14 5,0:N:FW
+led 14 5,0:N:FW:0
-led 15 4,0:N:FW
+led 15 4,0:N:FW:0
 # left front cluster
-led 16 2,0:N:F
+led 16 2,0:N:F:0
-led 17 1,1:N:F
+led 17 1,1:N:F:0
-led 18 0,2:N:IA
+led 18 0,2:N:IA:0
-led 19 0,3:W:IA
+led 19 0,3:W:IA:0
-led 20 1,4:S:F
+led 20 1,4:S:F:0
-led 21 2,5:S:F
+led 21 2,5:S:F:0
 # left rear cluster
-led 22 2,9:W:AT
+led 22 2,9:W:AT:0
-led 23 1,10:W:AT
+led 23 1,10:W:AT:0
-led 24 0,11:W:IA
+led 24 0,11:W:IA:0
-led 25 0,11:S:IA
+led 25 0,11:S:IA:0
-led 26 1,10:S:FWT
+led 26 1,10:S:FWT:0
-led 27 2,9:S:FWT
+led 27 2,9:S:FWT:0
 ```
 
 ```
-       17-19  10-12
+       16-18  9-11
-20-22 \           / 7-9
+19-21 \           / 6-8
        \  13-16  / 
         \ FRONT /
         /  BACK \
        /         \
-23-25 /           \ 4-6
+22-24 /           \ 3-5
-       26-28   1-3  
+       25-27   0-2  
 ```
 
 All LEDs should face outwards from the chassis in this configuration.

docs/Modes.md

@@ -3,30 +3,30 @@
 Cleanflight has various modes that can be toggled on or off.  Modes can be enabled/disabled by stick positions,
 auxillary receiver channels and other events such as failsafe detection.
 
-| ID  | Short Name | Function                                                             |
+| MSP ID  | Short Name | Function                                                             |
-| --- | ---------- | -------------------------------------------------------------------- |
+| ------- | ---------- | -------------------------------------------------------------------- |
-| 0   | ARM        | Enables motors and flight stabilisation                              |
+| 0       | ARM        | Enables motors and flight stabilisation                              |
-| 1   | ANGLE      | Legacy auto-level flight mode                                        |
+| 1       | ANGLE      | Legacy auto-level flight mode                                        |
-| 2   | HORIZON    | Auto-level flight mode                                               |
+| 2       | HORIZON    | Auto-level flight mode                                               |
-| 3   | BARO       | Altitude hold mode (Requires barometer sensor)                       |
+| 3       | BARO       | Altitude hold mode (Requires barometer sensor)                       |
-| 4   | MAG        | Heading lock                                                         |
+| 5       | MAG        | Heading lock                                                         |
-| 5   | HEADFREE   | Head Free - When enabled yaw has no effect on pitch/roll inputs      |
+| 6       | HEADFREE   | Head Free - When enabled yaw has no effect on pitch/roll inputs      |
-| 6   | HEADADJ    | Heading Adjust - Sets a new yaw origin for HEADFREE mode             |
+| 7       | HEADADJ    | Heading Adjust - Sets a new yaw origin for HEADFREE mode             |
-| 7   | CAMSTAB    | Camera Stabilisation                                                 |
+| 8       | CAMSTAB    | Camera Stabilisation                                                 |
-| 8   | CAMTRIG    |                                                                      |
+| 9       | CAMTRIG    |                                                                      |
-| 9   | GPSHOME    | Autonomous flight to HOME position                                   |
+| 10      | GPSHOME    | Autonomous flight to HOME position                                   |
-| 10  | GPSHOLD    | Maintain the same longitude/lattitude                                |
+| 11      | GPSHOLD    | Maintain the same longitude/lattitude                                |
-| 11  | PASSTHRU   |                                                                      |
+| 12      | PASSTHRU   |                                                                      |
-| 12  | BEEPERON   | Enable beeping - useful for locating a crashed aircraft              |
+| 13      | BEEPERON   | Enable beeping - useful for locating a crashed aircraft              |
-| 13  | LEDMAX     |                                                                      |
+| 14      | LEDMAX     |                                                                      |
-| 14  | LEDLOW     |                                                                      |
+| 15      | LEDLOW     |                                                                      |
-| 15  | LLIGHTS    |                                                                      |
+| 16      | LLIGHTS    |                                                                      |
-| 16  | CALIB      |                                                                      |
+| 17      | CALIB      |                                                                      |
-| 17  | GOV        |                                                                      |
+| 18      | GOV        |                                                                      |
-| 18  | OSD        | Enable/Disable On-Screen-Display (OSD)                               |
+| 19      | OSD        | Enable/Disable On-Screen-Display (OSD)                               |
-| 19  | TELEMETRY  | Enable telemetry via switch                                          |
+| 20      | TELEMETRY  | Enable telemetry via switch                                          |
-| 20  | AUTOTUNE   | Autotune Pitch/Roll PIDs                                             |
+| 21      | AUTOTUNE   | Autotune Pitch/Roll PIDs                                             |
-| 21  | SONAR      | Altitude hold mode (sonar sensor only)                               |
+| 22      | SONAR      | Altitude hold mode (sonar sensor only)                               |
 
 ## Mode details
 

docs/PID tuning.md

@@ -0,0 +1,136 @@
+# 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 6 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=n` on the CLI tab of the Cleanflight
+Configurator, where `n` 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, "LuxFloat"
+
+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.
+
+It is the first PID Controller designed for 32-bit processors and not derived from MultiWii.
+
+The strength of the auto-leveling correction applied during Angle mode is set by the parameter "level_angle" which
+is labeled "LEVEL Proportional" 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.
+
+### PID controller 3, "MultiWii23" (default for the ALIENWIIF1 and ALIENWIIF3 targets)
+
+PID Controller 3 is an direct port of the PID controller from MultiWii 2.3 and later.
+
+The algorithm is handling roll and pitch differently to yaw. Users with problems on yaw authority should try this one.
+
+For the ALIENWII32 targets the gyroscale is removed for even more yaw authority. This will provide best performance on very small multicopters with brushed motors.
+
+### PID controller 4, "MultiWiiHybrid"
+
+PID Controller 4 is an hybrid version of two MultiWii PID controllers. Roll and pitch is using the MultiWii 2.2 algorithm and yaw is using the 2.3 algorithm. 
+
+This PID controller was initialy implemented for testing purposes but is also performing quite well.
+
+For the ALIENWII32 targets the gyroscale is removed for more yaw authority. This will provide best performance on very small multicopters with brushed motors.
+
+### PID controller 5, "Harakiri"
+
+PID Controller 5 is an port of the PID controller from the Harakiri firmware.
+
+The algorithm is leveraging more floating point math. This PID controller also compensates for different looptimes on roll and pitch. It likely don<6F>t need retuning of the PID values when looptime is changing. Actually there are two settings hardcoded which are configurable via the GUI in Harakiri:
+
+        OLD_YAW 0 // [0/1] 0 = multiwii 2.3 yaw, 1 = older yaw.
+        MAIN_CUT_HZ 12.0f // (default 12Hz, Range 1-50Hz)
+
+The PID controller is flight tested and running well with the default PID settings. If you want do acrobatics start slowly.
+
+Yaw authority is also quite good.
+
+
+

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
+1. Parallel PWM Receivers
-PPM Receivers
+2. PPM Receivers
-Serial Receivers
+3. Serial Receivers
 
 ## Parallel PWM Receivers
 
@@ -61,6 +61,10 @@ http://www.frsky-rc.com/product/pro.php?pro_id=135
 FrSky X8R 8/16ch Telemetry Receiver
 http://www.frsky-rc.com/product/pro.php?pro_id=105
 
+Futaba R2008SB 2.4GHz S-FHSS
+http://www.futaba-rc.com/systems/futk8100-8j/
+
+
 #### OpenTX S.BUS configuration
 
 If using OpenTX set the transmitter module to D16 mode and select CH1-16 on the transmitter before binding to allow reception
@@ -82,6 +86,11 @@ These receivers are reported working:
 XG14 14ch DMSS System w/RG731BX XBus Receiver
 http://www.jramericas.com/233794/JRP00631/
 
+There exist a remote receiver made for small BNF-models like the Align T-Rex 150 helicopter. The code also supports using the Align DMSS RJ01 receiver directly with the cleanflight software.
+To use this receiver you must power it with 3V from the hardware, and then connect the serial line as other serial RX receivers.
+In order for this receiver to work, you need to specify the XBUS_MODE_B_RJ01 for serialrx_provider. Note that you need to set your radio mode for XBUS "MODE B" also for this receiver to work.
+Receiver name: Align DMSS RJ01 (HER15001)
+
 ### SUMD
 
 16 channels via serial currently supported.
@@ -131,7 +140,7 @@ For Serial RX enable `RX_SERIAL` and set the `serialrx_provider` CLI setting as
 | SUMD               | 3     |
 | SUMH               | 4     |
 | XBUS_MODE_B        | 5     |
-
+| XBUS_MODE_B_RJ01   | 6     |
 
 ### PPM/PWM input filtering.
 

docs/Spektrum bind.md

@@ -2,7 +2,7 @@
 
 Spektrum bind with hardware bind plug support.
  
-The Spektrum bind code is actually enabled for the NAZE, NAZE32PRO, CJMCU, EUSTM32F103RC, SPARKY, CC3D targets.
+The Spektrum bind code is actually enabled for the NAZE, NAZE32PRO, CJMCU, EUSTM32F103RC, SPARKY, CC3D, ALIENWIIF1, ALIENWIIF3 targets.
 
 ## Configure the bind code
 
@@ -46,4 +46,4 @@ http://wiki.openpilot.org/display/Doc/Spektrum+Satellite
 
 ### Supported Hardware
 
-NAZE, NAZE32PRO, CJMCU, SPARKY, EUSTM32F103RC, CC3D targets (AlienWii32 with hardware bind plug)
+NAZE, NAZE32PRO, CJMCU, SPARKY, EUSTM32F103RC, CC3D targets and ALIENWIIF1, ALIENWIIF3 targets with hardware bind plug

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)
+* HoTT TX/RX `T` -> Serial RX (connect directly)
-Serial TX -> 1N4148 Diode -(|  )-> HoTT TX/RX (connect via diode)
+* 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.

docs/development/Building in Ubuntu.md

@@ -0,0 +1,89 @@
+# Building in Ubuntu
+
+Building for Ubuntu platform is remarkably easy. The only trick to understand is that the Ubuntu toolchain,
+which they are downstreaming from Debian, is not compatible with Cleanflight. We suggest that you take an
+alternative PPA from Terry Guo, found here:
+https://launchpad.net/~terry.guo/+archive/ubuntu/gcc-arm-embedded
+
+This PPA has several compiler versions and platforms available. For many hardware platforms (notably Naze)
+the 4.9.3 compiler will work fine. For some, older compiler 4.8 (notably Sparky) is more appropriate. We
+suggest you build with 4.9.3 first, and try to see if you can connect to the CLI or run the Configurator.
+If you cannot, please see the section below for further hints on what you might do.
+
+## Setup GNU ARM Toolchain
+
+Note specifically the last paragraph of Terry's PPA documentation -- Ubuntu carries its own package for
+`gcc-arm-none-eabi`, so you'll have to remove it, and then pin the one from the PPA.
+For your release, you should first remove any older pacakges (from Debian or Ubuntu directly), introduce
+Terry's PPA, and update:
+```
+sudo apt-get remove binutils-arm-none-eabi gcc-arm-none-eabi
+sudo add-apt-repository ppa:terry.guo/gcc-arm-embedded
+sudo apt-get update
+```
+
+For Ubuntu 14.10 (current release, called Utopic Unicorn), you should pin:
+```
+sudo apt-get install gcc-arm-none-eabi=4.9.3.2014q4-0utopic12
+```
+
+For Ubuntu 14.04 (an LTS as of Q1'2015, called Trusty Tahr), you should pin:
+```
+sudo apt-get install gcc-arm-none-eabi=4.9.3.2014q4-0trusty12
+```
+
+For Ubuntu 12.04 (previous LTS, called Precise Penguin), you should pin:
+```
+sudo apt-get install gcc-arm-none-eabi=4.9.3.2014q4-0precise12
+```
+
+## Building on Ubuntu
+
+After the ARM toolchain from Terry is installed, you should be able to build from source.
+```
+cd src
+git clone git@github.com:cleanflight/cleanflight.git
+cd cleanflight
+make TARGET=NAZE
+```
+
+You'll see a set of files being compiled, and finally linked, yielding both an ELF and then a HEX:
+```
+...
+arm-none-eabi-size ./obj/main/cleanflight_NAZE.elf 
+   text    data     bss     dec     hex filename
+  97164     320   11080  108564   1a814 ./obj/main/cleanflight_NAZE.elf
+arm-none-eabi-objcopy -O ihex --set-start 0x8000000 obj/main/cleanflight_NAZE.elf obj/cleanflight_NAZE.hex
+$ ls -la obj/cleanflight_NAZE.hex                                                                                                                                                 
+-rw-rw-r-- 1 pim pim 274258 Jan 12 21:45 obj/cleanflight_NAZE.hex
+```
+
+You can use the Cleanflight-Configurator to flash the ```obj/cleanflight_NAZE.hex``` file.
+
+## Bricked/Bad build?
+
+Users have reported that the 4.9.3 compiler for ARM produces bad builds, for example on the Sparky hardware platform.
+It is very likely that using an older compiler would be fine -- Terry happens to have also a 4.8 2014q2 build in his
+PPA - to install this, you can fetch the `.deb` directly:
+http://ppa.launchpad.net/terry.guo/gcc-arm-embedded/ubuntu/pool/main/g/gcc-arm-none-eabi/
+
+and use `dpkg` to install:
+```
+sudo dpkg -i gcc-arm-none-eabi_4-8-2014q2-0saucy9_amd64.deb
+```
+
+Make sure to remove `obj/` and `make clean`, before building again.
+
+## Updating and rebuilding
+
+Navigate to the local cleanflight repository and use the following steps to pull the latest changes and rebuild your version of cleanflight:
+
+```
+cd src/cleanflight
+git reset --hard
+git pull
+make clean TARGET=NAZE
+make
+```
+
+Credit goes to K.C. Budd, AKfreak for testing, and pulsar for doing the long legwork that yielded this very short document.

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```. 

docs/development/Development.md

@@ -16,6 +16,8 @@ This project could really do with some functional tests which test the behaviour
 
 All pull requests to add/improve the testability of the code or testing methods are highly sought!
 
+Note: Tests are written in C++ and linked with with firmware's C code.
+
 ##General principals
 
 1. Name everything well.
@@ -26,7 +28,7 @@ All pull requests to add/improve the testability of the code or testing methods
 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.
@@ -48,22 +50,3 @@ You can run them on the command line to execute the tests and to see the test re
 You can also step-debug the tests in eclipse and you can use the GoogleTest test runner to make building and re-running the tests simple.
 
 The tests are currently always compiled with debugging information enabled, there may be additional warnings, if you see any warnings please attempt to fix them and submit pull requests with the fixes.
-
-
-##TODO
-
-* Test OpenLRSNG's RSSI PWM on AUX5-8.
-* Add support for UART3/4 on STM32F3.
-* Cleanup validateAndFixConfig and pwm_mapping.c to use some kind of feature/timer/io pin mapping to remove #ifdef
-* Split RX config into RC config and RX config.
-* Enabling/disabling features should not take effect until reboot since.  Main loop executes and uses new flags as they are set in the cli but
-appropriate init methods will not have been called which results in undefined behaviour and could damage connected devices - this is a legacy
-problem from baseflight.
-* Solve all the naze rev4/5 HSE_VALUE == 8000000/1200000 checking, the checks should only apply to the naze32 target.  See system_stm32f10x.c/SetSysClock().
-
-##Known Issues
-
-* Softserial RX on STM32F3 does not work. TX is fine.
-* Dynamic throttle PID does not work with new pid controller.
-* Autotune does not work yet with with new pid controller.
-

src/main/blackbox/blackbox.h

@@ -17,9 +17,12 @@
 
 #pragma once
 
-#include "common/axis.h"
 #include <stdint.h>
 
+#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);

src/main/blackbox/blackbox_fielddefs.h

@@ -29,21 +29,21 @@ typedef enum FlightLogFieldCondition {
     FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_8,
     FLIGHT_LOG_FIELD_CONDITION_TRICOPTER,
 
-    FLIGHT_LOG_FIELD_CONDITION_MAG = 20,
+    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_P_0 = 40,
-    FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_P_1,
-    FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_P_2,
-    FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_I_0,
-    FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_I_1,
-    FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_I_2,
     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_NEVER = 255,
+    FLIGHT_LOG_FIELD_CONDITION_NOT_LOGGING_EVERY_FRAME,
+
+    FLIGHT_LOG_FIELD_CONDITION_NEVER,
+
+    FLIGHT_LOG_FIELD_CONDITION_FIRST = FLIGHT_LOG_FIELD_CONDITION_ALWAYS,
+    FLIGHT_LOG_FIELD_CONDITION_LAST = FLIGHT_LOG_FIELD_CONDITION_NEVER
 } FlightLogFieldCondition;
 
 typedef enum FlightLogFieldPredictor {
@@ -75,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;
 
@@ -96,5 +99,51 @@ 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_AUTOTUNE_TARGETS = 12,
     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;
+    uint8_t rising;
+} flightLogEvent_autotuneCycleStart_t;
+
+#define FLIGHT_LOG_EVENT_AUTOTUNE_FLAG_OVERSHOT 1
+#define FLIGHT_LOG_EVENT_AUTOTUNE_FLAG_TIMEDOUT 2
+
+typedef struct flightLogEvent_autotuneCycleResult_t {
+    uint8_t flags;
+    uint8_t p;
+    uint8_t i;
+    uint8_t d;
+} flightLogEvent_autotuneCycleResult_t;
+
+typedef struct flightLogEvent_autotuneTargets_t {
+    uint16_t currentAngle;
+    int8_t targetAngle, targetAngleAtPeak;
+    uint16_t firstPeakAngle, secondPeakAngle;
+} flightLogEvent_autotuneTargets_t;
+
+typedef union flightLogEventData_t
+{
+    flightLogEvent_syncBeep_t syncBeep;
+    flightLogEvent_autotuneCycleStart_t autotuneCycleStart;
+    flightLogEvent_autotuneCycleResult_t autotuneCycleResult;
+    flightLogEvent_autotuneTargets_t autotuneTargets;
+} flightLogEventData_t;
+
+typedef struct flightLogEvent_t
+{
+    FlightLogEvent event;
+    flightLogEventData_t data;
+} flightLogEvent_t;

src/main/blackbox/blackbox_io.c

@@ -0,0 +1,437 @@
+#include <stdlib.h>
+#include <stdarg.h>
+
+#include "blackbox_io.h"
+
+#include "platform.h"
+#include "version.h"
+
+#include "common/maths.h"
+#include "common/axis.h"
+#include "common/color.h"
+
+#include "drivers/gpio.h"
+#include "drivers/sensor.h"
+#include "drivers/system.h"
+#include "drivers/serial.h"
+#include "drivers/compass.h"
+#include "drivers/timer.h"
+#include "drivers/pwm_rx.h"
+#include "drivers/accgyro.h"
+#include "drivers/light_led.h"
+#include "drivers/sound_beeper.h"
+
+#include "flight/flight.h"
+#include "sensors/sensors.h"
+#include "sensors/boardalignment.h"
+#include "sensors/acceleration.h"
+#include "sensors/barometer.h"
+#include "sensors/gyro.h"
+#include "sensors/battery.h"
+#include "io/beeper.h"
+#include "io/display.h"
+#include "io/escservo.h"
+#include "rx/rx.h"
+#include "io/rc_controls.h"
+#include "flight/mixer.h"
+#include "flight/altitudehold.h"
+#include "flight/failsafe.h"
+#include "flight/imu.h"
+#include "flight/autotune.h"
+#include "flight/navigation.h"
+#include "io/gimbal.h"
+#include "io/gps.h"
+#include "io/ledstrip.h"
+#include "io/serial.h"
+#include "io/serial_cli.h"
+#include "io/serial_msp.h"
+#include "io/statusindicator.h"
+#include "rx/msp.h"
+#include "telemetry/telemetry.h"
+#include "common/printf.h"
+
+#include "config/runtime_config.h"
+#include "config/config.h"
+#include "config/config_profile.h"
+#include "config/config_master.h"
+
+#define BLACKBOX_BAUDRATE 115200
+#define BLACKBOX_INITIAL_PORT_MODE MODE_TX
+
+// How many bytes should we transmit per loop iteration?
+uint8_t blackboxWriteChunkSize = 16;
+
+static serialPort_t *blackboxPort;
+static portMode_t previousPortMode;
+static uint32_t previousBaudRate;
+
+void blackboxWrite(uint8_t value)
+{
+    serialWrite(blackboxPort, value);
+}
+
+static void _putc(void *p, char c)
+{
+    (void)p;
+    blackboxWrite(c);
+}
+
+//printf() to the blackbox serial port with no blocking shenanigans (so it's caller's responsibility to not write too fast!)
+void blackboxPrintf(char *fmt, ...)
+{
+    va_list va;
+    va_start(va, fmt);
+    tfp_format(NULL, _putc, fmt, va);
+    va_end(va);
+}
+
+// Print the null-terminated string 's' to the serial port and return the number of bytes written
+int blackboxPrint(const char *s)
+{
+    const char *pos = s;
+
+    while (*pos) {
+        blackboxWrite(*pos);
+        pos++;
+    }
+
+    return pos - s;
+}
+
+/**
+ * Write an unsigned integer to the blackbox serial port using variable byte encoding.
+ */
+void blackboxWriteUnsignedVB(uint32_t value)
+{
+    //While this isn't the final byte (we can only write 7 bits at a time)
+    while (value > 127) {
+        blackboxWrite((uint8_t) (value | 0x80)); // Set the high bit to mean "more bytes follow"
+        value >>= 7;
+    }
+    blackboxWrite(value);
+}
+
+/**
+ * Write a signed integer to the blackbox serial port using ZigZig and variable byte encoding.
+ */
+void blackboxWriteSignedVB(int32_t value)
+{
+    //ZigZag encode to make the value always positive
+    blackboxWriteUnsignedVB((uint32_t)((value << 1) ^ (value >> 31)));
+}
+
+void blackboxWriteS16(int16_t value)
+{
+    blackboxWrite(value & 0xFF);
+    blackboxWrite((value >> 8) & 0xFF);
+}
+
+/**
+ * Write a 2 bit tag followed by 3 signed fields of 2, 4, 6 or 32 bits
+ */
+void blackboxWriteTag2_3S32(int32_t *values) {
+    static const int NUM_FIELDS = 3;
+
+    //Need to be enums rather than const ints if we want to switch on them (due to being C)
+    enum {
+        BITS_2  = 0,
+        BITS_4  = 1,
+        BITS_6  = 2,
+        BITS_32 = 3
+    };
+
+    enum {
+        BYTES_1  = 0,
+        BYTES_2  = 1,
+        BYTES_3  = 2,
+        BYTES_4  = 3
+    };
+
+    int x;
+    int selector = BITS_2, selector2;
+
+    /*
+     * Find out how many bits the largest value requires to encode, and use it to choose one of the packing schemes
+     * below:
+     *
+     * Selector possibilities
+     *
+     * 2 bits per field  ss11 2233,
+     * 4 bits per field  ss00 1111 2222 3333
+     * 6 bits per field  ss11 1111 0022 2222 0033 3333
+     * 32 bits per field sstt tttt followed by fields of various byte counts
+     */
+    for (x = 0; x < NUM_FIELDS; x++) {
+        //Require more than 6 bits?
+        if (values[x] >= 32 || values[x] < -32) {
+            selector = BITS_32;
+            break;
+        }
+
+        //Require more than 4 bits?
+        if (values[x] >= 8 || values[x] < -8) {
+             if (selector < BITS_6) {
+                 selector = BITS_6;
+             }
+        } else if (values[x] >= 2 || values[x] < -2) { //Require more than 2 bits?
+            if (selector < BITS_4) {
+                selector = BITS_4;
+            }
+        }
+    }
+
+    switch (selector) {
+        case BITS_2:
+            blackboxWrite((selector << 6) | ((values[0] & 0x03) << 4) | ((values[1] & 0x03) << 2) | (values[2] & 0x03));
+        break;
+        case BITS_4:
+            blackboxWrite((selector << 6) | (values[0] & 0x0F));
+            blackboxWrite((values[1] << 4) | (values[2] & 0x0F));
+        break;
+        case BITS_6:
+            blackboxWrite((selector << 6) | (values[0] & 0x3F));
+            blackboxWrite((uint8_t)values[1]);
+            blackboxWrite((uint8_t)values[2]);
+        break;
+        case BITS_32:
+            /*
+             * Do another round to compute a selector for each field, assuming that they are at least 8 bits each
+             *
+             * Selector2 field possibilities
+             * 0 - 8 bits
+             * 1 - 16 bits
+             * 2 - 24 bits
+             * 3 - 32 bits
+             */
+            selector2 = 0;
+
+            //Encode in reverse order so the first field is in the low bits:
+            for (x = NUM_FIELDS - 1; x >= 0; x--) {
+                selector2 <<= 2;
+
+                if (values[x] < 128 && values[x] >= -128) {
+                    selector2 |= BYTES_1;
+                } else if (values[x] < 32768 && values[x] >= -32768) {
+                    selector2 |= BYTES_2;
+                } else if (values[x] < 8388608 && values[x] >= -8388608) {
+                    selector2 |= BYTES_3;
+                } else {
+                    selector2 |= BYTES_4;
+                }
+            }
+
+            //Write the selectors
+            blackboxWrite((selector << 6) | selector2);
+
+            //And now the values according to the selectors we picked for them
+            for (x = 0; x < NUM_FIELDS; x++, selector2 >>= 2) {
+                switch (selector2 & 0x03) {
+                    case BYTES_1:
+                        blackboxWrite(values[x]);
+                    break;
+                    case BYTES_2:
+                        blackboxWrite(values[x]);
+                        blackboxWrite(values[x] >> 8);
+                    break;
+                    case BYTES_3:
+                        blackboxWrite(values[x]);
+                        blackboxWrite(values[x] >> 8);
+                        blackboxWrite(values[x] >> 16);
+                    break;
+                    case BYTES_4:
+                        blackboxWrite(values[x]);
+                        blackboxWrite(values[x] >> 8);
+                        blackboxWrite(values[x] >> 16);
+                        blackboxWrite(values[x] >> 24);
+                    break;
+                }
+            }
+        break;
+    }
+}
+
+/**
+ * Write an 8-bit selector followed by four signed fields of size 0, 4, 8 or 16 bits.
+ */
+void blackboxWriteTag8_4S16(int32_t *values) {
+
+    //Need to be enums rather than const ints if we want to switch on them (due to being C)
+    enum {
+        FIELD_ZERO  = 0,
+        FIELD_4BIT  = 1,
+        FIELD_8BIT  = 2,
+        FIELD_16BIT = 3
+    };
+
+    uint8_t selector, buffer;
+    int nibbleIndex;
+    int x;
+
+    selector = 0;
+    //Encode in reverse order so the first field is in the low bits:
+    for (x = 3; x >= 0; x--) {
+        selector <<= 2;
+
+        if (values[x] == 0) {
+            selector |= FIELD_ZERO;
+        } else if (values[x] < 8 && values[x] >= -8) {
+            selector |= FIELD_4BIT;
+        } else if (values[x] < 128 && values[x] >= -128) {
+            selector |= FIELD_8BIT;
+        } else {
+            selector |= FIELD_16BIT;
+        }
+    }
+
+    blackboxWrite(selector);
+
+    nibbleIndex = 0;
+    buffer = 0;
+    for (x = 0; x < 4; x++, selector >>= 2) {
+        switch (selector & 0x03) {
+            case FIELD_ZERO:
+                //No-op
+            break;
+            case FIELD_4BIT:
+                if (nibbleIndex == 0) {
+                    //We fill high-bits first
+                    buffer = values[x] << 4;
+                    nibbleIndex = 1;
+                } else {
+                    blackboxWrite(buffer | (values[x] & 0x0F));
+                    nibbleIndex = 0;
+                }
+            break;
+            case FIELD_8BIT:
+                if (nibbleIndex == 0) {
+                    blackboxWrite(values[x]);
+                } else {
+                    //Write the high bits of the value first (mask to avoid sign extension)
+                    blackboxWrite(buffer | ((values[x] >> 4) & 0x0F));
+                    //Now put the leftover low bits into the top of the next buffer entry
+                    buffer = values[x] << 4;
+                }
+            break;
+            case FIELD_16BIT:
+                if (nibbleIndex == 0) {
+                    //Write high byte first
+                    blackboxWrite(values[x] >> 8);
+                    blackboxWrite(values[x]);
+                } else {
+                    //First write the highest 4 bits
+                    blackboxWrite(buffer | ((values[x] >> 12) & 0x0F));
+                    // Then the middle 8
+                    blackboxWrite(values[x] >> 4);
+                    //Only the smallest 4 bits are still left to write
+                    buffer = values[x] << 4;
+                }
+            break;
+        }
+    }
+    //Anything left over to write?
+    if (nibbleIndex == 1) {
+        blackboxWrite(buffer);
+    }
+}
+
+/**
+ * Write `valueCount` fields from `values` to the Blackbox using signed variable byte encoding. A 1-byte header is
+ * written first which specifies which fields are non-zero (so this encoding is compact when most fields are zero).
+ *
+ * valueCount must be 8 or less.
+ */
+void blackboxWriteTag8_8SVB(int32_t *values, int valueCount)
+{
+    uint8_t header;
+    int i;
+
+    if (valueCount > 0) {
+        //If we're only writing one field then we can skip the header
+        if (valueCount == 1) {
+            blackboxWriteSignedVB(values[0]);
+        } else {
+            //First write a one-byte header that marks which fields are non-zero
+            header = 0;
+
+            // First field should be in low bits of header
+            for (i = valueCount - 1; i >= 0; i--) {
+                header <<= 1;
+
+                if (values[i] != 0) {
+                    header |= 0x01;
+                }
+            }
+
+            blackboxWrite(header);
+
+            for (i = 0; i < valueCount; i++) {
+                if (values[i] != 0) {
+                    blackboxWriteSignedVB(values[i]);
+                }
+            }
+        }
+    }
+}
+
+/**
+ * If there is data waiting to be written to the blackbox device, attempt to write that now.
+ */
+void blackboxDeviceFlush(void)
+{
+    //Presently a no-op on serial
+}
+
+/**
+ * Attempt to open the logging device. Returns true if successful.
+ */
+bool blackboxDeviceOpen(void)
+{
+    blackboxPort = findOpenSerialPort(FUNCTION_BLACKBOX);
+    if (blackboxPort) {
+        previousPortMode = blackboxPort->mode;
+        previousBaudRate = blackboxPort->baudRate;
+
+        serialSetBaudRate(blackboxPort, BLACKBOX_BAUDRATE);
+        serialSetMode(blackboxPort, BLACKBOX_INITIAL_PORT_MODE);
+        beginSerialPortFunction(blackboxPort, FUNCTION_BLACKBOX);
+    } else {
+        blackboxPort = openSerialPort(FUNCTION_BLACKBOX, NULL, BLACKBOX_BAUDRATE, BLACKBOX_INITIAL_PORT_MODE, SERIAL_NOT_INVERTED);
+
+        if (blackboxPort) {
+            previousPortMode = blackboxPort->mode;
+            previousBaudRate = blackboxPort->baudRate;
+        }
+    }
+
+    /*
+     * We want to write at about 7200 bytes per second to give the OpenLog a good chance to save to disk. If
+     * about looptime microseconds elapse between our writes, this is the budget of how many bytes we should
+     * transmit with each write.
+     *
+     * 9 / 1250 = 7200 / 1000000
+     */
+    blackboxWriteChunkSize = MAX((masterConfig.looptime * 9) / 1250, 4);
+
+    return blackboxPort != NULL;
+}
+
+void blackboxDeviceClose(void)
+{
+    serialSetMode(blackboxPort, previousPortMode);
+    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);
+    }
+}
+
+bool isBlackboxDeviceIdle(void)
+{
+    return isSerialTransmitBufferEmpty(blackboxPort);
+}

src/main/blackbox/blackbox_io.h

@@ -0,0 +1,41 @@
+/*
+ * 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 <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include <stdint.h>
+#include <stdbool.h>
+
+uint8_t blackboxWriteChunkSize;
+
+void blackboxWrite(uint8_t value);
+
+void blackboxPrintf(char *fmt, ...);
+int blackboxPrint(const char *s);
+
+void blackboxWriteUnsignedVB(uint32_t value);
+void blackboxWriteSignedVB(int32_t value);
+void blackboxWriteS16(int16_t value);
+void blackboxWriteTag2_3S32(int32_t *values);
+void blackboxWriteTag8_4S16(int32_t *values);
+void blackboxWriteTag8_8SVB(int32_t *values, int valueCount);
+
+void blackboxDeviceFlush(void);
+bool blackboxDeviceOpen(void);
+void blackboxDeviceClose(void);
+
+bool isBlackboxDeviceIdle(void);

src/main/common/maths.c

@@ -22,7 +22,7 @@
 
 int32_t applyDeadband(int32_t value, int32_t deadband)
 {
-    if (abs(value) < deadband) {
+    if (ABS(value) < deadband) {
         value = 0;
     } else if (value > 0) {
         value -= deadband;
@@ -92,3 +92,52 @@ int scaleRange(int x, int srcMin, int srcMax, int destMin, int destMax) {
     return ((a / b) - (destMax - destMin)) + destMax;
 }
 
+// Normalize a vector
+void normalizeV(struct fp_vector *src, struct fp_vector *dest)
+{
+    float length;
+
+    length = sqrtf(src->X * src->X + src->Y * src->Y + src->Z * src->Z);
+    if (length != 0) {
+        dest->X = src->X / length;
+        dest->Y = src->Y / length;
+        dest->Z = src->Z / length;
+    }
+}
+
+// Rotate a vector *v by the euler angles defined by the 3-vector *delta.
+void rotateV(struct fp_vector *v, fp_angles_t *delta)
+{
+    struct fp_vector v_tmp = *v;
+
+    float mat[3][3];
+    float cosx, sinx, cosy, siny, cosz, sinz;
+    float coszcosx, sinzcosx, coszsinx, sinzsinx;
+
+    cosx = cosf(delta->angles.roll);
+    sinx = sinf(delta->angles.roll);
+    cosy = cosf(delta->angles.pitch);
+    siny = sinf(delta->angles.pitch);
+    cosz = cosf(delta->angles.yaw);
+    sinz = sinf(delta->angles.yaw);
+
+    coszcosx = cosz * cosx;
+    sinzcosx = sinz * cosx;
+    coszsinx = sinx * cosz;
+    sinzsinx = sinx * sinz;
+
+    mat[0][0] = cosz * cosy;
+    mat[0][1] = -cosy * sinz;
+    mat[0][2] = siny;
+    mat[1][0] = sinzcosx + (coszsinx * siny);
+    mat[1][1] = coszcosx - (sinzsinx * siny);
+    mat[1][2] = -sinx * cosy;
+    mat[2][0] = (sinzsinx) - (coszcosx * siny);
+    mat[2][1] = (coszsinx) + (sinzcosx * siny);
+    mat[2][2] = cosy * cosx;
+
+    v->X = v_tmp.X * mat[0][0] + v_tmp.Y * mat[1][0] + v_tmp.Z * mat[2][0];
+    v->Y = v_tmp.X * mat[0][1] + v_tmp.Y * mat[1][1] + v_tmp.Z * mat[2][1];
+    v->Z = v_tmp.X * mat[0][2] + v_tmp.Y * mat[1][2] + v_tmp.Z * mat[2][2];
+}
+

src/main/common/maths.h

@@ -21,20 +21,14 @@
 #define sq(x) ((x)*(x))
 #endif
 
-#ifdef M_PI
+// Use floating point M_PI instead explicitly.
-// M_PI should be float, but previous definition may be double
+#define M_PIf       3.14159265358979323846f
-# undef M_PI
-#endif
-#define M_PI       3.14159265358979323846f
 
-#define RADX10 (M_PI / 1800.0f)                  // 0.001745329252f
+#define RAD    (M_PIf / 180.0f)
-#define RAD    (M_PI / 180.0f)
 
-#define DEG2RAD(degrees) (degrees * RAD)
+#define MIN(a, b) ((a) < (b) ? (a) : (b))
-
+#define MAX(a, b) ((a) > (b) ? (a) : (b))
-#define min(a, b) ((a) < (b) ? (a) : (b))
+#define ABS(x) ((x) > 0 ? (x) : -(x))
-#define max(a, b) ((a) > (b) ? (a) : (b))
-#define abs(x) ((x) > 0 ? (x) : -(x))
 
 typedef struct stdev_t
 {
@@ -42,6 +36,31 @@ typedef struct stdev_t
     int m_n;
 } stdev_t;
 
+// Floating point 3 vector.
+typedef struct fp_vector {
+    float X;
+    float Y;
+    float Z;
+} t_fp_vector_def;
+
+typedef union {
+    float A[3];
+    t_fp_vector_def V;
+} t_fp_vector;
+
+// Floating point Euler angles.
+// Be carefull, could be either of degrees or radians.
+typedef struct fp_angles {
+    float roll;
+    float pitch;
+    float yaw;
+} fp_angles_def;
+
+typedef union {
+    float raw[3];
+    fp_angles_def angles;
+} fp_angles_t;
+
 int32_t applyDeadband(int32_t value, int32_t deadband);
 
 int constrain(int amt, int low, int high);
@@ -54,3 +73,7 @@ float devStandardDeviation(stdev_t *dev);
 float degreesToRadians(int16_t degrees);
 
 int scaleRange(int x, int srcMin, int srcMax, int destMin, int destMax);
+
+void normalizeV(struct fp_vector *src, struct fp_vector *dest);
+
+void rotateV(struct fp_vector *v, fp_angles_t *delta);

src/main/common/printf.c

@@ -69,7 +69,7 @@ static void putchw(void *putp, putcf putf, int n, char z, char *bf)
         putf(putp, ch);
 }
 
-void tfp_format(void *putp, putcf putf, char *fmt, va_list va)
+void tfp_format(void *putp, putcf putf, const char *fmt, va_list va)
 {
     char bf[12];
 
@@ -154,7 +154,7 @@ void init_printf(void *putp, void (*putf) (void *, char))
     stdout_putp = putp;
 }
 
-void tfp_printf(char *fmt, ...)
+void tfp_printf(const char *fmt, ...)
 {
     va_list va;
     va_start(va, fmt);
@@ -168,7 +168,7 @@ static void putcp(void *p, char c)
     *(*((char **) p))++ = c;
 }
 
-void tfp_sprintf(char *s, char *fmt, ...)
+void tfp_sprintf(char *s, const char *fmt, ...)
 {
     va_list va;
 

src/main/common/printf.h

@@ -107,10 +107,10 @@ regs Kusti, 23.10.2004
 
 void init_printf(void *putp, void (*putf) (void *, char));
 
-void tfp_printf(char *fmt, ...);
+void tfp_printf(const char *fmt, ...);
-void tfp_sprintf(char *s, char *fmt, ...);
+void tfp_sprintf(char *s, const char *fmt, ...);
 
-void tfp_format(void *putp, void (*putf) (void *, char), char *fmt, va_list va);
+void tfp_format(void *putp, void (*putf) (void *, char), const char *fmt, va_list va);
 
 #define printf tfp_printf
 #define sprintf tfp_sprintf

src/main/common/typeconversion.c

@@ -90,9 +90,9 @@ int a2d(char ch)
         return -1;
 }
 
-char a2i(char ch, char **src, int base, int *nump)
+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) {
@@ -159,7 +159,7 @@ char *ftoa(float x, char *floatString)
 
     value = (int32_t)(x * 1000.0f);    // Convert float * 1000 to an integer
 
-    itoa(abs(value), intString1, 10);   // Create string from abs of integer value
+    itoa(ABS(value), intString1, 10);   // Create string from abs of integer value
 
     if (value >= 0)
         intString2[0] = ' ';    // Positive number, add a pad space

src/main/common/typeconversion.h

@@ -20,7 +20,7 @@ void uli2a(unsigned long int num, unsigned int base, int uc, char *bf);
 void li2a(long num, char *bf);
 void ui2a(unsigned int num, unsigned int base, int uc, char *bf);
 void i2a(int num, char *bf);
-char a2i(char ch, char **src, int base, int *nump);
+char a2i(char ch, const char **src, int base, int *nump);
 char *ftoa(float x, char *floatString);
 float fastA2F(const char *p);
 

src/main/config/config.c

@@ -25,6 +25,8 @@
 
 #include "common/color.h"
 #include "common/axis.h"
+#include "common/maths.h"
+
 #include "flight/flight.h"
 
 #include "drivers/sensor.h"
@@ -108,7 +110,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 = 91;
 
 static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims)
 {
@@ -119,6 +121,8 @@ static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims)
 
 static void resetPidProfile(pidProfile_t *pidProfile)
 {
+    pidProfile->pidController = 0;
+
     pidProfile->P8[ROLL] = 40;
     pidProfile->I8[ROLL] = 30;
     pidProfile->D8[ROLL] = 23;
@@ -159,6 +163,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
@@ -194,6 +199,7 @@ void resetEscAndServoConfig(escAndServoConfig_t *escAndServoConfig)
     escAndServoConfig->minthrottle = 1150;
     escAndServoConfig->maxthrottle = 1850;
     escAndServoConfig->mincommand = 1000;
+    escAndServoConfig->servoCenterPulse = 1500;
 }
 
 void resetFlight3DConfig(flight3DConfig_t *flight3DConfig)
@@ -224,7 +230,7 @@ void resetBatteryConfig(batteryConfig_t *batteryConfig)
     batteryConfig->currentMeterOffset = 0;
     batteryConfig->currentMeterScale = 400; // for Allegro ACS758LCB-100U (40mV/A)
     batteryConfig->batteryCapacity = 0;
-
+    batteryConfig->currentMeterType = CURRENT_SENSOR_ADC;
 }
 
 void resetSerialConfig(serialConfig_t *serialConfig)
@@ -382,7 +388,6 @@ static void resetConf(void)
     masterConfig.looptime = 3500;
     masterConfig.emf_avoidance = 0;
 
-    currentProfile->pidController = 0;
     resetPidProfile(&currentProfile->pidProfile);
 
     resetControlRateConfig(&masterConfig.controlRateProfiles[0]);
@@ -452,20 +457,33 @@ static void resetConf(void)
     masterConfig.blackbox_rate_denom = 1;
 #endif
 
-    // alternative defaults AlienWii32 (activate via OPTIONS="ALIENWII32" during make for NAZE target)
+    // alternative defaults settings for ALIENWIIF1 and ALIENWIIF3 targets
 #ifdef ALIENWII32
     featureSet(FEATURE_RX_SERIAL);
     featureSet(FEATURE_MOTOR_STOP);
     featureSet(FEATURE_FAILSAFE);
+    featureClear(FEATURE_VBAT);
+#ifdef ALIENWIIF3
+    masterConfig.serialConfig.serial_port_scenario[2] = lookupScenarioIndex(SCENARIO_SERIAL_RX_ONLY);
+#else
     masterConfig.serialConfig.serial_port_scenario[1] = lookupScenarioIndex(SCENARIO_SERIAL_RX_ONLY);
+#endif
     masterConfig.rxConfig.serialrx_provider = 1;
     masterConfig.rxConfig.spektrum_sat_bind = 5;
     masterConfig.escAndServoConfig.minthrottle = 1000;
     masterConfig.escAndServoConfig.maxthrottle = 2000;
     masterConfig.motor_pwm_rate = 32000;
+    masterConfig.looptime = 2000;
+    currentProfile->pidProfile.pidController = 3;
+    currentProfile->pidProfile.P8[ROLL] = 36;
+    currentProfile->pidProfile.P8[PITCH] = 36;
+    currentProfile->failsafeConfig.failsafe_delay = 2;
+    currentProfile->failsafeConfig.failsafe_off_delay = 0;
+    currentProfile->failsafeConfig.failsafe_throttle = 1000;
     currentControlRateProfile->rcRate8 = 130;
     currentControlRateProfile->rollPitchRate = 20;
     currentControlRateProfile->yawRate = 60;
+    currentControlRateProfile->yawRate = 100;
     parseRcChannels("TAER1234", &masterConfig.rxConfig);
 
     //  { 1.0f, -0.5f,  1.0f, -1.0f },          // REAR_R
@@ -588,7 +606,7 @@ void activateConfig(void)
     useTelemetryConfig(&masterConfig.telemetryConfig);
 #endif
 
-    setPIDController(currentProfile->pidController);
+    setPIDController(currentProfile->pidProfile.pidController);
 
 #ifdef GPS
     gpsUseProfile(&currentProfile->gpsProfile);
@@ -614,10 +632,12 @@ void activateConfig(void)
     imuRuntimeConfig.acc_unarmedcal = currentProfile->acc_unarmedcal;;
     imuRuntimeConfig.small_angle = masterConfig.small_angle;
 
-    configureImu(
+    configureIMU(
         &imuRuntimeConfig,
         &currentProfile->pidProfile,
-        &currentProfile->accDeadband
+        &currentProfile->accDeadband,
+        currentProfile->accz_lpf_cutoff,
+        currentProfile->throttle_correction_angle
     );
 
     configureAltitudeHold(
@@ -627,9 +647,6 @@ void activateConfig(void)
         &masterConfig.escAndServoConfig
     );
 
-    calculateThrottleAngleScale(currentProfile->throttle_correction_angle);
-    calculateAccZLowPassFilterRCTimeConstant(currentProfile->accz_lpf_cutoff);
-
 #ifdef BARO
     useBarometerConfig(&currentProfile->barometerConfig);
 #endif
@@ -705,6 +722,12 @@ void validateAndFixConfig(void)
     }
 #endif
 
+#if defined(CC3D) && defined(DISPLAY) && defined(USE_USART3)
+    if (doesConfigurationUsePort(SERIAL_PORT_USART3) && feature(FEATURE_DISPLAY)) {
+        featureClear(FEATURE_DISPLAY);
+    }
+#endif
+
     useRxConfig(&masterConfig.rxConfig);
 
     serialConfig_t *serialConfig = &masterConfig.serialConfig;

src/main/config/config_master.h

@@ -85,8 +85,10 @@ typedef struct master_t {
     uint8_t current_profile_index;
     controlRateConfig_t controlRateProfiles[MAX_CONTROL_RATE_PROFILE_COUNT];
 
+#ifdef BLACKBOX
     uint8_t blackbox_rate_num;
     uint8_t blackbox_rate_denom;
+#endif
 
     uint8_t magic_ef;                       // magic number, should be 0xEF
     uint8_t chk;                            // XOR checksum

src/main/config/config_profile.h

@@ -18,8 +18,6 @@
 #pragma once
 
 typedef struct profile_s {
-    uint8_t pidController;                  // 0 = multiwii original, 1 = rewrite from http://www.multiwii.com/forum/viewtopic.php?f=8&t=3671, 1, 2 = Luggi09s new baseflight pid
-
     pidProfile_t pidProfile;
 
     uint8_t defaultRateProfileIndex;

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;
 

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) {
+#ifdef STM32F303
-        RCC_APB2PeriphClockCmd(mpu6050Config->gpioAPB2Peripherals, ENABLE);
+        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;

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;

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;
 }

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)

src/main/drivers/adc.c

@@ -18,11 +18,14 @@
 #include <stdbool.h>
 #include <stdint.h>
 
+#include "build_config.h"
+
 #include "platform.h"
 #include "system.h"
 
 #include "adc.h"
 
+#ifdef USE_ADC
 adc_config_t adcConfig[ADC_CHANNEL_COUNT];
 volatile uint16_t adcValues[ADC_CHANNEL_COUNT];
 
@@ -47,3 +50,10 @@ uint16_t adcGetChannel(uint8_t channel)
     return adcValues[adcConfig[channel].dmaIndex];
 }
 
+#else
+uint16_t adcGetChannel(uint8_t channel)
+{
+    UNUSED(channel);
+    return 0;
+}
+#endif

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
+#ifdef VBAT_ADC_GPIO_PIN
-    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0;
+    GPIO_InitStructure.GPIO_Pin = VBAT_ADC_GPIO_PIN;
-    adcConfig[ADC_BATTERY].adcChannel = ADC_Channel_0;
+    GPIO_Init(VBAT_ADC_GPIO, &GPIO_InitStructure);
-    adcConfig[ADC_BATTERY].dmaIndex = configuredAdcChannels++;
+    adcConfig[ADC_BATTERY].adcChannel = VBAT_ADC_CHANNEL;
-    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;
     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 = EXTERNAL1_ADC_GPIO_PIN;
-        GPIO_InitStructure.GPIO_Pin   |= GPIO_Pin_5;
+        GPIO_Init(EXTERNAL1_ADC_GPIO, &GPIO_InitStructure);
-        adcConfig[ADC_EXTERNAL1].adcChannel = ADC_Channel_5;
+        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;
+        GPIO_InitStructure.GPIO_Pin = RSSI_ADC_GPIO_PIN;
-        adcConfig[ADC_RSSI].adcChannel = ADC_Channel_1;
+        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_InitStructure.GPIO_Pin   = CURRENT_METER_ADC_GPIO_PIN;
-        GPIO_Init(GPIOB, &GPIO_InitStructure);
+        GPIO_Init(CURRENT_METER_ADC_GPIO, &GPIO_InitStructure);
-        adcConfig[ADC_CURRENT].adcChannel = ADC_Channel_9;
+        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);

src/main/drivers/compass_hmc5883l.c

@@ -142,10 +142,16 @@ void hmc5883lInit(void)
     gpio_config_t gpio;
 
     if (hmc5883Config) {
+#ifdef STM32F303
+        if (hmc5883Config->gpioAHBPeripherals) {
+            RCC_AHBPeriphClockCmd(hmc5883Config->gpioAHBPeripherals, ENABLE);
+        }
+#endif
+#ifdef STM32F10X
         if (hmc5883Config->gpioAPB2Peripherals) {
             RCC_APB2PeriphClockCmd(hmc5883Config->gpioAPB2Peripherals, ENABLE);
         }
-
+#endif
         gpio.pin = hmc5883Config->gpioPin;
         gpio.speed = Speed_2MHz;
         gpio.mode = Mode_IN_FLOATING;
@@ -171,7 +177,7 @@ void hmc5883lInit(void)
         xyz_total[Z] += magADC[Z];
 
         // Detect saturation.
-        if (-4096 >= min(magADC[X], min(magADC[Y], magADC[Z]))) {
+        if (-4096 >= MIN(magADC[X], MIN(magADC[Y], magADC[Z]))) {
             bret = false;
             break;              // Breaks out of the for loop.  No sense in continuing if we saturated.
         }
@@ -191,7 +197,7 @@ void hmc5883lInit(void)
         xyz_total[Z] -= magADC[Z];
 
         // Detect saturation.
-        if (-4096 >= min(magADC[X], min(magADC[Y], magADC[Z]))) {
+        if (-4096 >= MIN(magADC[X], MIN(magADC[Y], magADC[Z]))) {
             bret = false;
             break;              // Breaks out of the for loop.  No sense in continuing if we saturated.
         }

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;

src/main/drivers/light_ws2811strip_stm32f10x.c

@@ -35,13 +35,16 @@ void ws2811LedStripHardwareInit(void)
 
 #ifdef CC3D
     RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);
+    GPIO_StructInit(&GPIO_InitStructure);
     GPIO_InitStructure.GPIO_Pin = GPIO_Pin_4;
     GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
     GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
     GPIO_Init(GPIOB, &GPIO_InitStructure);
 #else
     RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE);
+
     /* GPIOA Configuration: TIM3 Channel 1 as alternate function push-pull */
+    GPIO_StructInit(&GPIO_InitStructure);
     GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6;
     GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
     GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
@@ -52,6 +55,7 @@ void ws2811LedStripHardwareInit(void)
     /* Compute the prescaler value */
     prescalerValue = (uint16_t) (SystemCoreClock / 24000000) - 1;
     /* Time base configuration */
+    TIM_TimeBaseStructInit(&TIM_TimeBaseStructure);
     TIM_TimeBaseStructure.TIM_Period = 29; // 800kHz
     TIM_TimeBaseStructure.TIM_Prescaler = prescalerValue;
     TIM_TimeBaseStructure.TIM_ClockDivision = 0;
@@ -59,6 +63,7 @@ void ws2811LedStripHardwareInit(void)
     TIM_TimeBaseInit(TIM3, &TIM_TimeBaseStructure);
 
     /* PWM1 Mode configuration: Channel1 */
+    TIM_OCStructInit(&TIM_OCInitStructure);
     TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
     TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
     TIM_OCInitStructure.TIM_Pulse = 0;
@@ -75,6 +80,7 @@ void ws2811LedStripHardwareInit(void)
     /* DMA1 Channel6 Config */
     DMA_DeInit(DMA1_Channel6);
 
+    DMA_StructInit(&DMA_InitStructure);
     DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)&TIM3->CCR1;
     DMA_InitStructure.DMA_MemoryBaseAddr = (uint32_t)ledStripDMABuffer;
     DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralDST;

src/main/drivers/light_ws2811strip_stm32f30x.c

@@ -26,10 +26,18 @@
 #include "common/color.h"
 #include "drivers/light_ws2811strip.h"
 
-#define WS2811_GPIO   GPIOB
+#ifndef WS2811_GPIO
-#define WS2811_PIN    Pin_8 // TIM16_CH1
+#define USE_LED_STRIP_ON_DMA1_CHANNEL3
-#define WS2811_PERIPHERAL RCC_AHBPeriph_GPIOB
+#define WS2811_GPIO                     GPIOB
-
+#define WS2811_GPIO_AHB_PERIPHERAL      RCC_AHBPeriph_GPIOB
+#define WS2811_GPIO_AF                  GPIO_AF_1
+#define WS2811_PIN                      GPIO_Pin_8 // TIM16_CH1
+#define WS2811_PIN_SOURCE               GPIO_PinSource8
+#define WS2811_TIMER                    TIM16
+#define WS2811_TIMER_APB2_PERIPHERAL    RCC_APB2Periph_TIM16
+#define WS2811_DMA_CHANNEL              DMA1_Channel3
+#define WS2811_IRQ                      DMA1_Channel3_IRQn
+#endif
 
 void ws2811LedStripHardwareInit(void)
 {
@@ -40,47 +48,53 @@ void ws2811LedStripHardwareInit(void)
 
     uint16_t prescalerValue;
 
-    RCC_AHBPeriphClockCmd(RCC_AHBPeriph_GPIOB, ENABLE);
+    RCC_AHBPeriphClockCmd(WS2811_GPIO_AHB_PERIPHERAL, ENABLE);
 
-    GPIO_PinAFConfig(GPIOB, GPIO_PinSource8,  GPIO_AF_1);
+    GPIO_PinAFConfig(WS2811_GPIO, WS2811_PIN_SOURCE,  WS2811_GPIO_AF);
 
-    /* GPIOA Configuration: TIM16 Channel 1 as alternate function push-pull */
+    /* Configuration alternate function push-pull */
+    GPIO_StructInit(&GPIO_InitStructure);
     GPIO_InitStructure.GPIO_Pin = WS2811_PIN;
     GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
+    GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
     GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;
     GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
     GPIO_Init(WS2811_GPIO, &GPIO_InitStructure);
 
 
-    RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM16, ENABLE);
+    RCC_APB2PeriphClockCmd(WS2811_TIMER_APB2_PERIPHERAL, ENABLE);
+
     /* Compute the prescaler value */
     prescalerValue = (uint16_t) (SystemCoreClock / 24000000) - 1;
     /* Time base configuration */
+    TIM_TimeBaseStructInit(&TIM_TimeBaseStructure);
     TIM_TimeBaseStructure.TIM_Period = 29; // 800kHz
     TIM_TimeBaseStructure.TIM_Prescaler = prescalerValue;
     TIM_TimeBaseStructure.TIM_ClockDivision = 0;
     TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
-    TIM_TimeBaseInit(TIM16, &TIM_TimeBaseStructure);
+    TIM_TimeBaseInit(WS2811_TIMER, &TIM_TimeBaseStructure);
 
-    /* PWM1 Mode configuration: Channel1 */
+    /* PWM1 Mode configuration */
+    TIM_OCStructInit(&TIM_OCInitStructure);
     TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
     TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
     TIM_OCInitStructure.TIM_Pulse = 0;
     TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
-    TIM_OC1Init(TIM16, &TIM_OCInitStructure);
+    TIM_OC1Init(WS2811_TIMER, &TIM_OCInitStructure);
-    TIM_OC1PreloadConfig(TIM3, TIM_OCPreload_Enable);
+    TIM_OC1PreloadConfig(WS2811_TIMER, TIM_OCPreload_Enable);
 
 
-    TIM_CtrlPWMOutputs(TIM16, ENABLE);
+    TIM_CtrlPWMOutputs(WS2811_TIMER, ENABLE);
 
     /* configure DMA */
     /* DMA clock enable */
     RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1, ENABLE);
 
     /* DMA1 Channel Config */
-    DMA_DeInit(DMA1_Channel3);
+    DMA_DeInit(WS2811_DMA_CHANNEL);
 
-    DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)&TIM16->CCR1;
+    DMA_StructInit(&DMA_InitStructure);
+    DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)&WS2811_TIMER->CCR1;
     DMA_InitStructure.DMA_MemoryBaseAddr = (uint32_t)ledStripDMABuffer;
     DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralDST;
     DMA_InitStructure.DMA_BufferSize = WS2811_DMA_BUFFER_SIZE;
@@ -92,16 +106,15 @@ void ws2811LedStripHardwareInit(void)
     DMA_InitStructure.DMA_Priority = DMA_Priority_High;
     DMA_InitStructure.DMA_M2M = DMA_M2M_Disable;
 
-    DMA_Init(DMA1_Channel3, &DMA_InitStructure);
+    DMA_Init(WS2811_DMA_CHANNEL, &DMA_InitStructure);
 
-    /* TIM16 CC1 DMA Request enable */
+    TIM_DMACmd(WS2811_TIMER, TIM_DMA_CC1, ENABLE);
-    TIM_DMACmd(TIM16, TIM_DMA_CC1, ENABLE);
 
-    DMA_ITConfig(DMA1_Channel3, DMA_IT_TC, ENABLE);
+    DMA_ITConfig(WS2811_DMA_CHANNEL, DMA_IT_TC, ENABLE);
 
     NVIC_InitTypeDef NVIC_InitStructure;
 
-    NVIC_InitStructure.NVIC_IRQChannel = DMA1_Channel3_IRQn;
+    NVIC_InitStructure.NVIC_IRQChannel = WS2811_IRQ;
     NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(NVIC_PRIO_WS2811_DMA);
     NVIC_InitStructure.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(NVIC_PRIO_WS2811_DMA);
     NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
@@ -111,21 +124,45 @@ void ws2811LedStripHardwareInit(void)
     ws2811UpdateStrip();
 }
 
+#ifdef USE_LED_STRIP_ON_DMA1_CHANNEL3
 void DMA1_Channel3_IRQHandler(void)
 {
     if (DMA_GetFlagStatus(DMA1_FLAG_TC3)) {
         ws2811LedDataTransferInProgress = 0;
-        DMA_Cmd(DMA1_Channel3, DISABLE);            // disable DMA channel 6
+        DMA_Cmd(DMA1_Channel3, DISABLE);            // disable DMA channel
-        DMA_ClearFlag(DMA1_FLAG_TC3);               // clear DMA1 Channel 6 transfer complete flag
+        DMA_ClearFlag(DMA1_FLAG_TC3);               // clear DMA1 Channel transfer complete flag
     }
 }
+#endif
+
+#ifdef USE_LED_STRIP_ON_DMA1_CHANNEL2
+void DMA1_Channel2_IRQHandler(void)
+{
+    if (DMA_GetFlagStatus(DMA1_FLAG_TC2)) {
+        ws2811LedDataTransferInProgress = 0;
+        DMA_Cmd(DMA1_Channel2, DISABLE);            // disable DMA channel
+        DMA_ClearFlag(DMA1_FLAG_TC2);               // clear DMA1 Channel transfer complete flag
+    }
+}
+#endif
+
+#ifdef USE_LED_STRIP_ON_DMA1_CHANNEL7
+void DMA1_Channel7_IRQHandler(void)
+{
+    if (DMA_GetFlagStatus(DMA1_FLAG_TC7)) {
+        ws2811LedDataTransferInProgress = 0;
+        DMA_Cmd(DMA1_Channel7, DISABLE);            // disable DMA channel
+        DMA_ClearFlag(DMA1_FLAG_TC7);               // clear DMA1 Channel transfer complete flag
+    }
+}
+#endif
 
 void ws2811LedStripDMAEnable(void)
 {
-    DMA_SetCurrDataCounter(DMA1_Channel3, WS2811_DMA_BUFFER_SIZE);  // load number of bytes to be transferred
+    DMA_SetCurrDataCounter(WS2811_DMA_CHANNEL, WS2811_DMA_BUFFER_SIZE);  // load number of bytes to be transferred
-    TIM_SetCounter(TIM16, 0);
+    TIM_SetCounter(WS2811_TIMER, 0);
-    TIM_Cmd(TIM16, ENABLE);
+    TIM_Cmd(WS2811_TIMER, ENABLE);
-    DMA_Cmd(DMA1_Channel3, ENABLE);
+    DMA_Cmd(WS2811_DMA_CHANNEL, ENABLE);
 }
 
 

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
@@ -235,7 +235,7 @@ static const uint16_t airPWM[] = {
 };
 #endif
 
-#ifdef SPARKY
+#if defined(SPARKY) || defined(ALIENWIIF3)
 static const uint16_t multiPPM[] = {
     PWM11 | (MAP_TO_PPM_INPUT << 8), // PPM input
 
@@ -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,
@@ -334,28 +383,31 @@ pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init)
 
 #ifdef LED_STRIP_TIMER
         // skip LED Strip output
-        if (init->useLEDStrip && timerHardwarePtr->tim == LED_STRIP_TIMER)
+        if (init->useLEDStrip) {
-            continue;
+            if (timerHardwarePtr->tim == LED_STRIP_TIMER)
+                continue;
+#if defined(STM32F303xC) && defined(WS2811_GPIO) && defined(WS2811_PIN_SOURCE)
+            if (timerHardwarePtr->gpio == WS2811_GPIO && timerHardwarePtr->gpioPinSource == WS2811_PIN_SOURCE)
+                continue;
+#endif
+        }
+
 #endif
 
-#ifdef STM32F10X
+#ifdef VBAT_ADC_GPIO
-        // skip ADC for RSSI
+        if (init->useVbat && timerHardwarePtr->gpio == VBAT_ADC_GPIO && timerHardwarePtr->pin == VBAT_ADC_GPIO_PIN) {
-        if (init->useRSSIADC && timerIndex == PWM2)
-            continue;
-#endif
-
-#ifdef CC3D
-        if (init->useVbat && timerIndex == Vbat_TIMER) {
             continue;
         }
 #endif
-#ifdef CC3D
+
-        if (init->useCurrentMeterADC && timerIndex == CurrentMeter_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->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
@@ -380,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) {

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
 };

src/main/drivers/pwm_rx.c

@@ -234,9 +234,7 @@ static void pwmOverflowCallback(timerOvrHandlerRec_t* cbRec, captureCompare_t ca
     pwmInputPort_t *pwmInputPort = container_of(cbRec, pwmInputPort_t, overflowCb);
 
     if (++pwmInputPort->missedEvents > MAX_MISSED_PWM_EVENTS) {
-        if (pwmInputPort->state == 0) {
+        captures[pwmInputPort->channel] = PPM_RCVR_TIMEOUT;
-            captures[pwmInputPort->channel] = PPM_RCVR_TIMEOUT;
-        }
         pwmInputPort->missedEvents = 0;
     }
 }

src/main/drivers/serial_uart_stm32f10x.c

@@ -47,13 +47,20 @@ static uartPort_t uartPort2;
 static uartPort_t uartPort3;
 #endif
 
+// Using RX DMA disables the use of receive callbacks
+#define USE_USART1_RX_DMA
+
+#if defined(CC3D) // FIXME move board specific code to target.h files.
+#undef USE_USART1_RX_DMA
+#endif
+
 void uartStartTxDMA(uartPort_t *s);
 
 void usartIrqCallback(uartPort_t *s)
 {
     uint16_t SR = s->USARTx->SR;
 
-    if (SR & USART_FLAG_RXNE) {
+    if (SR & USART_FLAG_RXNE && !s->rxDMAChannel) {
         // If we registered a callback, pass crap there
         if (s->port.callback) {
             s->port.callback(s->USARTx->DR);
@@ -98,11 +105,13 @@ uartPort_t *serialUSART1(uint32_t baudRate, portMode_t mode)
     
     s->USARTx = USART1;
 
-    s->txDMAPeripheralBaseAddr = (uint32_t)&s->USARTx->DR;
-    s->rxDMAPeripheralBaseAddr = (uint32_t)&s->USARTx->DR;
 
+#ifdef USE_USART1_RX_DMA
     s->rxDMAChannel = DMA1_Channel5;
+    s->rxDMAPeripheralBaseAddr = (uint32_t)&s->USARTx->DR;
+#endif
     s->txDMAChannel = DMA1_Channel4;
+    s->txDMAPeripheralBaseAddr = (uint32_t)&s->USARTx->DR;
 
     RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1, ENABLE);
     RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1, ENABLE);
@@ -129,6 +138,15 @@ uartPort_t *serialUSART1(uint32_t baudRate, portMode_t mode)
     NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
     NVIC_Init(&NVIC_InitStructure);
 
+#ifndef USE_USART1_RX_DMA
+    // RX/TX Interrupt
+    NVIC_InitStructure.NVIC_IRQChannel = USART1_IRQn;
+    NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(NVIC_PRIO_SERIALUART1);
+    NVIC_InitStructure.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(NVIC_PRIO_SERIALUART1);
+    NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
+    NVIC_Init(&NVIC_InitStructure);
+#endif
+
     return s;
 }
 
@@ -146,20 +164,11 @@ void DMA1_Channel4_IRQHandler(void)
         s->txDMAEmpty = true;
 }
 
-// USART1 Tx IRQ Handler
+// USART1 Rx/Tx IRQ Handler
 void USART1_IRQHandler(void)
 {
     uartPort_t *s = &uartPort1;
-    uint16_t SR = s->USARTx->SR;
+    usartIrqCallback(s);
-
-    if (SR & USART_FLAG_TXE) {
-        if (s->port.txBufferTail != s->port.txBufferHead) {
-            s->USARTx->DR = s->port.txBuffer[s->port.txBufferTail];
-            s->port.txBufferTail = (s->port.txBufferTail + 1) % s->port.txBufferSize;
-        } else {
-            USART_ITConfig(s->USARTx, USART_IT_TXE, DISABLE);
-        }
-    }
 }
 
 #endif

src/main/drivers/serial_uart_stm32f30x.c

@@ -326,6 +326,7 @@ void DMA1_Channel4_IRQHandler(void)
     handleUsartTxDma(s);
 }
 
+#ifdef USE_USART2_TX_DMA
 // USART2 Tx DMA Handler
 void DMA1_Channel7_IRQHandler(void)
 {
@@ -334,8 +335,10 @@ void DMA1_Channel7_IRQHandler(void)
     DMA_Cmd(DMA1_Channel7, DISABLE);
     handleUsartTxDma(s);
 }
+#endif
 
 // USART3 Tx DMA Handler
+#ifdef USE_USART2_TX_DMA
 void DMA1_Channel2_IRQHandler(void)
 {
     uartPort_t *s = &uartPort3;
@@ -343,6 +346,8 @@ void DMA1_Channel2_IRQHandler(void)
     DMA_Cmd(DMA1_Channel2, DISABLE);
     handleUsartTxDma(s);
 }
+#endif
+
 
 void usartIrqHandler(uartPort_t *s)
 {

src/main/drivers/serial_usb_vcp.c

@@ -17,17 +17,17 @@
 
 #include <stdint.h>
 #include <stdlib.h>
+#include <stdbool.h>
+#include <string.h>
 
 #include "platform.h"
 
+#include "build_config.h"
+
 #include "usb_core.h"
 #include "usb_init.h"
 #include "hw_config.h"
 
-#include <stdbool.h>
-#include <string.h>
-
-
 #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();
 

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,

src/main/drivers/timer.c

@@ -175,7 +175,7 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = {
 
 #endif
 
-#ifdef SPARKY
+#if defined(SPARKY) || defined(ALIENWIIF3)
 const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = {
     //
     // 6 x 3 pin headers
@@ -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
 

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

src/main/flight/altitudehold.c

@@ -53,7 +53,6 @@ uint8_t velocityControl = 0;
 int32_t errorVelocityI = 0;
 int32_t altHoldThrottleAdjustment = 0;
 int32_t AltHold;
-int32_t EstAlt;                // in cm
 int32_t vario = 0;                      // variometer in cm/s
 
 
@@ -78,7 +77,7 @@ void configureAltitudeHold(
 #if defined(BARO) || defined(SONAR)
 
 static int16_t initialThrottleHold;
-
+static int32_t EstAlt;                // in cm
 
 // 40hz update rate (20hz LPF on acc)
 #define BARO_UPDATE_FREQUENCY_40HZ (1000 * 25)
@@ -91,7 +90,7 @@ static void applyMultirotorAltHold(void)
     // multirotor alt hold
     if (rcControlsConfig->alt_hold_fast_change) {
         // rapid alt changes
-        if (abs(rcCommand[THROTTLE] - initialThrottleHold) > rcControlsConfig->alt_hold_deadband) {
+        if (ABS(rcCommand[THROTTLE] - initialThrottleHold) > rcControlsConfig->alt_hold_deadband) {
             errorVelocityI = 0;
             isAltHoldChanged = 1;
             rcCommand[THROTTLE] += (rcCommand[THROTTLE] > initialThrottleHold) ? -rcControlsConfig->alt_hold_deadband : rcControlsConfig->alt_hold_deadband;
@@ -104,7 +103,7 @@ static void applyMultirotorAltHold(void)
         }
     } else {
         // slow alt changes, mostly used for aerial photography
-        if (abs(rcCommand[THROTTLE] - initialThrottleHold) > rcControlsConfig->alt_hold_deadband) {
+        if (ABS(rcCommand[THROTTLE] - initialThrottleHold) > rcControlsConfig->alt_hold_deadband) {
             // set velocity proportional to stick movement +100 throttle gives ~ +50 cm/s
             setVelocity = (rcCommand[THROTTLE] - initialThrottleHold) / 2;
             velocityControl = 1;
@@ -172,15 +171,19 @@ void updateSonarAltHoldState(void)
 
 bool isThrustFacingDownwards(rollAndPitchInclination_t *inclination)
 {
-    return abs(inclination->values.rollDeciDegrees) < DEGREES_80_IN_DECIDEGREES && abs(inclination->values.pitchDeciDegrees) < DEGREES_80_IN_DECIDEGREES;
+    return ABS(inclination->values.rollDeciDegrees) < DEGREES_80_IN_DECIDEGREES && ABS(inclination->values.pitchDeciDegrees) < DEGREES_80_IN_DECIDEGREES;
 }
 
+/*
+* This (poorly named) function merely returns whichever is higher, roll inclination or pitch inclination.
+* //TODO: Fix this up. We could either actually return the angle between 'down' and the normal of the craft
+* (my best interpretation of scalar 'tiltAngle') or rename the function.
+*/
 int16_t calculateTiltAngle(rollAndPitchInclination_t *inclination)
 {
-    return max(abs(inclination->values.rollDeciDegrees), abs(inclination->values.pitchDeciDegrees));
+    return MAX(ABS(inclination->values.rollDeciDegrees), ABS(inclination->values.pitchDeciDegrees));
 }
 
-
 int32_t calculateAltHoldThrottleAdjustment(int32_t vel_tmp, float accZ_tmp, float accZ_old)
 {
     int32_t result = 0;
@@ -226,6 +229,7 @@ void calculateEstimatedAltitude(uint32_t currentTime)
     float vel_acc;
     int32_t vel_tmp;
     float accZ_tmp;
+    int32_t sonarAlt = -1;
     static float accZ_old = 0.0f;
     static float vel = 0.0f;
     static float accAlt = 0.0f;
@@ -238,8 +242,6 @@ void calculateEstimatedAltitude(uint32_t currentTime)
     int16_t tiltAngle;
 #endif
 
-
-
     dTime = currentTime - previousTime;
     if (dTime < BARO_UPDATE_FREQUENCY_40HZ)
         return;
@@ -325,5 +327,11 @@ void calculateEstimatedAltitude(uint32_t currentTime)
 
     accZ_old = accZ_tmp;
 }
+
+int32_t altitudeHoldGetEstimatedAltitude(void)
+{
+    return EstAlt;
+}
+
 #endif
 

src/main/flight/altitudehold.h

@@ -15,14 +15,16 @@
  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  */
 
-//extern int32_t errorVelocityI;
+#include "flight/flight.h"
-//extern int32_t altHoldThrottleAdjustment;
-//extern uint8_t velocityControl;
-//extern int32_t setVelocity;
 
+#include "io/escservo.h"
+#include "io/rc_controls.h"
+
+#include "sensors/barometer.h"
 
 void configureAltitudeHold(pidProfile_t *initialPidProfile, barometerConfig_t *intialBarometerConfig, rcControlsConfig_t *initialRcControlsConfig, escAndServoConfig_t *initialEscAndServoConfig);
 void applyAltHold(airplaneConfig_t *airplaneConfig);
 void updateAltHoldState(void);
 void updateSonarAltHoldState(void);
 
+int32_t altitudeHoldGetEstimatedAltitude(void);

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;
@@ -120,7 +129,7 @@ static autotunePhase_e nextPhase = FIRST_TUNE_PHASE;
 
 static float targetAngle = 0;
 static float targetAngleAtPeak;
-static float secondPeakAngle, firstPeakAngle; // deci dgrees, 180 deg = 1800
+static float firstPeakAngle, secondPeakAngle; // in degrees
 
 typedef struct fp_pid {
     float p;
@@ -140,10 +149,54 @@ 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;
+        eventData.rising = rising ? 1 : 0;
+
+        blackboxLogEvent(FLIGHT_LOG_EVENT_AUTOTUNE_CYCLE_START, (flightLogEventData_t*)&eventData);
+    }
+}
+
+static void autotuneLogAngleTargets(float currentAngle)
+{
+    if (feature(FEATURE_BLACKBOX)) {
+        flightLogEvent_autotuneTargets_t eventData;
+
+        // targetAngle is always just -AUTOTUNE_TARGET_ANGLE or +AUTOTUNE_TARGET_ANGLE so no need for float precision:
+        eventData.targetAngle = (int) targetAngle;
+        // and targetAngleAtPeak is set to targetAngle so it has the same small precision requirement:
+        eventData.targetAngleAtPeak = (int) targetAngleAtPeak;
+
+        // currentAngle is integer decidegrees divided by 10, so just reverse that process to get an integer again:
+        eventData.currentAngle = round(currentAngle * 10);
+        // the peak angles are only ever set to currentAngle, so they get the same treatment:
+        eventData.firstPeakAngle = round(firstPeakAngle * 10);
+        eventData.secondPeakAngle = round(secondPeakAngle * 10);
+
+        blackboxLogEvent(FLIGHT_LOG_EVENT_AUTOTUNE_TARGETS, (flightLogEventData_t*)&eventData);
+    }
+}
+
+#endif
+
 static void startNewCycle(void)
 {
     rising = !rising;
-    secondPeakAngle = firstPeakAngle = 0;
+    firstPeakAngle = secondPeakAngle = 0;
+
+#ifdef BLACKBOX
+    autotuneLogCycleStart();
+#endif
 }
 
 static void updatePidIndex(void)
@@ -155,8 +208,7 @@ static void updateTargetAngle(void)
 {
     if (rising) {
         targetAngle = AUTOTUNE_TARGET_ANGLE;
-    }
+    } else {
-    else  {
         targetAngle = -AUTOTUNE_TARGET_ANGLE;
     }
 
@@ -168,13 +220,14 @@ static void updateTargetAngle(void)
 float autotune(angle_index_t angleIndex, const rollAndPitchInclination_t *inclination, float errorAngle)
 {
     float currentAngle;
+    bool overshot;
 
     if (!(phase == PHASE_TUNE_ROLL || phase == PHASE_TUNE_PITCH) || autoTuneAngleIndex != angleIndex) {
         return errorAngle;
     }
 
-    if (pidController == 2) {
+    if (IS_PID_CONTROLLER_FP_BASED(pidController)) {
-        // TODO support new baseflight pid controller
+        // TODO support floating point based pid controllers
         return errorAngle;
     }
 
@@ -198,61 +251,85 @@ float autotune(angle_index_t angleIndex, const rollAndPitchInclination_t *inclin
     debug[2] = DEGREES_TO_DECIDEGREES(targetAngle);
 #endif
 
-    if (firstPeakAngle == 0) {
+#ifdef BLACKBOX
+    autotuneLogAngleTargets(currentAngle);
+#endif
+
+    if (secondPeakAngle == 0) {
         // The peak will be when our angular velocity is negative.  To be sure we are in the right place,
         // we also check to make sure our angle position is greater than zero.
 
-        if (currentAngle > secondPeakAngle) {
+        if (currentAngle > firstPeakAngle) {
             // we are still going up
-            secondPeakAngle = currentAngle;
+            firstPeakAngle = currentAngle;
             targetAngleAtPeak = targetAngle;
 
-            debug[3] = DEGREES_TO_DECIDEGREES(secondPeakAngle);
+            debug[3] = DEGREES_TO_DECIDEGREES(firstPeakAngle);
 
-        } else if (secondPeakAngle > 0) {
+        } else if (firstPeakAngle > 0) {
-            if (cycleCount == 0) {
+            switch (cycle) {
-                // when checking the I value, we would like to overshoot the target position by half of the max oscillation.
+                case CYCLE_TUNE_I:
-                if (currentAngle - targetAngle < AUTOTUNE_MAX_OSCILLATION_ANGLE / 2) {
+                    // when checking the I value, we would like to overshoot the target position by half of the max oscillation.
-                    pid.i *= AUTOTUNE_INCREASE_MULTIPLIER;
+                    overshot = currentAngle - targetAngle >= AUTOTUNE_MAX_OSCILLATION_ANGLE / 2;
-                } else {
+                    
-                    pid.i *= AUTOTUNE_DECREASE_MULTIPLIER;
+                    if (overshot) {
-                    if (pid.i < AUTOTUNE_MINIMUM_I_VALUE) {
+                        pid.i *= AUTOTUNE_DECREASE_MULTIPLIER;
-                        pid.i = AUTOTUNE_MINIMUM_I_VALUE;
+                        if (pid.i < AUTOTUNE_MINIMUM_I_VALUE) {
+                            pid.i = AUTOTUNE_MINIMUM_I_VALUE;
+                        }
+                    } else {
+                        pid.i *= AUTOTUNE_INCREASE_MULTIPLIER;
                     }
-                }
 
-                // go back to checking P and D
+#ifdef BLACKBOX
-                cycleCount = 1;
+                    if (feature(FEATURE_BLACKBOX)) {
-                pidProfile->I8[pidIndex] = 0;
+                        flightLogEvent_autotuneCycleResult_t eventData;
-                startNewCycle();
+
-            } else {
+                        eventData.flags = overshot ? FLIGHT_LOG_EVENT_AUTOTUNE_FLAG_OVERSHOT: 0;
-                // we are checking P and D values
+                        eventData.p = pidProfile->P8[pidIndex];
-                // set up to look for the 2nd peak
+                        eventData.i = pidProfile->I8[pidIndex];
-                firstPeakAngle = currentAngle;
+                        eventData.d = pidProfile->D8[pidIndex];
-                timeoutAt = millis() + AUTOTUNE_SETTLING_DELAY_MS;
+
+                        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
+                    secondPeakAngle = 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) {
+        if (currentAngle < secondPeakAngle) {
-            firstPeakAngle = currentAngle;
+            secondPeakAngle = currentAngle;
-            debug[3] = DEGREES_TO_DECIDEGREES(firstPeakAngle);
+            debug[3] = DEGREES_TO_DECIDEGREES(secondPeakAngle);
         }
 
-        float oscillationAmplitude = secondPeakAngle - firstPeakAngle;
+        float oscillationAmplitude = firstPeakAngle - secondPeakAngle;
 
         uint32_t now = millis();
         int32_t signedDiff = now - timeoutAt;
         bool timedOut = signedDiff >= 0L;
 
         // stop looking for the 2nd peak if we time out or if we change direction again after moving by more than half the maximum oscillation
-        if (timedOut || (oscillationAmplitude > AUTOTUNE_MAX_OSCILLATION_ANGLE / 2 && currentAngle > firstPeakAngle)) {
+        if (timedOut || (oscillationAmplitude > AUTOTUNE_MAX_OSCILLATION_ANGLE / 2 && currentAngle > secondPeakAngle)) {
             // analyze the data
             // Our goal is to have zero overshoot and to have AUTOTUNE_MAX_OSCILLATION_ANGLE amplitude
 
-            if (secondPeakAngle > targetAngleAtPeak) {
+            overshot = firstPeakAngle > targetAngleAtPeak;
-                // overshot
+            if (overshot) {
 #ifdef DEBUG_AUTOTUNE
                 debug[0] = 1;
 #endif
@@ -266,11 +343,10 @@ float autotune(angle_index_t angleIndex, const rollAndPitchInclination_t *inclin
                     pid.d *= AUTOTUNE_INCREASE_MULTIPLIER;
                 }
 #else
-				pid.p *= AUTOTUNE_DECREASE_MULTIPLIER;
+                pid.p *= AUTOTUNE_DECREASE_MULTIPLIER;
-				pid.d *= AUTOTUNE_INCREASE_MULTIPLIER;
+                pid.d *= AUTOTUNE_INCREASE_MULTIPLIER;
 #endif
             } else {
-                // undershot
 #ifdef DEBUG_AUTOTUNE
                 debug[0] = 2;
 #endif
@@ -286,15 +362,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
+#ifdef BLACKBOX
-            startNewCycle();
+            if (feature(FEATURE_BLACKBOX)) {
+                flightLogEvent_autotuneCycleResult_t eventData;
 
-            if (++cycleCount == 3) {
+                eventData.flags = (overshot ? FLIGHT_LOG_EVENT_AUTOTUNE_FLAG_OVERSHOT : 0) | (timedOut ? FLIGHT_LOG_EVENT_AUTOTUNE_FLAG_TIMEDOUT : 0);
-                // switch to testing I value
+                eventData.p = pidProfile->P8[pidIndex];
-                cycleCount = 0;
+                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();
         }
     }
 
@@ -324,7 +415,7 @@ void restorePids(pidProfile_t *pidProfileToTune)
     memcpy(pidProfileToTune, &pidBackup, sizeof(pidBackup));
 }
 
-void autotuneBeginNextPhase(pidProfile_t *pidProfileToTune, uint8_t pidControllerInUse)
+void autotuneBeginNextPhase(pidProfile_t *pidProfileToTune)
 {
     phase = nextPhase;
 
@@ -344,11 +435,11 @@ void autotuneBeginNextPhase(pidProfile_t *pidProfileToTune, uint8_t pidControlle
     }
 
     rising = true;
-    cycleCount = 1;
+    cycle = CYCLE_TUNE_PD;
-    secondPeakAngle = firstPeakAngle = 0;
+    firstPeakAngle = secondPeakAngle = 0;
 
     pidProfile = pidProfileToTune;
-    pidController = pidControllerInUse;
+    pidController = pidProfile->pidController;
 
     updatePidIndex();
     updateTargetAngle();
@@ -360,6 +451,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)

src/main/flight/autotune.h

@@ -18,7 +18,7 @@
 #pragma once
 
 void autotuneReset();
-void autotuneBeginNextPhase(pidProfile_t *pidProfileToTune, uint8_t pidControllerInUse);
+void autotuneBeginNextPhase(pidProfile_t *pidProfileToTune);
 float autotune(angle_index_t angleIndex, const rollAndPitchInclination_t *inclination, float errorAngle);
 void autotuneEndPhase();
 

src/main/flight/flight.c

@@ -42,6 +42,7 @@
 #include "io/gps.h"
 
 extern uint16_t cycleTime;
+extern uint8_t motorCount;
 
 int16_t heading, magHold;
 int16_t axisPID[3];
@@ -57,10 +58,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
 
@@ -96,19 +97,43 @@ bool shouldAutotune(void)
 }
 #endif
 
-static void pidBaseflight(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
+static void pidLuxFloat(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 +164,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;
                 }
             }
         }
@@ -176,13 +201,20 @@ static void pidBaseflight(pidProfile_t *pidProfile, controlRateConfig_t *control
 
         // -----calculate total PID output
         axisPID[axis] = constrain(lrintf(PTerm + ITerm - DTerm), -1000, 1000);
-    }
 
+#ifdef BLACKBOX
+        axisPID_P[axis] = PTerm;
+        axisPID_I[axis] = ITerm;
+        axisPID_D[axis] = -DTerm;
+#endif
+    }
 }
 
 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;
@@ -194,7 +226,7 @@ static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
     UNUSED(controlRateConfig);
 
     // **** PITCH & ROLL & YAW PID ****
-    prop = min(max(abs(rcCommand[PITCH]), abs(rcCommand[ROLL])), 500); // range [0;500]
+    prop = MIN(MAX(ABS(rcCommand[PITCH]), ABS(rcCommand[ROLL])), 500); // range [0;500]
 
     for (axis = 0; axis < 3; axis++) {
         if ((FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) && (axis == FD_ROLL || axis == FD_PITCH)) { // MODE relying on ACC
@@ -226,7 +258,7 @@ static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
             PTermGYRO = rcCommand[axis];
 
             errorGyroI[axis] = constrain(errorGyroI[axis] + error, -16000, +16000); // WindUp
-            if ((abs(gyroData[axis]) > (640 * 4)) || (axis == FD_YAW && abs(rcCommand[axis]) > 100))
+            if ((ABS(gyroData[axis]) > (640 * 4)) || (axis == FD_YAW && ABS(rcCommand[axis]) > 100))
                 errorGyroI[axis] = 0;
 
             ITermGYRO = (errorGyroI[axis] / 125 * pidProfile->I8[axis]) / 64;
@@ -261,11 +293,359 @@ static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
     }
 }
 
+#define GYRO_P_MAX 300
 #define GYRO_I_MAX 256
 
-static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination,
+static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination,
-        rollAndPitchTrims_t *angleTrim)
+            rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
 {
+    UNUSED(rxConfig);
+
+    int axis, prop = 0;
+    int32_t rc, error, errorAngle;
+    int32_t PTerm, ITerm, PTermACC, ITermACC, DTerm;
+    static int16_t lastGyro[2] = { 0, 0 };
+    static int32_t delta1[2] = { 0, 0 }, delta2[2] = { 0, 0 };
+    int32_t delta;
+
+    if (FLIGHT_MODE(HORIZON_MODE)) prop = MIN(MAX(ABS(rcCommand[PITCH]), ABS(rcCommand[ROLL])), 512);
+
+    // PITCH & ROLL
+    for (axis = 0; axis < 2; axis++) {
+        rc = rcCommand[axis] << 1;
+        error = rc - (gyroData[axis] / 4);
+        errorGyroI[axis]  = constrain(errorGyroI[axis] + error, -16000, +16000);   // WindUp   16 bits is ok here
+        if (ABS(gyroData[axis]) > (640 * 4)) errorGyroI[axis] = 0;
+
+        ITerm = (errorGyroI[axis] >> 7) * pidProfile->I8[axis] >> 6;   // 16 bits is ok here 16000/125 = 128 ; 128*250 = 32000
+
+        PTerm = (int32_t)rc * pidProfile->P8[axis] >> 6;
+
+        if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) {   // axis relying on ACC
+            // 50 degrees max inclination
+#ifdef GPS
+            errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination),
+                +max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis];
+#else
+            errorAngle = constrain(2 * rcCommand[axis], -((int) max_angle_inclination),
+                +max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis];
+#endif
+
+#ifdef AUTOTUNE
+            if (shouldAutotune()) {
+                errorAngle = DEGREES_TO_DECIDEGREES(autotune(rcAliasToAngleIndexMap[axis], &inclination, DECIDEGREES_TO_DEGREES(errorAngle)));
+            }
+#endif
+
+            errorAngleI[axis]  = constrain(errorAngleI[axis] + errorAngle, -10000, +10000);                                                // WindUp     //16 bits is ok here
+
+            PTermACC = ((int32_t)errorAngle * pidProfile->P8[PIDLEVEL]) >> 7;   // 32 bits is needed for calculation: errorAngle*P8 could exceed 32768   16 bits is ok for result
+
+            int16_t limit = pidProfile->D8[PIDLEVEL] * 5;
+            PTermACC = constrain(PTermACC, -limit, +limit);
+
+            ITermACC = ((int32_t)errorAngleI[axis] * pidProfile->I8[PIDLEVEL]) >> 12;  // 32 bits is needed for calculation:10000*I8 could exceed 32768   16 bits is ok for result
+
+            ITerm = ITermACC + ((ITerm - ITermACC) * prop >> 9);
+            PTerm = PTermACC + ((PTerm - PTermACC) * prop >> 9);
+        }
+
+        PTerm -= ((int32_t)(gyroData[axis] / 4) * dynP8[axis]) >> 6;   // 32 bits is needed for calculation
+
+        delta = (gyroData[axis] - lastGyro[axis]) / 4;   // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
+        lastGyro[axis] = gyroData[axis];
+        DTerm = delta1[axis] + delta2[axis] + delta;
+        delta2[axis] = delta1[axis];
+        delta1[axis] = delta;
+
+        DTerm = ((int32_t)DTerm * dynD8[axis]) >> 5;   // 32 bits is needed for calculation
+
+        axisPID[axis] = PTerm + ITerm - DTerm;
+
+#ifdef BLACKBOX
+        axisPID_P[axis] = PTerm;
+        axisPID_I[axis] = ITerm;
+        axisPID_D[axis] = -DTerm;
+#endif
+    }
+
+    //YAW
+    rc = (int32_t)rcCommand[FD_YAW] * (2 * controlRateConfig->yawRate + 30)  >> 5;
+#ifdef ALIENWII32
+    error = rc - gyroData[FD_YAW];
+#else
+    error = rc - (gyroData[FD_YAW] / 4);
+#endif
+    errorGyroI[FD_YAW]  += (int32_t)error * pidProfile->I8[FD_YAW];
+    errorGyroI[FD_YAW]  = constrain(errorGyroI[FD_YAW], 2 - ((int32_t)1 << 28), -2 + ((int32_t)1 << 28));
+    if (ABS(rc) > 50) errorGyroI[FD_YAW] = 0;
+
+    PTerm = (int32_t)error * pidProfile->P8[FD_YAW] >> 6;
+
+    // Constrain YAW by D value if not servo driven in that case servolimits apply
+    if(motorCount > 3) {
+        int16_t limit = GYRO_P_MAX - pidProfile->D8[FD_YAW];
+        PTerm = constrain(PTerm, -limit, +limit);
+    }
+
+    ITerm = constrain((int16_t)(errorGyroI[FD_YAW] >> 13), -GYRO_I_MAX, +GYRO_I_MAX);
+
+    axisPID[FD_YAW] =  PTerm + ITerm;
+
+#ifdef BLACKBOX
+    axisPID_P[FD_YAW] = PTerm;
+    axisPID_I[FD_YAW] = ITerm;
+    axisPID_D[FD_YAW] = 0;
+#endif
+}
+
+static void pidMultiWiiHybrid(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
+        uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
+{
+    UNUSED(rxConfig);
+
+    int axis, prop;
+    int32_t rc, error, errorAngle;
+    int32_t PTerm, ITerm, PTermACC = 0, ITermACC = 0, PTermGYRO = 0, ITermGYRO = 0, DTerm;
+    static int16_t lastGyro[2] = { 0, 0 };
+    static int32_t delta1[2] = { 0, 0 }, delta2[2] = { 0, 0 };
+    int32_t deltaSum;
+    int32_t delta;
+
+    UNUSED(controlRateConfig);
+
+    // **** PITCH & ROLL ****
+    prop = MIN(MAX(ABS(rcCommand[PITCH]), ABS(rcCommand[ROLL])), 500); // range [0;500]
+
+    for (axis = 0; axis < 2; axis++) {
+        if ((FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) { // MODE relying on ACC
+            // observe max inclination
+#ifdef GPS
+            errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination),
+                    +max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis];
+#else
+            errorAngle = constrain(2 * rcCommand[axis], -((int) max_angle_inclination),
+                    +max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis];
+#endif
+
+#ifdef AUTOTUNE
+            if (shouldAutotune()) {
+                errorAngle = DEGREES_TO_DECIDEGREES(autotune(rcAliasToAngleIndexMap[axis], &inclination, DECIDEGREES_TO_DEGREES(errorAngle)));
+            }
+#endif
+
+            PTermACC = errorAngle * pidProfile->P8[PIDLEVEL] / 100; // 32 bits is needed for calculation: errorAngle*P8[PIDLEVEL] could exceed 32768   16 bits is ok for result
+            PTermACC = constrain(PTermACC, -pidProfile->D8[PIDLEVEL] * 5, +pidProfile->D8[PIDLEVEL] * 5);
+
+            errorAngleI[axis] = constrain(errorAngleI[axis] + errorAngle, -10000, +10000); // WindUp
+            ITermACC = (errorAngleI[axis] * pidProfile->I8[PIDLEVEL]) >> 12;
+        }
+        if (!FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) { // MODE relying on GYRO
+            error = (int32_t) rcCommand[axis] * 10 * 8 / pidProfile->P8[axis];
+            error -= gyroData[axis] / 4;
+
+            PTermGYRO = rcCommand[axis];
+
+            errorGyroI[axis] = constrain(errorGyroI[axis] + error, -16000, +16000); // WindUp
+            if (ABS(gyroData[axis]) > (640 * 4))
+                errorGyroI[axis] = 0;
+
+            ITermGYRO = (errorGyroI[axis] / 125 * pidProfile->I8[axis]) / 64;
+        }
+        if (FLIGHT_MODE(HORIZON_MODE)) {
+            PTerm = (PTermACC * (500 - prop) + PTermGYRO * prop) / 500;
+            ITerm = (ITermACC * (500 - prop) + ITermGYRO * prop) / 500;
+        } else {
+            if (FLIGHT_MODE(ANGLE_MODE)) {
+                PTerm = PTermACC;
+                ITerm = ITermACC;
+            } else {
+                PTerm = PTermGYRO;
+                ITerm = ITermGYRO;
+            }
+        }
+
+        PTerm -= ((int32_t)gyroData[axis] / 4) * dynP8[axis] / 10 / 8; // 32 bits is needed for calculation
+        delta = (gyroData[axis] - lastGyro[axis]) / 4;
+        lastGyro[axis] = gyroData[axis];
+        deltaSum = delta1[axis] + delta2[axis] + delta;
+        delta2[axis] = delta1[axis];
+        delta1[axis] = delta;
+        DTerm = (deltaSum * dynD8[axis]) / 32;
+        axisPID[axis] = PTerm + ITerm - DTerm;
+
+#ifdef BLACKBOX
+        axisPID_P[axis] = PTerm;
+        axisPID_I[axis] = ITerm;
+        axisPID_D[axis] = -DTerm;
+#endif
+    }
+    //YAW
+    rc = (int32_t)rcCommand[FD_YAW] * (2 * controlRateConfig->yawRate + 30)  >> 5;
+#ifdef ALIENWII32
+    error = rc - gyroData[FD_YAW];
+#else
+    error = rc - (gyroData[FD_YAW] / 4);
+#endif
+    errorGyroI[FD_YAW]  += (int32_t)error * pidProfile->I8[FD_YAW];
+    errorGyroI[FD_YAW]  = constrain(errorGyroI[FD_YAW], 2 - ((int32_t)1 << 28), -2 + ((int32_t)1 << 28));
+    if (ABS(rc) > 50) errorGyroI[FD_YAW] = 0;
+
+    PTerm = (int32_t)error * pidProfile->P8[FD_YAW] >> 6;
+
+    // Constrain YAW by D value if not servo driven in that case servolimits apply
+    if(motorCount > 3) {
+        int16_t limit = GYRO_P_MAX - pidProfile->D8[FD_YAW];
+        PTerm = constrain(PTerm, -limit, +limit);
+    }
+
+    ITerm = constrain((int16_t)(errorGyroI[FD_YAW] >> 13), -GYRO_I_MAX, +GYRO_I_MAX);
+
+    axisPID[FD_YAW] =  PTerm + ITerm;
+
+
+#ifdef BLACKBOX
+    axisPID_P[FD_YAW] = PTerm;
+    axisPID_I[FD_YAW] = ITerm;
+    axisPID_D[FD_YAW] = 0;
+#endif
+}
+
+#define RCconstPI   0.159154943092f // 0.5f / M_PI;
+#define MAIN_CUT_HZ 12.0f // (default 12Hz, Range 1-50Hz)
+#define OLD_YAW	0 // [0/1] 0 = multiwii 2.3 yaw, 1 = older yaw.
+
+static void pidHarakiri(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination,
+rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
+{
+    UNUSED(rxConfig);
+
+    float delta, RCfactor, rcCommandAxis, MainDptCut;
+    float PTerm = 0, ITerm = 0, DTerm = 0, PTermACC = 0, ITermACC = 0, ITermGYRO = 0, error = 0, prop = 0;
+    static float lastGyro[2] = {0, 0}, lastDTerm[2] = {0, 0};
+    float tmp0flt;
+    int32_t tmp0;
+    uint8_t axis;
+    float ACCDeltaTimeINS = 0;
+    float FLOATcycleTime = 0;
+
+//    MainDptCut = RCconstPI / (float)cfg.maincuthz;                           // Initialize Cut off frequencies for mainpid D
+    MainDptCut = RCconstPI / MAIN_CUT_HZ;                                      // maincuthz (default 12Hz, Range 1-50Hz), hardcoded for now
+    FLOATcycleTime  = (float)constrain(cycleTime, 1, 100000);                  // 1us - 100ms
+    ACCDeltaTimeINS = FLOATcycleTime * 0.000001f;                              // ACCDeltaTimeINS is in seconds now
+    RCfactor = ACCDeltaTimeINS / (MainDptCut + ACCDeltaTimeINS);               // used for pt1 element
+
+    if (FLIGHT_MODE(HORIZON_MODE)) {
+        prop = (float)MIN(MAX(ABS(rcCommand[PITCH]), ABS(rcCommand[ROLL])), 450) / 450.0f;
+    }
+
+    for (axis = 0; axis < 2; axis++) {
+        rcCommandAxis = (float)rcCommand[axis];                                // Calculate common values for pid controllers
+        if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) {
+#ifdef GPS
+            error = constrain(2.0f * rcCommandAxis + GPS_angle[axis], -((int) max_angle_inclination), +max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis];
+#else
+            error = constrain(2.0f * rcCommandAxis, -((int) max_angle_inclination), +max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis];
+#endif
+
+#ifdef AUTOTUNE
+            if (shouldAutotune()) {
+                error = DEGREES_TO_DECIDEGREES(autotune(rcAliasToAngleIndexMap[axis], &inclination, DECIDEGREES_TO_DEGREES(error)));
+            }
+#endif
+            PTermACC = error * (float)pidProfile->P8[PIDLEVEL] * 0.008f;
+            tmp0flt = (float)pidProfile->D8[PIDLEVEL] * 5.0f;
+            PTermACC = constrain(PTermACC, -tmp0flt, +tmp0flt);
+            errorAngleI[axis] = constrain(errorAngleI[axis] + error * ACCDeltaTimeINS, -30.0f, +30.0f);
+            ITermACC = errorAngleI[axis] * (float)pidProfile->I8[PIDLEVEL] * 0.08f;
+        }
+
+        if (!FLIGHT_MODE(ANGLE_MODE)) {
+            if (ABS((int16_t)gyroData[axis]) > 2560) {
+                errorGyroI[axis] = 0.0f;
+            } else {
+                error = (rcCommandAxis * 320.0f / (float)pidProfile->P8[axis]) - gyroData[axis];
+                errorGyroI[axis] = constrain(errorGyroI[axis] + error * ACCDeltaTimeINS, -192.0f, +192.0f);
+            }
+
+            ITermGYRO = errorGyroI[axis] * (float)pidProfile->I8[axis] * 0.01f;
+
+            if (FLIGHT_MODE(HORIZON_MODE)) {
+                PTerm = PTermACC + prop * (rcCommandAxis - PTermACC);
+                ITerm = ITermACC + prop * (ITermGYRO - ITermACC);
+            } else {
+                PTerm = rcCommandAxis;
+                ITerm = ITermGYRO;
+            }
+        } else {
+            PTerm = PTermACC;
+            ITerm = ITermACC;
+        }
+
+        PTerm -= gyroData[axis] * dynP8[axis] * 0.003f;
+        delta = (gyroData[axis] - lastGyro[axis]) / ACCDeltaTimeINS;
+
+        lastGyro[axis] = gyroData[axis];
+        lastDTerm[axis] += RCfactor * (delta - lastDTerm[axis]);
+        DTerm = lastDTerm[axis] * dynD8[axis] * 0.00007f;
+
+        axisPID[axis] = lrintf(PTerm + ITerm - DTerm);                         // Round up result.
+
+#ifdef BLACKBOX
+        axisPID_P[axis] = PTerm;
+        axisPID_I[axis] = ITerm;
+        axisPID_D[axis] = -DTerm;
+#endif
+    }
+
+    tmp0flt = (int32_t)FLOATcycleTime & (int32_t)~3;                          // Filter last 2 bit jitter
+    tmp0flt /= 3000.0f;
+
+    if (OLD_YAW) { // [0/1] 0 = multiwii 2.3 yaw, 1 = older yaw. hardcoded for now
+        PTerm = ((int32_t)pidProfile->P8[FD_YAW] * (100 - (int32_t)controlRateConfig->yawRate * (int32_t)ABS(rcCommand[FD_YAW]) / 500)) / 100;
+        tmp0 = lrintf(gyroData[FD_YAW] * 0.25f);
+        PTerm = rcCommand[FD_YAW] - tmp0 * PTerm / 80;
+        if ((ABS(tmp0) > 640) || (ABS(rcCommand[FD_YAW]) > 100)) {
+            errorGyroI[FD_YAW] = 0;
+        } else {
+            error = ((int32_t)rcCommand[FD_YAW] * 80 / (int32_t)pidProfile->P8[FD_YAW]) - tmp0;
+            errorGyroI[FD_YAW] = constrain(errorGyroI[FD_YAW] + (int32_t)(error * tmp0flt), -16000, +16000); // WindUp
+            ITerm = (errorGyroI[FD_YAW] / 125 * pidProfile->I8[FD_YAW]) >> 6;
+        }
+    } else {
+        tmp0 = ((int32_t)rcCommand[FD_YAW] * (((int32_t)controlRateConfig->yawRate << 1) + 40)) >> 5;
+        error = tmp0 - lrintf(gyroData[FD_YAW] * 0.25f);                       // Less Gyrojitter works actually better
+
+        if (ABS(tmp0) > 50) {
+            errorGyroI[FD_YAW] = 0;
+        } else {
+            errorGyroI[FD_YAW] = constrain(errorGyroI[FD_YAW] + (int32_t)(error * (float)pidProfile->I8[FD_YAW] * tmp0flt), -268435454, +268435454);
+        }
+
+        ITerm = constrain(errorGyroI[FD_YAW] >> 13, -GYRO_I_MAX, +GYRO_I_MAX);
+        PTerm = ((int32_t)error * (int32_t)pidProfile->P8[FD_YAW]) >> 6;
+
+        if (motorCount >= 4) { // Constrain FD_YAW by D value if not servo driven in that case servolimits apply
+            tmp0 = 300;
+            if (pidProfile->D8[FD_YAW]) tmp0 -= (int32_t)pidProfile->D8[FD_YAW];
+            PTerm = constrain(PTerm, -tmp0, tmp0);
+        }
+    }
+    axisPID[FD_YAW] = PTerm + ITerm;
+    axisPID[FD_YAW] = lrintf(axisPID[FD_YAW]);                                 // Round up result.
+
+#ifdef BLACKBOX
+    axisPID_P[FD_YAW] = PTerm;
+    axisPID_I[FD_YAW] = ITerm;
+    axisPID_D[FD_YAW] = 0;
+#endif
+}
+
+static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination,
+        rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
+{
+    UNUSED(rxConfig);
+
     int32_t errorAngle;
     int axis;
     int32_t delta, deltaSum;
@@ -361,7 +741,16 @@ void setPIDController(int type)
             pid_controller = pidRewrite;
             break;
         case 2:
-            pid_controller = pidBaseflight;
+            pid_controller = pidLuxFloat;
+            break;
+        case 3:
+            pid_controller = pidMultiWii23;
+            break;
+        case 4:
+            pid_controller = pidMultiWiiHybrid;
+            break;
+        case 5:
+            pid_controller = pidHarakiri;
     }
 }
 

src/main/flight/flight.h

@@ -32,16 +32,21 @@ typedef enum {
     PID_ITEM_COUNT
 } pidIndex_e;
 
+#define IS_PID_CONTROLLER_FP_BASED(pidController) (pidController == 2)
+
 typedef struct pidProfile_s {
+    uint8_t pidController;                  // 0 = multiwii original, 1 = rewrite from http://www.multiwii.com/forum/viewtopic.php?f=8&t=3671, 1, 2 = Luggi09s new baseflight pid
+
     uint8_t P8[PID_ITEM_COUNT];
     uint8_t I8[PID_ITEM_COUNT];
     uint8_t D8[PID_ITEM_COUNT];
 
-    float P_f[3];                           // float p i and d factors for the new baseflight pid
+    float P_f[3];                           // float p i and d factors for lux float pid controller
     float I_f[3];
     float D_f[3];
     float A_level;
     float H_level;
+    uint8_t H_sensitivity;
 } pidProfile_t;
 
 typedef enum {
@@ -60,28 +65,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;
@@ -132,7 +115,6 @@ extern int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3];
 extern int16_t heading, magHold;
 
 extern int32_t AltHold;
-extern int32_t EstAlt;
 extern int32_t vario;
 
 void setPIDController(int type);

src/main/flight/imu.c

@@ -73,59 +73,46 @@ int16_t gyroZero[FLIGHT_DYNAMICS_INDEX_COUNT] = { 0, 0, 0 };
 rollAndPitchInclination_t inclination = { { 0, 0 } };     // absolute angle inclination in multiple of 0.1 degree    180 deg = 1800
 float anglerad[2] = { 0.0f, 0.0f };    // absolute angle inclination in radians
 
-static void getEstimatedAttitude(void);
-
 imuRuntimeConfig_t *imuRuntimeConfig;
 pidProfile_t *pidProfile;
 accDeadband_t *accDeadband;
 
-void configureImu(imuRuntimeConfig_t *initialImuRuntimeConfig, pidProfile_t *initialPidProfile, accDeadband_t *initialAccDeadband)
+void configureIMU(
+    imuRuntimeConfig_t *initialImuRuntimeConfig,
+    pidProfile_t *initialPidProfile,
+    accDeadband_t *initialAccDeadband,
+    float accz_lpf_cutoff,
+    //TODO: Move throttle angle correction code into flight/throttle_angle_correction.c
+    uint16_t throttle_correction_angle
+)
 {
     imuRuntimeConfig = initialImuRuntimeConfig;
     pidProfile = initialPidProfile;
     accDeadband = initialAccDeadband;
+    fc_acc = calculateAccZLowPassFilterRCTimeConstant(accz_lpf_cutoff);
+    //TODO: Move throttle angle correction code into flight/throttle_angle_correction.c
+    throttleAngleScale = calculateThrottleAngleScale(throttle_correction_angle);
 }
 
-void imuInit()
+void initIMU()
 {
-    smallAngle = lrintf(acc_1G * cosf(RAD * imuRuntimeConfig->small_angle));
+    smallAngle = lrintf(acc_1G * cosf(degreesToRadians(imuRuntimeConfig->small_angle)));
     accVelScale = 9.80665f / acc_1G / 10000.0f;
-    gyroScaleRad = gyro.scale * (M_PI / 180.0f) * 0.000001f;
+    gyroScaleRad = gyro.scale * (M_PIf / 180.0f) * 0.000001f;
 }
 
-void calculateThrottleAngleScale(uint16_t throttle_correction_angle)
+//TODO: Move throttle angle correction code into flight/throttle_angle_correction.c
+float calculateThrottleAngleScale(uint16_t throttle_correction_angle)
 {
-    throttleAngleScale = (1800.0f / M_PI) * (900.0f / throttle_correction_angle);
+    return (1800.0f / M_PIf) * (900.0f / throttle_correction_angle);
 }
 
-void calculateAccZLowPassFilterRCTimeConstant(float accz_lpf_cutoff)
+/*
+* Calculate RC time constant used in the accZ lpf.
+*/
+float calculateAccZLowPassFilterRCTimeConstant(float accz_lpf_cutoff)
 {
-    fc_acc = 0.5f / (M_PI * accz_lpf_cutoff); // calculate RC time constant used in the accZ lpf
+    return 0.5f / (M_PIf * accz_lpf_cutoff);
-}
-
-void computeIMU(rollAndPitchTrims_t *accelerometerTrims, uint8_t mixerMode)
-{
-    static int16_t gyroYawSmooth = 0;
-
-    gyroGetADC();
-    if (sensors(SENSOR_ACC)) {
-        updateAccelerationReadings(accelerometerTrims);
-        getEstimatedAttitude();
-    } else {
-        accADC[X] = 0;
-        accADC[Y] = 0;
-        accADC[Z] = 0;
-    }
-
-    gyroData[FD_ROLL] = gyroADC[FD_ROLL];
-    gyroData[FD_PITCH] = gyroADC[FD_PITCH];
-
-    if (mixerMode == MIXER_TRI) {
-        gyroData[FD_YAW] = (gyroYawSmooth * 2 + gyroADC[FD_YAW]) / 3;
-        gyroYawSmooth = gyroData[FD_YAW];
-    } else {
-        gyroData[FD_YAW] = gyroADC[FD_YAW];
-    }
 }
 
 // **************************************************
@@ -145,56 +132,6 @@ void computeIMU(rollAndPitchTrims_t *accelerometerTrims, uint8_t mixerMode)
 
 t_fp_vector EstG;
 
-// Normalize a vector
-void normalizeV(struct fp_vector *src, struct fp_vector *dest)
-{
-    float length;
-
-    length = sqrtf(src->X * src->X + src->Y * src->Y + src->Z * src->Z);
-    if (length != 0) {
-        dest->X = src->X / length;
-        dest->Y = src->Y / length;
-        dest->Z = src->Z / length;
-    }
-}
-
-// Rotate Estimated vector(s) with small angle approximation, according to the gyro data
-void rotateV(struct fp_vector *v, fp_angles_t *delta)
-{
-    struct fp_vector v_tmp = *v;
-
-    // This does a  "proper" matrix rotation using gyro deltas without small-angle approximation
-    float mat[3][3];
-    float cosx, sinx, cosy, siny, cosz, sinz;
-    float coszcosx, sinzcosx, coszsinx, sinzsinx;
-
-    cosx = cosf(delta->angles.roll);
-    sinx = sinf(delta->angles.roll);
-    cosy = cosf(delta->angles.pitch);
-    siny = sinf(delta->angles.pitch);
-    cosz = cosf(delta->angles.yaw);
-    sinz = sinf(delta->angles.yaw);
-
-    coszcosx = cosz * cosx;
-    sinzcosx = sinz * cosx;
-    coszsinx = sinx * cosz;
-    sinzsinx = sinx * sinz;
-
-    mat[0][0] = cosz * cosy;
-    mat[0][1] = -cosy * sinz;
-    mat[0][2] = siny;
-    mat[1][0] = sinzcosx + (coszsinx * siny);
-    mat[1][1] = coszcosx - (sinzsinx * siny);
-    mat[1][2] = -sinx * cosy;
-    mat[2][0] = (sinzsinx) - (coszcosx * siny);
-    mat[2][1] = (coszsinx) + (sinzcosx * siny);
-    mat[2][2] = cosy * cosx;
-
-    v->X = v_tmp.X * mat[0][0] + v_tmp.Y * mat[1][0] + v_tmp.Z * mat[2][0];
-    v->Y = v_tmp.X * mat[0][1] + v_tmp.Y * mat[1][1] + v_tmp.Z * mat[2][1];
-    v->Z = v_tmp.X * mat[0][2] + v_tmp.Y * mat[1][2] + v_tmp.Z * mat[2][2];
-}
-
 void accSum_reset(void)
 {
     accSum[0] = 0;
@@ -248,8 +185,37 @@ void acc_calc(uint32_t deltaT)
     accSumCount++;
 }
 
-// baseflight calculation by Luggi09 originates from arducopter
+/*
-static int16_t calculateHeading(t_fp_vector *vec)
+* Baseflight calculation by Luggi09 originates from arducopter
+* ============================================================
+* This function rotates magnetic vector to cancel actual yaw and
+* pitch of craft. Then it computes it's direction in X/Y plane.
+* This value is returned as compass heading, value is 0-360 degrees.
+*
+* Note that Earth's magnetic field is not parallel with ground unless
+* you are near equator. Its inclination is considerable, >60 degrees
+* towards ground in most of Europe.
+*
+* First we consider it in 2D:
+*
+* An example, the vector <1, 1> would be turned into the heading
+* 45 degrees, representing it's angle clockwise from north.
+*
+*      ***************** *
+*      *       |   <1,1> *
+*      *       |  /      *
+*      *       | /       *
+*      *       |/        *
+*      *       *         *
+*      *                 *
+*      *                 *
+*      *                 *
+*      *                 *
+*      *******************
+*
+* //TODO: Add explanation for how it uses the Z dimension.
+*/
+int16_t calculateHeading(t_fp_vector *vec)
 {
     int16_t head;
 
@@ -259,8 +225,12 @@ static int16_t calculateHeading(t_fp_vector *vec)
     float sinePitch = sinf(anglerad[AI_PITCH]);
     float Xh = vec->A[X] * cosinePitch + vec->A[Y] * sineRoll * sinePitch + vec->A[Z] * sinePitch * cosineRoll;
     float Yh = vec->A[Y] * cosineRoll - vec->A[Z] * sineRoll;
-    float hd = (atan2f(Yh, Xh) * 1800.0f / M_PI + magneticDeclination) / 10.0f;
+    //TODO: Replace this comment with an explanation of why Yh and Xh can never simultanoeusly be zero,
+    // or handle the case in which they are and (atan2f(0, 0) is undefined.
+    float hd = (atan2f(Yh, Xh) * 1800.0f / M_PIf + magneticDeclination) / 10.0f;
     head = lrintf(hd);
+
+    // Arctan returns a value in the range -180 to 180 degrees. We 'normalize' negative angles to be positive.
     if (head < 0)
         head += 360;
 
@@ -318,8 +288,8 @@ static void getEstimatedAttitude(void)
     // Attitude of the estimated vector
     anglerad[AI_ROLL] = atan2f(EstG.V.Y, EstG.V.Z);
     anglerad[AI_PITCH] = atan2f(-EstG.V.X, sqrtf(EstG.V.Y * EstG.V.Y + EstG.V.Z * EstG.V.Z));
-    inclination.values.rollDeciDegrees = lrintf(anglerad[AI_ROLL] * (1800.0f / M_PI));
+    inclination.values.rollDeciDegrees = lrintf(anglerad[AI_ROLL] * (1800.0f / M_PIf));
-    inclination.values.pitchDeciDegrees = lrintf(anglerad[AI_PITCH] * (1800.0f / M_PI));
+    inclination.values.pitchDeciDegrees = lrintf(anglerad[AI_PITCH] * (1800.0f / M_PIf));
 
     if (sensors(SENSOR_MAG)) {
         rotateV(&EstM.V, &deltaGyroAngle);
@@ -338,16 +308,46 @@ static void getEstimatedAttitude(void)
     acc_calc(deltaT); // rotate acc vector into earth frame
 }
 
-// correction of throttle in lateral wind,
+void computeIMU(rollAndPitchTrims_t *accelerometerTrims, uint8_t mixerMode)
+{
+    static int16_t gyroYawSmooth = 0;
+
+    gyroGetADC();
+    if (sensors(SENSOR_ACC)) {
+        updateAccelerationReadings(accelerometerTrims);
+        getEstimatedAttitude();
+    } else {
+        accADC[X] = 0;
+        accADC[Y] = 0;
+        accADC[Z] = 0;
+    }
+
+    gyroData[FD_ROLL] = gyroADC[FD_ROLL];
+    gyroData[FD_PITCH] = gyroADC[FD_PITCH];
+
+    if (mixerMode == MIXER_TRI) {
+        gyroData[FD_YAW] = (gyroYawSmooth * 2 + gyroADC[FD_YAW]) / 3;
+        gyroYawSmooth = gyroData[FD_YAW];
+    } else {
+        gyroData[FD_YAW] = gyroADC[FD_YAW];
+    }
+}
+
+//TODO: Move throttle angle correction code into flight/throttle_angle_correction.c
 int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value)
 {
     float cosZ = EstG.V.Z / sqrtf(EstG.V.X * EstG.V.X + EstG.V.Y * EstG.V.Y + EstG.V.Z * EstG.V.Z);
 
-    if (cosZ <= 0.015f) { // we are inverted, vertical or with a small angle < 0.86 deg
+    /*
+    * Use 0 as the throttle angle correction if we are inverted, vertical or with a
+    * small angle < 0.86 deg
+    * TODO: Define this small angle in config.
+    */
+    if (cosZ <= 0.015f) {
         return 0;
     }
     int angle = lrintf(acosf(cosZ) * throttleAngleScale);
     if (angle > 900)
         angle = 900;
-    return lrintf(throttle_correction_value * sinf(angle / (900.0f * M_PI / 2.0f)));
+    return lrintf(throttle_correction_value * sinf(angle / (900.0f * M_PIf / 2.0f)));
 }

src/main/flight/imu.h

@@ -30,12 +30,20 @@ typedef struct imuRuntimeConfig_s {
     int8_t small_angle;
 } imuRuntimeConfig_t;
 
-void configureImu(imuRuntimeConfig_t *initialImuRuntimeConfig, pidProfile_t *initialPidProfile, accDeadband_t *initialAccDeadband);
+void configureIMU(
+	imuRuntimeConfig_t *initialImuRuntimeConfig,
+	pidProfile_t *initialPidProfile,
+	accDeadband_t *initialAccDeadband,
+	float accz_lpf_cutoff,
+	uint16_t throttle_correction_angle
+);
 
 void calculateEstimatedAltitude(uint32_t currentTime);
 void computeIMU(rollAndPitchTrims_t *accelerometerTrims, uint8_t mixerMode);
-void calculateThrottleAngleScale(uint16_t throttle_correction_angle);
+float calculateThrottleAngleScale(uint16_t throttle_correction_angle);
 int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value);
-void calculateAccZLowPassFilterRCTimeConstant(float accz_lpf_cutoff);
+float calculateAccZLowPassFilterRCTimeConstant(float accz_lpf_cutoff);
+
+int16_t calculateHeading(t_fp_vector *vec);
 
 void accSum_reset(void);

src/main/flight/mixer.c

@@ -490,14 +490,14 @@ static void airplaneMixer(void)
         int16_t lFlap = determineServoMiddleOrForwardFromChannel(2);
 
         lFlap = constrain(lFlap, servoConf[2].min, servoConf[2].max);
-        lFlap = rxConfig->midrc - lFlap;
+        lFlap = escAndServoConfig->servoCenterPulse - lFlap;
         if (slow_LFlaps < lFlap)
             slow_LFlaps += airplaneConfig->flaps_speed;
         else if (slow_LFlaps > lFlap)
             slow_LFlaps -= airplaneConfig->flaps_speed;
 
         servo[2] = ((int32_t)servoConf[2].rate * slow_LFlaps) / 100L;
-        servo[2] += rxConfig->midrc;
+        servo[2] += escAndServoConfig->servoCenterPulse;
     }
 
     if (FLIGHT_MODE(PASSTHRU_MODE)) {   // Direct passthru from RX
@@ -531,7 +531,7 @@ void mixTable(void)
 
     if (motorCount > 3) {
         // prevent "yaw jump" during yaw correction
-        axisPID[YAW] = constrain(axisPID[YAW], -100 - abs(rcCommand[YAW]), +100 + abs(rcCommand[YAW]));
+        axisPID[YAW] = constrain(axisPID[YAW], -100 - ABS(rcCommand[YAW]), +100 + ABS(rcCommand[YAW]));
     }
 
     // motors for non-servo mixes

src/main/flight/navigation.c

@@ -163,7 +163,7 @@ static int32_t get_D(int32_t input, float *dt, PID *pid, PID_PARAM *pid_param)
 
     // Low pass filter cut frequency for derivative calculation
     // Set to  "1 / ( 2 * PI * gps_lpf )
-    float pidFilter = (1.0f / (2.0f * M_PI * (float)gpsProfile->gps_lpf));
+    float pidFilter = (1.0f / (2.0f * M_PIf * (float)gpsProfile->gps_lpf));
     // discrete low pass filter, cuts out the
     // high frequency noise that can drive the controller crazy
     pid->derivative = pid->last_derivative + (*dt / (pidFilter + *dt)) * (pid->derivative - pid->last_derivative);
@@ -306,7 +306,7 @@ void onGpsNewData(void)
     dTnav = (float)(millis() - nav_loopTimer) / 1000.0f;
     nav_loopTimer = millis();
     // prevent runup from bad GPS
-    dTnav = min(dTnav, 1.0f);
+    dTnav = MIN(dTnav, 1.0f);
 
     GPS_calculateDistanceAndDirectionToHome();
 
@@ -413,7 +413,7 @@ void gpsUsePIDs(pidProfile_t *pidProfile)
 //
 static void GPS_calc_longitude_scaling(int32_t lat)
 {
-    float rads = (abs((float)lat) / 10000000.0f) * 0.0174532925f;
+    float rads = (ABS((float)lat) / 10000000.0f) * 0.0174532925f;
     GPS_scaleLonDown = cosf(rads);
 }
 
@@ -442,7 +442,7 @@ static bool check_missed_wp(void)
     int32_t temp;
     temp = target_bearing - original_target_bearing;
     temp = wrap_18000(temp);
-    return (abs(temp) > 10000); // we passed the waypoint by 100 degrees
+    return (ABS(temp) > 10000); // we passed the waypoint by 100 degrees
 }
 
 #define DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR_IN_HUNDREDS_OF_KILOMETERS 1.113195f
@@ -536,7 +536,7 @@ static void GPS_calc_poshold(void)
 
         // get rid of noise
 #if defined(GPS_LOW_SPEED_D_FILTER)
-        if (abs(actual_speed[axis]) < 50)
+        if (ABS(actual_speed[axis]) < 50)
             d = 0;
 #endif
 
@@ -582,7 +582,7 @@ static void GPS_calc_nav_rate(uint16_t max_speed)
 //
 static void GPS_update_crosstrack(void)
 {
-    if (abs(wrap_18000(target_bearing - original_target_bearing)) < 4500) {     // If we are too far off or too close we don't do track following
+    if (ABS(wrap_18000(target_bearing - original_target_bearing)) < 4500) {     // If we are too far off or too close we don't do track following
         float temp = (target_bearing - original_target_bearing) * RADX100;
         crosstrack_error = sinf(temp) * (wp_distance * CROSSTRACK_GAIN); // Meters we are off track line
         nav_bearing = target_bearing + constrain(crosstrack_error, -3000, 3000);
@@ -607,10 +607,10 @@ static uint16_t GPS_calc_desired_speed(uint16_t max_speed, bool _slow)
 {
     // max_speed is default 400 or 4m/s
     if (_slow) {
-        max_speed = min(max_speed, wp_distance / 2);
+        max_speed = MIN(max_speed, wp_distance / 2);
     } else {
-        max_speed = min(max_speed, wp_distance);
+        max_speed = MIN(max_speed, wp_distance);
-        max_speed = max(max_speed, gpsProfile->nav_speed_min);      // go at least 100cm/s
+        max_speed = MAX(max_speed, gpsProfile->nav_speed_min);      // go at least 100cm/s
     }
 
     // limit the ramp up of the speed

src/main/io/display.c

@@ -292,12 +292,12 @@ void showProfilePage(void)
 void showGpsPage() {
     uint8_t rowIndex = PAGE_TITLE_LINE_COUNT;
 
-    i2c_OLED_set_xy(max(0, SATELLITE_GRAPH_LEFT_OFFSET), rowIndex++);
+    i2c_OLED_set_xy(MAX(0, SATELLITE_GRAPH_LEFT_OFFSET), rowIndex++);
 
     uint32_t index;
     for (index = 0; index < SATELLITE_COUNT && index < SCREEN_CHARACTER_COLUMN_COUNT; index++) {
         uint8_t bargraphValue = ((uint16_t) GPS_svinfo_cno[index] * VERTICAL_BARGRAPH_CHARACTER_COUNT) / (GPS_DBHZ_MAX - 1);
-        bargraphValue = min(bargraphValue, VERTICAL_BARGRAPH_CHARACTER_COUNT - 1);
+        bargraphValue = MIN(bargraphValue, VERTICAL_BARGRAPH_CHARACTER_COUNT - 1);
         i2c_OLED_send_char(VERTICAL_BARGRAPH_ZERO_CHARACTER + bargraphValue);
     }
 
@@ -378,24 +378,28 @@ void showBatteryPage(void)
 void showSensorsPage(void)
 {
     uint8_t rowIndex = PAGE_TITLE_LINE_COUNT;
+    static const char *format = "%c = %5d %5d %5d";
 
     i2c_OLED_set_line(rowIndex++);
     i2c_OLED_send_string("        X     Y     Z");
+
     if (sensors(SENSOR_ACC)) {
-        tfp_sprintf(lineBuffer, "A = %5d %5d %5d", accSmooth[X], accSmooth[Y], accSmooth[Z]);
+        tfp_sprintf(lineBuffer, format, 'A', accSmooth[X], accSmooth[Y], accSmooth[Z]);
         padLineBuffer();
         i2c_OLED_set_line(rowIndex++);
         i2c_OLED_send_string(lineBuffer);
     }
+
     if (sensors(SENSOR_GYRO)) {
-        tfp_sprintf(lineBuffer, "G = %5d %5d %5d", gyroADC[X], gyroADC[Y], gyroADC[Z]);
+        tfp_sprintf(lineBuffer, format, 'G', gyroADC[X], gyroADC[Y], gyroADC[Z]);
         padLineBuffer();
         i2c_OLED_set_line(rowIndex++);
         i2c_OLED_send_string(lineBuffer);
     }
+
 #ifdef MAG
     if (sensors(SENSOR_MAG)) {
-        tfp_sprintf(lineBuffer, "M = %5d %5d %5d", magADC[X], magADC[Y], magADC[Z]);
+        tfp_sprintf(lineBuffer, format, 'M', magADC[X], magADC[Y], magADC[Z]);
         padLineBuffer();
         i2c_OLED_set_line(rowIndex++);
         i2c_OLED_send_string(lineBuffer);

src/main/io/escservo.h

@@ -18,7 +18,10 @@
 #pragma once
 
 typedef struct escAndServoConfig_s {
+
+    // PWM values, in milliseconds, common range is 1000-2000 (1 to 2ms)
     uint16_t minthrottle;                   // Set the minimum throttle command sent to the ESC (Electronic Speed Controller). This is the minimum value that allow motors to run at a idle speed.
     uint16_t maxthrottle;                   // This is the maximum value for the ESCs at full power this value can be increased up to 2000
     uint16_t mincommand;                    // This is the value for the ESCs when they are not armed. In some cases, this value must be lowered down to 900 for some specific ESCs
+    uint16_t servoCenterPulse;              // This is the value for servos when they should be in the middle. e.g. 1500.
 } escAndServoConfig_t;

src/main/io/ledstrip.c

@@ -48,16 +48,34 @@
 #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
+static uint32_t nextAnimationUpdateAt = 0;
+#endif
+
+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)
+#define LED_STRIP_5HZ ((1000 * 1000) / 5)
+
 #if MAX_LED_STRIP_LENGTH > WS2811_LED_STRIP_LENGTH
 #error "Led strip length must match driver"
 #endif
 
 hsvColor_t *colors;
 
-//#define USE_LED_ANIMATION
-
 //                          H    S    V
 #define LED_BLACK        {  0,   0,   0}
 #define LED_WHITE        {  0, 255, 255}
@@ -185,6 +203,7 @@ static const modeColorIndexes_t angleModeColors = {
         COLOR_ORANGE
 };
 
+#ifdef MAG
 static const modeColorIndexes_t magModeColors = {
         COLOR_MINT_GREEN,
         COLOR_DARK_VIOLET,
@@ -193,6 +212,7 @@ static const modeColorIndexes_t magModeColors = {
         COLOR_BLUE,
         COLOR_ORANGE
 };
+#endif
 
 static const modeColorIndexes_t baroModeColors = {
         COLOR_LIGHT_BLUE,
@@ -207,23 +227,70 @@ 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,  2), 3, LED_FUNCTION_THRUST_RING},
-    { CALCULATE_LED_XY( 2,  1), LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING },
+    { CALCULATE_LED_XY( 2,  1), 3, LED_FUNCTION_THRUST_RING},
-    { CALCULATE_LED_XY( 2,  0), LED_DIRECTION_NORTH | LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE },
+    { CALCULATE_LED_XY( 2,  0), 3, LED_FUNCTION_THRUST_RING},
-    { CALCULATE_LED_XY( 1,  0), LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE },
+    { CALCULATE_LED_XY( 1,  0), 3, LED_FUNCTION_THRUST_RING},
-    { CALCULATE_LED_XY( 0,  0), LED_DIRECTION_NORTH | LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE },
+    { CALCULATE_LED_XY( 0,  0), 3, LED_FUNCTION_THRUST_RING},
-    { CALCULATE_LED_XY( 0,  1), LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING },
+    { CALCULATE_LED_XY( 0,  1), 3, LED_FUNCTION_THRUST_RING},
-    { CALCULATE_LED_XY( 0,  2), LED_DIRECTION_SOUTH | LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE },
+    { CALCULATE_LED_XY( 0,  2), 3, LED_FUNCTION_THRUST_RING},
-    { CALCULATE_LED_XY( 1,  2), LED_DIRECTION_SOUTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING },
+    { CALCULATE_LED_XY( 1,  2), 3, LED_FUNCTION_THRUST_RING},
-    { CALCULATE_LED_XY( 1,  1), LED_DIRECTION_UP | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING },
+    { CALCULATE_LED_XY( 1,  1), 3, LED_FUNCTION_THRUST_RING},
-    { CALCULATE_LED_XY( 1,  1), LED_DIRECTION_UP | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING },
+    { CALCULATE_LED_XY( 1,  1), 3, LED_FUNCTION_THRUST_RING},
-    { CALCULATE_LED_XY( 1,  1), LED_DIRECTION_DOWN | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING },
+    { CALCULATE_LED_XY( 1,  1), 3, LED_FUNCTION_THRUST_RING},
-    { CALCULATE_LED_XY( 1,  1), LED_DIRECTION_DOWN | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING },
+    { CALCULATE_LED_XY( 1,  1), 3, LED_FUNCTION_THRUST_RING},
 };
+#else
+const ledConfig_t defaultLedStripConfig[] = {
+    { CALCULATE_LED_XY(15, 15), 0, LED_DIRECTION_SOUTH | LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE },
+
+    { CALCULATE_LED_XY(15,  8), 0, LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING },
+    { CALCULATE_LED_XY(15,  7), 0, LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING },
+
+    { CALCULATE_LED_XY(15,  0), 0, LED_DIRECTION_NORTH | LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE },
+
+    { CALCULATE_LED_XY( 8,  0), 0, LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE },
+    { CALCULATE_LED_XY( 7,  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,  7), 0, LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING },
+    { CALCULATE_LED_XY( 0,  8), 0, LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING },
+
+    { CALCULATE_LED_XY( 0, 15), 0, LED_DIRECTION_SOUTH | LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE },
+
+    { CALCULATE_LED_XY( 7, 15), 0, LED_DIRECTION_SOUTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING },
+    { CALCULATE_LED_XY( 8, 15), 0, LED_DIRECTION_SOUTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING },
+
+    { CALCULATE_LED_XY( 7,  7), 0, LED_DIRECTION_UP | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING },
+    { CALCULATE_LED_XY( 8,  7), 0, LED_DIRECTION_UP | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING },
+    { CALCULATE_LED_XY( 7,  8), 0, LED_DIRECTION_DOWN | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING },
+    { CALCULATE_LED_XY( 8,  8), 0, LED_DIRECTION_DOWN | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING },
+
+    { CALCULATE_LED_XY( 8,  9), 3, LED_FUNCTION_THRUST_RING},
+    { CALCULATE_LED_XY( 9, 10), 3, LED_FUNCTION_THRUST_RING},
+    { CALCULATE_LED_XY(10, 11), 3, LED_FUNCTION_THRUST_RING},
+    { CALCULATE_LED_XY(10, 12), 3, LED_FUNCTION_THRUST_RING},
+    { CALCULATE_LED_XY( 9, 13), 3, LED_FUNCTION_THRUST_RING},
+    { CALCULATE_LED_XY( 8, 14), 3, LED_FUNCTION_THRUST_RING},
+    { CALCULATE_LED_XY( 7, 14), 3, LED_FUNCTION_THRUST_RING},
+    { CALCULATE_LED_XY( 6, 13), 3, LED_FUNCTION_THRUST_RING},
+    { CALCULATE_LED_XY( 5, 12), 3, LED_FUNCTION_THRUST_RING},
+    { CALCULATE_LED_XY( 5, 11), 3, LED_FUNCTION_THRUST_RING},
+    { CALCULATE_LED_XY( 6, 10), 3, LED_FUNCTION_THRUST_RING},
+    { CALCULATE_LED_XY( 7,  9), 3, LED_FUNCTION_THRUST_RING},
+
+};
+#endif
+
 
 
 /*
@@ -240,12 +307,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]))
@@ -258,14 +326,16 @@ 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', 'C' };
 #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,
+	LED_FUNCTION_COLOR
 };
 
 // grid offsets
@@ -309,17 +379,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();
@@ -392,6 +473,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++;
@@ -415,6 +505,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);
@@ -433,18 +524,9 @@ 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);
 }
 
-// timers
-uint32_t nextAnimationUpdateAt = 0;
-uint32_t nextIndicatorFlashAt = 0;
-uint32_t nextWarningFlashAt = 0;
-
-#define LED_STRIP_20HZ ((1000 * 1000) / 20)
-#define LED_STRIP_10HZ ((1000 * 1000) / 10)
-#define LED_STRIP_5HZ ((1000 * 1000) / 5)
-
 void applyDirectionalModeColor(const uint8_t ledIndex, const ledConfig_t *ledConfig, const modeColorIndexes_t *modeColors)
 {
     // apply up/down colors regardless of quadrant.
@@ -520,7 +602,13 @@ void applyLedModeLayer(void)
 
         ledConfig = &ledConfigs[ledIndex];
 
-        setLedHsv(ledIndex, &hsv_black);
+        if (!(ledConfig->flags & LED_FUNCTION_THRUST_RING)) {
+            if (ledConfig->flags & LED_FUNCTION_COLOR) {
+                setLedHsv(ledIndex, &colors[ledConfig->color]);
+            } else {
+                setLedHsv(ledIndex, &hsv_black);
+            }
+        }
 
         if (!(ledConfig->flags & LED_FUNCTION_FLIGHT_MODE)) {
             if (ledConfig->flags & LED_FUNCTION_ARM_STATE) {
@@ -673,7 +761,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;
         }
 
@@ -686,14 +774,63 @@ void applyLedThrottleLayer()
     }
 }
 
-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;
+void applyLedThrustRingLayer(void)
-static uint8_t currentRow;
-static uint8_t nextRow;
-
-static void updateLedAnimationState(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;
@@ -703,7 +840,6 @@ static void updateLedAnimationState(void)
     frameCounter = (frameCounter + 1) % animationFrames;
 }
 
-#ifdef USE_LED_ANIMATION
 static void applyLedAnimationLayer(void)
 {
     const ledConfig_t *ledConfig;
@@ -732,17 +868,41 @@ 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 animationUpdateNow = (int32_t)(now - nextAnimationUpdateAt) >= 0L;
+    bool indicatorFlashNow = indicatorFlashNow = (int32_t)(now - nextIndicatorFlashAt) >= 0L;
-    bool indicatorFlashNow = (int32_t)(now - nextIndicatorFlashAt) >= 0L;
+    bool warningFlashNow = warningFlashNow = (int32_t)(now - nextWarningFlashAt) >= 0L;
-    bool warningFlashNow = (int32_t)(now - nextWarningFlashAt) >= 0L;
+    bool rotationUpdateNow = (int32_t)(now - nextRotationUpdateAt) >= 0L;
-
+#ifdef USE_LED_ANIMATION
-    if (!(warningFlashNow || indicatorFlashNow || animationUpdateNow)) {
+    bool animationUpdateNow = animationUpdateNow = (int32_t)(now - nextAnimationUpdateAt) >= 0L;
+#endif
+    if (!(
+            indicatorFlashNow ||
+            rotationUpdateNow ||
+            warningFlashNow
+#ifdef USE_LED_ANIMATION
+            || animationUpdateNow
+#endif
+    )) {
         return;
     }
 
@@ -763,10 +923,10 @@ void updateLedStrip(void)
 
     if (indicatorFlashNow) {
 
-        uint8_t rollScale = abs(rcCommand[ROLL]) / 50;
+        uint8_t rollScale = ABS(rcCommand[ROLL]) / 50;
-        uint8_t pitchScale = abs(rcCommand[PITCH]) / 50;
+        uint8_t pitchScale = ABS(rcCommand[PITCH]) / 50;
-        uint8_t scale = max(rollScale, pitchScale);
+        uint8_t scale = MAX(rollScale, pitchScale);
-        nextIndicatorFlashAt = now + (LED_STRIP_5HZ / max(1, scale));
+        nextIndicatorFlashAt = now + (LED_STRIP_5HZ / MAX(1, scale));
 
         if (indicatorFlashState == 0) {
             indicatorFlashState = 1;
@@ -777,20 +937,33 @@ void updateLedStrip(void)
 
     applyLedIndicatorLayer(indicatorFlashState);
 
+#ifdef USE_LED_ANIMATION
     if (animationUpdateNow) {
         nextAnimationUpdateAt = now + LED_STRIP_20HZ;
         updateLedAnimationState();
     }
-
-#ifdef USE_LED_ANIMATION
     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();
 }
 
-bool parseColor(uint8_t index, char *colorConfig)
+bool parseColor(uint8_t index, const char *colorConfig)
 {
-    char *remainingCharacters = colorConfig;
+    const char *remainingCharacters = colorConfig;
 
     hsvColor_t *color = &colors[index];
 
@@ -804,6 +977,7 @@ bool parseColor(uint8_t index, char *colorConfig)
                 if (val > HSV_HUE_MAX) {
                     ok = false;
                     continue;
+
                 }
                 colors[index].h = val;
                 break;
@@ -870,4 +1044,11 @@ void ledStripEnable(void)
 
     ws2811LedStripInit();
 }
+
+static void ledStripDisable(void)
+{
+	setStripColor(&hsv_black);
+    
+	ws2811UpdateStrip();
+}
 #endif

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,33 +46,53 @@ 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),
+    LED_FUNCTION_COLOR       = (1 << 12),
 } 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 | \
+    LED_FUNCTION_COLOR \
+)
 
 
 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;
+extern uint8_t ledsInRingCount;
 
-#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);
 
-bool parseColor(uint8_t index, char *colorConfig);
+bool parseColor(uint8_t index, const char *colorConfig);
 void applyDefaultColors(hsvColor_t *colors, uint8_t colorCount);
 
 void ledStripEnable(void);
+void reevalulateLedConfig(void);
 

src/main/io/rc_controls.c

@@ -21,6 +21,8 @@
 
 #include <math.h>
 
+#include "platform.h"
+
 #include "common/axis.h"
 #include "common/maths.h"
 
@@ -30,10 +32,12 @@
 #include "drivers/system.h"
 
 #include "flight/flight.h"
+#include "flight/navigation.h"
 
 #include "drivers/sensor.h"
 #include "drivers/accgyro.h"
 
+#include "sensors/barometer.h"
 #include "sensors/battery.h"
 #include "sensors/sensors.h"
 #include "sensors/gyro.h"
@@ -64,7 +68,7 @@ bool isUsingSticksForArming(void)
 
 bool areSticksInApModePosition(uint16_t ap_mode)
 {
-    return abs(rcCommand[ROLL]) < ap_mode && abs(rcCommand[PITCH]) < ap_mode;
+    return ABS(rcCommand[ROLL]) < ap_mode && ABS(rcCommand[PITCH]) < ap_mode;
 }
 
 throttleStatus_e calculateThrottleStatus(rxConfig_t *rxConfig, uint16_t deadband3d_throttle)
@@ -360,6 +364,7 @@ void configureAdjustment(uint8_t index, uint8_t auxSwitchChannelIndex, const adj
 
 void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustmentFunction, int delta) {
     int newValue;
+    float newFloatValue;
 
     if (delta > 0) {
         queueConfirmationBeep(2);
@@ -391,34 +396,70 @@ void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustm
             controlRateConfig->yawRate = constrain(newValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c
             break;
         case ADJUSTMENT_PITCH_ROLL_P:
-            newValue = (int)pidProfile->P8[PIDPITCH] + delta;
+            if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) {
-            pidProfile->P8[PIDPITCH] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c
+                newFloatValue = pidProfile->P_f[PIDPITCH] + (float)(delta / 10.0f);
-            newValue = (int)pidProfile->P8[PIDROLL] + delta;
+                pidProfile->P_f[PIDPITCH] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c
-            pidProfile->P8[PIDROLL] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c
+                newFloatValue = pidProfile->P_f[PIDROLL] + (float)(delta / 10.0f);
+                pidProfile->P_f[PIDROLL] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c
+            } else {
+                newValue = (int)pidProfile->P8[PIDPITCH] + delta;
+                pidProfile->P8[PIDPITCH] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c
+                newValue = (int)pidProfile->P8[PIDROLL] + delta;
+                pidProfile->P8[PIDROLL] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c
+            }
             break;
         case ADJUSTMENT_PITCH_ROLL_I:
-            newValue = (int)pidProfile->I8[PIDPITCH] + delta;
+            if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) {
-            pidProfile->I8[PIDPITCH] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c
+                newFloatValue = pidProfile->I_f[PIDPITCH] + (float)(delta / 100.0f);
-            newValue = (int)pidProfile->I8[PIDROLL] + delta;
+                pidProfile->I_f[PIDPITCH] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c
-            pidProfile->I8[PIDROLL] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c
+                newFloatValue = pidProfile->I_f[PIDROLL] + (float)(delta / 100.0f);
+                pidProfile->I_f[PIDROLL] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c
+            } else {
+                newValue = (int)pidProfile->I8[PIDPITCH] + delta;
+                pidProfile->I8[PIDPITCH] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c
+                newValue = (int)pidProfile->I8[PIDROLL] + delta;
+                pidProfile->I8[PIDROLL] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c
+            }
             break;
         case ADJUSTMENT_PITCH_ROLL_D:
-            newValue = (int)pidProfile->D8[PIDPITCH] + delta;
+            if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) {
-            pidProfile->D8[PIDPITCH] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c
+                newFloatValue = pidProfile->D_f[PIDPITCH] + (float)(delta / 1000.0f);
-            newValue = (int)pidProfile->D8[PIDROLL] + delta;
+                pidProfile->D_f[PIDPITCH] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c
-            pidProfile->D8[PIDROLL] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c
+                newFloatValue = pidProfile->D_f[PIDROLL] + (float)(delta / 1000.0f);
+                pidProfile->D_f[PIDROLL] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c
+            } else {
+                newValue = (int)pidProfile->D8[PIDPITCH] + delta;
+                pidProfile->D8[PIDPITCH] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c
+                newValue = (int)pidProfile->D8[PIDROLL] + delta;
+                pidProfile->D8[PIDROLL] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c
+            }
             break;
         case ADJUSTMENT_YAW_P:
-            newValue = (int)pidProfile->P8[PIDYAW] + delta;
+            if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) {
-            pidProfile->P8[PIDYAW] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c
+                newFloatValue = pidProfile->P_f[PIDYAW] + (float)(delta / 10.0f);
+                pidProfile->P_f[PIDYAW] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c
+            } else {
+                newValue = (int)pidProfile->P8[PIDYAW] + delta;
+                pidProfile->P8[PIDYAW] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c
+            }
             break;
         case ADJUSTMENT_YAW_I:
-            newValue = (int)pidProfile->I8[PIDYAW] + delta;
+            if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) {
-            pidProfile->I8[PIDYAW] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c
+                newFloatValue = pidProfile->I_f[PIDYAW] + (float)(delta / 100.0f);
+                pidProfile->I_f[PIDYAW] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c
+            } else {
+                newValue = (int)pidProfile->I8[PIDYAW] + delta;
+                pidProfile->I8[PIDYAW] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c
+            }
             break;
         case ADJUSTMENT_YAW_D:
-            newValue = (int)pidProfile->D8[PIDYAW] + delta;
+            if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) {
-            pidProfile->D8[PIDYAW] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c
+                newFloatValue = pidProfile->D_f[PIDYAW] + (float)(delta / 1000.0f);
+                pidProfile->D_f[PIDYAW] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c
+            } else {
+                newValue = (int)pidProfile->D8[PIDYAW] + delta;
+                pidProfile->D8[PIDYAW] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c
+            }
             break;
         default:
             break;
@@ -517,6 +558,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;

src/main/io/rc_controls.h

@@ -17,6 +17,8 @@
 
 #pragma once
 
+#include "rx/rx.h"
+
 typedef enum {
     BOXARM = 0,
     BOXANGLE,
@@ -205,7 +207,10 @@ typedef struct adjustmentState_s {
     uint32_t timeoutAt;
 } adjustmentState_t;
 
+
+#ifndef MAX_SIMULTANEOUS_ADJUSTMENT_COUNT
 #define MAX_SIMULTANEOUS_ADJUSTMENT_COUNT 4 // enough for 4 x 3position switches / 4 aux channel
+#endif
 
 #define MAX_ADJUSTMENT_RANGE_COUNT 12 // enough for 2 * 6pos switches.
 
@@ -214,3 +219,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);

src/main/io/serial.c

@@ -44,7 +44,6 @@
 
 void updateSerialRxFunctionConstraint(functionConstraint_t *functionConstraintToUpdate);
 
-void mspInit(serialConfig_t *serialConfig);
 void cliInit(serialConfig_t *serialConfig);
 
 // this exists so the user can reference scenarios by a number in the CLI instead of an unuser-friendly bitmask.
@@ -73,26 +72,32 @@ static serialPort_t *serialPorts[SERIAL_PORT_COUNT];
 
 #ifdef STM32F303xC
 static serialPortFunction_t serialPortFunctions[SERIAL_PORT_COUNT] = {
+#ifdef USE_VCP
     {SERIAL_PORT_USB_VCP,     NULL, SCENARIO_UNUSED, FUNCTION_NONE},
-    {SERIAL_PORT_USART1,      NULL, SCENARIO_UNUSED, FUNCTION_NONE},
-    {SERIAL_PORT_USART2,      NULL, SCENARIO_UNUSED, FUNCTION_NONE},
-#if (SERIAL_PORT_COUNT > 3)
-    {SERIAL_PORT_USART3,      NULL, SCENARIO_UNUSED, FUNCTION_NONE},
-#if (SERIAL_PORT_COUNT > 4)
-    {SERIAL_PORT_USART4,      NULL, SCENARIO_UNUSED, FUNCTION_NONE}
 #endif
+#ifdef USE_USART1
+    {SERIAL_PORT_USART1,      NULL, SCENARIO_UNUSED, FUNCTION_NONE},
+#endif
+#ifdef USE_USART2
+    {SERIAL_PORT_USART2,      NULL, SCENARIO_UNUSED, FUNCTION_NONE},
+#endif
+#ifdef USE_USART3
+    {SERIAL_PORT_USART3,      NULL, SCENARIO_UNUSED, FUNCTION_NONE},
 #endif
 };
 
 const serialPortConstraint_t serialPortConstraints[SERIAL_PORT_COUNT] = {
+#ifdef USE_VCP
     {SERIAL_PORT_USB_VCP,       9600,   115200,   SPF_NONE },
-    {SERIAL_PORT_USART1,        9600,   115200,   SPF_NONE | SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_BIDIR_MODE},
-    {SERIAL_PORT_USART2,        9600,   115200,   SPF_SUPPORTS_CALLBACK | SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_BIDIR_MODE},
-#if (SERIAL_PORT_COUNT > 3)
-    {SERIAL_PORT_USART3,        9600,   115200,   SPF_SUPPORTS_CALLBACK | SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_BIDIR_MODE},
-#if (SERIAL_PORT_COUNT > 4)
-    {SERIAL_PORT_USART4,        9600,   115200,   SPF_SUPPORTS_CALLBACK}
 #endif
+#ifdef USE_USART1
+    {SERIAL_PORT_USART1,        9600,   250000,   SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_BIDIR_MODE},
+#endif
+#ifdef USE_USART2
+    {SERIAL_PORT_USART2,        9600,   250000,   SPF_SUPPORTS_CALLBACK | SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_BIDIR_MODE},
+#endif
+#ifdef USE_USART3
+    {SERIAL_PORT_USART3,        9600,   250000,   SPF_SUPPORTS_CALLBACK | SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_BIDIR_MODE},
 #endif
 };
 
@@ -100,14 +105,20 @@ const serialPortConstraint_t serialPortConstraints[SERIAL_PORT_COUNT] = {
 
 #ifdef CC3D
 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_USART3,      NULL, SCENARIO_UNUSED, FUNCTION_NONE},
     {SERIAL_PORT_SOFTSERIAL1, NULL, SCENARIO_UNUSED, FUNCTION_NONE}
 };
 
 const serialPortConstraint_t serialPortConstraints[SERIAL_PORT_COUNT] = {
-    {SERIAL_PORT_USART1,        9600, 115200,   SPF_NONE | SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_BIDIR_MODE},
+#ifdef USE_VCP
-    {SERIAL_PORT_USART3,        9600, 115200,   SPF_SUPPORTS_CALLBACK | SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_BIDIR_MODE},
+    {SERIAL_PORT_USB_VCP,       9600,   115200,   SPF_NONE },
+#endif
+    {SERIAL_PORT_USART1,        9600, 250000,   SPF_SUPPORTS_CALLBACK | SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_BIDIR_MODE},
+    {SERIAL_PORT_USART3,        9600, 250000,   SPF_SUPPORTS_CALLBACK | SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_BIDIR_MODE},
     {SERIAL_PORT_SOFTSERIAL1,   9600, 19200,    SPF_SUPPORTS_CALLBACK | SPF_IS_SOFTWARE_INVERTABLE}
 };
 #else
@@ -122,8 +133,8 @@ static serialPortFunction_t serialPortFunctions[SERIAL_PORT_COUNT] = {
 };
 
 const serialPortConstraint_t serialPortConstraints[SERIAL_PORT_COUNT] = {
-    {SERIAL_PORT_USART1,        9600, 115200,   SPF_NONE | SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_BIDIR_MODE},
+    {SERIAL_PORT_USART1,        9600, 250000,   SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_BIDIR_MODE},
-    {SERIAL_PORT_USART2,        9600, 115200,   SPF_SUPPORTS_CALLBACK | SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_BIDIR_MODE},
+    {SERIAL_PORT_USART2,        9600, 250000,   SPF_SUPPORTS_CALLBACK | SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_BIDIR_MODE},
 #if (SERIAL_PORT_COUNT > 2)
     {SERIAL_PORT_SOFTSERIAL1,   9600, 19200,    SPF_SUPPORTS_CALLBACK | SPF_IS_SOFTWARE_INVERTABLE},
     {SERIAL_PORT_SOFTSERIAL2,   9600, 19200,    SPF_SUPPORTS_CALLBACK | SPF_IS_SOFTWARE_INVERTABLE}
@@ -137,7 +148,7 @@ const functionConstraint_t functionConstraints[] = {
         { FUNCTION_GPS,                 9600,  115200, AUTOBAUD,    SPF_NONE },
         { FUNCTION_GPS_PASSTHROUGH,     9600,  115200, NO_AUTOBAUD, SPF_NONE },
         { FUNCTION_MSP,                 9600,  115200, NO_AUTOBAUD, SPF_NONE },
-        { FUNCTION_SERIAL_RX,           9600,  115200, NO_AUTOBAUD, SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_CALLBACK },
+        { FUNCTION_SERIAL_RX,           9600,  250000, NO_AUTOBAUD, SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_CALLBACK },
         { FUNCTION_TELEMETRY,           9600,  19200,  NO_AUTOBAUD, SPF_NONE },
         { FUNCTION_SMARTPORT_TELEMETRY, 57600, 57600,  NO_AUTOBAUD, SPF_SUPPORTS_BIDIR_MODE },
         { FUNCTION_BLACKBOX,            115200,115200, NO_AUTOBAUD, SPF_NONE }

src/main/io/serial_cli.c

@@ -163,7 +163,7 @@ const clicmd_t cmdTable[] = {
     { "color", "configure colors", cliColor },
 #endif
     { "defaults", "reset to defaults and reboot", cliDefaults },
-    { "dump", "print configurable settings in a pastable form", cliDump },
+    { "dump", "dump configuration", cliDump },
     { "exit", "", cliExit },
     { "feature", "list or -val or val", cliFeature },
     { "get", "get variable value", cliGet },
@@ -226,6 +226,7 @@ const clivalue_t valueTable[] = {
     { "min_throttle",               VAR_UINT16 | MASTER_VALUE,  &masterConfig.escAndServoConfig.minthrottle, PWM_RANGE_ZERO, PWM_RANGE_MAX },
     { "max_throttle",               VAR_UINT16 | MASTER_VALUE,  &masterConfig.escAndServoConfig.maxthrottle, PWM_RANGE_ZERO, PWM_RANGE_MAX },
     { "min_command",                VAR_UINT16 | MASTER_VALUE,  &masterConfig.escAndServoConfig.mincommand, PWM_RANGE_ZERO, PWM_RANGE_MAX },
+    { "servo_center_pulse",         VAR_UINT16 | MASTER_VALUE,  &masterConfig.escAndServoConfig.servoCenterPulse, PWM_RANGE_ZERO, PWM_RANGE_MAX },
 
     { "3d_deadband_low",            VAR_UINT16 | MASTER_VALUE,  &masterConfig.flight3DConfig.deadband3d_low, PWM_RANGE_ZERO, PWM_RANGE_MAX }, // FIXME upper limit should match code in the mixer, 1500 currently
     { "3d_deadband_high",           VAR_UINT16 | MASTER_VALUE,  &masterConfig.flight3DConfig.deadband3d_high, PWM_RANGE_ZERO, PWM_RANGE_MAX }, // FIXME lower limit should match code in the mixer, 1500 currently,
@@ -248,7 +249,7 @@ const clivalue_t valueTable[] = {
     { "serial_port_2_scenario",     VAR_UINT8  | MASTER_VALUE,  &masterConfig.serialConfig.serial_port_scenario[1], 0, SERIAL_PORT_SCENARIO_MAX },
 #if (SERIAL_PORT_COUNT > 2)
     { "serial_port_3_scenario",     VAR_UINT8  | MASTER_VALUE,  &masterConfig.serialConfig.serial_port_scenario[2], 0, SERIAL_PORT_SCENARIO_MAX },
-#if !defined(CC3D)
+#if (SERIAL_PORT_COUNT > 3)
     { "serial_port_4_scenario",     VAR_UINT8  | MASTER_VALUE,  &masterConfig.serialConfig.serial_port_scenario[3], 0, SERIAL_PORT_SCENARIO_MAX },
 #if (SERIAL_PORT_COUNT > 4)
     { "serial_port_5_scenario",     VAR_UINT8  | MASTER_VALUE,  &masterConfig.serialConfig.serial_port_scenario[4], 0, SERIAL_PORT_SCENARIO_MAX },
@@ -302,10 +303,10 @@ const clivalue_t valueTable[] = {
     { "vbat_max_cell_voltage",      VAR_UINT8  | MASTER_VALUE,  &masterConfig.batteryConfig.vbatmaxcellvoltage, 10, 50 },
     { "vbat_min_cell_voltage",      VAR_UINT8  | MASTER_VALUE,  &masterConfig.batteryConfig.vbatmincellvoltage, 10, 50 },
     { "vbat_warning_cell_voltage",  VAR_UINT8  | MASTER_VALUE,  &masterConfig.batteryConfig.vbatwarningcellvoltage, 10, 50 },
-    { "current_meter_scale",        VAR_UINT16 | MASTER_VALUE,  &masterConfig.batteryConfig.currentMeterScale, 1, 10000 },
+    { "current_meter_scale",        VAR_INT16 | MASTER_VALUE,  &masterConfig.batteryConfig.currentMeterScale, -10000, 10000 },
-    { "current_meter_offset",       VAR_UINT16 | MASTER_VALUE,  &masterConfig.batteryConfig.currentMeterOffset, 0, 1650 },
+    { "current_meter_offset",       VAR_UINT16 | MASTER_VALUE,  &masterConfig.batteryConfig.currentMeterOffset, 0, 3300 },
     { "multiwii_current_meter_output", VAR_UINT8  | MASTER_VALUE,  &masterConfig.batteryConfig.multiwiiCurrentMeterOutput, 0, 1 },
-
+    { "current_meter_type",         VAR_UINT8  | MASTER_VALUE,  &masterConfig.batteryConfig.currentMeterType, 0, CURRENT_SENSOR_MAX },
 
     { "align_gyro",                 VAR_UINT8  | MASTER_VALUE,  &masterConfig.sensorAlignmentConfig.gyro_align, 0, 8 },
     { "align_acc",                  VAR_UINT8  | MASTER_VALUE,  &masterConfig.sensorAlignmentConfig.acc_align, 0, 8 },
@@ -371,7 +372,7 @@ const clivalue_t valueTable[] = {
     { "mag_hardware",               VAR_UINT8  | MASTER_VALUE,  &masterConfig.mag_hardware, 0, MAG_NONE },
     { "mag_declination",            VAR_INT16  | PROFILE_VALUE, &masterConfig.profile[0].mag_declination, -18000, 18000 },
 
-    { "pid_controller",             VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].pidController, 0, 2 },
+    { "pid_controller",             VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.pidController, 0, 5 },
 
     { "p_pitch",                    VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.P8[PITCH], 0, 200 },
     { "i_pitch",                    VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I8[PITCH], 0, 200 },
@@ -395,6 +396,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 },
@@ -408,8 +410,10 @@ const clivalue_t valueTable[] = {
     { "i_vel",                      VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I8[PIDVEL], 0, 200 },
     { "d_vel",                      VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.D8[PIDVEL], 0, 200 },
 
+#ifdef BLACKBOX
     { "blackbox_rate_num",          VAR_UINT8  | MASTER_VALUE,  &masterConfig.blackbox_rate_num, 1, 32 },
     { "blackbox_rate_denom",        VAR_UINT8  | MASTER_VALUE,  &masterConfig.blackbox_rate_denom, 1, 32 },
+#endif
 };
 
 #define VALUE_COUNT (sizeof(valueTable) / sizeof(clivalue_t))

src/main/io/serial_msp.c

@@ -45,6 +45,8 @@
 #include "flight/mixer.h"
 #include "flight/failsafe.h"
 #include "flight/navigation.h"
+#include "flight/altitudehold.h"
+
 #include "rx/rx.h"
 #include "rx/msp.h"
 #include "io/escservo.h"
@@ -54,9 +56,11 @@
 #include "io/serial.h"
 #include "io/ledstrip.h"
 #include "telemetry/telemetry.h"
+
 #include "sensors/boardalignment.h"
 #include "sensors/sensors.h"
 #include "sensors/battery.h"
+#include "sensors/sonar.h"
 #include "sensors/acceleration.h"
 #include "sensors/barometer.h"
 #include "sensors/compass.h"
@@ -121,7 +125,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                   5 // increment when any change is made, reset to zero when major changes are released after changing API_VERSION_MAJOR
 
 #define API_VERSION_LENGTH                  2
 
@@ -200,6 +204,13 @@ const char *boardIdentifier = TARGET_BOARD_IDENTIFIER;
 #define MSP_CF_SERIAL_CONFIG            54
 #define MSP_SET_CF_SERIAL_CONFIG        55
 
+#define MSP_VOLTAGE_METER_CONFIG        56
+#define MSP_SET_VOLTAGE_METER_CONFIG    57
+
+#define MSP_SONAR_ALTITUDE              58 //out message get sonar altitude [cm]
+
+#define MSP_PID_CONTROLLER              59
+#define MSP_SET_PID_CONTROLLER          60
 //
 // Baseflight MSP commands (if enabled they exist in Cleanflight)
 //
@@ -365,7 +376,7 @@ static mspPort_t mspPorts[MAX_MSP_PORT_COUNT];
 
 static mspPort_t *currentPort;
 
-void serialize32(uint32_t a)
+static void serialize32(uint32_t a)
 {
     static uint8_t t;
     t = a;
@@ -382,7 +393,7 @@ void serialize32(uint32_t a)
     currentPort->checksum ^= t;
 }
 
-void serialize16(int16_t a)
+static void serialize16(int16_t a)
 {
     static uint8_t t;
     t = a;
@@ -393,32 +404,32 @@ void serialize16(int16_t a)
     currentPort->checksum ^= t;
 }
 
-void serialize8(uint8_t a)
+static void serialize8(uint8_t a)
 {
     serialWrite(mspSerialPort, a);
     currentPort->checksum ^= a;
 }
 
-uint8_t read8(void)
+static uint8_t read8(void)
 {
     return currentPort->inBuf[currentPort->indRX++] & 0xff;
 }
 
-uint16_t read16(void)
+static uint16_t read16(void)
 {
     uint16_t t = read8();
     t += (uint16_t)read8() << 8;
     return t;
 }
 
-uint32_t read32(void)
+static uint32_t read32(void)
 {
     uint32_t t = read16();
     t += (uint32_t)read16() << 16;
     return t;
 }
 
-void headSerialResponse(uint8_t err, uint8_t s)
+static void headSerialResponse(uint8_t err, uint8_t s)
 {
     serialize8('$');
     serialize8('M');
@@ -428,36 +439,36 @@ void headSerialResponse(uint8_t err, uint8_t s)
     serialize8(currentPort->cmdMSP);
 }
 
-void headSerialReply(uint8_t s)
+static void headSerialReply(uint8_t s)
 {
     headSerialResponse(0, s);
 }
 
-void headSerialError(uint8_t s)
+static void headSerialError(uint8_t s)
 {
     headSerialResponse(1, s);
 }
 
-void tailSerialReply(void)
+static void tailSerialReply(void)
 {
     serialize8(currentPort->checksum);
 }
 
-void s_struct(uint8_t *cb, uint8_t siz)
+static void s_struct(uint8_t *cb, uint8_t siz)
 {
     headSerialReply(siz);
     while (siz--)
         serialize8(*cb++);
 }
 
-void serializeNames(const char *s)
+static void serializeNames(const char *s)
 {
     const char *c;
     for (c = s; *c; c++)
         serialize8(*c);
 }
 
-const box_t *findBoxByActiveBoxId(uint8_t activeBoxId)
+static const box_t *findBoxByActiveBoxId(uint8_t activeBoxId)
 {
     uint8_t boxIndex;
     const box_t *candidate;
@@ -470,7 +481,7 @@ const box_t *findBoxByActiveBoxId(uint8_t activeBoxId)
     return NULL;
 }
 
-const box_t *findBoxByPermenantId(uint8_t permenantId)
+static const box_t *findBoxByPermenantId(uint8_t permenantId)
 {
     uint8_t boxIndex;
     const box_t *candidate;
@@ -483,7 +494,7 @@ const box_t *findBoxByPermenantId(uint8_t permenantId)
     return NULL;
 }
 
-void serializeBoxNamesReply(void)
+static void serializeBoxNamesReply(void)
 {
     int i, activeBoxId, j, flag = 1, count = 0, len;
     const box_t *box;
@@ -617,6 +628,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;
 
@@ -643,7 +660,6 @@ static bool processOutCommand(uint8_t cmdMSP)
 {
     uint32_t i, tmp, junk;
 
-
 #ifdef GPS
     uint8_t wp_no;
     int32_t lat = 0, lon = 0;
@@ -746,6 +762,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 |
@@ -811,18 +828,30 @@ static bool processOutCommand(uint8_t cmdMSP)
         break;
     case MSP_ALTITUDE:
         headSerialReply(6);
-        serialize32(EstAlt);
+#if defined(BARO) || defined(SONAR)
+        serialize32(altitudeHoldGetEstimatedAltitude());
+#else
+        serialize32(0);
+#endif
         serialize16(vario);
         break;
+    case MSP_SONAR_ALTITUDE:
+        headSerialReply(4);
+#if defined(SONAR)
+        serialize32(sonarGetLatestAltitude());
+#else
+        serialize32(0);
+#endif
+        break;
     case MSP_ANALOG:
         headSerialReply(7);
         serialize8((uint8_t)constrain(vbat, 0, 255));
         serialize16((uint16_t)constrain(mAhDrawn, 0, 0xFFFF)); // milliamphours drawn from battery
         serialize16(rssi);
         if(masterConfig.batteryConfig.multiwiiCurrentMeterOutput) {
-            serialize16((uint16_t)constrain((abs(amperage) * 10), 0, 0xFFFF)); // send amperage in 0.001 A steps
+            serialize16((uint16_t)constrain(amperage * 10, 0, 0xFFFF)); // send amperage in 0.001 A steps. Negative range is truncated to zero
         } else
-            serialize16((uint16_t)constrain(abs(amperage), 0, 0xFFFF)); // send amperage in 0.01 A steps
+            serialize16((int16_t)constrain(amperage, -0x8000, 0x7FFF)); // send amperage in 0.01 A steps, range is -320A to 320A
         break;
     case MSP_RC_TUNING:
         headSerialReply(7);
@@ -836,7 +865,7 @@ static bool processOutCommand(uint8_t cmdMSP)
         break;
     case MSP_PID:
         headSerialReply(3 * PID_ITEM_COUNT);
-        if (currentProfile->pidController == 2) { // convert float stuff into uint8_t to keep backwards compatability with all 8-bit shit with new pid
+        if (IS_PID_CONTROLLER_FP_BASED(currentProfile->pidProfile.pidController)) { // convert float stuff into uint8_t to keep backwards compatability with all 8-bit shit with new pid
             for (i = 0; i < 3; i++) {
                 serialize8(constrain(lrintf(currentProfile->pidProfile.P_f[i] * 10.0f), 0, 250));
                 serialize8(constrain(lrintf(currentProfile->pidProfile.I_f[i] * 100.0f), 0, 250));
@@ -846,7 +875,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]);
@@ -865,6 +894,10 @@ static bool processOutCommand(uint8_t cmdMSP)
         headSerialReply(sizeof(pidnames) - 1);
         serializeNames(pidnames);
         break;
+    case MSP_PID_CONTROLLER:
+        headSerialReply(1);
+        serialize8(currentProfile->pidProfile.pidController);
+        break;
     case MSP_MODE_RANGES:
         headSerialReply(4 * MAX_MODE_ACTIVATION_CONDITION_COUNT);
         for (i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) {
@@ -1017,16 +1050,26 @@ static bool processOutCommand(uint8_t cmdMSP)
         break;
 
     case MSP_BOARD_ALIGNMENT:
-        headSerialReply(3);
+        headSerialReply(6);
         serialize16(masterConfig.boardAlignment.rollDegrees);
         serialize16(masterConfig.boardAlignment.pitchDegrees);
         serialize16(masterConfig.boardAlignment.yawDegrees);
         break;
 
-    case MSP_CURRENT_METER_CONFIG:
+    case MSP_VOLTAGE_METER_CONFIG:
         headSerialReply(4);
+        serialize8(masterConfig.batteryConfig.vbatscale);
+        serialize8(masterConfig.batteryConfig.vbatmincellvoltage);
+        serialize8(masterConfig.batteryConfig.vbatmaxcellvoltage);
+        serialize8(masterConfig.batteryConfig.vbatwarningcellvoltage);
+        break;
+
+    case MSP_CURRENT_METER_CONFIG:
+        headSerialReply(7);
         serialize16(masterConfig.batteryConfig.currentMeterScale);
         serialize16(masterConfig.batteryConfig.currentMeterOffset);
+        serialize8(masterConfig.batteryConfig.currentMeterType);
+        serialize16(masterConfig.batteryConfig.batteryCapacity);
         break;
 
     case MSP_MIXER:
@@ -1035,7 +1078,7 @@ static bool processOutCommand(uint8_t cmdMSP)
         break;
 
     case MSP_RX_CONFIG:
-        headSerialReply(7);
+        headSerialReply(8);
         serialize8(masterConfig.rxConfig.serialrx_provider);
         serialize16(masterConfig.rxConfig.maxcheck);
         serialize16(masterConfig.rxConfig.midrc);
@@ -1097,13 +1140,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
@@ -1160,8 +1204,12 @@ static bool processInCommand(void)
         currentProfile->accelerometerTrims.values.pitch = read16();
         currentProfile->accelerometerTrims.values.roll  = read16();
         break;
+    case MSP_SET_PID_CONTROLLER:
+        currentProfile->pidProfile.pidController = read8();
+        setPIDController(currentProfile->pidProfile.pidController);
+        break;
     case MSP_SET_PID:
-        if (currentProfile->pidController == 2) {
+        if (IS_PID_CONTROLLER_FP_BASED(currentProfile->pidProfile.pidController)) {
             for (i = 0; i < 3; i++) {
                 currentProfile->pidProfile.P_f[i] = (float)read8() / 10.0f;
                 currentProfile->pidProfile.I_f[i] = (float)read8() / 100.0f;
@@ -1171,7 +1219,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();
@@ -1362,9 +1410,18 @@ static bool processInCommand(void)
         masterConfig.boardAlignment.yawDegrees = read16();
         break;
 
+    case MSP_SET_VOLTAGE_METER_CONFIG:
+        masterConfig.batteryConfig.vbatscale = read8();           // actual vbatscale as intended
+        masterConfig.batteryConfig.vbatmincellvoltage = read8();  // vbatlevel_warn1 in MWC2.3 GUI
+        masterConfig.batteryConfig.vbatmaxcellvoltage = read8();  // vbatlevel_warn2 in MWC2.3 GUI
+        masterConfig.batteryConfig.vbatwarningcellvoltage = read8();  // vbatlevel when buzzer starts to alert
+        break;
+
     case MSP_SET_CURRENT_METER_CONFIG:
         masterConfig.batteryConfig.currentMeterScale = read16();
         masterConfig.batteryConfig.currentMeterOffset = read16();
+        masterConfig.batteryConfig.currentMeterType = read8();
+        masterConfig.batteryConfig.batteryCapacity = read16();
         break;
 
 #ifndef USE_QUAD_MIXER_ONLY
@@ -1442,28 +1499,30 @@ static bool processInCommand(void)
 
     case MSP_SET_LED_STRIP_CONFIG:
         {
-            uint8_t ledCount = currentPort->dataSize / 6;
+            i = read8();
-            if (ledCount != MAX_LED_STRIP_LENGTH) {
+            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];
-                ledConfig_t *ledConfig = &masterConfig.ledConfigs[i];
+            uint16_t mask;
-                uint16_t mask;
+            // currently we're storing directions and functions in a uint16 (flags)
-                // currently we're storing directions and functions in a uint16 (flags)
+            // the msp uses 2 x uint16_t to cater for future expansion
-                // the msp uses 2 x uint16_t to cater for future expansion
+            mask = read16();
-                mask = read16();
+            ledConfig->flags = (mask << LED_DIRECTION_BIT_OFFSET) & LED_DIRECTION_MASK;
-                ledConfig->flags = (mask << LED_DIRECTION_BIT_OFFSET) & LED_DIRECTION_MASK;
 
-                mask = read16();
+            mask = read16();
-                ledConfig->flags |= (mask << LED_FUNCTION_BIT_OFFSET) & LED_FUNCTION_MASK;
+            ledConfig->flags |= (mask << LED_FUNCTION_BIT_OFFSET) & LED_FUNCTION_MASK;
 
-                mask = read8();
+            mask = read8();
-                ledConfig->xy = CALCULATE_LED_X(mask);
+            ledConfig->xy = CALCULATE_LED_X(mask);
 
-                mask = read8();
+            mask = read8();
-                ledConfig->xy |= CALCULATE_LED_Y(mask);
+            ledConfig->xy |= CALCULATE_LED_Y(mask);
-            }
+
+            ledConfig->color = read8();
+
+            reevalulateLedConfig();
         }
         break;
 #endif

src/main/io/serial_msp.h

@@ -17,9 +17,14 @@
 
 #pragma once
 
+#include "io/serial.h"
+#include "drivers/serial.h"
+
 // Each MSP port requires state and a receive buffer, revisit this default if someone needs more than 2 MSP ports.
 #define MAX_MSP_PORT_COUNT 2
 
+void mspInit(serialConfig_t *serialConfig);
+
 void mspProcess(void);
 void sendMspTelemetry(void);
 void mspSetTelemetryPort(serialPort_t *mspTelemetryPort);

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
@@ -287,7 +290,8 @@ void init(void)
     pwm_params.useSoftSerial = feature(FEATURE_SOFTSERIAL);
     pwm_params.useParallelPWM = feature(FEATURE_RX_PARALLEL_PWM);
     pwm_params.useRSSIADC = feature(FEATURE_RSSI_ADC);
-    pwm_params.useCurrentMeterADC = feature(FEATURE_CURRENT_METER);
+    pwm_params.useCurrentMeterADC = feature(FEATURE_CURRENT_METER)
+        && masterConfig.batteryConfig.currentMeterType == CURRENT_SENSOR_ADC;
     pwm_params.useLEDStrip = feature(FEATURE_LED_STRIP);
     pwm_params.usePPM = feature(FEATURE_RX_PPM);
     pwm_params.useOneshot = feature(FEATURE_ONESHOT125);
@@ -301,7 +305,7 @@ void init(void)
         pwm_params.idlePulse = masterConfig.flight3DConfig.neutral3d;
     if (pwm_params.motorPwmRate > 500)
         pwm_params.idlePulse = 0; // brushed motors
-    pwm_params.servoCenterPulse = masterConfig.rxConfig.midrc;
+    pwm_params.servoCenterPulse = masterConfig.escAndServoConfig.servoCenterPulse;
 
     pwmRxInit(masterConfig.inputFilteringMode);
 
@@ -328,7 +332,7 @@ void init(void)
 
 #ifdef SONAR
     if (feature(FEATURE_SONAR)) {
-        Sonar_init();
+        sonarInit();
     }
 #endif
 

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;
 
@@ -123,7 +124,7 @@ void updateAutotuneState(void)
                     autotuneReset();
                     landedAfterAutoTuning = false;
                 }
-                autotuneBeginNextPhase(&currentProfile->pidProfile, currentProfile->pidController);
+                autotuneBeginNextPhase(&currentProfile->pidProfile);
                 ENABLE_FLIGHT_MODE(AUTOTUNE_MODE);
                 autoTuneWasUsed = true;
             } else {
@@ -150,8 +151,8 @@ void updateAutotuneState(void)
 bool isCalibrating()
 {
 #ifdef BARO
-    if (sensors(SENSOR_ACC) && !isBaroCalibrationComplete()) {
+    if (sensors(SENSOR_BARO) && !isBaroCalibrationComplete()) {
-        return false;
+        return true;
     }
 #endif
 
@@ -181,7 +182,7 @@ void annexCode(void)
     }
 
     for (axis = 0; axis < 3; axis++) {
-        tmp = min(abs(rcData[axis] - masterConfig.rxConfig.midrc), 500);
+        tmp = MIN(ABS(rcData[axis] - masterConfig.rxConfig.midrc), 500);
         if (axis == ROLL || axis == PITCH) {
             if (currentProfile->rcControlsConfig.deadband) {
                 if (tmp > currentProfile->rcControlsConfig.deadband) {
@@ -204,7 +205,7 @@ void annexCode(void)
                 }
             }
             rcCommand[axis] = tmp * -masterConfig.yaw_control_direction;
-            prop1 = 100 - (uint16_t)currentControlRateProfile->yawRate * abs(tmp) / 500;
+            prop1 = 100 - (uint16_t)currentControlRateProfile->yawRate * ABS(tmp) / 500;
         }
         // FIXME axis indexes into pids.  use something like lookupPidIndex(rc_alias_e alias) to reduce coupling.
         dynP8[axis] = (uint16_t)currentProfile->pidProfile.P8[axis] * prop1 / 100;
@@ -311,9 +312,6 @@ void mwDisarm(void)
 #ifdef BLACKBOX
         if (feature(FEATURE_BLACKBOX)) {
             finishBlackbox();
-            if (isSerialPortFunctionShared(FUNCTION_BLACKBOX, FUNCTION_MSP)) {
-                mspAllocateSerialPorts(&masterConfig.serialConfig);
-            }
         }
 #endif
     }
@@ -399,7 +397,7 @@ void updateInflightCalibrationState(void)
 
 void updateMagHold(void)
 {
-    if (abs(rcCommand[YAW]) < 70 && FLIGHT_MODE(MAG_MODE)) {
+    if (ABS(rcCommand[YAW]) < 70 && FLIGHT_MODE(MAG_MODE)) {
         int16_t dif = heading - magHold;
         if (dif <= -180)
             dif += 360;
@@ -471,7 +469,7 @@ void executePeriodicTasks(void)
 #ifdef SONAR
     case UPDATE_SONAR_TASK:
         if (sensors(SENSOR_SONAR)) {
-            Sonar_update();
+            sonarUpdate();
         }
         break;
 #endif
@@ -712,7 +710,8 @@ void loop(void)
             &currentProfile->pidProfile,
             currentControlRateProfile,
             masterConfig.max_angle_inclination,
-            &currentProfile->accelerometerTrims
+            &currentProfile->accelerometerTrims,
+            &masterConfig.rxConfig
         );
 
         mixTable();

src/main/rx/rx.c

@@ -21,6 +21,8 @@
 
 #include <string.h>
 
+#include "build_config.h"
+
 #include "platform.h"
 
 #include "common/maths.h"
@@ -59,7 +61,7 @@ bool rxMspInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadR
 
 const char rcChannelLetters[] = "AERT12345678abcdefgh";
 
-uint16_t rssi;                  // range: [0;1023]
+uint16_t rssi = 0;                  // range: [0;1023]
 
 int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];     // interval [1000;2000]
 
@@ -102,6 +104,7 @@ void updateSerialRxFunctionConstraint(functionConstraint_t *functionConstraintTo
             sumhUpdateSerialRxFunctionConstraint(functionConstraintToUpdate);
             break;
         case SERIALRX_XBUS_MODE_B:
+        case SERIALRX_XBUS_MODE_B_RJ01:
             xBusUpdateSerialRxFunctionConstraint(functionConstraintToUpdate);
             break;
     }
@@ -158,6 +161,7 @@ void serialRxInit(rxConfig_t *rxConfig)
             enabled = sumhInit(rxConfig, &rxRuntimeConfig, &rcReadRawFunc);
             break;
         case SERIALRX_XBUS_MODE_B:
+        case SERIALRX_XBUS_MODE_B_RJ01:
             enabled = xBusInit(rxConfig, &rxRuntimeConfig, &rcReadRawFunc);
             break;
     }
@@ -170,7 +174,15 @@ void serialRxInit(rxConfig_t *rxConfig)
 
 bool isSerialRxFrameComplete(rxConfig_t *rxConfig)
 {
-
+    /**
+     * FIXME: Each of the xxxxFrameComplete() methods MUST be able to survive being called without the
+     * corresponding xxxInit() method having been called first.
+     *
+     * This situation arises when the cli or the msp changes the value of rxConfig->serialrx_provider
+     *
+     * A solution is for the ___Init() to configure the serialRxFrameComplete function pointer which
+     * should be used instead of the switch statement below.
+     */
     switch (rxConfig->serialrx_provider) {
         case SERIALRX_SPEKTRUM1024:
         case SERIALRX_SPEKTRUM2048:
@@ -182,6 +194,7 @@ bool isSerialRxFrameComplete(rxConfig_t *rxConfig)
         case SERIALRX_SUMH:
             return sumhFrameComplete();
         case SERIALRX_XBUS_MODE_B:
+        case SERIALRX_XBUS_MODE_B_RJ01:
             return xBusFrameComplete();
     }
     return false;
@@ -289,7 +302,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
@@ -366,6 +379,9 @@ void updateRSSIPWM(void)
 
 void updateRSSIADC(uint32_t currentTime)
 {
+#ifndef USE_ADC
+    UNUSED(currentTime);
+#else
     static uint8_t adcRssiSamples[RSSI_ADC_SAMPLE_COUNT];
     static uint8_t adcRssiSampleIndex = 0;
     static uint32_t rssiUpdateAt = 0;
@@ -392,6 +408,7 @@ void updateRSSIADC(uint32_t currentTime)
     adcRssiMean = adcRssiMean / RSSI_ADC_SAMPLE_COUNT;
 
     rssi = (uint16_t)((constrain(adcRssiMean, 0, 100) / 100.0f) * 1023.0f);
+#endif
 }
 
 void updateRSSI(uint32_t currentTime)

src/main/rx/rx.h

@@ -33,7 +33,8 @@ typedef enum {
     SERIALRX_SUMD = 3,
     SERIALRX_SUMH = 4,
     SERIALRX_XBUS_MODE_B = 5,
-    SERIALRX_PROVIDER_MAX = SERIALRX_XBUS_MODE_B
+    SERIALRX_XBUS_MODE_B_RJ01 = 6,
+    SERIALRX_PROVIDER_MAX = SERIALRX_XBUS_MODE_B_RJ01
 } SerialRXType;
 
 #define SERIALRX_PROVIDER_COUNT (SERIALRX_PROVIDER_MAX + 1)
@@ -67,11 +68,11 @@ typedef struct rxConfig_s {
     uint8_t rcmap[MAX_MAPPABLE_RX_INPUTS];  // mapping of radio channels to internal RPYTA+ order
     uint8_t serialrx_provider;              // type of UART-based receiver (0 = spek 10, 1 = spek 11, 2 = sbus). Must be enabled by FEATURE_RX_SERIAL first.
     uint8_t spektrum_sat_bind;              // number of bind pulses for Spektrum satellite receivers
+    uint8_t rssi_channel;
+    uint8_t rssi_scale;
     uint16_t midrc;                         // Some radios have not a neutral point centered on 1500. can be changed here
     uint16_t mincheck;                      // minimum rc end
     uint16_t maxcheck;                      // maximum rc end
-    uint8_t rssi_channel;
-    uint8_t rssi_scale;
 } rxConfig_t;
 
 #define REMAPPABLE_CHANNEL_COUNT (sizeof(((rxConfig_t *)0)->rcmap) / sizeof(((rxConfig_t *)0)->rcmap[0]))

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;

src/main/rx/xbus.c

@@ -35,17 +35,21 @@
 //
 
 #define XBUS_CHANNEL_COUNT 12
+#define XBUS_RJ01_CHANNEL_COUNT 12
 
 // Frame is: ID(1 byte) + 12*channel(2 bytes) + CRC(2 bytes) = 27
 #define XBUS_FRAME_SIZE 27
-#define XBUS_CRC_BYTE_1 25
+
-#define XBUS_CRC_BYTE_2 26
+#define XBUS_RJ01_FRAME_SIZE 33
+#define XBUS_RJ01_MESSAGE_LENGTH 30
+#define XBUS_RJ01_OFFSET_BYTES 3
 
 #define XBUS_CRC_AND_VALUE 0x8000
 #define XBUS_CRC_POLY 0x1021
 
 #define XBUS_BAUDRATE 115200
-#define XBUS_MAX_FRAME_TIME 5000
+#define XBUS_RJ01_BAUDRATE 250000
+#define XBUS_MAX_FRAME_TIME 8000
 
 // NOTE!
 // This is actually based on ID+LENGTH (nibble each)
@@ -68,9 +72,14 @@
 static bool xBusFrameReceived = false;
 static bool xBusDataIncoming = false;
 static uint8_t xBusFramePosition;
+static uint8_t xBusFrameLength;
+static uint8_t xBusChannelCount;
+static uint8_t xBusProvider;
 
-static volatile uint8_t xBusFrame[XBUS_FRAME_SIZE];
+
-static uint16_t xBusChannelData[XBUS_CHANNEL_COUNT];
+// Use max values for ram areas
+static volatile uint8_t xBusFrame[XBUS_RJ01_FRAME_SIZE];
+static uint16_t xBusChannelData[XBUS_RJ01_CHANNEL_COUNT];
 
 static void xBusDataReceive(uint16_t c);
 static uint16_t xBusReadRawRC(rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan);
@@ -80,25 +89,41 @@ static serialPort_t *xBusPort;
 void xBusUpdateSerialRxFunctionConstraint(functionConstraint_t *functionConstraint)
 {
     functionConstraint->minBaudRate = XBUS_BAUDRATE;
-    functionConstraint->maxBaudRate = XBUS_BAUDRATE;
+    functionConstraint->maxBaudRate = XBUS_RJ01_BAUDRATE;
     functionConstraint->requiredSerialPortFeatures = SPF_SUPPORTS_CALLBACK;
 }
 
 bool xBusInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback)
 {
+    uint32_t baudRate;
+
     switch (rxConfig->serialrx_provider) {
         case SERIALRX_XBUS_MODE_B:
             rxRuntimeConfig->channelCount = XBUS_CHANNEL_COUNT;
             xBusFrameReceived = false;
             xBusDataIncoming = false;
             xBusFramePosition = 0;
+            baudRate = XBUS_BAUDRATE;
+            xBusFrameLength = XBUS_FRAME_SIZE;
+            xBusChannelCount = XBUS_CHANNEL_COUNT;
+            xBusProvider = SERIALRX_XBUS_MODE_B;
+            break;
+        case SERIALRX_XBUS_MODE_B_RJ01:
+            rxRuntimeConfig->channelCount = XBUS_RJ01_CHANNEL_COUNT;
+            xBusFrameReceived = false;
+            xBusDataIncoming = false;
+            xBusFramePosition = 0;
+            baudRate = XBUS_RJ01_BAUDRATE;
+            xBusFrameLength = XBUS_RJ01_FRAME_SIZE;
+            xBusChannelCount = XBUS_RJ01_CHANNEL_COUNT;
+            xBusProvider = SERIALRX_XBUS_MODE_B_RJ01;
             break;
         default:
             return false;
             break;
     }
 
-    xBusPort = openSerialPort(FUNCTION_SERIAL_RX, xBusDataReceive, XBUS_BAUDRATE, MODE_RX, SERIAL_NOT_INVERTED);
+    xBusPort = openSerialPort(FUNCTION_SERIAL_RX, xBusDataReceive, baudRate, MODE_RX, SERIAL_NOT_INVERTED);
     if (callback) {
         *callback = xBusReadRawRC;
     }
@@ -123,7 +148,31 @@ static uint16_t xBusCRC16(uint16_t crc, uint8_t value)
     return crc;
 }
 
-static void xBusUnpackFrame(void)
+// Full RJ01 message CRC calculations
+uint8_t xBusRj01CRC8(uint8_t inData, uint8_t seed)
+{
+    uint8_t bitsLeft;
+    uint8_t temp;
+
+    for (bitsLeft = 8; bitsLeft > 0; bitsLeft--) {
+        temp = ((seed ^ inData) & 0x01);
+
+        if (temp == 0) {
+            seed >>= 1;
+        } else {
+            seed ^= 0x18;
+            seed >>= 1;
+            seed |= 0x80;
+        }
+
+        inData >>= 1;
+    }
+
+    return seed;    
+}
+
+
+static void xBusUnpackModeBFrame(uint8_t offsetBytes)
 {
     // Calculate the CRC of the incoming frame
     uint16_t crc = 0;
@@ -134,18 +183,18 @@ static void xBusUnpackFrame(void)
 
     // Calculate on all bytes except the final two CRC bytes
     for (i = 0; i < XBUS_FRAME_SIZE - 2; i++) {
-        inCrc = xBusCRC16(inCrc, xBusFrame[i]);
+        inCrc = xBusCRC16(inCrc, xBusFrame[i+offsetBytes]);
     }
 
     // Get the received CRC
-    crc = ((uint16_t)xBusFrame[XBUS_CRC_BYTE_1]) << 8;
+    crc = ((uint16_t)xBusFrame[offsetBytes + XBUS_FRAME_SIZE - 2]) << 8;
-    crc = crc + ((uint16_t)xBusFrame[XBUS_CRC_BYTE_2]);
+    crc = crc + ((uint16_t)xBusFrame[offsetBytes + XBUS_FRAME_SIZE - 1]);
 
     if (crc == inCrc) {
         // Unpack the data, we have a valid frame
-        for (i = 0; i < XBUS_CHANNEL_COUNT; i++) {
+        for (i = 0; i < xBusChannelCount; i++) {
 
-            frameAddr = 1 + i * 2;
+            frameAddr = offsetBytes + 1 + i * 2;
             value = ((uint16_t)xBusFrame[frameAddr]) << 8;
             value = value + ((uint16_t)xBusFrame[frameAddr + 1]);
 
@@ -158,9 +207,68 @@ static void xBusUnpackFrame(void)
 
 }
 
+static void xBusUnpackRJ01Frame(void)
+{
+    // Calculate the CRC of the incoming frame
+    uint8_t outerCrc = 0;
+    uint8_t i = 0;
+
+    // When using the Align RJ01 receiver with 
+    // a MODE B setting in the radio (XG14 tested)
+    // the MODE_B -frame is packed within some
+    // at the moment unknown bytes before and after:
+    // 0xA1 LEN __ 0xA1 12*(High + Low) CRC1 CRC2 + __ __ CRC_OUTER
+    // Compared to a standard MODE B frame that only
+    // contains the "middle" package.
+    // Hence, at the moment, the unknown header and footer
+    // of the RJ01 MODEB packages are discarded. 
+    // However, the LAST byte (CRC_OUTER) is infact an 8-bit
+    // CRC for the whole package, using the Dallas-One-Wire CRC
+    // method.
+    // So, we check both these values as well as the provided length
+    // of the outer/full message (LEN)
+    
+    //
+    // Check we have correct length of message
+    //
+    if (xBusFrame[1] != XBUS_RJ01_MESSAGE_LENGTH)
+    {
+        // Unknown package as length is not ok
+        return;
+    }
+    
+    //
+    // CRC calculation & check for full message
+    //
+    for (i = 0; i < xBusFrameLength - 1; i++) {
+        outerCrc = xBusRj01CRC8(outerCrc, xBusFrame[i]);
+    }
+    
+    if (outerCrc != xBusFrame[xBusFrameLength - 1])
+    {
+        // CRC does not match, skip this frame
+        return;
+    }
+
+    // Now unpack the "embedded MODE B frame"
+    xBusUnpackModeBFrame(XBUS_RJ01_OFFSET_BYTES);
+}
+
 // Receive ISR callback
 static void xBusDataReceive(uint16_t c)
 {
+    uint32_t now;
+    static uint32_t xBusTimeLast, xBusTimeInterval;
+
+    // Check if we shall reset frame position due to time
+    now = micros();
+    xBusTimeInterval = now - xBusTimeLast;
+    xBusTimeLast = now;
+    if (xBusTimeInterval > XBUS_MAX_FRAME_TIME) {
+        xBusFramePosition = 0;
+        xBusDataIncoming = false;
+    }
+    
     // Check if we shall start a frame?
     if ((xBusFramePosition == 0) && (c == XBUS_START_OF_FRAME_BYTE)) {
         xBusDataIncoming = true;
@@ -174,8 +282,13 @@ static void xBusDataReceive(uint16_t c)
     }
     
     // Done?
-    if (xBusFramePosition == XBUS_FRAME_SIZE) {
+    if (xBusFramePosition == xBusFrameLength) {
-        xBusUnpackFrame();
+        switch (xBusProvider) {
+            case SERIALRX_XBUS_MODE_B:
+                xBusUnpackModeBFrame(0);
+            case SERIALRX_XBUS_MODE_B_RJ01:
+                xBusUnpackRJ01Frame();
+        }
         xBusDataIncoming = false;
         xBusFramePosition = 0;
     }

src/main/sensors/battery.c

@@ -20,9 +20,14 @@
 
 #include "common/maths.h"
 
+#include "config/runtime_config.h"
+
 #include "drivers/adc.h"
 #include "drivers/system.h"
 
+#include "rx/rx.h"
+#include "io/rc_controls.h"
+
 #include "sensors/battery.h"
 
 // Battery monitoring stuff
@@ -31,7 +36,8 @@ uint16_t batteryWarningVoltage;
 uint16_t batteryCriticalVoltage;
 
 uint8_t vbat = 0;                   // battery voltage in 0.1V steps
-uint16_t vbatLatest = 0;            // most recent unsmoothed raw reading from vbat adc
+uint16_t vbatLatestADC = 0;         // most recent unsmoothed raw reading from vbat ADC
+uint16_t amperageLatestADC = 0;     // most recent raw reading from current ADC
 
 int32_t amperage = 0;               // amperage read by current sensor in centiampere (1/100th A)
 int32_t mAhDrawn = 0;               // milliampere hours drawn from the battery since start
@@ -55,7 +61,7 @@ void updateBatteryVoltage(void)
     uint16_t vbatSampleTotal = 0;
 
     // store the battery voltage with some other recent battery voltage readings
-    vbatSamples[(currentSampleIndex++) % BATTERY_SAMPLE_COUNT] = vbatLatest = adcGetChannel(ADC_BATTERY);
+    vbatSamples[(currentSampleIndex++) % BATTERY_SAMPLE_COUNT] = vbatLatestADC = adcGetChannel(ADC_BATTERY);
 
     // calculate vbat based on the average of recent readings
     for (index = 0; index < BATTERY_SAMPLE_COUNT; index++) {
@@ -112,13 +118,29 @@ void updateCurrentMeter(int32_t lastUpdateAt)
 {
     static int32_t amperageRaw = 0;
     static int64_t mAhdrawnRaw = 0;
+    int32_t throttleOffset = (int32_t)rcCommand[THROTTLE] - 1000;
+    int32_t throttleFactor = 0;
 
-	amperageRaw -= amperageRaw / 8;
+    switch(batteryConfig->currentMeterType) {
-	amperageRaw += adcGetChannel(ADC_CURRENT);
+        case CURRENT_SENSOR_ADC:
-	amperage = currentSensorToCentiamps(amperageRaw / 8);
+            amperageRaw -= amperageRaw / 8;
+            amperageRaw += (amperageLatestADC = adcGetChannel(ADC_CURRENT));
+            amperage = currentSensorToCentiamps(amperageRaw / 8);
+            break;
+        case CURRENT_SENSOR_VIRTUAL:
+            amperage = (int32_t)batteryConfig->currentMeterOffset;
+            if(ARMING_FLAG(ARMED)) {
+                throttleFactor = throttleOffset + (throttleOffset * throttleOffset / 50);
+                amperage += throttleFactor * (int32_t)batteryConfig->currentMeterScale  / 1000;
+            }
+            break;
+        case CURRENT_SENSOR_NONE:
+            amperage = 0;
+            break;
+    }
 
-	mAhdrawnRaw += (amperage * lastUpdateAt) / 1000;
+    mAhdrawnRaw += (amperage * lastUpdateAt) / 1000;
-	mAhDrawn = mAhdrawnRaw / (3600 * 100);
+    mAhDrawn = mAhdrawnRaw / (3600 * 100);
 }
 
 uint8_t calculateBatteryPercentage(void)

src/main/sensors/battery.h

@@ -21,14 +21,22 @@
 #define VBAT_SCALE_MIN 0
 #define VBAT_SCALE_MAX 255
 
+typedef enum {
+    CURRENT_SENSOR_NONE = 0,
+    CURRENT_SENSOR_ADC,
+    CURRENT_SENSOR_VIRTUAL,
+    CURRENT_SENSOR_MAX = CURRENT_SENSOR_VIRTUAL
+} currentSensor_e;
+
 typedef struct batteryConfig_s {
     uint8_t vbatscale;                      // adjust this to match battery voltage to reported value
     uint8_t vbatmaxcellvoltage;             // maximum voltage per cell, used for auto-detecting battery voltage in 0.1V units, default is 43 (4.3V)
     uint8_t vbatmincellvoltage;             // minimum voltage per cell, this triggers battery critical alarm, in 0.1V units, default is 33 (3.3V)
     uint8_t vbatwarningcellvoltage;         // warning voltage per cell, this triggers battery warning alarm, in 0.1V units, default is 35 (3.5V)
 
-    uint16_t currentMeterScale;             // scale the current sensor output voltage to milliamps. Value in 1/10th mV/A
+    int16_t currentMeterScale;             // scale the current sensor output voltage to milliamps. Value in 1/10th mV/A
     uint16_t currentMeterOffset;            // offset of the current sensor in millivolt steps
+    currentSensor_e  currentMeterType;      // type of current meter used, either ADC or virtual
 
     // FIXME this doesn't belong in here since it's a concern of MSP, not of the battery code.
     uint8_t multiwiiCurrentMeterOutput;     // if set to 1 output the amperage in milliamp steps instead of 0.01A steps via msp
@@ -42,9 +50,10 @@ typedef enum {
 } batteryState_e;
 
 extern uint8_t vbat;
-extern uint16_t vbatLatest;
+extern uint16_t vbatLatestADC;
 extern uint8_t batteryCellCount;
 extern uint16_t batteryWarningVoltage;
+extern uint16_t amperageLatestADC;
 extern int32_t amperage;
 extern int32_t mAhDrawn;
 

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
+#ifdef GYRO_L3GD20_ALIGN
-        gyroAlign = GYRO_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:

src/main/sensors/sonar.c

@@ -33,11 +33,13 @@
 #include "sensors/sensors.h"
 #include "sensors/sonar.h"
 
-int32_t sonarAlt = -1;                  // in cm , -1 indicate sonar is not in range - inclination adjusted by imu
+// in cm , -1 indicate sonar is not in range - inclination adjusted by imu
 
 #ifdef SONAR
 
-void Sonar_init(void)
+static int32_t calculatedAltitude;
+
+void sonarInit(void)
 {
 #if defined(NAZE) || defined(EUSTM32F103RC) || defined(PORT103R)
     static const sonarHardware_t const sonarPWM56 = {
@@ -74,25 +76,33 @@ void Sonar_init(void)
 #endif
 
     sensorsSet(SENSOR_SONAR);
-    sonarAlt = 0;
+    calculatedAltitude = -1;
 }
 
-void Sonar_update(void)
+void sonarUpdate(void)
 {
     hcsr04_start_reading();
 }
 
+int32_t sonarRead(void)
+{
+    return hcsr04_get_distance();
+}
+
 int32_t sonarCalculateAltitude(int32_t sonarAlt, int16_t tiltAngle)
 {
     // calculate sonar altitude only if the sonar is facing downwards(<25deg)
     if (tiltAngle > 250)
-        return -1;
+        calculatedAltitude = -1;
+    else
+        calculatedAltitude = sonarAlt * (900.0f - tiltAngle) / 900.0f;
 
-    return sonarAlt * (900.0f - tiltAngle) / 900.0f;
+    return calculatedAltitude;
 }
 
-int32_t sonarRead(void) {
+int32_t sonarGetLatestAltitude(void)
-    return hcsr04_get_distance();
+{
+    return calculatedAltitude;
 }
 
 #endif

src/main/sensors/sonar.h

@@ -17,10 +17,9 @@
 
 #pragma once
 
-extern int32_t sonarAlt;
+void sonarInit(void);
-
+void sonarUpdate(void);
-void Sonar_init(void);
-void Sonar_update(void);
 
 int32_t sonarRead(void);
 int32_t sonarCalculateAltitude(int32_t sonarAlt, int16_t tiltAngle);
+int32_t sonarGetLatestAltitude(void);

src/main/startup/startup_stm32f30x_md_gcc.S

@@ -70,6 +70,12 @@ defined in linker script */
   .weak  Reset_Handler
   .type  Reset_Handler, %function
 Reset_Handler:
+  ldr r0, =0x20009FFC         // HJI 11/9/2012
+  ldr r1, =0xDEADBEEF         // HJI 11/9/2012
+  ldr r2, [r0, #0]            // HJI 11/9/2012
+  str r0, [r0, #0]            // HJI 11/9/2012
+  cmp r2, r1                  // HJI 11/9/2012
+  beq Reboot_Loader           // HJI 11/9/2012
 
 /* Copy the data segment initializers from flash to SRAM */
   movs  r1, #0
@@ -104,6 +110,18 @@ LoopFillZerobss:
 /* Call the application's entry point.*/
   bl  main
   bx  lr
+
+LoopForever:
+  b LoopForever
+
+Reboot_Loader:                // HJI 11/9/2012
+
+  // Reboot to ROM            // HJI 11/9/2012
+  ldr     r0, =0x1FFFD800     // HJI 4/26/2013
+  ldr     sp,[r0, #0]         // HJI 11/9/2012
+  ldr     r0,[r0, #4]         // HJI 11/9/2012
+  bx      r0                  // HJI 11/9/2012
+
 .size  Reset_Handler, .-Reset_Handler
 
 /**

src/main/target/ALIENWIIF3/system_stm32f30x.c

@@ -55,9 +55,9 @@
   *-----------------------------------------------------------------------------
   *        APB1 Prescaler                         | 2
   *-----------------------------------------------------------------------------
-  *        HSE Frequency(Hz)                      | 12000000
+  *        HSE Frequency(Hz)                      | 8000000
   *----------------------------------------------------------------------------
-  *        PLLMUL                                 | 6
+  *        PLLMUL                                 | 9
   *-----------------------------------------------------------------------------
   *        PREDIV                                 | 1
   *-----------------------------------------------------------------------------

src/main/target/ALIENWIIF3/target.h

@@ -0,0 +1,144 @@
+/*
+ * 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 <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#define TARGET_BOARD_IDENTIFIER "AWF3" // AlienWii32 F3.
+
+#define LED0_GPIO   GPIOB
+#define LED0_PIN    Pin_4 // Blue LEDs - PB4
+#define LED0_PERIPHERAL RCC_AHBPeriph_GPIOB
+#define LED1_GPIO   GPIOB
+#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
+
+#define GYRO_MPU6050_ALIGN CW270_DEG
+
+#define ACC
+#define USE_ACC_MPU6050
+
+#define ACC_MPU6050_ALIGN CW270_DEG
+
+//#define BARO
+//#define USE_BARO_MS5611
+
+#define MAG
+#define USE_MAG_AK8975
+
+#define MAG_AK8975_ALIGN CW0_DEG_FLIP
+
+#define LED0
+#define LED1
+
+#define USE_VCP
+#define USE_USART1 // Conn 1 - TX (PB6) RX PB7 (AF7)
+#define USE_USART2 // Input - RX (PA3)
+#define USE_USART3 // Servo out - 10/RX (PB11) 11/TX (PB10)
+#define SERIAL_PORT_COUNT 4
+
+#define UART1_TX_PIN        GPIO_Pin_6 // PB6
+#define UART1_RX_PIN        GPIO_Pin_7 // PB7
+#define UART1_GPIO          GPIOB
+#define UART1_GPIO_AF       GPIO_AF_7
+#define UART1_TX_PINSOURCE  GPIO_PinSource6
+#define UART1_RX_PINSOURCE  GPIO_PinSource7
+
+#define UART2_TX_PIN        GPIO_Pin_2 // PA2 - Clashes with PWM6 input.
+#define UART2_RX_PIN        GPIO_Pin_3 // PA3
+#define UART2_GPIO          GPIOA
+#define UART2_GPIO_AF       GPIO_AF_7
+#define UART2_TX_PINSOURCE  GPIO_PinSource2
+#define UART2_RX_PINSOURCE  GPIO_PinSource3
+
+// Note: PA5 and PA0 are N/C on the sparky - potentially use for ADC or LED STRIP?
+
+#define USE_I2C
+#define I2C_DEVICE (I2CDEV_2) // SDA (PA10/AF4), SCL (PA9/AF4)
+
+#define I2C2_SCL_GPIO        GPIOA
+#define I2C2_SCL_GPIO_AF     GPIO_AF_4
+#define I2C2_SCL_PIN         GPIO_Pin_9
+#define I2C2_SCL_PIN_SOURCE  GPIO_PinSource9
+#define I2C2_SCL_CLK_SOURCE  RCC_AHBPeriph_GPIOA
+#define I2C2_SDA_GPIO        GPIOA
+#define I2C2_SDA_GPIO_AF     GPIO_AF_4
+#define I2C2_SDA_PIN         GPIO_Pin_10
+#define I2C2_SDA_PIN_SOURCE  GPIO_PinSource10
+#define I2C2_SDA_CLK_SOURCE  RCC_AHBPeriph_GPIOA
+
+
+#define SENSORS_SET (SENSOR_ACC | SENSOR_BARO | SENSOR_MAG)
+
+#define BLACKBOX
+#define SERIAL_RX
+#define GPS
+#define DISPLAY
+
+#define LED_STRIP
+#if 1
+// LED strip configuration using PWM motor output pin 5.
+#define LED_STRIP_TIMER TIM16
+
+#define USE_LED_STRIP_ON_DMA1_CHANNEL3
+#define WS2811_GPIO                     GPIOA
+#define WS2811_GPIO_AHB_PERIPHERAL      RCC_AHBPeriph_GPIOA
+#define WS2811_GPIO_AF                  GPIO_AF_1
+#define WS2811_PIN                      GPIO_Pin_6 // TIM16_CH1
+#define WS2811_PIN_SOURCE               GPIO_PinSource6
+#define WS2811_TIMER                    TIM16
+#define WS2811_TIMER_APB2_PERIPHERAL    RCC_APB2Periph_TIM16
+#define WS2811_DMA_CHANNEL              DMA1_Channel3
+#define WS2811_IRQ                      DMA1_Channel3_IRQn
+#endif
+
+#if 0
+// Alternate LED strip pin
+// FIXME DMA IRQ Transfer Complete is never called because the  TIM17_DMA_RMP needs to be set in SYSCFG_CFGR1
+#define LED_STRIP_TIMER TIM17
+
+#define USE_LED_STRIP_ON_DMA1_CHANNEL7
+#define WS2811_GPIO                     GPIOA
+#define WS2811_GPIO_AHB_PERIPHERAL      RCC_AHBPeriph_GPIOA
+#define WS2811_GPIO_AF                  GPIO_AF_1
+#define WS2811_PIN                      GPIO_Pin_7 // TIM17_CH1
+#define WS2811_PIN_SOURCE               GPIO_PinSource7
+#define WS2811_TIMER                    TIM17
+#define WS2811_TIMER_APB2_PERIPHERAL    RCC_APB2Periph_TIM17
+#define WS2811_DMA_CHANNEL              DMA1_Channel7
+#define WS2811_IRQ                      DMA1_Channel7_IRQn
+#endif
+
+
+#define SPEKTRUM_BIND
+// USART2, PA3
+#define BIND_PORT GPIOA
+#define BIND_PIN Pin_3
+
+// Hardware bind plug at PB12 (Pin 25)
+#define BINDPLUG_PORT  GPIOB
+#define BINDPLUG_PIN   Pin_12
+
+// alternative defaults for AlienWii32 F3 target
+#define ALIENWII32
+#define BRUSHED_MOTORS
+#define HARDWARE_BIND_PLUG

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,22 +47,29 @@
 
 #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_VCP
 #define USE_USART1
 #define USE_USART3
 #define USE_SOFTSERIAL1
-#define SERIAL_PORT_COUNT 3
+#define SERIAL_PORT_COUNT 4
 
 #define SOFTSERIAL_1_TIMER TIM3
 #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 +80,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

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,11 +68,29 @@
 #define USE_I2C
 #define I2C_DEVICE (I2CDEV_1)
 
+#define USE_ADC
+
 #define SENSORS_SET (SENSOR_ACC | SENSOR_BARO | SENSOR_MAG)
 
 #define GPS
 #define LED_STRIP
+#if 1
 #define LED_STRIP_TIMER TIM16
+#else
+// alternative LED strip configuration, tested working.
+#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
+#endif
 
 #define BLACKBOX
 #define TELEMETRY

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

src/main/target/MASSIVEF3/target.h

@@ -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 <http://www.gnu.org/licenses/>.
- */
-
-#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

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)
 
@@ -135,8 +152,10 @@
 #define BIND_PORT  GPIOA
 #define BIND_PIN   Pin_3
 
-// alternative defaults AlienWii32 (activate via OPTIONS="ALIENWII32" during make for NAZE target)
+// alternative defaults for AlienWii32 F1 target
 #ifdef ALIENWII32
+#undef TARGET_BOARD_IDENTIFIER
+#define TARGET_BOARD_IDENTIFIER "AWF1" // AlienWii32 F1.
 #define BRUSHED_MOTORS
 #define HARDWARE_BIND_PLUG
 // Hardware bind plug at PB5 (Pin 41)