1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-23 16:25:31 +03:00

Merge remote-tracking branch 'upstream/master' into documentation-edits

This commit is contained in:
Andrew Payne 2015-04-22 18:10:29 -04:00
commit c1b93ff92a
37 changed files with 1219 additions and 181 deletions

View file

@ -19,8 +19,9 @@ Cleanflight also has additional features not found in baseflight.
* Oneshot ESC support.
* Blackbox flight recorder logging (to onboard flash or external SD card).
* Support for additional targets that use the STM32F3 processors (baseflight only supports STM32F1).
* Support for the TauLabs Sparky board (~$35 STM32F303 I2C sensors, based board with acc/gyro/compass and baro!)
* Support for the OpenPilot CC3D board. (~$20 STM32F103 board, SPI acc/gyro)
* Support for the Seriously Pro Racing F3 board (STM32F303, I2C sensors, large flash, excellent I/O.)
* Support for the TauLabs Sparky board (STM32F303, I2C sensors, based board with acc/gyro/compass and baro, ~$35)
* Support for the OpenPilot CC3D board. (STM32F103, board, SPI acc/gyro, ~$20)
* Support for the CJMCU nano quadcopter board.
* Support for developer breakout boards: (Port103R, EUSTM32F103RC, Olimexino, STM32F3Discovery).
* Support for more than 8 RC channels - (e.g. 16 Channels via FrSky X4RSB SBus).
@ -30,6 +31,7 @@ Cleanflight also has additional features not found in baseflight.
* Better PWM and PPM input and failsafe detection than baseflight.
* Better FrSky Telemetry than baseflight.
* MSP Telemetry.
* Smartport Telemetry.
* RSSI via ADC - Uses ADC to read PWM RSSI signals, tested with FrSky D4R-II and X8R.
* Autotune - ported from BradWii, experimental - feedback welcomed.
* OLED Displays - Display information on: Battery voltage, profile, rate profile, version, sensors, RC, etc.

View file

@ -3,6 +3,7 @@
filename=Manual
doc_files=(
'Introduction.md'
'Getting Started.md'
'Safety.md'
'Installation.md'
'Configuration.md'

View file

@ -28,6 +28,8 @@ has a voltage divider that is capable of measuring your particular battery volta
The Naze32 has an on-board battery divider circuit, connect your main battery to the VBAT connector.
**CAUTION:** When installing connection from main battery to the VBAT connector, be sure to first disconnect the main battery from the frame / power distribution board. Check the wiring very carefully before connecting battery again. Incorrect connections can immediately and completely destroy the flight controller and connected peripherals (ESC, GPS, Receiver etc.)
### CC3D
The CC3D has no battery divider, create one that gives you a 3.3v MAXIMUM output when your battery is

View file

@ -187,13 +187,13 @@ Re-apply any new defaults as desired.
| thr_expo | | 0 | 100 | 0 | Rate Profile | UINT8 |
| roll_pitch_rate | | 0 | 100 | 0 | Rate Profile | UINT8 |
| yaw_rate | | 0 | 100 | 0 | Rate Profile | UINT8 |
| tpa_rate | | 0 | 100 | 0 | Rate Profile | UINT8 |
| tpa_breakpoint | Throttle PID attenuation. Affects only P, I and D on ROLL and PITCH, it does not affect YAW. | 1000 | 2000 | 1500 | Rate Profile | UINT16 |
| 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_min_usec | | 100 | 2000 | 985 | Profile | UINT16 |
| failsafe_max_usec | | 100 | 3000 | 2115 | Profile | UINT16 |
| rx_min_usec | | 100 | 2000 | 985 | Profile | UINT16 |
| rx_max_usec | | 100 | 3000 | 2115 | Profile | UINT16 |
| gimbal_flags | When feature SERVO_TILT is enabled, this can be a combination of the following numbers: 1=normal gimbal (default), 2=tiltmix gimbal, 4=in PPM (or SERIALRX) input mode, this will forward AUX1..4 RC inputs to PWM5..8 pins | 0 | 255 | 1 | Profile | UINT8 |
| acc_hardware | This is used to suggest which accelerometer driver should load, or to force no accelerometer in case gyro-only flight is needed. Default (0) will attempt to auto-detect among enabled drivers. Otherwise, to force a particular device, set it to 2 for ADXL345, 3 for MPU6050 integrated accelerometer, 4 for MMA8452, 5 for BMA280, 6 for LSM303DLHC, 7 for SPI_MPU6000, 8 for SPI_MPU6500 or 1 to disable accelerometer alltogether - resulting in gyro-only operation. | 0 | 9 | 0 | Master | UINT8 |
| acc_lpf_factor | This setting controls the Low Pass Filter factor for ACC. Increasing this value reduces ACC noise (visible in GUI), but would increase ACC lag time. Zero = no filter | 0 | 250 | 4 | Profile | UINT8 |

View file

@ -85,7 +85,11 @@ When configuring the flight controller failsafe, use the following steps:
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 'failsafe_min_usec')
b) 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.
See your receiver's documentation for direction on how to accomplish one of these.
@ -96,6 +100,7 @@ See your receiver's documentation for direction on how to accomplish one of thes
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
@ -120,19 +125,15 @@ Throttle level used for landing. Specify a value that causes the aircraft to de
Use standard RX usec values. See RX documentation.
### `failsafe_min_usec`
### `rx_min_usec`
The shortest PWM/PPM pulse considered valid.
The lowest channel value considered valid. e.g. PWM/PPM pulse length
Only valid when using Parallel PWM or PPM receivers.
### `rx_max_usec`
### `failsafe_max_usec`
The highest channel value considered valid. e.g. PWM/PPM pulse length
The longest PWM/PPM pulse considered valid.
Only valid when using Parallel PWM or PPM receivers.
This setting helps detect when your RX stops sending any data when the RX looses signal.
The `rx_min_usec` and `rx_max_usec` settings helps detect when your RX stops sending any data, enters failsafe mode or when the RX 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.

104
docs/Getting Started.md Normal file
View file

@ -0,0 +1,104 @@
# Getting Started
This is a step-by-step guide that can help a person that has never used Cleanflight before set up a flight controller and the aircraft around it for flight. Basic RC knowledge is required, though. A total beginner should first familiarize themselves with concepts and techniques of RC before using this (e.g. basic controls, soldering, transmitter operation etc). One could use [RCGroups](http://www.rcgroups.com/forums/index.php) and/or [the Youtube show FliteTest](https://www.youtube.com/user/flitetest) for this.
DISCLAIMER: This documents is a work in progress. We cannot guarantee the safety or success of your project. At this point the document is only meant to be a helping guide, not an authoritative checklist of everything you should do to be safe and successful. Always exercise common sense, critical thinking and caution.
Read the [Introduction](Introduction.md) chapter for an overview of Cleanflight and how the community works.
## Hardware
NOTE: Flight Controllers are typically equipped with accelerometers. These devices are sensitive to shocks. When the device is not yet installed to an aircraft, it has very little mass by itself. If you drop or bump the controller, a big force will be applied on its accelerometers, which could potentially damage them. Bottom line: Handle the board very carefully until it's installed on an aircraft!
For an overview of the hardware Cleanflight (hereby CF) can run on, see [Boards.md](Boards.md). For information about specific boards, see the board specific documentation.
* Assuming that you have a flight controller board (hereby FC) in hand, you should first read through the manual that it came with. You can skip the details about software setup, as we'll cover that here.
* Decide how you'll connect your receiver by reading the [receiver](Rx.md) chapter, and how many pins you need on the outputs (to connect ESCs and servos) by reading about [Mixers](Mixer.md).
* If you're interested in monitoring your flight battery with CF, see [Battery Monitoring](Battery.md).
* You may want audible feedback from your copter so skim through [Buzzer](Buzzer.md) and mark the pins that will be used.
* Do you want your RC Receiver's RSSI to be sent to the board? [The RSSI chapter](Rssi.md) explains how. You may or may not need to make an additional connection from your Receiver to the FC.
* Would you like to try using a GPS unit to get your aircraft to Loiter or Return-To-Launch? Take a look at the [GPS](Gps.md) and [GPS Tested Hardware](Gps_-_Tested_Hardware.md) chapters.
* You may also want to read the [Serial](Serial.md) chapter to determine what extra devices (such as Blackbox, OSD, Telemetry) you may want to use, and how they should be connected.
* Now that you know what features you are going to use, and which pins you need, you can go ahead and solder them to your board, if they are not soldered already. Soldering only the pins required for the application may save weight and contribute to a neater looking setup, but if you need to use a new feature later you may have to unmount the board from the craft and solder missing pins, so plan accordingly. Before soldering your FC please review a how-to-solder tutorial to avoid expensive mistakes, practice soldering on some scrap before soldering your FC.
* If you are going to use [Oneshot125](Oneshot.md), you may need to enable that on your ESCs using a jumper or flashing them with the latest stable firmware and enable Damped Light in their settings, if it's supported. Refer to the ESCs' documentation or online discussions to determine this.
## Software setup
Now that your board has pins on it, you are ready to connect it to your PC and flash it with CF. Install the Chromium browser or Google Chrome to your PC, if you don't have it already, add the [Cleanflight Configurator](https://chrome.google.com/webstore/detail/cleanflight-configurator/enacoimjcgeinfnnnpajinjgmkahmfgb) to it, and start it.
Then follow these instructions for [Installation](Installation.md) of the firmware to the FC.
## Cleanflight Configuration
Your FC should now be running CF, and you should be able to connect to it using the Configurator. If that is not the case, please go back to the previous sections and follow the steps carefully.
<!--- This next paragraph should probably contain less info, as this info already exists in Configuration.md -->
Now, there are two ways to [configure CF](Configuration.md); via the Configurator's tabs (in a "graphical" way, clicking through and selecting/changing values and tickboxes) and using the [Command Line Interface (CLI)](Cli.md). Some settings may only be configurable using the CLI and some settings are best configured using the GUI (particularly the ports settings, which aren't documented for the CLI as they're not human friendly).
* It is now a good time to setup your RC Receiver and Transmitter. Set the Tx so that it outputs at least 4 channels (Aileron, Elevator, Throttle, Rudder) but preferably more. E.g. you can set channels 5 and 6 to be controlled by 3-position switches, to be used later. Maybe set up EXPO on AIL/ELE/RUD, but you should know that it can also be done in CF's software later. If using RSSI over PPM or PWM, it's now time to configure your Rx to output it on a spare channel.
* Connect the Rx to the FC, and the FC to the PC. You may need to power the Rx through a BEC (its 5V rail - observe polarity!).
* On your PC, connect to the Configurator, and go to the first tab. Check that the board animation is moving properly when you move the actual board. Do an accelerometer calibration.
* Configuration tab: Select your aircraft configuration (e.g. Quad X), and go through each option in the tab to check if relevant for you.
* E.g. you may want to enable ONESHOT125 for Oneshot-capable ESCs.
* You may need RX_PPM if you're using an RC Receiver with PPM output etc.
* If planning to use the battery measurement feature of the FC, check VBAT under Battery Voltage.
* If using analog RSSI, enable that under RSSI. Do not enable this setting if using RSSI injected into the PPM stream.
* Motors will spin by default when the FC is armed. If you don't like this, enable MOTOR_STOP.
* Also, adjust the minimum, middle and maximum throttle according to these guidelines:
* Minimum Throttle - Set this to the minimum throttle level that enables all motors to start reliably. If this is too low, some motors may not start properly after spindowns, which can cause loss of stability and control. A typical value would be 1100.
* Middle Throttle - The throttle level for middle stick position. Many radios use 1500, but some (e.g. Futaba) may use 1520 or other values.
* Maximum Throttle - The maximum throttle level that the ESCs should receive. A typical value would be 2000.
* Minimum Command - This is the lowest signal level that the FC expects from the radio/transmitter (lowest throttle) or outputs to the ESCs. It should match the radio's low throttle signal, and be the lowest number of them all. A typical value would be 1000.
* Finally, click Save and Reboot.
* Receiver tab: Check that the channel inputs move according to your Tx inputs. Check that the Channel map is correct, along with the RSSI Channel, if you use that. You can also set EXPO here instead of your Tx. Click Save!
* Modes tab: Setup the desired modes. See the [modes](Modes.md) chapter for what each mode does, but for the beginning you mainly need HORIZON, if any.
* Before finishing this section, you should calibrate the ESCs, install the FC to the frame, and connect the RSSI cable, buzzer and battery if you have chosen to use those.
## Final testing and safety
It's important that you have configured CF properly, so that your aircraft does not fly away, or even worse fly into property and people! This is an important step that you should NOT postpone until after your maiden flight. Please do this now, before you head off to the flying field.
* First, learn how to arm your FC, and about other [controls](Controls.md).
* Next up, setup [Failsafe](Failsafe.md). Take your time, do it properly.
* Now, on the bench, without props, test that failsafe works properly, according to the above doc.
* Additionally, test the effect of AIL/ELE input of your Tx. Is the aircraft responding properly? Do the same for RUD input.
* Test the direction of AIL/ELE auto correction. Raise throttle at 30% (no blades!); when you tilt the aircraft, do the motors try to compensate momentarily? This should simulate random wind forces that the FC should counteract
* Test the direction of AIL/ELE auto correction in HORIZON mode. With throttle at 30%, if you tilt the aircraft so that one motor is lowered towards the ground, does it spin up and stay at high RPM until you level it off again? This tests the auto-leveling direction.
If one of these tests fail, do not attempt to fly, but go back to the configuration phase instead. Some channel may need reversing, or the direction of the board is wrong.
## Using it (AKA: Flying)
Go to the field, turn Tx on, place aircraft on the ground, connect flight battery and wait. Arm and fly. Good luck!
## Advanced Matters
Some advanced configurations and features are documented in the following pages, but have not been touched-upon earlier:
* [Profiles](Profiles.md)
* [PID tuning](PID tuning.md)
* [In-flight Adjustments](Inflight Adjustments.md)
* [Autotune](Autotune.md)
* [Blackbox logging](Blackbox.md)
* [Using a Sonar](Sonar.md)
* [Spektrum Bind](Spektrum bind.md)
* [Telemetry](Telemetry.md)
* [Using a Display](Display.md)
* [Using a LED strip](Ledstrip.md)
* [Migrating from baseflight](Migrating from baseflight.md)

View file

@ -55,7 +55,7 @@ Example: to use RSSI on AUX1 in Cleanflight use `set rssi_channel = 5`, since 5
### failsafe_detect_threshold
reason: improved functionality
See `failsafe_min_usec` and `failsafe_max_usec` in Failsafe documentation.
See `rx_min_usec` and `rx_max_usec` in Failsafe documentation.
### emfavoidance
reason: renamed to `emf_avoidance` for consistency

View file

@ -130,6 +130,12 @@ Only one receiver feature can be enabled at a time.
See the Serial chapter for some some RX configuration examples.
To setup spectrum on the Naze32 or clones in the GUI:
1. Start on the "Ports" tab make sure that UART2 has serial RX. If not set the checkbox, save and reboot.
2. Move to the "Configuration" page and in the upper lefthand corner choose Serial RX as the receiver type.
3. Below that choose the type of serial receiver that you are using. Save and reboot.
Using CLI:
For Serial RX enable `RX_SERIAL` and set the `serialrx_provider` CLI setting as follows.
| Serial RX Provider | Value |

View file

@ -0,0 +1,49 @@
# Short Version
Install the latest Eclipse Standard/SDK and install the **C/C++ developments Tools** plugins
![plugin eclipse](http://i.imgur.com/IdJ8ki1.png)
Import the project using the wizard **Existing Code as Makefile Project**
![](http://i.imgur.com/XsVCwe2.png)
Adjust your build option if necessary
![](https://camo.githubusercontent.com/64a1d32400d6be64dd4b5d237df1e7f1b817f61b/687474703a2f2f692e696d6775722e636f6d2f6641306d30784d2e706e67)
Make sure you have a valid ARM toolchain in the path
![](http://i.imgur.com/dAbscJo.png)
# Long version
* First you need an ARM toolchain. Good choices are **GCC ARM Embedded** (https://launchpad.net/gcc-arm-embedded) or **Yagarto** (http://www.yagarto.de).
* Now download Eclipse and unpack it somewhere. At the time of writing Eclipse 4.2 was the latest stable version.
* To work with ARM projects in Eclipse you need a few plugins:
+ **Eclipse C Development Tools** (CDT) (available via *Help > Install new Software*).
+ **Zylin Embedded CDT Plugin** (http://opensource.zylin.com/embeddedcdt.html).
+ **GNU ARM Eclipse** (http://sourceforge.net/projects/gnuarmeclipse/).
+ If you want to hook up an SWD debugger you also need the **GDB Hardware Debugging** plugin (Also available via *Install new Software*).
* Now clone the project to your harddrive.
* Create a new C project in Eclipse and choose ARM Cross Target Application and your ARM toolchain.
* Import the Git project into the C project in Eclipse via *File > Import > General > File System*.
* Activate Git via *Project > Team > Share Project*.
* Switch to the master branch in Eclipse (*Project > Team > Switch To > master*).
* The next thing you need to do is adjust the project configuration. There is a Makefile included that works but you might want to use GNU ARM Eclipse's automatic Makefile generation. Open the Project configuration and go to *C/C++ Build > Settings*
* Under *Target Processor* choose "cortex-m3"
* Under *ARM Yagarto [Windows/Mac OS] Linker > General* (or whatever toolchain you chose)
+ Browse to the Script file *stm32_flash.ld*
+ Uncheck "Do not use standard start files"
+ Check "Remove unused sections"
* Under *ARM Yagarto [Windows/Mac OS] Linker > Libraries*
+ Add "m" for the math library
* Under *ARM Yagarto [Windows/Mac OS] Compiler > Preprocessor* add the following 2 items to "Defined Symbols":
+ STM32F10X_MD
+ USE_STDPERIPH_DRIVER
* Under *ARM Yagarto [Windows/Mac OS] Compiler > Directories* add the following 3 items
+ ${workspace_loc:/${ProjName}/lib/CMSIS/CM3/CoreSupport}
+ ${workspace_loc:/${ProjName}/lib/CMSIS/CM3/DeviceSupport/ST/STM32F10x}
+ ${workspace_loc:/${ProjName}/lib/STM32F10x_StdPeriph_Driver/inc}
* Under *ARM Yagarto [Windows/Mac OS] Compiler > Miscellaneous* add the following item to "Other flags":
+ -fomit-frame-pointer
* The code in the support directory is for uploading firmware to the board and is meant for your host machine. Hence, it must not be included in the build process. Just right-click on it to open its properties and choose "Exclude from build" under *C/C++ Build > Settings*
* The last thing you need to do is adding your toolchain to the PATH environment variable.
+ Go to *Project > Properties > C/C++ Build > Environment*, add a variable named "PATH" and fill in the full path of your toolchain's binaries.
+ Make sure "Append variables to native environment" is selected.
* Try to build the project via *Project > Build Project*.
* **Note**: If you're getting "...could not be resolved" errors for data types like int32_t etc. try to disable and re-enable the Indexer under *Project > Properties > C/C++ General > Indexer*.

View file

@ -107,3 +107,9 @@ git pull
make clean TARGET=NAZE
make TARGET=NAZE
```
Or in the case of CC3D in need of a `obj/cleanflight_CC3D.bin`
```
make clean TARGET=CC3D
make TARGET=CC3D OPBL=yes
```

View file

@ -0,0 +1,169 @@
# Hardware
Various debugging hardware solutions exist, the Segger J-Link clones are cheap and are known to work on Windows with both the Naze and Olimexino platforms.
USB-MiniJTAG J-Link JTAG/SWD Debugger/Emulator
http://www.hotmcu.com/usbminijtag-jlink-jtagswd-debuggeremula%E2%80%8Btor-p-29.html?cPath=3_25&zenid=fdefvpnod186umrhsek225dc10
![](https://raw.github.com/wiki/hydra/cleanflight/images/hardware/THAOYU USB-MiniJTAG.jpg)
ARM-JTAG-20-10 adapter
https://www.olimex.com/Products/ARM/JTAG/ARM-JTAG-20-10/
http://uk.farnell.com/jsp/search/productdetail.jsp?sku=2144328
![](https://raw.github.com/wiki/hydra/cleanflight/images/hardware/OLIMEX ARM-JTAG ADAPTER 2144328-40.jpg)
The Segger J-Link server can be obtained from here
http://www.segger.com/jlink-software.html
# Build with DEBUG=GDB
## Naze target (default)
### via Command line
```
make clean TARGET=NAZE
make TARGET=NAZE DEBUG=GDB
```
### via Eclipse make target
![](https://raw.github.com/wiki/hydra/cleanflight/images/eclipse-gdb-debugging/make 2 - NAZE GDB.PNG)
### via configuration
use this method if you want to build automatically when launching the debug configuration
![make debug](http://i.imgur.com/fA0m0xM.png)
## Olimexino target
### via command line
```
make clean TARGET=OLIMEXINO
make TARGET=OLIMEXINO DEBUG=GDB
```
### via Eclipse make target
![](https://raw.github.com/wiki/hydra/cleanflight/images/eclipse-gdb-debugging/make 1 - OLIMEXINO GDB.PNG)
# GDB and OpenOCD
start openocd:
openocd -f /usr/share/openocd/scripts/board/stm32vldiscovery.cfg
Create a new debug configuration in eclipse :
![connect to openocd](http://i.imgur.com/somJLnq.png)
![use workspace default](http://i.imgur.com/LTtioaF.png)
you can control openocd with a telnet connection:
telnet localhost 4444
stop the board, flash the firmware, restart:
reset halt
wait_halt
sleep 100
poll
flash probe 0
flash write_image erase /home/user/git/cleanflight/obj/cleanflight_NAZE.hex 0x08000000
sleep 200
soft_reset_halt
wait_halt
poll
reset halt
A this point you can launch the debug in Eclispe.
![](http://i.imgur.com/u7wDgxv.png)
# GDB and J Link
Here are some screenshots showing Hydra's configuration of Eclipse (Kepler)
If you use cygwin to build the binaries then be sure to have configured your common `Source Lookup Path`, `Path Mappings` first, like this:
![](https://raw.github.com/wiki/cleanflight/cleanflight/images/eclipse-gdb-debugging/config 7.PNG)
Create a new `GDB Hardware Debugging` launch configuration from the `Run` menu
It's important to have build the executable compiled with GDB debugging information first.
Select the appropriate .elf file (not hex file) - In these examples the target platform is an OLIMEXINO, not a naze32.
DISABLE auto-build
![](https://raw.github.com/wiki/cleanflight/cleanflight/images/eclipse-gdb-debugging/config 1.PNG)
Choose the appropriate gdb executable - ideally from the same toolchain that you use to build the executable.
![](https://raw.github.com/wiki/cleanflight/cleanflight/images/eclipse-gdb-debugging/config 2.PNG)
Configure Startup as follows
Initialization commands
```
target remote localhost:2331
monitor interface SWD
monitor speed 2000
monitor flash device = STM32F103RB
monitor flash download = 1
monitor flash breakpoints = 1
monitor endian little
monitor reset
```
![](https://raw.github.com/wiki/cleanflight/cleanflight/images/eclipse-gdb-debugging/config 3.PNG)
![](https://raw.github.com/wiki/cleanflight/cleanflight/images/eclipse-gdb-debugging/config 4.PNG)
It may be useful to specify run commands too:
```
monitor reg r13 = (0x00000000)
monitor reg pc = (0x00000004)
continue
```
![](https://raw.github.com/wiki/cleanflight/cleanflight/images/eclipse-gdb-debugging/config 13.PNG)
If you use cygwin an additional entry should be shown on the Source tab (not present in this screenshot)
![](https://raw.github.com/wiki/cleanflight/cleanflight/images/eclipse-gdb-debugging/config 5.PNG)
Nothing to change from the defaults on the Common tab
![](https://raw.github.com/wiki/cleanflight/cleanflight/images/eclipse-gdb-debugging/config 6.PNG)
Start up the J-Link server in USB mode
![](https://raw.github.com/wiki/cleanflight/cleanflight/images/eclipse-gdb-debugging/config 9.PNG)
If it connects to your target device it should look like this
![](https://raw.github.com/wiki/cleanflight/cleanflight/images/eclipse-gdb-debugging/config 10.PNG)
From Eclipse launch the application using the Run/Debug Configurations..., Eclipse should upload the compiled file to the target device which looks like this
![](https://raw.github.com/wiki/cleanflight/cleanflight/images/eclipse-gdb-debugging/config 11.PNG)
When it's running the J-Link server should look like this.
![](https://raw.github.com/wiki/cleanflight/cleanflight/images/eclipse-gdb-debugging/config 12.PNG)
Then finally you can use Eclipse debug features to inspect variables, memory, stacktrace, set breakpoints, step over code, etc.
![](https://raw.github.com/wiki/cleanflight/cleanflight/images/eclipse-gdb-debugging/debugging.PNG)
If Eclipse can't find your breakpoints and they are ignored then check your path mappings (if using cygwin) or use the other debugging launcher as follows. Note the 'Select other...' at the bottom of the configuration window.
![](https://raw.github.com/wiki/cleanflight/cleanflight/images/eclipse-gdb-debugging/config 8 - If breakpoints do not work.PNG)

View file

@ -2,12 +2,14 @@
The code can be compiled with debugging information, you can then upload a debug version to a board via a JLink/St-Link debug adapter and step through the code in your IDE.
More information about the necessary hardware can and setting up the eclipse IDE can be found int the wiki:
https://github.com/cleanflight/cleanflight/wiki/Debugging-in-Eclipse
More information about the necessary hardware and setting up the eclipse IDE can be found [here](Hardware Debugging in Eclipse.md)
A guide for visual studio can be found here:
http://visualgdb.com/tutorials/arm/st-link/
This video is also helpful in understanding the proces:
https://www.youtube.com/watch?v=kjvqySyNw20
## Compilation options
use `DEBUG=GDB` make argument.

View file

@ -12,8 +12,8 @@
* is using in the C source code, usually in main.c. This file contains:
* - Configuration section that allows to select:
* - The device used in the target application
* - To use or not the peripherals drivers in application code(i.e.
* code will be based on direct access to peripherals registers
* - To use or not the peripheral's drivers in application code(i.e.
* code will be based on direct access to peripheral's registers
* rather than drivers API), this option is controlled by
* "#define USE_STDPERIPH_DRIVER"
* - To change few application-specific parameters such as the HSE

View file

@ -15,15 +15,15 @@
* is using in the C source code, usually in main.c. This file contains:
* - Configuration section that allows to select:
* - The device used in the target application
* - To use or not the peripherals drivers in application code(i.e.
* code will be based on direct access to peripherals registers
* - To use or not the peripheral's drivers in application code(i.e.
* code will be based on direct access to peripheral's registers
* rather than drivers API), this option is controlled by
* "#define USE_STDPERIPH_DRIVER"
* - To change few application-specific parameters such as the HSE
* crystal frequency
* - Data structures and the address mapping for all peripherals
* - Peripheral's registers declarations and bits definition
* - Macros to access peripherals registers hardware
* - Macros to access peripheral's registers hardware
*
******************************************************************************
* @attention

View file

@ -138,7 +138,7 @@ profile_t *currentProfile;
static uint8_t currentControlRateProfileIndex = 0;
controlRateConfig_t *currentControlRateProfile;
static const uint8_t EEPROM_CONF_VERSION = 95;
static const uint8_t EEPROM_CONF_VERSION = 96;
static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims)
{
@ -407,6 +407,9 @@ static void resetConf(void)
masterConfig.rxConfig.midrc = 1500;
masterConfig.rxConfig.mincheck = 1100;
masterConfig.rxConfig.maxcheck = 1900;
masterConfig.rxConfig.rx_min_usec = 985; // any of first 4 channels below this value will trigger rx loss detection
masterConfig.rxConfig.rx_max_usec = 2115; // any of first 4 channels above this value will trigger rx loss detection
masterConfig.rxConfig.rssi_channel = 0;
masterConfig.rxConfig.rssi_scale = RSSI_SCALE_DEFAULT;
@ -474,11 +477,9 @@ static void resetConf(void)
currentProfile->throttle_correction_angle = 800; // could be 80.0 deg with atlhold or 45.0 for fpv
// Failsafe Variables
currentProfile->failsafeConfig.failsafe_delay = 10; // 1sec
currentProfile->failsafeConfig.failsafe_off_delay = 200; // 20sec
currentProfile->failsafeConfig.failsafe_throttle = 1000; // default throttle off.
currentProfile->failsafeConfig.failsafe_min_usec = 985; // any of first 4 channels below this value will trigger failsafe
currentProfile->failsafeConfig.failsafe_max_usec = 2115; // any of first 4 channels above this value will trigger failsafe
masterConfig.failsafeConfig.failsafe_delay = 10; // 1sec
masterConfig.failsafeConfig.failsafe_off_delay = 200; // 20sec
masterConfig.failsafeConfig.failsafe_throttle = 1000; // default throttle off.
#ifdef USE_SERVOS
// servos
@ -533,9 +534,9 @@ static void resetConf(void)
currentProfile->pidProfile.pidController = 3;
currentProfile->pidProfile.P8[ROLL] = 36;
currentProfile->pidProfile.P8[PITCH] = 36;
currentProfile->failsafeConfig.failsafe_delay = 2;
currentProfile->failsafeConfig.failsafe_off_delay = 0;
currentProfile->failsafeConfig.failsafe_throttle = 1000;
masterConfig.failsafeConfig.failsafe_delay = 2;
masterConfig.failsafeConfig.failsafe_off_delay = 0;
masterConfig.failsafeConfig.failsafe_throttle = 1000;
currentControlRateProfile->rcRate8 = 130;
currentControlRateProfile->rates[FD_PITCH] = 20;
currentControlRateProfile->rates[FD_ROLL] = 20;
@ -671,7 +672,7 @@ void activateConfig(void)
gpsUsePIDs(&currentProfile->pidProfile);
#endif
useFailsafeConfig(&currentProfile->failsafeConfig);
useFailsafeConfig(&masterConfig.failsafeConfig);
setAccelerationTrims(&masterConfig.accZero);
mixerUseConfigs(

View file

@ -61,6 +61,8 @@ typedef struct master_t {
rxConfig_t rxConfig;
inputFilteringMode_e inputFilteringMode; // Use hardware input filtering, e.g. for OrangeRX PPM/PWM receivers.
failsafeConfig_t failsafeConfig;
uint8_t retarded_arm; // allow disarm/arm on throttle down + roll left/right
uint8_t disarm_kill_switch; // allow disarm via AUX switch regardless of throttle value
uint8_t auto_disarm_delay; // allow automatically disarming multicopters after auto_disarm_delay seconds of zero throttle. Disabled when 0

View file

@ -54,9 +54,6 @@ typedef struct profile_s {
gimbalConfig_t gimbalConfig;
#endif
// Failsafe related configuration
failsafeConfig_t failsafeConfig;
#ifdef GPS
gpsProfile_t gpsProfile;
#endif

View file

@ -106,10 +106,12 @@ typedef struct
GPIO_Speed speed;
} gpio_config_t;
#ifndef UNIT_TEST
static inline void digitalHi(GPIO_TypeDef *p, uint16_t i) { p->BSRR = i; }
static inline void digitalLo(GPIO_TypeDef *p, uint16_t i) { p->BRR = i; }
static inline void digitalToggle(GPIO_TypeDef *p, uint16_t i) { p->ODR ^= i; }
static inline uint16_t digitalIn(GPIO_TypeDef *p, uint16_t i) {return p->IDR & i; }
#endif
void gpioInit(GPIO_TypeDef *gpio, gpio_config_t *config);
void gpioExtiLineConfig(uint8_t portsrc, uint8_t pinsrc);

View file

@ -33,6 +33,11 @@ typedef uint16_t timCCR_t;
typedef uint16_t timCCER_t;
typedef uint16_t timSR_t;
typedef uint16_t timCNT_t;
#elif defined(UNIT_TEST)
typedef uint32_t timCCR_t;
typedef uint32_t timCCER_t;
typedef uint32_t timSR_t;
typedef uint32_t timCNT_t;
#else
# error "Unknown CPU defined"
#endif

View file

@ -44,9 +44,10 @@ static failsafeConfig_t *failsafeConfig;
static rxConfig_t *rxConfig;
void failsafeReset(void)
static void failsafeReset(void)
{
failsafeState.counter = 0;
failsafeState.phase = FAILSAFE_IDLE;
}
/*
@ -58,54 +59,75 @@ void useFailsafeConfig(failsafeConfig_t *failsafeConfigToUse)
failsafeReset();
}
failsafeState_t* failsafeInit(rxConfig_t *intialRxConfig)
void failsafeInit(rxConfig_t *intialRxConfig)
{
rxConfig = intialRxConfig;
failsafeState.events = 0;
failsafeState.enabled = false;
failsafeState.monitoring = false;
return &failsafeState;
return;
}
bool failsafeIsIdle(void)
failsafePhase_e failsafePhase()
{
return failsafeState.counter == 0;
return failsafeState.phase;
}
bool failsafeIsEnabled(void)
#define MAX_COUNTER_VALUE_WHEN_RX_IS_RECEIVED_AFTER_RX_CYCLE 1
bool failsafeIsReceivingRxData(void)
{
return failsafeState.enabled;
return failsafeState.counter <= MAX_COUNTER_VALUE_WHEN_RX_IS_RECEIVED_AFTER_RX_CYCLE;
}
void failsafeEnable(void)
bool failsafeIsMonitoring(void)
{
failsafeState.enabled = true;
return failsafeState.monitoring;
}
bool failsafeHasTimerElapsed(void)
bool failsafeIsActive(void)
{
return failsafeState.active;
}
void failsafeStartMonitoring(void)
{
failsafeState.monitoring = true;
}
static bool failsafeHasTimerElapsed(void)
{
return failsafeState.counter > (5 * failsafeConfig->failsafe_delay);
}
bool failsafeShouldForceLanding(bool armed)
static bool failsafeShouldForceLanding(bool armed)
{
return failsafeHasTimerElapsed() && armed;
}
bool failsafeShouldHaveCausedLandingByNow(void)
static bool failsafeShouldHaveCausedLandingByNow(void)
{
return failsafeState.counter > 5 * (failsafeConfig->failsafe_delay + failsafeConfig->failsafe_off_delay);
}
static void failsafeAvoidRearm(void)
static void failsafeActivate(void)
{
// 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 = true;
failsafeState.phase = FAILSAFE_LANDING;
failsafeState.events++;
}
static void failsafeOnValidDataReceived(void)
static void failsafeApplyControlInput(void)
{
for (int i = 0; i < 3; i++) {
rcData[i] = rxConfig->midrc;
}
rcData[THROTTLE] = failsafeConfig->failsafe_throttle;
}
void failsafeOnValidDataReceived(void)
{
if (failsafeState.counter > 20)
failsafeState.counter -= 20;
@ -115,58 +137,76 @@ static void failsafeOnValidDataReceived(void)
void failsafeUpdateState(void)
{
uint8_t i;
bool receivingRxData = failsafeIsReceivingRxData();
bool armed = ARMING_FLAG(ARMED);
if (!failsafeHasTimerElapsed()) {
return;
if (receivingRxData) {
failsafeState.phase = FAILSAFE_IDLE;
failsafeState.active = false;
}
if (!failsafeIsEnabled()) {
failsafeReset();
return;
}
if (failsafeShouldForceLanding(ARMING_FLAG(ARMED))) { // Stabilize, and set Throttle to specified level
failsafeAvoidRearm();
bool reprocessState;
for (i = 0; i < 3; i++) {
rcData[i] = rxConfig->midrc; // after specified guard time after RC signal is lost (in 0.1sec)
do {
reprocessState = false;
switch (failsafeState.phase) {
case FAILSAFE_IDLE:
if (!receivingRxData && armed) {
failsafeState.phase = FAILSAFE_RX_LOSS_DETECTED;
reprocessState = true;
}
break;
case FAILSAFE_RX_LOSS_DETECTED:
if (failsafeShouldForceLanding(armed)) {
// Stabilize, and set Throttle to specified level
failsafeActivate();
reprocessState = true;
}
break;
case FAILSAFE_LANDING:
if (armed) {
failsafeApplyControlInput();
}
if (failsafeShouldHaveCausedLandingByNow() || !armed) {
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;
mwDisarm();
break;
default:
break;
}
rcData[THROTTLE] = failsafeConfig->failsafe_throttle;
failsafeState.events++;
}
} while (reprocessState);
if (failsafeShouldHaveCausedLandingByNow() || !ARMING_FLAG(ARMED)) {
mwDisarm();
}
}
/**
* Should be called once each time RX data is processed by the system.
* Should be called once when RX data is processed by the system.
*/
void failsafeOnRxCycle(void)
void failsafeOnRxCycleStarted(void)
{
failsafeState.counter++;
}
#define REQUIRED_CHANNEL_MASK 0x0F // first 4 channels
// pulse duration is in micro seconds (usec)
void failsafeCheckPulse(uint8_t channel, uint16_t pulseDuration)
{
static uint8_t goodChannelMask = 0;
if (channel < 4 &&
pulseDuration > failsafeConfig->failsafe_min_usec &&
pulseDuration < failsafeConfig->failsafe_max_usec
) {
// if signal is valid - mark channel as OK
goodChannelMask |= (1 << channel);
}
if (goodChannelMask == REQUIRED_CHANNEL_MASK) {
goodChannelMask = 0;
failsafeOnValidDataReceived();
}
}

View file

@ -23,30 +23,37 @@ 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.
uint16_t failsafe_min_usec;
uint16_t failsafe_max_usec;
} failsafeConfig_t;
typedef enum {
FAILSAFE_IDLE = 0,
FAILSAFE_RX_LOSS_DETECTED,
FAILSAFE_LANDING,
FAILSAFE_LANDED
} failsafePhase_e;
typedef struct failsafeState_s {
int16_t counter;
int16_t events;
bool enabled;
bool monitoring;
bool active;
failsafePhase_e phase;
} failsafeState_t;
void useFailsafeConfig(failsafeConfig_t *failsafeConfigToUse);
void failsafeEnable(void);
void failsafeOnRxCycle(void);
void failsafeCheckPulse(uint8_t channel, uint16_t pulseDuration);
void failsafeStartMonitoring(void);
void failsafeUpdateState(void);
void failsafeReset(void);
failsafePhase_e failsafePhase();
bool failsafeIsMonitoring(void);
bool failsafeIsActive(void);
bool failsafeIsReceivingRxData(void);
void failsafeOnValidDataReceived(void);
void failsafeOnRxCycleStarted(void);
bool failsafeIsEnabled(void);
bool failsafeIsIdle(void);
bool failsafeHasTimerElapsed(void);
bool failsafeShouldForceLanding(bool armed);
bool failsafeShouldHaveCausedLandingByNow(void);

View file

@ -49,9 +49,9 @@ static void beep_code(char first, char second, char third, char pause);
static uint8_t toggleBeep = 0;
typedef enum {
FAILSAFE_IDLE = 0,
FAILSAFE_LANDING,
FAILSAFE_FIND_ME
FAILSAFE_WARNING_NONE = 0,
FAILSAFE_WARNING_LANDING,
FAILSAFE_WARNING_FIND_ME
} failsafeBeeperWarnings_e;
void beepcodeInit(void)
@ -64,7 +64,7 @@ void beepcodeUpdateState(batteryState_e batteryState)
#ifdef GPS
static uint8_t warn_noGPSfix = 0;
#endif
static failsafeBeeperWarnings_e warn_failsafe = FAILSAFE_IDLE;
static failsafeBeeperWarnings_e warn_failsafe = FAILSAFE_WARNING_NONE;
//===================== BeeperOn via rcOptions =====================
if (IS_RC_MODE_ACTIVE(BOXBEEPERON)) { // unconditional beeper on via AUXn switch
@ -74,20 +74,19 @@ void beepcodeUpdateState(batteryState_e batteryState)
}
//===================== Beeps for failsafe =====================
if (feature(FEATURE_FAILSAFE)) {
if (failsafeShouldForceLanding(ARMING_FLAG(ARMED))) {
warn_failsafe = FAILSAFE_LANDING;
if (failsafeShouldHaveCausedLandingByNow()) {
warn_failsafe = FAILSAFE_FIND_ME;
}
switch (failsafePhase()) {
case FAILSAFE_LANDING:
warn_failsafe = FAILSAFE_WARNING_LANDING;
break;
case FAILSAFE_LANDED:
warn_failsafe = FAILSAFE_WARNING_FIND_ME;
break;
default:
warn_failsafe = FAILSAFE_WARNING_NONE;
}
if (failsafeHasTimerElapsed() && !ARMING_FLAG(ARMED)) {
warn_failsafe = FAILSAFE_FIND_ME;
}
if (failsafeIsIdle()) {
warn_failsafe = FAILSAFE_IDLE; // turn off alarm if TX is okay
if (rxIsReceivingSignal()) {
warn_failsafe = FAILSAFE_WARNING_NONE;
}
}

View file

@ -51,6 +51,7 @@
#include "flight/pid.h"
#include "flight/imu.h"
#include "flight/failsafe.h"
#ifdef GPS
#include "io/gps.h"
@ -204,6 +205,33 @@ void updateTicker(void)
tickerIndex = tickerIndex % TICKER_CHARACTER_COUNT;
}
void updateRxStatus(void)
{
i2c_OLED_set_xy(SCREEN_CHARACTER_COLUMN_COUNT - 2, 0);
i2c_OLED_send_char(rxIsReceivingSignal() ? 'R' : '!');
}
void updateFailsafeStatus(void)
{
char failsafeIndicator = '?';
switch (failsafePhase()) {
case FAILSAFE_IDLE:
failsafeIndicator = '-';
break;
case FAILSAFE_RX_LOSS_DETECTED:
failsafeIndicator = 'R';
break;
case FAILSAFE_LANDING:
failsafeIndicator = 'l';
break;
case FAILSAFE_LANDED:
failsafeIndicator = 'L';
break;
}
i2c_OLED_set_xy(SCREEN_CHARACTER_COLUMN_COUNT - 3, 0);
i2c_OLED_send_char(failsafeIndicator);
}
void showTitle()
{
i2c_OLED_set_line(0);
@ -582,8 +610,11 @@ void updateDisplay(void)
#endif
}
if (!armedState) {
updateFailsafeStatus();
updateRxStatus();
updateTicker();
}
}
void displaySetPage(pageId_e pageId)

View file

@ -661,7 +661,7 @@ void applyLedWarningLayer(uint8_t updateNow)
if (feature(FEATURE_VBAT) && calculateBatteryState() != BATTERY_OK) {
warningFlags |= WARNING_FLAG_LOW_BATTERY;
}
if (feature(FEATURE_FAILSAFE) && failsafeHasTimerElapsed()) {
if (feature(FEATURE_FAILSAFE) && failsafeIsActive()) {
warningFlags |= WARNING_FLAG_FAILSAFE;
}
if (!ARMING_FLAG(ARMED) && !ARMING_FLAG(OK_TO_ARM)) {
@ -714,6 +714,9 @@ void applyLedIndicatorLayer(uint8_t indicatorFlashState)
const ledConfig_t *ledConfig;
static const hsvColor_t *flashColor;
if (!rxIsReceivingSignal()) {
return;
}
if (indicatorFlashState == 0) {
flashColor = &hsv_orange;

View file

@ -51,6 +51,7 @@
#include "flight/pid.h"
#include "flight/navigation.h"
#include "flight/failsafe.h"
#include "mw.h"
@ -124,7 +125,8 @@ void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStat
}
} else {
// Disarming via ARM BOX
if (ARMING_FLAG(ARMED)) {
if (ARMING_FLAG(ARMED) && rxIsReceivingSignal() && !failsafeIsActive() ) {
if (disarm_kill_switch) {
mwDisarm();
} else if (throttleStatus == THROTTLE_LOW) {

View file

@ -404,11 +404,12 @@ const clivalue_t valueTable[] = {
{ "tpa_rate", VAR_UINT8 | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].dynThrPID, 0, CONTROL_RATE_CONFIG_TPA_MAX},
{ "tpa_breakpoint", VAR_UINT16 | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].tpa_breakpoint, PWM_RANGE_MIN, PWM_RANGE_MAX},
{ "failsafe_delay", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].failsafeConfig.failsafe_delay, 0, 200 },
{ "failsafe_off_delay", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].failsafeConfig.failsafe_off_delay, 0, 200 },
{ "failsafe_throttle", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].failsafeConfig.failsafe_throttle, PWM_RANGE_MIN, PWM_RANGE_MAX },
{ "failsafe_min_usec", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].failsafeConfig.failsafe_min_usec, 100, PWM_RANGE_MAX },
{ "failsafe_max_usec", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].failsafeConfig.failsafe_max_usec, 100, PWM_RANGE_MAX + (PWM_RANGE_MAX - PWM_RANGE_MIN) },
{ "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 },
{ "rx_min_usec", VAR_UINT16 | MASTER_VALUE, &masterConfig.rxConfig.rx_min_usec, 100, PWM_RANGE_MAX },
{ "rx_max_usec", VAR_UINT16 | MASTER_VALUE, &masterConfig.rxConfig.rx_max_usec, 100, PWM_RANGE_MAX + (PWM_RANGE_MAX - PWM_RANGE_MIN) },
#ifdef USE_SERVOS
{ "gimbal_flags", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].gimbalConfig.gimbal_flags, 0, 255},

View file

@ -1003,7 +1003,7 @@ static bool processOutCommand(uint8_t cmdMSP)
serialize16(masterConfig.escAndServoConfig.maxthrottle);
serialize16(masterConfig.escAndServoConfig.mincommand);
serialize16(currentProfile->failsafeConfig.failsafe_throttle);
serialize16(masterConfig.failsafeConfig.failsafe_throttle);
#ifdef GPS
serialize8(masterConfig.gpsConfig.provider); // gps_type
@ -1378,7 +1378,7 @@ static bool processInCommand(void)
masterConfig.escAndServoConfig.maxthrottle = read16();
masterConfig.escAndServoConfig.mincommand = read16();
currentProfile->failsafeConfig.failsafe_throttle = read16();
masterConfig.failsafeConfig.failsafe_throttle = read16();
#ifdef GPS
masterConfig.gpsConfig.provider = read8(); // gps_type

View file

@ -175,7 +175,7 @@ void annexCode(void)
static uint8_t vbatTimer = 0;
static int32_t vbatCycleTime = 0;
// PITCH & ROLL only dynamic PID adjustemnt, depending on throttle value
// PITCH & ROLL only dynamic PID adjustment, depending on throttle value
if (rcData[THROTTLE] < currentControlRateProfile->tpa_breakpoint) {
prop2 = 100;
} else {
@ -509,8 +509,8 @@ void processRx(void)
if (feature(FEATURE_FAILSAFE)) {
if (currentTime > FAILSAFE_POWER_ON_DELAY_US && !failsafeIsEnabled()) {
failsafeEnable();
if (currentTime > FAILSAFE_POWER_ON_DELAY_US && !failsafeIsMonitoring()) {
failsafeStartMonitoring();
}
failsafeUpdateState();
@ -527,7 +527,8 @@ void processRx(void)
if (ARMING_FLAG(ARMED)
&& feature(FEATURE_MOTOR_STOP) && !STATE(FIXED_WING)
&& masterConfig.auto_disarm_delay != 0
&& isUsingSticksForArming()) {
&& isUsingSticksForArming())
{
if (throttleStatus == THROTTLE_LOW) {
if ((int32_t)(disarmAt - millis()) < 0) // delay is over
mwDisarm();
@ -551,7 +552,7 @@ void processRx(void)
bool canUseHorizonMode = true;
if ((IS_RC_MODE_ACTIVE(BOXANGLE) || (feature(FEATURE_FAILSAFE) && failsafeHasTimerElapsed())) && (sensors(SENSOR_ACC))) {
if ((IS_RC_MODE_ACTIVE(BOXANGLE) || (feature(FEATURE_FAILSAFE) && failsafeIsActive())) && (sensors(SENSOR_ACC))) {
// bumpless transfer to Level mode
canUseHorizonMode = false;
@ -628,7 +629,7 @@ void loop(void)
static bool haveProcessedAnnexCodeOnce = false;
#endif
updateRx();
updateRx(currentTime);
if (shouldProcessRx(currentTime)) {
processRx();
@ -702,7 +703,12 @@ void loop(void)
// If we're armed, at minimum throttle, and we do arming via the
// sticks, do not process yaw input from the rx. We do this so the
// motors do not spin up while we are trying to arm or disarm.
if (isUsingSticksForArming() && rcData[THROTTLE] <= masterConfig.rxConfig.mincheck) {
// Allow yaw control for tricopters if the user wants the servo to move even when unarmed.
if (isUsingSticksForArming() && rcData[THROTTLE] <= masterConfig.rxConfig.mincheck
#ifndef USE_QUAD_MIXER_ONLY
&& !(masterConfig.mixerMode == MIXER_TRI && masterConfig.mixerConfig.tri_unarmed_servo)
#endif
) {
rcCommand[YAW] = 0;
}

View file

@ -54,12 +54,10 @@ bool rxMspFrameComplete(void)
return true;
}
bool rxMspInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback)
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.
if (callback)
*callback = rxMspReadRawRC;
return true;
}

View file

@ -56,12 +56,19 @@ bool spektrumInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcRe
bool sumdInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback);
bool sumhInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback);
bool rxMspInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback);
void rxMspInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback);
const char rcChannelLetters[] = "AERT12345678abcdefgh";
uint16_t rssi = 0; // range: [0;1023]
static bool rxDataReceived = false;
static bool rxSignalReceived = false;
static bool shouldCheckPulse = true;
static uint32_t rxUpdateAt = 0;
static uint32_t needRxSignalBefore = 0;
int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; // interval [1000;2000]
#define PPM_AND_PWM_SAMPLE_COUNT 4
@ -70,6 +77,7 @@ int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; // interval [1000;2000]
#define PULSE_MAX 2250 // maximum PWM pulse width which is considered valid
#define DELAY_50_HZ (1000000 / 50)
#define DELAY_10_HZ (1000000 / 10)
static rcReadRawDataPtr rcReadRawFunc = NULL; // receive data from default (pwm/ppm) or additional (spek/sbus/?? receiver drivers)
@ -83,7 +91,28 @@ void useRxConfig(rxConfig_t *rxConfigToUse)
rxConfig = rxConfigToUse;
}
#define STICK_CHANNEL_COUNT 4
#define REQUIRED_CHANNEL_MASK 0x0F // first 4 channels
// pulse duration is in micro seconds (usec)
STATIC_UNIT_TESTED void rxCheckPulse(uint8_t channel, uint16_t pulseDuration)
{
static uint8_t goodChannelMask = 0;
if (channel < 4 &&
pulseDuration >= rxConfig->rx_min_usec &&
pulseDuration <= rxConfig->rx_max_usec
) {
// if signal is valid - mark channel as OK
goodChannelMask |= (1 << channel);
}
if (goodChannelMask == REQUIRED_CHANNEL_MASK) {
goodChannelMask = 0;
failsafeOnValidDataReceived();
rxSignalReceived = true;
}
}
void rxInit(rxConfig_t *rxConfig)
{
@ -179,43 +208,71 @@ uint8_t calculateChannelRemapping(uint8_t *channelMap, uint8_t channelMapEntryCo
return channelToRemap;
}
static bool rcDataReceived = false;
static uint32_t rxUpdateAt = 0;
void updateRx(void)
bool rxIsReceivingSignal(void)
{
rcDataReceived = false;
return rxSignalReceived;
}
static bool isRxDataDriven(void) {
return !(feature(FEATURE_RX_PARALLEL_PWM | FEATURE_RX_PPM));
}
void updateRx(uint32_t currentTime)
{
rxDataReceived = false;
shouldCheckPulse = true;
if (rxSignalReceived) {
if (((int32_t)(currentTime - needRxSignalBefore) >= 0)) {
rxSignalReceived = false;
}
}
#ifdef SERIAL_RX
if (feature(FEATURE_RX_SERIAL)) {
uint8_t frameStatus = serialRxFrameStatus(rxConfig);
if (frameStatus & SERIAL_RX_FRAME_COMPLETE) {
rcDataReceived = true;
if ((frameStatus & SERIAL_RX_FRAME_FAILSAFE) == 0 && feature(FEATURE_FAILSAFE)) {
failsafeReset();
rxDataReceived = true;
rxSignalReceived = (frameStatus & SERIAL_RX_FRAME_FAILSAFE) == 0;
if (rxSignalReceived && feature(FEATURE_FAILSAFE)) {
shouldCheckPulse = false;
failsafeOnValidDataReceived();
}
} else {
shouldCheckPulse = false;
}
}
#endif
if (feature(FEATURE_RX_MSP)) {
rcDataReceived = rxMspFrameComplete();
if (rcDataReceived && feature(FEATURE_FAILSAFE)) {
failsafeReset();
rxDataReceived = rxMspFrameComplete();
if (rxDataReceived) {
if (feature(FEATURE_FAILSAFE)) {
failsafeOnValidDataReceived();
}
}
}
if (feature(FEATURE_RX_SERIAL | FEATURE_RX_MSP) && rxDataReceived) {
needRxSignalBefore = currentTime + DELAY_10_HZ;
}
if (feature(FEATURE_RX_PPM)) {
if (isPPMDataBeingReceived()) {
rxSignalReceived = true;
needRxSignalBefore = currentTime + DELAY_10_HZ;
resetPPMDataReceivedState();
}
shouldCheckPulse = rxSignalReceived;
}
}
bool shouldProcessRx(uint32_t currentTime)
{
return rcDataReceived || ((int32_t)(currentTime - rxUpdateAt) >= 0); // data driven or 50Hz
}
static bool isRxDataDriven(void) {
return !(feature(FEATURE_RX_PARALLEL_PWM | FEATURE_RX_PPM));
return rxDataReceived || ((int32_t)(currentTime - rxUpdateAt) >= 0); // data driven or 50Hz
}
static uint8_t rcSampleIndex = 0;
@ -256,17 +313,6 @@ static void processRxChannels(void)
return; // rcData will have already been updated by MSP_SET_RAW_RC
}
bool shouldCheckPulse = true;
if (feature(FEATURE_FAILSAFE)) {
if (feature(FEATURE_RX_PPM)) {
shouldCheckPulse = isPPMDataBeingReceived();
resetPPMDataReceivedState();
} else {
shouldCheckPulse = !isRxDataDriven();
}
}
for (chan = 0; chan < rxRuntimeConfig.channelCount; chan++) {
if (!rcReadRawFunc) {
@ -279,8 +325,8 @@ static void processRxChannels(void)
// sample the channel
uint16_t sample = rcReadRawFunc(&rxRuntimeConfig, rawChannel);
if (feature(FEATURE_FAILSAFE) && shouldCheckPulse) {
failsafeCheckPulse(chan, sample);
if (shouldCheckPulse) {
rxCheckPulse(chan, sample);
}
// validate the range
@ -311,9 +357,7 @@ void calculateRxChannelsAndUpdateFailsafe(uint32_t currentTime)
{
rxUpdateAt = currentTime + DELAY_50_HZ;
if (feature(FEATURE_FAILSAFE)) {
failsafeOnRxCycle();
}
failsafeOnRxCycleStarted();
if (isRxDataDriven()) {
processDataDrivenRx();

View file

@ -17,6 +17,8 @@
#pragma once
#define STICK_CHANNEL_COUNT 4
#define PWM_RANGE_ZERO 0 // FIXME should all usages of this be changed to use PWM_RANGE_MIN?
#define PWM_RANGE_MIN 1000
#define PWM_RANGE_MAX 2000
@ -79,6 +81,10 @@ typedef struct rxConfig_s {
uint16_t midrc; // Some radios have not a neutral point centered on 1500. can be changed here
uint16_t mincheck; // minimum rc end
uint16_t maxcheck; // maximum rc end
uint16_t rx_min_usec;
uint16_t rx_max_usec;
} rxConfig_t;
#define REMAPPABLE_CHANNEL_COUNT (sizeof(((rxConfig_t *)0)->rcmap) / sizeof(((rxConfig_t *)0)->rcmap[0]))
@ -94,7 +100,8 @@ void useRxConfig(rxConfig_t *rxConfigToUse);
typedef uint16_t (*rcReadRawDataPtr)(rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan); // used by receiver driver to return channel data
void updateRx(void);
void updateRx(uint32_t currentTime);
bool rxIsReceivingSignal(void);
bool shouldProcessRx(uint32_t currentTime);
void calculateRxChannelsAndUpdateFailsafe(uint32_t currentTime);

View file

@ -47,11 +47,13 @@ TESTS = \
battery_unittest \
flight_imu_unittest \
flight_mixer_unittest \
flight_failsafe_unittest \
altitude_hold_unittest \
maths_unittest \
gps_conversion_unittest \
telemetry_hott_unittest \
rc_controls_unittest \
rx_rx_unittest \
ledstrip_unittest \
ws2811_unittest \
encoding_unittest \
@ -390,6 +392,30 @@ flight_mixer_unittest : \
$(CXX) $(CXX_FLAGS) -lpthread $^ -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 $@
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) -lpthread $^ -o $(OBJECT_DIR)/$@
$(OBJECT_DIR)/io/serial.o : \
$(USER_DIR)/io/serial.c \
$(USER_DIR)/io/serial.h \
@ -413,6 +439,30 @@ io_serial_unittest : \
$(CXX) $(CXX_FLAGS) -lpthread $^ -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 $@
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) -lpthread $^ -o $(OBJECT_DIR)/$@
test: $(TESTS)
set -e && for test in $(TESTS) ; do \
$(OBJECT_DIR)/$$test; \

View file

@ -0,0 +1,317 @@
/*
* 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 <stdint.h>
#include <stdbool.h>
#include <limits.h>
extern "C" {
#include "debug.h"
#include "platform.h"
#include "common/axis.h"
#include "common/maths.h"
#include "config/runtime_config.h"
#include "rx/rx.h"
#include "flight/failsafe.h"
failsafeState_t* failsafeInit(rxConfig_t *intialRxConfig);
}
#include "unittest_macros.h"
#include "gtest/gtest.h"
uint32_t testFeatureMask = 0;
enum {
COUNTER_MW_DISARM = 0,
};
#define CALL_COUNT_ITEM_COUNT 1
static int callCounts[CALL_COUNT_ITEM_COUNT];
#define CALL_COUNTER(item) (callCounts[item])
void resetCallCounters(void) {
memset(&callCounts, 0, sizeof(callCounts));
}
#define TEST_MID_RC 1495 // something other than the default 1500 will suffice.
rxConfig_t rxConfig;
failsafeConfig_t failsafeConfig;
void configureFailsafe(void)
{
memset(&rxConfig, 0, sizeof(rxConfig));
rxConfig.midrc = TEST_MID_RC;
memset(&failsafeConfig, 0, sizeof(failsafeConfig));
failsafeConfig.failsafe_delay = 10; // 1 second
failsafeConfig.failsafe_off_delay = 50; // 5 seconds
}
//
// Stepwise tests
//
TEST(FlightFailsafeTest, TestFailsafeInitialState)
{
// given
configureFailsafe();
// and
DISABLE_ARMING_FLAG(ARMED);
// when
useFailsafeConfig(&failsafeConfig);
failsafeInit(&rxConfig);
// then
EXPECT_EQ(false, failsafeIsMonitoring());
EXPECT_EQ(false, failsafeIsActive());
EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
}
TEST(FlightFailsafeTest, TestFailsafeStartMonitoring)
{
// when
failsafeStartMonitoring();
// then
EXPECT_EQ(true, failsafeIsMonitoring());
EXPECT_EQ(false, failsafeIsActive());
EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
}
TEST(FlightFailsafeTest, TestFailsafeFirstArmedCycle)
{
// given
ENABLE_ARMING_FLAG(ARMED);
// when
failsafeOnRxCycleStarted();
failsafeOnValidDataReceived();
// and
failsafeUpdateState();
// then
EXPECT_EQ(false, failsafeIsActive());
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();
failsafeOnValidDataReceived();
failsafeUpdateState();
// then
EXPECT_EQ(false, failsafeIsActive());
EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
}
}
TEST(FlightFailsafeTest, TestFailsafeDetectsRxLossAndStartsLanding)
{
// given
ENABLE_ARMING_FLAG(ARMED);
// when
failsafeOnRxCycleStarted();
// no call to failsafeOnValidDataReceived();
failsafeUpdateState();
// then
EXPECT_EQ(false, failsafeIsActive());
EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
//
// currently one cycle must occur (above) so that the next cycle (below) can detect the lack of an update.
//
// when
for (int i = 0; i < FAILSAFE_UPDATE_HZ - 1; 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.
//
// 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(1, CALL_COUNTER(COUNTER_MW_DISARM));
EXPECT_TRUE(ARMING_FLAG(PREVENT_ARMING));
// given
DISABLE_ARMING_FLAG(ARMED);
// when
failsafeOnRxCycleStarted();
// no call to failsafeOnValidDataReceived();
failsafeUpdateState();
// then
EXPECT_EQ(false, failsafeIsActive());
EXPECT_EQ(FAILSAFE_LANDED, failsafePhase());
EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM)); // disarm not called repeatedly.
EXPECT_TRUE(ARMING_FLAG(PREVENT_ARMING));
}
//
// Additional non-stepwise tests
//
TEST(FlightFailsafeTest, TestFailsafeNotActivatedWhenDisarmedAndRXLossIsDetected)
{
// given
configureFailsafe();
// and
useFailsafeConfig(&failsafeConfig);
failsafeInit(&rxConfig);
// and
DISABLE_ARMING_FLAG(ARMED);
// when
failsafeStartMonitoring();
// and
int callsToMakeToSimulateTenSeconds = FAILSAFE_UPDATE_HZ * 10;
for (int i = 0; i < callsToMakeToSimulateTenSeconds; i++) {
failsafeOnRxCycleStarted();
// no call to failsafeOnValidDataReceived();
failsafeUpdateState();
// then
EXPECT_EQ(true, failsafeIsMonitoring());
EXPECT_EQ(false, failsafeIsActive());
EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
}
}
// STUBS
extern "C" {
int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];
uint8_t armingFlags;
void delay(uint32_t) {}
bool feature(uint32_t mask) {
return (mask & testFeatureMask);
}
void mwDisarm(void) {
callCounts[COUNTER_MW_DISARM]++;
}
}

View file

@ -418,6 +418,7 @@ int scaleRange(int x, int srcMin, int srcMax, int destMin, int destMax) {
return 0;
}
bool failsafeHasTimerElapsed(void) { return false; }
bool failsafeIsActive() { return false; }
bool rxIsReceivingSignal() { return true; }
}

View file

@ -27,3 +27,21 @@
#define SERIAL_PORT_COUNT 4
#define MAX_SIMULTANEOUS_ADJUSTMENT_COUNT 6
typedef enum
{
Mode_TEST = 0x0,
} GPIO_Mode;
typedef struct
{
void* test;
} GPIO_TypeDef;
typedef struct
{
void* test;
} TIM_TypeDef;
typedef enum {DISABLE = 0, ENABLE = !DISABLE} FunctionalState;

View file

@ -711,6 +711,9 @@ void mwDisarm(void) {}
void displayDisablePageCycling() {}
void displayEnablePageCycling() {}
bool failsafeIsActive() { return false; }
bool rxIsReceivingSignal() { return true; }
uint8_t getCurrentControlRateProfile(void) {
return 0;
}

View file

@ -0,0 +1,162 @@
/*
* 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 <stdint.h>
#include <stdbool.h>
#include <limits.h>
extern "C" {
#include "platform.h"
#include "rx/rx.h"
void rxInit(rxConfig_t *rxConfig);
void rxCheckPulse(uint8_t channel, uint16_t pulseDuration);
}
#include "unittest_macros.h"
#include "gtest/gtest.h"
enum {
COUNTER_FAILSAFE_ON_VALID_DATA_RECEIVED = 0,
};
#define CALL_COUNT_ITEM_COUNT 1
static int callCounts[CALL_COUNT_ITEM_COUNT];
#define CALL_COUNTER(item) (callCounts[item])
void resetCallCounters(void) {
memset(&callCounts, 0, sizeof(callCounts));
}
typedef struct testData_s {
bool isPPMDataBeingReceived;
} testData_t;
static testData_t testData;
TEST(RxTest, TestFailsafeInformedOfValidData)
{
// given
resetCallCounters();
memset(&testData, 0, sizeof(testData));
// and
rxConfig_t rxConfig;
memset(&rxConfig, 0, sizeof(rxConfig));
rxConfig.rx_min_usec = 1000;
rxConfig.rx_max_usec = 2000;
// when
rxInit(&rxConfig);
for (uint8_t channelIndex = 0; channelIndex < MAX_SUPPORTED_RC_CHANNEL_COUNT; channelIndex++) {
rxCheckPulse(channelIndex, 1500);
}
// then
EXPECT_EQ(1, CALL_COUNTER(COUNTER_FAILSAFE_ON_VALID_DATA_RECEIVED));
}
TEST(RxTest, TestFailsafeNotInformedOfValidDataWhenStickChannelsAreBad)
{
// given
memset(&testData, 0, sizeof(testData));
// and
rxConfig_t rxConfig;
memset(&rxConfig, 0, sizeof(rxConfig));
rxConfig.rx_min_usec = 1000;
rxConfig.rx_max_usec = 2000;
// and
uint16_t channelPulses[MAX_SUPPORTED_RC_CHANNEL_COUNT];
memset(&channelPulses, 1500, sizeof(channelPulses));
// and
rxInit(&rxConfig);
// and
for (uint8_t stickChannelIndex = 0; stickChannelIndex < STICK_CHANNEL_COUNT; stickChannelIndex++) {
// given
resetCallCounters();
for (uint8_t channelIndex = 0; channelIndex < STICK_CHANNEL_COUNT; channelIndex++) {
channelPulses[stickChannelIndex] = rxConfig.rx_min_usec;
}
channelPulses[stickChannelIndex] = rxConfig.rx_min_usec - 1;
// when
for (uint8_t channelIndex = 0; channelIndex < MAX_SUPPORTED_RC_CHANNEL_COUNT; channelIndex++) {
rxCheckPulse(channelIndex, channelPulses[channelIndex]);
}
// then
EXPECT_EQ(0, CALL_COUNTER(COUNTER_FAILSAFE_ON_VALID_DATA_RECEIVED));
// given
resetCallCounters();
for (uint8_t channelIndex = 0; channelIndex < STICK_CHANNEL_COUNT; channelIndex++) {
channelPulses[stickChannelIndex] = rxConfig.rx_max_usec;
}
channelPulses[stickChannelIndex] = rxConfig.rx_max_usec + 1;
// when
for (uint8_t channelIndex = 0; channelIndex < MAX_SUPPORTED_RC_CHANNEL_COUNT; channelIndex++) {
rxCheckPulse(channelIndex, channelPulses[channelIndex]);
}
// then
EXPECT_EQ(0, CALL_COUNTER(COUNTER_FAILSAFE_ON_VALID_DATA_RECEIVED));
}
}
// STUBS
extern "C" {
void failsafeOnRxCycleStarted() {}
void failsafeOnValidDataReceived() {
callCounts[COUNTER_FAILSAFE_ON_VALID_DATA_RECEIVED]++;
}
bool feature(uint32_t mask) {
UNUSED(mask);
return false;
}
bool isPPMDataBeingReceived(void) {
return testData.isPPMDataBeingReceived;
}
void resetPPMDataReceivedState(void) {}
bool rxMspFrameComplete(void) { return false; }
void rxMspInit(rxConfig_t *, rxRuntimeConfig_t *, rcReadRawDataPtr *) {}
void rxPwmInit(rxRuntimeConfig_t *, rcReadRawDataPtr *) {}
}