Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-14 20:10:18 +03:00
Code Issues Wiki Activity

Merge branch 'master' into serial-cleanup

Browse source 
Conflicts:
	src/main/blackbox/blackbox_io.c
	src/main/config/config.c
This commit is contained in:
Dominic Clifton 2015-02-26 22:43:29 +00:00
commit b6509dd1eb
60 changed files with 2471 additions and 386 deletions
Download patch file Download diff file Expand all files Collapse all files

8
Makefile
View file

@ -199,6 +199,7 @@ COMMON_SRC = build_config.c \
common/maths.c \ common/maths.c \
common/printf.c \ common/printf.c \
common/typeconversion.c \ common/typeconversion.c \
common/encoding.c \
main.c \ main.c \
mw.c \ mw.c \
flight/altitudehold.c \ flight/altitudehold.c \
@ -206,6 +207,7 @@ COMMON_SRC = build_config.c \
flight/pid.c \ flight/pid.c \
flight/imu.c \ flight/imu.c \
flight/mixer.c \ flight/mixer.c \
flight/lowpass.c \
drivers/bus_i2c_soft.c \ drivers/bus_i2c_soft.c \
drivers/serial.c \ drivers/serial.c \
drivers/sound_beeper.c \ drivers/sound_beeper.c \
@ -293,6 +295,8 @@ NAZE_SRC = startup_stm32f10x_md_gcc.S \
drivers/system_stm32f10x.c \ drivers/system_stm32f10x.c \
drivers/timer.c \ drivers/timer.c \
drivers/timer_stm32f10x.c \ drivers/timer_stm32f10x.c \
drivers/flash_m25p16.c \
io/flashfs.c \
hardware_revision.c \ hardware_revision.c \
$(HIGHEND_SRC) \ $(HIGHEND_SRC) \
$(COMMON_SRC) $(COMMON_SRC)
@ -396,6 +400,8 @@ CJMCU_SRC = \
drivers/timer.c \ drivers/timer.c \
drivers/timer_stm32f10x.c \ drivers/timer_stm32f10x.c \
hardware_revision.c \ hardware_revision.c \
blackbox/blackbox.c \
blackbox/blackbox_io.c \
$(COMMON_SRC) $(COMMON_SRC)
CC3D_SRC = \ CC3D_SRC = \
@ -499,6 +505,8 @@ SPRACINGF3_SRC = \
drivers/barometer_ms5611.c \ drivers/barometer_ms5611.c \
drivers/compass_hmc5883l.c \ drivers/compass_hmc5883l.c \
drivers/display_ug2864hsweg01.h \ drivers/display_ug2864hsweg01.h \
drivers/flash_m25p16.c \
io/flashfs.c \
$(HIGHEND_SRC) \ $(HIGHEND_SRC) \
$(COMMON_SRC) $(COMMON_SRC)

163
docs/Blackbox.md
View file

@ -5,7 +5,7 @@
## Introduction ## Introduction
This feature transmits your flight data information on every control loop iteration over a serial port to an external This feature transmits your flight data information on every control loop iteration over a serial port to an external
logging device to be recorded. logging device to be recorded, or to a dataflash chip which is present on some flight controllers.
After your flight, you can process the resulting logs on your computer to either turn them into CSV (comma-separated After your flight, you can process the resulting logs on your computer to either turn them into CSV (comma-separated
values) or render your flight log as a video using the tools `blackbox_decode` and `blackbox_render`. Those tools can be values) or render your flight log as a video using the tools `blackbox_decode` and `blackbox_render`. Those tools can be
@ -21,20 +21,17 @@ https://github.com/cleanflight/blackbox-log-viewer
The blackbox records flight data on every iteration of the flight control loop. It records the current time in The blackbox records flight data on every iteration of the flight control loop. It records the current time in
microseconds, P, I and D corrections for each axis, your RC command stick positions (after applying expo curves), microseconds, P, I and D corrections for each axis, your RC command stick positions (after applying expo curves),
gyroscope data, accelerometer data (after your configured low-pass filtering), barometer readings, 3-axis magnetometer gyroscope data, accelerometer data (after your configured low-pass filtering), barometer readings, 3-axis magnetometer
readings, raw VBAT measurements, and the command being sent to each motor speed controller. This is all stored without readings, raw VBAT and current measurements, and the command being sent to each motor speed controller. This is all
any approximation or loss of precision, so even quite subtle problems should be detectable from the fight data log. stored without any approximation or loss of precision, so even quite subtle problems should be detectable from the fight
data log.
Currently, the blackbox attempts to log GPS data whenever new GPS data is available, but this has not been tested yet. GPS data is logged whenever new GPS data is available. Although the CSV decoder will decode this data, the video
The CSV decoder and video renderer do not yet show any of the GPS data (though this will be added). If you have a working renderer does not yet show any of the GPS information (this will be added later).
GPS, please send in your logs so I can get the decoding implemented.
The data rate for my quadcopter using a looptime of 2400 is about 10.25kB/s. This allows about 18 days of flight logs
to fit on a 16GB MicroSD card, which ought to be enough for anybody :).
## Supported configurations ## Supported configurations
The maximum data rate for the flight log is fairly restricted, so anything that increases the load can cause the flight The maximum data rate that can be recorded to the flight log is fairly restricted, so anything that increases the load
log to drop frames and contain errors. can cause the flight log to drop frames and contain errors.
The Blackbox was developed and tested on a quadcopter. It has also been tested on a tricopter. It should work on The Blackbox was developed and tested on a quadcopter. It has also been tested on a tricopter. It should work on
hexacopters or octocopters, but as they transmit more information to the flight log (due to having more motors), the hexacopters or octocopters, but as they transmit more information to the flight log (due to having more motors), the
@ -49,10 +46,11 @@ details.
## Hardware ## Hardware
The blackbox software is designed to be used with an [OpenLog serial data logger][] and a microSDHC card. You need a There are two options for storing your flight logs. You can either transmit the log data over a serial port to an
little prep to get the OpenLog ready for use, so here are the details: external logging device like the [OpenLog serial data logger][] to be recorded to a microSDHC card, or if you have a
compatible flight controller you can store the logs on the onboard dataflash storage instead.
### Firmware ### OpenLog serial data logger
The OpenLog ships with standard OpenLog 3 firmware installed. However, in order to reduce the number of dropped frames, The OpenLog ships with standard OpenLog 3 firmware installed. However, in order to reduce the number of dropped frames,
it should be reflashed with the [OpenLog Light firmware][] or the special [OpenLog Blackbox firmware][] . The Blackbox it should be reflashed with the [OpenLog Light firmware][] or the special [OpenLog Blackbox firmware][] . The Blackbox
@ -66,18 +64,18 @@ along with instructions for installing it onto your OpenLog.
[OpenLog Light firmware]: https://github.com/sparkfun/OpenLog/tree/master/firmware/OpenLog_v3_Light [OpenLog Light firmware]: https://github.com/sparkfun/OpenLog/tree/master/firmware/OpenLog_v3_Light
[OpenLog Blackbox firmware]: https://github.com/cleanflight/blackbox-firmware [OpenLog Blackbox firmware]: https://github.com/cleanflight/blackbox-firmware
### microSDHC #### microSDHC
Your choice of microSDHC card is very important to the performance of the system. The OpenLog relies on being able to Your choice of microSDHC card is very important to the performance of the system. The OpenLog relies on being able to
make many small writes to the card with minimal delay, which not every card is good at. A faster SD-card speed rating is make many small writes to the card with minimal delay, which not every card is good at. A faster SD-card speed rating is
not a guarantee of better performance. not a guarantee of better performance.
#### microSDHC cards known to have poor performance ##### microSDHC cards known to have poor performance
- Generic 4GB Class 4 microSDHC card - the rate of missing frames is about 1%, and is concentrated around the most - Generic 4GB Class 4 microSDHC card - the rate of missing frames is about 1%, and is concentrated around the most
interesting parts of the log! interesting parts of the log!
#### microSDHC cards known to have good performance ##### microSDHC cards known to have good performance
- Transcend 16GB Class 10 UHS-I microSDHC (typical error rate < 0.1%) - Transcend 16GB Class 10 UHS-I microSDHC (typical error rate < 0.1%)
- Sandisk Extreme 16GB Class 10 UHS-I microSDHC (typical error rate < 0.1%) - Sandisk Extreme 16GB Class 10 UHS-I microSDHC (typical error rate < 0.1%)
@ -87,7 +85,7 @@ the best chance of writing at high speed. You must format it with either FAT, or
[SD Association's special formatting tool]: https://www.sdcard.org/downloads/formatter_4/ [SD Association's special formatting tool]: https://www.sdcard.org/downloads/formatter_4/
### OpenLog configuration #### OpenLog configuration
This section applies only if you are using the OpenLog or OpenLog Light original firmware on the OpenLog. If you flashed This section applies only if you are using the OpenLog or OpenLog Light original firmware on the OpenLog. If you flashed
it with the special OpenLog Blackbox firmware, you don't need to configure it further. it with the special OpenLog Blackbox firmware, you don't need to configure it further.
@ -105,42 +103,16 @@ of the MicroSD card:
baud,escape,esc#,mode,verb,echo,ignoreRX baud,escape,esc#,mode,verb,echo,ignoreRX
``` ```
## Enabling this feature (CLI) #### Serial port
Enable the Blackbox feature by typing in `feature BLACKBOX` and pressing enter. You also need to assign the Blackbox to A 115200 baud serial port is required to connect the OpenLog to your flight controller (such as `serial_port_1` on the
one of [your serial ports][] . A 115200 baud port is required (such as serial_port_1 on the Naze32, the two-pin Tx/Rx Naze32, the two-pin Tx/Rx header in the center of the board). The Blackbox can not be used with softserial ports as they
header in the center of the board). are too slow.
For example, use `set serial_port_1_scenario=11` to switch the main serial port to MSP, CLI, Blackbox and GPS Passthrough.
Enter `save`. Your configuration should be saved and the flight controller will reboot. You're ready to go!
[your serial ports]: https://github.com/cleanflight/cleanflight/blob/master/docs/Serial.md
[Cleanflight Configurator]: https://chrome.google.com/webstore/detail/cleanflight-configurator/enacoimjcgeinfnnnpajinjgmkahmfgb?hl=en
## Configuring this feature
The Blackbox currently provides two settings (`blackbox_rate_num` and `blackbox_rate_denom`) that allow you to control
the rate at which data is logged. These two together form a fraction (`blackbox_rate_num / blackbox_rate_denom`) which
decides what portion of the flight controller's control loop iterations should be logged. The default is 1/1 which logs
every iteration.
If you are using a short looptime like 2000 or faster, or you're using a slower MicroSD card, you will need to reduce
this rate to reduce the number of corrupted logged frames. A rate of 1/2 is likely to work for most craft.
You can change these settings by entering the CLI tab in the Cleanflight Configurator and using the `set` command, like so:
```
set blackbox_rate_num = 1
set blackbox_rate_denom = 2
```
### Serial port
Connect the "TX" pin of the serial port you've chosen to the OpenLog's "RXI" pin. Don't connect the serial port's RX Connect the "TX" pin of the serial port you've chosen to the OpenLog's "RXI" pin. Don't connect the serial port's RX
pin to the OpenLog. pin to the OpenLog.
### Protection #### Protection
The OpenLog can be wrapped in black electrical tape or heat-shrink in order to insulate it from conductive frames (like The OpenLog can be wrapped in black electrical tape or heat-shrink in order to insulate it from conductive frames (like
carbon fiber), but this makes its status LEDs impossible to see. I recommend wrapping it with some clear heatshrink carbon fiber), but this makes its status LEDs impossible to see. I recommend wrapping it with some clear heatshrink
@ -148,30 +120,93 @@ tubing instead.
![OpenLog installed](Wiring/blackbox-installation-1.jpg "OpenLog installed with double-sided tape, SDCard slot pointing outward") ![OpenLog installed](Wiring/blackbox-installation-1.jpg "OpenLog installed with double-sided tape, SDCard slot pointing outward")
### Onboard dataflash storage
The full version of the Naze32 has an onboard "m25p16" 2 megayte dataflash storage chip which can be used to store
flight logs instead of using an OpenLog. This is the small chip at the base of the Naze32's direction arrow.
This chip is not present on the "Acro" version of the Naze32.
## Enabling the Blackbox (CLI)
In the [Cleanflight Configurator][] , enter the CLI tab. Enable the Blackbox feature by typing in `feature BLACKBOX` and
pressing enter. Now choose the device that you want to log to:
### OpenLog serial data logger
Enter `set blackbox_device=0` to switch to logging to a serial port (this is the default).
You then need to let Cleanflight know which of [your serial ports][] you connected the OpenLog to. A 115200 baud port
is required (such as `serial_port_1` on the Naze32, the two-pin Tx/Rx header in the center of the board).
For example, use `set serial_port_1_scenario=11` to switch the main serial port to MSP, CLI, Blackbox and GPS
Passthrough. You can also use the GUI to configure a port for the Blackbox feature on the Ports tab.
### Onboard dataflash
Enter `set blackbox_device=1` to switch to logging to an onboard dataflash chip, if your flight controller has one.
[your serial ports]: https://github.com/cleanflight/cleanflight/blob/master/docs/Serial.md
[Cleanflight Configurator]: https://chrome.google.com/webstore/detail/cleanflight-configurator/enacoimjcgeinfnnnpajinjgmkahmfgb?hl=en
## Configuring the Blackbox
The Blackbox currently provides two settings (`blackbox_rate_num` and `blackbox_rate_denom`) that allow you to control
the rate at which data is logged. These two together form a fraction (`blackbox_rate_num / blackbox_rate_denom`) which
decides what portion of the flight controller's control loop iterations should be logged. The default is 1/1 which logs
every iteration.
If you are using a short looptime like 2000 or faster, or you're using a slower MicroSD card, you will need to reduce
this rate to reduce the number of corrupted logged frames that `blackbox_decode` complains about. A rate of 1/2 is likely
to work for most craft.
You can change these settings by entering the CLI tab in the [Cleanflight Configurator][] and using the `set` command, like so:
```
set blackbox_rate_num = 1
set blackbox_rate_denom = 2
```
The data rate for my quadcopter using a looptime of 2400 and a rate of 1/1 is about 10.25kB/s. This allows about 18
days of flight logs to fit on my OpenLog's 16GB MicroSD card, which ought to be enough for anybody :).
If you're logging to an onboard dataflash chip instead of an OpenLog, be aware that the 2MB of storage space it offers
is pretty small. At the default 1/1 logging rate, and a 2400 looptime, this is only enough for about 3 minutes of
flight. This could be long enough for you to investigate some flying problem with your craft, but you may want to reduce
the logging rate in order to extend your recording time.
To maximize your recording time, you could drop the rate way down to 1/32 (the smallest possible rate) which would
result in a logging rate of about 10-20Hz and about 650 bytes/second of data. At that logging rate, the 2MB flash chip
can store around 50 minutes of flight data, though the level of detail is severely reduced and you could not diagnose
flight problems like vibration or PID setting issues.
## Usage ## Usage
The Blackbox starts recording data as soon as you arm your craft, and stops when you disarm. Each time the OpenLog is The Blackbox starts recording data as soon as you arm your craft, and stops when you disarm.
power-cycled, it begins a fresh new log file. If you arm and disarm several times without cycling the power (recording
several flights), those logs will be combined together into one file. The command line tools will ask you to pick which
one of these flights you want to display/decode.
If your craft has a buzzer attached, a short beep will be played when you arm. You can later use this beep to If your craft has a buzzer attached, a short beep will be played when you arm and recording begins. You can later use
synchronize your recorded flight video with the rendered flight data log (the beep is shown as a blue line in the flight this beep to synchronize your recorded flight video with the rendered flight data log (the beep is shown as a blue line
data log, which you can sync against the beep in your recorded audio track). in the flight data log, which you can sync against the beep in your recorded audio track).
The OpenLog requires a couple of seconds of delay after connecting the battery before it's ready to record, so don't You should wait a few seconds after disarming your craft to allow the Blackbox to finish saving its data.
arm your craft immediately after connecting the battery (you'll probably be waiting for the flight controller to become
ready during that time anyway!)
You should also wait a few seconds after disarming the quad to allow the OpenLog to finish saving its data. ### Usage - OpenLog
Each time the OpenLog is power-cycled, it begins a fresh new log file. If you arm and disarm several times without
cycling the power (recording several flights), those logs will be combined together into one file. The command line
tools will ask you to pick which one of these flights you want to display/decode.
Don't insert or remove the SD card while the OpenLog is powered up. Don't insert or remove the SD card while the OpenLog is powered up.
### Usage - Dataflash chip
After your flights, you can use the [Cleanflight Configurator][] to download the contents of the dataflash to your
computer. Go to the "dataflash" tab and click the "save flash to file..." button. Saving the log can take 2 or 3
minutes.
![Dataflash tab in Configurator](Screenshots/blackbox-dataflash.png)
After downloading the log, be sure to erase the chip to make it ready for reuse by clicking the "erase flash" button.
If you try to start recording a new flight when the dataflash is already full, the Blackbox will not make its regular
arming beep and nothing will be recorded.
## Converting logs to CSV or PNG ## Converting logs to CSV or PNG
After your flights, you'll have a series of files labeled "LOG00001.TXT" etc. on the microSD card. You'll need to After your flights, you'll have a series of flight log files with a .TXT extension. You'll need to decode these with
decode these with the `blackbox_decode` tool to create a CSV (comma-separated values) file for analysis, or render them the `blackbox_decode` tool to create CSV (comma-separated values) files for analysis, or render them into a series of PNG
into a series of PNG frames with `blackbox_render` tool, which you could then convert into a video using another frames with `blackbox_render` tool, which you could then convert into a video using another software package.
software package.
You'll find those tools along with instructions for using them in this repository: You'll find those tools along with instructions for using them in this repository:

2
docs/Cli.md
View file

@ -201,7 +201,7 @@ Re-apply any new defaults as desired.
| failsafe_min_usec | | 100 | 2000 | 985 | Profile | UINT16 | | failsafe_min_usec | | 100 | 2000 | 985 | Profile | UINT16 |
| failsafe_max_usec | | 100 | 3000 | 2115 | Profile | UINT16 | | failsafe_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 | | 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 1 for ADXL345, 2 for MPU6050 integrated accelerometer, 3 for MMA8452, 4 for BMA280, or 5 to disable accelerometer alltogether - resulting in gyro-only operation. | 0 | 9 | 0 | Master | 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 | | 0 | 250 | 4 | Profile | UINT8 | | acc_lpf_factor | | 0 | 250 | 4 | Profile | UINT8 |
| accxy_deadband | | 0 | 100 | 40 | Profile | UINT8 | | accxy_deadband | | 0 | 100 | 40 | Profile | UINT8 |
| accz_deadband | | 0 | 100 | 40 | Profile | UINT8 | | accz_deadband | | 0 | 100 | 40 | Profile | UINT8 |

2
docs/Controls.md
View file

@ -28,6 +28,8 @@ HIGH - the channel value for the mapped channel input is around 2000
| Trim Acc Right | HIGH | CENTER | CENTER | HIGH | | Trim Acc Right | HIGH | CENTER | CENTER | HIGH |
| Trim Acc Forwards | HIGH | CENTER | HIGH | CENTER | | Trim Acc Forwards | HIGH | CENTER | HIGH | CENTER |
| Trim Acc Backwards | HIGH | CENTER | LOW | CENTER | | Trim Acc Backwards | HIGH | CENTER | LOW | CENTER |
| Disable LCD Page Cycling | LOW | CENTER | HIGH | LOW |
| Enable LCD Page Cycling | LOW | CENTER | HIGH | HIGH |
| Save setting | LOW | LOW | LOW | HIGH | | Save setting | LOW | LOW | LOW | HIGH |

22
docs/LedStrip.md
View file

@ -23,14 +23,12 @@ Likewise, support for more than 32 LEDs is possible, it just requires additional
## Supported hardware ## Supported hardware
Only strips of 32 WS2812 LEDs are supported currently. If the strip is longer than 32 LEDs it does not matter, Only strips of 32 WS2811/WS2812 LEDs are supported currently. If the strip is longer than 32 LEDs it does not matter,
but only the first 32 are used. but only the first 32 are used.
WS2812 LEDs require an 800khz signal and precise timings and thus requires the use of a dedicated hardware timer. WS2812 LEDs require an 800khz signal and precise timings and thus requires the use of a dedicated hardware timer.
Note: The initial code may work with WS2801 + External LEDs since the protocol is the same, WS2811/WS2812B should also work but Note: Not all WS2812 ICs use the same timings, some batches use different timings.
may require very simple timing adjustments to be made in the source.
Not all WS2812 ICs use the same timings, some batches use different timings.
It could be possible to be able to specify the timings required via CLI if users request it. It could be possible to be able to specify the timings required via CLI if users request it.
@ -63,14 +61,11 @@ Since RC5 is also used for SoftSerial on the Naze/Olimexino it means that you ca
Additionally, since RC5 is also used for Parallel PWM RC input on both the Naze, Chebuzz and STM32F3Discovery targets, led strips Additionally, since RC5 is also used for Parallel PWM RC input on both the Naze, Chebuzz and STM32F3Discovery targets, led strips
can not be used at the same time at Parallel PWM. can not be used at the same time at Parallel PWM.
Ensure that your 5V supply is not too high, if the voltage at the input to the LED strip is to high then the LEDs may not light; The If you have LEDs that are intermittent, flicker or show the wrong colors then drop the VIN to less than 4.7v, e.g. by using an inline
problem occurs because of the difference in voltage between the data signal and the power signal. diode on the VIN to the LED strip. The problem occurs because of the difference in voltage between the data signal and the power
signal. The WS2811 LED's require the data signal (Din) to be MAX 0.7 * VIN. Data In on the LEDs will allways be around ~3.3v,
so the Vin MAX should be 4.7v (3.3v / 0.7 = 4.71v). Some LEDs are more tolerant of this than others.
If you are using an BEC from an ESC to for the 5v supply check the output is as close to 5v as possible.
It was observed that a 5.4v supply from a BEC was fine for powering the FC and other devices but the LED strip would not light until
the voltage was reduced, this was acheived by placing an IN4007 diode between the BEC 5v output and the 5V input pin of the LED strip.
## Configuration ## Configuration
@ -411,7 +406,7 @@ led 27 2,9:S:FWT:0
``` ```
16-18 9-11 16-18 9-11
19-21 \ / 6-8 19-21 \ / 6-8
\ 13-16 / \ 12-15 /
\ FRONT / \ FRONT /
/ BACK \ / BACK \
/ \ / \
@ -433,7 +428,4 @@ This also means that you can make sure that each R,G and B LED in each LED modul
After a short delay the LEDs will show the unarmed color sequence and or low-battery warning sequence. After a short delay the LEDs will show the unarmed color sequence and or low-battery warning sequence.
If the LEDs flash intermittently or do not show the correct colors verify all connections and check the specifications of the
LEDs you have against the supported timings (for now, you'll have to look in the source).
Also check that the feature `LED_STRIP` was correctly enabled and that it does not conflict with other features, as above. Also check that the feature `LED_STRIP` was correctly enabled and that it does not conflict with other features, as above.

12
docs/Mixer.md
View file

@ -45,14 +45,12 @@ You can also use the Command Line Interface (CLI) to set the mixer type:
A low-pass filter can be enabled for the servos. It may be useful for avoiding structural modes in the airframe, for example. Currently it can only be configured via the CLI: A low-pass filter can be enabled for the servos. It may be useful for avoiding structural modes in the airframe, for example. Currently it can only be configured via the CLI:
1. Use `set servo_lowpass_freq_idx = nn` to select the cutoff frequency. Valid values range from 0 to 99. 1. Use `set servo_lowpass_freq = nnn` to select the cutoff frequency. Valid values range from 10 to 400. This is a fraction of the loop frequency in 1/1000ths. For example, `40` means `0.040`.
2. Use `set servo_lowpass_enable = 1` to enable filtering. 2. Use `set servo_lowpass_enable = 1` to enable filtering.
The actual cutoff frequency is determined by the value of the `looptime` variable and the selected index. The cutoff frequency can be determined by the following formula:
The formula is: `Frequency = 1000 * servo_lowpass_freq / looptime`
`Frequency = 1000000 * (servo_lowpass_freq_idx + 1)*0.0025 / looptime )`
For example, if `servo_lowpass_freq` is set to 40, and looptime is set to the default of 3500 us, the cutoff frequency will be 11.43 Hz.
For example, if `servo_lowpass_freq_idx` is set to 40, and looptime is set to the default of 3500 us (0.0035 s), the cutoff frequency will be 29.3 Hz.

BIN
docs/Screenshots/blackbox-dataflash.png Normal file
View file

Binary file not shown.
Side by side

After

Width:  |  Height:  |  Size: 53 KiB

175
src/main/blackbox/blackbox.c
View file

@ -21,9 +21,12 @@
#include "platform.h" #include "platform.h"
#include "version.h" #include "version.h"
#ifdef BLACKBOX
#include "common/maths.h" #include "common/maths.h"
#include "common/axis.h" #include "common/axis.h"
#include "common/color.h" #include "common/color.h"
#include "common/encoding.h"
#include "drivers/gpio.h" #include "drivers/gpio.h"
#include "drivers/sensor.h" #include "drivers/sensor.h"
@ -77,6 +80,7 @@
#include "blackbox_io.h" #include "blackbox_io.h"
#define BLACKBOX_I_INTERVAL 32 #define BLACKBOX_I_INTERVAL 32
#define BLACKBOX_SHUTDOWN_TIMEOUT_MILLIS 200
#define ARRAY_LENGTH(x) (sizeof((x))/sizeof((x)[0])) #define ARRAY_LENGTH(x) (sizeof((x))/sizeof((x)[0]))
@ -130,11 +134,17 @@ static const char* const blackboxGPSHHeaderNames[] = {
/* All field definition structs should look like this (but with longer arrs): */ /* All field definition structs should look like this (but with longer arrs): */
typedef struct blackboxFieldDefinition_t { typedef struct blackboxFieldDefinition_t {
const char *name; const char *name;
// If the field name has a number to be included in square brackets [1] afterwards, set it here, or -1 for no brackets:
int8_t fieldNameIndex;
// Each member of this array will be the value to print for this field for the given header index
uint8_t arr[1]; uint8_t arr[1];
} blackboxFieldDefinition_t; } blackboxFieldDefinition_t;
typedef struct blackboxMainFieldDefinition_t { typedef struct blackboxMainFieldDefinition_t {
const char *name; const char *name;
int8_t fieldNameIndex;
uint8_t isSigned; uint8_t isSigned;
uint8_t Ipredict; uint8_t Ipredict;
uint8_t Iencode; uint8_t Iencode;
@ -145,6 +155,8 @@ typedef struct blackboxMainFieldDefinition_t {
typedef struct blackboxGPSFieldDefinition_t { typedef struct blackboxGPSFieldDefinition_t {
const char *name; const char *name;
int8_t fieldNameIndex;
uint8_t isSigned; uint8_t isSigned;
uint8_t predict; uint8_t predict;
uint8_t encode; uint8_t encode;
@ -159,73 +171,76 @@ typedef struct blackboxGPSFieldDefinition_t {
*/ */
static const blackboxMainFieldDefinition_t blackboxMainFields[] = { static const blackboxMainFieldDefinition_t blackboxMainFields[] = {
/* loopIteration doesn't appear in P frames since it always increments */ /* loopIteration doesn't appear in P frames since it always increments */
{"loopIteration", UNSIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(INC), .Pencode = FLIGHT_LOG_FIELD_ENCODING_NULL, CONDITION(ALWAYS)}, {"loopIteration",-1, UNSIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(INC), .Pencode = FLIGHT_LOG_FIELD_ENCODING_NULL, CONDITION(ALWAYS)},
/* Time advances pretty steadily so the P-frame prediction is a straight line */ /* Time advances pretty steadily so the P-frame prediction is a straight line */
{"time", UNSIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(STRAIGHT_LINE), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)}, {"time", -1, UNSIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(STRAIGHT_LINE), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
{"axisP[0]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)}, {"axisP", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
{"axisP[1]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)}, {"axisP", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
{"axisP[2]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)}, {"axisP", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
/* I terms get special packed encoding in P frames: */ /* I terms get special packed encoding in P frames: */
{"axisI[0]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG2_3S32), CONDITION(ALWAYS)}, {"axisI", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG2_3S32), CONDITION(ALWAYS)},
{"axisI[1]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG2_3S32), CONDITION(ALWAYS)}, {"axisI", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG2_3S32), CONDITION(ALWAYS)},
{"axisI[2]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG2_3S32), CONDITION(ALWAYS)}, {"axisI", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG2_3S32), CONDITION(ALWAYS)},
{"axisD[0]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(NONZERO_PID_D_0)}, {"axisD", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(NONZERO_PID_D_0)},
{"axisD[1]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(NONZERO_PID_D_1)}, {"axisD", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(NONZERO_PID_D_1)},
{"axisD[2]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(NONZERO_PID_D_2)}, {"axisD", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(NONZERO_PID_D_2)},
/* rcCommands are encoded together as a group in P-frames: */ /* rcCommands are encoded together as a group in P-frames: */
{"rcCommand[0]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_4S16), CONDITION(ALWAYS)}, {"rcCommand", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_4S16), CONDITION(ALWAYS)},
{"rcCommand[1]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_4S16), CONDITION(ALWAYS)}, {"rcCommand", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_4S16), CONDITION(ALWAYS)},
{"rcCommand[2]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_4S16), CONDITION(ALWAYS)}, {"rcCommand", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_4S16), CONDITION(ALWAYS)},
/* Throttle is always in the range [minthrottle..maxthrottle]: */ /* Throttle is always in the range [minthrottle..maxthrottle]: */
{"rcCommand[3]", UNSIGNED, .Ipredict = PREDICT(MINTHROTTLE), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_4S16), CONDITION(ALWAYS)}, {"rcCommand", 3, UNSIGNED, .Ipredict = PREDICT(MINTHROTTLE), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_4S16), CONDITION(ALWAYS)},
{"vbatLatest", -1, UNSIGNED, .Ipredict = PREDICT(VBATREF), .Iencode = ENCODING(NEG_14BIT), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_8SVB), FLIGHT_LOG_FIELD_CONDITION_VBAT},
{"amperageLatest",-1, UNSIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_8SVB), FLIGHT_LOG_FIELD_CONDITION_AMPERAGE},
{"vbatLatest", UNSIGNED, .Ipredict = PREDICT(VBATREF), .Iencode = ENCODING(NEG_14BIT), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_8SVB), FLIGHT_LOG_FIELD_CONDITION_VBAT},
{"amperageLatest",UNSIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_8SVB), FLIGHT_LOG_FIELD_CONDITION_AMPERAGE},
#ifdef MAG #ifdef MAG
{"magADC[0]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_8SVB), FLIGHT_LOG_FIELD_CONDITION_MAG}, {"magADC", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_8SVB), FLIGHT_LOG_FIELD_CONDITION_MAG},
{"magADC[1]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_8SVB), FLIGHT_LOG_FIELD_CONDITION_MAG}, {"magADC", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_8SVB), FLIGHT_LOG_FIELD_CONDITION_MAG},
{"magADC[2]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_8SVB), FLIGHT_LOG_FIELD_CONDITION_MAG}, {"magADC", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_8SVB), FLIGHT_LOG_FIELD_CONDITION_MAG},
#endif #endif
#ifdef BARO #ifdef BARO
{"BaroAlt", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_8SVB), FLIGHT_LOG_FIELD_CONDITION_BARO}, {"BaroAlt", -1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_8SVB), FLIGHT_LOG_FIELD_CONDITION_BARO},
#endif #endif
/* Gyros and accelerometers base their P-predictions on the average of the previous 2 frames to reduce noise impact */ /* Gyros and accelerometers base their P-predictions on the average of the previous 2 frames to reduce noise impact */
{"gyroData[0]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)}, {"gyroData", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
{"gyroData[1]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)}, {"gyroData", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
{"gyroData[2]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)}, {"gyroData", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
{"accSmooth[0]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)}, {"accSmooth", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
{"accSmooth[1]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)}, {"accSmooth", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
{"accSmooth[2]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)}, {"accSmooth", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
/* Motors only rarely drops under minthrottle (when stick falls below mincommand), so predict minthrottle for it and use *unsigned* encoding (which is large for negative numbers but more compact for positive ones): */ /* Motors only rarely drops under minthrottle (when stick falls below mincommand), so predict minthrottle for it and use *unsigned* encoding (which is large for negative numbers but more compact for positive ones): */
{"motor[0]", UNSIGNED, .Ipredict = PREDICT(MINTHROTTLE), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_MOTORS_1)}, {"motor", 0, UNSIGNED, .Ipredict = PREDICT(MINTHROTTLE), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_MOTORS_1)},
/* Subsequent motors base their I-frame values on the first one, P-frame values on the average of last two frames: */ /* Subsequent motors base their I-frame values on the first one, P-frame values on the average of last two frames: */
{"motor[1]", UNSIGNED, .Ipredict = PREDICT(MOTOR_0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_MOTORS_2)}, {"motor", 1, UNSIGNED, .Ipredict = PREDICT(MOTOR_0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_MOTORS_2)},
{"motor[2]", UNSIGNED, .Ipredict = PREDICT(MOTOR_0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_MOTORS_3)}, {"motor", 2, UNSIGNED, .Ipredict = PREDICT(MOTOR_0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_MOTORS_3)},
{"motor[3]", UNSIGNED, .Ipredict = PREDICT(MOTOR_0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_MOTORS_4)}, {"motor", 3, UNSIGNED, .Ipredict = PREDICT(MOTOR_0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_MOTORS_4)},
{"motor[4]", UNSIGNED, .Ipredict = PREDICT(MOTOR_0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_MOTORS_5)}, {"motor", 4, UNSIGNED, .Ipredict = PREDICT(MOTOR_0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_MOTORS_5)},
{"motor[5]", UNSIGNED, .Ipredict = PREDICT(MOTOR_0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_MOTORS_6)}, {"motor", 5, UNSIGNED, .Ipredict = PREDICT(MOTOR_0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_MOTORS_6)},
{"motor[6]", UNSIGNED, .Ipredict = PREDICT(MOTOR_0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_MOTORS_7)}, {"motor", 6, UNSIGNED, .Ipredict = PREDICT(MOTOR_0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_MOTORS_7)},
{"motor[7]", UNSIGNED, .Ipredict = PREDICT(MOTOR_0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_MOTORS_8)}, {"motor", 7, UNSIGNED, .Ipredict = PREDICT(MOTOR_0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_MOTORS_8)},
{"servo[5]", UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(TRICOPTER)}
/* Tricopter tail servo */
{"servo", 5, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(TRICOPTER)}
}; };
#ifdef GPS #ifdef GPS
// GPS position/vel frame // GPS position/vel frame
static const blackboxGPSFieldDefinition_t blackboxGpsGFields[] = { static const blackboxGPSFieldDefinition_t blackboxGpsGFields[] = {
{"time", UNSIGNED, PREDICT(LAST_MAIN_FRAME_TIME), ENCODING(UNSIGNED_VB), CONDITION(NOT_LOGGING_EVERY_FRAME)}, {"time", -1, UNSIGNED, PREDICT(LAST_MAIN_FRAME_TIME), ENCODING(UNSIGNED_VB), CONDITION(NOT_LOGGING_EVERY_FRAME)},
{"GPS_numSat", UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB), CONDITION(ALWAYS)}, {"GPS_numSat", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB), CONDITION(ALWAYS)},
{"GPS_coord[0]", SIGNED, PREDICT(HOME_COORD), ENCODING(SIGNED_VB), CONDITION(ALWAYS)}, {"GPS_coord", 0, SIGNED, PREDICT(HOME_COORD), ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
{"GPS_coord[1]", SIGNED, PREDICT(HOME_COORD), ENCODING(SIGNED_VB), CONDITION(ALWAYS)}, {"GPS_coord", 1, SIGNED, PREDICT(HOME_COORD), ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
{"GPS_altitude", UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB), CONDITION(ALWAYS)}, {"GPS_altitude", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB), CONDITION(ALWAYS)},
{"GPS_speed", UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB), CONDITION(ALWAYS)}, {"GPS_speed", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB), CONDITION(ALWAYS)},
{"GPS_ground_course",UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB), CONDITION(ALWAYS)} {"GPS_ground_course", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB), CONDITION(ALWAYS)}
}; };
// GPS home frame // GPS home frame
static const blackboxGPSFieldDefinition_t blackboxGpsHFields[] = { static const blackboxGPSFieldDefinition_t blackboxGpsHFields[] = {
{"GPS_home[0]", SIGNED, PREDICT(0), ENCODING(SIGNED_VB), CONDITION(ALWAYS)}, {"GPS_home", 0, SIGNED, PREDICT(0), ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
{"GPS_home[1]", SIGNED, PREDICT(0), ENCODING(SIGNED_VB), CONDITION(ALWAYS)} {"GPS_home", 1, SIGNED, PREDICT(0), ENCODING(SIGNED_VB), CONDITION(ALWAYS)}
}; };
#endif #endif
@ -282,6 +297,7 @@ static uint32_t blackboxPFrameIndex, blackboxIFrameIndex;
* to encode: * to encode:
*/ */
static uint16_t vbatReference; static uint16_t vbatReference;
static gpsState_t gpsHistory; static gpsState_t gpsHistory;
// Keep a history of length 2, plus a buffer for MW to store the new values into // Keep a history of length 2, plus a buffer for MW to store the new values into
@ -602,13 +618,23 @@ static void validateBlackboxConfig()
masterConfig.blackbox_rate_num = 1; masterConfig.blackbox_rate_num = 1;
masterConfig.blackbox_rate_denom = 1; masterConfig.blackbox_rate_denom = 1;
} else { } else {
/* Reduce the fraction the user entered as much as possible (makes the recorded/skipped frame pattern repeat
* itself more frequently)
*/
div = gcd(masterConfig.blackbox_rate_num, masterConfig.blackbox_rate_denom); div = gcd(masterConfig.blackbox_rate_num, masterConfig.blackbox_rate_denom);
masterConfig.blackbox_rate_num /= div; masterConfig.blackbox_rate_num /= div;
masterConfig.blackbox_rate_denom /= div; masterConfig.blackbox_rate_denom /= div;
} }
if (masterConfig.blackbox_device >= BLACKBOX_DEVICE_END) {
masterConfig.blackbox_device = BLACKBOX_DEVICE_SERIAL;
}
} }
/**
* Start Blackbox logging if it is not already running. Intended to be called upon arming.
*/
void startBlackbox(void) void startBlackbox(void)
{ {
if (blackboxState == BLACKBOX_STATE_STOPPED) { if (blackboxState == BLACKBOX_STATE_STOPPED) {
@ -640,6 +666,9 @@ void startBlackbox(void)
} }
} }
/**
* Begin Blackbox shutdown.
*/
void finishBlackbox(void) void finishBlackbox(void)
{ {
if (blackboxState == BLACKBOX_STATE_RUNNING) { if (blackboxState == BLACKBOX_STATE_RUNNING) {
@ -747,8 +776,10 @@ static void loadBlackboxState(void)
blackboxCurrent->BaroAlt = BaroAlt; blackboxCurrent->BaroAlt = BaroAlt;
#endif #endif
#ifdef USE_SERVOS
//Tail servo for tricopters //Tail servo for tricopters
blackboxCurrent->servo[5] = servo[5]; blackboxCurrent->servo[5] = servo[5];
#endif
} }
/** /**
@ -787,7 +818,8 @@ static bool sendFieldDefinition(const char * const *headerNames, unsigned int he
charsWritten = blackboxPrint("H Field "); charsWritten = blackboxPrint("H Field ");
charsWritten += blackboxPrint(headerNames[xmitState.headerIndex]); charsWritten += blackboxPrint(headerNames[xmitState.headerIndex]);
charsWritten += blackboxPrint(":"); blackboxWrite(':');
charsWritten++;
xmitState.u.fieldIndex++; xmitState.u.fieldIndex++;
needComma = false; needComma = false;
@ -808,6 +840,15 @@ static bool sendFieldDefinition(const char * const *headerNames, unsigned int he
// The first header is a field name // The first header is a field name
if (xmitState.headerIndex == 0) { if (xmitState.headerIndex == 0) {
charsWritten += blackboxPrint(def->name); charsWritten += blackboxPrint(def->name);
// Do we need to print an index in brackets after the name?
if (def->fieldNameIndex != -1) {
blackboxWrite('[');
// Assume the field index is a single digit:
blackboxWrite(def->fieldNameIndex + '0');
blackboxWrite(']');
charsWritten += 3;
}
} else { } else {
//The other headers are integers //The other headers are integers
if (def->arr[xmitState.headerIndex - 1] >= 10) { if (def->arr[xmitState.headerIndex - 1] >= 10) {
@ -838,11 +879,6 @@ static bool sendFieldDefinition(const char * const *headerNames, unsigned int he
*/ */
static bool blackboxWriteSysinfo() static bool blackboxWriteSysinfo()
{ {
union floatConvert_t {
float f;
uint32_t u;
} floatConvert;
if (xmitState.headerIndex == 0) { if (xmitState.headerIndex == 0) {
xmitState.u.serialBudget = 0; xmitState.u.serialBudget = 0;
xmitState.headerIndex = 1; xmitState.headerIndex = 1;
@ -861,26 +897,24 @@ static bool blackboxWriteSysinfo()
//Shouldn't ever get here //Shouldn't ever get here
break; break;
case 1: case 1:
blackboxPrintf("H Firmware type:Cleanflight\n"); xmitState.u.serialBudget -= blackboxPrint("H Firmware type:Cleanflight\n");
xmitState.u.serialBudget -= strlen("H Firmware type:Cleanflight\n");
break; break;
case 2: case 2:
blackboxPrintf("H Firmware revision:%s\n", shortGitRevision); blackboxPrintf("H Firmware revision:%s\n", shortGitRevision);
/* Don't need to be super exact about the budget so don't mind the fact that we're including the length of xmitState.u.serialBudget -= strlen("H Firmware revision:\n") + strlen(shortGitRevision);
* the placeholder "%s"
*/
xmitState.u.serialBudget -= strlen("H Firmware revision:%s\n") + strlen(shortGitRevision);
break; break;
case 3: case 3:
blackboxPrintf("H Firmware date:%s %s\n", buildDate, buildTime); blackboxPrintf("H Firmware date:%s %s\n", buildDate, buildTime);
xmitState.u.serialBudget -= strlen("H Firmware date:%s %s\n") + strlen(buildDate) + strlen(buildTime); xmitState.u.serialBudget -= strlen("H Firmware date: \n") + strlen(buildDate) + strlen(buildTime);
break; break;
case 4: case 4:
blackboxPrintf("H P interval:%d/%d\n", masterConfig.blackbox_rate_num, masterConfig.blackbox_rate_denom); blackboxPrintf("H P interval:%d/%d\n", masterConfig.blackbox_rate_num, masterConfig.blackbox_rate_denom);
/* Don't need to be super exact about the budget so don't mind the fact that we're using the length of
* the placeholder "%d" instead of the actual integer size.
*/
xmitState.u.serialBudget -= strlen("H P interval:%d/%d\n"); xmitState.u.serialBudget -= strlen("H P interval:%d/%d\n");
break; break;
case 5: case 5:
@ -899,10 +933,9 @@ static bool blackboxWriteSysinfo()
xmitState.u.serialBudget -= strlen("H maxthrottle:%d\n"); xmitState.u.serialBudget -= strlen("H maxthrottle:%d\n");
break; break;
case 8: case 8:
floatConvert.f = gyro.scale; blackboxPrintf("H gyro.scale:0x%x\n", castFloatBytesToInt(gyro.scale));
blackboxPrintf("H gyro.scale:0x%x\n", floatConvert.u);
xmitState.u.serialBudget -= strlen("H gyro.scale:0x%x\n") + 6; xmitState.u.serialBudget -= strlen("H gyro.scale:0x\n") + 6;
break; break;
case 9: case 9:
blackboxPrintf("H acc_1G:%u\n", acc_1G); blackboxPrintf("H acc_1G:%u\n", acc_1G);
@ -1002,6 +1035,9 @@ static void blackboxPlaySyncBeep()
blackboxLogEvent(FLIGHT_LOG_EVENT_SYNC_BEEP, (flightLogEventData_t *) &eventData); blackboxLogEvent(FLIGHT_LOG_EVENT_SYNC_BEEP, (flightLogEventData_t *) &eventData);
} }
/**
* Call each flight loop iteration to perform blackbox logging.
*/
void handleBlackbox(void) void handleBlackbox(void)
{ {
int i; int i;
@ -1074,6 +1110,9 @@ void handleBlackbox(void)
loadBlackboxState(); loadBlackboxState();
writeIntraframe(); writeIntraframe();
} else { } else {
/* Adding a magic shift of "masterConfig.blackbox_rate_num - 1" in here creates a better spread of
* recorded / skipped frames when the I frame's position is considered:
*/
if ((blackboxPFrameIndex + masterConfig.blackbox_rate_num - 1) % masterConfig.blackbox_rate_denom < masterConfig.blackbox_rate_num) { if ((blackboxPFrameIndex + masterConfig.blackbox_rate_num - 1) % masterConfig.blackbox_rate_denom < masterConfig.blackbox_rate_num) {
loadBlackboxState(); loadBlackboxState();
writeInterframe(); writeInterframe();
@ -1118,7 +1157,7 @@ void handleBlackbox(void)
* *
* Don't wait longer than it could possibly take if something funky happens. * Don't wait longer than it could possibly take if something funky happens.
*/ */
if (millis() > xmitState.u.startTime + 200 || isBlackboxDeviceIdle()) { if (millis() > xmitState.u.startTime + BLACKBOX_SHUTDOWN_TIMEOUT_MILLIS || blackboxDeviceFlush()) {
blackboxDeviceClose(); blackboxDeviceClose();
blackboxSetState(BLACKBOX_STATE_STOPPED); blackboxSetState(BLACKBOX_STATE_STOPPED);
} }
@ -1126,6 +1165,11 @@ void handleBlackbox(void)
default: default:
break; break;
} }
// Did we run out of room on the device? Stop!
if (isBlackboxDeviceFull()) {
blackboxSetState(BLACKBOX_STATE_STOPPED);
}
} }
static bool canUseBlackboxWithCurrentConfiguration(void) static bool canUseBlackboxWithCurrentConfiguration(void)
@ -1133,6 +1177,9 @@ static bool canUseBlackboxWithCurrentConfiguration(void)
return feature(FEATURE_BLACKBOX); return feature(FEATURE_BLACKBOX);
} }
/**
* Call during system startup to initialize the blackbox.
*/
void initBlackbox(void) void initBlackbox(void)
{ {
if (canUseBlackboxWithCurrentConfiguration()) { if (canUseBlackboxWithCurrentConfiguration()) {
@ -1141,3 +1188,5 @@ void initBlackbox(void)
blackboxSetState(BLACKBOX_STATE_DISABLED); blackboxSetState(BLACKBOX_STATE_DISABLED);
} }
} }
#endif

160
src/main/blackbox/blackbox_io.c
View file

@ -1,14 +1,16 @@
#include <stdlib.h> #include <stdlib.h>
#include <stdarg.h> #include <stdarg.h>
#include <string.h>
#include "blackbox_io.h" #include "blackbox_io.h"
#include "platform.h"
#include "version.h" #include "version.h"
#include "build_config.h"
#include "common/maths.h" #include "common/maths.h"
#include "common/axis.h" #include "common/axis.h"
#include "common/color.h" #include "common/color.h"
#include "common/encoding.h"
#include "drivers/gpio.h" #include "drivers/gpio.h"
#include "drivers/sensor.h" #include "drivers/sensor.h"
@ -56,6 +58,10 @@
#include "config/config_profile.h" #include "config/config_profile.h"
#include "config/config_master.h" #include "config/config_master.h"
#include "io/flashfs.h"
#ifdef BLACKBOX
#define BLACKBOX_BAUDRATE 115200 #define BLACKBOX_BAUDRATE 115200
#define BLACKBOX_INITIAL_PORT_MODE MODE_TX #define BLACKBOX_INITIAL_PORT_MODE MODE_TX
@ -68,7 +74,17 @@ static portSharing_e blackboxPortSharing;
void blackboxWrite(uint8_t value) void blackboxWrite(uint8_t value)
{ {
serialWrite(blackboxPort, value); switch (masterConfig.blackbox_device) {
#ifdef USE_FLASHFS
case BLACKBOX_DEVICE_FLASH:
flashfsWriteByte(value); // Write byte asynchronously
break;
#endif
case BLACKBOX_DEVICE_SERIAL:
default:
serialWrite(blackboxPort, value);
break;
}
} }
static void _putc(void *p, char c) static void _putc(void *p, char c)
@ -89,14 +105,31 @@ void blackboxPrintf(char *fmt, ...)
// Print the null-terminated string 's' to the serial port and return the number of bytes written // Print the null-terminated string 's' to the serial port and return the number of bytes written
int blackboxPrint(const char *s) int blackboxPrint(const char *s)
{ {
const char *pos = s; int length;
const uint8_t *pos;
while (*pos) { switch (masterConfig.blackbox_device) {
blackboxWrite(*pos);
pos++; #ifdef USE_FLASHFS
case BLACKBOX_DEVICE_FLASH:
length = strlen(s);
flashfsWrite((const uint8_t*) s, length, false); // Write asynchronously
break;
#endif
case BLACKBOX_DEVICE_SERIAL:
default:
pos = (uint8_t*) s;
while (*pos) {
serialWrite(blackboxPort, *pos);
pos++;
}
length = pos - (uint8_t*) s;
break;
} }
return pos - s; return length;
} }
/** /**
@ -118,7 +151,7 @@ void blackboxWriteUnsignedVB(uint32_t value)
void blackboxWriteSignedVB(int32_t value) void blackboxWriteSignedVB(int32_t value)
{ {
//ZigZag encode to make the value always positive //ZigZag encode to make the value always positive
blackboxWriteUnsignedVB((uint32_t)((value << 1) ^ (value >> 31))); blackboxWriteUnsignedVB(zigzagEncode(value));
} }
void blackboxWriteS16(int16_t value) void blackboxWriteS16(int16_t value)
@ -375,11 +408,25 @@ void blackboxWriteTag8_8SVB(int32_t *values, int valueCount)
} }
/** /**
* If there is data waiting to be written to the blackbox device, attempt to write that now. * If there is data waiting to be written to the blackbox device, attempt to write (a portion of) that now.
*
* Returns true if all data has been flushed to the device.
*/ */
void blackboxDeviceFlush(void) bool blackboxDeviceFlush(void)
{ {
//Presently a no-op on serial switch (masterConfig.blackbox_device) {
case BLACKBOX_DEVICE_SERIAL:
//Nothing to speed up flushing on serial, as serial is continuously being drained out of its buffer
return isSerialTransmitBufferEmpty(blackboxPort);
#ifdef USE_FLASHFS
case BLACKBOX_DEVICE_FLASH:
return flashfsFlushAsync();
#endif
default:
return false;
}
} }
/** /**
@ -387,18 +434,6 @@ void blackboxDeviceFlush(void)
*/ */
bool blackboxDeviceOpen(void) bool blackboxDeviceOpen(void)
{ {
serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_BLACKBOX);
if (!portConfig) {
return false;
}
blackboxPortSharing = determinePortSharing(portConfig, FUNCTION_BLACKBOX);
blackboxPort = openSerialPort(portConfig->identifier, FUNCTION_BLACKBOX, NULL, BLACKBOX_BAUDRATE, BLACKBOX_INITIAL_PORT_MODE, SERIAL_NOT_INVERTED);
if (!blackboxPort) {
return false;
}
/* /*
* We want to write at about 7200 bytes per second to give the OpenLog a good chance to save to disk. If * We want to write at about 7200 bytes per second to give the OpenLog a good chance to save to disk. If
* about looptime microseconds elapse between our writes, this is the budget of how many bytes we should * about looptime microseconds elapse between our writes, this is the budget of how many bytes we should
@ -408,24 +443,73 @@ bool blackboxDeviceOpen(void)
*/ */
blackboxWriteChunkSize = MAX((masterConfig.looptime * 9) / 1250, 4); blackboxWriteChunkSize = MAX((masterConfig.looptime * 9) / 1250, 4);
return true; switch (masterConfig.blackbox_device) {
} case BLACKBOX_DEVICE_SERIAL:
{
serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_BLACKBOX);
if (!portConfig) {
return false;
}
blackboxPortSharing = determinePortSharing(portConfig, FUNCTION_BLACKBOX);
void blackboxDeviceClose(void) blackboxPort = openSerialPort(portConfig->identifier, FUNCTION_BLACKBOX, NULL, BLACKBOX_BAUDRATE, BLACKBOX_INITIAL_PORT_MODE, SERIAL_NOT_INVERTED);
{ return blackboxPort != NULL;
closeSerialPort(blackboxPort); }
blackboxPort = NULL; break;
#ifdef USE_FLASHFS
/* case BLACKBOX_DEVICE_FLASH:
* Normally this would be handled by mw.c, but since we take an unknown amount if (flashfsGetSize() == 0 || isBlackboxDeviceFull()) {
* of time to shut down asynchronously, we're the only ones that know when to call it. return false;
*/ }
if (blackboxPortSharing == PORTSHARING_SHARED) {
mspAllocateSerialPorts(&masterConfig.serialConfig); return true;
break;
#endif
default:
return false;
} }
} }
bool isBlackboxDeviceIdle(void) /**
* Close the Blackbox logging device immediately without attempting to flush any remaining data.
*/
void blackboxDeviceClose(void)
{ {
return isSerialTransmitBufferEmpty(blackboxPort); switch (masterConfig.blackbox_device) {
case BLACKBOX_DEVICE_SERIAL:
closeSerialPort(blackboxPort);
blackboxPort = NULL;
/*
* Normally this would be handled by mw.c, but since we take an unknown amount
* of time to shut down asynchronously, we're the only ones that know when to call it.
*/
if (blackboxPortSharing == PORTSHARING_SHARED) {
mspAllocateSerialPorts(&masterConfig.serialConfig);
}
break;
#ifdef USE_FLASHFS
case BLACKBOX_DEVICE_FLASH:
// No-op since the flash doesn't have a "close" and there's nobody else to hand control of it to.
break;
#endif
}
} }
bool isBlackboxDeviceFull(void)
{
switch (masterConfig.blackbox_device) {
case BLACKBOX_DEVICE_SERIAL:
return false;
#ifdef USE_FLASHFS
case BLACKBOX_DEVICE_FLASH:
return flashfsIsEOF();
#endif
default:
return false;
}
}
#endif

16
src/main/blackbox/blackbox_io.h
View file

@ -20,6 +20,18 @@
#include <stdint.h> #include <stdint.h>
#include <stdbool.h> #include <stdbool.h>
#include "platform.h"
typedef enum BlackboxDevice {
BLACKBOX_DEVICE_SERIAL = 0,
#ifdef USE_FLASHFS
BLACKBOX_DEVICE_FLASH,
#endif
BLACKBOX_DEVICE_END
} BlackboxDevice;
uint8_t blackboxWriteChunkSize; uint8_t blackboxWriteChunkSize;
void blackboxWrite(uint8_t value); void blackboxWrite(uint8_t value);
@ -34,8 +46,8 @@ void blackboxWriteTag2_3S32(int32_t *values);
void blackboxWriteTag8_4S16(int32_t *values); void blackboxWriteTag8_4S16(int32_t *values);
void blackboxWriteTag8_8SVB(int32_t *values, int valueCount); void blackboxWriteTag8_8SVB(int32_t *values, int valueCount);
void blackboxDeviceFlush(void); bool blackboxDeviceFlush(void);
bool blackboxDeviceOpen(void); bool blackboxDeviceOpen(void);
void blackboxDeviceClose(void); void blackboxDeviceClose(void);
bool isBlackboxDeviceIdle(void); bool isBlackboxDeviceFull(void);

31
src/main/common/encoding.c Normal file
View file

@ -0,0 +1,31 @@
#include "encoding.h"
/**
* Cast the in-memory representation of the given float directly to an int.
*
* This is useful for printing the hex representation of a float number (which is considerably cheaper
* than a full decimal float formatter, in both code size and output length).
*/
uint32_t castFloatBytesToInt(float f)
{
union floatConvert_t {
float f;
uint32_t u;
} floatConvert;
floatConvert.f = f;
return floatConvert.u;
}
/**
* ZigZag encoding maps all values of a signed integer into those of an unsigned integer in such
* a way that numbers of small absolute value correspond to small integers in the result.
*
* (Compared to just casting a signed to an unsigned which creates huge resulting numbers for
* small negative integers).
*/
uint32_t zigzagEncode(int32_t value)
{
return (uint32_t)((value << 1) ^ (value >> 31));
}

23
src/main/common/encoding.h Normal file
View file

@ -0,0 +1,23 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Cleanflight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#include <stdint.h>
uint32_t castFloatBytesToInt(float f);
uint32_t zigzagEncode(int32_t value);

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

@ -74,9 +74,17 @@
#define BRUSHLESS_MOTORS_PWM_RATE 400 #define BRUSHLESS_MOTORS_PWM_RATE 400
void setPIDController(int type); // FIXME PID code needs to be in flight_pid.c/h void setPIDController(int type); // FIXME PID code needs to be in flight_pid.c/h
void mixerUseConfigs(servoParam_t *servoConfToUse, flight3DConfig_t *flight3DConfigToUse, void mixerUseConfigs(
escAndServoConfig_t *escAndServoConfigToUse, mixerConfig_t *mixerConfigToUse, #ifdef USE_SERVOS
airplaneConfig_t *airplaneConfigToUse, rxConfig_t *rxConfig, gimbalConfig_t *gimbalConfigToUse); servoParam_t *servoConfToUse,
gimbalConfig_t *gimbalConfigToUse,
#endif
flight3DConfig_t *flight3DConfigToUse,
escAndServoConfig_t *escAndServoConfigToUse,
mixerConfig_t *mixerConfigToUse,
airplaneConfig_t *airplaneConfigToUse,
rxConfig_t *rxConfig
);
void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, escAndServoConfig_t *escAndServoConfigToUse, pidProfile_t *pidProfileToUse); void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, escAndServoConfig_t *escAndServoConfigToUse, pidProfile_t *pidProfileToUse);
#define FLASH_TO_RESERVE_FOR_CONFIG 0x800 #define FLASH_TO_RESERVE_FOR_CONFIG 0x800
@ -111,7 +119,7 @@ profile_t *currentProfile;
static uint8_t currentControlRateProfileIndex = 0; static uint8_t currentControlRateProfileIndex = 0;
controlRateConfig_t *currentControlRateProfile; controlRateConfig_t *currentControlRateProfile;
static const uint8_t EEPROM_CONF_VERSION = 92; static const uint8_t EEPROM_CONF_VERSION = 93;
static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims) static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims)
{ {
@ -310,7 +318,10 @@ static void setControlRateProfile(uint8_t profileIndex)
static void resetConf(void) static void resetConf(void)
{ {
int i; int i;
int8_t servoRates[8] = { 30, 30, 100, 100, 100, 100, 100, 100 }; #ifdef USE_SERVOS
int8_t servoRates[MAX_SUPPORTED_SERVOS] = { 30, 30, 100, 100, 100, 100, 100, 100 };
;
#endif
// Clear all configuration // Clear all configuration
memset(&masterConfig, 0, sizeof(master_t)); memset(&masterConfig, 0, sizeof(master_t));
@ -425,8 +436,9 @@ static void resetConf(void)
currentProfile->failsafeConfig.failsafe_min_usec = 985; // any of first 4 channels below this value will trigger failsafe 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 currentProfile->failsafeConfig.failsafe_max_usec = 2115; // any of first 4 channels above this value will trigger failsafe
#ifdef USE_SERVOS
// servos // servos
for (i = 0; i < 8; i++) { for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
currentProfile->servoConf[i].min = DEFAULT_SERVO_MIN; currentProfile->servoConf[i].min = DEFAULT_SERVO_MIN;
currentProfile->servoConf[i].max = DEFAULT_SERVO_MAX; currentProfile->servoConf[i].max = DEFAULT_SERVO_MAX;
currentProfile->servoConf[i].middle = DEFAULT_SERVO_MIDDLE; currentProfile->servoConf[i].middle = DEFAULT_SERVO_MIDDLE;
@ -436,9 +448,12 @@ static void resetConf(void)
currentProfile->mixerConfig.yaw_direction = 1; currentProfile->mixerConfig.yaw_direction = 1;
currentProfile->mixerConfig.tri_unarmed_servo = 1; currentProfile->mixerConfig.tri_unarmed_servo = 1;
currentProfile->mixerConfig.servo_lowpass_freq = 400;
currentProfile->mixerConfig.servo_lowpass_enable = 0;
// gimbal // gimbal
currentProfile->gimbalConfig.gimbal_flags = GIMBAL_NORMAL; currentProfile->gimbalConfig.gimbal_flags = GIMBAL_NORMAL;
#endif
#ifdef GPS #ifdef GPS
resetGpsProfile(&currentProfile->gpsProfile); resetGpsProfile(&currentProfile->gpsProfile);
@ -454,6 +469,7 @@ static void resetConf(void)
#endif #endif
#ifdef BLACKBOX #ifdef BLACKBOX
masterConfig.blackbox_device = 0;
masterConfig.blackbox_rate_num = 1; masterConfig.blackbox_rate_num = 1;
masterConfig.blackbox_rate_denom = 1; masterConfig.blackbox_rate_denom = 1;
#endif #endif
@ -595,6 +611,8 @@ void activateConfig(void)
activateControlRateConfig(); activateControlRateConfig();
resetAdjustmentStates();
useRcControlsConfig( useRcControlsConfig(
currentProfile->modeActivationConditions, currentProfile->modeActivationConditions,
&masterConfig.escAndServoConfig, &masterConfig.escAndServoConfig,
@ -618,13 +636,15 @@ void activateConfig(void)
setAccelerationTrims(&masterConfig.accZero); setAccelerationTrims(&masterConfig.accZero);
mixerUseConfigs( mixerUseConfigs(
#ifdef USE_SERVOS
currentProfile->servoConf, currentProfile->servoConf,
&currentProfile->gimbalConfig,
#endif
&masterConfig.flight3DConfig, &masterConfig.flight3DConfig,
&masterConfig.escAndServoConfig, &masterConfig.escAndServoConfig,
&currentProfile->mixerConfig, &currentProfile->mixerConfig,
&masterConfig.airplaneConfig, &masterConfig.airplaneConfig,
&masterConfig.rxConfig, &masterConfig.rxConfig
&currentProfile->gimbalConfig
); );
imuRuntimeConfig.gyro_cmpf_factor = masterConfig.gyro_cmpf_factor; imuRuntimeConfig.gyro_cmpf_factor = masterConfig.gyro_cmpf_factor;

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

@ -88,6 +88,7 @@ typedef struct master_t {
#ifdef BLACKBOX #ifdef BLACKBOX
uint8_t blackbox_rate_num; uint8_t blackbox_rate_num;
uint8_t blackbox_rate_denom; uint8_t blackbox_rate_denom;
uint8_t blackbox_device;
#endif #endif
uint8_t magic_ef; // magic number, should be 0xEF uint8_t magic_ef; // magic number, should be 0xEF

7
src/main/config/config_profile.h
View file

@ -47,8 +47,12 @@ typedef struct profile_s {
uint16_t throttle_correction_angle; // the angle when the throttle correction is maximal. in 0.1 degres, ex 225 = 22.5 ,30.0, 450 = 45.0 deg uint16_t throttle_correction_angle; // the angle when the throttle correction is maximal. in 0.1 degres, ex 225 = 22.5 ,30.0, 450 = 45.0 deg
uint8_t throttle_correction_value; // the correction that will be applied at throttle_correction_angle. uint8_t throttle_correction_value; // the correction that will be applied at throttle_correction_angle.
#ifdef USE_SERVOS
// Servo-related stuff // Servo-related stuff
servoParam_t servoConf[MAX_SUPPORTED_SERVOS]; // servo configuration servoParam_t servoConf[MAX_SUPPORTED_SERVOS]; // servo configuration
// gimbal-related configuration
gimbalConfig_t gimbalConfig;
#endif
// Failsafe related configuration // Failsafe related configuration
failsafeConfig_t failsafeConfig; failsafeConfig_t failsafeConfig;
@ -56,9 +60,6 @@ typedef struct profile_s {
// mixer-related configuration // mixer-related configuration
mixerConfig_t mixerConfig; mixerConfig_t mixerConfig;
// gimbal-related configuration
gimbalConfig_t gimbalConfig;
#ifdef GPS #ifdef GPS
gpsProfile_t gpsProfile; gpsProfile_t gpsProfile;
#endif #endif

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

@ -68,39 +68,12 @@ extern int16_t debug[4];
#define BOOT ((uint8_t)0x80) #define BOOT ((uint8_t)0x80)
#define SPI1_GPIO GPIOA
#define SPI1_SCK_PIN GPIO_Pin_5
#define SPI1_SCK_PIN_SOURCE GPIO_PinSource5
#define SPI1_SCK_CLK RCC_AHBPeriph_GPIOA
#define SPI1_MISO_PIN GPIO_Pin_6
#define SPI1_MISO_PIN_SOURCE GPIO_PinSource6
#define SPI1_MISO_CLK RCC_AHBPeriph_GPIOA
#define SPI1_MOSI_PIN GPIO_Pin_7
#define SPI1_MOSI_PIN_SOURCE GPIO_PinSource7
#define SPI1_MOSI_CLK RCC_AHBPeriph_GPIOA
static void l3gd20SpiInit(SPI_TypeDef *SPIx) static void l3gd20SpiInit(SPI_TypeDef *SPIx)
{ {
GPIO_InitTypeDef GPIO_InitStructure; GPIO_InitTypeDef GPIO_InitStructure;
SPI_InitTypeDef SPI_InitStructure; SPI_InitTypeDef SPI_InitStructure;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_SPI1, ENABLE); RCC_AHBPeriphClockCmd(L3GD20_CS_GPIO_CLK_PERIPHERAL, ENABLE);
RCC_AHBPeriphClockCmd(SPI1_SCK_CLK | SPI1_MISO_CLK | SPI1_MOSI_CLK, ENABLE);
GPIO_PinAFConfig(SPI1_GPIO, SPI1_SCK_PIN_SOURCE, GPIO_AF_5);
GPIO_PinAFConfig(SPI1_GPIO, SPI1_MISO_PIN_SOURCE, GPIO_AF_5);
GPIO_PinAFConfig(SPI1_GPIO, SPI1_MOSI_PIN_SOURCE, GPIO_AF_5);
// Init pins
GPIO_InitStructure.GPIO_Pin = SPI1_SCK_PIN | SPI1_MISO_PIN | SPI1_MOSI_PIN;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
GPIO_Init(SPI1_GPIO, &GPIO_InitStructure);
RCC_AHBPeriphClockCmd(L3GD20_CS_GPIO_CLK, ENABLE);
GPIO_InitStructure.GPIO_Pin = L3GD20_CS_PIN; GPIO_InitStructure.GPIO_Pin = L3GD20_CS_PIN;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT; GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
@ -112,28 +85,11 @@ static void l3gd20SpiInit(SPI_TypeDef *SPIx)
GPIO_SetBits(L3GD20_CS_GPIO, L3GD20_CS_PIN); GPIO_SetBits(L3GD20_CS_GPIO, L3GD20_CS_PIN);
SPI_I2S_DeInit(SPIx); spiSetDivisor(L3GD20_SPI, SPI_9MHZ_CLOCK_DIVIDER);
SPI_InitStructure.SPI_Direction = SPI_Direction_2Lines_FullDuplex;
SPI_InitStructure.SPI_Mode = SPI_Mode_Master;
SPI_InitStructure.SPI_DataSize = SPI_DataSize_8b;
SPI_InitStructure.SPI_CPOL = SPI_CPOL_Low;
SPI_InitStructure.SPI_CPHA = SPI_CPHA_1Edge;
SPI_InitStructure.SPI_NSS = SPI_NSS_Soft;
SPI_InitStructure.SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_4; // 36/4 = 9 MHz SPI Clock
SPI_InitStructure.SPI_FirstBit = SPI_FirstBit_MSB;
SPI_InitStructure.SPI_CRCPolynomial = 7;
SPI_Init(SPIx, &SPI_InitStructure);
SPI_RxFIFOThresholdConfig(SPIx, SPI_RxFIFOThreshold_QF);
SPI_Cmd(SPIx, ENABLE);
} }
void l3gd20GyroInit(void) void l3gd20GyroInit(void)
{ {
l3gd20SpiInit(L3GD20_SPI); l3gd20SpiInit(L3GD20_SPI);
GPIO_ResetBits(L3GD20_CS_GPIO, L3GD20_CS_PIN); GPIO_ResetBits(L3GD20_CS_GPIO, L3GD20_CS_PIN);

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

@ -17,10 +17,4 @@
#pragma once #pragma once
#define L3GD20_SPI SPI1
#define L3GD20_CS_GPIO GPIOE
#define L3GD20_CS_PIN GPIO_Pin_3
#define L3GD20_CS_GPIO_CLK RCC_AHBPeriph_GPIOE
bool l3gd20Detect(gyro_t *gyro, uint16_t lpf); bool l3gd20Detect(gyro_t *gyro, uint16_t lpf);

151
src/main/drivers/bus_spi.c
View file

@ -33,22 +33,56 @@ static volatile uint16_t spi3ErrorCount = 0;
#endif #endif
#ifdef USE_SPI_DEVICE_1 #ifdef USE_SPI_DEVICE_1
#define SPI1_GPIO GPIOA
#define SPI1_GPIO_PERIPHERAL RCC_AHBPeriph_GPIOB
#define SPI1_SCK_PIN GPIO_Pin_5
#define SPI1_SCK_PIN_SOURCE GPIO_PinSource5
#define SPI1_SCK_CLK RCC_AHBPeriph_GPIOA
#define SPI1_MISO_PIN GPIO_Pin_6
#define SPI1_MISO_PIN_SOURCE GPIO_PinSource6
#define SPI1_MISO_CLK RCC_AHBPeriph_GPIOA
#define SPI1_MOSI_PIN GPIO_Pin_7
#define SPI1_MOSI_PIN_SOURCE GPIO_PinSource7
#define SPI1_MOSI_CLK RCC_AHBPeriph_GPIOA
void initSpi1(void) void initSpi1(void)
{ {
// Specific to the STM32F103 // Specific to the STM32F103
// SPI1 Driver // SPI1 Driver
// PA7 17 SPI1_MOSI
// PA6 16 SPI1_MISO
// PA5 15 SPI1_SCK
// PA4 14 SPI1_NSS // PA4 14 SPI1_NSS
// PA5 15 SPI1_SCK
// PA6 16 SPI1_MISO
// PA7 17 SPI1_MOSI
gpio_config_t gpio;
SPI_InitTypeDef spi; SPI_InitTypeDef spi;
// Enable SPI1 clock // Enable SPI1 clock
RCC_APB2PeriphClockCmd(RCC_APB2Periph_SPI1, ENABLE); RCC_APB2PeriphClockCmd(RCC_APB2Periph_SPI1, ENABLE);
RCC_APB2PeriphResetCmd(RCC_APB2Periph_SPI1, ENABLE); RCC_APB2PeriphResetCmd(RCC_APB2Periph_SPI1, ENABLE);
#ifdef STM32F303xC
GPIO_InitTypeDef GPIO_InitStructure;
RCC_AHBPeriphClockCmd(SPI1_GPIO_PERIPHERAL, ENABLE);
GPIO_PinAFConfig(SPI1_GPIO, SPI1_SCK_PIN_SOURCE, GPIO_AF_5);
GPIO_PinAFConfig(SPI1_GPIO, SPI1_MISO_PIN_SOURCE, GPIO_AF_5);
GPIO_PinAFConfig(SPI1_GPIO, SPI1_MOSI_PIN_SOURCE, GPIO_AF_5);
// Init pins
GPIO_InitStructure.GPIO_Pin = SPI1_SCK_PIN | SPI1_MISO_PIN | SPI1_MOSI_PIN;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
GPIO_Init(SPI1_GPIO, &GPIO_InitStructure);
#endif
#ifdef STM32F10X
gpio_config_t gpio;
// MOSI + SCK as output // MOSI + SCK as output
gpio.mode = Mode_AF_PP; gpio.mode = Mode_AF_PP;
gpio.pin = Pin_7 | Pin_5; gpio.pin = Pin_7 | Pin_5;
@ -62,8 +96,9 @@ void initSpi1(void)
gpio.pin = Pin_4; gpio.pin = Pin_4;
gpio.mode = Mode_Out_PP; gpio.mode = Mode_Out_PP;
gpioInit(GPIOA, &gpio); gpioInit(GPIOA, &gpio);
#endif
// Init SPI2 hardware // Init SPI hardware
SPI_I2S_DeInit(SPI1); SPI_I2S_DeInit(SPI1);
spi.SPI_Mode = SPI_Mode_Master; spi.SPI_Mode = SPI_Mode_Master;
@ -76,60 +111,116 @@ void initSpi1(void)
spi.SPI_CPHA = SPI_CPHA_2Edge; spi.SPI_CPHA = SPI_CPHA_2Edge;
spi.SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_8; spi.SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_8;
#ifdef STM32F303xC
// Configure for 8-bit reads.
SPI_RxFIFOThresholdConfig(SPI1, SPI_RxFIFOThreshold_QF);
#endif
SPI_Init(SPI1, &spi); SPI_Init(SPI1, &spi);
SPI_Cmd(SPI1, ENABLE); SPI_Cmd(SPI1, ENABLE);
} }
#endif #endif
#ifdef USE_SPI_DEVICE_2 #ifdef USE_SPI_DEVICE_2
#ifndef SPI2_GPIO
#define SPI2_GPIO GPIOB
#define SPI2_GPIO_PERIPHERAL RCC_AHBPeriph_GPIOB
#define SPI2_NSS_PIN GPIO_Pin_12
#define SPI2_NSS_PIN_SOURCE GPIO_PinSource12
#define SPI2_SCK_PIN GPIO_Pin_13
#define SPI2_SCK_PIN_SOURCE GPIO_PinSource13
#define SPI2_MISO_PIN GPIO_Pin_14
#define SPI2_MISO_PIN_SOURCE GPIO_PinSource14
#define SPI2_MOSI_PIN GPIO_Pin_15
#define SPI2_MOSI_PIN_SOURCE GPIO_PinSource15
#endif
void initSpi2(void) void initSpi2(void)
{ {
// Specific to the STM32F103 / STM32F303 (AF5)
// Specific to the STM32F103
// SPI2 Driver // SPI2 Driver
// PB15 28 SPI2_MOSI
// PB14 27 SPI2_MISO
// PB13 26 SPI2_SCK
// PB12 25 SPI2_NSS // PB12 25 SPI2_NSS
// PB13 26 SPI2_SCK
// PB14 27 SPI2_MISO
// PB15 28 SPI2_MOSI
gpio_config_t gpio;
SPI_InitTypeDef spi; SPI_InitTypeDef spi;
// Enable SPI2 clock // Enable SPI2 clock
RCC_APB1PeriphClockCmd(RCC_APB1Periph_SPI2, ENABLE); RCC_APB1PeriphClockCmd(RCC_APB1Periph_SPI2, ENABLE);
RCC_APB1PeriphResetCmd(RCC_APB1Periph_SPI2, ENABLE); RCC_APB1PeriphResetCmd(RCC_APB1Periph_SPI2, ENABLE);
#ifdef STM32F303xC
GPIO_InitTypeDef GPIO_InitStructure;
RCC_AHBPeriphClockCmd(SPI2_GPIO_PERIPHERAL, ENABLE);
GPIO_PinAFConfig(SPI2_GPIO, SPI2_SCK_PIN_SOURCE, GPIO_AF_5);
GPIO_PinAFConfig(SPI2_GPIO, SPI2_MISO_PIN_SOURCE, GPIO_AF_5);
GPIO_PinAFConfig(SPI2_GPIO, SPI2_MOSI_PIN_SOURCE, GPIO_AF_5);
GPIO_InitStructure.GPIO_Pin = SPI2_SCK_PIN | SPI2_MISO_PIN | SPI2_MOSI_PIN;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
GPIO_Init(SPI2_GPIO, &GPIO_InitStructure);
GPIO_InitStructure.GPIO_Pin = SPI2_NSS_PIN;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
GPIO_Init(SPI2_GPIO, &GPIO_InitStructure);
#endif
#ifdef STM32F10X
gpio_config_t gpio;
// MOSI + SCK as output // MOSI + SCK as output
gpio.mode = Mode_AF_PP; gpio.mode = Mode_AF_PP;
gpio.pin = Pin_13 | Pin_15; gpio.pin = SPI2_SCK_PIN | SPI2_MOSI_PIN;
gpio.speed = Speed_50MHz; gpio.speed = Speed_50MHz;
gpioInit(GPIOB, &gpio); gpioInit(SPI2_GPIO, &gpio);
// MISO as input // MISO as input
gpio.pin = Pin_14; gpio.pin = SPI2_MISO_PIN;
gpio.mode = Mode_IN_FLOATING; gpio.mode = Mode_IN_FLOATING;
gpioInit(GPIOB, &gpio); gpioInit(SPI2_GPIO, &gpio);
// NSS as gpio slave select
gpio.pin = Pin_12;
gpio.mode = Mode_Out_PP;
gpioInit(GPIOB, &gpio);
GPIO_SetBits(GPIOB, GPIO_Pin_12); // NSS as gpio slave select
gpio.pin = SPI2_NSS_PIN;
gpio.mode = Mode_Out_PP;
gpioInit(SPI2_GPIO, &gpio);
#endif
// Init SPI2 hardware // Init SPI2 hardware
SPI_I2S_DeInit(SPI2); SPI_I2S_DeInit(SPI2);
spi.SPI_Mode = SPI_Mode_Master;
spi.SPI_Direction = SPI_Direction_2Lines_FullDuplex; spi.SPI_Direction = SPI_Direction_2Lines_FullDuplex;
spi.SPI_Mode = SPI_Mode_Master;
spi.SPI_DataSize = SPI_DataSize_8b; spi.SPI_DataSize = SPI_DataSize_8b;
spi.SPI_NSS = SPI_NSS_Soft;
spi.SPI_FirstBit = SPI_FirstBit_MSB;
spi.SPI_CRCPolynomial = 7;
spi.SPI_CPOL = SPI_CPOL_High; spi.SPI_CPOL = SPI_CPOL_High;
spi.SPI_CPHA = SPI_CPHA_2Edge; spi.SPI_CPHA = SPI_CPHA_2Edge;
spi.SPI_NSS = SPI_NSS_Soft;
spi.SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_8; spi.SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_8;
spi.SPI_FirstBit = SPI_FirstBit_MSB;
spi.SPI_CRCPolynomial = 7;
#ifdef STM32F303xC
// Configure for 8-bit reads.
SPI_RxFIFOThresholdConfig(SPI2, SPI_RxFIFOThreshold_QF);
#endif
SPI_Init(SPI2, &spi); SPI_Init(SPI2, &spi);
SPI_Cmd(SPI2, ENABLE); SPI_Cmd(SPI2, ENABLE);
// Drive NSS high to disable connected SPI device.
GPIO_SetBits(SPI2_GPIO, SPI2_NSS_PIN);
} }
#endif #endif
@ -198,7 +289,7 @@ uint8_t spiTransferByte(SPI_TypeDef *instance, uint8_t data)
#endif #endif
} }
bool spiTransfer(SPI_TypeDef *instance, uint8_t *out, uint8_t *in, int len) bool spiTransfer(SPI_TypeDef *instance, uint8_t *out, const uint8_t *in, int len)
{ {
uint16_t spiTimeout = 1000; uint16_t spiTimeout = 1000;
@ -210,8 +301,9 @@ bool spiTransfer(SPI_TypeDef *instance, uint8_t *out, uint8_t *in, int len)
if ((spiTimeout--) == 0) if ((spiTimeout--) == 0)
return spiTimeoutUserCallback(instance); return spiTimeoutUserCallback(instance);
} }
#ifdef STM32F303 #ifdef STM32F303xC
SPI_I2S_SendData16(instance, b); SPI_SendData8(instance, b);
//SPI_I2S_SendData16(instance, b);
#endif #endif
#ifdef STM32F10X #ifdef STM32F10X
SPI_I2S_SendData(instance, b); SPI_I2S_SendData(instance, b);
@ -220,8 +312,9 @@ bool spiTransfer(SPI_TypeDef *instance, uint8_t *out, uint8_t *in, int len)
if ((spiTimeout--) == 0) if ((spiTimeout--) == 0)
return spiTimeoutUserCallback(instance); return spiTimeoutUserCallback(instance);
} }
#ifdef STM32F303 #ifdef STM32F303xC
b = SPI_I2S_ReceiveData16(instance); b = SPI_ReceiveData8(instance);
//b = SPI_I2S_ReceiveData16(instance);
#endif #endif
#ifdef STM32F10X #ifdef STM32F10X
b = SPI_I2S_ReceiveData(instance); b = SPI_I2S_ReceiveData(instance);

3
src/main/drivers/bus_spi.h
View file

@ -19,12 +19,13 @@
#define SPI_0_5625MHZ_CLOCK_DIVIDER 128 #define SPI_0_5625MHZ_CLOCK_DIVIDER 128
#define SPI_18MHZ_CLOCK_DIVIDER 2 #define SPI_18MHZ_CLOCK_DIVIDER 2
#define SPI_9MHZ_CLOCK_DIVIDER 4
bool spiInit(SPI_TypeDef *instance); bool spiInit(SPI_TypeDef *instance);
void spiSetDivisor(SPI_TypeDef *instance, uint16_t divisor); void spiSetDivisor(SPI_TypeDef *instance, uint16_t divisor);
uint8_t spiTransferByte(SPI_TypeDef *instance, uint8_t in); uint8_t spiTransferByte(SPI_TypeDef *instance, uint8_t in);
bool spiTransfer(SPI_TypeDef *instance, uint8_t *out, uint8_t *in, int len); bool spiTransfer(SPI_TypeDef *instance, uint8_t *out, const uint8_t *in, int len);
uint16_t spiGetErrorCounter(SPI_TypeDef *instance); uint16_t spiGetErrorCounter(SPI_TypeDef *instance);
void spiResetErrorCounter(SPI_TypeDef *instance); void spiResetErrorCounter(SPI_TypeDef *instance);

31
src/main/drivers/flash.h Normal file
View file

@ -0,0 +1,31 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Cleanflight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#include <stdint.h>
typedef struct flashGeometry_t {
uint8_t sectors;
uint16_t pagesPerSector;
uint16_t pageSize;
uint32_t sectorSize;
uint32_t totalSize;
} flashGeometry_t;

289
src/main/drivers/flash_m25p16.c Normal file
View file

@ -0,0 +1,289 @@
/*
* 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 <stdlib.h>
#include <stdbool.h>
#include <stdint.h>
#include "platform.h"
#include "drivers/flash_m25p16.h"
#include "drivers/bus_spi.h"
#include "drivers/system.h"
#define M25P16_INSTRUCTION_RDID 0x9F
#define M25P16_INSTRUCTION_READ_BYTES 0x03
#define M25P16_INSTRUCTION_READ_STATUS_REG 0x05
#define M25P16_INSTRUCTION_WRITE_STATUS_REG 0x01
#define M25P16_INSTRUCTION_WRITE_ENABLE 0x06
#define M25P16_INSTRUCTION_WRITE_DISABLE 0x04
#define M25P16_INSTRUCTION_PAGE_PROGRAM 0x02
#define M25P16_INSTRUCTION_SECTOR_ERASE 0xD8
#define M25P16_INSTRUCTION_BULK_ERASE 0xC7
#define M25P16_STATUS_FLAG_WRITE_IN_PROGRESS 0x01
#define M25P16_STATUS_FLAG_WRITE_ENABLED 0x02
#define DISABLE_M25P16 GPIO_SetBits(M25P16_CS_GPIO, M25P16_CS_PIN)
#define ENABLE_M25P16 GPIO_ResetBits(M25P16_CS_GPIO, M25P16_CS_PIN)
// The timeout we expect between being able to issue page program instructions
#define DEFAULT_TIMEOUT_MILLIS 6
// These take sooooo long:
#define SECTOR_ERASE_TIMEOUT_MILLIS 5000
#define BULK_ERASE_TIMEOUT_MILLIS 21000
static flashGeometry_t geometry;
/*
* Whether we've performed an action that could have made the device busy for writes.
*
* This allows us to avoid polling for writable status when it is definitely ready already.
*/
static bool couldBeBusy = false;
/**
* Send the given command byte to the device.
*/
static void m25p16_performOneByteCommand(uint8_t command)
{
ENABLE_M25P16;
spiTransferByte(M25P16_SPI_INSTANCE, command);
DISABLE_M25P16;
}
/**
* The flash requires this write enable command to be sent before commands that would cause
* a write like program and erase.
*/
static void m25p16_writeEnable()
{
m25p16_performOneByteCommand(M25P16_INSTRUCTION_WRITE_ENABLE);
// Assume that we're about to do some writing, so the device is just about to become busy
couldBeBusy = true;
}
static uint8_t m25p16_readStatus()
{
uint8_t command[2] = {M25P16_INSTRUCTION_READ_STATUS_REG, 0};
uint8_t in[2];
ENABLE_M25P16;
spiTransfer(M25P16_SPI_INSTANCE, in, command, sizeof(command));
DISABLE_M25P16;
return in[1];
}
bool m25p16_isReady()
{
// If couldBeBusy is false, don't bother to poll the flash chip for its status
couldBeBusy = couldBeBusy && ((m25p16_readStatus() & M25P16_STATUS_FLAG_WRITE_IN_PROGRESS) != 0);
return !couldBeBusy;
}
bool m25p16_waitForReady(uint32_t timeoutMillis)
{
uint32_t time = millis();
while (!m25p16_isReady()) {
if (millis() - time > timeoutMillis) {
return false;
}
}
return true;
}
/**
* Read chip identification and geometry information (into global `geometry`).
*
* Returns true if we get valid ident, false if something bad happened like there is no M25P16.
*/
static bool m25p16_readIdentification()
{
uint8_t out[] = { M25P16_INSTRUCTION_RDID, 0, 0, 0};
uint8_t in[4];
delay(50); // short delay required after initialisation of SPI device instance.
/* Just in case transfer fails and writes nothing, so we don't try to verify the ID against random garbage
* from the stack:
*/
in[1] = 0;
ENABLE_M25P16;
spiTransfer(M25P16_SPI_INSTANCE, in, out, sizeof(out));
// Clearing the CS bit terminates the command early so we don't have to read the chip UID:
DISABLE_M25P16;
// Check manufacturer, memory type, and capacity
if (in[1] == 0x20 && in[2] == 0x20 && in[3] == 0x15) {
// In the future we can support other chip geometries here:
geometry.sectors = 32;
geometry.pagesPerSector = 256;
geometry.pageSize = 256;
geometry.sectorSize = geometry.pagesPerSector * geometry.pageSize;
geometry.totalSize = geometry.sectorSize * geometry.sectors;
couldBeBusy = true; // Just for luck we'll assume the chip could be busy even though it isn't specced to be
return true;
}
geometry.sectors = 0;
geometry.pagesPerSector = 0;
geometry.pageSize = 0;
geometry.sectorSize = 0;
geometry.totalSize = 0;
return false;
}
/**
* Initialize the driver, must be called before any other routines.
*
* Attempts to detect a connected m25p16. If found, true is returned and device capacity can be fetched with
* m25p16_getGeometry().
*/
bool m25p16_init()
{
//Maximum speed for standard READ command is 20mHz, other commands tolerate 25mHz
spiSetDivisor(M25P16_SPI_INSTANCE, SPI_18MHZ_CLOCK_DIVIDER);
return m25p16_readIdentification();
}
/**
* Erase a sector full of bytes to all 1's at the given byte offset in the flash chip.
*/
void m25p16_eraseSector(uint32_t address)
{
uint8_t out[] = { M25P16_INSTRUCTION_SECTOR_ERASE, (address >> 16) & 0xFF, (address >> 8) & 0xFF, address & 0xFF};
m25p16_waitForReady(SECTOR_ERASE_TIMEOUT_MILLIS);
m25p16_writeEnable();
ENABLE_M25P16;
spiTransfer(M25P16_SPI_INSTANCE, NULL, out, sizeof(out));
DISABLE_M25P16;
}
void m25p16_eraseCompletely()
{
m25p16_waitForReady(BULK_ERASE_TIMEOUT_MILLIS);
m25p16_writeEnable();
m25p16_performOneByteCommand(M25P16_INSTRUCTION_BULK_ERASE);
}
void m25p16_pageProgramBegin(uint32_t address)
{
uint8_t command[] = { M25P16_INSTRUCTION_PAGE_PROGRAM, (address >> 16) & 0xFF, (address >> 8) & 0xFF, address & 0xFF};
m25p16_waitForReady(DEFAULT_TIMEOUT_MILLIS);
m25p16_writeEnable();
ENABLE_M25P16;
spiTransfer(M25P16_SPI_INSTANCE, NULL, command, sizeof(command));
}
void m25p16_pageProgramContinue(const uint8_t *data, int length)
{
spiTransfer(M25P16_SPI_INSTANCE, NULL, data, length);
}
void m25p16_pageProgramFinish()
{
DISABLE_M25P16;
}
/**
* Write bytes to a flash page. Address must not cross a page boundary.
*
* Bits can only be set to zero, not from zero back to one again. In order to set bits to 1, use the erase command.
*
* Length must be smaller than the page size.
*
* This will wait for the flash to become ready before writing begins.
*
* Datasheet indicates typical programming time is 0.8ms for 256 bytes, 0.2ms for 64 bytes, 0.05ms for 16 bytes.
* (Although the maximum possible write time is noted as 5ms).
*
* If you want to write multiple buffers (whose sum of sizes is still not more than the page size) then you can
* break this operation up into one beginProgram call, one or more continueProgram calls, and one finishProgram call.
*/
void m25p16_pageProgram(uint32_t address, const uint8_t *data, int length)
{
m25p16_pageProgramBegin(address);
m25p16_pageProgramContinue(data, length);
m25p16_pageProgramFinish();
}
/**
* Read `length` bytes into the provided `buffer` from the flash starting from the given `address` (which need not lie
* on a page boundary).
*
* Waits up to DEFAULT_TIMEOUT_MILLIS milliseconds for the flash to become ready before reading.
*
* The number of bytes actually read is returned, which can be zero if an error or timeout occurred.
*/
int m25p16_readBytes(uint32_t address, uint8_t *buffer, int length)
{
uint8_t command[] = { M25P16_INSTRUCTION_READ_BYTES, (address >> 16) & 0xFF, (address >> 8) & 0xFF, address & 0xFF};
if (!m25p16_waitForReady(DEFAULT_TIMEOUT_MILLIS)) {
return 0;
}
ENABLE_M25P16;
spiTransfer(M25P16_SPI_INSTANCE, NULL, command, sizeof(command));
spiTransfer(M25P16_SPI_INSTANCE, buffer, NULL, length);
DISABLE_M25P16;
return length;
}
/**
* Fetch information about the detected flash chip layout.
*
* Can be called before calling m25p16_init() (the result would have totalSize = 0).
*/
const flashGeometry_t* m25p16_getGeometry()
{
return &geometry;
}

39
src/main/drivers/flash_m25p16.h Normal file
View file

@ -0,0 +1,39 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Cleanflight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#include <stdint.h>
#include "flash.h"
bool m25p16_init();
void m25p16_eraseSector(uint32_t address);
void m25p16_eraseCompletely();
void m25p16_pageProgram(uint32_t address, const uint8_t *data, int length);
void m25p16_pageProgramBegin(uint32_t address);
void m25p16_pageProgramContinue(const uint8_t *data, int length);
void m25p16_pageProgramFinish();
int m25p16_readBytes(uint32_t address, uint8_t *buffer, int length);
bool m25p16_isReady();
bool m25p16_waitForReady(uint32_t timeoutMillis);
const flashGeometry_t* m25p16_getGeometry();

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

@ -419,6 +419,7 @@ pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init)
if (type == MAP_TO_PPM_INPUT && !init->usePPM) if (type == MAP_TO_PPM_INPUT && !init->usePPM)
continue; continue;
#ifdef USE_SERVOS
if (init->useServos && !init->airplane) { if (init->useServos && !init->airplane) {
#if defined(NAZE) #if defined(NAZE)
// remap PWM9+10 as servos // remap PWM9+10 as servos
@ -455,6 +456,7 @@ pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init)
if (timerIndex >= PWM5 && timerIndex <= PWM8) if (timerIndex >= PWM5 && timerIndex <= PWM8)
type = MAP_TO_SERVO_OUTPUT; type = MAP_TO_SERVO_OUTPUT;
} }
#endif
#ifdef CC3D #ifdef CC3D
if (init->useParallelPWM) { if (init->useParallelPWM) {
@ -489,8 +491,10 @@ pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init)
} }
pwmOutputConfiguration.motorCount++; pwmOutputConfiguration.motorCount++;
} else if (type == MAP_TO_SERVO_OUTPUT) { } else if (type == MAP_TO_SERVO_OUTPUT) {
#ifdef USE_SERVOS
pwmServoConfig(timerHardwarePtr, pwmOutputConfiguration.servoCount, init->servoPwmRate, init->servoCenterPulse); pwmServoConfig(timerHardwarePtr, pwmOutputConfiguration.servoCount, init->servoPwmRate, init->servoCenterPulse);
pwmOutputConfiguration.servoCount++; pwmOutputConfiguration.servoCount++;
#endif
} }
} }

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

@ -49,14 +49,16 @@ typedef struct drv_pwm_config_t {
bool useOneshot; bool useOneshot;
bool useSoftSerial; bool useSoftSerial;
bool useLEDStrip; bool useLEDStrip;
#ifdef USE_SERVOS
bool useServos; bool useServos;
bool extraServos; // configure additional 4 channels in PPM mode as servos, not motors bool extraServos; // configure additional 4 channels in PPM mode as servos, not motors
uint16_t servoPwmRate;
uint16_t servoCenterPulse;
#endif
bool airplane; // fixed wing hardware config, lots of servos etc bool airplane; // fixed wing hardware config, lots of servos etc
uint16_t motorPwmRate; uint16_t motorPwmRate;
uint16_t servoPwmRate;
uint16_t idlePulse; // PWM value to use when initializing the driver. set this to either PULSE_1MS (regular pwm), uint16_t idlePulse; // PWM value to use when initializing the driver. set this to either PULSE_1MS (regular pwm),
// some higher value (used by 3d mode), or 0, for brushed pwm drivers. // some higher value (used by 3d mode), or 0, for brushed pwm drivers.
uint16_t servoCenterPulse;
} drv_pwm_config_t; } drv_pwm_config_t;

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

@ -43,8 +43,10 @@ typedef struct {
static pwmOutputPort_t pwmOutputPorts[MAX_PWM_OUTPUT_PORTS]; static pwmOutputPort_t pwmOutputPorts[MAX_PWM_OUTPUT_PORTS];
static pwmOutputPort_t *motors[MAX_PWM_MOTORS]; static pwmOutputPort_t *motors[MAX_PWM_MOTORS];
static pwmOutputPort_t *servos[MAX_PWM_SERVOS];
#ifdef USE_SERVOS
static pwmOutputPort_t *servos[MAX_PWM_SERVOS];
#endif
#define PWM_BRUSHED_TIMER_MHZ 8 #define PWM_BRUSHED_TIMER_MHZ 8
static uint8_t allocatedOutputPortCount = 0; static uint8_t allocatedOutputPortCount = 0;
@ -160,13 +162,6 @@ void pwmCompleteOneshotMotorUpdate(uint8_t motorCount)
} }
} }
void pwmWriteServo(uint8_t index, uint16_t value)
{
if (servos[index] && index < MAX_SERVOS)
*servos[index]->ccr = value;
}
void pwmBrushedMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse) void pwmBrushedMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse)
{ {
uint32_t hz = PWM_BRUSHED_TIMER_MHZ * 1000000; uint32_t hz = PWM_BRUSHED_TIMER_MHZ * 1000000;
@ -187,7 +182,15 @@ void pwmOneshotMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIn
motors[motorIndex]->pwmWritePtr = pwmWriteStandard; motors[motorIndex]->pwmWritePtr = pwmWriteStandard;
} }
#ifdef USE_SERVOS
void pwmServoConfig(const timerHardware_t *timerHardware, uint8_t servoIndex, uint16_t servoPwmRate, uint16_t servoCenterPulse) void pwmServoConfig(const timerHardware_t *timerHardware, uint8_t servoIndex, uint16_t servoPwmRate, uint16_t servoCenterPulse)
{ {
servos[servoIndex] = pwmOutConfig(timerHardware, PWM_TIMER_MHZ, 1000000 / servoPwmRate, servoCenterPulse); servos[servoIndex] = pwmOutConfig(timerHardware, PWM_TIMER_MHZ, 1000000 / servoPwmRate, servoCenterPulse);
} }
void pwmWriteServo(uint8_t index, uint16_t value)
{
if (servos[index] && index < MAX_SERVOS)
*servos[index]->ccr = value;
}
#endif

117
src/main/flight/lowpass.c Executable file
View file

@ -0,0 +1,117 @@
/*
* 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 <stdbool.h>
#include <stdint.h>
#include <stdlib.h>
#include <math.h>
#include "flight/lowpass.h"
//#define DEBUG_LOWPASS
void generateLowpassCoeffs2(int16_t freq, lowpass_t *filter)
{
float fixedScaler;
int i;
// generates coefficients for a 2nd-order butterworth low-pass filter
float freqf = (float)freq*0.001f;
float omega = tanf((float)M_PI*freqf/2.0f);
float scaling = 1.0f / (omega*omega + 1.4142136f*omega + 1.0f);
#if defined(UNIT_TEST) && defined(DEBUG_LOWPASS)
printf("lowpass cutoff: %f, omega: %f\n", freqf, omega);
#endif
filter->bf[0] = scaling * omega*omega;
filter->bf[1] = 2.0f * filter->bf[0];
filter->bf[2] = filter->bf[0];
filter->af[0] = 1.0f;
filter->af[1] = scaling * (2.0f * omega*omega - 2.0f);
filter->af[2] = scaling * (omega*omega - 1.4142136f * omega + 1.0f);
// Scale for fixed-point
filter->input_bias = 1500; // Typical servo range is 1500 +/- 500
filter->input_shift = 16;
filter->coeff_shift = 24;
fixedScaler = (float)(1ULL << filter->coeff_shift);
for (i = 0; i < LOWPASS_NUM_COEF; i++) {
filter->a[i] = LPF_ROUND(filter->af[i] * fixedScaler);
filter->b[i] = LPF_ROUND(filter->bf[i] * fixedScaler);
#if defined(UNIT_TEST) && defined(DEBUG_LOWPASS)
printf("(%d) bf: %f af: %f b: %ld a: %ld\n", i,
filter->bf[i], filter->af[i], filter->b[i], filter->a[i]);
#endif
}
filter->freq = freq;
}
int32_t lowpassFixed(lowpass_t *filter, int32_t in, int16_t freq)
{
int16_t coefIdx;
int64_t out;
int32_t in_s;
// Check to see if cutoff frequency changed
if (freq != filter->freq) {
filter->init = false;
}
// Initialize if needed
if (!filter->init) {
generateLowpassCoeffs2(freq, filter);
for (coefIdx = 0; coefIdx < LOWPASS_NUM_COEF; coefIdx++) {
filter->x[coefIdx] = (in - filter->input_bias) << filter->input_shift;
filter->y[coefIdx] = (in - filter->input_bias) << filter->input_shift;
}
filter->init = true;
}
// Unbias input and scale
in_s = (in - filter->input_bias) << filter->input_shift;
// Delays
for (coefIdx = LOWPASS_NUM_COEF-1; coefIdx > 0; coefIdx--) {
filter->x[coefIdx] = filter->x[coefIdx-1];
filter->y[coefIdx] = filter->y[coefIdx-1];
}
filter->x[0] = in_s;
// Accumulate result
out = filter->x[0] * filter->b[0];
for (coefIdx = 1; coefIdx < LOWPASS_NUM_COEF; coefIdx++) {
out -= filter->y[coefIdx] * filter->a[coefIdx];
out += filter->x[coefIdx] * filter->b[coefIdx];
}
// Scale output by coefficient shift
out >>= filter->coeff_shift;
filter->y[0] = (int32_t)out;
// Scale output by input shift and round
out = (out + (1 << (filter->input_shift-1))) >> filter->input_shift;
// Reapply bias
out += filter->input_bias;
return (int32_t)out;
}

41
src/main/flight/lowpass.h Normal file
View file

@ -0,0 +1,41 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Cleanflight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#define LOWPASS_NUM_COEF 3
#define LPF_ROUND(x) (x < 0 ? (x - 0.5f) : (x + 0.5f))
typedef struct lowpass_t {
bool init;
int16_t freq; // Normalized freq in 1/1000ths
float bf[LOWPASS_NUM_COEF];
float af[LOWPASS_NUM_COEF];
int64_t b[LOWPASS_NUM_COEF];
int64_t a[LOWPASS_NUM_COEF];
int16_t coeff_shift;
int16_t input_shift;
int32_t input_bias;
float xf[LOWPASS_NUM_COEF];
float yf[LOWPASS_NUM_COEF];
int32_t x[LOWPASS_NUM_COEF];
int32_t y[LOWPASS_NUM_COEF];
} lowpass_t;
void generateLowpassCoeffs2(int16_t freq, lowpass_t *filter);
int32_t lowpassFixed(lowpass_t *filter, int32_t in, int16_t freq);

154
src/main/flight/mixer.c Normal file → Executable file
View file

@ -26,6 +26,7 @@
#include "common/axis.h" #include "common/axis.h"
#include "common/maths.h" #include "common/maths.h"
#include "drivers/system.h"
#include "drivers/gpio.h" #include "drivers/gpio.h"
#include "drivers/timer.h" #include "drivers/timer.h"
#include "drivers/pwm_output.h" #include "drivers/pwm_output.h"
@ -45,6 +46,7 @@
#include "flight/mixer.h" #include "flight/mixer.h"
#include "flight/pid.h" #include "flight/pid.h"
#include "flight/imu.h" #include "flight/imu.h"
#include "flight/lowpass.h"
#include "config/runtime_config.h" #include "config/runtime_config.h"
#include "config/config.h" #include "config/config.h"
@ -54,26 +56,32 @@
#define AUX_FORWARD_CHANNEL_TO_SERVO_COUNT 4 #define AUX_FORWARD_CHANNEL_TO_SERVO_COUNT 4
//#define MIXER_DEBUG
extern int16_t debug[4];
uint8_t motorCount = 0; uint8_t motorCount = 0;
int16_t motor[MAX_SUPPORTED_MOTORS]; int16_t motor[MAX_SUPPORTED_MOTORS];
int16_t motor_disarmed[MAX_SUPPORTED_MOTORS]; int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
int16_t servo[MAX_SUPPORTED_SERVOS] = { 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500 };
static int useServo;
static uint8_t servoCount;
static servoParam_t *servoConf;
static mixerConfig_t *mixerConfig; static mixerConfig_t *mixerConfig;
static flight3DConfig_t *flight3DConfig; static flight3DConfig_t *flight3DConfig;
static escAndServoConfig_t *escAndServoConfig; static escAndServoConfig_t *escAndServoConfig;
static airplaneConfig_t *airplaneConfig; static airplaneConfig_t *airplaneConfig;
static rxConfig_t *rxConfig; static rxConfig_t *rxConfig;
static gimbalConfig_t *gimbalConfig;
static motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS]; static motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS];
static mixerMode_e currentMixerMode; static mixerMode_e currentMixerMode;
#ifdef USE_SERVOS
static gimbalConfig_t *gimbalConfig;
int16_t servo[MAX_SUPPORTED_SERVOS];
static int useServo;
static uint8_t servoCount;
static servoParam_t *servoConf;
static lowpass_t lowpassFilters[MAX_SUPPORTED_SERVOS];
#endif
static const motorMixer_t mixerQuadX[] = { static const motorMixer_t mixerQuadX[] = {
{ 1.0f, -1.0f, 1.0f, -1.0f }, // REAR_R { 1.0f, -1.0f, 1.0f, -1.0f }, // REAR_R
{ 1.0f, -1.0f, -1.0f, 1.0f }, // FRONT_R { 1.0f, -1.0f, -1.0f, 1.0f }, // FRONT_R
@ -224,17 +232,31 @@ const mixer_t mixers[] = {
}; };
#endif #endif
void mixerUseConfigs(servoParam_t *servoConfToUse, flight3DConfig_t *flight3DConfigToUse, escAndServoConfig_t *escAndServoConfigToUse, mixerConfig_t *mixerConfigToUse, airplaneConfig_t *airplaneConfigToUse, rxConfig_t *rxConfigToUse, gimbalConfig_t *gimbalConfigToUse) static motorMixer_t *customMixers;
void mixerUseConfigs(
#ifdef USE_SERVOS
servoParam_t *servoConfToUse,
gimbalConfig_t *gimbalConfigToUse,
#endif
flight3DConfig_t *flight3DConfigToUse,
escAndServoConfig_t *escAndServoConfigToUse,
mixerConfig_t *mixerConfigToUse,
airplaneConfig_t *airplaneConfigToUse,
rxConfig_t *rxConfigToUse)
{ {
#ifdef USE_SERVOS
servoConf = servoConfToUse; servoConf = servoConfToUse;
gimbalConfig = gimbalConfigToUse;
#endif
flight3DConfig = flight3DConfigToUse; flight3DConfig = flight3DConfigToUse;
escAndServoConfig = escAndServoConfigToUse; escAndServoConfig = escAndServoConfigToUse;
mixerConfig = mixerConfigToUse; mixerConfig = mixerConfigToUse;
airplaneConfig = airplaneConfigToUse; airplaneConfig = airplaneConfigToUse;
rxConfig = rxConfigToUse; rxConfig = rxConfigToUse;
gimbalConfig = gimbalConfigToUse;
} }
#ifdef USE_SERVOS
int16_t determineServoMiddleOrForwardFromChannel(int nr) int16_t determineServoMiddleOrForwardFromChannel(int nr)
{ {
uint8_t channelToForwardFrom = servoConf[nr].forwardFromChannel; uint8_t channelToForwardFrom = servoConf[nr].forwardFromChannel;
@ -263,9 +285,7 @@ int servoDirection(int nr, int lr)
else else
return 1; return 1;
} }
#endif
static motorMixer_t *customMixers;
#ifndef USE_QUAD_MIXER_ONLY #ifndef USE_QUAD_MIXER_ONLY
void mixerInit(mixerMode_e mixerMode, motorMixer_t *initialCustomMixers) void mixerInit(mixerMode_e mixerMode, motorMixer_t *initialCustomMixers)
@ -279,6 +299,11 @@ void mixerInit(mixerMode_e mixerMode, motorMixer_t *initialCustomMixers)
// if we want camstab/trig, that also enables servos, even if mixer doesn't // if we want camstab/trig, that also enables servos, even if mixer doesn't
if (feature(FEATURE_SERVO_TILT)) if (feature(FEATURE_SERVO_TILT))
useServo = 1; useServo = 1;
// give all servos a default command
for (uint8_t i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
servo[i] = DEFAULT_SERVO_MIDDLE;
}
} }
void mixerUsePWMOutputConfiguration(pwmOutputConfiguration_t *pwmOutputConfiguration) void mixerUsePWMOutputConfiguration(pwmOutputConfiguration_t *pwmOutputConfiguration)
@ -350,15 +375,15 @@ void mixerInit(mixerMode_e mixerMode, motorMixer_t *initialCustomMixers)
currentMixerMode = mixerMode; currentMixerMode = mixerMode;
customMixers = initialCustomMixers; customMixers = initialCustomMixers;
useServo = false;
} }
void mixerUsePWMOutputConfiguration(pwmOutputConfiguration_t *pwmOutputConfiguration) void mixerUsePWMOutputConfiguration(pwmOutputConfiguration_t *pwmOutputConfiguration)
{ {
UNUSED(pwmOutputConfiguration); UNUSED(pwmOutputConfiguration);
motorCount = 4; motorCount = 4;
#ifdef USE_SERVOS
servoCount = 0; servoCount = 0;
#endif
uint8_t i; uint8_t i;
for (i = 0; i < motorCount; i++) { for (i = 0; i < motorCount; i++) {
@ -377,6 +402,7 @@ void mixerResetMotors(void)
motor_disarmed[i] = feature(FEATURE_3D) ? flight3DConfig->neutral3d : escAndServoConfig->mincommand; motor_disarmed[i] = feature(FEATURE_3D) ? flight3DConfig->neutral3d : escAndServoConfig->mincommand;
} }
#ifdef USE_SERVOS
static void updateGimbalServos(void) static void updateGimbalServos(void)
{ {
pwmWriteServo(0, servo[0]); pwmWriteServo(0, servo[0]);
@ -437,6 +463,7 @@ void writeServos(void)
break; break;
} }
} }
#endif
void writeMotors(void) void writeMotors(void)
{ {
@ -461,6 +488,13 @@ void writeAllMotors(int16_t mc)
writeMotors(); writeMotors();
} }
void stopMotors(void)
{
writeAllMotors(escAndServoConfig->mincommand);
delay(50); // give the timers and ESCs a chance to react.
}
#ifndef USE_QUAD_MIXER_ONLY #ifndef USE_QUAD_MIXER_ONLY
static void airplaneMixer(void) static void airplaneMixer(void)
{ {
@ -511,7 +545,6 @@ static void airplaneMixer(void)
void mixTable(void) void mixTable(void)
{ {
int16_t maxMotor;
uint32_t i; uint32_t i;
if (motorCount > 3) { if (motorCount > 3) {
@ -520,15 +553,17 @@ void mixTable(void)
} }
// motors for non-servo mixes // motors for non-servo mixes
if (motorCount > 1) if (motorCount > 1) {
for (i = 0; i < motorCount; i++) for (i = 0; i < motorCount; i++) {
motor[i] = motor[i] =
rcCommand[THROTTLE] * currentMixer[i].throttle + rcCommand[THROTTLE] * currentMixer[i].throttle +
axisPID[PITCH] * currentMixer[i].pitch + axisPID[PITCH] * currentMixer[i].pitch +
axisPID[ROLL] * currentMixer[i].roll + axisPID[ROLL] * currentMixer[i].roll +
-mixerConfig->yaw_direction * axisPID[YAW] * currentMixer[i].yaw; -mixerConfig->yaw_direction * axisPID[YAW] * currentMixer[i].yaw;
}
}
#ifndef USE_QUAD_MIXER_ONLY #if !defined(USE_QUAD_MIXER_ONLY) || defined(USE_SERVOS)
// airplane / servo mixes // airplane / servo mixes
switch (currentMixerMode) { switch (currentMixerMode) {
case MIXER_BI: case MIXER_BI:
@ -586,7 +621,6 @@ void mixTable(void)
default: default:
break; break;
} }
#endif
// do camstab // do camstab
if (feature(FEATURE_SERVO_TILT)) { if (feature(FEATURE_SERVO_TILT)) {
@ -626,37 +660,75 @@ void mixTable(void)
pwmWriteServo(firstServo + servoOffset, rcData[channelOffset++]); pwmWriteServo(firstServo + servoOffset, rcData[channelOffset++]);
} }
} }
#endif
maxMotor = motor[0]; if (ARMING_FLAG(ARMED)) {
for (i = 1; i < motorCount; i++)
if (motor[i] > maxMotor) int16_t maxMotor = motor[0];
maxMotor = motor[i]; for (i = 1; i < motorCount; i++) {
for (i = 0; i < motorCount; i++) { if (motor[i] > maxMotor) {
if (maxMotor > escAndServoConfig->maxthrottle) // this is a way to still have good gyro corrections if at least one motor reaches its max. maxMotor = motor[i];
motor[i] -= maxMotor - escAndServoConfig->maxthrottle;
if (feature(FEATURE_3D)) {
if ((rcData[THROTTLE]) > rxConfig->midrc) {
motor[i] = constrain(motor[i], flight3DConfig->deadband3d_high, escAndServoConfig->maxthrottle);
} else {
motor[i] = constrain(motor[i], escAndServoConfig->mincommand, flight3DConfig->deadband3d_low);
}
} else {
motor[i] = constrain(motor[i], escAndServoConfig->minthrottle, escAndServoConfig->maxthrottle);
if ((rcData[THROTTLE]) < rxConfig->mincheck) {
if (!feature(FEATURE_MOTOR_STOP))
motor[i] = escAndServoConfig->minthrottle;
else
motor[i] = escAndServoConfig->mincommand;
} }
} }
if (!ARMING_FLAG(ARMED)) {
for (i = 0; i < motorCount; i++) {
if (maxMotor > escAndServoConfig->maxthrottle) {
// this is a way to still have good gyro corrections if at least one motor reaches its max.
motor[i] -= maxMotor - escAndServoConfig->maxthrottle;
}
if (feature(FEATURE_3D)) {
if ((rcData[THROTTLE]) > rxConfig->midrc) {
motor[i] = constrain(motor[i], flight3DConfig->deadband3d_high, escAndServoConfig->maxthrottle);
} else {
motor[i] = constrain(motor[i], escAndServoConfig->mincommand, flight3DConfig->deadband3d_low);
}
} else {
motor[i] = constrain(motor[i], escAndServoConfig->minthrottle, escAndServoConfig->maxthrottle);
if ((rcData[THROTTLE]) < rxConfig->mincheck) {
if (!feature(FEATURE_MOTOR_STOP))
motor[i] = escAndServoConfig->minthrottle;
else
motor[i] = escAndServoConfig->mincommand;
}
}
}
} else {
for (i = 0; i < motorCount; i++) {
motor[i] = motor_disarmed[i]; motor[i] = motor_disarmed[i];
} }
} }
} }
#ifdef USE_SERVOS
bool isMixerUsingServos(void) bool isMixerUsingServos(void)
{ {
return useServo; return useServo;
} }
#endif
void filterServos(void)
{
#ifdef USE_SERVOS
int16_t servoIdx;
#if defined(MIXER_DEBUG)
uint32_t startTime = micros();
#endif
if (mixerConfig->servo_lowpass_enable) {
for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) {
servo[servoIdx] = (int16_t)lowpassFixed(&lowpassFilters[servoIdx], servo[servoIdx], mixerConfig->servo_lowpass_freq);
// Sanity check
servo[servoIdx] = constrain(servo[servoIdx], servoConf[servoIdx].min, servoConf[servoIdx].max);
}
}
#if defined(MIXER_DEBUG)
debug[0] = (int16_t)(micros() - startTime);
#endif
#endif
}

15
src/main/flight/mixer.h
View file

@ -65,7 +65,11 @@ typedef struct mixer_t {
typedef struct mixerConfig_s { typedef struct mixerConfig_s {
int8_t yaw_direction; int8_t yaw_direction;
#ifdef USE_SERVOS
uint8_t tri_unarmed_servo; // send tail servo correction pulses even when unarmed uint8_t tri_unarmed_servo; // send tail servo correction pulses even when unarmed
int16_t servo_lowpass_freq; // lowpass servo filter frequency selection; 1/1000ths of loop freq
int8_t servo_lowpass_enable; // enable/disable lowpass filter
#endif
} mixerConfig_t; } mixerConfig_t;
typedef struct flight3DConfig_s { typedef struct flight3DConfig_s {
@ -82,6 +86,7 @@ typedef struct airplaneConfig_t {
#define CHANNEL_FORWARDING_DISABLED (uint8_t)0xFF #define CHANNEL_FORWARDING_DISABLED (uint8_t)0xFF
#ifdef USE_SERVOS
typedef struct servoParam_t { typedef struct servoParam_t {
int16_t min; // servo min int16_t min; // servo min
int16_t max; // servo max int16_t max; // servo max
@ -90,14 +95,18 @@ typedef struct servoParam_t {
int8_t forwardFromChannel; // RX channel index, 0 based. See CHANNEL_FORWARDING_DISABLED int8_t forwardFromChannel; // RX channel index, 0 based. See CHANNEL_FORWARDING_DISABLED
} servoParam_t; } servoParam_t;
extern int16_t servo[MAX_SUPPORTED_SERVOS];
bool isMixerUsingServos(void);
void writeServos(void);
void filterServos(void);
#endif
extern int16_t motor[MAX_SUPPORTED_MOTORS]; extern int16_t motor[MAX_SUPPORTED_MOTORS];
extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS]; extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
extern int16_t servo[MAX_SUPPORTED_SERVOS];
bool isMixerUsingServos(void);
void writeAllMotors(int16_t mc); void writeAllMotors(int16_t mc);
void mixerLoadMix(int index, motorMixer_t *customMixers); void mixerLoadMix(int index, motorMixer_t *customMixers);
void mixerResetMotors(void); void mixerResetMotors(void);
void mixTable(void); void mixTable(void);
void writeServos(void);
void writeMotors(void); void writeMotors(void);
void stopMotors(void);

6
src/main/flight/pid.c
View file

@ -539,8 +539,8 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
} }
for (axis = 0; axis < 2; axis++) { for (axis = 0; axis < 2; axis++) {
int32_t tmp = (int32_t)((float)gyroData[axis] * 0.3125f); // Multiwii masks out the last 2 bits, this has the same idea int32_t tmp = (int32_t)((float)gyroData[axis] * 0.3125f); // Multiwii masks out the last 2 bits, this has the same idea
gyroDataQuant = (float)tmp * 3.2f; // but delivers more accuracy and also reduces jittery flight gyroDataQuant = (float)tmp * 3.2f; // but delivers more accuracy and also reduces jittery flight
rcCommandAxis = (float)rcCommand[axis]; // Calculate common values for pid controllers rcCommandAxis = (float)rcCommand[axis]; // Calculate common values for pid controllers
if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) { if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) {
#ifdef GPS #ifdef GPS
@ -557,7 +557,7 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
PTermACC = error * (float)pidProfile->P8[PIDLEVEL] * 0.008f; PTermACC = error * (float)pidProfile->P8[PIDLEVEL] * 0.008f;
float limitf = (float)pidProfile->D8[PIDLEVEL] * 5.0f; float limitf = (float)pidProfile->D8[PIDLEVEL] * 5.0f;
PTermACC = constrain(PTermACC, -limitf, +limitf); PTermACC = constrain(PTermACC, -limitf, +limitf);
errorAngleIf[axis] = constrain(errorAngleIf[axis] + error * ACCDeltaTimeINS, -30.0f, +30.0f); errorAngleIf[axis] = constrainf(errorAngleIf[axis] + error * ACCDeltaTimeINS, -30.0f, +30.0f);
ITermACC = errorAngleIf[axis] * (float)pidProfile->I8[PIDLEVEL] * 0.08f; ITermACC = errorAngleIf[axis] * (float)pidProfile->I8[PIDLEVEL] * 0.08f;
} }

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

@ -578,7 +578,12 @@ void displaySetNextPageChangeAt(uint32_t futureMicros)
void displayEnablePageCycling(void) void displayEnablePageCycling(void)
{ {
pageState.pageFlags |= PAGE_STATE_FLAG_CYCLE_ENABLED; pageState.pageFlags |= PAGE_STATE_FLAG_CYCLE_ENABLED;
}
void displayResetPageCycling(void)
{
pageState.cycleIndex = CYCLE_PAGE_ID_COUNT - 1; // start at first page pageState.cycleIndex = CYCLE_PAGE_ID_COUNT - 1; // start at first page
} }
void displayDisablePageCycling(void) void displayDisablePageCycling(void)

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

@ -35,4 +35,5 @@ void displayShowFixedPage(pageId_e pageId);
void displayEnablePageCycling(void); void displayEnablePageCycling(void);
void displayDisablePageCycling(void); void displayDisablePageCycling(void);
void displayResetPageCycling(void);
void displaySetNextPageChangeAt(uint32_t futureMicros); void displaySetNextPageChangeAt(uint32_t futureMicros);

563
src/main/io/flashfs.c Normal file
View file

@ -0,0 +1,563 @@
/*
* 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/>.
*/
/**
* This provides a stream interface to a flash chip if one is present.
*
* On statup, call flashfsInit() after initialising the flash chip in order to init the filesystem. This will
* result in the file pointer being pointed at the first free block found, or at the end of the device if the
* flash chip is full.
*
* Note that bits can only be set to 0 when writing, not back to 1 from 0. You must erase sectors in order
* to bring bits back to 1 again.
*
* In future, we can add support for multiple different flash chips by adding a flash device driver vtable
* and make calls through that, at the moment flashfs just calls m25p16_* routines explicitly.
*/
#include <stdint.h>
#include <stdbool.h>
#include <string.h>
#include "drivers/flash_m25p16.h"
#include "flashfs.h"
static uint8_t flashWriteBuffer[FLASHFS_WRITE_BUFFER_SIZE];
/* The position of our head and tail in the circular flash write buffer.
*
* The head is the index that a byte would be inserted into on writing, while the tail is the index of the
* oldest byte that has yet to be written to flash.
*
* When the circular buffer is empty, head == tail
*/
static uint8_t bufferHead = 0, bufferTail = 0;
// The position of the buffer's tail in the overall flash address space:
static uint32_t tailAddress = 0;
// The index of the tail within the flash page it is inside
static uint16_t tailIndexInPage = 0;
static bool shouldFlush = false;
static void flashfsClearBuffer()
{
bufferTail = bufferHead = 0;
shouldFlush = false;
}
static bool flashfsBufferIsEmpty()
{
return bufferTail == bufferHead;
}
static void flashfsSetTailAddress(uint32_t address)
{
tailAddress = address;
if (m25p16_getGeometry()->pageSize > 0) {
tailIndexInPage = tailAddress % m25p16_getGeometry()->pageSize;
}
}
void flashfsEraseCompletely()
{
m25p16_eraseCompletely();
flashfsClearBuffer();
flashfsSetTailAddress(0);
}
/**
* Start and end must lie on sector boundaries, or they will be rounded out to sector boundaries such that
* all the bytes in the range [start...end) are erased.
*/
void flashfsEraseRange(uint32_t start, uint32_t end)
{
const flashGeometry_t *geometry = m25p16_getGeometry();
if (geometry->sectorSize <= 0)
return;
// Round the start down to a sector boundary
int startSector = start / geometry->sectorSize;
// And the end upward
int endSector = end / geometry->sectorSize;
int endRemainder = end % geometry->sectorSize;
if (endRemainder > 0) {
endSector++;
}
for (int i = startSector; i < endSector; i++) {
m25p16_eraseSector(i * geometry->sectorSize);
}
}
/**
* Return true if the flash is not currently occupied with an operation.
*/
bool flashfsIsReady()
{
return m25p16_isReady();
}
uint32_t flashfsGetSize()
{
return m25p16_getGeometry()->totalSize;
}
const flashGeometry_t* flashfsGetGeometry()
{
return m25p16_getGeometry();
}
static uint32_t flashfsTransmitBufferUsed()
{
if (bufferHead >= bufferTail)
return bufferHead - bufferTail;
return FLASHFS_WRITE_BUFFER_SIZE - bufferTail + bufferHead;
}
/**
* Write the given buffers to flash sequentially at the current tail address, advancing the tail address after
* each write.
*
* In synchronous mode, waits for the flash to become ready before writing so that every byte requested can be written.
*
* In asynchronous mode, if the flash is busy, then the write is aborted and the routine returns immediately.
* In this case the returned number of bytes written will be less than the total amount requested.
*
* Modifies the supplied buffer pointers and sizes to reflect how many bytes remain in each of them.
*
* bufferCount: the number of buffers provided
* buffers: an array of pointers to the beginning of buffers
* bufferSizes: an array of the sizes of those buffers
* sync: true if we should wait for the device to be idle before writes, otherwise if the device is busy the
* write will be aborted and this routine will return immediately.
*
* Returns the number of bytes written
*/
static uint32_t flashfsWriteBuffers(uint8_t const **buffers, uint32_t *bufferSizes, int bufferCount, bool sync)
{
const flashGeometry_t *geometry = m25p16_getGeometry();
uint32_t bytesTotal = 0;
int i;
for (i = 0; i < bufferCount; i++) {
bytesTotal += bufferSizes[i];
}
if (!sync && !m25p16_isReady()) {
return 0;
}
uint32_t bytesTotalRemaining = bytesTotal;
while (bytesTotalRemaining > 0) {
uint32_t bytesTotalThisIteration;
uint32_t bytesRemainThisIteration;
/*
* Each page needs to be saved in a separate program operation, so
* if we would cross a page boundary, only write up to the boundary in this iteration:
*/
if (tailIndexInPage + bytesTotalRemaining > geometry->pageSize) {
bytesTotalThisIteration = geometry->pageSize - tailIndexInPage;
} else {
bytesTotalThisIteration = bytesTotalRemaining;
}
// Are we at EOF already? Abort.
if (flashfsIsEOF()) {
// May as well throw away any buffered data
flashfsClearBuffer();
break;
}
m25p16_pageProgramBegin(tailAddress);
bytesRemainThisIteration = bytesTotalThisIteration;
for (i = 0; i < bufferCount; i++) {
if (bufferSizes[i] > 0) {
// Is buffer larger than our write limit? Write our limit out of it
if (bufferSizes[i] >= bytesRemainThisIteration) {
m25p16_pageProgramContinue(buffers[i], bytesRemainThisIteration);
buffers[i] += bytesRemainThisIteration;
bufferSizes[i] -= bytesRemainThisIteration;
bytesRemainThisIteration = 0;
break;
} else {
// We'll still have more to write after finishing this buffer off
m25p16_pageProgramContinue(buffers[i], bufferSizes[i]);
bytesRemainThisIteration -= bufferSizes[i];
buffers[i] += bufferSizes[i];
bufferSizes[i] = 0;
}
}
}
m25p16_pageProgramFinish();
bytesTotalRemaining -= bytesTotalThisIteration;
// Advance the cursor in the file system to match the bytes we wrote
flashfsSetTailAddress(tailAddress + bytesTotalThisIteration);
/*
* We'll have to wait for that write to complete before we can issue the next one, so if
* the user requested asynchronous writes, break now.
*/
if (!sync)
break;
}
return bytesTotal - bytesTotalRemaining;
}
/*
* Since the buffered data might wrap around the end of the circular buffer, we can have two segments of data to write,
* an initial portion and a possible wrapped portion.
*
* This routine will fill the details of those buffers into the provided arrays, which must be at least 2 elements long.
*/
static void flashfsGetDirtyDataBuffers(uint8_t const *buffers[], uint32_t bufferSizes[])
{
buffers[0] = flashWriteBuffer + bufferTail;
buffers[1] = flashWriteBuffer + 0;
if (bufferHead >= bufferTail) {
bufferSizes[0] = bufferHead - bufferTail;
bufferSizes[1] = 0;
} else {
bufferSizes[0] = FLASHFS_WRITE_BUFFER_SIZE - bufferTail;
bufferSizes[1] = bufferHead;
}
}
/**
* Get the current offset of the file pointer within the volume.
*/
uint32_t flashfsGetOffset()
{
uint8_t const * buffers[2];
uint32_t bufferSizes[2];
// Dirty data in the buffers contributes to the offset
flashfsGetDirtyDataBuffers(buffers, bufferSizes);
return tailAddress + bufferSizes[0] + bufferSizes[1];
}
/**
* Called after bytes have been written from the buffer to advance the position of the tail by the given amount.
*/
static void flashfsAdvanceTailInBuffer(uint32_t delta)
{
bufferTail += delta;
// Wrap tail around the end of the buffer
if (bufferTail >= FLASHFS_WRITE_BUFFER_SIZE) {
bufferTail -= FLASHFS_WRITE_BUFFER_SIZE;
}
if (flashfsBufferIsEmpty()) {
flashfsClearBuffer(); // Bring buffer pointers back to the start to be tidier
}
}
/**
* If the flash is ready to accept writes, flush the buffer to it, otherwise schedule
* a flush for later and return immediately.
*
* Returns true if all data in the buffer has been flushed to the device.
*/
bool flashfsFlushAsync()
{
if (flashfsBufferIsEmpty()) {
shouldFlush = false;
return true; // Nothing to flush
}
uint8_t const * buffers[2];
uint32_t bufferSizes[2];
uint32_t bytesWritten;
flashfsGetDirtyDataBuffers(buffers, bufferSizes);
bytesWritten = flashfsWriteBuffers(buffers, bufferSizes, 2, false);
flashfsAdvanceTailInBuffer(bytesWritten);
shouldFlush = !flashfsBufferIsEmpty();
return flashfsBufferIsEmpty();
}
/**
* Wait for the flash to become ready and begin flushing any buffered data to flash.
*
* The flash will still be busy some time after this sync completes, but space will
* be freed up to accept more writes in the write buffer.
*/
void flashfsFlushSync()
{
if (flashfsBufferIsEmpty()) {
shouldFlush = false;
return; // Nothing to flush
}
uint8_t const * buffers[2];
uint32_t bufferSizes[2];
flashfsGetDirtyDataBuffers(buffers, bufferSizes);
flashfsWriteBuffers(buffers, bufferSizes, 2, true);
// We've written our entire buffer now:
flashfsClearBuffer();
}
void flashfsSeekAbs(uint32_t offset)
{
flashfsFlushSync();
flashfsSetTailAddress(offset);
}
void flashfsSeekRel(int32_t offset)
{
flashfsFlushSync();
flashfsSetTailAddress(tailAddress + offset);
}
/**
* Write the given byte asynchronously to the flash. If the buffer overflows, data is silently discarded.
*/
void flashfsWriteByte(uint8_t byte)
{
flashWriteBuffer[bufferHead++] = byte;
if (bufferHead >= FLASHFS_WRITE_BUFFER_SIZE) {
bufferHead = 0;
}
if (shouldFlush || flashfsTransmitBufferUsed() >= FLASHFS_WRITE_BUFFER_AUTO_FLUSH_LEN) {
flashfsFlushAsync();
}
}
/**
* Write the given buffer to the flash either synchronously or asynchronously depending on the 'sync' parameter.
*
* If writing asynchronously, data will be silently discarded if the buffer overflows.
* If writing synchronously, the routine will block waiting for the flash to become ready so will never drop data.
*/
void flashfsWrite(const uint8_t *data, unsigned int len, bool sync)
{
uint8_t const * buffers[3];
uint32_t bufferSizes[3];
// There could be two dirty buffers to write out already:
flashfsGetDirtyDataBuffers(buffers, bufferSizes);
// Plus the buffer the user supplied:
buffers[2] = data;
bufferSizes[2] = len;
/*
* Would writing this data to our buffer cause our buffer to reach the flush threshold? If so try to write through
* to the flash now
*/
if (shouldFlush || bufferSizes[0] + bufferSizes[1] + bufferSizes[2] >= FLASHFS_WRITE_BUFFER_AUTO_FLUSH_LEN) {
uint32_t bytesWritten;
// Attempt to write all three buffers through to the flash asynchronously
bytesWritten = flashfsWriteBuffers(buffers, bufferSizes, 3, false);
if (bufferSizes[0] == 0 && bufferSizes[1] == 0) {
// We wrote all the data that was previously buffered
flashfsClearBuffer();
if (bufferSizes[2] == 0) {
// And we wrote all the data the user supplied! Job done!
return;
}
} else {
// We only wrote a portion of the old data, so advance the tail to remove the bytes we did write from the buffer
flashfsAdvanceTailInBuffer(bytesWritten);
}
// Is the remainder of the data to be written too big to fit in the buffers?
if (bufferSizes[0] + bufferSizes[1] + bufferSizes[2] > FLASHFS_WRITE_BUFFER_USABLE) {
if (sync) {
// Write it through synchronously
flashfsWriteBuffers(buffers, bufferSizes, 3, true);
flashfsClearBuffer();
} else {
/*
* Silently drop the data the user asked to write (i.e. no-op) since we can't buffer it and they
* requested async.
*/
}
return;
}
// Fall through and add the remainder of the incoming data to our buffer
data = buffers[2];
len = bufferSizes[2];
}
// Buffer up the data the user supplied instead of writing it right away
// First write the portion before we wrap around the end of the circular buffer
unsigned int bufferBytesBeforeWrap = FLASHFS_WRITE_BUFFER_SIZE - bufferHead;
unsigned int firstPortion = len < bufferBytesBeforeWrap ? len : bufferBytesBeforeWrap;
memcpy(flashWriteBuffer + bufferHead, data, firstPortion);
bufferHead += firstPortion;
data += firstPortion;
len -= firstPortion;
// If we wrap the head around, write the remainder to the start of the buffer (if any)
if (bufferHead == FLASHFS_WRITE_BUFFER_SIZE) {
memcpy(flashWriteBuffer + 0, data, len);
bufferHead = len;
}
}
/**
* Read `len` bytes from the given address into the supplied buffer.
*
* Returns the number of bytes actually read which may be less than that requested.
*/
int flashfsReadAbs(uint32_t address, uint8_t *buffer, unsigned int len)
{
int bytesRead;
// Did caller try to read past the end of the volume?
if (address + len > flashfsGetSize()) {
// Truncate their request
len = flashfsGetSize() - address;
}
// Since the read could overlap data in our dirty buffers, force a sync to clear those first
flashfsFlushSync();
bytesRead = m25p16_readBytes(address, buffer, len);
return bytesRead;
}
/**
* Find the offset of the start of the free space on the device (or the size of the device if it is full).
*/
int flashfsIdentifyStartOfFreeSpace()
{
/* Find the start of the free space on the device by examining the beginning of blocks with a binary search,
* looking for ones that appear to be erased. We can achieve this with good accuracy because an erased block
* is all bits set to 1, which pretty much never appears in reasonable size substrings of blackbox logs.
*
* To do better we might write a volume header instead, which would mark how much free space remains. But keeping
* a header up to date while logging would incur more writes to the flash, which would consume precious write
* bandwidth and block more often.
*/
enum {
/* 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,
/* 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
FREE_BLOCK_TEST_SIZE_BYTES = FREE_BLOCK_TEST_SIZE_INTS * sizeof(uint32_t),
};
union {
uint8_t bytes[FREE_BLOCK_TEST_SIZE_BYTES];
uint32_t ints[FREE_BLOCK_TEST_SIZE_INTS];
} testBuffer;
int left = 0;
int right = flashfsGetSize() / FREE_BLOCK_SIZE;
int mid, 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);
// 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;
for (i = 0; i < FREE_BLOCK_TEST_SIZE_INTS; i++) {
if (testBuffer.ints[i] != 0xFFFFFFFF) {
blockErased = false;
break;
}
}
if (blockErased) {
/* This erased block might be the leftmost erased block in the volume, but we'll need to continue the
* search leftwards to find out:
*/
result = mid;
right = mid;
} else {
left = mid + 1;
}
}
return result * FREE_BLOCK_SIZE;
}
/**
* Returns true if the file pointer is at the end of the device.
*/
bool flashfsIsEOF() {
return tailAddress >= flashfsGetSize();
}
/**
* Call after initializing the flash chip in order to set up the filesystem.
*/
void flashfsInit()
{
// If we have a flash chip present at all
if (flashfsGetSize() > 0) {
// Start the file pointer off at the beginning of free space so caller can start writing immediately
flashfsSeekAbs(flashfsIdentifyStartOfFreeSpace());
}
}

52
src/main/io/flashfs.h Normal file
View file

@ -0,0 +1,52 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Cleanflight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#include <stdint.h>
#include "drivers/flash.h"
#define FLASHFS_WRITE_BUFFER_SIZE 128
#define FLASHFS_WRITE_BUFFER_USABLE (FLASHFS_WRITE_BUFFER_SIZE - 1)
// Automatically trigger a flush when this much data is in the buffer
#define FLASHFS_WRITE_BUFFER_AUTO_FLUSH_LEN 64
void flashfsEraseCompletely();
void flashfsEraseRange(uint32_t start, uint32_t end);
uint32_t flashfsGetSize();
uint32_t flashfsGetOffset();
int flashfsIdentifyStartOfFreeSpace();
const flashGeometry_t* flashfsGetGeometry();
void flashfsSeekAbs(uint32_t offset);
void flashfsSeekRel(int32_t offset);
void flashfsWriteByte(uint8_t byte);
void flashfsWrite(const uint8_t *data, unsigned int len, bool sync);
int flashfsReadAbs(uint32_t offset, uint8_t *data, unsigned int len);
bool flashfsFlushAsync();
void flashfsFlushSync();
void flashfsInit();
bool flashfsIsReady();
bool flashfsIsEOF();

19
src/main/io/rc_controls.c
View file

@ -47,6 +47,8 @@
#include "io/rc_controls.h" #include "io/rc_controls.h"
#include "io/rc_curves.h" #include "io/rc_curves.h"
#include "io/display.h"
#include "flight/pid.h" #include "flight/pid.h"
#include "flight/navigation.h" #include "flight/navigation.h"
@ -247,6 +249,17 @@ void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStat
rcDelayCommand = 0; // allow autorepetition rcDelayCommand = 0; // allow autorepetition
return; return;
} }
#ifdef DISPLAY
if (rcSticks == THR_LO + YAW_CE + PIT_HI + ROL_LO) {
displayDisablePageCycling();
}
if (rcSticks == THR_LO + YAW_CE + PIT_HI + ROL_HI) {
displayEnablePageCycling();
}
#endif
} }
bool isRangeActive(uint8_t auxChannelIndex, channelRange_t *range) { bool isRangeActive(uint8_t auxChannelIndex, channelRange_t *range) {
@ -580,3 +593,9 @@ void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, es
} }
} }
} }
void resetAdjustmentStates(void)
{
memset(adjustmentStates, 0, sizeof(adjustmentStates));
}

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

@ -214,6 +214,7 @@ typedef struct adjustmentState_s {
#define MAX_ADJUSTMENT_RANGE_COUNT 12 // enough for 2 * 6pos switches. #define MAX_ADJUSTMENT_RANGE_COUNT 12 // enough for 2 * 6pos switches.
void resetAdjustmentStates(void);
void configureAdjustment(uint8_t index, uint8_t auxChannelIndex, const adjustmentConfig_t *adjustmentConfig); void configureAdjustment(uint8_t index, uint8_t auxChannelIndex, const adjustmentConfig_t *adjustmentConfig);
void updateAdjustmentStates(adjustmentRange_t *adjustmentRanges); void updateAdjustmentStates(adjustmentRange_t *adjustmentRanges);
void processRcAdjustments(controlRateConfig_t *controlRateConfig, rxConfig_t *rxConfig); void processRcAdjustments(controlRateConfig_t *controlRateConfig, rxConfig_t *rxConfig);

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

@ -52,6 +52,7 @@
#include "io/rc_controls.h" #include "io/rc_controls.h"
#include "io/serial.h" #include "io/serial.h"
#include "io/ledstrip.h" #include "io/ledstrip.h"
#include "io/flashfs.h"
#include "rx/rx.h" #include "rx/rx.h"
#include "rx/spektrum.h" #include "rx/spektrum.h"
@ -121,6 +122,13 @@ static void cliColor(char *cmdline);
static void cliMixer(char *cmdline); static void cliMixer(char *cmdline);
#endif #endif
#ifdef USE_FLASHFS
static void cliFlashInfo(char *cmdline);
static void cliFlashErase(char *cmdline);
static void cliFlashWrite(char *cmdline);
static void cliFlashRead(char *cmdline);
#endif
// signal that we're in cli mode // signal that we're in cli mode
uint8_t cliMode = 0; uint8_t cliMode = 0;
@ -155,13 +163,11 @@ static const char * const sensorTypeNames[] = {
"GYRO", "ACC", "BARO", "MAG", "SONAR", "GPS", "GPS+MAG", NULL "GYRO", "ACC", "BARO", "MAG", "SONAR", "GPS", "GPS+MAG", NULL
}; };
// FIXME the next time the EEPROM is bumped change the order of acc and gyro names so that "None" is second.
#define SENSOR_NAMES_MASK (SENSOR_GYRO | SENSOR_ACC | SENSOR_BARO | SENSOR_MAG) #define SENSOR_NAMES_MASK (SENSOR_GYRO | SENSOR_ACC | SENSOR_BARO | SENSOR_MAG)
static const char * const sensorHardwareNames[4][11] = { static const char * const sensorHardwareNames[4][11] = {
{ "", "None", "MPU6050", "L3G4200D", "MPU3050", "L3GD20", "MPU6000", "MPU6500", "FAKE", NULL }, { "", "None", "MPU6050", "L3G4200D", "MPU3050", "L3GD20", "MPU6000", "MPU6500", "FAKE", NULL },
{ "", "ADXL345", "MPU6050", "MMA845x", "BMA280", "LSM303DLHC", "MPU6000", "MPU6500", "FAKE", "None", NULL }, { "", "None", "ADXL345", "MPU6050", "MMA845x", "BMA280", "LSM303DLHC", "MPU6000", "MPU6500", "FAKE", NULL },
{ "", "None", "BMP085", "MS5611", NULL }, { "", "None", "BMP085", "MS5611", NULL },
{ "", "None", "HMC5883", "AK8975", NULL } { "", "None", "HMC5883", "AK8975", NULL }
}; };
@ -185,6 +191,12 @@ const clicmd_t cmdTable[] = {
{ "dump", "dump configuration", cliDump }, { "dump", "dump configuration", cliDump },
{ "exit", "", cliExit }, { "exit", "", cliExit },
{ "feature", "list or -val or val", cliFeature }, { "feature", "list or -val or val", cliFeature },
#ifdef USE_FLASHFS
{ "flash_erase", "erase flash chip", cliFlashErase },
{ "flash_info", "get flash chip details", cliFlashInfo },
{ "flash_read", "read text from the given address", cliFlashRead },
{ "flash_write", "write text to the given address", cliFlashWrite },
#endif
{ "get", "get variable value", cliGet }, { "get", "get variable value", cliGet },
#ifdef GPS #ifdef GPS
{ "gpspassthrough", "passthrough gps to serial", cliGpsPassthrough }, { "gpspassthrough", "passthrough gps to serial", cliGpsPassthrough },
@ -201,7 +213,7 @@ const clicmd_t cmdTable[] = {
{ "profile", "index (0 to 2)", cliProfile }, { "profile", "index (0 to 2)", cliProfile },
{ "rateprofile", "index (0 to 2)", cliRateProfile }, { "rateprofile", "index (0 to 2)", cliRateProfile },
{ "save", "save and reboot", cliSave }, { "save", "save and reboot", cliSave },
#ifndef CJMCU #ifdef USE_SERVOS
{ "servo", "servo config", cliServo }, { "servo", "servo config", cliServo },
#endif #endif
{ "set", "name=value or blank or * for list", cliSet }, { "set", "name=value or blank or * for list", cliSet },
@ -355,8 +367,11 @@ const clivalue_t valueTable[] = {
{ "yaw_control_direction", VAR_INT8 | MASTER_VALUE, &masterConfig.yaw_control_direction, -1, 1 }, { "yaw_control_direction", VAR_INT8 | MASTER_VALUE, &masterConfig.yaw_control_direction, -1, 1 },
{ "yaw_direction", VAR_INT8 | PROFILE_VALUE, &masterConfig.profile[0].mixerConfig.yaw_direction, -1, 1 }, { "yaw_direction", VAR_INT8 | PROFILE_VALUE, &masterConfig.profile[0].mixerConfig.yaw_direction, -1, 1 },
#ifdef USE_SERVOS
{ "tri_unarmed_servo", VAR_INT8 | PROFILE_VALUE, &masterConfig.profile[0].mixerConfig.tri_unarmed_servo, 0, 1 }, { "tri_unarmed_servo", VAR_INT8 | PROFILE_VALUE, &masterConfig.profile[0].mixerConfig.tri_unarmed_servo, 0, 1 },
{ "servo_lowpass_freq", VAR_INT16 | PROFILE_VALUE, &masterConfig.profile[0].mixerConfig.servo_lowpass_freq, 10, 400},
{ "servo_lowpass_enable", VAR_INT8 | PROFILE_VALUE, &masterConfig.profile[0].mixerConfig.servo_lowpass_enable, 0, 1 },
#endif
{ "default_rate_profile", VAR_UINT8 | PROFILE_VALUE , &masterConfig.profile[0].defaultRateProfileIndex, 0, MAX_CONTROL_RATE_PROFILE_COUNT - 1 }, { "default_rate_profile", VAR_UINT8 | PROFILE_VALUE , &masterConfig.profile[0].defaultRateProfileIndex, 0, MAX_CONTROL_RATE_PROFILE_COUNT - 1 },
{ "rc_rate", VAR_UINT8 | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].rcRate8, 0, 250 }, { "rc_rate", VAR_UINT8 | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].rcRate8, 0, 250 },
{ "rc_expo", VAR_UINT8 | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].rcExpo8, 0, 100 }, { "rc_expo", VAR_UINT8 | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].rcExpo8, 0, 100 },
@ -373,7 +388,9 @@ const clivalue_t valueTable[] = {
{ "failsafe_min_usec", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].failsafeConfig.failsafe_min_usec, 100, 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_max_usec", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].failsafeConfig.failsafe_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}, { "gimbal_flags", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].gimbalConfig.gimbal_flags, 0, 255},
#endif
{ "acc_hardware", VAR_UINT8 | MASTER_VALUE, &masterConfig.acc_hardware, 0, ACC_NONE }, { "acc_hardware", VAR_UINT8 | MASTER_VALUE, &masterConfig.acc_hardware, 0, ACC_NONE },
{ "acc_lpf_factor", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].acc_lpf_factor, 0, 250 }, { "acc_lpf_factor", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].acc_lpf_factor, 0, 250 },
@ -433,6 +450,7 @@ const clivalue_t valueTable[] = {
#ifdef BLACKBOX #ifdef BLACKBOX
{ "blackbox_rate_num", VAR_UINT8 | MASTER_VALUE, &masterConfig.blackbox_rate_num, 1, 32 }, { "blackbox_rate_num", VAR_UINT8 | MASTER_VALUE, &masterConfig.blackbox_rate_num, 1, 32 },
{ "blackbox_rate_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.blackbox_rate_denom, 1, 32 }, { "blackbox_rate_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.blackbox_rate_denom, 1, 32 },
{ "blackbox_device", VAR_UINT8 | MASTER_VALUE, &masterConfig.blackbox_device, 0, 1 },
#endif #endif
}; };
@ -752,7 +770,7 @@ static void cliColor(char *cmdline)
static void cliServo(char *cmdline) static void cliServo(char *cmdline)
{ {
#ifdef CJMCU #ifndef USE_SERVOS
UNUSED(cmdline); UNUSED(cmdline);
#else #else
enum { SERVO_ARGUMENT_COUNT = 6 }; enum { SERVO_ARGUMENT_COUNT = 6 };
@ -822,6 +840,88 @@ static void cliServo(char *cmdline)
#endif #endif
} }
#ifdef USE_FLASHFS
static void cliFlashInfo(char *cmdline)
{
const flashGeometry_t *layout = flashfsGetGeometry();
UNUSED(cmdline);
printf("Flash sectors=%u, sectorSize=%u, pagesPerSector=%u, pageSize=%u, totalSize=%u, usedSize=%u\r\n",
layout->sectors, layout->sectorSize, layout->pagesPerSector, layout->pageSize, layout->totalSize, flashfsGetOffset());
}
static void cliFlashErase(char *cmdline)
{
UNUSED(cmdline);
printf("Erasing, please wait...\r\n");
flashfsEraseCompletely();
while (!flashfsIsReady()) {
delay(100);
}
printf("Done.\r\n");
}
static void cliFlashWrite(char *cmdline)
{
uint32_t address = atoi(cmdline);
char *text = strchr(cmdline, ' ');
if (!text) {
printf("Missing text to write.\r\n");
} else {
flashfsSeekAbs(address);
flashfsWrite((uint8_t*)text, strlen(text), true);
flashfsFlushSync();
printf("Wrote %u bytes at %u.\r\n", strlen(text), address);
}
}
static void cliFlashRead(char *cmdline)
{
uint32_t address = atoi(cmdline);
uint32_t length;
int i;
uint8_t buffer[32];
char *nextArg = strchr(cmdline, ' ');
if (!nextArg) {
printf("Missing length argument.\r\n");
} else {
length = atoi(nextArg);
printf("Reading %u bytes at %u:\r\n", length, address);
while (length > 0) {
int bytesRead;
bytesRead = flashfsReadAbs(address, buffer, length < sizeof(buffer) ? length : sizeof(buffer));
for (i = 0; i < bytesRead; i++) {
cliWrite(buffer[i]);
}
length -= bytesRead;
address += bytesRead;
if (bytesRead == 0) {
//Assume we reached the end of the volume or something fatal happened
break;
}
}
printf("\r\n");
}
}
#endif
static void dumpValues(uint16_t mask) static void dumpValues(uint16_t mask)
{ {
uint32_t i; uint32_t i;
@ -1238,6 +1338,7 @@ static void cliRateProfile(char *cmdline)
static void cliReboot(void) { static void cliReboot(void) {
cliPrint("\r\nRebooting"); cliPrint("\r\nRebooting");
waitForSerialPortToFinishTransmitting(cliPort); waitForSerialPortToFinishTransmitting(cliPort);
stopMotors();
systemReset(); systemReset();
} }

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

@ -50,6 +50,7 @@
#include "io/gimbal.h" #include "io/gimbal.h"
#include "io/serial.h" #include "io/serial.h"
#include "io/ledstrip.h" #include "io/ledstrip.h"
#include "io/flashfs.h"
#include "telemetry/telemetry.h" #include "telemetry/telemetry.h"
@ -232,6 +233,10 @@ const char *boardIdentifier = TARGET_BOARD_IDENTIFIER;
// DEPRECATED - Use MSP_BUILD_INFO instead // DEPRECATED - Use MSP_BUILD_INFO instead
#define MSP_BF_BUILD_INFO 69 //out message build date as well as some space for future expansion #define MSP_BF_BUILD_INFO 69 //out message build date as well as some space for future expansion
#define MSP_DATAFLASH_SUMMARY 70 //out message - get description of dataflash chip
#define MSP_DATAFLASH_READ 71 //out message - get content of dataflash chip
#define MSP_DATAFLASH_ERASE 72 //in message - erase dataflash chip
// //
// Multwii original MSP commands // Multwii original MSP commands
// //
@ -435,24 +440,24 @@ static uint32_t read32(void)
return t; return t;
} }
static void headSerialResponse(uint8_t err, uint8_t s) static void headSerialResponse(uint8_t err, uint8_t responseBodySize)
{ {
serialize8('$'); serialize8('$');
serialize8('M'); serialize8('M');
serialize8(err ? '!' : '>'); serialize8(err ? '!' : '>');
currentPort->checksum = 0; // start calculating a new checksum currentPort->checksum = 0; // start calculating a new checksum
serialize8(s); serialize8(responseBodySize);
serialize8(currentPort->cmdMSP); serialize8(currentPort->cmdMSP);
} }
static void headSerialReply(uint8_t s) static void headSerialReply(uint8_t responseBodySize)
{ {
headSerialResponse(0, s); headSerialResponse(0, responseBodySize);
} }
static void headSerialError(uint8_t s) static void headSerialError(uint8_t responseBodySize)
{ {
headSerialResponse(1, s); headSerialResponse(1, responseBodySize);
} }
static void tailSerialReply(void) static void tailSerialReply(void)
@ -532,6 +537,46 @@ reset:
} }
} }
static void serializeDataflashSummaryReply(void)
{
headSerialReply(1 + 3 * 4);
#ifdef USE_FLASHFS
const flashGeometry_t *geometry = flashfsGetGeometry();
serialize8(flashfsIsReady() ? 1 : 0);
serialize32(geometry->sectors);
serialize32(geometry->totalSize);
serialize32(flashfsGetOffset()); // Effectively the current number of bytes stored on the volume
#else
serialize8(0);
serialize32(0);
serialize32(0);
serialize32(0);
#endif
}
#ifdef USE_FLASHFS
static void serializeDataflashReadReply(uint32_t address, uint8_t size)
{
uint8_t buffer[128];
int bytesRead;
if (size > sizeof(buffer)) {
size = sizeof(buffer);
}
headSerialReply(4 + size);
serialize32(address);
// bytesRead will be lower than that requested if we reach end of volume
bytesRead = flashfsReadAbs(address, buffer, size);
for (int i = 0; i < bytesRead; i++) {
serialize8(buffer[i]);
}
}
#endif
static void resetMspPort(mspPort_t *mspPortToReset, serialPort_t *serialPort, mspPortUsage_e usage) static void resetMspPort(mspPort_t *mspPortToReset, serialPort_t *serialPort, mspPortUsage_e usage)
{ {
memset(mspPortToReset, 0, sizeof(mspPort_t)); memset(mspPortToReset, 0, sizeof(mspPort_t));
@ -783,6 +828,7 @@ static bool processOutCommand(uint8_t cmdMSP)
for (i = 0; i < 3; i++) for (i = 0; i < 3; i++)
serialize16(magADC[i]); serialize16(magADC[i]);
break; break;
#ifdef USE_SERVOS
case MSP_SERVO: case MSP_SERVO:
s_struct((uint8_t *)&servo, 16); s_struct((uint8_t *)&servo, 16);
break; break;
@ -801,6 +847,7 @@ static bool processOutCommand(uint8_t cmdMSP)
serialize8(currentProfile->servoConf[i].forwardFromChannel); serialize8(currentProfile->servoConf[i].forwardFromChannel);
} }
break; break;
#endif
case MSP_MOTOR: case MSP_MOTOR:
s_struct((uint8_t *)motor, 16); s_struct((uint8_t *)motor, 16);
break; break;
@ -1136,6 +1183,21 @@ static bool processOutCommand(uint8_t cmdMSP)
} }
break; break;
#endif #endif
case MSP_DATAFLASH_SUMMARY:
serializeDataflashSummaryReply();
break;
#ifdef USE_FLASHFS
case MSP_DATAFLASH_READ:
{
uint32_t readAddress = read32();
serializeDataflashReadReply(readAddress, 128);
}
break;
#endif
case MSP_BF_BUILD_INFO: case MSP_BF_BUILD_INFO:
headSerialReply(11 + 4 + 4); headSerialReply(11 + 4 + 4);
for (i = 0; i < 11; i++) for (i = 0; i < 11; i++)
@ -1304,6 +1366,7 @@ static bool processInCommand(void)
motor_disarmed[i] = read16(); motor_disarmed[i] = read16();
break; break;
case MSP_SET_SERVO_CONF: case MSP_SET_SERVO_CONF:
#ifdef USE_SERVOS
for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) { for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
currentProfile->servoConf[i].min = read16(); currentProfile->servoConf[i].min = read16();
currentProfile->servoConf[i].max = read16(); currentProfile->servoConf[i].max = read16();
@ -1317,11 +1380,14 @@ static bool processInCommand(void)
} }
currentProfile->servoConf[i].rate = read8(); currentProfile->servoConf[i].rate = read8();
} }
#endif
break; break;
case MSP_SET_CHANNEL_FORWARDING: case MSP_SET_CHANNEL_FORWARDING:
#ifdef USE_SERVOS
for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) { for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
currentProfile->servoConf[i].forwardFromChannel = read8(); currentProfile->servoConf[i].forwardFromChannel = read8();
} }
#endif
break; break;
case MSP_RESET_CONF: case MSP_RESET_CONF:
if (!ARMING_FLAG(ARMED)) { if (!ARMING_FLAG(ARMED)) {
@ -1345,6 +1411,13 @@ static bool processInCommand(void)
writeEEPROM(); writeEEPROM();
readEEPROM(); readEEPROM();
break; break;
#ifdef USE_FLASHFS
case MSP_DATAFLASH_ERASE:
flashfsEraseCompletely();
break;
#endif
#ifdef GPS #ifdef GPS
case MSP_SET_RAW_GPS: case MSP_SET_RAW_GPS:
if (read8()) { if (read8()) {
@ -1609,6 +1682,7 @@ void mspProcess(void)
while (!isSerialTransmitBufferEmpty(candidatePort->port)) { while (!isSerialTransmitBufferEmpty(candidatePort->port)) {
delay(50); delay(50);
} }
stopMotors();
systemReset(); systemReset();
} }
} }

58
src/main/main.c
View file

@ -46,10 +46,12 @@
#include "drivers/bus_i2c.h" #include "drivers/bus_i2c.h"
#include "drivers/bus_spi.h" #include "drivers/bus_spi.h"
#include "drivers/inverter.h" #include "drivers/inverter.h"
#include "drivers/flash_m25p16.h"
#include "rx/rx.h" #include "rx/rx.h"
#include "io/serial.h" #include "io/serial.h"
#include "io/flashfs.h"
#include "io/gps.h" #include "io/gps.h"
#include "io/escservo.h" #include "io/escservo.h"
#include "io/rc_controls.h" #include "io/rc_controls.h"
@ -127,11 +129,20 @@ void SetSysClock(void);
void SetSysClock(bool overclock); void SetSysClock(bool overclock);
#endif #endif
typedef enum {
SYSTEM_STATE_INITIALISING = 0,
SYSTEM_STATE_CONFIG_LOADED = (1 << 0),
SYSTEM_STATE_SENSORS_READY = (1 << 1),
SYSTEM_STATE_MOTORS_READY = (1 << 2),
SYSTEM_STATE_READY = (1 << 7)
} systemState_e;
static uint8_t systemState = SYSTEM_STATE_INITIALISING;
void init(void) void init(void)
{ {
uint8_t i; uint8_t i;
drv_pwm_config_t pwm_params; drv_pwm_config_t pwm_params;
bool sensorsOK = false;
printfSupportInit(); printfSupportInit();
@ -140,6 +151,8 @@ void init(void)
ensureEEPROMContainsValidData(); ensureEEPROMContainsValidData();
readEEPROM(); readEEPROM();
systemState |= SYSTEM_STATE_CONFIG_LOADED;
#ifdef STM32F303 #ifdef STM32F303
// start fpu // start fpu
SCB->CPACR = (0x3 << (10*2)) | (0x3 << (11*2)); SCB->CPACR = (0x3 << (10*2)) | (0x3 << (11*2));
@ -199,18 +212,22 @@ void init(void)
&& masterConfig.batteryConfig.currentMeterType == CURRENT_SENSOR_ADC; && masterConfig.batteryConfig.currentMeterType == CURRENT_SENSOR_ADC;
pwm_params.useLEDStrip = feature(FEATURE_LED_STRIP); pwm_params.useLEDStrip = feature(FEATURE_LED_STRIP);
pwm_params.usePPM = feature(FEATURE_RX_PPM); pwm_params.usePPM = feature(FEATURE_RX_PPM);
pwm_params.useOneshot = feature(FEATURE_ONESHOT125);
pwm_params.useSerialRx = feature(FEATURE_RX_SERIAL); pwm_params.useSerialRx = feature(FEATURE_RX_SERIAL);
#ifdef USE_SERVOS
pwm_params.useServos = isMixerUsingServos(); pwm_params.useServos = isMixerUsingServos();
pwm_params.extraServos = currentProfile->gimbalConfig.gimbal_flags & GIMBAL_FORWARDAUX; pwm_params.extraServos = currentProfile->gimbalConfig.gimbal_flags & GIMBAL_FORWARDAUX;
pwm_params.motorPwmRate = masterConfig.motor_pwm_rate; pwm_params.servoCenterPulse = masterConfig.escAndServoConfig.servoCenterPulse;
pwm_params.servoPwmRate = masterConfig.servo_pwm_rate; pwm_params.servoPwmRate = masterConfig.servo_pwm_rate;
#endif
pwm_params.useOneshot = feature(FEATURE_ONESHOT125);
pwm_params.motorPwmRate = masterConfig.motor_pwm_rate;
pwm_params.idlePulse = PULSE_1MS; // standard PWM for brushless ESC (default, overridden below) pwm_params.idlePulse = PULSE_1MS; // standard PWM for brushless ESC (default, overridden below)
if (feature(FEATURE_3D)) if (feature(FEATURE_3D))
pwm_params.idlePulse = masterConfig.flight3DConfig.neutral3d; pwm_params.idlePulse = masterConfig.flight3DConfig.neutral3d;
if (pwm_params.motorPwmRate > 500) if (pwm_params.motorPwmRate > 500)
pwm_params.idlePulse = 0; // brushed motors pwm_params.idlePulse = 0; // brushed motors
pwm_params.servoCenterPulse = masterConfig.escAndServoConfig.servoCenterPulse;
pwmRxInit(masterConfig.inputFilteringMode); pwmRxInit(masterConfig.inputFilteringMode);
@ -218,6 +235,8 @@ void init(void)
mixerUsePWMOutputConfiguration(pwmOutputConfiguration); mixerUsePWMOutputConfiguration(pwmOutputConfiguration);
systemState |= SYSTEM_STATE_MOTORS_READY;
#ifdef BEEPER #ifdef BEEPER
beeperConfig_t beeperConfig = { beeperConfig_t beeperConfig = {
.gpioPin = BEEP_PIN, .gpioPin = BEEP_PIN,
@ -296,11 +315,12 @@ void init(void)
} }
#endif #endif
sensorsOK = 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, currentProfile->mag_declination)) {
// if gyro was not detected due to whatever reason, we give up now.
// if gyro was not detected due to whatever reason, we give up now.
if (!sensorsOK)
failureMode(3); failureMode(3);
}
systemState |= SYSTEM_STATE_SENSORS_READY;
LED1_ON; LED1_ON;
LED0_OFF; LED0_OFF;
@ -328,7 +348,9 @@ void init(void)
cliInit(&masterConfig.serialConfig); cliInit(&masterConfig.serialConfig);
failsafe = failsafeInit(&masterConfig.rxConfig); failsafe = failsafeInit(&masterConfig.rxConfig);
beepcodeInit(failsafe); beepcodeInit(failsafe);
rxInit(&masterConfig.rxConfig, failsafe); rxInit(&masterConfig.rxConfig, failsafe);
#ifdef GPS #ifdef GPS
@ -364,6 +386,18 @@ void init(void)
} }
#endif #endif
#ifdef USE_FLASHFS
#ifdef NAZE
if (hardwareRevision == NAZE32_REV5) {
m25p16_init();
}
#endif
#ifdef SPRACINGF3
m25p16_init();
#endif
flashfsInit();
#endif
#ifdef BLACKBOX #ifdef BLACKBOX
initBlackbox(); initBlackbox();
#endif #endif
@ -404,6 +438,7 @@ void init(void)
#ifdef USE_OLED_GPS_DEBUG_PAGE_ONLY #ifdef USE_OLED_GPS_DEBUG_PAGE_ONLY
displayShowFixedPage(PAGE_GPS); displayShowFixedPage(PAGE_GPS);
#else #else
displayResetPageCycling();
displayEnablePageCycling(); displayEnablePageCycling();
#endif #endif
} }
@ -412,6 +447,8 @@ void init(void)
#ifdef CJMCU #ifdef CJMCU
LED2_ON; LED2_ON;
#endif #endif
systemState |= SYSTEM_STATE_READY;
} }
#ifdef SOFTSERIAL_LOOPBACK #ifdef SOFTSERIAL_LOOPBACK
@ -440,6 +477,9 @@ int main(void) {
void HardFault_Handler(void) void HardFault_Handler(void)
{ {
// fall out of the sky // fall out of the sky
writeAllMotors(masterConfig.escAndServoConfig.mincommand); uint8_t requiredState = SYSTEM_STATE_CONFIG_LOADED | SYSTEM_STATE_MOTORS_READY;
if ((systemState & requiredState) == requiredState) {
stopMotors();
}
while (1); while (1);
} }

5
src/main/mw.c
View file

@ -725,7 +725,12 @@ void loop(void)
); );
mixTable(); mixTable();
#ifdef USE_SERVOS
filterServos();
writeServos(); writeServos();
#endif
writeMotors(); writeMotors();
#ifdef BLACKBOX #ifdef BLACKBOX

18
src/main/sensors/acceleration.h
View file

@ -20,15 +20,15 @@
// Type of accelerometer used/detected // Type of accelerometer used/detected
typedef enum { typedef enum {
ACC_DEFAULT = 0, ACC_DEFAULT = 0,
ACC_ADXL345 = 1, ACC_NONE = 1,
ACC_MPU6050 = 2, ACC_ADXL345 = 2,
ACC_MMA8452 = 3, ACC_MPU6050 = 3,
ACC_BMA280 = 4, ACC_MMA8452 = 4,
ACC_LSM303DLHC = 5, ACC_BMA280 = 5,
ACC_SPI_MPU6000 = 6, ACC_LSM303DLHC = 6,
ACC_SPI_MPU6500 = 7, ACC_SPI_MPU6000 = 7,
ACC_FAKE = 8, ACC_SPI_MPU6500 = 8,
ACC_NONE = 9 ACC_FAKE = 9,
} accelerationSensor_e; } accelerationSensor_e;
extern sensor_align_e accAlign; extern sensor_align_e accAlign;

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

@ -110,7 +110,7 @@ const mpu6050Config_t *selectMPU6050Config(void)
#ifdef USE_FAKE_GYRO #ifdef USE_FAKE_GYRO
static void fakeGyroInit(void) {} static void fakeGyroInit(void) {}
static void fakeGyroRead(int16_t *gyroData) { static void fakeGyroRead(int16_t *gyroData) {
UNUSED(gyroData); memset(gyroData, 0, sizeof(int16_t[XYZ_AXIS_COUNT]));
} }
static void fakeGyroReadTemp(int16_t *tempData) { static void fakeGyroReadTemp(int16_t *tempData) {
UNUSED(tempData); UNUSED(tempData);
@ -129,7 +129,7 @@ bool fakeGyroDetect(gyro_t *gyro, uint16_t lpf)
#ifdef USE_FAKE_ACC #ifdef USE_FAKE_ACC
static void fakeAccInit(void) {} static void fakeAccInit(void) {}
static void fakeAccRead(int16_t *accData) { static void fakeAccRead(int16_t *accData) {
UNUSED(accData); memset(accData, 0, sizeof(int16_t[XYZ_AXIS_COUNT]));
} }
bool fakeAccDetect(acc_t *acc) bool fakeAccDetect(acc_t *acc)
@ -267,14 +267,6 @@ retry:
switch (accHardwareToUse) { switch (accHardwareToUse) {
case ACC_DEFAULT: case ACC_DEFAULT:
; // fallthrough ; // fallthrough
case ACC_FAKE:
#ifdef USE_FAKE_ACC
if (fakeAccDetect(&acc)) {
accHardware = ACC_FAKE;
break;
}
#endif
; // fallthrough
case ACC_ADXL345: // ADXL345 case ACC_ADXL345: // ADXL345
#ifdef USE_ACC_ADXL345 #ifdef USE_ACC_ADXL345
acc_params.useFifo = false; acc_params.useFifo = false;
@ -365,6 +357,14 @@ retry:
accHardware = ACC_SPI_MPU6500; accHardware = ACC_SPI_MPU6500;
break; break;
} }
#endif
; // fallthrough
case ACC_FAKE:
#ifdef USE_FAKE_ACC
if (fakeAccDetect(&acc)) {
accHardware = ACC_FAKE;
break;
}
#endif #endif
; // fallthrough ; // fallthrough
case ACC_NONE: // disable ACC case ACC_NONE: // disable ACC

1
src/main/target/ALIENWIIF3/target.h
View file

@ -92,6 +92,7 @@
#define GPS #define GPS
//#define DISPLAY //#define DISPLAY
#define AUTOTUNE #define AUTOTUNE
#define USE_SERVOS
#define SPEKTRUM_BIND #define SPEKTRUM_BIND

1
src/main/target/CC3D/target.h
View file

@ -106,6 +106,7 @@
#define TELEMETRY #define TELEMETRY
#define SERIAL_RX #define SERIAL_RX
#define AUTOTUNE #define AUTOTUNE
#define USE_SERVOS
#define SPEKTRUM_BIND #define SPEKTRUM_BIND
// USART3, PB11 (Flexport) // USART3, PB11 (Flexport)

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

@ -35,10 +35,18 @@
#define USABLE_TIMER_CHANNEL_COUNT 18 #define USABLE_TIMER_CHANNEL_COUNT 18
#define USE_SPI
#define USE_SPI_DEVICE_1
#define GYRO #define GYRO
#define USE_GYRO_L3GD20 #define USE_GYRO_L3GD20
#define USE_GYRO_MPU6050 #define USE_GYRO_MPU6050
#define L3GD20_SPI SPI1
#define L3GD20_CS_GPIO_CLK_PERIPHERAL RCC_AHBPeriph_GPIOE
#define L3GD20_CS_GPIO GPIOE
#define L3GD20_CS_PIN GPIO_Pin_3
#define GYRO_L3GD20_ALIGN CW90_DEG #define GYRO_L3GD20_ALIGN CW90_DEG
#define GYRO_MPU6050_ALIGN CW0_DEG #define GYRO_MPU6050_ALIGN CW0_DEG
@ -114,3 +122,4 @@
#define TELEMETRY #define TELEMETRY
#define SERIAL_RX #define SERIAL_RX
#define AUTOTUNE #define AUTOTUNE
#define USE_SERVOS

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

@ -61,6 +61,8 @@
// #define SOFT_I2C_PB67 // #define SOFT_I2C_PB67
#define SERIAL_RX #define SERIAL_RX
//#define BLACKBOX
//#define USE_SERVOS
#define SPEKTRUM_BIND #define SPEKTRUM_BIND
// USART2, PA3 // USART2, PA3

1
src/main/target/EUSTM32F103RC/target.h
View file

@ -124,6 +124,7 @@
#define TELEMETRY #define TELEMETRY
#define SERIAL_RX #define SERIAL_RX
#define AUTOTUNE #define AUTOTUNE
#define USE_SERVOS
#define SPEKTRUM_BIND #define SPEKTRUM_BIND
// USART2, PA3 // USART2, PA3

4
src/main/target/NAZE/hardware_revision.c
View file

@ -58,7 +58,7 @@ void detectHardwareRevision(void)
#define SPI_DEVICE_MPU (2) #define SPI_DEVICE_MPU (2)
#define M25P16_INSTRUCTION_RDID 0x9F #define M25P16_INSTRUCTION_RDID 0x9F
#define FLASH_M25P16 (0x202015) #define FLASH_M25P16_ID (0x202015)
uint8_t detectSpiDevice(void) uint8_t detectSpiDevice(void)
{ {
@ -74,7 +74,7 @@ uint8_t detectSpiDevice(void)
DISABLE_SPI_CS; DISABLE_SPI_CS;
flash_id = in[1] << 16 | in[2] << 8 | in[3]; flash_id = in[1] << 16 | in[2] << 8 | in[3];
if (flash_id == FLASH_M25P16) if (flash_id == FLASH_M25P16_ID)
return SPI_DEVICE_FLASH; return SPI_DEVICE_FLASH;
// try autodetect MPU // try autodetect MPU

10
src/main/target/NAZE/target.h
View file

@ -56,10 +56,19 @@
#define NAZE_SPI_CS_GPIO GPIOB #define NAZE_SPI_CS_GPIO GPIOB
#define NAZE_SPI_CS_PIN GPIO_Pin_12 #define NAZE_SPI_CS_PIN GPIO_Pin_12
// We either have this 16mbit flash chip on SPI or the MPU6500 acc/gyro depending on board revision:
#define M25P16_CS_GPIO NAZE_SPI_CS_GPIO
#define M25P16_CS_PIN NAZE_SPI_CS_PIN
#define M25P16_SPI_INSTANCE NAZE_SPI_INSTANCE
#define MPU6500_CS_GPIO NAZE_SPI_CS_GPIO #define MPU6500_CS_GPIO NAZE_SPI_CS_GPIO
#define MPU6500_CS_PIN NAZE_SPI_CS_PIN #define MPU6500_CS_PIN NAZE_SPI_CS_PIN
#define MPU6500_SPI_INSTANCE NAZE_SPI_INSTANCE #define MPU6500_SPI_INSTANCE NAZE_SPI_INSTANCE
#define USE_FLASHFS
#define USE_FLASH_M25P16
#define GYRO #define GYRO
#define USE_GYRO_MPU3050 #define USE_GYRO_MPU3050
#define USE_GYRO_MPU6050 #define USE_GYRO_MPU6050
@ -144,6 +153,7 @@
#define TELEMETRY #define TELEMETRY
#define SERIAL_RX #define SERIAL_RX
#define AUTOTUNE #define AUTOTUNE
#define USE_SERVOS
#define SPEKTRUM_BIND #define SPEKTRUM_BIND
// USART2, PA3 // USART2, PA3

1
src/main/target/NAZE32PRO/target.h
View file

@ -45,6 +45,7 @@
#define TELEMETRY #define TELEMETRY
#define SERIAL_RX #define SERIAL_RX
#define AUTOTUNE #define AUTOTUNE
#define USE_SERVOS
#define SPEKTRUM_BIND #define SPEKTRUM_BIND
// USART2, PA3 // USART2, PA3

1
src/main/target/OLIMEXINO/target.h
View file

@ -111,3 +111,4 @@
#define SERIAL_RX #define SERIAL_RX
#define AUTOTUNE #define AUTOTUNE
#define BLACKBOX #define BLACKBOX
#define USE_SERVOS

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

@ -135,4 +135,4 @@
#define TELEMETRY #define TELEMETRY
#define SERIAL_RX #define SERIAL_RX
#define AUTOTUNE #define AUTOTUNE
#define USE_SERVOS

1
src/main/target/SPARKY/target.h
View file

@ -90,6 +90,7 @@
#define SERIAL_RX #define SERIAL_RX
#define GPS #define GPS
#define DISPLAY #define DISPLAY
#define USE_SERVOS
#define LED_STRIP #define LED_STRIP
#if 1 #if 1

23
src/main/target/SPRACINGF3/target.h
View file

@ -45,6 +45,9 @@
#define USE_MAG_HMC5883 #define USE_MAG_HMC5883
#define MAG_HMC5883_ALIGN CW270_DEG #define MAG_HMC5883_ALIGN CW270_DEG
#define USE_FLASHFS
#define USE_FLASH_M25P16
#define BEEPER #define BEEPER
#define LED0 #define LED0
@ -81,8 +84,23 @@
#define USE_I2C #define USE_I2C
#define I2C_DEVICE (I2CDEV_1) // PB6/SCL, PB7/SDA #define I2C_DEVICE (I2CDEV_1) // PB6/SCL, PB7/SDA
//#define USE_SPI #define USE_SPI
//#define USE_SPI_DEVICE_2 // PB12,13,14,15 on AF5 #define USE_SPI_DEVICE_2 // PB12,13,14,15 on AF5
#define SPI2_GPIO GPIOB
#define SPI2_GPIO_PERIPHERAL RCC_AHBPeriph_GPIOB
#define SPI2_NSS_PIN Pin_12
#define SPI2_NSS_PIN_SOURCE GPIO_PinSource12
#define SPI2_SCK_PIN Pin_13
#define SPI2_SCK_PIN_SOURCE GPIO_PinSource13
#define SPI2_MISO_PIN Pin_14
#define SPI2_MISO_PIN_SOURCE GPIO_PinSource14
#define SPI2_MOSI_PIN Pin_15
#define SPI2_MOSI_PIN_SOURCE GPIO_PinSource15
#define M25P16_CS_GPIO GPIOB
#define M25P16_CS_PIN GPIO_Pin_12
#define M25P16_SPI_INSTANCE SPI2
#define USE_ADC #define USE_ADC
@ -126,3 +144,4 @@
#define SERIAL_RX #define SERIAL_RX
#define AUTOTUNE #define AUTOTUNE
#define DISPLAY #define DISPLAY
#define USE_SERVOS

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

@ -36,9 +36,17 @@
#define BEEPER_INVERTED #define BEEPER_INVERTED
#define USE_SPI
#define USE_SPI_DEVICE_1
#define GYRO #define GYRO
#define USE_GYRO_L3GD20 #define USE_GYRO_L3GD20
#define L3GD20_SPI SPI1
#define L3GD20_CS_GPIO_CLK_PERIPHERAL RCC_AHBPeriph_GPIOE
#define L3GD20_CS_GPIO GPIOE
#define L3GD20_CS_PIN GPIO_Pin_3
#define GYRO_L3GD20_ALIGN CW90_DEG #define GYRO_L3GD20_ALIGN CW90_DEG
#define ACC #define ACC
@ -88,3 +96,4 @@
#define TELEMETRY #define TELEMETRY
#define SERIAL_RX #define SERIAL_RX
#define AUTOTUNE #define AUTOTUNE
#define USE_SERVOS

48
src/test/Makefile
View file

@ -52,7 +52,9 @@ TESTS = \
telemetry_hott_unittest \ telemetry_hott_unittest \
rc_controls_unittest \ rc_controls_unittest \
ledstrip_unittest \ ledstrip_unittest \
ws2811_unittest ws2811_unittest \
encoding_unittest \
lowpass_unittest
# All Google Test headers. Usually you shouldn't change this # All Google Test headers. Usually you shouldn't change this
# definition. # definition.
@ -129,6 +131,25 @@ battery_unittest : \
$(OBJECT_DIR)/gtest_main.a $(OBJECT_DIR)/gtest_main.a
$(CXX) $(CXX_FLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@ $(CXX) $(CXX_FLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@
$(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 $@
encoding_unittest : \
$(OBJECT_DIR)/common/encoding.o \
$(OBJECT_DIR)/encoding_unittest.o \
$(OBJECT_DIR)/gtest_main.a
$(CXX) $(CXX_FLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@
$(OBJECT_DIR)/flight/imu.o : \ $(OBJECT_DIR)/flight/imu.o : \
$(USER_DIR)/flight/imu.c \ $(USER_DIR)/flight/imu.c \
@ -319,6 +340,31 @@ ws2811_unittest : \
$(CXX) $(CXX_FLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@ $(CXX) $(CXX_FLAGS) -lpthread $^ -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 $@
lowpass_unittest : \
$(OBJECT_DIR)/flight/lowpass.o \
$(OBJECT_DIR)/lowpass_unittest.o \
$(OBJECT_DIR)/gtest_main.a
$(CXX) $(CXX_FLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@
test: $(TESTS) test: $(TESTS)
set -e && for test in $(TESTS) ; do \ set -e && for test in $(TESTS) ; do \
$(OBJECT_DIR)/$$test; \ $(OBJECT_DIR)/$$test; \

84
src/test/unit/encoding_unittest.cc Normal file
View file

@ -0,0 +1,84 @@
/*
* 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>
extern "C" {
#include "common/encoding.h"
}
#include "unittest_macros.h"
#include "gtest/gtest.h"
typedef struct zigzagEncodingExpectation_t {
int32_t input;
uint32_t expected;
} zigzagEncodingExpectation_t;
typedef struct floatToIntEncodingExpectation_t {
float input;
uint32_t expected;
} floatToIntEncodingExpectation_t;
TEST(EncodingTest, ZigzagEncodingTest)
{
// given
zigzagEncodingExpectation_t expectations[] = {
{ 0, 0},
{-1, 1},
{ 1, 2},
{-2, 3},
{ 2, 4},
{ 2147483646, 4294967292},
{-2147483647, 4294967293},
{ 2147483647, 4294967294},
{-2147483648, 4294967295},
};
int expectationCount = sizeof(expectations) / sizeof(expectations[0]);
// expect
for (int i = 0; i < expectationCount; i++) {
zigzagEncodingExpectation_t *expectation = &expectations[i];
EXPECT_EQ(expectation->expected, zigzagEncode(expectation->input));
}
}
TEST(EncodingTest, FloatToIntEncodingTest)
{
// given
floatToIntEncodingExpectation_t expectations[] = {
{0.0, 0x00000000},
{2.0, 0x40000000}, // Exponent should be in the top bits
{4.5, 0x40900000}
};
int expectationCount = sizeof(expectations) / sizeof(expectations[0]);
// expect
for (int i = 0; i < expectationCount; i++) {
floatToIntEncodingExpectation_t *expectation = &expectations[i];
EXPECT_EQ(expectation->expected, castFloatBytesToInt(expectation->input));
}
}
// STUBS
extern "C" {
}

128
src/test/unit/lowpass_unittest.cc Normal file
View file

@ -0,0 +1,128 @@
/*
* 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 <limits.h>
extern "C" {
#include "flight/lowpass.h"
}
static lowpass_t lowpassFilterReference;
static lowpass_t lowpassFilter;
#include "unittest_macros.h"
#include "gtest/gtest.h"
static float lowpassRef(lowpass_t *filter, float in, int16_t freq)
{
int16_t coefIdx;
float out;
// Check to see if cutoff frequency changed
if (freq != filter->freq) {
filter->init = false;
}
// Initialize if needed
if (!filter->init) {
generateLowpassCoeffs2(freq, filter);
for (coefIdx = 0; coefIdx < LOWPASS_NUM_COEF; coefIdx++) {
filter->xf[coefIdx] = in;
filter->yf[coefIdx] = in;
}
filter->init = true;
}
// Delays
for (coefIdx = LOWPASS_NUM_COEF-1; coefIdx > 0; coefIdx--) {
filter->xf[coefIdx] = filter->xf[coefIdx-1];
filter->yf[coefIdx] = filter->yf[coefIdx-1];
}
filter->xf[0] = in;
// Accumulate result
out = filter->xf[0] * filter->bf[0];
for (coefIdx = 1; coefIdx < LOWPASS_NUM_COEF; coefIdx++) {
out += filter->xf[coefIdx] * filter->bf[coefIdx];
out -= filter->yf[coefIdx] * filter->af[coefIdx];
}
filter->yf[0] = out;
return out;
}
TEST(LowpassTest, Lowpass)
{
int16_t servoCmds[3000];
int16_t expectedOut[3000];
int16_t referenceOut;
int16_t filterOut;
uint16_t sampleIdx;
int16_t freq;
uint16_t sampleCount = sizeof(servoCmds) / sizeof(int16_t);
// generate inputs and expecteds
for (sampleIdx = 0; sampleIdx < sampleCount; sampleIdx++) {
if (sampleIdx < 1000) {
servoCmds[sampleIdx] = 500;
} else if (sampleIdx >= 1000 && sampleIdx < 2000) {
servoCmds[sampleIdx] = 2500;
} else {
servoCmds[sampleIdx] = 1500;
}
if ((sampleIdx >= 900 && sampleIdx < 1000) ||
(sampleIdx >= 1900 && sampleIdx < 2000)||
(sampleIdx >= 2900 && sampleIdx < 3000)) {
expectedOut[sampleIdx] = servoCmds[sampleIdx];
} else {
expectedOut[sampleIdx] = -1;
}
}
// Test all frequencies
for (freq = 10; freq <= 400; freq++) {
printf("*** Testing freq: %d (%f)\n", freq, ((float)freq * 0.001f));
// Run tests
for (sampleIdx = 0; sampleIdx < sampleCount; sampleIdx++)
{
// Filter under test
filterOut = (int16_t)lowpassFixed(&lowpassFilter, servoCmds[sampleIdx], freq);
// Floating-point reference
referenceOut = (int16_t)(lowpassRef(&lowpassFilterReference, (float)servoCmds[sampleIdx], freq) + 0.5f);
if (expectedOut[sampleIdx] >= 0) {
EXPECT_EQ(filterOut, expectedOut[sampleIdx]);
}
// Some tolerance
// TODO adjust precision to remove the need for this?
EXPECT_LE(filterOut, referenceOut + 1);
EXPECT_GE(filterOut, referenceOut - 1);
} // for each sample
} // for each freq
}
// STUBS
extern "C" {
}

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

@ -22,6 +22,7 @@
#define GPS #define GPS
#define TELEMETRY #define TELEMETRY
#define LED_STRIP #define LED_STRIP
#define USE_SERVOS
#define SERIAL_PORT_COUNT 4 #define SERIAL_PORT_COUNT 4

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

@ -705,6 +705,9 @@ void mwArm(void) {}
void feature(uint32_t) {} void feature(uint32_t) {}
void sensors(uint32_t) {} void sensors(uint32_t) {}
void mwDisarm(void) {} void mwDisarm(void) {}
void displayDisablePageCycling() {}
void displayEnablePageCycling() {}
uint8_t getCurrentControlRateProfile(void) { uint8_t getCurrentControlRateProfile(void) {
return 0; return 0;
} }