Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-23 16:25:26 +03:00
Code Issues Wiki Activity

Merge branch 'master' into serial1wire-blheli-multiesc

Browse source
This commit is contained in:
nathan 2015-09-30 08:52:09 -07:00
commit e466ed2618
74 changed files with 2625 additions and 590 deletions
Download patch file Download diff file Expand all files Collapse all files

1
.travis.yml
View file

@ -4,6 +4,7 @@ env:
- PUBLISHDOCS=True
- TARGET=CC3D
- TARGET=CC3D OPBL=yes
- TARGET=COLIBRI_RACE
- TARGET=CHEBUZZF3
- TARGET=CJMCU
- TARGET=EUSTM32F103RC

16
Makefile
View file

@ -72,6 +72,8 @@ VPATH := $(SRC_DIR):$(SRC_DIR)/startup
USBFS_DIR = $(ROOT)/lib/main/STM32_USB-FS-Device_Driver
USBPERIPH_SRC = $(notdir $(wildcard $(USBFS_DIR)/src/*.c))
CSOURCES := $(shell find $(SRC_DIR) -name '*.c')
ifeq ($(TARGET),$(filter $(TARGET),STM32F3DISCOVERY CHEBUZZF3 NAZE32PRO SPRACINGF3 SPARKY ALIENWIIF3 COLIBRI_RACE))
STDPERIPH_DIR = $(ROOT)/lib/main/STM32F30x_StdPeriph_Driver
@ -574,7 +576,7 @@ OPTIMIZE = -Os
LTO_FLAGS = -flto -fuse-linker-plugin $(OPTIMIZE)
endif
DEBUG_FLAGS = -ggdb3
DEBUG_FLAGS = -ggdb3 -DDEBUG
CFLAGS = $(ARCH_FLAGS) \
$(LTO_FLAGS) \
@ -616,6 +618,11 @@ LDFLAGS = -lm \
# No user-serviceable parts below
###############################################################################
CPPCHECK = cppcheck $(CSOURCES) --enable=all --platform=unix64 \
--std=c99 --inline-suppr --quiet --force \
$(addprefix -I,$(INCLUDE_DIRS)) \
-I/usr/include -I/usr/include/linux
#
# Things we will build
#
@ -685,6 +692,13 @@ unbrick_$(TARGET): $(TARGET_HEX)
unbrick: unbrick_$(TARGET)
## cppcheck : run static analysis on C source code
cppcheck: $(CSOURCES)
$(CPPCHECK)
cppcheck-result.xml: $(CSOURCES)
$(CPPCHECK) --xml-version=2 2> cppcheck-result.xml
help:
@echo ""
@echo "Makefile for the $(FORKNAME) firmware"

2
README.md
View file

@ -43,7 +43,7 @@ Cleanflight also has additional features not found in baseflight.
* Graupner HoTT telemetry.
* Multiple simultaneous telemetry providers.
* Configurable serial ports for Serial RX, Telemetry, MSP, GPS - Use most devices on any port, softserial too.
+ more many minor bug fixes.
* And many more minor bug fixes.
For a list of features, changes and some discussion please review the thread on MultiWii forums and consult the documentation.

5
docs/Board - CC3D.md
View file

@ -122,6 +122,11 @@ 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.
The following features are not available:
* Autotune
* Display
* Sonar
# Restoring OpenPilot bootloader
If you have a JLink debugger, you can use JLinkExe to flash the open pilot bootloader.

126
docs/Board - ColibriRace.md Executable file
View file

@ -0,0 +1,126 @@
# Board - Colibri RACE
The Colibri RACE is a STM32F3 based flight control designed specifically to work with the TBS POWERCUBE multi rotor stack.
## Hardware Features:
* STM32F303 based chipset for ultimate performance
* PPM, SBUS, DSM, DSMX input (5V and 3.3V provided over internal BUS). No inverters or hacks needed.
* 6 PWM ESC output channels (autoconnect, internal BUS)
* RGB LED strip support incl. power management
* Extension port for GPS / external compass / pressure sensor
* UART port for peripherals (Blackbox, FrSky telemetry etc.)
* Choose between plug & play sockets or solder pads for R/C and buzzer
* 5V buzzer output
* MPU6500 new generation accelerometer/gyro
* 3x status LED (DCDC pwr/ 3.3V pwr/ status)
* Battery monitoring for 12V, 5V and VBat supply
* Size: 36mmx36mm (30.5mm standard raster)
* Weight: 4.4g
For more details please visit:
http://www.team-blacksheep.com/powercube
## Serial Ports
| Value | Identifier | Board Markings | Notes |
| ----- | ------------ | -------------- | ------------------------------------------|
| 1 | VCP | USB PORT | Main Port For MSP |
| 2 | USART1 | FREE PORT | PC4 and PC5(Tx and Rx respectively) |
| 3 | USART2 | PPM Serial | PA15 |
| 4 | USART3 | GPS PORT | PB10 and PB11(Tx and Rx respectively) |
## Pinouts
Full pinout details are available in the manual, here:
http://www.team-blacksheep.com/colibri_race
### SWD - ICSP
| Pin | Function | Notes |
| --- | -------------- | -------------------------------------------- |
| 1 | VCC_IN | 3.3 Volt |
| 2 | SWDIO | |
| 3 | nRESET | |
| 4 | SWCLK | |
| 5 | Ground | |
| 6 | SWO/TDO | |
### Internal Bus
| Pin | Function | Notes |
| --- | -------------- | -------------------------------------------- |
| 1 | PWM1 | MOTOR 1 |
| 2 | PWM2 | MOTOR 2 |
| 3 | PWM3 | MOTOR 3 |
| 4 | PWM4 | MOTOR 4 |
| 5 | PWM5 | MOTOR 5 (For Y6 or Hex X) |
| 6 | PWM6 | MOTOR 6 (For Y6 or Hex X) |
| 7 | BST SDA | Use For TBS CorePro Control Device |
| 8 | BST SCL | Use For TBS CorePro Control Device |
| 9 | PWM7 | Can be a normal GPIO (PA2) or PWM |
| 10 | PWM8 | Can be a normal GPIO (PA2) or PWM |
| 11 | 12.2V DCDC | If 12v is detected, the Blue LED will turn on|
| 12 | 5.1V DCDC | Voltage for MCU |
### Servo
| Pin | Function | Notes |
| --- | -------------- | -------------------------------------------- |
| 1 | Ground | |
| 2 | VCC_OUT | 5.1 Volt output to LCD Strip |
| 3 | PWM Servo | PB14 - PWM10 |
### IO_1 - LED Strip
| Pin | Function | Notes |
| --- | ----------------- | -------------------------------------------- |
| 1 | LED_STRIP | Enable `feature LED_STRIP` |
| 2 | VCC_OUT | 5.1 Volt output to LCD Strip |
| 3 | Ground | |
### IO_2 - Sensor Interface
| Pin | Function | Notes |
| --- | ----------------- | -------------------------------------------- |
| 1 | VCC_OUT | 4.7 Volt output to the device |
| 2 | Ground | |
| 3 | UART3 TX | GPS |
| 4 | UART3 RX | GPS |
| 5 | SDA | mag, pressure, or other i2c device |
| 6 | SCL | mag, pressure, or other i2c device |
### IO_3 - RC input
IO_3 is used for RX_PPM/RX_SERIAL. Under the `PORT` tab, set RX_SERIAL to UART2 when using RX_SERIAL.
| Pin | Function | Notes |
| --- | ----------------- | -------------------------------------------- |
| 1 | PPM/Serial | Can PPM or Serial input |
| 2 | VCC_OUT | 3.3 Volt output to the device |
| 3 | Ground | |
| 4 | VCC_OUT | 5.1 Volt output to the device |
### IO_4 - Buzzer
| Pin | Function | Notes |
| --- | ----------------- | -------------------------------------------- |
| 1 | BUZZER | Normal high (5.1v) |
| 2 | VCC_OUT | 5.1 Volt output to the device |
### IO_5 - Free UART
| Pin | Function | Notes |
| --- | ----------------- | -------------------------------------------- |
| 1 | UART1 TX | Free UART |
| 2 | UART1 RX | Free UART |
| 3 | Ground | |
| 4 | VCC_OUT | 4.7 Volt output to the device |
### IO_6 - IR TX (extension)
| Pin | Function | Notes |
| --- | ----------------- | -------------------------------------------- |
| 1 | IR TX | |
| 2 | Ground | |

43
docs/Buzzer.md
View file

@ -16,26 +16,34 @@ Three beeps immediately after powering the board means that the gyroscope calibr
There is a special arming tone used if a GPS fix has been attained, and there's a "ready" tone sounded after a GPS fix has been attained (only happens once). The tone sounded via the TX-AUX-switch will count out the number of satellites (if GPS fix).
The CLI command `play_sound` is useful for demonstrating the buzzer tones. Repeatedly entering the command will play the various tones in turn. Entering the command with a numeric-index parameter will play the associated tone.
Available buzzer tones include the following:
RX_LOST_LANDING Beeps SOS when armed and TX is turned off or signal lost (autolanding/autodisarm)
RX_LOST Beeps when TX is turned off or signal lost (repeat until TX is okay)
DISARMING Beep when disarming the board
ARMING Beep when arming the board
ARMING_GPS_FIX Beep a special tone when arming the board and GPS has fix
BAT_CRIT_LOW Longer warning beeps when battery is critically low (repeats)
BAT_LOW Warning beeps when battery is getting low (repeats)
RX_SET Beeps when aux channel is set for beep or beep sequence how many satellites has found if GPS enabled
DISARM_REPEAT Beeps sounded while stick held in disarm position
ACC_CALIBRATION ACC inflight calibration completed confirmation
ACC_CALIBRATION_FAIL ACC inflight calibration failed
READY_BEEP Ring a tone when GPS is locked and ready
ARMED Warning beeps when board is armed (repeats until board is disarmed or throttle is increased)
The CLI command `play_sound` is useful for demonstrating the buzzer tones. Repeatedly entering the command will play the various tones in turn. Entering the command with a numeric-index parameter (see below) will play the associated tone.
Buzzer is enabled by default on platforms that have buzzer connections.
## Tone sequences
Buzzer tone sequences (square wave generation) are made so that : 1st, 3rd, 5th, .. are the delays how long the beeper is on and 2nd, 4th, 6th, .. are the delays how long beeper is off. Delays are in milliseconds/10 (i.e., 5 => 50ms).
Sequences available in Cleanflight v1.9 and above are :
0 GYRO_CALIBRATED 20, 10, 20, 10, 20, 10 Gyro is calibrated
1 RX_LOST_LANDING 10, 10, 10, 10, 10, 40, 40, 10, 40, 10, 40, 40, 10, 10, 10, 10, 10, 70 SOS morse code
2 RX_LOST 50, 50 TX off or signal lost (repeats until TX is okay)
3 DISARMING 15, 5, 15, 5 Disarming the board
4 ARMING 30, 5, 5, 5 Arming the board
5 ARMING_GPS_FIX 5, 5, 15, 5, 5, 5, 15, 30 Arming and GPS has fix
6 BAT_CRIT_LOW 50, 2 Battery is critically low (repeats)
7 BAT_LOW 25, 50 Battery is getting low (repeats)
8 NULL multi beeps GPS status (sat count)
9 RX_SET 10, 10 RX is set (when aux channel is set for beep or beep sequence how many satellites has found if GPS enabled)
10 ACC_CALIBRATION 5, 5, 5, 5 ACC inflight calibration completed
11 ACC_CALIBRATION_FAIL 20, 15, 35, 5 ACC inflight calibration failed
12 READY_BEEP 4, 5, 4, 5, 8, 5, 15, 5, 8, 5, 4, 5, 4, 5 GPS locked and copter ready
13 NULL multi beeps Variable # of beeps (confirmation, GPS sat count, etc)
14 DISARM_REPEAT 0, 100, 10 Stick held in disarm position (after pause)
15 ARMED 0, 245, 10, 5 Board is armed (after pause ; repeats until board is disarmed or throttle is increased)
## Types of buzzer supported
The buzzers are enabled/disabled by simply enabling or disabling a GPIO output pin on the board.
@ -49,6 +57,7 @@ Examples of a known-working buzzers.
* [5V Electromagnetic Active Buzzer Continuous Beep](http://www.banggood.com/10Pcs-5V-Electromagnetic-Active-Buzzer-Continuous-Beep-Continuously-p-943524.html)
* [Radio Shack Model: 273-074 PC-BOARD 12VDC (3-16v) 70DB PIEZO BUZZER](http://www.radioshack.com/pc-board-12vdc-70db-piezo-buzzer/2730074.html#.VIAtpzHF_Si)
* [MultiComp MCKPX-G1205A-3700 TRANSDUCER, THRU-HOLE, 4V, 30MA](http://uk.farnell.com/multicomp/mckpx-g1205a-3700/transducer-thru-hole-4v-30ma/dp/2135914?CMP=i-bf9f-00001000)
* [3-24V Piezo Electronic Tone Buzzer Alarm 95DB](http://www.banggood.com/3-24V-Piezo-Electronic-Tone-Buzzer-Alarm-95DB-Continuous-Sound-p-919348.html)
## Connections

6
docs/Cli.md
View file

@ -198,9 +198,9 @@ Re-apply any new defaults as desired.
| `yaw_rate` | | 0 | 100 | 0 | Rate Profile | UINT8 |
| `tpa_rate` | Throttle PID attenuation reduces influence of P on ROLL and PITCH as throttle increases. For every 1% throttle after the TPA breakpoint, P is reduced by the TPA rate. | 0 | 100 | 0 | Rate Profile | UINT8 |
| `tpa_breakpoint` | See tpa_rate. | 1000 | 2000 | 1500 | Rate Profile | UINT16 |
| `failsafe_delay` | | 0 | 200 | 10 | Profile | UINT8 |
| `failsafe_off_delay` | | 0 | 200 | 200 | Profile | UINT8 |
| `failsafe_throttle` | | 1000 | 2000 | 1200 | Profile | UINT16 |
| `failsafe_delay` | Time in deciseconds to wait before activating failsafe when signal is lost. See [Failsafe documentation](Failsafe.md#failsafe_delay). | 0 | 200 | 10 | Profile | UINT8 |
| `failsafe_off_delay` | Time in deciseconds to wait before turning off motors when failsafe is activated. See [Failsafe documentation](Failsafe.md#failsafe_off_delay). | 0 | 200 | 200 | Profile | UINT8 |
| `failsafe_throttle` | Throttle level used for landing when failsafe is enabled. See [Failsafe documentation](Failsafe.md#failsafe_throttle). | 1000 | 2000 | 1000 | Profile | UINT16 |
| `rx_min_usec` | Defines the shortest pulse width value used when ensuring the channel value is valid. If the receiver gives a pulse value lower than this value then the channel will be marked as bad and will default to the value of `mid_rc`. | 100 | 2000 | 985 | Profile | UINT16 |
| `rx_max_usec` | Defines the longest pulse width value used when ensuring the channel value is valid. If the receiver gives a pulse value higher than this value then the channel will be marked as bad and will default to the value of `mid_rc`. | 100 | 3000 | 2115 | Profile | UINT16 |
| `gimbal_mode` | When feature SERVO_TILT is enabled, this can be either 0=normal gimbal (default) or 1=tiltmix gimbal | 0 | 1 | 0 | Profile | UINT8 |

146
docs/Failsafe.md
View file

@ -5,77 +5,39 @@ There are two types of failsafe:
1. Receiver based failsafe
2. Flight controller based failsafe
Receiver based failsafe is where you, from your transmitter and receiver, configure channels to output desired signals if your receiver detects signal loss.
Receiver based failsafe is where you, from your transmitter and receiver, configure channels to output desired signals if your receiver detects signal loss and goes to __rx-failsafe-state__.
The idea is that you set throttle and other controls so the aircraft descends in a controlled manner. See your receiver's documentation for this method.
Flight controller based failsafe is where the flight controller attempts to detect signal loss from your receiver.
Flight controller based failsafe is where the flight controller attempts to detect signal loss and/or the __rx-failsafe-state__ of your receiver and upon detection goes to __fc-failsafe-state__. The idea is that the flight controller starts using substitutes for all controls, which are set by you, using the CLI command `rxfail` (see `rxfail` document) or the cleanflight-configurator GUI.
It is possible to use both types at the same time, which may be desirable. Flight controller failsafe can even help if your receiver signal wires come loose, get damaged or your receiver malfunctions in a way the receiver itself cannot detect.
Alternatively you may configure a transmitter switch to activate failsafe mode. This is useful for fieldtesting the failsafe system and as a **_`PANIC`_** switch when you lose orientation.
## Flight controller failsafe system
The failsafe system is not activated until 5 seconds after the flight controller boots up. This is to prevent failsafe from activating, as in the case of TX/RX gear with long bind procedures, before the RX sends out valid data. Note that you need to activate the 'FAILSAFE' feature in order to activate failsafe on flight controller.
The __failsafe-auto-landing__ system is not activated until 5 seconds after the flight controller boots up. This is to prevent __failsafe-auto-landing__ from activating, as in the case of TX/RX gear with long bind procedures, before the RX sends out valid data.
After the failsafe has forced a landing, the flight controller cannot be armed and has to be reset.
The failsafe system attempts to detect when your receiver loses signal. It then attempts to prevent your aircraft from flying away uncontrollably by enabling an auto-level mode and setting the throttle that should allow the craft to come to a safer landing.
The __failsafe-detection__ system attempts to detect when your receiver loses signal *continuously* but the __failsafe-auto-landing__ starts *only when your craft is __armed__*. It then attempts to prevent your aircraft from flying away uncontrollably by enabling an auto-level mode and setting the throttle that should allow the craft to come to a safer landing.
The failsafe is activated when:
**The failsafe is activated when the craft is armed and either:**
Either:
* The control (stick) channels do not have valid signals AND the failsafe guard time specified by `failsafe_delay` has elapsed.
* A transmitter switch that is configured to control the failsafe mode is switched ON (and 'failsafe_kill_switch' is set to 0).
a) no valid channel data from the RX is received via Serial RX.
Failsafe intervention will be aborted when it was due to:
b) the first 4 channels do not have valid signals.
And when:
c) the failsafe guard time specified by `failsafe_delay` has elapsed.
* a lost RC signal and the RC signal has recovered.
* a transmitter failsafe switch was set to ON position and the switch is set to OFF position (and 'failsafe_kill_switch' is set to 0).
Note that:
* At the end of a failsafe intervention, the flight controller will be disarmed and re-arming will be locked. From that moment on it is no longer possible to abort or re-arm and the flight controller has to be reset.
* When 'failsafe_kill_switch' is set to 1 and the rc switch configured for failsafe is set to ON, the craft is instantly disarmed (but re-arming is not locked). Similar effect can be achieved by setting 'failsafe_throttle' to 1000 and 'failsafe_off_delay' to 0 (but arming is locked).
* Prior to starting a failsafe intervention it is checked if the throttle position was below 'min_throttle' level for the last 'failsafe_throttle_low_delay' seconds. If it was the craft is assumed to be on the ground and is only disarmed. It may be re-armed without a power cycle.
d) The failsafe system will be activated regardless of current throttle position.
e) The craft may already be on the ground with motors stopped and that motors and props could spin again - the software does not currently detect if the craft is on the ground. Take care when using MOTOR_STOP feature.
### Testing
Bench test the failsafe system before flying - remove props while doing so.
1. Arm the craft.
1. Turn off transmitter or unplug RX.
1. Observe motors spin at configured throttle setting for configured duration.
1. Observe motors turn off after configured duration.
1. Ensure that when you turn on your TX again or reconnect the RX that you cannot re-arm once the motors have stopped.
1. Power cycle the FC.
1. Arm the craft.
1. Turn off transmitter or unplug RX.
1. Observe motors spin at configured throttle setting for configured duration.
1. Turn on TX or reconnect RX.
1. Ensure that your switch positions don't now cause the craft to disarm (otherwise it would fall out of the sky on regained signal).
1. Observe that normal flight behavior is resumed.
1. Disarm.
Field test the failsafe system
1. Perform bench testing first!
1. On a calm day go to an unpopulated area away from buildings or test indoors in a safe controlled environment - e.g. inside a big net.
1. Arm the craft.
1. Hover over something soft (long grass, ferns, heather, foam, etc.).
1. Descend the craft and observe throttle position and record throttle value from your TX channel monitor. Ideally 1500 should be hover. So your value should be less than 1500.
1. Stop, disarm.
1. Set failsafe throttle to the recorded value.
1. Arm, hover over something soft again.
1. Turn off TX (!)
1. Observe craft descends and motors continue to spin for the configured duration.
1. Observe FC disarms after the configured duration.
1. Remove flight battery.
If craft descends too quickly then increase failsafe throttle setting.
Ensure that the duration is long enough for your craft to land at the altitudes you normally fly at.
Some notes about **SAFETY**:
* The failsafe system will be activated regardless of current throttle position. So when the failsafe intervention is aborted (RC signal restored/failsafe switch set to OFF) the current stick position will direct the craft !
* The craft may already be on the ground with motors stopped and that motors and props could spin again - the software does not currently detect if the craft is on the ground. Take care when using `MOTOR_STOP` feature. **Props will spin up without warning**, when armed with `MOTOR_STOP` feature ON (props are not spinning) **_and_** failsafe is activated !
## Configuration
@ -83,27 +45,27 @@ When configuring the flight controller failsafe, use the following steps:
1. Configure your receiver to do one of the following:
a) Upon signal loss, send no signal/pulses over the channels
b) Send an invalid signal over the channels (for example, send values lower than 'rx_min_usec')
* Upon signal loss, send no signal/pulses over the channels
* Send an invalid signal over the channels (for example, send values lower than 'rx_min_usec')
and
c) Ensure your receiver does not send out channel data that would cause a disarm by switch or sticks to be registered by the FC. This is especially important for those using a switch to arm.
* Ensure your receiver does not send out channel data that would cause a disarm by switch or sticks to be registered by the FC. This is especially important for those using a switch to arm.
See your receiver's documentation for direction on how to accomplish one of these.
* Configure one of the transmitter switches to activate the failsafe mode.
2. Set 'failsafe_off_delay' to an appropriate value based on how high you fly
3. Set 'failsafe_throttle' to a value that allows the aircraft to descend at approximately one meter per second (default is 1000 which should be throttle off).
4. Enable 'FAILSAFE' feature in Cleanflight GUI or via CLI using `feature FAILSAFE`
These are the basic steps for flight controller failsafe configuration; see Failsafe Settings below for additional settings that may be changed.
##Failsafe Settings
## Failsafe Settings
Failsafe delays are configured in 0.1 second steps.
@ -123,5 +85,63 @@ Delay after failsafe activates before motors finally turn off. This is the amou
Throttle level used for landing. Specify a value that causes the aircraft to descend at about 1M/sec. Default is set to 1000 which should correspond to throttle off.
Use standard RX usec values. See RX documentation.
### `failsafe_kill_switch`
Configure the rc switched failsafe action: the same action as when the rc link is lost (set to 0) or disarms instantly (set to 1). Also see above.
### `failsafe_throttle_low_delay`
Time throttle level must have been below 'min_throttle' to _only disarm_ instead of _full failsafe procedure_.
Use standard RX usec values. See Rx documentation.
### `rx_min_usec`
The lowest channel value considered valid. e.g. PWM/PPM pulse length
### `rx_max_usec`
The highest channel value considered valid. e.g. PWM/PPM pulse length
The `rx_min_usec` and `rx_max_usec` settings helps detect when your RX stops sending any data, enters failsafe mode or when the RX looses signal.
With a Graupner GR-24 configured for PWM output with failsafe on channels 1-4 set to OFF in the receiver settings then this setting, at its default value, will allow failsafe to be activated.
## Testing
**Bench test the failsafe system before flying - _remove props while doing so_.**
1. Arm the craft.
1. Turn off transmitter or unplug RX.
1. Observe motors spin at configured throttle setting for configured duration.
1. Observe motors turn off after configured duration.
1. Ensure that when you turn on your TX again or reconnect the RX that you cannot re-arm once the motors have stopped.
1. Power cycle the FC.
1. Arm the craft.
1. Turn off transmitter or unplug RX.
1. Observe motors spin at configured throttle setting for configured duration.
1. Turn on TX or reconnect RX.
1. Ensure that your switch positions don't now cause the craft to disarm (otherwise it would fall out of the sky on regained signal).
1. Observe that normal flight behavior is resumed.
1. Disarm.
**Field test the failsafe system.**
1. Perform bench testing first!
1. On a calm day go to an unpopulated area away from buildings or test indoors in a safe controlled environment - e.g. inside a big net.
1. Arm the craft.
1. Hover over something soft (long grass, ferns, heather, foam, etc.).
1. Descend the craft and observe throttle position and record throttle value from your TX channel monitor. Ideally 1500 should be hover. So your value should be less than 1500.
1. Stop, disarm.
1. Set failsafe throttle to the recorded value.
1. Arm, hover over something soft again.
1. Turn off TX (!)
1. Observe craft descends and motors continue to spin for the configured duration.
1. Observe FC disarms after the configured duration.
1. Remove flight battery.
If craft descends too quickly then increase failsafe throttle setting.
Ensure that the duration is long enough for your craft to land at the altitudes you normally fly at.
Using a configured transmitter switch to activate failsafe mode, instead of switching off your TX, is good primary testing method in addition to the above procedure.

40
docs/Mixer.md
View file

@ -42,6 +42,10 @@ You can also use the Command Line Interface (CLI) to set the mixer type:
| CUSTOM AIRPLANE | User-defined airplane | | |
| CUSTOM TRICOPTER | User-defined tricopter | | |
## Servo configuration
The cli `servo` command defines the settings for the servo outputs.
The cli mixer `smix` command controllers how the mixer maps internal FC data (RC input, PID stabilisation output, channel forwarding, etc) to servo outputs.
## Servo filtering
@ -98,6 +102,7 @@ Note: the `mmix` command may show a motor mix that is not active, custom motor m
Custom servo mixing rules can be applied to each servo. Rules are applied in the order they are defined.
| id | Servo slot |
|----|--------------|
| 0 | GIMBAL PITCH |
| 1 | GIMBAL ROLL |
| 2 | FLAPS |
@ -109,7 +114,7 @@ Custom servo mixing rules can be applied to each servo. Rules are applied in th
| id | Input sources |
| -- | ------------- |
|----|-----------------|
| 0 | Stabilised ROLL |
| 1 | Stabilised PITCH |
| 2 | Stabilised YAW |
@ -127,6 +132,18 @@ Custom servo mixing rules can be applied to each servo. Rules are applied in th
Note: the `smix` command may show a servo mix that is not active, custom servo mixes are only active for models that use custom mixers.
## Servo Reversing
Servos are reversed using the `smix reverse` command.
e.g. when using the TRI mixer to reverse the tail servo on a tricopter use this:
`smix reverse 5 2 r`
i.e. when mixing rudder servo slot (`5`) using Stabilised YAW input source (`2`) reverse the direction (`r`)
`smix reverse` is a per-profile setting. So ensure you configure it for your profiles as required.
### Example 1: A KK2.0 wired motor setup
Here's an example of a X configuration quad, but the motors are still wired using the KK board motor numbering scheme.
@ -179,17 +196,12 @@ mmix 0 1.000 0.000 1.333 0.000
mmix 1 1.000 -1.000 -0.667 0.000
mmix 2 1.000 1.000 -0.667 0.000
smix reset
smix 0 6 3 100 0 0 100 0
smix 0 5 2 100 0 0 100 0
profile 0
smix reverse 5 2 r
profile 1
smix reverse 5 2 r
profile 2
smix reverse 5 2 r
```
## Servo Reversing
Servos are reversed using the `smix reverse` command.
e.g. when using the TRI mixer to reverse the tail servo on a tricopter use this:
`smix reverse 5 2 r`
i.e. when mixing rudder servo slot (`5`) using Stabilised YAW input source (`2`) reverse the direction (`r`)

13
docs/Rx.md
View file

@ -144,13 +144,14 @@ Signal loss can be detected when:
### RX loss configuration
The `rxfail` cli command is used to configure per-channel rx-loss behaviour.
You can use the `rxfail` command to change this behaviour, a channel can either be AUTOMATIC, HOLD or SET.
You can use the `rxfail` command to change this behaviour.
A flight channel can either be AUTOMATIC or HOLD, an AUX channel can either be SET or HOLD.
* AUTOMATIC - Flight channels are set to safe values (low throttle, mid position for yaw/pitch/roll), all AUX channels HOLD last value.
* AUTOMATIC - Flight channels are set to safe values (low throttle, mid position for yaw/pitch/roll).
* HOLD - Channel holds the last value.
* SET - Channel is set to a specific configured value.
The default mode for all channels is AUTOMATIC.
The default mode is AUTOMATIC for flight channels and HOLD for AUX channels.
The rxfail command can be used in conjunction with mode ranges to trigger various actions.
@ -232,8 +233,12 @@ Set the RX for 'No Pulses'. Turn OFF TX and RX, Turn ON RX. Press and release
### Graupner GR-24 PWM
Set failsafe on channels 1-4 set to OFF in the receiver settings (via transmitter menu).
Set failsafe on the throttle channel in the receiver settings (via transmitter menu) to a value below `rx_min_usec` using channel mode FAILSAFE.
This is the prefered way, since this is *much faster* detected by the FC then a channel that sends no pulses (OFF).
__NOTE:__
One or more control channels may be set to OFF to signal a failsafe condition to the FC, all other channels *must* be set to either HOLD or OFF.
Do __NOT USE__ the mode indicated with FAILSAFE instead, as this combination is NOT handled correctly by the FC.
## Receiver Channel Range Configuration.

674
docs/development/Blackbox Internals.md Normal file
View file

@ -0,0 +1,674 @@
# Blackbox logging internals
The Blackbox is designed to record the raw internal state of the flight controller at near-maximum rate. By logging the
raw inputs and outputs of key flight systems, the Blackbox log aims to allow the offline bench-top simulation, debugging,
and testing of flight control algorithms using data collected from real flights.
A typical logging regime might capture 30 different state variables (for an average of 28 bytes per frame) at a sample
rate of 900Hz. That's about 25,000 bytes per second, which is 250,000 baud with a typical 8-N-1 serial encoding.
## References
Please refer to the source code to clarify anything this document leaves unclear:
* Cleanflight's Blackbox logger: [blackbox.c](https://github.com/cleanflight/cleanflight/blob/master/src/main/blackbox/blackbox.c),
[blackbox_io.c](https://github.com/cleanflight/cleanflight/blob/master/src/main/blackbox/blackbox_io.c),
[blackbox_fielddefs.h](https://github.com/cleanflight/cleanflight/blob/master/src/main/blackbox/blackbox_fielddefs.h)
* [C implementation of the Blackbox log decoder](https://github.com/cleanflight/blackbox-tools/blob/master/src/parser.c)
* [JavaScript implementation of the Blackbox log decoder](https://github.com/cleanflight/blackbox-log-viewer/blob/master/js/flightlog_parser.js)
## Logging cycle
Blackbox is designed for flight controllers that are based around the concept of a "main loop". During each main loop
iteration, the flight controller will read some state from sensors, run some flight control algorithms, and produce some
outputs. For each of these loop iterations, a Blackbox "logging iteration" will be executed. This will read data that
was stored during the execution of the main loop and log this data to an attached logging device. The data will include
algorithm inputs such as sensor and RC data, intermediate results from flight control algorithms, and algorithm outputs
such as motor commands.
## Log frame types
Each event which is recorded to the log is packaged as a "log frame". Blackbox only uses a handful of different types of
log frames. Each frame type is identified by a single capital letter.
### Main frames: I, P
The most basic kind of logged frames are the "main frames". These record the primary state of the flight controller (RC
input, gyroscopes, flight control algorithm intermediates, motor outputs), and are logged during every logging
iteration.
Each main frame must contain at least two fields, "loopIteration" which records the index of the current main loop
iteration (starting at zero for the first logged iteration), and "time" which records the timestamp of the beginning of
the main loop in microseconds (this needn't start at zero, on Cleanflight it represents the system uptime).
There are two kinds of main frames, "I" and "P". "I", or "intra" frames are like video keyframes. They can be decoded
without reference to any previous frame, so they allow log decoding to be resynchronized in the event of log damage. "P"
or "inter" frames use an encoding which references previously logged frames in order to reduce the required datarate.
When one interframe fails to decode, all following interframes will be undecodable up until the next intraframe.
### GPS frames: G, H
Because the GPS is updated so infrequently, GPS data is logged in its own dedicated frames. These are recorded whenever
the GPS data changes (not necessarily alongside every main frame). Like the main frames, the GPS frames have their own
intra/inter encoding system.
The "H" or "home" frame records the lat/lon of a reference point. The "G" or "GPS" frame records the current state of
the GPS system (current position, altitude etc.) based on the reference point. The reference point can be updated
(infrequently) during the flight, and is logged whenever it changes.
To allow "G" frames to continue be decoded in the event that an "H" update is dropped from the log, the "H" frame is
logged periodically even if it has not changed (say, every 10 seconds). This caps the duration of unreadble "G" frames
that will result from a single missed "H" change.
### Slow frames: S
Some flight controller state is updated very infrequently (on the order of once or twice a minute). Logging the fact
that this data had not been updated during every single logging iteration would be a waste of bandwidth, so these frames
are only logged when the "slow" state actually changes.
All Slow frames are logged as intraframes. An interframe encoding scheme can't be used for Slow frames, because one
damaged frame causes all subsequent interframes to be undecodable. Because Slow frames are written so infrequently, one
missing Slow frame could invalidate minutes worth of Slow state.
On Cleanflight, Slow frames are currently used to log data like the user-chosen flight mode and the current failsafe
state.
### Event frames: E
Some flight controller data is updated so infrequently or exists so transiently that we do not log it as a flight
controller "state". Instead, we log it as a state *transition* . This data is logged in "E" or "event" frames. Each event
frame payload begins with a single byte "event type" field. The format of the rest of the payload is not encoded in the
flight log, so its interpretation is left up to an agreement of the writer and the decoder.
For example, one event that Cleanflight logs is that the user has adjusted a system setting (such as a PID setting)
using Cleanflight's inflight adjustments feature. The event payload notes which setting was adjusted and the new value
for the setting.
Because these setting updates are so rare, it would be wasteful to treat the settings as "state" and log the fact that
the setting had not been changed during every logging iteration. It would be infeasible to periodically log the system
settings using an intra/interframe scheme, because the intraframes would be so large. Instead we only log the
transitions as events, accept the small probability that any one of those events will be damaged/absent in the log, and
leave it up to log readers to decide the extent to which they are willing to assume that the state of the setting
between successfully-decoded transition events was truly unchanged.
## Log field format
For every field in a given frame type, there is an associated name, predictor, and encoding.
When a field is written, the chosen predictor is computed for the field, then this predictor value is subtracted from
the raw field value. Finally, the encoder is used to transform the value into bytes to be written to the logging device.
### Field predictors
The job of the predictor is to bring the value to be encoded as close to zero as possible. The predictor may be based
on the values seen for the field in a previous frame, or some other value such as a fixed value or a value recorded in
the log headers. For example, the battery voltage values in "I" intraframes in Cleanflight use a reference voltage that
is logged as part of the headers as a predictor. This assumes that battery voltages will be broadly similar to the
initial pack voltage of the flight (e.g. 4S battery voltages are likely to lie within a small range for the whole
flight). In "P" interframes, the battery voltage will instead use the previously-logged voltage as a predictor, because
the correlation between successive voltage readings is high.
These predictors are presently available:
#### Predict zero (0)
This predictor is the null-predictor which doesn't modify the field value at all. It is a common choice for fields
which are already close to zero, or where no better history is available (e.g. in intraframes which may not rely on the
previous value of fields).
#### Predict last value (1)
This is the most common predictor in interframes. The last-logged value of the field will be used as the predictor, and
subtracted from the raw field value. For fields which don't change very often, this will make their encoded value be
normally zero. Most fields have some time-correlation, so this predictor should reduce the magnitude of all but the
noisiest fields.
#### Predict straight line (2)
This predictor assumes that the slope between the current measurement and the previous one will be similar to the
slope between the previous measurement and the one before that. This is common for fields which increase at a steady rate,
such as the "time" field. The predictor is `history_age_2 - 2 * history_age_1`.
#### Predict average 2 (3)
This predictor is the average of the two previously logged values of the field (i.e. `(history_age_1 + history_age_2) / 2`
). It is used when there is significant random noise involved in the field, which means that the average of the recent
history is a better predictor of the next value than the previous value on its own would be (for example, in gyroscope
or motor measurements).
#### Predict minthrottle (4)
This predictor subtracts the value of "minthrottle" which is included in the log header. In Cleanflight, motors always
lie in the range of `[minthrottle ... maxthrottle]` when the craft is armed, so this predictor is used for the first
motor value in intraframes.
#### Predict motor[0] (5)
This predictor is set to the value of `motor[0]` which was decoded earlier within the current frame. It is used in
intraframes for every motor after the first one, because the motor commands typically lie in a tight grouping.
#### Predict increment (6)
This predictor assumes that the field will be incremented by 1 unit for every main loop iteration. This is used to
predict the `loopIteration` field, which increases by 1 for every loop iteration.
#### Predict home-coord (7)
This predictor is set to the corresponding latitude or longitude field from the GPS home coordinate (which was logged in
a preceding "H" frame). If no preceding "H" frame exists, the value is marked as invalid.
#### Predict 1500 (8)
This predictor is set to a fixed value of 1500. It is preferred for logging servo values in intraframes, since these
typically lie close to the midpoint of 1500us.
#### Predict vbatref (9)
This predictor is set to the "vbatref" field written in the log header. It is used when logging intraframe battery
voltages in Cleanflight, since these are expected to be broadly similar to the first battery voltage seen during
arming.
#### Predict last main-frame time (10)
This predictor is set to the last logged `time` field from the main frame. This is used when predicting timestamps of
non-main frames (e.g. that might be logging the timing of an event that happened during the main loop cycle, like a GPS
reading).
### Field encoders
The field encoder's job is to use fewer bits to represent values which are closer to zero than for values that are
further from zero. Blackbox supports a range of different encoders, which should be chosen on a per-field basis in order
to minimize the encoded data size. The choice of best encoder is based on the probability distribution of the values
which are to be encoded. For example, if a field is almost always zero, then an encoding should be chosen for it which
can encode that case into a very small number of bits, such as one. Conversely, if a field is normally 8-16 bits large,
it would be wasteful to use an encoder which provided a special short encoded representation for zero values, because
this will increase the encoded length of larger values.
These encoders are presently available:
#### Unsigned variable byte (1)
This is the most straightforward encoding. This encoding uses the lower 7 bits of an encoded byte to store the lower 7
bits of the field's value. The high bit of that encoded byte is set to one if more than 7 bits are required to store the
value. If the value did exceed 7 bits, the lower 7 bits of the value (which were written to the log) are removed from
the value (by right shift), and the encoding process begins again with the new value.
This can be represented by the following algorithm:
```c
while (value > 127) {
writeByte((uint8_t) (value | 0x80)); // Set the high bit to mean "more bytes follow"
value >>= 7;
}
writeByte(value);
```
Here are some example values encoded using variable-byte encoding:
| Input value | Output encoding |
| ----------- | --------------- |
| 1 | 0x01 |
| 42 | 0x2A |
| 127 | 0x7F |
| 128 | 0x80 0x01 |
| 129 | 0x81 0x01 |
| 23456 | 0xA0 0xB7 0x01 |
#### Signed variable byte (0)
This encoding applies a pre-processing step to fold negative values into positive ones, then the resulting unsigned
number is encoded using unsigned variable byte encoding. The folding is accomplished by "ZigZag" encoding, which is
represented by:
```c
unsigned32 = (signed32 << 1) ^ (signed32 >> 31)
```
ZigZag encoding is preferred against simply casting the signed integer to unsigned, because casting would cause small
negative quantities to appear to be very large unsigned integers, causing the encoded length to be similarly large.
ZigZag encoding ensures that values near zero are still near zero after encoding.
Here are some example integers encoded using ZigZag encoding:
| Input value | ZigZag encoding |
| ----------- | --------------- |
| 0 | 0 |
| -1 | 1 |
| 1 | 2 |
| -2 | 3 |
| 2147483647 | 4294967294 |
| -2147483648 | 4294967295 |
#### Neg 14-bit (3)
The value is negated, treated as an unsigned 14 bit integer, then encoded using unsigned variable byte encoding. This
bizarre encoding is used in Cleanflight for battery pack voltages. This is because battery voltages are measured using a
14-bit ADC, with a predictor which is set to the battery voltage during arming, which is expected to be higher than any
voltage experienced during flight. After the predictor is subtracted, the battery voltage will almost certainly be below
zero.
This results in small encoded values when the voltage is closely below the initial one, at the expense of very large
encoded values if the voltage rises higher than the initial one.
#### Elias delta unsigned 32-bit (4)
Because this encoding produces a bitstream, this is the only encoding for which the encoded value might not be a whole
number of bytes. If the bitstream isn't aligned on a byte boundary by the time the next non-Elias Delta field arrives,
or the end of the frame is reached, the final byte is padded with zeros byte-align the stream. This encoding requires
more CPU time than the other encodings because of the bit juggling involved in writing the bitstream.
When this encoder is chosen to encode all of the values in Cleanflight interframes, it saves about 10% bandwidth
compared to using a mixture of the other encodings, but uses too much CPU time to be practical.
[The basic encoding algorithm is defined on Wikipedia](https://en.wikipedia.org/wiki/Elias_delta_coding). Given these
utility functions:
```c
/* Write `bitCount` bits from the least-significant end of the `bits` integer to the bitstream. The most-significant bit
* will be written first
*/
void writeBits(uint32_t bits, unsigned int bitCount);
/* Returns the number of bits needed to hold the top-most 1-bit of the integer 'i'. 'i' must not be zero. */
unsigned int numBitsToStoreInteger(uint32_t i);
```
This is our reference implementation of Elias Delta:
```c
// Value must be more than zero
void writeU32EliasDeltaInternal(uint32_t value)
{
unsigned int valueLen, lengthOfValueLen;
valueLen = numBitsToStoreInteger(value);
lengthOfValueLen = numBitsToStoreInteger(valueLen);
// Use unary to encode the number of bits we'll need to write the length of the value
writeBits(0, lengthOfValueLen - 1);
// Now write the length of the value
writeBits(valueLen, lengthOfValueLen);
// Having now encoded the position of the top bit of value, write its remaining bits
writeBits(value, valueLen - 1);
}
```
To this we add a wrapper which allows encoding both the value zero and MAXINT:
```c
void writeU32EliasDelta(uint32_t value)
{
/* We can't encode value==0, so we need to add 1 to the value before encoding
*
* That would make it impossible to encode MAXINT, so use 0xFFFFFFFF as an escape
* code with an additional bit to choose between MAXINT-1 or MAXINT.
*/
if (value >= 0xFFFFFFFE) {
// Write the escape code
writeU32EliasDeltaInternal(0xFFFFFFFF);
// Add a one bit after the escape code if we wanted "MAXINT", or a zero if we wanted "MAXINT - 1"
writeBits(value - 0xFFFFFFFE, 1);
} else {
writeU32EliasDeltaInternal(value + 1);
}
}
```
Here are some reference encoded bit patterns produced by writeU32EliasDelta:
| Input value | Encoded bit string |
| ----------- | ------------------ |
| 0 | 1 |
| 1 | 0100 |
| 2 | 0101 |
| 3 | 01100 |
| 4 | 01101 |
| 5 | 01110 |
| 6 | 01111 |
| 7 | 00100000 |
| 8 | 00100001 |
| 9 | 00100010 |
| 10 | 00100011 |
| 11 | 00100100 |
| 12 | 00100101 |
| 13 | 00100110 |
| 14 | 00100111 |
| 15 | 001010000 |
| 225 | 00010001100010 |
| 4294967292 | 000001000001111111111111111111111111111101 |
| 4294967293 | 000001000001111111111111111111111111111110 |
| 4294967294 | 0000010000011111111111111111111111111111110 |
| 4294967295 | 0000010000011111111111111111111111111111111 |
Note that the very common value of zero encodes to a single bit, medium-sized values like 225 encode to 14 bits (an
overhead of 4 bits over writing a plain 8 bit value), and typical 32-bit values like 4294967293 encode into 42 bits, an
overhead of 10 bits.
#### Elias delta signed 32-bit (5)
The value is first converted to unsigned using ZigZag encoding, then unsigned Elias-delta encoding is applied.
#### TAG8_8SVB (6)
First, an 8-bit (one byte) header is written. This header has its bits set to zero when the corresponding field (from a
maximum of 8 fields) is set to zero, otherwise the bit is set to one. The least-signficant bit in the header corresponds
to the first field to be written. This header is followed by the values of only the fields which are non-zero, written
using signed variable byte encoding.
This encoding is preferred for groups of fields in interframes which are infrequently updated by the flight controller.
This will mean that their predictions are usually perfect, and so the value to be encoded for each field will normally
be zero. This is common for values like RC inputs and barometer readings, which are updated in only a fraction of main
loop iterations.
For example, given these field values to encode:
```
0, 0, 4, 0, 8
```
This would be encoded:
```
0b00010100, 0x04, 0x08
```
#### TAG2_3S32 (7)
A 2-bit header is written, followed by 3 signed field values of up to 32 bits each. The header value is based on the
maximum size in bits of the three values to be encoded as follows:
| Header value | Maximum field value size | Field range |
| ------------ | ------------------------ | -------------------------- |
| 0 | 2 bits | [-2...1] |
| 1 | 4 bits | [-8...7] |
| 2 | 6 bits | [-32...31] |
| 3 | Up to 32 bits | [-2147483648...2147483647] |
If any of the three values requires more than 6 bits to encode, a second, 6-bit header value is written in the lower
bits of the initial header byte. This second header has 2 bits for each of the encoded values which represents how many
bytes are required to encode that value. The least-significant bits of the header represent the first field which is
encoded. The values for each possibility are as follows:
| Header value | Field size | Field range |
| ------------ | ---------- | -------------------------- |
| 0 | 1 byte | [-127...128] |
| 1 | 2 bytes | [-32768...32767] |
| 2 | 3 bytes | [-8388608...8388607] |
| 3 | 4 bytes | [-2147483648...2147483647] |
This header is followed by the actual field values in order, written least-significant byte first, using the byte
lengths specified in the header.
So bringing it all together, these encoded bit patterns are possible, where "0" and "1" mean bits fixed to be those
values, "A", "B", and "C" represent the first, second and third fields, and "s" represents the bits of the secondary
header in the case that any field is larger than 6 bits:
```
00AA BBCC,
0100 AAAA BBBB CCCC
10AA AAAA 00BB BBBB 00CC CCCC
11ss ssss (followed by fields of byte lengths specified in the "s" header)
```
This encoding is useful for fields like 3-axis gyroscopes, which are frequently small and typically have similar
magnitudes.
#### TAG8_4S16 (8)
An 8-bit header is written, followed by 4 signed field values of up to 16 bits each. The 8-bit header value has 2 bits
for each of the encoded fields (the least-significant bits represent the first field) which represent the
number of bits required to encode that field as follows:
| Header value | Field value size | Field range |
| ------------ | ---------------- | ---------------- |
| 0 | 0 bits | [0...0] |
| 1 | 4 bits | [-8...7] |
| 2 | 8 bits | [-128...127] |
| 3 | 16 bits | [-32768...32767] |
This header is followed by the actual field values in order, written as if the output stream was a bit-stream, with the
most-significant bit of the first field ending up in the most-significant bits of the first written byte. If the number
of nibbles written is odd, the final byte has its least-significant nibble set to zero.
For example, given these field values:
```
13, 0, 4, 2
```
Choosing from the allowable field value sizes, they may be encoded using this many bits each:
```
8, 0, 4, 4
```
The corresponding header values for these lengths would be:
```
2, 0, 1, 1
```
So the header and fields would be encoded together as:
```
0b01010010, 0x0D, 0x42
```
#### NULL (9)
This encoding does not write any bytes to the file. It is used when the predictor will always perfectly predict the
value of the field, so the remainder is always zero. In practice this is only used for the "loopIteration" field in
interframes, which is always perfectly predictable based on the logged frame's position in the sequence of frames and
the "P interval" setting from the header.
## Log file structure
A logging session begins with a log start marker, then a header section which describes the format of the log, then the
log payload data, and finally an optional "log end" event ("E" frame).
A single log file can be comprised of one or more logging sessions. Each session may be preceded and followed by any
amount of non-Blackbox data. This data is ignored by the Blackbox log decoding tools. This allows for the logging device
to be alternately used by the Blackbox and some other system (such as MSP) without requiring the ability to begin a
separate log file for each separate activity.
### Log start marker
The log start marker is "H Product:Blackbox flight data recorder by Nicholas Sherlock\n". This marker is
used to discover the beginning of the flight log if the log begins partway through a file. Because it is such a long
string, it is not expected to occur by accident in any sequence of random bytes from other log device users.
### Log header
The header is comprised of a sequence of lines of plain ASCII text. Each header line has the format `H fieldname:value`
and ends with a '\n'. The overall header does not have a terminator to separate it from the log payload
(the header implicitly ends when a line does not begin with an 'H' character).
The header can contain some of these fields:
#### Data version (required)
When the interpretation of the Blackbox header changes due to Blackbox specification updates, the log version is
incremented to allow backwards-compatibility in the decoder:
```
H Data version:2
```
#### Logging interval
Not every main loop iteration needs to result in a Blackbox logging iteration. When a loop iteration is not logged,
Blackbox is not called, no state is read from the flight controller, and nothing is written to the log. Two header lines
are included to note which main loop iterations will be logged:
##### I interval
This header notes which of the main loop iterations will record an "I" intraframe to the log. If main loop iterations
with indexes divisible by 32 will be logged as "I" frames, the header will be:
```
H I interval: 32
```
The first main loop iteration seen by Blackbox will be numbered with index 0, so the first main loop iteration will
always be logged as an intraframe.
##### P interval
Not every "P" interframe needs to be logged. Blackbox will log a portion of iterations in order to bring the total
portion of logged main frames to a user-chosen fraction. This fraction is called the logging rate. The smallest possible
logging rate is `(1/I interval)` which corresponds to logging only "I" frames at the "I" interval and discarding all
other loop iterations. The maximum logging rate is `1/1`, where every main loop iteration that is not an "I" frame is
logged as a "P" frame. The header records the logging rate fraction in `numerator/denominator` format like so:
```
H P interval:1/2
```
The logging fraction given by `num/denom` should be simplified (i.e. rather than 2/6, a logging rate of 1/3 should
be used).
Given a logging rate of `num/denom` and an I-frame interval of `I_INTERVAL`, the frame type to log for an iteration
of index `iteration` is given by:
```c
if (iteration % I_INTERVAL == 0)
return 'I';
if ((iteration % I_INTERVAL + num - 1) % denom < num)
return 'P';
return '.'; // i.e. don't log this iteration
```
For an I-interval of 32, these are the resulting logging patterns at some different P logging rates.
| Logging rate | Main frame pattern | Actual portion logged |
| ------------ | ----------------------------------------------------------------- | --------------------- |
| 1 / 32 | I...............................I...............................I | 0.03 |
| 1 / 6 | I.....P.....P.....P.....P.....P.I.....P.....P.....P.....P.....P.I | 0.19 |
| 1 / 3 | I..P..P..P..P..P..P..P..P..P..P.I..P..P..P..P..P..P..P..P..P..P.I | 0.34 |
| 1 / 2 | I.P.P.P.P.P.P.P.P.P.P.P.P.P.P.P.I.P.P.P.P.P.P.P.P.P.P.P.P.P.P.P.I | 0.50 |
| 2 / 3 | I.PP.PP.PP.PP.PP.PP.PP.PP.PP.PP.I.PP.PP.PP.PP.PP.PP.PP.PP.PP.PP.I | 0.66 |
| 5 / 6 | I.PPPPP.PPPPP.PPPPP.PPPPP.PPPPP.I.PPPPP.PPPPP.PPPPP.PPPPP.PPPPP.I | 0.81 |
| 1 / 1 | IPPPPPPPPPPPPPPPPPPPPPPPPPPPPPPPIPPPPPPPPPPPPPPPPPPPPPPPPPPPPPPPI | 1.00 |
#### Firmware type (optional)
Because Blackbox records the internal flight controller state, the interpretation of the logged data will depend
on knowing which flight controller recorded it. To accomodate this, the name of the flight controller should be recorded:
```
H Firmware type:Cleanflight
```
More details should be included to help narrow down the precise flight-controller version (but these are not required):
```
H Firmware revision:c49bd40
H Firmware date:Aug 28 2015 16:49:11
```
#### Field X name (required)
This header is a comma-separated list of the names for the fields in the 'X' frame type:
```
H Field I name:loopIteration,time,axisP[0],axisP[1]...
```
The decoder assumes that the fields in the 'P' frame type will all have the same names as those in the 'I' frame, so
a "Field P name" header does not need to be supplied.
#### Field X signed (optional)
This is a comma-separated list of integers which are set to '1' when their corresponding field's value should be
interpreted as signed after decoding, or '0' otherwise:
```
H Field I signed:0,0,1,1...
```
#### Field X predictor (required)
This is a comma-separated list of integers which specify the predictors for each field in the specified frame type:
```
H Field I predictor:0,0,0,0...
```
#### Field X encoding (required)
This is a comma-separated list of integers which specify the encoding used for each field in the specified frame type:
```
H Field X encoding:1,1,0,0...
```
#### vbatref
This header provides the reference voltage that will be used by predictor #9.
#### minthrottle
This header provides the minimum value sent by Cleanflight to the ESCs when armed, it is used by predictor #4.
#### Additional headers
The decoder ignores headers that it does not understand, so you can freely add any headers that you require in order to
properly interpret the meaning of the logged values.
For example, to create a graphical displays of RC sticks and motor percentages, the Blackbox rendering tool requires
the additional headers "rcRate" and "maxthrottle". In order to convert raw gyroscope, accelerometer and voltage readings
into real-world units, the Blackbox decoder requires the calibration constants "gyro.scale", "acc_1G" and "vbatscale".
These headers might look like:
```
H rcRate:100
H maxthrottle:1980
H gyro.scale:0x3d79c190
H acc_1G:4096
H vbatscale:110
```
### Log payload
The log payload is a concatenated sequence of logged frames. Each frame type which is present in the log payload must
have been previously described in the log header (with Frame X name, etc. headers). Each frame begins with a single
capital letter to specify the type of frame (I, P, etc), which is immediately followed by the frame's field data. There
is no frame length field, checksum, or trailer.
The field data is encoded by taking an array of raw field data, computing the predictor for each field, subtrating this
predictor from the field, then applying the field encoders to each field in sequence to serialize them to the log.
For example, imagine that we are encoding three fields in an intraframe, are using zero-predictors for each field (#0),
and are encoding the values using the unsigned variable byte encoding (#1). For these field values:
```
1, 2, 3
```
We would encode a frame:
```
'I', 0x01, 0x02, 0x03
```
Imagine that we are encoding an array of motor commands in an interframe. We will use the previous motor commands as a
predictor, and encode the resulting values using signed variable byte encoding. The motor command values seen in the
previous logged iteration were:
```
1430, 1500, 1470, 1490
```
And the motor commands to be logged in this iteration are:
```
1635, 1501, 1469, 1532
```
After subtracting the predictors for each field, we will be left with:
```
205, 1, -1, 42
```
We will apply ZigZag encoding to each field, which will give us:
```
410, 2, 1, 84
```
We will use unsigned variable byte encoding to write the resulting values to the log, which will give us:
```
'P', 0x9A, 0x03, 0x02, 0x01, 0x54
```
### Log end marker
The log end marker is an optional Event ("E") frame of type 0xFF whose payload is the string "End of log\0". The
payload ensures that random data does not look like an end-of-log marker by chance. This event signals the tidy ending
of the log. All following bytes until the next log-begin marker (or end of file) should be ignored by the log
decoder.
```
'E', 0xFF, "End of log", 0x00
```
## Log validation
Any damage experienced to the log during recording is overwhelmingly due to subsequences of bytes being dropped by the
logging device due to overflowing buffers. Accordingly, Blackbox logs do not bother to include any checksums (bytes are
not expected to be damaged by the logging device without changing the length of the message). Because of the tight
bandwidth requirements of logging, neither a frame length field nor frame trailer is recorded that would allow for the
detection of missing bytes.
Instead, the decoder uses a heuristic in order to detect damaged frames. The decoder reads an entire frame from the log
(using the decoder for each field which is the counterpart of the encoder specified in the header), then it checks to
see if the byte immediately following the frame, which should be the beginning of a next frame, is a recognized
frame-type byte (e.g. 'I', 'P', 'E', etc). If that following byte represents a valid frame type, it is assumed that the
decoded frame was the correct length (so was unlikely to have had random ranges of bytes removed from it, which would
have likely altered the frame length). Otherwise, the frame is rejected, and a valid frame-type byte is looked for
immediately after the frame-start byte of the frame that was rejected. A rejected frame causes all subsequent
interframes to be rejected as well, up until the next intraframe.
A frame is also rejected if the "loopIteration" or "time" fields have made unreasonable leaps forward, or moved at
all backwards. This suffices to detect almost all log corruption.

13
docs/development/Development.md
View file

@ -13,7 +13,7 @@ This document is primarily for developers only.
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 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.
10. If you need to document a variable do it at the declaration, 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.
13. Know that there's always more than one way to do something and that code is never final - but it does have to work.
@ -46,16 +46,15 @@ Note: Tests are written in C++ and linked with with firmware's C code.
### Running the tests.
The tests and test build system is very simple and based of the googletest example files, it will be improved in due course.
The tests and test build system is very simple and based off the googletest example files, it will be improved in due course. From the root folder of the project simply do:
```
cd test
make
make test
```
This will build a set of executable files, one for each `*_unittest.cc` file.
This will build a set of executable files in the `obj/test` folder, one for each `*_unittest.cc` file.
You can run them on the command line to execute the tests and to see the test report.
After they have been executed by the make invocation, you can still run them on the command line to execute the tests and to see the test report.
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.
@ -75,7 +74,7 @@ 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`.
1. Login to github, go to 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`

22
fake_travis_build.sh
View file

@ -1,10 +1,22 @@
#!/bin/bash
targets=("PUBLISHMETA=True" "RUNTESTS=True" \
"TARGET=CC3D" "TARGET=CC3D OPBL=yes" "TARGET=CHEBUZZF3" "TARGET=CJMCU" \
"TARGET=EUSTM32F103RC" "TARGET=SPRACINGF3" "TARGET=NAZE" "TARGET=NAZE32PRO" \
"TARGET=OLIMEXINO" "TARGET=PORT103R" "TARGET=SPARKY" "TARGET=STM32F3DISCOVERY" \
"TARGET=ALIENWIIF1" "TARGET=ALIENWIIF3")
targets=("PUBLISHMETA=True" \
"RUNTESTS=True" \
"TARGET=CC3D" \
"TARGET=CC3D OPBL=yes" \
"TARGET=CHEBUZZF3" \
"TARGET=CJMCU" \
"TARGET=COLIBRI_RACE" \
"TARGET=EUSTM32F103RC" \
"TARGET=SPRACINGF3" \
"TARGET=NAZE" \
"TARGET=NAZE32PRO" \
"TARGET=OLIMEXINO" \
"TARGET=PORT103R" \
"TARGET=SPARKY" \
"TARGET=STM32F3DISCOVERY" \
"TARGET=ALIENWIIF1" \
"TARGET=ALIENWIIF3")
#fake a travis build environment
export TRAVIS_BUILD_NUMBER=$(date +%s)

31
src/main/config/config.c
View file

@ -128,7 +128,7 @@ static uint32_t activeFeaturesLatch = 0;
static uint8_t currentControlRateProfileIndex = 0;
controlRateConfig_t *currentControlRateProfile;
static const uint8_t EEPROM_CONF_VERSION = 104;
static const uint8_t EEPROM_CONF_VERSION = 105;
static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims)
{
@ -348,6 +348,11 @@ static void setControlRateProfile(uint8_t profileIndex)
currentControlRateProfile = &masterConfig.controlRateProfiles[profileIndex];
}
uint16_t getCurrentMinthrottle(void)
{
return masterConfig.escAndServoConfig.minthrottle;
}
// Default settings
static void resetConf(void)
{
@ -392,6 +397,7 @@ static void resetConf(void)
masterConfig.gyroConfig.gyroMovementCalibrationThreshold = 32;
masterConfig.mag_hardware = MAG_DEFAULT; // default/autodetect
masterConfig.baro_hardware = BARO_DEFAULT; // default/autodetect
resetBatteryConfig(&masterConfig.batteryConfig);
@ -407,8 +413,8 @@ static void resetConf(void)
for (i = 0; i < MAX_SUPPORTED_RC_CHANNEL_COUNT; i++) {
rxFailsafeChannelConfiguration_t *channelFailsafeConfiguration = &masterConfig.rxConfig.failsafe_channel_configurations[i];
channelFailsafeConfiguration->mode = RX_FAILSAFE_MODE_AUTO;
channelFailsafeConfiguration->step = CHANNEL_VALUE_TO_RXFAIL_STEP(masterConfig.rxConfig.midrc);
channelFailsafeConfiguration->mode = (i < NON_AUX_CHANNEL_COUNT) ? RX_FAILSAFE_MODE_AUTO : RX_FAILSAFE_MODE_HOLD;
channelFailsafeConfiguration->step = (i == THROTTLE) ? CHANNEL_VALUE_TO_RXFAIL_STEP(masterConfig.rxConfig.rx_min_usec) : CHANNEL_VALUE_TO_RXFAIL_STEP(masterConfig.rxConfig.midrc);
}
masterConfig.rxConfig.rssi_channel = 0;
@ -483,6 +489,8 @@ static void resetConf(void)
masterConfig.failsafeConfig.failsafe_delay = 10; // 1sec
masterConfig.failsafeConfig.failsafe_off_delay = 200; // 20sec
masterConfig.failsafeConfig.failsafe_throttle = 1000; // default throttle off.
masterConfig.failsafeConfig.failsafe_kill_switch = 0; // default failsafe switch action is identical to rc link loss
masterConfig.failsafeConfig.failsafe_throttle_low_delay = 100; // default throttle low delay for "just disarm" on failsafe condition
#ifdef USE_SERVOS
// servos
@ -796,6 +804,11 @@ void validateAndFixConfig(void)
}
#endif
#ifdef STM32F303xC
// hardware supports serial port inversion, make users life easier for those that want to connect SBus RX's
masterConfig.telemetryConfig.telemetry_inversion = 1;
#endif
/*
* The retarded_arm setting is incompatible with pid_at_min_throttle because full roll causes the craft to roll over on the ground.
* The pid_at_min_throttle implementation ignores yaw on the ground, but doesn't currently ignore roll when retarded_arm is enabled.
@ -827,7 +840,9 @@ void readEEPROM(void)
{
// Sanity check
if (!isEEPROMContentValid())
failureMode(10);
failureMode(FAILURE_INVALID_EEPROM_CONTENTS);
suspendRxSignal();
// Read flash
memcpy(&masterConfig, (char *) CONFIG_START_FLASH_ADDRESS, sizeof(master_t));
@ -844,6 +859,8 @@ void readEEPROM(void)
validateAndFixConfig();
activateConfig();
resumeRxSignal();
}
void readEEPROMAndNotify(void)
@ -862,6 +879,8 @@ void writeEEPROM(void)
uint32_t wordOffset;
int8_t attemptsRemaining = 3;
suspendRxSignal();
// prepare checksum/version constants
masterConfig.version = EEPROM_CONF_VERSION;
masterConfig.size = sizeof(master_t);
@ -901,8 +920,10 @@ void writeEEPROM(void)
// Flash write failed - just die now
if (status != FLASH_COMPLETE || !isEEPROMContentValid()) {
failureMode(10);
failureMode(FAILURE_FLASH_WRITE_FAILED);
}
resumeRxSignal();
}
void ensureEEPROMContainsValidData(void)

1
src/main/config/config.h
View file

@ -73,3 +73,4 @@ void changeControlRateProfile(uint8_t profileIndex);
bool canSoftwareSerialBeUsed(void);
uint16_t getCurrentMinthrottle(void);

1
src/main/config/config_master.h
View file

@ -53,6 +53,7 @@ typedef struct master_t {
gyroConfig_t gyroConfig;
uint8_t mag_hardware; // Which mag hardware to use on boards with more than one device
uint8_t baro_hardware; // Barometer hardware to use
uint16_t max_angle_inclination; // max inclination allowed in angle (level) mode. default 500 (50 degrees).
flightDynamicsTrims_t accZero;

1
src/main/config/runtime_config.h
View file

@ -41,6 +41,7 @@ typedef enum {
AUTOTUNE_MODE = (1 << 7),
PASSTHRU_MODE = (1 << 8),
SONAR_MODE = (1 << 9),
FAILSAFE_MODE = (1 << 10),
} flightModeFlags_e;
extern uint16_t flightModeFlags;

18
src/main/drivers/accgyro_adxl345.c
View file

@ -57,7 +57,7 @@
#define ADXL345_FIFO_STREAM 0x80
static void adxl345Init(void);
static void adxl345Read(int16_t *accelData);
static bool adxl345Read(int16_t *accelData);
static bool useFifo = false;
@ -96,7 +96,7 @@ static void adxl345Init(void)
uint8_t acc_samples = 0;
static void adxl345Read(int16_t *accelData)
static bool adxl345Read(int16_t *accelData)
{
uint8_t buf[8];
@ -109,7 +109,11 @@ static void adxl345Read(int16_t *accelData)
do {
i++;
i2cRead(ADXL345_ADDRESS, ADXL345_DATA_OUT, 8, buf);
if (!i2cRead(ADXL345_ADDRESS, ADXL345_DATA_OUT, 8, buf)) {
return false;;
}
x += (int16_t)(buf[0] + (buf[1] << 8));
y += (int16_t)(buf[2] + (buf[3] << 8));
z += (int16_t)(buf[4] + (buf[5] << 8));
@ -120,9 +124,15 @@ static void adxl345Read(int16_t *accelData)
accelData[2] = z / i;
acc_samples = i;
} else {
i2cRead(ADXL345_ADDRESS, ADXL345_DATA_OUT, 6, buf);
if (!i2cRead(ADXL345_ADDRESS, ADXL345_DATA_OUT, 6, buf)) {
return false;
}
accelData[0] = buf[0] + (buf[1] << 8);
accelData[1] = buf[2] + (buf[3] << 8);
accelData[2] = buf[4] + (buf[5] << 8);
}
return true;
}

10
src/main/drivers/accgyro_bma280.c
View file

@ -33,7 +33,7 @@
#define BMA280_PMU_RANGE 0x0F
static void bma280Init(void);
static void bma280Read(int16_t *accelData);
static bool bma280Read(int16_t *accelData);
bool bma280Detect(acc_t *acc)
{
@ -57,14 +57,18 @@ static void bma280Init(void)
acc_1G = 512 * 8;
}
static void bma280Read(int16_t *accelData)
static bool bma280Read(int16_t *accelData)
{
uint8_t buf[6];
i2cRead(BMA280_ADDRESS, BMA280_ACC_X_LSB, 6, buf);
if (!i2cRead(BMA280_ADDRESS, BMA280_ACC_X_LSB, 6, buf)) {
return false;
}
// Data format is lsb<5:0><crap><new_data_bit> | msb<13:6>
accelData[0] = (int16_t)((buf[0] >> 2) + (buf[1] << 8));
accelData[1] = (int16_t)((buf[2] >> 2) + (buf[3] << 8));
accelData[2] = (int16_t)((buf[4] >> 2) + (buf[5] << 8));
return true;
}

12
src/main/drivers/accgyro_l3g4200d.c
View file

@ -57,7 +57,7 @@
static uint8_t mpuLowPassFilter = L3G4200D_DLPF_32HZ;
static void l3g4200dInit(void);
static void l3g4200dRead(int16_t *gyroADC);
static bool l3g4200dRead(int16_t *gyroADC);
bool l3g4200dDetect(gyro_t *gyro, uint16_t lpf)
{
@ -103,20 +103,24 @@ static void l3g4200dInit(void)
ack = i2cWrite(L3G4200D_ADDRESS, L3G4200D_CTRL_REG4, L3G4200D_FS_SEL_2000DPS);
if (!ack)
failureMode(3);
failureMode(FAILURE_ACC_INIT);
delay(5);
i2cWrite(L3G4200D_ADDRESS, L3G4200D_CTRL_REG1, L3G4200D_POWER_ON | mpuLowPassFilter);
}
// Read 3 gyro values into user-provided buffer. No overrun checking is done.
static void l3g4200dRead(int16_t *gyroADC)
static bool l3g4200dRead(int16_t *gyroADC)
{
uint8_t buf[6];
i2cRead(L3G4200D_ADDRESS, L3G4200D_AUTOINCR | L3G4200D_GYRO_OUT, 6, buf);
if (!i2cRead(L3G4200D_ADDRESS, L3G4200D_AUTOINCR | L3G4200D_GYRO_OUT, 6, buf)) {
return false;
}
gyroADC[X] = (int16_t)((buf[0] << 8) | buf[1]);
gyroADC[Y] = (int16_t)((buf[2] << 8) | buf[3]);
gyroADC[Z] = (int16_t)((buf[4] << 8) | buf[5]);
return true;
}

6
src/main/drivers/accgyro_l3gd20.c
View file

@ -68,8 +68,8 @@
static void l3gd20SpiInit(SPI_TypeDef *SPIx)
{
UNUSED(SPIx); // FIXME
GPIO_InitTypeDef GPIO_InitStructure;
SPI_InitTypeDef SPI_InitStructure;
RCC_AHBPeriphClockCmd(L3GD20_CS_GPIO_CLK_PERIPHERAL, ENABLE);
@ -120,7 +120,7 @@ void l3gd20GyroInit(void)
delay(100);
}
static void l3gd20GyroRead(int16_t *gyroADC)
static bool l3gd20GyroRead(int16_t *gyroADC)
{
uint8_t buf[6];
@ -143,6 +143,8 @@ static void l3gd20GyroRead(int16_t *gyroADC)
debug[1] = (int16_t)((buf[3] << 8) | buf[2]);
debug[2] = (int16_t)((buf[5] << 8) | buf[4]);
#endif
return true;
}
// Page 9 in datasheet, So - Sensitivity, Full Scale = 2000, 70 mdps/digit

11
src/main/drivers/accgyro_lsm303dlhc.c
View file

@ -131,14 +131,15 @@ void lsm303dlhcAccInit(void)
}
// Read 3 gyro values into user-provided buffer. No overrun checking is done.
static void lsm303dlhcAccRead(int16_t *gyroADC)
static bool lsm303dlhcAccRead(int16_t *gyroADC)
{
uint8_t buf[6];
bool ok = i2cRead(LSM303DLHC_ACCEL_ADDRESS, AUTO_INCREMENT_ENABLE | OUT_X_L_A, 6, buf);
bool ack = i2cRead(LSM303DLHC_ACCEL_ADDRESS, AUTO_INCREMENT_ENABLE | OUT_X_L_A, 6, buf);
if (!ok)
return;
if (!ack) {
return false;
}
// the values range from -8192 to +8191
gyroADC[X] = (int16_t)((buf[1] << 8) | buf[0]) / 2;
@ -150,6 +151,8 @@ static void lsm303dlhcAccRead(int16_t *gyroADC)
debug[1] = (int16_t)((buf[3] << 8) | buf[2]);
debug[2] = (int16_t)((buf[5] << 8) | buf[4]);
#endif
return true;
}
bool lsm303dlhcAccDetect(acc_t *acc)

11
src/main/drivers/accgyro_mma845x.c
View file

@ -78,7 +78,7 @@
static uint8_t device_id;
static void mma8452Init(void);
static void mma8452Read(int16_t *accelData);
static bool mma8452Read(int16_t *accelData);
bool mma8452Detect(acc_t *acc)
{
@ -132,12 +132,17 @@ static void mma8452Init(void)
acc_1G = 256;
}
static void mma8452Read(int16_t *accelData)
static bool mma8452Read(int16_t *accelData)
{
uint8_t buf[6];
i2cRead(MMA8452_ADDRESS, MMA8452_OUT_X_MSB, 6, buf);
if (!i2cRead(MMA8452_ADDRESS, MMA8452_OUT_X_MSB, 6, buf)) {
return false;
}
accelData[0] = ((int16_t)((buf[0] << 8) | buf[1]) >> 2) / 4;
accelData[1] = ((int16_t)((buf[2] << 8) | buf[3]) >> 2) / 4;
accelData[2] = ((int16_t)((buf[4] << 8) | buf[5]) >> 2) / 4;
return true;
}

22
src/main/drivers/accgyro_mpu3050.c
View file

@ -58,8 +58,8 @@
static uint8_t mpuLowPassFilter = MPU3050_DLPF_42HZ;
static void mpu3050Init(void);
static void mpu3050Read(int16_t *gyroADC);
static void mpu3050ReadTemp(int16_t *tempData);
static bool mpu3050Read(int16_t *gyroADC);
static bool mpu3050ReadTemp(int16_t *tempData);
bool mpu3050Detect(gyro_t *gyro, uint16_t lpf)
{
@ -112,7 +112,7 @@ static void mpu3050Init(void)
ack = i2cWrite(MPU3050_ADDRESS, MPU3050_SMPLRT_DIV, 0);
if (!ack)
failureMode(3);
failureMode(FAILURE_ACC_INIT);
i2cWrite(MPU3050_ADDRESS, MPU3050_DLPF_FS_SYNC, MPU3050_FS_SEL_2000DPS | mpuLowPassFilter);
i2cWrite(MPU3050_ADDRESS, MPU3050_INT_CFG, 0);
@ -121,21 +121,29 @@ static void mpu3050Init(void)
}
// Read 3 gyro values into user-provided buffer. No overrun checking is done.
static void mpu3050Read(int16_t *gyroADC)
static bool mpu3050Read(int16_t *gyroADC)
{
uint8_t buf[6];
i2cRead(MPU3050_ADDRESS, MPU3050_GYRO_OUT, 6, buf);
if (!i2cRead(MPU3050_ADDRESS, MPU3050_GYRO_OUT, 6, buf)) {
return false;
}
gyroADC[0] = (int16_t)((buf[0] << 8) | buf[1]);
gyroADC[1] = (int16_t)((buf[2] << 8) | buf[3]);
gyroADC[2] = (int16_t)((buf[4] << 8) | buf[5]);
return true;
}
static void mpu3050ReadTemp(int16_t *tempData)
static bool mpu3050ReadTemp(int16_t *tempData)
{
uint8_t buf[2];
i2cRead(MPU3050_ADDRESS, MPU3050_TEMP_OUT, 2, buf);
if (!i2cRead(MPU3050_ADDRESS, MPU3050_TEMP_OUT, 2, buf)) {
return false;
}
*tempData = 35 + ((int32_t)(buf[0] << 8 | buf[1]) + 13200) / 280;
return true;
}

44
src/main/drivers/accgyro_mpu6050.c
View file

@ -173,9 +173,9 @@ enum accel_fsr_e {
static uint8_t mpuLowPassFilter = INV_FILTER_42HZ;
static void mpu6050AccInit(void);
static void mpu6050AccRead(int16_t *accData);
static bool mpu6050AccRead(int16_t *accData);
static void mpu6050GyroInit(void);
static void mpu6050GyroRead(int16_t *gyroADC);
static bool mpu6050GyroRead(int16_t *gyroADC);
typedef enum {
MPU_6050_HALF_RESOLUTION,
@ -336,13 +336,13 @@ bool mpu6050AccDetect(const mpu6050Config_t *configToUse, acc_t *acc)
} else if (revision == 2) {
mpuAccelTrim = MPU_6050_FULL_RESOLUTION;
} else {
failureMode(5);
failureMode(FAILURE_ACC_INCOMPATIBLE);
}
} else {
i2cRead(MPU6050_ADDRESS, MPU_RA_PRODUCT_ID, 1, &productId);
revision = productId & 0x0F;
if (!revision) {
failureMode(5);
failureMode(FAILURE_ACC_INCOMPATIBLE);
} else if (revision == 4) {
mpuAccelTrim = MPU_6050_HALF_RESOLUTION;
} else {
@ -406,52 +406,60 @@ static void mpu6050AccInit(void)
}
}
static void mpu6050AccRead(int16_t *accData)
static bool mpu6050AccRead(int16_t *accData)
{
uint8_t buf[6];
if (!i2cRead(MPU6050_ADDRESS, MPU_RA_ACCEL_XOUT_H, 6, buf)) {
return;
bool ack = i2cRead(MPU6050_ADDRESS, MPU_RA_ACCEL_XOUT_H, 6, buf);
if (!ack) {
return false;
}
accData[0] = (int16_t)((buf[0] << 8) | buf[1]);
accData[1] = (int16_t)((buf[2] << 8) | buf[3]);
accData[2] = (int16_t)((buf[4] << 8) | buf[5]);
return true;
}
static void mpu6050GyroInit(void)
{
mpu6050GpioInit();
i2cWrite(MPU6050_ADDRESS, MPU_RA_PWR_MGMT_1, 0x80); //PWR_MGMT_1 -- DEVICE_RESET 1
bool ack;
ack = i2cWrite(MPU6050_ADDRESS, MPU_RA_PWR_MGMT_1, 0x80); //PWR_MGMT_1 -- DEVICE_RESET 1
delay(100);
i2cWrite(MPU6050_ADDRESS, MPU_RA_SMPLRT_DIV, 0x00); //SMPLRT_DIV -- SMPLRT_DIV = 0 Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)
i2cWrite(MPU6050_ADDRESS, MPU_RA_PWR_MGMT_1, 0x03); //PWR_MGMT_1 -- SLEEP 0; CYCLE 0; TEMP_DIS 0; CLKSEL 3 (PLL with Z Gyro reference)
ack = i2cWrite(MPU6050_ADDRESS, MPU_RA_SMPLRT_DIV, 0x00); //SMPLRT_DIV -- SMPLRT_DIV = 0 Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)
ack = i2cWrite(MPU6050_ADDRESS, MPU_RA_PWR_MGMT_1, 0x03); //PWR_MGMT_1 -- SLEEP 0; CYCLE 0; TEMP_DIS 0; CLKSEL 3 (PLL with Z Gyro reference)
delay(15); //PLL Settling time when changing CLKSEL is max 10ms. Use 15ms to be sure
i2cWrite(MPU6050_ADDRESS, MPU_RA_CONFIG, mpuLowPassFilter); //CONFIG -- EXT_SYNC_SET 0 (disable input pin for data sync) ; default DLPF_CFG = 0 => ACC bandwidth = 260Hz GYRO bandwidth = 256Hz)
i2cWrite(MPU6050_ADDRESS, MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3); //GYRO_CONFIG -- FS_SEL = 3: Full scale set to 2000 deg/sec
ack = i2cWrite(MPU6050_ADDRESS, MPU_RA_CONFIG, mpuLowPassFilter); //CONFIG -- EXT_SYNC_SET 0 (disable input pin for data sync) ; default DLPF_CFG = 0 => ACC bandwidth = 260Hz GYRO bandwidth = 256Hz)
ack = i2cWrite(MPU6050_ADDRESS, MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3); //GYRO_CONFIG -- FS_SEL = 3: Full scale set to 2000 deg/sec
// ACC Init stuff. Moved into gyro init because the reset above would screw up accel config. Oops.
// Accel scale 8g (4096 LSB/g)
i2cWrite(MPU6050_ADDRESS, MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3);
ack = i2cWrite(MPU6050_ADDRESS, MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3);
i2cWrite(MPU6050_ADDRESS, MPU_RA_INT_PIN_CFG,
ack = i2cWrite(MPU6050_ADDRESS, MPU_RA_INT_PIN_CFG,
0 << 7 | 0 << 6 | 0 << 5 | 0 << 4 | 0 << 3 | 0 << 2 | 1 << 1 | 0 << 0); // INT_PIN_CFG -- INT_LEVEL_HIGH, INT_OPEN_DIS, LATCH_INT_DIS, INT_RD_CLEAR_DIS, FSYNC_INT_LEVEL_HIGH, FSYNC_INT_DIS, I2C_BYPASS_EN, CLOCK_DIS
#ifdef USE_MPU_DATA_READY_SIGNAL
i2cWrite(MPU6050_ADDRESS, MPU_RA_INT_ENABLE, MPU_RF_DATA_RDY_EN);
ack = i2cWrite(MPU6050_ADDRESS, MPU_RA_INT_ENABLE, MPU_RF_DATA_RDY_EN);
#endif
UNUSED(ack);
}
static void mpu6050GyroRead(int16_t *gyroADC)
static bool mpu6050GyroRead(int16_t *gyroADC)
{
uint8_t buf[6];
if (!i2cRead(MPU6050_ADDRESS, MPU_RA_GYRO_XOUT_H, 6, buf)) {
return;
bool ack = i2cRead(MPU6050_ADDRESS, MPU_RA_GYRO_XOUT_H, 6, buf);
if (!ack) {
return false;
}
gyroADC[0] = (int16_t)((buf[0] << 8) | buf[1]);
gyroADC[1] = (int16_t)((buf[2] << 8) | buf[3]);
gyroADC[2] = (int16_t)((buf[4] << 8) | buf[5]);
return true;
}

12
src/main/drivers/accgyro_spi_mpu6000.c
View file

@ -125,8 +125,8 @@ static bool mpuSpi6000InitDone = false;
#define DISABLE_MPU6000 GPIO_SetBits(MPU6000_CS_GPIO, MPU6000_CS_PIN)
#define ENABLE_MPU6000 GPIO_ResetBits(MPU6000_CS_GPIO, MPU6000_CS_PIN)
void mpu6000SpiGyroRead(int16_t *gyroADC);
void mpu6000SpiAccRead(int16_t *gyroADC);
bool mpu6000SpiGyroRead(int16_t *gyroADC);
bool mpu6000SpiAccRead(int16_t *gyroADC);
static void mpu6000WriteRegister(uint8_t reg, uint8_t data)
{
@ -313,7 +313,7 @@ bool mpu6000SpiGyroDetect(gyro_t *gyro, uint16_t lpf)
return true;
}
void mpu6000SpiGyroRead(int16_t *gyroData)
bool mpu6000SpiGyroRead(int16_t *gyroData)
{
uint8_t buf[6];
@ -324,9 +324,11 @@ void mpu6000SpiGyroRead(int16_t *gyroData)
gyroData[X] = (int16_t)((buf[0] << 8) | buf[1]);
gyroData[Y] = (int16_t)((buf[2] << 8) | buf[3]);
gyroData[Z] = (int16_t)((buf[4] << 8) | buf[5]);
return true;
}
void mpu6000SpiAccRead(int16_t *gyroData)
bool mpu6000SpiAccRead(int16_t *gyroData)
{
uint8_t buf[6];
@ -337,4 +339,6 @@ void mpu6000SpiAccRead(int16_t *gyroData)
gyroData[X] = (int16_t)((buf[0] << 8) | buf[1]);
gyroData[Y] = (int16_t)((buf[2] << 8) | buf[3]);
gyroData[Z] = (int16_t)((buf[4] << 8) | buf[5]);
return true;
}

4
src/main/drivers/accgyro_spi_mpu6000.h
View file

@ -16,5 +16,5 @@
bool mpu6000SpiAccDetect(acc_t *acc);
bool mpu6000SpiGyroDetect(gyro_t *gyro, uint16_t lpf);
void mpu6000SpiGyroRead(int16_t *gyroADC);
void mpu6000SpiAccRead(int16_t *gyroADC);
bool mpu6000SpiGyroRead(int16_t *gyroADC);
bool mpu6000SpiAccRead(int16_t *gyroADC);

12
src/main/drivers/accgyro_spi_mpu6500.c
View file

@ -72,9 +72,9 @@ enum accel_fsr_e {
static uint8_t mpuLowPassFilter = INV_FILTER_42HZ;
static void mpu6500AccInit(void);
static void mpu6500AccRead(int16_t *accData);
static bool mpu6500AccRead(int16_t *accData);
static void mpu6500GyroInit(void);
static void mpu6500GyroRead(int16_t *gyroADC);
static bool mpu6500GyroRead(int16_t *gyroADC);
extern uint16_t acc_1G;
@ -194,7 +194,7 @@ static void mpu6500AccInit(void)
acc_1G = 512 * 8;
}
static void mpu6500AccRead(int16_t *accData)
static bool mpu6500AccRead(int16_t *accData)
{
uint8_t buf[6];
@ -203,6 +203,8 @@ static void mpu6500AccRead(int16_t *accData)
accData[X] = (int16_t)((buf[0] << 8) | buf[1]);
accData[Y] = (int16_t)((buf[2] << 8) | buf[3]);
accData[Z] = (int16_t)((buf[4] << 8) | buf[5]);
return true;
}
static void mpu6500GyroInit(void)
@ -229,7 +231,7 @@ static void mpu6500GyroInit(void)
mpu6500WriteRegister(MPU6500_RA_RATE_DIV, 0); // 1kHz S/R
}
static void mpu6500GyroRead(int16_t *gyroADC)
static bool mpu6500GyroRead(int16_t *gyroADC)
{
uint8_t buf[6];
@ -238,4 +240,6 @@ static void mpu6500GyroRead(int16_t *gyroADC)
gyroADC[X] = (int16_t)((buf[0] << 8) | buf[1]);
gyroADC[Y] = (int16_t)((buf[2] << 8) | buf[3]);
gyroADC[Z] = (int16_t)((buf[4] << 8) | buf[5]);
return true;
}

22
src/main/drivers/adc_stm32f10x.c
View file

@ -73,17 +73,6 @@ void adcInit(drv_adc_config_t *init)
}
#endif
#ifdef EXTERNAL1_ADC_GPIO
if (init->enableExternal1) {
GPIO_InitStructure.GPIO_Pin = EXTERNAL1_ADC_GPIO_PIN;
GPIO_Init(EXTERNAL1_ADC_GPIO, &GPIO_InitStructure);
adcConfig[ADC_EXTERNAL1].adcChannel = EXTERNAL1_ADC_CHANNEL;
adcConfig[ADC_EXTERNAL1].dmaIndex = configuredAdcChannels++;
adcConfig[ADC_EXTERNAL1].enabled = true;
adcConfig[ADC_EXTERNAL1].sampleTime = ADC_SampleTime_239Cycles5;
}
#endif
#ifdef RSSI_ADC_GPIO
if (init->enableRSSI) {
GPIO_InitStructure.GPIO_Pin = RSSI_ADC_GPIO_PIN;
@ -95,6 +84,17 @@ void adcInit(drv_adc_config_t *init)
}
#endif
#ifdef EXTERNAL1_ADC_GPIO
if (init->enableExternal1) {
GPIO_InitStructure.GPIO_Pin = EXTERNAL1_ADC_GPIO_PIN;
GPIO_Init(EXTERNAL1_ADC_GPIO, &GPIO_InitStructure);
adcConfig[ADC_EXTERNAL1].adcChannel = EXTERNAL1_ADC_CHANNEL;
adcConfig[ADC_EXTERNAL1].dmaIndex = configuredAdcChannels++;
adcConfig[ADC_EXTERNAL1].enabled = true;
adcConfig[ADC_EXTERNAL1].sampleTime = ADC_SampleTime_239Cycles5;
}
#endif
#ifdef CURRENT_METER_ADC_GPIO
if (init->enableCurrentMeter) {
GPIO_InitStructure.GPIO_Pin = CURRENT_METER_ADC_GPIO_PIN;

53
src/main/drivers/barometer_bmp085.c
View file

@ -34,9 +34,8 @@
#ifdef BARO
#if defined(BARO_EOC_GPIO)
// BMP085, Standard address 0x77
static bool isConversionComplete = false;
static uint16_t bmp085ConversionOverrun = 0;
static bool isEOCConnected = true;
// EXTI14 for BMP085 End of Conversion Interrupt
void BMP085_EOC_EXTI_Handler(void) {
@ -107,6 +106,10 @@ typedef struct {
#define SMD500_PARAM_MI 3791 //calibration parameter
STATIC_UNIT_TESTED bmp085_t bmp085;
#define UT_DELAY 6000 // 1.5ms margin according to the spec (4.5ms T conversion time)
#define UP_DELAY 27000 // 6000+21000=27000 1.5ms margin according to the spec (25.5ms P conversion time with OSS=3)
static bool bmp085InitDone = false;
STATIC_UNIT_TESTED uint16_t bmp085_ut; // static result of temperature measurement
STATIC_UNIT_TESTED uint32_t bmp085_up; // static result of pressure measurement
@ -133,7 +136,6 @@ void bmp085InitXCLRGpio(const bmp085Config_t *config) {
RCC_APB2PeriphClockCmd(config->gpioAPB2Peripherals, ENABLE);
// PC13, PC14 (Barometer XCLR reset output, EOC input)
gpio.pin = config->xclrGpioPin;
gpio.speed = Speed_2MHz;
gpio.mode = Mode_Out_PP;
@ -163,7 +165,7 @@ bool bmp085Detect(const bmp085Config_t *config, baro_t *baro)
bmp085InitXCLRGpio(config);
gpio.pin = config->eocGpioPin;
gpio.mode = Mode_IN_FLOATING;
gpio.mode = Mode_IPD;
gpioInit(config->eocGpioPort, &gpio);
BMP085_ON;
@ -200,19 +202,22 @@ bool bmp085Detect(const bmp085Config_t *config, baro_t *baro)
bmp085.ml_version = BMP085_GET_BITSLICE(data, BMP085_ML_VERSION); /* get ML Version */
bmp085.al_version = BMP085_GET_BITSLICE(data, BMP085_AL_VERSION); /* get AL Version */
bmp085_get_cal_param(); /* readout bmp085 calibparam structure */
bmp085InitDone = true;
baro->ut_delay = 6000; // 1.5ms margin according to the spec (4.5ms T conversion time)
baro->up_delay = 27000; // 6000+21000=27000 1.5ms margin according to the spec (25.5ms P conversion time with OSS=3)
baro->ut_delay = UT_DELAY;
baro->up_delay = UP_DELAY;
baro->start_ut = bmp085_start_ut;
baro->get_ut = bmp085_get_ut;
baro->start_up = bmp085_start_up;
baro->get_up = bmp085_get_up;
baro->calculate = bmp085_calculate;
#if defined(BARO_EOC_GPIO)
isEOCConnected = bmp085TestEOCConnected(config);
#endif
bmp085InitDone = true;
return true;
}
}
#ifdef BARO_EOC_GPIO
#if defined(BARO_EOC_GPIO)
EXTI_InitTypeDef EXTI_InitStructure;
EXTI_StructInit(&EXTI_InitStructure);
EXTI_InitStructure.EXTI_Line = EXTI_Line14;
@ -293,9 +298,9 @@ static void bmp085_get_ut(void)
uint8_t data[2];
#if defined(BARO_EOC_GPIO)
if (!isConversionComplete) {
bmp085ConversionOverrun++;
return; // keep old value
// return old baro value if conversion time exceeds datasheet max when EOC is connected
if ((isEOCConnected) && (!isConversionComplete)) {
return;
}
#endif
@ -324,11 +329,10 @@ static void bmp085_get_up(void)
{
uint8_t data[3];
#if defined(BARO_EOC_GPIO)
// wait in case of cockup
if (!isConversionComplete) {
bmp085ConversionOverrun++;
return; // keep old value
#if defined(BARO_EOC_GPIO)
// return old baro value if conversion time exceeds datasheet max when EOC is connected
if ((isEOCConnected) && (!isConversionComplete)) {
return;
}
#endif
@ -372,4 +376,21 @@ static void bmp085_get_cal_param(void)
bmp085.cal_param.md = (data[20] << 8) | data[21];
}
#if defined(BARO_EOC_GPIO)
bool bmp085TestEOCConnected(const bmp085Config_t *config)
{
if (!bmp085InitDone) {
bmp085_start_ut();
delayMicroseconds(UT_DELAY * 2); // wait twice as long as normal, just to be sure
// conversion should have finished now so check if EOC is high
uint8_t status = GPIO_ReadInputDataBit(config->eocGpioPort, config->eocGpioPin);
if (status) {
return true;
}
}
return false; // assume EOC is not connected
}
#endif
#endif /* BARO */

4
src/main/drivers/barometer_bmp085.h
View file

@ -28,6 +28,10 @@ typedef struct bmp085Config_s {
bool bmp085Detect(const bmp085Config_t *config, baro_t *baro);
void bmp085Disable(const bmp085Config_t *config);
#if defined(BARO_EOC_GPIO)
bool bmp085TestEOCConnected(const bmp085Config_t *config);
#endif
#ifdef UNIT_TEST
void RCC_APB2PeriphClockCmd(uint32_t RCC_APB2Periph, FunctionalState NewState);
#endif

13
src/main/drivers/compass_ak8975.c
View file

@ -111,7 +111,7 @@ void ak8975Init()
#define BIT_STATUS2_REG_DATA_ERROR (1 << 2)
#define BIT_STATUS2_REG_MAG_SENSOR_OVERFLOW (1 << 3)
void ak8975Read(int16_t *magData)
bool ak8975Read(int16_t *magData)
{
bool ack;
UNUSED(ack);
@ -120,7 +120,7 @@ void ak8975Read(int16_t *magData)
ack = i2cRead(AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_STATUS1, 1, &status);
if (!ack || (status & BIT_STATUS1_REG_DATA_READY) == 0) {
return;
return false;
}
#if 1 // USE_I2C_SINGLE_BYTE_READS
@ -129,22 +129,22 @@ void ak8975Read(int16_t *magData)
for (uint8_t i = 0; i < 6; i++) {
ack = i2cRead(AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_HXL + i, 1, &buf[i]); // read from AK8975_MAG_REG_HXL to AK8975_MAG_REG_HZH
if (!ack) {
break;
return false
}
}
#endif
ack = i2cRead(AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_STATUS2, 1, &status);
if (!ack) {
return;
return false;
}
if (status & BIT_STATUS2_REG_DATA_ERROR) {
return;
return false;
}
if (status & BIT_STATUS2_REG_MAG_SENSOR_OVERFLOW) {
return;
return false;
}
magData[X] = -(int16_t)(buf[1] << 8 | buf[0]) * 4;
@ -153,4 +153,5 @@ void ak8975Read(int16_t *magData)
ack = i2cWrite(AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_CNTL, 0x01); // start reading again
return true;
}

2
src/main/drivers/compass_ak8975.h
View file

@ -19,4 +19,4 @@
bool ak8975detect(mag_t *mag);
void ak8975Init(void);
void ak8975Read(int16_t *magData);
bool ak8975Read(int16_t *magData);

9
src/main/drivers/compass_hmc5883l.c
View file

@ -305,14 +305,19 @@ void hmc5883lInit(void)
hmc5883lConfigureDataReadyInterruptHandling();
}
void hmc5883lRead(int16_t *magData)
bool hmc5883lRead(int16_t *magData)
{
uint8_t buf[6];
i2cRead(MAG_ADDRESS, MAG_DATA_REGISTER, 6, buf);
bool ack = i2cRead(MAG_ADDRESS, MAG_DATA_REGISTER, 6, buf);
if (!ack) {
return false;
}
// During calibration, magGain is 1.0, so the read returns normal non-calibrated values.
// After calibration is done, magGain is set to calculated gain values.
magData[X] = (int16_t)(buf[0] << 8 | buf[1]) * magGain[X];
magData[Z] = (int16_t)(buf[2] << 8 | buf[3]) * magGain[Z];
magData[Y] = (int16_t)(buf[4] << 8 | buf[5]) * magGain[Y];
return true;
}

2
src/main/drivers/compass_hmc5883l.h
View file

@ -35,4 +35,4 @@ typedef struct hmc5883Config_s {
bool hmc5883lDetect(mag_t* mag, const hmc5883Config_t *hmc5883ConfigToUse);
void hmc5883lInit(void);
void hmc5883lRead(int16_t *magData);
bool hmc5883lRead(int16_t *magData);

6
src/main/drivers/pwm_mapping.c
View file

@ -580,12 +580,12 @@ pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init)
if (type == MAP_TO_PPM_INPUT) {
#ifdef CC3D
if (init->useOneshot) {
if (init->useOneshot || isMotorBrushed(init->motorPwmRate)) {
ppmAvoidPWMTimerClash(timerHardwarePtr, TIM4);
}
#endif
#ifdef SPARKY
if (init->useOneshot) {
if (init->useOneshot || isMotorBrushed(init->motorPwmRate)) {
ppmAvoidPWMTimerClash(timerHardwarePtr, TIM2);
}
#endif
@ -596,7 +596,7 @@ pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init)
} else if (type == MAP_TO_MOTOR_OUTPUT) {
if (init->useOneshot) {
pwmOneshotMotorConfig(timerHardwarePtr, pwmOutputConfiguration.motorCount);
} else if (init->motorPwmRate > 500) {
} else if (isMotorBrushed(init->motorPwmRate)) {
pwmBrushedMotorConfig(timerHardwarePtr, pwmOutputConfiguration.motorCount, init->motorPwmRate, init->idlePulse);
} else {
pwmBrushlessMotorConfig(timerHardwarePtr, pwmOutputConfiguration.motorCount, init->motorPwmRate, init->idlePulse);

2
src/main/drivers/pwm_mapping.h
View file

@ -35,6 +35,8 @@
#define PWM_TIMER_MHZ 1
#define ONESHOT125_TIMER_MHZ 8
#define PWM_BRUSHED_TIMER_MHZ 8
typedef struct sonarGPIOConfig_s {
GPIO_TypeDef *gpio;

6
src/main/drivers/pwm_output.c
View file

@ -47,7 +47,6 @@ static pwmOutputPort_t *motors[MAX_PWM_MOTORS];
#ifdef USE_SERVOS
static pwmOutputPort_t *servos[MAX_PWM_SERVOS];
#endif
#define PWM_BRUSHED_TIMER_MHZ 8
static uint8_t allocatedOutputPortCount = 0;
@ -172,6 +171,11 @@ void pwmCompleteOneshotMotorUpdate(uint8_t motorCount)
}
}
bool isMotorBrushed(uint16_t motorPwmRate)
{
return (motorPwmRate > 500);
}
void pwmBrushedMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse)
{
uint32_t hz = PWM_BRUSHED_TIMER_MHZ * 1000000;

2
src/main/drivers/pwm_output.h
View file

@ -22,3 +22,5 @@ void pwmShutdownPulsesForAllMotors(uint8_t motorCount);
void pwmCompleteOneshotMotorUpdate(uint8_t motorCount);
void pwmWriteServo(uint8_t index, uint16_t value);
bool isMotorBrushed(uint16_t motorPwmRate);

2
src/main/drivers/sensor.h
View file

@ -18,4 +18,4 @@
#pragma once
typedef void (*sensorInitFuncPtr)(void); // sensor init prototype
typedef void (*sensorReadFuncPtr)(int16_t *data); // sensor read and align prototype
typedef bool (*sensorReadFuncPtr)(int16_t *data); // sensor read and align prototype

62
src/main/drivers/system.c
View file

@ -47,7 +47,7 @@ void registerExti15_10_CallbackHandler(extiCallbackHandler *fn)
return;
}
}
failureMode(15); // EXTI15_10_CALLBACK_HANDLER_COUNT is too low for the amount of handlers required.
failureMode(FAILURE_DEVELOPER); // EXTI15_10_CALLBACK_HANDLER_COUNT is too low for the amount of handlers required.
}
void unregisterExti15_10_CallbackHandler(extiCallbackHandler *fn)
@ -99,6 +99,12 @@ uint32_t micros(void)
do {
ms = sysTickUptime;
cycle_cnt = SysTick->VAL;
/*
* If the SysTick timer expired during the previous instruction, we need to give it a little time for that
* interrupt to be delivered before we can recheck sysTickUptime:
*/
asm volatile("\tnop\n");
} while (ms != sysTickUptime);
return (ms * 1000) + (usTicks * 1000 - cycle_cnt) / usTicks;
}
@ -188,21 +194,49 @@ void delay(uint32_t ms)
delayMicroseconds(1000);
}
// FIXME replace mode with an enum so usage can be tracked, currently mode is a magic number
void failureMode(uint8_t mode)
{
uint8_t flashesRemaining = 10;
#define SHORT_FLASH_DURATION 50
#define CODE_FLASH_DURATION 250
LED1_ON;
LED0_OFF;
while (flashesRemaining--) {
LED1_TOGGLE;
LED0_TOGGLE;
delay(475 * mode - 2);
BEEP_ON;
delay(25);
BEEP_OFF;
void failureMode(failureMode_e mode)
{
int codeRepeatsRemaining = 10;
int codeFlashesRemaining;
int shortFlashesRemaining;
while (codeRepeatsRemaining--) {
LED1_ON;
LED0_OFF;
shortFlashesRemaining = 5;
codeFlashesRemaining = mode + 1;
uint8_t flashDuration = SHORT_FLASH_DURATION;
while (shortFlashesRemaining || codeFlashesRemaining) {
LED1_TOGGLE;
LED0_TOGGLE;
BEEP_ON;
delay(flashDuration);
LED1_TOGGLE;
LED0_TOGGLE;
BEEP_OFF;
delay(flashDuration);
if (shortFlashesRemaining) {
shortFlashesRemaining--;
if (shortFlashesRemaining == 0) {
delay(500);
flashDuration = CODE_FLASH_DURATION;
}
} else {
codeFlashesRemaining--;
}
}
delay(1000);
}
#ifdef DEBUG
systemReset();
#else
systemResetToBootloader();
#endif
}

10
src/main/drivers/system.h
View file

@ -42,3 +42,13 @@ void registerExti15_10_CallbackHandler(extiCallbackHandler *fn);
void unregisterExti15_10_CallbackHandler(extiCallbackHandler *fn);
extern uint32_t cachedRccCsrValue;
typedef enum {
FAILURE_DEVELOPER = 0,
FAILURE_MISSING_ACC,
FAILURE_ACC_INIT,
FAILURE_ACC_INCOMPATIBLE,
FAILURE_INVALID_EEPROM_CONTENTS,
FAILURE_FLASH_WRITE_FAILED
} failureMode_e;

190
src/main/flight/failsafe.c
View file

@ -23,10 +23,12 @@
#include "common/axis.h"
#include "rx/rx.h"
#include "drivers/system.h"
#include "io/beeper.h"
#include "io/escservo.h"
#include "io/rc_controls.h"
#include "config/runtime_config.h"
#include "config/config.h"
#include "flight/failsafe.h"
@ -47,10 +49,19 @@ static failsafeConfig_t *failsafeConfig;
static rxConfig_t *rxConfig;
static uint16_t deadband3dThrottle; // default throttle deadband from MIDRC
static void failsafeReset(void)
{
failsafeState.counter = 0;
failsafeState.rxDataFailurePeriod = PERIOD_RXDATA_FAILURE + failsafeConfig->failsafe_delay * MILLIS_PER_TENTH_SECOND;
failsafeState.validRxDataReceivedAt = 0;
failsafeState.validRxDataFailedAt = 0;
failsafeState.throttleLowPeriod = 0;
failsafeState.landingShouldBeFinishedAt = 0;
failsafeState.receivingRxDataPeriod = 0;
failsafeState.receivingRxDataPeriodPreset = 0;
failsafeState.phase = FAILSAFE_IDLE;
failsafeState.rxLinkState = FAILSAFE_RXLINK_DOWN;
}
/*
@ -62,10 +73,11 @@ void useFailsafeConfig(failsafeConfig_t *failsafeConfigToUse)
failsafeReset();
}
void failsafeInit(rxConfig_t *intialRxConfig)
void failsafeInit(rxConfig_t *intialRxConfig, uint16_t deadband3d_throttle)
{
rxConfig = intialRxConfig;
deadband3dThrottle = deadband3d_throttle;
failsafeState.events = 0;
failsafeState.monitoring = false;
@ -77,13 +89,6 @@ failsafePhase_e failsafePhase()
return failsafeState.phase;
}
#define FAILSAFE_COUNTER_THRESHOLD 20
bool failsafeIsReceivingRxData(void)
{
return failsafeState.counter <= FAILSAFE_COUNTER_THRESHOLD;
}
bool failsafeIsMonitoring(void)
{
return failsafeState.monitoring;
@ -99,25 +104,17 @@ void failsafeStartMonitoring(void)
failsafeState.monitoring = true;
}
static bool failsafeHasTimerElapsed(void)
{
return failsafeState.counter > (5 * failsafeConfig->failsafe_delay);
}
static bool failsafeShouldForceLanding(bool armed)
{
return failsafeHasTimerElapsed() && armed;
}
static bool failsafeShouldHaveCausedLandingByNow(void)
{
return failsafeState.counter > 5 * (failsafeConfig->failsafe_delay + failsafeConfig->failsafe_off_delay);
return (millis() > failsafeState.landingShouldBeFinishedAt);
}
static void failsafeActivate(void)
{
failsafeState.active = true;
failsafeState.phase = FAILSAFE_LANDING;
ENABLE_FLIGHT_MODE(FAILSAFE_MODE);
failsafeState.landingShouldBeFinishedAt = millis() + failsafeConfig->failsafe_off_delay * MILLIS_PER_TENTH_SECOND;
failsafeState.events++;
}
@ -130,27 +127,52 @@ static void failsafeApplyControlInput(void)
rcData[THROTTLE] = failsafeConfig->failsafe_throttle;
}
bool failsafeIsReceivingRxData(void)
{
return (failsafeState.rxLinkState == FAILSAFE_RXLINK_UP);
}
void failsafeOnRxSuspend(uint32_t usSuspendPeriod)
{
failsafeState.validRxDataReceivedAt += (usSuspendPeriod / 1000); // / 1000 to convert micros to millis
}
void failsafeOnRxResume(void)
{
failsafeState.validRxDataReceivedAt = millis(); // prevent RX link down trigger, restart rx link up
failsafeState.rxLinkState = FAILSAFE_RXLINK_UP; // do so while rx link is up
}
void failsafeOnValidDataReceived(void)
{
if (failsafeState.counter > FAILSAFE_COUNTER_THRESHOLD)
failsafeState.counter -= FAILSAFE_COUNTER_THRESHOLD;
else
failsafeState.counter = 0;
failsafeState.validRxDataReceivedAt = millis();
if ((failsafeState.validRxDataReceivedAt - failsafeState.validRxDataFailedAt) > PERIOD_RXDATA_RECOVERY) {
failsafeState.rxLinkState = FAILSAFE_RXLINK_UP;
}
}
void failsafeOnValidDataFailed(void)
{
failsafeState.validRxDataFailedAt = millis();
if ((failsafeState.validRxDataFailedAt - failsafeState.validRxDataReceivedAt) > failsafeState.rxDataFailurePeriod) {
failsafeState.rxLinkState = FAILSAFE_RXLINK_DOWN;
}
}
void failsafeUpdateState(void)
{
bool receivingRxData = failsafeIsReceivingRxData();
bool armed = ARMING_FLAG(ARMED);
beeperMode_e beeperMode = BEEPER_SILENCE;
if (receivingRxData) {
failsafeState.phase = FAILSAFE_IDLE;
failsafeState.active = false;
} else {
beeperMode = BEEPER_RX_LOST;
if (!failsafeIsMonitoring()) {
return;
}
bool receivingRxData = failsafeIsReceivingRxData();
bool armed = ARMING_FLAG(ARMED);
bool failsafeSwitchIsOn = IS_RC_MODE_ACTIVE(BOXFAILSAFE);
beeperMode_e beeperMode = BEEPER_SILENCE;
if (!receivingRxData) {
beeperMode = BEEPER_RX_LOST;
}
bool reprocessState;
@ -159,50 +181,100 @@ void failsafeUpdateState(void)
switch (failsafeState.phase) {
case FAILSAFE_IDLE:
if (!receivingRxData && armed) {
failsafeState.phase = FAILSAFE_RX_LOSS_DETECTED;
reprocessState = true;
if (armed) {
// Track throttle command below minimum time
if (THROTTLE_HIGH == calculateThrottleStatus(rxConfig, deadband3dThrottle)) {
failsafeState.throttleLowPeriod = millis() + failsafeConfig->failsafe_throttle_low_delay * MILLIS_PER_TENTH_SECOND;
}
// Kill switch logic (must be independent of receivingRxData to skip PERIOD_RXDATA_FAILURE delay before disarming)
if (failsafeSwitchIsOn && failsafeConfig->failsafe_kill_switch) {
// KillswitchEvent: failsafe switch is configured as KILL switch and is switched ON
failsafeActivate();
failsafeState.phase = FAILSAFE_LANDED; // skip auto-landing procedure
failsafeState.receivingRxDataPeriodPreset = PERIOD_OF_1_SECONDS; // require 1 seconds of valid rxData
reprocessState = true;
} else if (!receivingRxData) {
if (millis() > failsafeState.throttleLowPeriod) {
// JustDisarm: throttle was LOW for at least 'failsafe_throttle_low_delay' seconds
failsafeActivate();
failsafeState.phase = FAILSAFE_LANDED; // skip auto-landing procedure
failsafeState.receivingRxDataPeriodPreset = PERIOD_OF_3_SECONDS; // require 3 seconds of valid rxData
} else {
failsafeState.phase = FAILSAFE_RX_LOSS_DETECTED;
}
reprocessState = true;
}
} else {
// When NOT armed, show rxLinkState of failsafe switch in GUI (failsafe mode)
if (failsafeSwitchIsOn) {
ENABLE_FLIGHT_MODE(FAILSAFE_MODE);
} else {
DISABLE_FLIGHT_MODE(FAILSAFE_MODE);
}
// Throttle low period expired (= low long enough for JustDisarm)
failsafeState.throttleLowPeriod = 0;
}
break;
case FAILSAFE_RX_LOSS_DETECTED:
if (failsafeShouldForceLanding(armed)) {
if (receivingRxData) {
failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED;
} else {
// Stabilize, and set Throttle to specified level
failsafeActivate();
reprocessState = true;
}
reprocessState = true;
break;
case FAILSAFE_LANDING:
if (receivingRxData) {
failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED;
reprocessState = true;
}
if (armed) {
failsafeApplyControlInput();
beeperMode = BEEPER_RX_LOST_LANDING;
}
if (failsafeShouldHaveCausedLandingByNow() || !armed) {
failsafeState.receivingRxDataPeriodPreset = PERIOD_OF_30_SECONDS; // require 30 seconds of valid rxData
failsafeState.phase = FAILSAFE_LANDED;
reprocessState = true;
}
break;
case FAILSAFE_LANDED:
if (!armed) {
break;
}
// This will prevent the automatic rearm if failsafe shuts it down and prevents
// to restart accidently by just reconnect to the tx - you will have to switch off first to rearm
ENABLE_ARMING_FLAG(PREVENT_ARMING);
failsafeState.active = false;
ENABLE_ARMING_FLAG(PREVENT_ARMING); // To prevent accidently rearming by an intermittent rx link
mwDisarm();
failsafeState.receivingRxDataPeriod = millis() + failsafeState.receivingRxDataPeriodPreset; // set required period of valid rxData
failsafeState.phase = FAILSAFE_RX_LOSS_MONITORING;
reprocessState = true;
break;
case FAILSAFE_RX_LOSS_MONITORING:
// Monitoring the rx link to allow rearming when it has become good for > `receivingRxDataPeriodPreset` time.
if (receivingRxData) {
if (millis() > failsafeState.receivingRxDataPeriod) {
// rx link is good now, when arming via ARM switch, it must be OFF first
if (!(!isUsingSticksForArming() && IS_RC_MODE_ACTIVE(BOXARM))) {
DISABLE_ARMING_FLAG(PREVENT_ARMING);
failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED;
reprocessState = true;
}
}
} else {
failsafeState.receivingRxDataPeriod = millis() + failsafeState.receivingRxDataPeriodPreset;
}
break;
case FAILSAFE_RX_LOSS_RECOVERED:
// Entering IDLE with the requirement that throttle first must be at min_check for failsafe_throttle_low_delay period.
// This is to prevent that JustDisarm is activated on the next iteration.
// Because that would have the effect of shutting down failsafe handling on intermittent connections.
failsafeState.throttleLowPeriod = millis() + failsafeConfig->failsafe_throttle_low_delay * MILLIS_PER_TENTH_SECOND;
failsafeState.phase = FAILSAFE_IDLE;
failsafeState.active = false;
DISABLE_FLIGHT_MODE(FAILSAFE_MODE);
reprocessState = true;
break;
default:
@ -214,11 +286,3 @@ void failsafeUpdateState(void)
beeper(beeperMode);
}
}
/**
* Should be called once when RX data is processed by the system.
*/
void failsafeOnRxCycleStarted(void)
{
failsafeState.counter++;
}

32
src/main/flight/failsafe.h
View file

@ -18,26 +18,50 @@
#pragma once
#define FAILSAFE_POWER_ON_DELAY_US (1000 * 1000 * 5)
#define MILLIS_PER_TENTH_SECOND 100
#define MILLIS_PER_SECOND 1000
#define PERIOD_OF_1_SECONDS 1 * MILLIS_PER_SECOND
#define PERIOD_OF_3_SECONDS 3 * MILLIS_PER_SECOND
#define PERIOD_OF_30_SECONDS 30 * MILLIS_PER_SECOND
#define PERIOD_RXDATA_FAILURE 200 // millis
#define PERIOD_RXDATA_RECOVERY 200 // millis
typedef struct failsafeConfig_s {
uint8_t failsafe_delay; // Guard time for failsafe activation after signal lost. 1 step = 0.1sec - 1sec in example (10)
uint8_t failsafe_off_delay; // Time for Landing before motors stop in 0.1sec. 1 step = 0.1sec - 20sec in example (200)
uint16_t failsafe_throttle; // Throttle level used for landing - specify value between 1000..2000 (pwm pulse width for slightly below hover). center throttle = 1500.
uint8_t failsafe_kill_switch; // failsafe switch action is 0: identical to rc link loss, 1: disarms instantly
uint16_t failsafe_throttle_low_delay; // Time throttle stick must have been below 'min_check' to "JustDisarm" instead of "full failsafe procedure".
} failsafeConfig_t;
typedef enum {
FAILSAFE_IDLE = 0,
FAILSAFE_RX_LOSS_DETECTED,
FAILSAFE_LANDING,
FAILSAFE_LANDED
FAILSAFE_LANDED,
FAILSAFE_RX_LOSS_MONITORING,
FAILSAFE_RX_LOSS_RECOVERED
} failsafePhase_e;
typedef enum {
FAILSAFE_RXLINK_DOWN = 0,
FAILSAFE_RXLINK_UP
} failsafeRxLinkState_e;
typedef struct failsafeState_s {
int16_t counter;
int16_t events;
bool monitoring;
bool active;
uint32_t rxDataFailurePeriod;
uint32_t validRxDataReceivedAt;
uint32_t validRxDataFailedAt;
uint32_t throttleLowPeriod; // throttle stick must have been below 'min_check' for this period
uint32_t landingShouldBeFinishedAt;
uint32_t receivingRxDataPeriod; // period for the required period of valid rxData
uint32_t receivingRxDataPeriodPreset; // preset for the required period of valid rxData
failsafePhase_e phase;
failsafeRxLinkState_e rxLinkState;
} failsafeState_t;
void useFailsafeConfig(failsafeConfig_t *failsafeConfigToUse);
@ -49,9 +73,11 @@ failsafePhase_e failsafePhase();
bool failsafeIsMonitoring(void);
bool failsafeIsActive(void);
bool failsafeIsReceivingRxData(void);
void failsafeOnRxSuspend(uint32_t suspendPeriod);
void failsafeOnRxResume(void);
void failsafeOnValidDataReceived(void);
void failsafeOnRxCycleStarted(void);
void failsafeOnValidDataFailed(void);

2
src/main/flight/imu.c
View file

@ -63,7 +63,7 @@ float gyroScaleRad;
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
float anglerad[ANGLE_INDEX_COUNT] = { 0.0f, 0.0f }; // absolute angle inclination in radians
static imuRuntimeConfig_t *imuRuntimeConfig;
static pidProfile_t *pidProfile;

8
src/main/flight/mixer.c
View file

@ -174,10 +174,10 @@ static const motorMixer_t mixerOctoFlatX[] = {
};
static const motorMixer_t mixerVtail4[] = {
{ 1.0f, 0.0f, 1.0f, 1.0f }, // REAR_R
{ 1.0f, -1.0f, -1.0f, 0.0f }, // FRONT_R
{ 1.0f, 0.0f, 1.0f, -1.0f }, // REAR_L
{ 1.0f, 1.0f, -1.0f, -0.0f }, // FRONT_L
{ 1.0f, -0.58f, 0.58f, 1.0f }, // REAR_R
{ 1.0f, -0.46f, -0.39f, -0.5f }, // FRONT_R
{ 1.0f, 0.58f, 0.58f, -1.0f }, // REAR_L
{ 1.0f, 0.46f, -0.39f, 0.5f }, // FRONT_L
};
static const motorMixer_t mixerAtail4[] = {

6
src/main/io/display.c
View file

@ -231,6 +231,12 @@ void updateFailsafeStatus(void)
case FAILSAFE_LANDED:
failsafeIndicator = 'L';
break;
case FAILSAFE_RX_LOSS_MONITORING:
failsafeIndicator = 'M';
break;
case FAILSAFE_RX_LOSS_RECOVERED:
failsafeIndicator = 'r';
break;
}
i2c_OLED_set_xy(SCREEN_CHARACTER_COLUMN_COUNT - 3, 0);
i2c_OLED_send_char(failsafeIndicator);

14
src/main/io/flashfs.c
View file

@ -481,7 +481,7 @@ int flashfsIdentifyStartOfFreeSpace()
/* We can choose whatever power of 2 size we like, which determines how much wastage of free space we'll have
* at the end of the last written data. But smaller blocksizes will require more searching.
*/
FREE_BLOCK_SIZE = 65536,
FREE_BLOCK_SIZE = 2048,
/* We don't expect valid data to ever contain this many consecutive uint32_t's of all 1 bits: */
FREE_BLOCK_TEST_SIZE_INTS = 4, // i.e. 16 bytes
@ -493,16 +493,20 @@ int flashfsIdentifyStartOfFreeSpace()
uint32_t ints[FREE_BLOCK_TEST_SIZE_INTS];
} testBuffer;
int left = 0;
int right = flashfsGetSize() / FREE_BLOCK_SIZE;
int mid, result = right;
int left = 0; // Smallest block index in the search region
int right = flashfsGetSize() / FREE_BLOCK_SIZE; // One past the largest block index in the search region
int mid;
int result = right;
int i;
bool blockErased;
while (left < right) {
mid = (left + right) / 2;
m25p16_readBytes(mid * FREE_BLOCK_SIZE, testBuffer.bytes, FREE_BLOCK_TEST_SIZE_BYTES);
if (m25p16_readBytes(mid * FREE_BLOCK_SIZE, testBuffer.bytes, FREE_BLOCK_TEST_SIZE_BYTES) < FREE_BLOCK_TEST_SIZE_BYTES) {
// Unexpected timeout from flash, so bail early (reporting the device fuller than it really is)
break;
}
// Checking the buffer 4 bytes at a time like this is probably faster than byte-by-byte, but I didn't benchmark it :)
blockErased = true;

1
src/main/io/rc_controls.h
View file

@ -47,6 +47,7 @@ typedef enum {
BOXSERVO2,
BOXSERVO3,
BOXBLACKBOX,
BOXFAILSAFE,
CHECKBOX_ITEM_COUNT
} boxId_e;

85
src/main/io/serial_cli.c
View file

@ -112,8 +112,12 @@ static void cliRateProfile(char *cmdline);
static void cliReboot(void);
static void cliSave(char *cmdline);
static void cliSerial(char *cmdline);
#ifdef USE_SERVOS
static void cliServo(char *cmdline);
static void cliServoMix(char *cmdline);
#endif
static void cliSet(char *cmdline);
static void cliGet(char *cmdline);
static void cliStatus(char *cmdline);
@ -175,7 +179,12 @@ static const char * const featureNames[] = {
};
// sync this with rxFailsafeChannelMode_e
static char rxFailsafeModes[RX_FAILSAFE_MODE_COUNT] = { 'a', 'h', 's'};
static const char rxFailsafeModeCharacters[] = "ahs";
static const rxFailsafeChannelMode_e rxFailsafeModesTable[RX_FAILSAFE_TYPE_COUNT][RX_FAILSAFE_MODE_COUNT] = {
{ RX_FAILSAFE_MODE_AUTO, RX_FAILSAFE_MODE_HOLD, RX_FAILSAFE_MODE_INVALID },
{ RX_FAILSAFE_MODE_INVALID, RX_FAILSAFE_MODE_HOLD, RX_FAILSAFE_MODE_SET }
};
#ifndef CJMCU
// sync this with sensors_e
@ -441,6 +450,8 @@ const clivalue_t valueTable[] = {
{ "failsafe_delay", VAR_UINT8 | MASTER_VALUE, &masterConfig.failsafeConfig.failsafe_delay, 0, 200 },
{ "failsafe_off_delay", VAR_UINT8 | MASTER_VALUE, &masterConfig.failsafeConfig.failsafe_off_delay, 0, 200 },
{ "failsafe_throttle", VAR_UINT16 | MASTER_VALUE, &masterConfig.failsafeConfig.failsafe_throttle, PWM_RANGE_MIN, PWM_RANGE_MAX },
{ "failsafe_kill_switch", VAR_UINT8 | MASTER_VALUE, &masterConfig.failsafeConfig.failsafe_kill_switch, 0, 1 },
{ "failsafe_throttle_low_delay",VAR_UINT16 | MASTER_VALUE, &masterConfig.failsafeConfig.failsafe_throttle_low_delay, 0, 300 },
{ "rx_min_usec", VAR_UINT16 | MASTER_VALUE, &masterConfig.rxConfig.rx_min_usec, PWM_PULSE_MIN, PWM_PULSE_MAX },
{ "rx_max_usec", VAR_UINT16 | MASTER_VALUE, &masterConfig.rxConfig.rx_max_usec, PWM_PULSE_MIN, PWM_PULSE_MAX },
@ -462,6 +473,7 @@ const clivalue_t valueTable[] = {
{ "baro_noise_lpf", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].barometerConfig.baro_noise_lpf, 0, 1 },
{ "baro_cf_vel", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].barometerConfig.baro_cf_vel, 0, 1 },
{ "baro_cf_alt", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].barometerConfig.baro_cf_alt, 0, 1 },
{ "baro_hardware", VAR_UINT8 | MASTER_VALUE, &masterConfig.baro_hardware, 0, BARO_MAX },
{ "mag_hardware", VAR_UINT8 | MASTER_VALUE, &masterConfig.mag_hardware, 0, MAG_MAX },
{ "mag_declination", VAR_INT16 | PROFILE_VALUE, &masterConfig.profile[0].mag_declination, -18000, 18000 },
@ -595,27 +607,22 @@ static void cliRxFail(char *cmdline)
rxFailsafeChannelConfiguration_t *channelFailsafeConfiguration = &masterConfig.rxConfig.failsafe_channel_configurations[channel];
uint16_t value;
rxFailsafeChannelType_e type = (channel < NON_AUX_CHANNEL_COUNT) ? RX_FAILSAFE_TYPE_FLIGHT : RX_FAILSAFE_TYPE_AUX;
rxFailsafeChannelMode_e mode = channelFailsafeConfiguration->mode;
bool requireValue = channelFailsafeConfiguration->mode == RX_FAILSAFE_MODE_SET;
// TODO optimize to use rxFailsafeModes - less logic.
ptr = strchr(ptr, ' ');
if (ptr) {
switch (*(++ptr)) {
case 'a':
mode = RX_FAILSAFE_MODE_AUTO;
break;
case 'h':
mode = RX_FAILSAFE_MODE_HOLD;
break;
case 's':
mode = RX_FAILSAFE_MODE_SET;
break;
default:
cliShowParseError();
return;
char *p = strchr(rxFailsafeModeCharacters, *(++ptr));
if (p) {
uint8_t requestedMode = p - rxFailsafeModeCharacters;
mode = rxFailsafeModesTable[type][requestedMode];
} else {
mode = RX_FAILSAFE_MODE_INVALID;
}
if (mode == RX_FAILSAFE_MODE_INVALID) {
cliShowParseError();
return;
}
requireValue = mode == RX_FAILSAFE_MODE_SET;
@ -634,12 +641,15 @@ static void cliRxFail(char *cmdline)
}
channelFailsafeConfiguration->step = value;
} else if (requireValue) {
cliShowParseError();
return;
}
channelFailsafeConfiguration->mode = mode;
}
char modeCharacter = rxFailsafeModes[channelFailsafeConfiguration->mode];
char modeCharacter = rxFailsafeModeCharacters[channelFailsafeConfiguration->mode];
// triple use of printf below
// 1. acknowledge interpretation on command,
@ -1064,11 +1074,9 @@ static void cliColor(char *cmdline)
}
#endif
#ifdef USE_SERVOS
static void cliServo(char *cmdline)
{
#ifndef USE_SERVOS
UNUSED(cmdline);
#else
enum { SERVO_ARGUMENT_COUNT = 8 };
int16_t arguments[SERVO_ARGUMENT_COUNT];
@ -1138,7 +1146,7 @@ static void cliServo(char *cmdline)
arguments[MAX] < PWM_PULSE_MIN || arguments[MAX] > PWM_PULSE_MAX ||
arguments[MIDDLE] < arguments[MIN] || arguments[MIDDLE] > arguments[MAX] ||
arguments[MIN] > arguments[MAX] || arguments[MAX] < arguments[MIN] ||
arguments[RATE] < 100 || arguments[RATE] > 100 ||
arguments[RATE] < -100 || arguments[RATE] > 100 ||
arguments[FORWARD] >= MAX_SUPPORTED_RC_CHANNEL_COUNT ||
arguments[ANGLE_AT_MIN] < 0 || arguments[ANGLE_AT_MIN] > 180 ||
arguments[ANGLE_AT_MAX] < 0 || arguments[ANGLE_AT_MAX] > 180
@ -1155,14 +1163,12 @@ static void cliServo(char *cmdline)
servo->rate = arguments[6];
servo->forwardFromChannel = arguments[7];
}
#endif
}
#endif
#ifdef USE_SERVOS
static void cliServoMix(char *cmdline)
{
#ifndef USE_SERVOS
UNUSED(cmdline);
#else
int i;
uint8_t len;
char *ptr;
@ -1290,8 +1296,8 @@ static void cliServoMix(char *cmdline)
cliShowParseError();
}
}
#endif
}
#endif
#ifdef USE_FLASHFS
@ -1411,7 +1417,7 @@ static const char* const sectionBreak = "\r\n";
static void cliDump(char *cmdline)
{
unsigned int i, channel;
unsigned int i;
char buf[16];
uint32_t mask;
@ -1485,15 +1491,6 @@ static void cliDump(char *cmdline)
);
}
// print servo directions
for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
for (channel = 0; channel < INPUT_SOURCE_COUNT; channel++) {
if (servoDirection(i, channel) < 0) {
printf("smix reverse %d %d r\r\n", i , channel);
}
}
}
#endif
cliPrint("\r\n\r\n# feature\r\n");
@ -1553,10 +1550,23 @@ static void cliDump(char *cmdline)
cliRxRange("");
#ifdef USE_SERVOS
cliPrint("\r\n# servo\r\n");
cliServo("");
// print servo directions
unsigned int channel;
for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
for (channel = 0; channel < INPUT_SOURCE_COUNT; channel++) {
if (servoDirection(i, channel) < 0) {
printf("smix reverse %d %d r\r\n", i , channel);
}
}
}
#endif
printSectionBreak();
dumpValues(PROFILE_VALUE);
@ -1581,6 +1591,7 @@ void cliEnter(serialPort_t *serialPort)
setPrintfSerialPort(cliPort);
cliPrint("\r\nEntering CLI Mode, type 'exit' to return, or 'help'\r\n");
cliPrompt();
ENABLE_ARMING_FLAG(PREVENT_ARMING);
}
static void cliExit(char *cmdline)

20
src/main/io/serial_msp.c
View file

@ -134,7 +134,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 12 // increment when any change is made, reset to zero when major changes are released after changing API_VERSION_MAJOR
#define API_VERSION_MINOR 13 // increment when any change is made, reset to zero when major changes are released after changing API_VERSION_MAJOR
#define API_VERSION_LENGTH 2
@ -350,6 +350,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
{ BOXSERVO2, "SERVO2;", 24 },
{ BOXSERVO3, "SERVO3;", 25 },
{ BOXBLACKBOX, "BLACKBOX;", 26 },
{ BOXFAILSAFE, "FAILSAFE;", 27 },
{ CHECKBOX_ITEM_COUNT, NULL, 0xFF }
};
@ -702,6 +703,10 @@ void mspInit(serialConfig_t *serialConfig)
}
#endif
if (feature(FEATURE_FAILSAFE)){
activeBoxIds[activeBoxIdCount++] = BOXFAILSAFE;
}
memset(mspPorts, 0x00, sizeof(mspPorts));
mspAllocateSerialPorts(serialConfig);
}
@ -823,7 +828,8 @@ static bool processOutCommand(uint8_t cmdMSP)
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXAUTOTUNE)) << BOXAUTOTUNE |
IS_ENABLED(FLIGHT_MODE(SONAR_MODE)) << BOXSONAR |
IS_ENABLED(ARMING_FLAG(ARMED)) << BOXARM |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBLACKBOX)) << BOXBLACKBOX;
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBLACKBOX)) << BOXBLACKBOX |
IS_ENABLED(FLIGHT_MODE(FAILSAFE_MODE)) << BOXFAILSAFE;
for (i = 0; i < activeBoxIdCount; i++) {
int flag = (tmp & (1 << activeBoxIds[i]));
if (flag)
@ -1308,9 +1314,13 @@ static bool processInCommand(void)
if (channelCount > MAX_SUPPORTED_RC_CHANNEL_COUNT) {
headSerialError(0);
} else {
for (i = 0; i < channelCount; i++)
rcData[i] = read16();
rxMspFrameRecieve();
uint16_t frame[MAX_SUPPORTED_RC_CHANNEL_COUNT];
for (i = 0; i < channelCount; i++) {
frame[i] = read16();
}
rxMspFrameReceive(frame, channelCount);
}
}
break;

8
src/main/main.c
View file

@ -104,7 +104,7 @@ void telemetryInit(void);
void serialInit(serialConfig_t *initialSerialConfig, bool softserialEnabled);
void mspInit(serialConfig_t *serialConfig);
void cliInit(serialConfig_t *serialConfig);
void failsafeInit(rxConfig_t *intialRxConfig);
void failsafeInit(rxConfig_t *intialRxConfig, uint16_t deadband3d_throttle);
pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init);
#ifdef USE_SERVOS
void mixerInit(mixerMode_e mixerMode, motorMixer_t *customMotorMixers, servoMixer_t *customServoMixers);
@ -371,9 +371,9 @@ void init(void)
}
#endif
if (!sensorsAutodetect(&masterConfig.sensorAlignmentConfig, masterConfig.gyro_lpf, masterConfig.acc_hardware, masterConfig.mag_hardware, currentProfile->mag_declination)) {
if (!sensorsAutodetect(&masterConfig.sensorAlignmentConfig, masterConfig.gyro_lpf, masterConfig.acc_hardware, masterConfig.mag_hardware, masterConfig.baro_hardware, currentProfile->mag_declination)) {
// if gyro was not detected due to whatever reason, we give up now.
failureMode(3);
failureMode(FAILURE_MISSING_ACC);
}
systemState |= SYSTEM_STATE_SENSORS_READY;
@ -404,7 +404,7 @@ void init(void)
cliInit(&masterConfig.serialConfig);
#endif
failsafeInit(&masterConfig.rxConfig);
failsafeInit(&masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle);
rxInit(&masterConfig.rxConfig);

5
src/main/mw.c
View file

@ -259,7 +259,7 @@ void annexCode(void)
if (ibatTimeSinceLastServiced >= IBATINTERVAL) {
ibatLastServiced = currentTime;
updateCurrentMeter((ibatTimeSinceLastServiced / 1000), &masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle);
updateCurrentMeter(ibatTimeSinceLastServiced, &masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle);
}
}
@ -342,6 +342,9 @@ void mwArm(void)
if (ARMING_FLAG(ARMED)) {
return;
}
if (IS_RC_MODE_ACTIVE(BOXFAILSAFE)) {
return;
}
if (!ARMING_FLAG(PREVENT_ARMING)) {
ENABLE_ARMING_FLAG(ARMED);
headFreeModeHold = heading;

16
src/main/rx/msp.c
View file

@ -31,16 +31,26 @@
#include "rx/rx.h"
#include "rx/msp.h"
static uint16_t mspFrame[MAX_SUPPORTED_RC_CHANNEL_COUNT];
static bool rxMspFrameDone = false;
static uint16_t rxMspReadRawRC(rxRuntimeConfig_t *rxRuntimeConfigPtr, uint8_t chan)
{
UNUSED(rxRuntimeConfigPtr);
return rcData[chan];
return mspFrame[chan];
}
void rxMspFrameRecieve(void)
void rxMspFrameReceive(uint16_t *frame, int channelCount)
{
for (int i = 0; i < channelCount; i++) {
mspFrame[i] = frame[i];
}
// Any channels not provided will be reset to zero
for (int i = channelCount; i < MAX_SUPPORTED_RC_CHANNEL_COUNT; i++) {
mspFrame[i] = 0;
}
rxMspFrameDone = true;
}
@ -57,7 +67,7 @@ bool rxMspFrameComplete(void)
void rxMspInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback)
{
UNUSED(rxConfig);
rxRuntimeConfig->channelCount = 8; // Limited to 8 channels due to MSP_SET_RAW_RC command.
rxRuntimeConfig->channelCount = MAX_SUPPORTED_RC_CHANNEL_COUNT;
if (callback)
*callback = rxMspReadRawRC;
}

2
src/main/rx/msp.h
View file

@ -18,4 +18,4 @@
#pragma once
bool rxMspFrameComplete(void);
void rxMspFrameRecieve(void);
void rxMspFrameReceive(uint16_t *frame, int channelCount);

88
src/main/rx/rx.c
View file

@ -40,6 +40,7 @@
#include "drivers/gpio.h"
#include "drivers/timer.h"
#include "drivers/pwm_rx.h"
#include "drivers/system.h"
#include "rx/pwm.h"
#include "rx/sbus.h"
#include "rx/spektrum.h"
@ -50,6 +51,7 @@
#include "rx/rx.h"
//#define DEBUG_RX_SIGNAL_LOSS
void rxPwmInit(rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback);
@ -71,6 +73,8 @@ static bool rxFlightChannelsValid = false;
static uint32_t rxUpdateAt = 0;
static uint32_t needRxSignalBefore = 0;
static uint32_t suspendRxSignalUntil = 0;
static uint8_t skipRxSamples = 0;
int16_t rcRaw[MAX_SUPPORTED_RC_CHANNEL_COUNT]; // interval [1000;2000]
int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; // interval [1000;2000]
@ -79,9 +83,13 @@ int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; // interval [1000;2000]
#define DELAY_50_HZ (1000000 / 50)
#define DELAY_10_HZ (1000000 / 10)
#define DELAY_5_HZ (1000000 / 5)
#define SKIP_RC_ON_SUSPEND_PERIOD 1500000 // 1.5 second period in usec (call frequency independent)
#define SKIP_RC_SAMPLES_ON_RESUME 2 // flush 2 samples to drop wrong measurements (timing independent)
rxRuntimeConfig_t rxRuntimeConfig;
static rxConfig_t *rxConfig;
static uint8_t rcSampleIndex = 0;
static uint16_t nullReadRawRC(rxRuntimeConfig_t *rxRuntimeConfig, uint8_t channel) {
UNUSED(rxRuntimeConfig);
@ -141,14 +149,13 @@ void rxInit(rxConfig_t *rxConfig)
uint8_t i;
useRxConfig(rxConfig);
rcSampleIndex = 0;
for (i = 0; i < MAX_SUPPORTED_RC_CHANNEL_COUNT; i++) {
rcData[i] = rxConfig->midrc;
}
if (!feature(FEATURE_3D)) {
rcData[0] = rxConfig->rx_min_usec;
}
rcData[THROTTLE] = (feature(FEATURE_3D)) ? rxConfig->midrc : rxConfig->rx_min_usec;
#ifdef SERIAL_RX
if (feature(FEATURE_RX_SERIAL)) {
@ -261,6 +268,20 @@ static void resetRxSignalReceivedFlagIfNeeded(uint32_t currentTime)
}
}
void suspendRxSignal(void)
{
suspendRxSignalUntil = micros() + SKIP_RC_ON_SUSPEND_PERIOD;
skipRxSamples = SKIP_RC_SAMPLES_ON_RESUME;
failsafeOnRxSuspend(SKIP_RC_ON_SUSPEND_PERIOD);
}
void resumeRxSignal(void)
{
suspendRxSignalUntil = micros();
skipRxSamples = SKIP_RC_SAMPLES_ON_RESUME;
failsafeOnRxResume();
}
void updateRx(uint32_t currentTime)
{
resetRxSignalReceivedFlagIfNeeded(currentTime);
@ -277,16 +298,18 @@ void updateRx(uint32_t currentTime)
if (frameStatus & SERIAL_RX_FRAME_COMPLETE) {
rxDataReceived = true;
rxSignalReceived = (frameStatus & SERIAL_RX_FRAME_FAILSAFE) == 0;
needRxSignalBefore = currentTime + DELAY_10_HZ;
}
}
#endif
if (feature(FEATURE_RX_MSP)) {
rxDataReceived = rxMspFrameComplete();
}
if (feature(FEATURE_RX_SERIAL | FEATURE_RX_MSP) && rxDataReceived) {
needRxSignalBefore = currentTime + DELAY_10_HZ;
if (rxDataReceived) {
rxSignalReceived = true;
needRxSignalBefore = currentTime + DELAY_5_HZ;
}
}
if (feature(FEATURE_RX_PPM)) {
@ -311,8 +334,6 @@ bool shouldProcessRx(uint32_t currentTime)
return rxDataReceived || ((int32_t)(currentTime - rxUpdateAt) >= 0); // data driven or 50Hz
}
static uint8_t rcSampleIndex = 0;
static uint16_t calculateNonDataDrivenChannel(uint8_t chan, uint16_t sample)
{
static int16_t rcSamples[MAX_SUPPORTED_RX_PARALLEL_PWM_OR_PPM_CHANNEL_COUNT][PPM_AND_PWM_SAMPLE_COUNT];
@ -346,27 +367,29 @@ static uint16_t getRxfailValue(uint8_t channel)
rxFailsafeChannelConfiguration_t *channelFailsafeConfiguration = &rxConfig->failsafe_channel_configurations[channel];
switch(channelFailsafeConfiguration->mode) {
default:
case RX_FAILSAFE_MODE_AUTO:
switch (channel) {
case ROLL:
case PITCH:
case YAW:
return rxConfig->midrc;
case THROTTLE:
if (feature(FEATURE_3D))
return rxConfig->midrc;
else
return rxConfig->rx_min_usec;
}
// fall though to HOLD if there's nothing specific to do.
/* no break */
default:
case RX_FAILSAFE_MODE_INVALID:
case RX_FAILSAFE_MODE_HOLD:
return rcData[channel];
case RX_FAILSAFE_MODE_SET:
return RXFAIL_STEP_TO_CHANNEL_VALUE(channelFailsafeConfiguration->step);
}
}
STATIC_UNIT_TESTED uint16_t applyRxChannelRangeConfiguraton(int sample, rxChannelRangeConfiguration_t range)
@ -402,27 +425,26 @@ static void readRxChannelsApplyRanges(void)
}
}
static void processNonDataDrivenRx(void)
{
rcSampleIndex++;
}
static void detectAndApplySignalLossBehaviour(void)
{
int channel;
uint16_t sample;
bool useValueFromRx = true;
bool rxIsDataDriven = isRxDataDriven();
if (!rxSignalReceived) {
if (rxIsDataDriven && rxDataReceived) {
// use the values from the RX
} else {
useValueFromRx = false;
}
}
rxResetFlightChannelStatus();
for (channel = 0; channel < rxRuntimeConfig.channelCount; channel++) {
uint16_t sample = rcRaw[channel];
if (!rxSignalReceived) {
if (isRxDataDriven() && rxDataReceived) {
// use the values from the RX
} else {
sample = PPM_RCVR_TIMEOUT;
}
}
sample = (useValueFromRx) ? rcRaw[channel] : PPM_RCVR_TIMEOUT;
bool validPulse = isPulseValid(sample);
@ -432,7 +454,7 @@ static void detectAndApplySignalLossBehaviour(void)
rxUpdateFlightChannelStatus(channel, validPulse);
if (isRxDataDriven()) {
if (rxIsDataDriven) {
rcData[channel] = sample;
} else {
rcData[channel] = calculateNonDataDrivenChannel(channel, sample);
@ -441,10 +463,11 @@ static void detectAndApplySignalLossBehaviour(void)
rxFlightChannelsValid = rxHaveValidFlightChannels();
if (rxFlightChannelsValid) {
if ((rxFlightChannelsValid) && !IS_RC_MODE_ACTIVE(BOXFAILSAFE)) {
failsafeOnValidDataReceived();
} else {
rxSignalReceived = false;
failsafeOnValidDataFailed();
for (channel = 0; channel < rxRuntimeConfig.channelCount; channel++) {
rcData[channel] = getRxfailValue(channel);
@ -457,19 +480,18 @@ void calculateRxChannelsAndUpdateFailsafe(uint32_t currentTime)
{
rxUpdateAt = currentTime + DELAY_50_HZ;
failsafeOnRxCycleStarted();
if (!feature(FEATURE_RX_MSP)) {
// rcData will have already been updated by MSP_SET_RAW_RC
if (!isRxDataDriven()) {
processNonDataDrivenRx();
// only proceed when no more samples to skip and suspend period is over
if (skipRxSamples) {
if (currentTime > suspendRxSignalUntil) {
skipRxSamples--;
}
return;
}
readRxChannelsApplyRanges();
detectAndApplySignalLossBehaviour();
rcSampleIndex++;
}
void parseRcChannels(const char *input, rxConfig_t *rxConfig)

10
src/main/rx/rx.h
View file

@ -85,10 +85,18 @@ typedef enum {
RX_FAILSAFE_MODE_AUTO = 0,
RX_FAILSAFE_MODE_HOLD,
RX_FAILSAFE_MODE_SET,
RX_FAILSAFE_MODE_INVALID,
} rxFailsafeChannelMode_e;
#define RX_FAILSAFE_MODE_COUNT 3
typedef enum {
RX_FAILSAFE_TYPE_FLIGHT = 0,
RX_FAILSAFE_TYPE_AUX,
} rxFailsafeChannelType_e;
#define RX_FAILSAFE_TYPE_COUNT 2
typedef struct rxFailsafeChannelConfiguration_s {
uint8_t mode; // See rxFailsafeChannelMode_e
uint8_t step;
@ -142,3 +150,5 @@ uint8_t serialRxFrameStatus(rxConfig_t *rxConfig);
void updateRSSI(uint32_t currentTime);
void resetAllRxChannelRangeConfigurations(rxChannelRangeConfiguration_t *rxChannelRangeConfiguration);
void suspendRxSignal(void);
void resumeRxSignal(void);

4
src/main/sensors/acceleration.c
View file

@ -171,7 +171,9 @@ void applyAccelerationTrims(flightDynamicsTrims_t *accelerationTrims)
void updateAccelerationReadings(rollAndPitchTrims_t *rollAndPitchTrims)
{
acc.read(accADC);
if (!acc.read(accADC)) {
return;
}
alignSensors(accADC, accADC, accAlign);
if (!isAccelerationCalibrationComplete()) {

5
src/main/sensors/barometer.h
View file

@ -18,13 +18,14 @@
#pragma once
typedef enum {
BARO_NONE = 0,
BARO_DEFAULT = 1,
BARO_DEFAULT = 0,
BARO_NONE = 1,
BARO_BMP085 = 2,
BARO_MS5611 = 3
} baroSensor_e;
#define BARO_SAMPLE_COUNT_MAX 48
#define BARO_MAX BARO_MS5611
typedef struct barometerConfig_s {
uint8_t baro_sample_count; // size of baro filter array

4
src/main/sensors/boardalignment.c
View file

@ -44,9 +44,9 @@ void initBoardAlignment(boardAlignment_t *boardAlignment)
standardBoardAlignment = false;
fp_angles_t rotationAngles;
rotationAngles.angles.roll = degreesToRadians(boardAlignment->rollDegrees);
rotationAngles.angles.roll = degreesToRadians(boardAlignment->rollDegrees );
rotationAngles.angles.pitch = degreesToRadians(boardAlignment->pitchDegrees);
rotationAngles.angles.yaw = degreesToRadians(boardAlignment->yawDegrees);
rotationAngles.angles.yaw = degreesToRadians(boardAlignment->yawDegrees );
buildRotationMatrix(&rotationAngles, boardRotation);
}

6
src/main/sensors/gyro.c
View file

@ -116,10 +116,10 @@ static void applyGyroZero(void)
void gyroUpdate(void)
{
// FIXME When gyro.read() fails due to i2c or other error gyroZero is continually re-applied to gyroADC resulting in a old reading that gets worse over time.
// range: +/- 8192; +/- 2000 deg/sec
gyro.read(gyroADC);
if (!gyro.read(gyroADC)) {
return;
}
alignSensors(gyroADC, gyroADC, gyroAlign);
if (!isGyroCalibrationComplete()) {

17
src/main/sensors/initialisation.c
View file

@ -125,11 +125,13 @@ const mpu6050Config_t *selectMPU6050Config(void)
#ifdef USE_FAKE_GYRO
static void fakeGyroInit(void) {}
static void fakeGyroRead(int16_t *gyroADC) {
static bool fakeGyroRead(int16_t *gyroADC) {
memset(gyroADC, 0, sizeof(int16_t[XYZ_AXIS_COUNT]));
return true;
}
static void fakeGyroReadTemp(int16_t *tempData) {
static bool fakeGyroReadTemp(int16_t *tempData) {
UNUSED(tempData);
return true;
}
bool fakeGyroDetect(gyro_t *gyro, uint16_t lpf)
@ -144,8 +146,9 @@ bool fakeGyroDetect(gyro_t *gyro, uint16_t lpf)
#ifdef USE_FAKE_ACC
static void fakeAccInit(void) {}
static void fakeAccRead(int16_t *accData) {
static bool fakeAccRead(int16_t *accData) {
memset(accData, 0, sizeof(int16_t[XYZ_AXIS_COUNT]));
return true;
}
bool fakeAccDetect(acc_t *acc)
@ -408,12 +411,12 @@ retry:
sensorsSet(SENSOR_ACC);
}
static void detectBaro()
static void detectBaro(baroSensor_e baroHardwareToUse)
{
#ifdef BARO
// Detect what pressure sensors are available. baro->update() is set to sensor-specific update function
baroSensor_e baroHardware = BARO_DEFAULT;
baroSensor_e baroHardware = baroHardwareToUse;
#ifdef USE_BARO_BMP085
@ -587,7 +590,7 @@ void reconfigureAlignment(sensorAlignmentConfig_t *sensorAlignmentConfig)
}
}
bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint16_t gyroLpf, uint8_t accHardwareToUse, uint8_t magHardwareToUse, int16_t magDeclinationFromConfig)
bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint16_t gyroLpf, uint8_t accHardwareToUse, uint8_t magHardwareToUse, uint8_t baroHardwareToUse, int16_t magDeclinationFromConfig)
{
int16_t deg, min;
@ -598,7 +601,7 @@ bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint16_t
return false;
}
detectAcc(accHardwareToUse);
detectBaro();
detectBaro(baroHardwareToUse);
// Now time to init things, acc first

2
src/main/sensors/initialisation.h
View file

@ -17,4 +17,4 @@
#pragma once
bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint16_t gyroLpf, uint8_t accHardwareToUse, uint8_t magHardwareToUse, int16_t magDeclinationFromConfig);
bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint16_t gyroLpf, uint8_t accHardwareToUse, uint8_t magHardwareToUse, uint8_t baroHardwareToUse, int16_t magDeclinationFromConfig);

2
src/main/target/CHEBUZZF3/target.h
View file

@ -47,7 +47,7 @@
#define L3GD20_CS_GPIO GPIOE
#define L3GD20_CS_PIN GPIO_Pin_3
#define GYRO_L3GD20_ALIGN CW90_DEG
#define GYRO_L3GD20_ALIGN CW270_DEG
#define GYRO_MPU6050_ALIGN CW0_DEG
#define ACC

2
src/main/target/STM32F3DISCOVERY/target.h
View file

@ -47,7 +47,7 @@
#define L3GD20_CS_GPIO GPIOE
#define L3GD20_CS_PIN GPIO_Pin_3
#define GYRO_L3GD20_ALIGN CW90_DEG
#define GYRO_L3GD20_ALIGN CW270_DEG
#define ACC
#define USE_ACC_LSM303DLHC

2
src/main/telemetry/smartport.c
View file

@ -331,7 +331,7 @@ void handleSmartPortTelemetry(void)
break;
case FSSP_DATAID_CURRENT :
if (feature(FEATURE_CURRENT_METER)) {
smartPortSendPackage(id, amperage); // given in 10mA steps, unknown requested unit
smartPortSendPackage(id, amperage / 10); // given in 10mA steps, unknown requested unit
smartPortHasRequest = 0;
}
break;

467
src/test/Makefile
View file

@ -94,44 +94,447 @@ TEST_INCLUDE_DIRS := $(TEST_DIR) \
TEST_CFLAGS = $(addprefix -I,$(TEST_INCLUDE_DIRS))
LIBCLEANFLIGHT_SRC = \
common/encoding.c \
common/maths.c \
drivers/barometer_ms5611.c \
drivers/barometer_bmp085.c \
drivers/light_ws2811strip.c \
flight/altitudehold.c \
flight/failsafe.c \
flight/gps_conversion.c \
flight/imu.c \
flight/lowpass.c \
flight/mixer.c \
io/ledstrip.c \
io/rc_controls.c \
io/serial.c \
rx/rx.c \
sensors/battery.c \
telemetry/hott.c
DEPS = $(TEST_BINARIES:%=%.d)
LIBCLEANFLIGHT_OBJ = $(LIBCLEANFLIGHT_SRC:%.c=$(OBJECT_DIR)/%.o)
$(OBJECT_DIR)/common/maths.o : \
$(USER_DIR)/common/maths.c \
$(USER_DIR)/common/maths.h \
$(GTEST_HEADERS)
DEPS = $(LIBCLEANFLIGHT_OBJ:%.o=%.d) \
$(TEST_BINARIES:%=%.d)
@mkdir -p $(dir $@)
$(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/common/maths.c -o $@
LIBS = $(OBJECT_DIR)/libcleanflight.a $(OBJECT_DIR)/gtest_main.a
$(OBJECT_DIR)/libcleanflight.a: $(LIBCLEANFLIGHT_OBJ)
$(AR) $(ARFLAGS) $@ $^
$(OBJECT_DIR)/sensors/battery.o : $(USER_DIR)/sensors/battery.c $(USER_DIR)/sensors/battery.h $(GTEST_HEADERS)
@mkdir -p $(dir $@)
$(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/sensors/battery.c -o $@
# Build a module from the flight software.
$(OBJECT_DIR)/%.o: $(USER_DIR)/%.c
mkdir -p $(@D)
$(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $< -o $@
$(OBJECT_DIR)/battery_unittest.o : \
$(TEST_DIR)/battery_unittest.cc \
$(USER_DIR)/sensors/battery.h \
$(GTEST_HEADERS)
# Build the unit test executable.
$(OBJECT_DIR)/%: $(TEST_DIR)/%.cc $(LIBS)
@mkdir -p $(@D)
$(CXX) $(CXX_FLAGS) $(TEST_CFLAGS) -o $@ $< $(LIBS)
@mkdir -p $(dir $@)
$(CXX) $(CXX_FLAGS) $(TEST_CFLAGS) -c $< -o $@
$(OBJECT_DIR)/battery_unittest : \
$(OBJECT_DIR)/sensors/battery.o \
$(OBJECT_DIR)/common/maths.o \
$(OBJECT_DIR)/battery_unittest.o \
$(OBJECT_DIR)/gtest_main.a
$(CXX) $(CXX_FLAGS) $^ -o $@
$(OBJECT_DIR)/common/encoding.o : $(USER_DIR)/common/encoding.c $(USER_DIR)/common/encoding.h $(GTEST_HEADERS)
@mkdir -p $(dir $@)
$(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/common/encoding.c -o $@
$(OBJECT_DIR)/encoding_unittest.o : \
$(TEST_DIR)/encoding_unittest.cc \
$(USER_DIR)/common/encoding.h \
$(GTEST_HEADERS)
@mkdir -p $(dir $@)
$(CXX) $(CXX_FLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/encoding_unittest.cc -o $@
$(OBJECT_DIR)/encoding_unittest : \
$(OBJECT_DIR)/common/encoding.o \
$(OBJECT_DIR)/encoding_unittest.o \
$(OBJECT_DIR)/gtest_main.a
$(CXX) $(CXX_FLAGS) $^ -o $(OBJECT_DIR)/$@
$(OBJECT_DIR)/flight/imu.o : \
$(USER_DIR)/flight/imu.c \
$(USER_DIR)/flight/imu.h \
$(GTEST_HEADERS)
@mkdir -p $(dir $@)
$(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/flight/imu.c -o $@
$(OBJECT_DIR)/flight_imu_unittest.o : \
$(TEST_DIR)/flight_imu_unittest.cc \
$(USER_DIR)/flight/imu.h \
$(GTEST_HEADERS)
@mkdir -p $(dir $@)
$(CXX) $(CXX_FLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/flight_imu_unittest.cc -o $@
$(OBJECT_DIR)/flight_imu_unittest : \
$(OBJECT_DIR)/flight/imu.o \
$(OBJECT_DIR)/flight/altitudehold.o \
$(OBJECT_DIR)/flight_imu_unittest.o \
$(OBJECT_DIR)/common/maths.o \
$(OBJECT_DIR)/gtest_main.a
$(CXX) $(CXX_FLAGS) $^ -o $(OBJECT_DIR)/$@
$(OBJECT_DIR)/maths_unittest.o : \
$(TEST_DIR)/maths_unittest.cc \
$(GTEST_HEADERS)
@mkdir -p $(dir $@)
$(CXX) $(CXX_FLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/maths_unittest.cc -o $@
$(OBJECT_DIR)/maths_unittest : \
$(OBJECT_DIR)/maths_unittest.o \
$(OBJECT_DIR)/common/maths.o \
$(OBJECT_DIR)/gtest_main.a
$(CXX) $(CXX_FLAGS) $^ -o $(OBJECT_DIR)/$@
$(OBJECT_DIR)/flight/altitudehold.o : \
$(USER_DIR)/flight/altitudehold.c \
$(USER_DIR)/flight/altitudehold.h \
$(GTEST_HEADERS)
@mkdir -p $(dir $@)
$(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/flight/altitudehold.c -o $@
$(OBJECT_DIR)/altitude_hold_unittest.o : \
$(TEST_DIR)/altitude_hold_unittest.cc \
$(USER_DIR)/flight/altitudehold.h \
$(GTEST_HEADERS)
@mkdir -p $(dir $@)
$(CXX) $(CXX_FLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/altitude_hold_unittest.cc -o $@
$(OBJECT_DIR)/altitude_hold_unittest : \
$(OBJECT_DIR)/flight/altitudehold.o \
$(OBJECT_DIR)/altitude_hold_unittest.o \
$(OBJECT_DIR)/gtest_main.a
$(CXX) $(CXX_FLAGS) $^ -o $(OBJECT_DIR)/$@
$(OBJECT_DIR)/flight/gps_conversion.o : \
$(USER_DIR)/flight/gps_conversion.c \
$(USER_DIR)/flight/gps_conversion.h \
$(GTEST_HEADERS)
@mkdir -p $(dir $@)
$(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/flight/gps_conversion.c -o $@
$(OBJECT_DIR)/gps_conversion_unittest.o : \
$(TEST_DIR)/gps_conversion_unittest.cc \
$(USER_DIR)/flight/gps_conversion.h \
$(GTEST_HEADERS)
@mkdir -p $(dir $@)
$(CXX) $(CXX_FLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/gps_conversion_unittest.cc -o $@
$(OBJECT_DIR)/gps_conversion_unittest : \
$(OBJECT_DIR)/flight/gps_conversion.o \
$(OBJECT_DIR)/gps_conversion_unittest.o \
$(OBJECT_DIR)/gtest_main.a
$(CXX) $(CXX_FLAGS) $^ -o $(OBJECT_DIR)/$@
$(OBJECT_DIR)/telemetry/hott.o : \
$(USER_DIR)/telemetry/hott.c \
$(USER_DIR)/telemetry/hott.h \
$(GTEST_HEADERS)
@mkdir -p $(dir $@)
$(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/telemetry/hott.c -o $@
$(OBJECT_DIR)/telemetry_hott_unittest.o : \
$(TEST_DIR)/telemetry_hott_unittest.cc \
$(USER_DIR)/telemetry/hott.h \
$(GTEST_HEADERS)
@mkdir -p $(dir $@)
$(CXX) $(CXX_FLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/telemetry_hott_unittest.cc -o $@
$(OBJECT_DIR)/telemetry_hott_unittest : \
$(OBJECT_DIR)/telemetry/hott.o \
$(OBJECT_DIR)/telemetry_hott_unittest.o \
$(OBJECT_DIR)/flight/gps_conversion.o \
$(OBJECT_DIR)/gtest_main.a
$(CXX) $(CXX_FLAGS) $^ -o $(OBJECT_DIR)/$@
$(OBJECT_DIR)/io/rc_controls.o : \
$(USER_DIR)/io/rc_controls.c \
$(USER_DIR)/io/rc_controls.h \
$(GTEST_HEADERS)
@mkdir -p $(dir $@)
$(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/io/rc_controls.c -o $@
$(OBJECT_DIR)/rc_controls_unittest.o : \
$(TEST_DIR)/rc_controls_unittest.cc \
$(USER_DIR)/io/rc_controls.h \
$(GTEST_HEADERS)
@mkdir -p $(dir $@)
$(CXX) $(CXX_FLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/rc_controls_unittest.cc -o $@
$(OBJECT_DIR)/rc_controls_unittest : \
$(OBJECT_DIR)/common/maths.o \
$(OBJECT_DIR)/io/rc_controls.o \
$(OBJECT_DIR)/rc_controls_unittest.o \
$(OBJECT_DIR)/gtest_main.a
$(CXX) $(CXX_FLAGS) $^ -o $(OBJECT_DIR)/$@
$(OBJECT_DIR)/io/ledstrip.o : \
$(USER_DIR)/io/ledstrip.c \
$(USER_DIR)/io/ledstrip.h \
$(GTEST_HEADERS)
@mkdir -p $(dir $@)
$(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/io/ledstrip.c -o $@
$(OBJECT_DIR)/ledstrip_unittest.o : \
$(TEST_DIR)/ledstrip_unittest.cc \
$(USER_DIR)/io/ledstrip.h \
$(GTEST_HEADERS)
@mkdir -p $(dir $@)
$(CXX) $(CXX_FLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/ledstrip_unittest.cc -o $@
$(OBJECT_DIR)/ledstrip_unittest : \
$(OBJECT_DIR)/io/ledstrip.o \
$(OBJECT_DIR)/ledstrip_unittest.o \
$(OBJECT_DIR)/gtest_main.a
$(CXX) $(CXX_FLAGS) $^ -o $(OBJECT_DIR)/$@
$(OBJECT_DIR)/drivers/light_ws2811strip.o : \
$(USER_DIR)/drivers/light_ws2811strip.c \
$(USER_DIR)/drivers/light_ws2811strip.h \
$(GTEST_HEADERS)
@mkdir -p $(dir $@)
$(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/drivers/light_ws2811strip.c -o $@
$(OBJECT_DIR)/ws2811_unittest.o : \
$(TEST_DIR)/ws2811_unittest.cc \
$(USER_DIR)/drivers/light_ws2811strip.h \
$(GTEST_HEADERS)
@mkdir -p $(dir $@)
$(CXX) $(CXX_FLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/ws2811_unittest.cc -o $@
$(OBJECT_DIR)/ws2811_unittest : \
$(OBJECT_DIR)/drivers/light_ws2811strip.o \
$(OBJECT_DIR)/ws2811_unittest.o \
$(OBJECT_DIR)/gtest_main.a
$(CXX) $(CXX_FLAGS) $^ -o $(OBJECT_DIR)/$@
$(OBJECT_DIR)/flight/lowpass.o : \
$(USER_DIR)/flight/lowpass.c \
$(USER_DIR)/flight/lowpass.h \
$(GTEST_HEADERS)
@mkdir -p $(dir $@)
$(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/flight/lowpass.c -o $@
$(OBJECT_DIR)/lowpass_unittest.o : \
$(TEST_DIR)/lowpass_unittest.cc \
$(USER_DIR)/flight/lowpass.h \
$(GTEST_HEADERS)
@mkdir -p $(dir $@)
$(CXX) $(CXX_FLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/lowpass_unittest.cc -o $@
$(OBJECT_DIR)/lowpass_unittest : \
$(OBJECT_DIR)/flight/lowpass.o \
$(OBJECT_DIR)/lowpass_unittest.o \
$(OBJECT_DIR)/gtest_main.a
$(CXX) $(CXX_FLAGS) $^ -o $(OBJECT_DIR)/$@
$(OBJECT_DIR)/flight/mixer.o : \
$(USER_DIR)/flight/mixer.c \
$(USER_DIR)/flight/mixer.h \
$(GTEST_HEADERS)
@mkdir -p $(dir $@)
$(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/flight/mixer.c -o $@
$(OBJECT_DIR)/flight_mixer_unittest.o : \
$(TEST_DIR)/flight_mixer_unittest.cc \
$(USER_DIR)/flight/mixer.h \
$(GTEST_HEADERS)
@mkdir -p $(dir $@)
$(CXX) $(CXX_FLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/flight_mixer_unittest.cc -o $@
$(OBJECT_DIR)/flight_mixer_unittest : \
$(OBJECT_DIR)/flight/mixer.o \
$(OBJECT_DIR)/flight_mixer_unittest.o \
$(OBJECT_DIR)/common/maths.o \
$(OBJECT_DIR)/gtest_main.a
$(CXX) $(CXX_FLAGS) $^ -o $(OBJECT_DIR)/$@
$(OBJECT_DIR)/flight/failsafe.o : \
$(USER_DIR)/flight/failsafe.c \
$(USER_DIR)/flight/failsafe.h \
$(GTEST_HEADERS)
@mkdir -p $(dir $@)
$(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/flight/failsafe.c -o $@
$(OBJECT_DIR)/flight_failsafe_unittest.o : \
$(TEST_DIR)/flight_failsafe_unittest.cc \
$(USER_DIR)/flight/failsafe.h \
$(GTEST_HEADERS)
@mkdir -p $(dir $@)
$(CXX) $(CXX_FLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/flight_failsafe_unittest.cc -o $@
$(OBJECT_DIR)/flight_failsafe_unittest : \
$(OBJECT_DIR)/flight/failsafe.o \
$(OBJECT_DIR)/flight_failsafe_unittest.o \
$(OBJECT_DIR)/common/maths.o \
$(OBJECT_DIR)/gtest_main.a
$(CXX) $(CXX_FLAGS) $^ -o $(OBJECT_DIR)/$@
$(OBJECT_DIR)/io/serial.o : \
$(USER_DIR)/io/serial.c \
$(USER_DIR)/io/serial.h \
$(GTEST_HEADERS)
@mkdir -p $(dir $@)
$(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/io/serial.c -o $@
$(OBJECT_DIR)/io_serial_unittest.o : \
$(TEST_DIR)/io_serial_unittest.cc \
$(USER_DIR)/io/serial.h \
$(GTEST_HEADERS)
@mkdir -p $(dir $@)
$(CXX) $(CXX_FLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/io_serial_unittest.cc -o $@
$(OBJECT_DIR)/io_serial_unittest : \
$(OBJECT_DIR)/io/serial.o \
$(OBJECT_DIR)/io_serial_unittest.o \
$(OBJECT_DIR)/gtest_main.a
$(CXX) $(CXX_FLAGS) $^ -o $(OBJECT_DIR)/$@
$(OBJECT_DIR)/rx/rx.o : \
$(USER_DIR)/rx/rx.c \
$(USER_DIR)/rx/rx.h \
$(GTEST_HEADERS)
@mkdir -p $(dir $@)
$(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/rx/rx.c -o $@
$(OBJECT_DIR)/rx_rx_unittest.o : \
$(TEST_DIR)/rx_rx_unittest.cc \
$(USER_DIR)/rx/rx.h \
$(GTEST_HEADERS)
@mkdir -p $(dir $@)
$(CXX) $(CXX_FLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/rx_rx_unittest.cc -o $@
$(OBJECT_DIR)/rx_rx_unittest : \
$(OBJECT_DIR)/rx/rx.o \
$(OBJECT_DIR)/rx_rx_unittest.o \
$(OBJECT_DIR)/common/maths.o \
$(OBJECT_DIR)/gtest_main.a
$(CXX) $(CXX_FLAGS) $^ -o $(OBJECT_DIR)/$@
$(OBJECT_DIR)/rx_ranges_unittest.o : \
$(TEST_DIR)/rx_ranges_unittest.cc \
$(USER_DIR)/rx/rx.h \
$(GTEST_HEADERS)
@mkdir -p $(dir $@)
$(CXX) $(CXX_FLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/rx_ranges_unittest.cc -o $@
$(OBJECT_DIR)/rx_ranges_unittest : \
$(OBJECT_DIR)/rx/rx.o \
$(OBJECT_DIR)/rx_ranges_unittest.o \
$(OBJECT_DIR)/common/maths.o \
$(OBJECT_DIR)/gtest_main.a
$(CXX) $(CXX_FLAGS) $^ -o $(OBJECT_DIR)/$@
$(OBJECT_DIR)/drivers/barometer_ms5611.o : \
$(USER_DIR)/drivers/barometer_ms5611.c \
$(USER_DIR)/drivers/barometer_ms5611.h \
$(GTEST_HEADERS)
@mkdir -p $(dir $@)
$(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/drivers/barometer_ms5611.c -o $@
$(OBJECT_DIR)/baro_ms5611_unittest.o : \
$(TEST_DIR)/baro_ms5611_unittest.cc \
$(USER_DIR)/drivers/barometer_ms5611.h \
$(GTEST_HEADERS)
@mkdir -p $(dir $@)
$(CXX) $(CXX_FLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/baro_ms5611_unittest.cc -o $@
$(OBJECT_DIR)/baro_ms5611_unittest : \
$(OBJECT_DIR)/drivers/barometer_ms5611.o \
$(OBJECT_DIR)/baro_ms5611_unittest.o \
$(OBJECT_DIR)/gtest_main.a
$(CXX) $(CXX_FLAGS) $^ -o $(OBJECT_DIR)/$@
$(OBJECT_DIR)/drivers/barometer_bmp085.o : \
$(USER_DIR)/drivers/barometer_bmp085.c \
$(USER_DIR)/drivers/barometer_bmp085.h \
$(GTEST_HEADERS)
@mkdir -p $(dir $@)
$(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/drivers/barometer_bmp085.c -o $@
$(OBJECT_DIR)/baro_bmp085_unittest.o : \
$(TEST_DIR)/baro_bmp085_unittest.cc \
$(USER_DIR)/drivers/barometer_bmp085.h \
$(GTEST_HEADERS)
@mkdir -p $(dir $@)
$(CXX) $(CXX_FLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/baro_bmp085_unittest.cc -o $@
$(OBJECT_DIR)/baro_bmp085_unittest : \
$(OBJECT_DIR)/drivers/barometer_bmp085.o \
$(OBJECT_DIR)/baro_bmp085_unittest.o \
$(OBJECT_DIR)/gtest_main.a
$(CXX) $(CXX_FLAGS) $^ -o $(OBJECT_DIR)/$@
$(OBJECT_DIR)/sensors/boardalignment.o : \
$(USER_DIR)/sensors/boardalignment.c \
$(USER_DIR)/sensors/boardalignment.h \
$(GTEST_HEADERS)
@mkdir -p $(dir $@)
$(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/sensors/boardalignment.c -o $@
$(OBJECT_DIR)/alignsensor_unittest.o : \
$(TEST_DIR)/alignsensor_unittest.cc \
$(USER_DIR)/sensors/boardalignment.h \
$(GTEST_HEADERS)
@mkdir -p $(dir $@)
$(CXX) $(CXX_FLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/alignsensor_unittest.cc -o $@
$(OBJECT_DIR)/alignsensor_unittest : \
$(OBJECT_DIR)/common/maths.o \
$(OBJECT_DIR)/sensors/boardalignment.o \
$(OBJECT_DIR)/alignsensor_unittest.o \
$(OBJECT_DIR)/gtest_main.a
$(CXX) $(CXX_FLAGS) $^ -o $(OBJECT_DIR)/$@
test: $(TESTS:%=test-%)

269
src/test/unit/alignsensor_unittest.cc Normal file
View file

@ -0,0 +1,269 @@
/*
* 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/>.
*/
#include "math.h"
#include "stdint.h"
#include "time.h"
extern "C" {
#include "common/axis.h"
#include "sensors/boardalignment.h"
#include "sensors/sensors.h"
}
#include "gtest/gtest.h"
/*
* This test file contains an independent method of rotating a vector.
* The output of alignSensor() is compared to the output of the test
* rotation method.
*
* For each alignment condition (CW0, CW90, etc) the source vector under
* test is set to a unit vector along each axis (x-axis, y-axis, z-axis)
* plus one additional random vector is tested.
*/
#define DEG2RAD 0.01745329251
static void rotateVector(int16_t mat[3][3], int16_t vec[3], int16_t *out)
{
int16_t tmp[3];
for(int i=0; i<3; i++) {
tmp[i] = 0;
for(int j=0; j<3; j++) {
tmp[i] += mat[j][i] * vec[j];
}
}
out[0]=tmp[0];
out[1]=tmp[1];
out[2]=tmp[2];
}
//static void initXAxisRotation(int16_t mat[][3], int16_t angle)
//{
// mat[0][0] = 1;
// mat[0][1] = 0;
// mat[0][2] = 0;
// mat[1][0] = 0;
// mat[1][1] = cos(angle*DEG2RAD);
// mat[1][2] = -sin(angle*DEG2RAD);
// mat[2][0] = 0;
// mat[2][1] = sin(angle*DEG2RAD);
// mat[2][2] = cos(angle*DEG2RAD);
//}
static void initYAxisRotation(int16_t mat[][3], int16_t angle)
{
mat[0][0] = cos(angle*DEG2RAD);
mat[0][1] = 0;
mat[0][2] = sin(angle*DEG2RAD);
mat[1][0] = 0;
mat[1][1] = 1;
mat[1][2] = 0;
mat[2][0] = -sin(angle*DEG2RAD);
mat[2][1] = 0;
mat[2][2] = cos(angle*DEG2RAD);
}
static void initZAxisRotation(int16_t mat[][3], int16_t angle)
{
mat[0][0] = cos(angle*DEG2RAD);
mat[0][1] = -sin(angle*DEG2RAD);
mat[0][2] = 0;
mat[1][0] = sin(angle*DEG2RAD);
mat[1][1] = cos(angle*DEG2RAD);
mat[1][2] = 0;
mat[2][0] = 0;
mat[2][1] = 0;
mat[2][2] = 1;
}
static void testCW(sensor_align_e rotation, int16_t angle)
{
int16_t src[XYZ_AXIS_COUNT];
int16_t dest[XYZ_AXIS_COUNT];
int16_t test[XYZ_AXIS_COUNT];
// unit vector along x-axis
src[X] = 1;
src[Y] = 0;
src[Z] = 0;
int16_t matrix[3][3];
initZAxisRotation(matrix, angle);
rotateVector(matrix, src, test);
alignSensors(src, dest, rotation);
EXPECT_EQ(test[X], dest[X]) << "X-Unit alignment does not match in X-Axis. " << test[X] << " " << dest[X];
EXPECT_EQ(test[Y], dest[Y]) << "X-Unit alignment does not match in Y-Axis. " << test[Y] << " " << dest[Y];
EXPECT_EQ(test[Z], dest[Z]) << "X-Unit alignment does not match in Z-Axis. " << test[Z] << " " << dest[Z];
// unit vector along y-axis
src[X] = 0;
src[Y] = 1;
src[Z] = 0;
rotateVector(matrix, src, test);
alignSensors(src, dest, rotation);
EXPECT_EQ(test[X], dest[X]) << "Y-Unit alignment does not match in X-Axis. " << test[X] << " " << dest[X];
EXPECT_EQ(test[Y], dest[Y]) << "Y-Unit alignment does not match in Y-Axis. " << test[Y] << " " << dest[Y];
EXPECT_EQ(test[Z], dest[Z]) << "Y-Unit alignment does not match in Z-Axis. " << test[Z] << " " << dest[Z];
// unit vector along z-axis
src[X] = 0;
src[Y] = 0;
src[Z] = 1;
rotateVector(matrix, src, test);
alignSensors(src, dest, rotation);
EXPECT_EQ(test[X], dest[X]) << "Z-Unit alignment does not match in X-Axis. " << test[X] << " " << dest[X];
EXPECT_EQ(test[Y], dest[Y]) << "Z-Unit alignment does not match in Y-Axis. " << test[Y] << " " << dest[Y];
EXPECT_EQ(test[Z], dest[Z]) << "Z-Unit alignment does not match in Z-Axis. " << test[Z] << " " << dest[Z];
// random vector to test
src[X] = rand() % 5;
src[Y] = rand() % 5;
src[Z] = rand() % 5;
rotateVector(matrix, src, test);
alignSensors(src, dest, rotation);
EXPECT_EQ(test[X], dest[X]) << "Random alignment does not match in X-Axis. " << test[X] << " " << dest[X];
EXPECT_EQ(test[Y], dest[Y]) << "Random alignment does not match in Y-Axis. " << test[Y] << " " << dest[Y];
EXPECT_EQ(test[Z], dest[Z]) << "Random alignment does not match in Z-Axis. " << test[Z] << " " << dest[Z];
}
/*
* Since the order of flip and rotation matters, these tests make the
* assumption that the 'flip' occurs first, followed by clockwise rotation
*/
static void testCWFlip(sensor_align_e rotation, int16_t angle)
{
int16_t src[XYZ_AXIS_COUNT];
int16_t dest[XYZ_AXIS_COUNT];
int16_t test[XYZ_AXIS_COUNT];
// unit vector along x-axis
src[X] = 1;
src[Y] = 0;
src[Z] = 0;
int16_t matrix[3][3];
initYAxisRotation(matrix, 180);
rotateVector(matrix, src, test);
initZAxisRotation(matrix, angle);
rotateVector(matrix, test, test);
alignSensors(src, dest, rotation);
EXPECT_EQ(test[X], dest[X]) << "X-Unit alignment does not match in X-Axis. " << test[X] << " " << dest[X];
EXPECT_EQ(test[Y], dest[Y]) << "X-Unit alignment does not match in Y-Axis. " << test[Y] << " " << dest[Y];
EXPECT_EQ(test[Z], dest[Z]) << "X-Unit alignment does not match in Z-Axis. " << test[Z] << " " << dest[Z];
// unit vector along y-axis
src[X] = 0;
src[Y] = 1;
src[Z] = 0;
initYAxisRotation(matrix, 180);
rotateVector(matrix, src, test);
initZAxisRotation(matrix, angle);
rotateVector(matrix, test, test);
alignSensors(src, dest, rotation);
EXPECT_EQ(test[X], dest[X]) << "Y-Unit alignment does not match in X-Axis. " << test[X] << " " << dest[X];
EXPECT_EQ(test[Y], dest[Y]) << "Y-Unit alignment does not match in Y-Axis. " << test[Y] << " " << dest[Y];
EXPECT_EQ(test[Z], dest[Z]) << "Y-Unit alignment does not match in Z-Axis. " << test[Z] << " " << dest[Z];
// unit vector along z-axis
src[X] = 0;
src[Y] = 0;
src[Z] = 1;
initYAxisRotation(matrix, 180);
rotateVector(matrix, src, test);
initZAxisRotation(matrix, angle);
rotateVector(matrix, test, test);
alignSensors(src, dest, rotation);
EXPECT_EQ(test[X], dest[X]) << "Z-Unit alignment does not match in X-Axis. " << test[X] << " " << dest[X];
EXPECT_EQ(test[Y], dest[Y]) << "Z-Unit alignment does not match in Y-Axis. " << test[Y] << " " << dest[Y];
EXPECT_EQ(test[Z], dest[Z]) << "Z-Unit alignment does not match in Z-Axis. " << test[Z] << " " << dest[Z];
// random vector to test
src[X] = rand() % 5;
src[Y] = rand() % 5;
src[Z] = rand() % 5;
initYAxisRotation(matrix, 180);
rotateVector(matrix, src, test);
initZAxisRotation(matrix, angle);
rotateVector(matrix, test, test);
alignSensors(src, dest, rotation);
EXPECT_EQ(test[X], dest[X]) << "Random alignment does not match in X-Axis. " << test[X] << " " << dest[X];
EXPECT_EQ(test[Y], dest[Y]) << "Random alignment does not match in Y-Axis. " << test[Y] << " " << dest[Y];
EXPECT_EQ(test[Z], dest[Z]) << "Random alignment does not match in Z-Axis. " << test[Z] << " " << dest[Z];
}
TEST(AlignSensorTest, ClockwiseZeroDegrees)
{
srand(time(NULL));
testCW(CW0_DEG, 0);
}
TEST(AlignSensorTest, ClockwiseNinetyDegrees)
{
testCW(CW90_DEG, 90);
}
TEST(AlignSensorTest, ClockwiseOneEightyDegrees)
{
testCW(CW180_DEG, 180);
}
TEST(AlignSensorTest, ClockwiseTwoSeventyDegrees)
{
testCW(CW270_DEG, 270);
}
TEST(AlignSensorTest, ClockwiseZeroDegreesFlip)
{
testCWFlip(CW0_DEG_FLIP, 0);
}
TEST(AlignSensorTest, ClockwiseNinetyDegreesFlip)
{
testCWFlip(CW90_DEG_FLIP, 90);
}
TEST(AlignSensorTest, ClockwiseOneEightyDegreesFlip)
{
testCWFlip(CW180_DEG_FLIP, 180);
}
TEST(AlignSensorTest, ClockwiseTwoSeventyDegreesFlip)
{
testCWFlip(CW270_DEG_FLIP, 270);
}

4
src/test/unit/baro_bmp085_unittest.cc
View file

@ -182,8 +182,8 @@ TEST(baroBmp085Test, TestBmp085CalculateOss3Hot)
extern "C" {
void gpioInit(){}
void RCC_APB2PeriphClockCmd(){}
void gpioInit() {}
void RCC_APB2PeriphClockCmd() {}
void delay(uint32_t) {}
void delayMicroseconds(uint32_t) {}
bool i2cWrite(uint8_t, uint8_t, uint8_t) {

362
src/test/unit/flight_failsafe_unittest.cc
View file

@ -31,6 +31,7 @@ extern "C" {
#include "config/runtime_config.h"
#include "io/beeper.h"
#include "io/rc_controls.h"
#include "rx/rx.h"
#include "flight/failsafe.h"
@ -42,6 +43,9 @@ extern "C" {
#include "gtest/gtest.h"
uint32_t testFeatureMask = 0;
uint16_t flightModeFlags = 0;
uint16_t testMinThrottle = 0;
throttleStatus_e throttleStatus = THROTTLE_HIGH;
enum {
COUNTER_MW_DISARM = 0,
@ -56,24 +60,34 @@ void resetCallCounters(void) {
memset(&callCounts, 0, sizeof(callCounts));
}
#define TEST_MID_RC 1495 // something other than the default 1500 will suffice.
#define TEST_MID_RC 1495 // something other than the default 1500 will suffice.
#define TEST_MIN_CHECK 1100;
#define PERIOD_OF_10_SCONDS 10000
#define DE_ACTIVATE_ALL_BOXES 0
rxConfig_t rxConfig;
failsafeConfig_t failsafeConfig;
uint32_t sysTickUptime;
void configureFailsafe(void)
{
memset(&rxConfig, 0, sizeof(rxConfig));
rxConfig.midrc = TEST_MID_RC;
rxConfig.mincheck = TEST_MIN_CHECK;
memset(&failsafeConfig, 0, sizeof(failsafeConfig));
failsafeConfig.failsafe_delay = 10; // 1 second
failsafeConfig.failsafe_off_delay = 50; // 5 seconds
failsafeConfig.failsafe_kill_switch = false;
failsafeConfig.failsafe_throttle = 1200;
failsafeConfig.failsafe_throttle_low_delay = 50; // 5 seconds
sysTickUptime = 0;
}
//
// Stepwise tests
//
/****************************************************************************************/
TEST(FlightFailsafeTest, TestFailsafeInitialState)
{
// given
@ -91,6 +105,7 @@ TEST(FlightFailsafeTest, TestFailsafeInitialState)
EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
}
/****************************************************************************************/
TEST(FlightFailsafeTest, TestFailsafeStartMonitoring)
{
// when
@ -102,14 +117,16 @@ TEST(FlightFailsafeTest, TestFailsafeStartMonitoring)
EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
}
/****************************************************************************************/
TEST(FlightFailsafeTest, TestFailsafeFirstArmedCycle)
{
// given
ENABLE_ARMING_FLAG(ARMED);
// when
failsafeOnRxCycleStarted();
failsafeOnValidDataReceived();
failsafeOnValidDataFailed(); // set last invalid sample at current time
sysTickUptime += PERIOD_RXDATA_RECOVERY + 1; // adjust time to point just past the recovery time to
failsafeOnValidDataReceived(); // cause a recovered link
// and
failsafeUpdateState();
@ -119,47 +136,11 @@ TEST(FlightFailsafeTest, TestFailsafeFirstArmedCycle)
EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
}
/*
* FIXME failsafe assumes that calls to failsafeUpdateState() happen at a set frequency (50hz)
* but that is NOT the case when using a RX_SERIAL or RX_MSP as in that case the rx data is processed as soon
* as it arrives which may be more or less frequent.
*
* Since the failsafe uses a counter the counter would not be updated at the same frequency that the maths
* in the failsafe code is expecting the failsafe will either be triggered to early or too late when using
* RX_SERIAL or RX_MSP.
*
* uint8_t failsafe_delay; // Guard time for failsafe activation after signal lost. 1 step = 0.1sec - 1sec in example (10)
*
* static bool failsafeHasTimerElapsed(void)
* {
* return failsafeState.counter > (5 * failsafeConfig->failsafe_delay);
* }
*
* static bool failsafeShouldHaveCausedLandingByNow(void)
* {
* return failsafeState.counter > 5 * (failsafeConfig->failsafe_delay + failsafeConfig->failsafe_off_delay);
* }
*
* void failsafeOnValidDataReceived(void)
* {
* if (failsafeState.counter > 20)
* failsafeState.counter -= 20;
* else
* failsafeState.counter = 0;
* }
*
* 1000ms / 50hz = 20
*/
#define FAILSAFE_UPDATE_HZ 50
/****************************************************************************************/
TEST(FlightFailsafeTest, TestFailsafeNotActivatedWhenReceivingData)
{
// when
int callsToMakeToSimulateTenSeconds = FAILSAFE_UPDATE_HZ * 10;
for (int i = 0; i < callsToMakeToSimulateTenSeconds; i++) {
failsafeOnRxCycleStarted();
for (sysTickUptime = 0; sysTickUptime < PERIOD_OF_10_SCONDS; sysTickUptime++) {
failsafeOnValidDataReceived();
failsafeUpdateState();
@ -170,108 +151,213 @@ TEST(FlightFailsafeTest, TestFailsafeNotActivatedWhenReceivingData)
}
}
/****************************************************************************************/
TEST(FlightFailsafeTest, TestFailsafeDetectsRxLossAndStartsLanding)
{
// given
ENABLE_ARMING_FLAG(ARMED);
//
// currently 20 cycles must occur before the lack of an update triggers RX loss detection.
//
// FIXME see comments about RX_SERIAL/RX_MSP above, the test should likely deal with time rather than counters.
int failsafeCounterThreshold = 20;
// and
failsafeStartMonitoring();
throttleStatus = THROTTLE_HIGH; // throttle HIGH to go for a failsafe landing procedure
sysTickUptime = 0; // restart time from 0
failsafeOnValidDataReceived(); // set last valid sample at current time
// when
for (int i = 0; i < failsafeCounterThreshold; i++) {
failsafeOnRxCycleStarted();
// no call to failsafeOnValidDataReceived();
for (sysTickUptime = 0; sysTickUptime < (uint32_t)(failsafeConfig.failsafe_delay * MILLIS_PER_TENTH_SECOND + PERIOD_RXDATA_FAILURE); sysTickUptime++) {
failsafeOnValidDataFailed();
failsafeUpdateState();
// then
EXPECT_EQ(false, failsafeIsActive());
EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
EXPECT_EQ(false, failsafeIsActive());
}
// when
for (int i = 0; i < FAILSAFE_UPDATE_HZ - failsafeCounterThreshold; i++) {
failsafeOnRxCycleStarted();
// no call to failsafeOnValidDataReceived();
failsafeUpdateState();
// then
EXPECT_EQ(FAILSAFE_RX_LOSS_DETECTED, failsafePhase());
EXPECT_EQ(false, failsafeIsActive());
}
//
// one more cycle currently needed before the counter is re-checked.
//
// given
sysTickUptime++; // adjust time to point just past the failure time to
failsafeOnValidDataFailed(); // cause a lost link
// when
failsafeUpdateState();
// then
EXPECT_EQ(FAILSAFE_LANDING, failsafePhase());
EXPECT_EQ(true, failsafeIsActive());
}
/****************************************************************************************/
TEST(FlightFailsafeTest, TestFailsafeCausesLanding)
{
// given
sysTickUptime += failsafeConfig.failsafe_off_delay * MILLIS_PER_TENTH_SECOND;
sysTickUptime++;
// when
failsafeOnRxCycleStarted();
// no call to failsafeOnValidDataReceived();
failsafeUpdateState();
// then
EXPECT_EQ(true, failsafeIsActive());
EXPECT_EQ(FAILSAFE_LANDING, failsafePhase());
}
TEST(FlightFailsafeTest, TestFailsafeCausesLanding)
{
// given
int callsToMakeToSimulateFiveSeconds = FAILSAFE_UPDATE_HZ * 5;
// when
for (int i = 0; i < callsToMakeToSimulateFiveSeconds - 1; i++) {
failsafeOnRxCycleStarted();
// no call to failsafeOnValidDataReceived();
failsafeUpdateState();
// then
EXPECT_EQ(FAILSAFE_LANDING, failsafePhase());
EXPECT_EQ(true, failsafeIsActive());
}
// when
failsafeOnRxCycleStarted();
// no call to failsafeOnValidDataReceived();
failsafeUpdateState();
// then
EXPECT_EQ(false, failsafeIsActive());
EXPECT_EQ(FAILSAFE_LANDED, failsafePhase());
EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase());
EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));
EXPECT_TRUE(ARMING_FLAG(PREVENT_ARMING));
// given
DISABLE_ARMING_FLAG(ARMED);
failsafeOnValidDataFailed(); // set last invalid sample at current time
sysTickUptime += PERIOD_RXDATA_RECOVERY + 1; // adjust time to point just past the recovery time to
failsafeOnValidDataReceived(); // cause a recovered link
// when
failsafeUpdateState();
// then
EXPECT_EQ(true, failsafeIsActive());
EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase());
EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));
EXPECT_TRUE(ARMING_FLAG(PREVENT_ARMING));
// given
sysTickUptime += PERIOD_OF_30_SECONDS + 1; // adjust time to point just past the required additional recovery time
failsafeOnValidDataReceived();
// when
failsafeOnRxCycleStarted();
// no call to failsafeOnValidDataReceived();
failsafeUpdateState();
// then
EXPECT_EQ(false, failsafeIsActive());
EXPECT_EQ(FAILSAFE_LANDED, failsafePhase());
EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM)); // disarm not called repeatedly.
EXPECT_TRUE(ARMING_FLAG(PREVENT_ARMING));
EXPECT_FALSE(ARMING_FLAG(PREVENT_ARMING));
}
/****************************************************************************************/
TEST(FlightFailsafeTest, TestFailsafeDetectsRxLossAndJustDisarms)
{
// given
DISABLE_ARMING_FLAG(ARMED);
resetCallCounters();
// and
failsafeStartMonitoring();
throttleStatus = THROTTLE_LOW; // throttle LOW to go for a failsafe just-disarm procedure
sysTickUptime = 0; // restart time from 0
failsafeOnValidDataReceived(); // set last valid sample at current time
// when
for (sysTickUptime = 0; sysTickUptime < (uint32_t)(failsafeConfig.failsafe_delay * MILLIS_PER_TENTH_SECOND + PERIOD_RXDATA_FAILURE); sysTickUptime++) {
failsafeOnValidDataFailed();
failsafeUpdateState();
// then
EXPECT_EQ(false, failsafeIsActive());
EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
}
// given
sysTickUptime++; // adjust time to point just past the failure time to
failsafeOnValidDataFailed(); // cause a lost link
ENABLE_ARMING_FLAG(ARMED); // armed from here (disarmed state has cleared throttleLowPeriod).
// when
failsafeUpdateState();
// then
EXPECT_EQ(true, failsafeIsActive());
EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase());
EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));
EXPECT_TRUE(ARMING_FLAG(PREVENT_ARMING));
// given
failsafeOnValidDataFailed(); // set last invalid sample at current time
sysTickUptime += PERIOD_RXDATA_RECOVERY + 1; // adjust time to point just past the recovery time to
failsafeOnValidDataReceived(); // cause a recovered link
// when
failsafeUpdateState();
// then
EXPECT_EQ(true, failsafeIsActive());
EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase());
EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));
EXPECT_TRUE(ARMING_FLAG(PREVENT_ARMING));
// given
sysTickUptime += PERIOD_OF_3_SECONDS + 1; // adjust time to point just past the required additional recovery time
failsafeOnValidDataReceived();
// when
failsafeUpdateState();
// then
EXPECT_EQ(false, failsafeIsActive());
EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM)); // disarm not called repeatedly.
EXPECT_FALSE(ARMING_FLAG(PREVENT_ARMING));
}
/****************************************************************************************/
TEST(FlightFailsafeTest, TestFailsafeDetectsKillswitchEvent)
{
// given
ENABLE_ARMING_FLAG(ARMED);
resetCallCounters();
failsafeStartMonitoring();
// and
throttleStatus = THROTTLE_HIGH; // throttle HIGH to go for a failsafe landing procedure
failsafeConfig.failsafe_kill_switch = 1; // configure AUX switch as kill switch
ACTIVATE_RC_MODE(BOXFAILSAFE); // and activate it
sysTickUptime = 0; // restart time from 0
failsafeOnValidDataReceived(); // set last valid sample at current time
sysTickUptime = PERIOD_RXDATA_FAILURE + 1; // adjust time to point just past the failure time to
failsafeOnValidDataFailed(); // cause a lost link
// when
failsafeUpdateState(); // kill switch handling should come first
// then
EXPECT_EQ(true, failsafeIsActive());
EXPECT_TRUE(ARMING_FLAG(PREVENT_ARMING));
EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));
EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase());
// given
failsafeOnValidDataFailed(); // set last invalid sample at current time
sysTickUptime += PERIOD_RXDATA_RECOVERY + 1; // adjust time to point just past the recovery time to
failsafeOnValidDataReceived(); // cause a recovered link
rcModeActivationMask = DE_ACTIVATE_ALL_BOXES; // BOXFAILSAFE must be off (kill switch)
// when
failsafeUpdateState();
// then
EXPECT_EQ(true, failsafeIsActive());
EXPECT_TRUE(ARMING_FLAG(PREVENT_ARMING));
EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));
EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase());
// given
sysTickUptime += PERIOD_OF_1_SECONDS + 1; // adjust time to point just past the required additional recovery time
failsafeOnValidDataReceived();
// when
failsafeUpdateState();
// then
EXPECT_EQ(false, failsafeIsActive());
EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM)); // disarm not called repeatedly.
EXPECT_FALSE(ARMING_FLAG(PREVENT_ARMING));
}
/****************************************************************************************/
//
// Additional non-stepwise tests
//
/****************************************************************************************/
TEST(FlightFailsafeTest, TestFailsafeNotActivatedWhenDisarmedAndRXLossIsDetected)
{
// given
@ -288,28 +374,58 @@ TEST(FlightFailsafeTest, TestFailsafeNotActivatedWhenDisarmedAndRXLossIsDetected
failsafeStartMonitoring();
// and
int callsToMakeToSimulateTenSeconds = FAILSAFE_UPDATE_HZ * 10;
sysTickUptime = 0; // restart time from 0
failsafeOnValidDataReceived(); // set last valid sample at current time
for (int i = 0; i < callsToMakeToSimulateTenSeconds; i++) {
failsafeOnRxCycleStarted();
// no call to failsafeOnValidDataReceived();
// when
for (sysTickUptime = 0; sysTickUptime < PERIOD_RXDATA_FAILURE; sysTickUptime++) {
failsafeOnValidDataFailed();
failsafeUpdateState();
// then
EXPECT_EQ(true, failsafeIsMonitoring());
EXPECT_EQ(false, failsafeIsActive());
EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
}
}
// given
sysTickUptime++; // adjust time to point just past the failure time to
failsafeOnValidDataFailed(); // cause a lost link
// when
failsafeUpdateState();
// then
EXPECT_EQ(true, failsafeIsMonitoring());
EXPECT_EQ(false, failsafeIsActive());
EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));
EXPECT_FALSE(ARMING_FLAG(PREVENT_ARMING));
}
// STUBS
extern "C" {
int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];
uint8_t armingFlags;
int16_t rcCommand[4];
uint32_t rcModeActivationMask;
int16_t debug[DEBUG16_VALUE_COUNT];
bool isUsingSticksToArm = true;
// Return system uptime in milliseconds (rollover in 49 days)
uint32_t millis(void)
{
return sysTickUptime;
}
throttleStatus_e calculateThrottleStatus(rxConfig_t *rxConfig, uint16_t deadband3d_throttle)
{
UNUSED(rxConfig);
UNUSED(deadband3d_throttle);
return throttleStatus;
}
void delay(uint32_t) {}
@ -325,4 +441,26 @@ void beeper(beeperMode_e mode) {
UNUSED(mode);
}
uint16_t enableFlightMode(flightModeFlags_e mask)
{
flightModeFlags |= (mask);
return flightModeFlags;
}
uint16_t disableFlightMode(flightModeFlags_e mask)
{
flightModeFlags &= ~(mask);
return flightModeFlags;
}
uint16_t getCurrentMinthrottle(void)
{
return testMinThrottle;
}
bool isUsingSticksForArming(void)
{
return isUsingSticksToArm;
}
}

1
src/test/unit/platform.h
View file

@ -20,6 +20,7 @@
#define MAG
#define BARO
#define GPS
#define DISPLAY
#define TELEMETRY
#define LED_STRIP
#define USE_SERVOS

24
src/test/unit/rc_controls_unittest.cc
View file

@ -298,6 +298,26 @@ TEST_F(RcControlsAdjustmentsTest, processRcAdjustmentsSticksInMiddle)
TEST_F(RcControlsAdjustmentsTest, processRcAdjustmentsWithRcRateFunctionSwitchUp)
{
// given
controlRateConfig_t controlRateConfig = {
.rcRate8 = 90,
.rcExpo8 = 0,
.thrMid8 = 0,
.thrExpo8 = 0,
.rates = {0,0,0},
.dynThrPID = 0,
.rcYawExpo8 = 0,
.tpa_breakpoint = 0
};
// and
memset(&rxConfig, 0, sizeof (rxConfig));
rxConfig.mincheck = DEFAULT_MIN_CHECK;
rxConfig.maxcheck = DEFAULT_MAX_CHECK;
rxConfig.midrc = 1500;
// and
adjustmentStateMask = 0;
memset(&adjustmentStates, 0, sizeof(adjustmentStates));
configureAdjustment(0, AUX3 - NON_AUX_CHANNEL_COUNT, &rateAdjustmentConfig);
// and
@ -673,9 +693,9 @@ void accSetCalibrationCycles(uint16_t) {}
void gyroSetCalibrationCycles(uint16_t) {}
void applyAndSaveAccelerometerTrimsDelta(rollAndPitchTrims_t*) {}
void handleInflightCalibrationStickPosition(void) {}
bool feature(uint32_t) { return false;}
bool sensors(uint32_t) { return false;}
void mwArm(void) {}
void feature(uint32_t) {}
void sensors(uint32_t) {}
void mwDisarm(void) {}
void displayDisablePageCycling() {}
void displayEnablePageCycling() {}

44
src/test/unit/rx_ranges_unittest.cc
View file

@ -31,7 +31,11 @@ extern "C" {
#include "unittest_macros.h"
#include "gtest/gtest.h"
#define DE_ACTIVATE_ALL_BOXES 0
extern "C" {
uint32_t rcModeActivationMask;
extern uint16_t applyRxChannelRangeConfiguraton(int sample, rxChannelRangeConfiguration_t range);
}
@ -39,6 +43,8 @@ extern uint16_t applyRxChannelRangeConfiguraton(int sample, rxChannelRangeConfig
TEST(RxChannelRangeTest, TestRxChannelRanges)
{
rcModeActivationMask = DE_ACTIVATE_ALL_BOXES; // BOXFAILSAFE must be OFF
// No signal, special condition
EXPECT_EQ(applyRxChannelRangeConfiguraton(0, RANGE_CONFIGURATION(1000, 2000)), 0);
EXPECT_EQ(applyRxChannelRangeConfiguraton(0, RANGE_CONFIGURATION(1300, 1700)), 0);
@ -91,6 +97,12 @@ TEST(RxChannelRangeTest, TestRxChannelRanges)
// stubs
extern "C" {
void failsafeOnRxSuspend(uint32_t ) {}
void failsafeOnRxResume(void) {}
uint32_t micros(void) { return 0; }
uint32_t millis(void) { return 0; }
void rxPwmInit(rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback)
{
UNUSED(rxRuntimeConfig);
@ -137,20 +149,8 @@ bool rxMspInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadR
return true;
}
void updateActivatedModes(modeActivationCondition_t *modeActivationConditions)
{
UNUSED(modeActivationConditions);
}
void configureAdjustment(uint8_t index, uint8_t auxChannelIndex, const adjustmentConfig_t *adjustmentConfig)
{
UNUSED(index);
UNUSED(auxChannelIndex);
UNUSED(adjustmentConfig);
}
void feature(uint32_t)
{
bool feature(uint32_t) {
return false;
}
bool rxMspFrameComplete(void)
@ -158,10 +158,6 @@ bool rxMspFrameComplete(void)
return false;
}
void failsafeReset(void)
{
}
bool isPPMDataBeingReceived(void)
{
return false;
@ -176,23 +172,13 @@ void resetPPMDataReceivedState(void)
{
}
void failsafeOnRxCycle(void)
{
}
void failsafeOnValidDataReceived(void)
{
}
void failsafeOnRxCycleStarted(void)
void failsafeOnValidDataFailed(void)
{
}
void failsafeCheckPulse(uint8_t channel, uint16_t pulseDuration)
{
UNUSED(channel);
UNUSED(pulseDuration);
}
}

15
src/test/unit/rx_rx_unittest.cc
View file

@ -25,6 +25,8 @@ extern "C" {
#include "rx/rx.h"
uint32_t rcModeActivationMask;
void rxInit(rxConfig_t *rxConfig);
void rxResetFlightChannelStatus(void);
bool rxHaveValidFlightChannels(void);
@ -35,6 +37,8 @@ extern "C" {
#include "unittest_macros.h"
#include "gtest/gtest.h"
#define DE_ACTIVATE_ALL_BOXES 0
typedef struct testData_s {
bool isPPMDataBeingReceived;
bool isPWMDataBeingReceived;
@ -46,6 +50,7 @@ TEST(RxTest, TestValidFlightChannels)
{
// given
memset(&testData, 0, sizeof(testData));
rcModeActivationMask = DE_ACTIVATE_ALL_BOXES; // BOXFAILSAFE must be OFF
// and
rxConfig_t rxConfig;
@ -131,9 +136,15 @@ TEST(RxTest, TestInvalidFlightChannels)
// STUBS
extern "C" {
void failsafeOnRxCycleStarted() {}
void failsafeOnValidDataFailed() {}
void failsafeOnValidDataReceived() {}
void failsafeOnRxSuspend(uint32_t ) {}
void failsafeOnRxResume(void) {}
uint32_t micros(void) { return 0; }
uint32_t millis(void) { return 0; }
bool feature(uint32_t mask) {
UNUSED(mask);
return false;
@ -154,6 +165,4 @@ extern "C" {
void rxMspInit(rxConfig_t *, rxRuntimeConfig_t *, rcReadRawDataPtr *) {}
void rxPwmInit(rxRuntimeConfig_t *, rcReadRawDataPtr *) {}
}