diff --git a/Makefile b/Makefile index 742235d822..68f2ee1be5 100644 --- a/Makefile +++ b/Makefile @@ -199,6 +199,7 @@ COMMON_SRC = build_config.c \ common/maths.c \ common/printf.c \ common/typeconversion.c \ + common/encoding.c \ main.c \ mw.c \ flight/altitudehold.c \ @@ -206,6 +207,7 @@ COMMON_SRC = build_config.c \ flight/pid.c \ flight/imu.c \ flight/mixer.c \ + flight/lowpass.c \ drivers/bus_i2c_soft.c \ drivers/serial.c \ drivers/sound_beeper.c \ @@ -293,6 +295,8 @@ NAZE_SRC = startup_stm32f10x_md_gcc.S \ drivers/system_stm32f10x.c \ drivers/timer.c \ drivers/timer_stm32f10x.c \ + drivers/flash_m25p16.c \ + io/flashfs.c \ hardware_revision.c \ $(HIGHEND_SRC) \ $(COMMON_SRC) @@ -396,6 +400,8 @@ CJMCU_SRC = \ drivers/timer.c \ drivers/timer_stm32f10x.c \ hardware_revision.c \ + blackbox/blackbox.c \ + blackbox/blackbox_io.c \ $(COMMON_SRC) CC3D_SRC = \ @@ -499,6 +505,8 @@ SPRACINGF3_SRC = \ drivers/barometer_ms5611.c \ drivers/compass_hmc5883l.c \ drivers/display_ug2864hsweg01.h \ + drivers/flash_m25p16.c \ + io/flashfs.c \ $(HIGHEND_SRC) \ $(COMMON_SRC) diff --git a/docs/Blackbox.md b/docs/Blackbox.md index 05effe956a..e7f4b4f1d6 100644 --- a/docs/Blackbox.md +++ b/docs/Blackbox.md @@ -5,7 +5,7 @@ ## Introduction 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 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 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 -readings, raw VBAT measurements, and the command being sent to each motor speed controller. This is all stored without -any approximation or loss of precision, so even quite subtle problems should be detectable from the fight data log. +readings, raw VBAT and current measurements, and the command being sent to each motor speed controller. This is all +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. -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 -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 :). +GPS data is logged whenever new GPS data is available. Although the CSV decoder will decode this data, the video +renderer does not yet show any of the GPS information (this will be added later). ## Supported configurations -The maximum data rate for the flight log is fairly restricted, so anything that increases the load can cause the flight -log to drop frames and contain errors. +The maximum data rate that can be recorded to the flight log is fairly restricted, so anything that increases the load +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 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 -The blackbox software is designed to be used with an [OpenLog serial data logger][] and a microSDHC card. You need a -little prep to get the OpenLog ready for use, so here are the details: +There are two options for storing your flight logs. You can either transmit the log data over a serial port to an +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, 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 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 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. -#### 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 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%) - 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/ -### 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 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 ``` -## 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 -one of [your serial ports][] . 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. - -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 +A 115200 baud serial port is required to connect the OpenLog to your flight controller (such as `serial_port_1` on the +Naze32, the two-pin Tx/Rx header in the center of the board). The Blackbox can not be used with softserial ports as they +are too slow. 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. -### Protection +#### Protection 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 @@ -148,30 +120,93 @@ tubing instead. ![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 -The Blackbox starts recording data as soon as you arm your craft, and stops when you disarm. 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. +The Blackbox starts recording data as soon as you arm your craft, and stops when you disarm. -If your craft has a buzzer attached, a short beep will be played when you arm. You can later use this beep to -synchronize your recorded flight video with the rendered flight data log (the beep is shown as a blue line in the flight -data log, which you can sync against the beep in your recorded audio track). +If your craft has a buzzer attached, a short beep will be played when you arm and recording begins. You can later use +this beep to synchronize your recorded flight video with the rendered flight data log (the beep is shown as a blue line +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 -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 wait a few seconds after disarming your craft to allow the Blackbox to finish saving its data. -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. +### 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 -After your flights, you'll have a series of files labeled "LOG00001.TXT" etc. on the microSD card. You'll need to -decode these with the `blackbox_decode` tool to create a CSV (comma-separated values) file for analysis, or render them -into a series of PNG frames with `blackbox_render` tool, which you could then convert into a video using another -software package. +After your flights, you'll have a series of flight log files with a .TXT extension. You'll need to decode these with +the `blackbox_decode` tool to create CSV (comma-separated values) files for analysis, or render them into a series of PNG +frames with `blackbox_render` tool, which you could then convert into a video using another software package. You'll find those tools along with instructions for using them in this repository: diff --git a/docs/Cli.md b/docs/Cli.md index 93b5bf1820..f07eedbf9f 100644 --- a/docs/Cli.md +++ b/docs/Cli.md @@ -201,7 +201,7 @@ Re-apply any new defaults as desired. | failsafe_min_usec | | 100 | 2000 | 985 | 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 | -| 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 | | accxy_deadband | | 0 | 100 | 40 | Profile | UINT8 | | accz_deadband | | 0 | 100 | 40 | Profile | UINT8 | diff --git a/docs/Controls.md b/docs/Controls.md index 57e5578221..1fdf539ca1 100644 --- a/docs/Controls.md +++ b/docs/Controls.md @@ -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 Forwards | HIGH | CENTER | HIGH | 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 | diff --git a/docs/LedStrip.md b/docs/LedStrip.md index 07f08a94f5..df0373ea8f 100644 --- a/docs/LedStrip.md +++ b/docs/LedStrip.md @@ -23,14 +23,12 @@ Likewise, support for more than 32 LEDs is possible, it just requires additional ## 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. 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 -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. +Note: 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. @@ -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 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 -problem occurs because of the difference in voltage between the data signal and the power signal. +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 +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 @@ -411,7 +406,7 @@ led 27 2,9:S:FWT:0 ``` 16-18 9-11 19-21 \ / 6-8 - \ 13-16 / + \ 12-15 / \ FRONT / / 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. -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. diff --git a/docs/Mixer.md b/docs/Mixer.md index 777cae38d1..c662b4342a 100644 --- a/docs/Mixer.md +++ b/docs/Mixer.md @@ -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: -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. -The actual cutoff frequency is determined by the value of the `looptime` variable and the selected index. -The formula is: -`Frequency = 1000000 * (servo_lowpass_freq_idx + 1)*0.0025 / looptime )` - - -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. +The cutoff frequency can be determined by the following formula: +`Frequency = 1000 * servo_lowpass_freq / 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. diff --git a/docs/Screenshots/blackbox-dataflash.png b/docs/Screenshots/blackbox-dataflash.png new file mode 100644 index 0000000000..1f891c0c57 Binary files /dev/null and b/docs/Screenshots/blackbox-dataflash.png differ diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 18589d73d4..ae94762975 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -21,9 +21,12 @@ #include "platform.h" #include "version.h" +#ifdef BLACKBOX + #include "common/maths.h" #include "common/axis.h" #include "common/color.h" +#include "common/encoding.h" #include "drivers/gpio.h" #include "drivers/sensor.h" @@ -77,6 +80,7 @@ #include "blackbox_io.h" #define BLACKBOX_I_INTERVAL 32 +#define BLACKBOX_SHUTDOWN_TIMEOUT_MILLIS 200 #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): */ typedef struct blackboxFieldDefinition_t { 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]; } blackboxFieldDefinition_t; typedef struct blackboxMainFieldDefinition_t { const char *name; + int8_t fieldNameIndex; + uint8_t isSigned; uint8_t Ipredict; uint8_t Iencode; @@ -145,6 +155,8 @@ typedef struct blackboxMainFieldDefinition_t { typedef struct blackboxGPSFieldDefinition_t { const char *name; + int8_t fieldNameIndex; + uint8_t isSigned; uint8_t predict; uint8_t encode; @@ -159,73 +171,76 @@ typedef struct blackboxGPSFieldDefinition_t { */ static const blackboxMainFieldDefinition_t blackboxMainFields[] = { /* 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", 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[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)}, + {"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", 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)}, /* 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[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)}, - {"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[2]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(NONZERO_PID_D_2)}, + {"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", 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", 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)}, /* 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[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", 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", 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]: */ - {"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 - {"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[2]", 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", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_8SVB), FLIGHT_LOG_FIELD_CONDITION_MAG}, #endif #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 /* 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[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)}, - {"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[2]", 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", 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", 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)}, /* 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: */ - {"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[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[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[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)} + {"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", 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", 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", 7, UNSIGNED, .Ipredict = PREDICT(MOTOR_0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_MOTORS_8)}, + + /* Tricopter tail servo */ + {"servo", 5, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(TRICOPTER)} }; #ifdef GPS // GPS position/vel frame static const blackboxGPSFieldDefinition_t blackboxGpsGFields[] = { - {"time", UNSIGNED, PREDICT(LAST_MAIN_FRAME_TIME), ENCODING(UNSIGNED_VB), CONDITION(NOT_LOGGING_EVERY_FRAME)}, - {"GPS_numSat", UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB), CONDITION(ALWAYS)}, - {"GPS_coord[0]", SIGNED, PREDICT(HOME_COORD), ENCODING(SIGNED_VB), CONDITION(ALWAYS)}, - {"GPS_coord[1]", SIGNED, PREDICT(HOME_COORD), ENCODING(SIGNED_VB), CONDITION(ALWAYS)}, - {"GPS_altitude", UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB), CONDITION(ALWAYS)}, - {"GPS_speed", UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB), CONDITION(ALWAYS)}, - {"GPS_ground_course",UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB), CONDITION(ALWAYS)} + {"time", -1, UNSIGNED, PREDICT(LAST_MAIN_FRAME_TIME), ENCODING(UNSIGNED_VB), CONDITION(NOT_LOGGING_EVERY_FRAME)}, + {"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", 1, SIGNED, PREDICT(HOME_COORD), ENCODING(SIGNED_VB), CONDITION(ALWAYS)}, + {"GPS_altitude", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB), CONDITION(ALWAYS)}, + {"GPS_speed", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB), CONDITION(ALWAYS)}, + {"GPS_ground_course", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB), CONDITION(ALWAYS)} }; // GPS home frame static const blackboxGPSFieldDefinition_t blackboxGpsHFields[] = { - {"GPS_home[0]", SIGNED, PREDICT(0), ENCODING(SIGNED_VB), CONDITION(ALWAYS)}, - {"GPS_home[1]", 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)} }; #endif @@ -282,6 +297,7 @@ static uint32_t blackboxPFrameIndex, blackboxIFrameIndex; * to encode: */ static uint16_t vbatReference; + static gpsState_t gpsHistory; // 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_denom = 1; } 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); masterConfig.blackbox_rate_num /= 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) { if (blackboxState == BLACKBOX_STATE_STOPPED) { @@ -640,6 +666,9 @@ void startBlackbox(void) } } +/** + * Begin Blackbox shutdown. + */ void finishBlackbox(void) { if (blackboxState == BLACKBOX_STATE_RUNNING) { @@ -747,8 +776,10 @@ static void loadBlackboxState(void) blackboxCurrent->BaroAlt = BaroAlt; #endif +#ifdef USE_SERVOS //Tail servo for tricopters 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(headerNames[xmitState.headerIndex]); - charsWritten += blackboxPrint(":"); + blackboxWrite(':'); + charsWritten++; xmitState.u.fieldIndex++; needComma = false; @@ -808,6 +840,15 @@ static bool sendFieldDefinition(const char * const *headerNames, unsigned int he // The first header is a field name if (xmitState.headerIndex == 0) { 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 { //The other headers are integers if (def->arr[xmitState.headerIndex - 1] >= 10) { @@ -838,11 +879,6 @@ static bool sendFieldDefinition(const char * const *headerNames, unsigned int he */ static bool blackboxWriteSysinfo() { - union floatConvert_t { - float f; - uint32_t u; - } floatConvert; - if (xmitState.headerIndex == 0) { xmitState.u.serialBudget = 0; xmitState.headerIndex = 1; @@ -861,26 +897,24 @@ static bool blackboxWriteSysinfo() //Shouldn't ever get here break; case 1: - blackboxPrintf("H Firmware type:Cleanflight\n"); - - xmitState.u.serialBudget -= strlen("H Firmware type:Cleanflight\n"); + xmitState.u.serialBudget -= blackboxPrint("H Firmware type:Cleanflight\n"); break; case 2: 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 - * the placeholder "%s" - */ - xmitState.u.serialBudget -= strlen("H Firmware revision:%s\n") + strlen(shortGitRevision); + xmitState.u.serialBudget -= strlen("H Firmware revision:\n") + strlen(shortGitRevision); break; case 3: 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; case 4: 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"); break; case 5: @@ -899,10 +933,9 @@ static bool blackboxWriteSysinfo() xmitState.u.serialBudget -= strlen("H maxthrottle:%d\n"); break; case 8: - floatConvert.f = gyro.scale; - blackboxPrintf("H gyro.scale:0x%x\n", floatConvert.u); + blackboxPrintf("H gyro.scale:0x%x\n", castFloatBytesToInt(gyro.scale)); - xmitState.u.serialBudget -= strlen("H gyro.scale:0x%x\n") + 6; + xmitState.u.serialBudget -= strlen("H gyro.scale:0x\n") + 6; break; case 9: blackboxPrintf("H acc_1G:%u\n", acc_1G); @@ -1002,6 +1035,9 @@ static void blackboxPlaySyncBeep() blackboxLogEvent(FLIGHT_LOG_EVENT_SYNC_BEEP, (flightLogEventData_t *) &eventData); } +/** + * Call each flight loop iteration to perform blackbox logging. + */ void handleBlackbox(void) { int i; @@ -1074,6 +1110,9 @@ void handleBlackbox(void) loadBlackboxState(); writeIntraframe(); } 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) { loadBlackboxState(); writeInterframe(); @@ -1118,7 +1157,7 @@ void handleBlackbox(void) * * 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(); blackboxSetState(BLACKBOX_STATE_STOPPED); } @@ -1126,6 +1165,11 @@ void handleBlackbox(void) default: break; } + + // Did we run out of room on the device? Stop! + if (isBlackboxDeviceFull()) { + blackboxSetState(BLACKBOX_STATE_STOPPED); + } } static bool canUseBlackboxWithCurrentConfiguration(void) @@ -1133,6 +1177,9 @@ static bool canUseBlackboxWithCurrentConfiguration(void) return feature(FEATURE_BLACKBOX); } +/** + * Call during system startup to initialize the blackbox. + */ void initBlackbox(void) { if (canUseBlackboxWithCurrentConfiguration()) { @@ -1141,3 +1188,5 @@ void initBlackbox(void) blackboxSetState(BLACKBOX_STATE_DISABLED); } } + +#endif diff --git a/src/main/blackbox/blackbox_io.c b/src/main/blackbox/blackbox_io.c index cdcda94104..4f1fefb1df 100644 --- a/src/main/blackbox/blackbox_io.c +++ b/src/main/blackbox/blackbox_io.c @@ -1,14 +1,16 @@ #include #include +#include #include "blackbox_io.h" -#include "platform.h" #include "version.h" +#include "build_config.h" #include "common/maths.h" #include "common/axis.h" #include "common/color.h" +#include "common/encoding.h" #include "drivers/gpio.h" #include "drivers/sensor.h" @@ -56,6 +58,10 @@ #include "config/config_profile.h" #include "config/config_master.h" +#include "io/flashfs.h" + +#ifdef BLACKBOX + #define BLACKBOX_BAUDRATE 115200 #define BLACKBOX_INITIAL_PORT_MODE MODE_TX @@ -68,7 +74,17 @@ static portSharing_e blackboxPortSharing; 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) @@ -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 int blackboxPrint(const char *s) { - const char *pos = s; + int length; + const uint8_t *pos; - while (*pos) { - blackboxWrite(*pos); - pos++; + switch (masterConfig.blackbox_device) { + +#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) { //ZigZag encode to make the value always positive - blackboxWriteUnsignedVB((uint32_t)((value << 1) ^ (value >> 31))); + blackboxWriteUnsignedVB(zigzagEncode(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) { - 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 * 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); - 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) -{ - 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); + blackboxPort = openSerialPort(portConfig->identifier, FUNCTION_BLACKBOX, NULL, BLACKBOX_BAUDRATE, BLACKBOX_INITIAL_PORT_MODE, SERIAL_NOT_INVERTED); + return blackboxPort != NULL; + } + break; +#ifdef USE_FLASHFS + case BLACKBOX_DEVICE_FLASH: + if (flashfsGetSize() == 0 || isBlackboxDeviceFull()) { + return false; + } + + 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 diff --git a/src/main/blackbox/blackbox_io.h b/src/main/blackbox/blackbox_io.h index 5abacecfa5..547850739c 100644 --- a/src/main/blackbox/blackbox_io.h +++ b/src/main/blackbox/blackbox_io.h @@ -20,6 +20,18 @@ #include #include +#include "platform.h" + +typedef enum BlackboxDevice { + BLACKBOX_DEVICE_SERIAL = 0, + +#ifdef USE_FLASHFS + BLACKBOX_DEVICE_FLASH, +#endif + + BLACKBOX_DEVICE_END +} BlackboxDevice; + uint8_t blackboxWriteChunkSize; void blackboxWrite(uint8_t value); @@ -34,8 +46,8 @@ void blackboxWriteTag2_3S32(int32_t *values); void blackboxWriteTag8_4S16(int32_t *values); void blackboxWriteTag8_8SVB(int32_t *values, int valueCount); -void blackboxDeviceFlush(void); +bool blackboxDeviceFlush(void); bool blackboxDeviceOpen(void); void blackboxDeviceClose(void); -bool isBlackboxDeviceIdle(void); +bool isBlackboxDeviceFull(void); diff --git a/src/main/common/encoding.c b/src/main/common/encoding.c new file mode 100644 index 0000000000..3823e862a7 --- /dev/null +++ b/src/main/common/encoding.c @@ -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)); +} diff --git a/src/main/common/encoding.h b/src/main/common/encoding.h new file mode 100644 index 0000000000..e0681ed2a6 --- /dev/null +++ b/src/main/common/encoding.h @@ -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 . + */ + +#pragma once + +#include + +uint32_t castFloatBytesToInt(float f); +uint32_t zigzagEncode(int32_t value); diff --git a/src/main/config/config.c b/src/main/config/config.c index 870b025bfb..74185f061a 100644 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -74,9 +74,17 @@ #define BRUSHLESS_MOTORS_PWM_RATE 400 void setPIDController(int type); // FIXME PID code needs to be in flight_pid.c/h -void mixerUseConfigs(servoParam_t *servoConfToUse, flight3DConfig_t *flight3DConfigToUse, - escAndServoConfig_t *escAndServoConfigToUse, mixerConfig_t *mixerConfigToUse, - airplaneConfig_t *airplaneConfigToUse, rxConfig_t *rxConfig, gimbalConfig_t *gimbalConfigToUse); +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 *rxConfig +); void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, escAndServoConfig_t *escAndServoConfigToUse, pidProfile_t *pidProfileToUse); #define FLASH_TO_RESERVE_FOR_CONFIG 0x800 @@ -111,7 +119,7 @@ profile_t *currentProfile; static uint8_t currentControlRateProfileIndex = 0; 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) { @@ -310,7 +318,10 @@ static void setControlRateProfile(uint8_t profileIndex) static void resetConf(void) { 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 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_max_usec = 2115; // any of first 4 channels above this value will trigger failsafe +#ifdef USE_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].max = DEFAULT_SERVO_MAX; currentProfile->servoConf[i].middle = DEFAULT_SERVO_MIDDLE; @@ -436,9 +448,12 @@ static void resetConf(void) currentProfile->mixerConfig.yaw_direction = 1; currentProfile->mixerConfig.tri_unarmed_servo = 1; + currentProfile->mixerConfig.servo_lowpass_freq = 400; + currentProfile->mixerConfig.servo_lowpass_enable = 0; // gimbal currentProfile->gimbalConfig.gimbal_flags = GIMBAL_NORMAL; +#endif #ifdef GPS resetGpsProfile(¤tProfile->gpsProfile); @@ -454,6 +469,7 @@ static void resetConf(void) #endif #ifdef BLACKBOX + masterConfig.blackbox_device = 0; masterConfig.blackbox_rate_num = 1; masterConfig.blackbox_rate_denom = 1; #endif @@ -595,6 +611,8 @@ void activateConfig(void) activateControlRateConfig(); + resetAdjustmentStates(); + useRcControlsConfig( currentProfile->modeActivationConditions, &masterConfig.escAndServoConfig, @@ -618,13 +636,15 @@ void activateConfig(void) setAccelerationTrims(&masterConfig.accZero); mixerUseConfigs( +#ifdef USE_SERVOS currentProfile->servoConf, + ¤tProfile->gimbalConfig, +#endif &masterConfig.flight3DConfig, &masterConfig.escAndServoConfig, ¤tProfile->mixerConfig, &masterConfig.airplaneConfig, - &masterConfig.rxConfig, - ¤tProfile->gimbalConfig + &masterConfig.rxConfig ); imuRuntimeConfig.gyro_cmpf_factor = masterConfig.gyro_cmpf_factor; diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index b99770729b..79a093cf44 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -88,6 +88,7 @@ typedef struct master_t { #ifdef BLACKBOX uint8_t blackbox_rate_num; uint8_t blackbox_rate_denom; + uint8_t blackbox_device; #endif uint8_t magic_ef; // magic number, should be 0xEF diff --git a/src/main/config/config_profile.h b/src/main/config/config_profile.h index 7968b97c04..1d7d285b59 100644 --- a/src/main/config/config_profile.h +++ b/src/main/config/config_profile.h @@ -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 uint8_t throttle_correction_value; // the correction that will be applied at throttle_correction_angle. +#ifdef USE_SERVOS // Servo-related stuff servoParam_t servoConf[MAX_SUPPORTED_SERVOS]; // servo configuration + // gimbal-related configuration + gimbalConfig_t gimbalConfig; +#endif // Failsafe related configuration failsafeConfig_t failsafeConfig; @@ -56,9 +60,6 @@ typedef struct profile_s { // mixer-related configuration mixerConfig_t mixerConfig; - // gimbal-related configuration - gimbalConfig_t gimbalConfig; - #ifdef GPS gpsProfile_t gpsProfile; #endif diff --git a/src/main/drivers/accgyro_l3gd20.c b/src/main/drivers/accgyro_l3gd20.c index 8671ce8be2..03ad6b4bd1 100644 --- a/src/main/drivers/accgyro_l3gd20.c +++ b/src/main/drivers/accgyro_l3gd20.c @@ -68,39 +68,12 @@ extern int16_t debug[4]; #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) { GPIO_InitTypeDef GPIO_InitStructure; SPI_InitTypeDef SPI_InitStructure; - RCC_APB2PeriphClockCmd(RCC_APB2Periph_SPI1, 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); + RCC_AHBPeriphClockCmd(L3GD20_CS_GPIO_CLK_PERIPHERAL, ENABLE); GPIO_InitStructure.GPIO_Pin = L3GD20_CS_PIN; 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); - SPI_I2S_DeInit(SPIx); - - 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); + spiSetDivisor(L3GD20_SPI, SPI_9MHZ_CLOCK_DIVIDER); } void l3gd20GyroInit(void) { - l3gd20SpiInit(L3GD20_SPI); GPIO_ResetBits(L3GD20_CS_GPIO, L3GD20_CS_PIN); diff --git a/src/main/drivers/accgyro_l3gd20.h b/src/main/drivers/accgyro_l3gd20.h index b5c0fba694..0e1766dd54 100644 --- a/src/main/drivers/accgyro_l3gd20.h +++ b/src/main/drivers/accgyro_l3gd20.h @@ -17,10 +17,4 @@ #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); diff --git a/src/main/drivers/bus_spi.c b/src/main/drivers/bus_spi.c index 28fece232c..e381fc64e6 100644 --- a/src/main/drivers/bus_spi.c +++ b/src/main/drivers/bus_spi.c @@ -33,22 +33,56 @@ static volatile uint16_t spi3ErrorCount = 0; #endif #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) { // Specific to the STM32F103 // SPI1 Driver - // PA7 17 SPI1_MOSI - // PA6 16 SPI1_MISO - // PA5 15 SPI1_SCK // PA4 14 SPI1_NSS + // PA5 15 SPI1_SCK + // PA6 16 SPI1_MISO + // PA7 17 SPI1_MOSI - gpio_config_t gpio; SPI_InitTypeDef spi; // Enable SPI1 clock RCC_APB2PeriphClockCmd(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 gpio.mode = Mode_AF_PP; gpio.pin = Pin_7 | Pin_5; @@ -62,8 +96,9 @@ void initSpi1(void) gpio.pin = Pin_4; gpio.mode = Mode_Out_PP; gpioInit(GPIOA, &gpio); +#endif - // Init SPI2 hardware + // Init SPI hardware SPI_I2S_DeInit(SPI1); spi.SPI_Mode = SPI_Mode_Master; @@ -76,60 +111,116 @@ void initSpi1(void) spi.SPI_CPHA = SPI_CPHA_2Edge; 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_Cmd(SPI1, ENABLE); } #endif #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) { - - // Specific to the STM32F103 + // Specific to the STM32F103 / STM32F303 (AF5) // SPI2 Driver - // PB15 28 SPI2_MOSI - // PB14 27 SPI2_MISO - // PB13 26 SPI2_SCK // PB12 25 SPI2_NSS + // PB13 26 SPI2_SCK + // PB14 27 SPI2_MISO + // PB15 28 SPI2_MOSI - gpio_config_t gpio; SPI_InitTypeDef spi; // Enable SPI2 clock RCC_APB1PeriphClockCmd(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 gpio.mode = Mode_AF_PP; - gpio.pin = Pin_13 | Pin_15; + gpio.pin = SPI2_SCK_PIN | SPI2_MOSI_PIN; gpio.speed = Speed_50MHz; - gpioInit(GPIOB, &gpio); + gpioInit(SPI2_GPIO, &gpio); // MISO as input - gpio.pin = Pin_14; + gpio.pin = SPI2_MISO_PIN; gpio.mode = Mode_IN_FLOATING; - gpioInit(GPIOB, &gpio); - // NSS as gpio slave select - gpio.pin = Pin_12; - gpio.mode = Mode_Out_PP; - gpioInit(GPIOB, &gpio); + gpioInit(SPI2_GPIO, &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 SPI_I2S_DeInit(SPI2); - spi.SPI_Mode = SPI_Mode_Master; spi.SPI_Direction = SPI_Direction_2Lines_FullDuplex; + spi.SPI_Mode = SPI_Mode_Master; 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_CPHA = SPI_CPHA_2Edge; + spi.SPI_NSS = SPI_NSS_Soft; 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_Cmd(SPI2, ENABLE); + + // Drive NSS high to disable connected SPI device. + GPIO_SetBits(SPI2_GPIO, SPI2_NSS_PIN); + + } #endif @@ -198,7 +289,7 @@ uint8_t spiTransferByte(SPI_TypeDef *instance, uint8_t data) #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; @@ -210,8 +301,9 @@ bool spiTransfer(SPI_TypeDef *instance, uint8_t *out, uint8_t *in, int len) if ((spiTimeout--) == 0) return spiTimeoutUserCallback(instance); } -#ifdef STM32F303 - SPI_I2S_SendData16(instance, b); +#ifdef STM32F303xC + SPI_SendData8(instance, b); + //SPI_I2S_SendData16(instance, b); #endif #ifdef STM32F10X 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) return spiTimeoutUserCallback(instance); } -#ifdef STM32F303 - b = SPI_I2S_ReceiveData16(instance); +#ifdef STM32F303xC + b = SPI_ReceiveData8(instance); + //b = SPI_I2S_ReceiveData16(instance); #endif #ifdef STM32F10X b = SPI_I2S_ReceiveData(instance); diff --git a/src/main/drivers/bus_spi.h b/src/main/drivers/bus_spi.h index fd94abe09a..35750ab2ca 100644 --- a/src/main/drivers/bus_spi.h +++ b/src/main/drivers/bus_spi.h @@ -19,12 +19,13 @@ #define SPI_0_5625MHZ_CLOCK_DIVIDER 128 #define SPI_18MHZ_CLOCK_DIVIDER 2 +#define SPI_9MHZ_CLOCK_DIVIDER 4 bool spiInit(SPI_TypeDef *instance); void spiSetDivisor(SPI_TypeDef *instance, uint16_t divisor); 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); void spiResetErrorCounter(SPI_TypeDef *instance); diff --git a/src/main/drivers/flash.h b/src/main/drivers/flash.h new file mode 100644 index 0000000000..6ccba0d7ba --- /dev/null +++ b/src/main/drivers/flash.h @@ -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 . + */ + +#pragma once + +#include + +typedef struct flashGeometry_t { + uint8_t sectors; + + uint16_t pagesPerSector; + uint16_t pageSize; + + uint32_t sectorSize; + + uint32_t totalSize; +} flashGeometry_t; diff --git a/src/main/drivers/flash_m25p16.c b/src/main/drivers/flash_m25p16.c new file mode 100644 index 0000000000..a6bd7b22fa --- /dev/null +++ b/src/main/drivers/flash_m25p16.c @@ -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 . + */ + +#include +#include +#include + +#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; +} diff --git a/src/main/drivers/flash_m25p16.h b/src/main/drivers/flash_m25p16.h new file mode 100644 index 0000000000..79d7827159 --- /dev/null +++ b/src/main/drivers/flash_m25p16.h @@ -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 . + */ + +#pragma once + +#include +#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(); diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c index 4edd6cbfa5..6ac1e78475 100644 --- a/src/main/drivers/pwm_mapping.c +++ b/src/main/drivers/pwm_mapping.c @@ -419,6 +419,7 @@ pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init) if (type == MAP_TO_PPM_INPUT && !init->usePPM) continue; +#ifdef USE_SERVOS if (init->useServos && !init->airplane) { #if defined(NAZE) // remap PWM9+10 as servos @@ -455,6 +456,7 @@ pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init) if (timerIndex >= PWM5 && timerIndex <= PWM8) type = MAP_TO_SERVO_OUTPUT; } +#endif #ifdef CC3D if (init->useParallelPWM) { @@ -489,8 +491,10 @@ pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init) } pwmOutputConfiguration.motorCount++; } else if (type == MAP_TO_SERVO_OUTPUT) { +#ifdef USE_SERVOS pwmServoConfig(timerHardwarePtr, pwmOutputConfiguration.servoCount, init->servoPwmRate, init->servoCenterPulse); pwmOutputConfiguration.servoCount++; +#endif } } diff --git a/src/main/drivers/pwm_mapping.h b/src/main/drivers/pwm_mapping.h index a78a6e4fa1..ee92173609 100644 --- a/src/main/drivers/pwm_mapping.h +++ b/src/main/drivers/pwm_mapping.h @@ -49,14 +49,16 @@ typedef struct drv_pwm_config_t { bool useOneshot; bool useSoftSerial; bool useLEDStrip; +#ifdef USE_SERVOS bool useServos; 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 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), // some higher value (used by 3d mode), or 0, for brushed pwm drivers. - uint16_t servoCenterPulse; } drv_pwm_config_t; diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index fddf2a82b5..bd2c014fdd 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -43,8 +43,10 @@ typedef struct { static pwmOutputPort_t pwmOutputPorts[MAX_PWM_OUTPUT_PORTS]; 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 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) { 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; } +#ifdef USE_SERVOS 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); } + +void pwmWriteServo(uint8_t index, uint16_t value) +{ + if (servos[index] && index < MAX_SERVOS) + *servos[index]->ccr = value; +} +#endif diff --git a/src/main/flight/lowpass.c b/src/main/flight/lowpass.c new file mode 100755 index 0000000000..ea5f548d4c --- /dev/null +++ b/src/main/flight/lowpass.c @@ -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 . + */ + +#include +#include +#include +#include + +#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; +} + diff --git a/src/main/flight/lowpass.h b/src/main/flight/lowpass.h new file mode 100644 index 0000000000..bbd20f4b93 --- /dev/null +++ b/src/main/flight/lowpass.h @@ -0,0 +1,41 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#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); diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c old mode 100644 new mode 100755 index 9a66157d97..ff779782ed --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -26,6 +26,7 @@ #include "common/axis.h" #include "common/maths.h" +#include "drivers/system.h" #include "drivers/gpio.h" #include "drivers/timer.h" #include "drivers/pwm_output.h" @@ -45,6 +46,7 @@ #include "flight/mixer.h" #include "flight/pid.h" #include "flight/imu.h" +#include "flight/lowpass.h" #include "config/runtime_config.h" #include "config/config.h" @@ -54,26 +56,32 @@ #define AUX_FORWARD_CHANNEL_TO_SERVO_COUNT 4 +//#define MIXER_DEBUG + +extern int16_t debug[4]; + uint8_t motorCount = 0; int16_t motor[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 flight3DConfig_t *flight3DConfig; static escAndServoConfig_t *escAndServoConfig; static airplaneConfig_t *airplaneConfig; static rxConfig_t *rxConfig; -static gimbalConfig_t *gimbalConfig; static motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS]; 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[] = { { 1.0f, -1.0f, 1.0f, -1.0f }, // REAR_R { 1.0f, -1.0f, -1.0f, 1.0f }, // FRONT_R @@ -224,17 +232,31 @@ const mixer_t mixers[] = { }; #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; + gimbalConfig = gimbalConfigToUse; +#endif flight3DConfig = flight3DConfigToUse; escAndServoConfig = escAndServoConfigToUse; mixerConfig = mixerConfigToUse; airplaneConfig = airplaneConfigToUse; rxConfig = rxConfigToUse; - gimbalConfig = gimbalConfigToUse; } +#ifdef USE_SERVOS int16_t determineServoMiddleOrForwardFromChannel(int nr) { uint8_t channelToForwardFrom = servoConf[nr].forwardFromChannel; @@ -263,9 +285,7 @@ int servoDirection(int nr, int lr) else return 1; } - -static motorMixer_t *customMixers; - +#endif #ifndef USE_QUAD_MIXER_ONLY 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 (feature(FEATURE_SERVO_TILT)) 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) @@ -350,15 +375,15 @@ void mixerInit(mixerMode_e mixerMode, motorMixer_t *initialCustomMixers) currentMixerMode = mixerMode; customMixers = initialCustomMixers; - - useServo = false; } void mixerUsePWMOutputConfiguration(pwmOutputConfiguration_t *pwmOutputConfiguration) { UNUSED(pwmOutputConfiguration); motorCount = 4; +#ifdef USE_SERVOS servoCount = 0; +#endif uint8_t i; for (i = 0; i < motorCount; i++) { @@ -377,6 +402,7 @@ void mixerResetMotors(void) motor_disarmed[i] = feature(FEATURE_3D) ? flight3DConfig->neutral3d : escAndServoConfig->mincommand; } +#ifdef USE_SERVOS static void updateGimbalServos(void) { pwmWriteServo(0, servo[0]); @@ -437,6 +463,7 @@ void writeServos(void) break; } } +#endif void writeMotors(void) { @@ -461,6 +488,13 @@ void writeAllMotors(int16_t mc) writeMotors(); } +void stopMotors(void) +{ + writeAllMotors(escAndServoConfig->mincommand); + + delay(50); // give the timers and ESCs a chance to react. +} + #ifndef USE_QUAD_MIXER_ONLY static void airplaneMixer(void) { @@ -511,7 +545,6 @@ static void airplaneMixer(void) void mixTable(void) { - int16_t maxMotor; uint32_t i; if (motorCount > 3) { @@ -520,15 +553,17 @@ void mixTable(void) } // motors for non-servo mixes - if (motorCount > 1) - for (i = 0; i < motorCount; i++) + if (motorCount > 1) { + for (i = 0; i < motorCount; i++) { motor[i] = rcCommand[THROTTLE] * currentMixer[i].throttle + axisPID[PITCH] * currentMixer[i].pitch + axisPID[ROLL] * currentMixer[i].roll + -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 switch (currentMixerMode) { case MIXER_BI: @@ -586,7 +621,6 @@ void mixTable(void) default: break; } -#endif // do camstab if (feature(FEATURE_SERVO_TILT)) { @@ -626,37 +660,75 @@ void mixTable(void) pwmWriteServo(firstServo + servoOffset, rcData[channelOffset++]); } } +#endif - maxMotor = motor[0]; - for (i = 1; i < motorCount; i++) - if (motor[i] > maxMotor) - maxMotor = motor[i]; - 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; + if (ARMING_FLAG(ARMED)) { + + int16_t maxMotor = motor[0]; + for (i = 1; i < motorCount; i++) { + if (motor[i] > maxMotor) { + maxMotor = motor[i]; } } - 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]; } } } - +#ifdef USE_SERVOS bool isMixerUsingServos(void) { 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 +} + diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index f198fd11dd..627199af3b 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -65,7 +65,11 @@ typedef struct mixer_t { typedef struct mixerConfig_s { int8_t yaw_direction; +#ifdef USE_SERVOS 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; typedef struct flight3DConfig_s { @@ -82,6 +86,7 @@ typedef struct airplaneConfig_t { #define CHANNEL_FORWARDING_DISABLED (uint8_t)0xFF +#ifdef USE_SERVOS typedef struct servoParam_t { int16_t min; // servo min 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 } 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_disarmed[MAX_SUPPORTED_MOTORS]; -extern int16_t servo[MAX_SUPPORTED_SERVOS]; -bool isMixerUsingServos(void); void writeAllMotors(int16_t mc); void mixerLoadMix(int index, motorMixer_t *customMixers); void mixerResetMotors(void); void mixTable(void); -void writeServos(void); void writeMotors(void); +void stopMotors(void); diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index d123250484..0f95588745 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -539,8 +539,8 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) } 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 - gyroDataQuant = (float)tmp * 3.2f; // but delivers more accuracy and also reduces jittery flight + 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 rcCommandAxis = (float)rcCommand[axis]; // Calculate common values for pid controllers if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) { #ifdef GPS @@ -557,7 +557,7 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) PTermACC = error * (float)pidProfile->P8[PIDLEVEL] * 0.008f; float limitf = (float)pidProfile->D8[PIDLEVEL] * 5.0f; 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; } diff --git a/src/main/io/display.c b/src/main/io/display.c index 0d054f4ad9..65f94d2178 100644 --- a/src/main/io/display.c +++ b/src/main/io/display.c @@ -578,7 +578,12 @@ void displaySetNextPageChangeAt(uint32_t futureMicros) void displayEnablePageCycling(void) { pageState.pageFlags |= PAGE_STATE_FLAG_CYCLE_ENABLED; +} + +void displayResetPageCycling(void) +{ pageState.cycleIndex = CYCLE_PAGE_ID_COUNT - 1; // start at first page + } void displayDisablePageCycling(void) diff --git a/src/main/io/display.h b/src/main/io/display.h index 5791e3e0f2..19dee3fe81 100644 --- a/src/main/io/display.h +++ b/src/main/io/display.h @@ -35,4 +35,5 @@ void displayShowFixedPage(pageId_e pageId); void displayEnablePageCycling(void); void displayDisablePageCycling(void); +void displayResetPageCycling(void); void displaySetNextPageChangeAt(uint32_t futureMicros); diff --git a/src/main/io/flashfs.c b/src/main/io/flashfs.c new file mode 100644 index 0000000000..b271cec3ee --- /dev/null +++ b/src/main/io/flashfs.c @@ -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 . + */ + +/** + * 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 +#include +#include + +#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()); + } +} diff --git a/src/main/io/flashfs.h b/src/main/io/flashfs.h new file mode 100644 index 0000000000..4188b6f9d0 --- /dev/null +++ b/src/main/io/flashfs.h @@ -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 . + */ + +#pragma once + +#include + +#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(); diff --git a/src/main/io/rc_controls.c b/src/main/io/rc_controls.c index 54ecb963ba..e14683dc0e 100644 --- a/src/main/io/rc_controls.c +++ b/src/main/io/rc_controls.c @@ -47,6 +47,8 @@ #include "io/rc_controls.h" #include "io/rc_curves.h" +#include "io/display.h" + #include "flight/pid.h" #include "flight/navigation.h" @@ -247,6 +249,17 @@ void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStat rcDelayCommand = 0; // allow autorepetition 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) { @@ -580,3 +593,9 @@ void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, es } } } + +void resetAdjustmentStates(void) +{ + memset(adjustmentStates, 0, sizeof(adjustmentStates)); +} + diff --git a/src/main/io/rc_controls.h b/src/main/io/rc_controls.h index dde0536f06..a1549f5c5f 100644 --- a/src/main/io/rc_controls.h +++ b/src/main/io/rc_controls.h @@ -214,6 +214,7 @@ typedef struct adjustmentState_s { #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 updateAdjustmentStates(adjustmentRange_t *adjustmentRanges); void processRcAdjustments(controlRateConfig_t *controlRateConfig, rxConfig_t *rxConfig); diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index bc5d015e16..b883db0b85 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -52,6 +52,7 @@ #include "io/rc_controls.h" #include "io/serial.h" #include "io/ledstrip.h" +#include "io/flashfs.h" #include "rx/rx.h" #include "rx/spektrum.h" @@ -121,6 +122,13 @@ static void cliColor(char *cmdline); static void cliMixer(char *cmdline); #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 uint8_t cliMode = 0; @@ -155,13 +163,11 @@ static const char * const sensorTypeNames[] = { "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) static const char * const sensorHardwareNames[4][11] = { { "", "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", "HMC5883", "AK8975", NULL } }; @@ -185,6 +191,12 @@ const clicmd_t cmdTable[] = { { "dump", "dump configuration", cliDump }, { "exit", "", cliExit }, { "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 }, #ifdef GPS { "gpspassthrough", "passthrough gps to serial", cliGpsPassthrough }, @@ -201,7 +213,7 @@ const clicmd_t cmdTable[] = { { "profile", "index (0 to 2)", cliProfile }, { "rateprofile", "index (0 to 2)", cliRateProfile }, { "save", "save and reboot", cliSave }, -#ifndef CJMCU +#ifdef USE_SERVOS { "servo", "servo config", cliServo }, #endif { "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_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 }, - + { "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 }, { "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 }, @@ -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_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}, +#endif { "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 }, @@ -433,6 +450,7 @@ const clivalue_t valueTable[] = { #ifdef BLACKBOX { "blackbox_rate_num", VAR_UINT8 | MASTER_VALUE, &masterConfig.blackbox_rate_num, 1, 32 }, { "blackbox_rate_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.blackbox_rate_denom, 1, 32 }, + { "blackbox_device", VAR_UINT8 | MASTER_VALUE, &masterConfig.blackbox_device, 0, 1 }, #endif }; @@ -752,7 +770,7 @@ static void cliColor(char *cmdline) static void cliServo(char *cmdline) { -#ifdef CJMCU +#ifndef USE_SERVOS UNUSED(cmdline); #else enum { SERVO_ARGUMENT_COUNT = 6 }; @@ -822,6 +840,88 @@ static void cliServo(char *cmdline) #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) { uint32_t i; @@ -1238,6 +1338,7 @@ static void cliRateProfile(char *cmdline) static void cliReboot(void) { cliPrint("\r\nRebooting"); waitForSerialPortToFinishTransmitting(cliPort); + stopMotors(); systemReset(); } diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 9ff2fd42e8..32b51e4b6b 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -50,6 +50,7 @@ #include "io/gimbal.h" #include "io/serial.h" #include "io/ledstrip.h" +#include "io/flashfs.h" #include "telemetry/telemetry.h" @@ -232,6 +233,10 @@ const char *boardIdentifier = TARGET_BOARD_IDENTIFIER; // 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_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 // @@ -435,24 +440,24 @@ static uint32_t read32(void) return t; } -static void headSerialResponse(uint8_t err, uint8_t s) +static void headSerialResponse(uint8_t err, uint8_t responseBodySize) { serialize8('$'); serialize8('M'); serialize8(err ? '!' : '>'); currentPort->checksum = 0; // start calculating a new checksum - serialize8(s); + serialize8(responseBodySize); 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) @@ -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) { memset(mspPortToReset, 0, sizeof(mspPort_t)); @@ -783,6 +828,7 @@ static bool processOutCommand(uint8_t cmdMSP) for (i = 0; i < 3; i++) serialize16(magADC[i]); break; +#ifdef USE_SERVOS case MSP_SERVO: s_struct((uint8_t *)&servo, 16); break; @@ -801,6 +847,7 @@ static bool processOutCommand(uint8_t cmdMSP) serialize8(currentProfile->servoConf[i].forwardFromChannel); } break; +#endif case MSP_MOTOR: s_struct((uint8_t *)motor, 16); break; @@ -1136,6 +1183,21 @@ static bool processOutCommand(uint8_t cmdMSP) } break; #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: headSerialReply(11 + 4 + 4); for (i = 0; i < 11; i++) @@ -1304,6 +1366,7 @@ static bool processInCommand(void) motor_disarmed[i] = read16(); break; case MSP_SET_SERVO_CONF: +#ifdef USE_SERVOS for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) { currentProfile->servoConf[i].min = read16(); currentProfile->servoConf[i].max = read16(); @@ -1317,11 +1380,14 @@ static bool processInCommand(void) } currentProfile->servoConf[i].rate = read8(); } +#endif break; case MSP_SET_CHANNEL_FORWARDING: +#ifdef USE_SERVOS for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) { currentProfile->servoConf[i].forwardFromChannel = read8(); } +#endif break; case MSP_RESET_CONF: if (!ARMING_FLAG(ARMED)) { @@ -1345,6 +1411,13 @@ static bool processInCommand(void) writeEEPROM(); readEEPROM(); break; + +#ifdef USE_FLASHFS + case MSP_DATAFLASH_ERASE: + flashfsEraseCompletely(); + break; +#endif + #ifdef GPS case MSP_SET_RAW_GPS: if (read8()) { @@ -1609,6 +1682,7 @@ void mspProcess(void) while (!isSerialTransmitBufferEmpty(candidatePort->port)) { delay(50); } + stopMotors(); systemReset(); } } diff --git a/src/main/main.c b/src/main/main.c index 94bc2a1b51..47249d9071 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -46,10 +46,12 @@ #include "drivers/bus_i2c.h" #include "drivers/bus_spi.h" #include "drivers/inverter.h" +#include "drivers/flash_m25p16.h" #include "rx/rx.h" #include "io/serial.h" +#include "io/flashfs.h" #include "io/gps.h" #include "io/escservo.h" #include "io/rc_controls.h" @@ -127,11 +129,20 @@ void SetSysClock(void); void SetSysClock(bool overclock); #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) { uint8_t i; drv_pwm_config_t pwm_params; - bool sensorsOK = false; printfSupportInit(); @@ -140,6 +151,8 @@ void init(void) ensureEEPROMContainsValidData(); readEEPROM(); + systemState |= SYSTEM_STATE_CONFIG_LOADED; + #ifdef STM32F303 // start fpu SCB->CPACR = (0x3 << (10*2)) | (0x3 << (11*2)); @@ -199,18 +212,22 @@ void init(void) && masterConfig.batteryConfig.currentMeterType == CURRENT_SENSOR_ADC; pwm_params.useLEDStrip = feature(FEATURE_LED_STRIP); pwm_params.usePPM = feature(FEATURE_RX_PPM); - pwm_params.useOneshot = feature(FEATURE_ONESHOT125); pwm_params.useSerialRx = feature(FEATURE_RX_SERIAL); + +#ifdef USE_SERVOS pwm_params.useServos = isMixerUsingServos(); 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; +#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) if (feature(FEATURE_3D)) pwm_params.idlePulse = masterConfig.flight3DConfig.neutral3d; if (pwm_params.motorPwmRate > 500) pwm_params.idlePulse = 0; // brushed motors - pwm_params.servoCenterPulse = masterConfig.escAndServoConfig.servoCenterPulse; pwmRxInit(masterConfig.inputFilteringMode); @@ -218,6 +235,8 @@ void init(void) mixerUsePWMOutputConfiguration(pwmOutputConfiguration); + systemState |= SYSTEM_STATE_MOTORS_READY; + #ifdef BEEPER beeperConfig_t beeperConfig = { .gpioPin = BEEP_PIN, @@ -296,11 +315,12 @@ void init(void) } #endif - sensorsOK = 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 (!sensorsOK) + 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. failureMode(3); + } + + systemState |= SYSTEM_STATE_SENSORS_READY; LED1_ON; LED0_OFF; @@ -328,7 +348,9 @@ void init(void) cliInit(&masterConfig.serialConfig); failsafe = failsafeInit(&masterConfig.rxConfig); + beepcodeInit(failsafe); + rxInit(&masterConfig.rxConfig, failsafe); #ifdef GPS @@ -364,6 +386,18 @@ void init(void) } #endif +#ifdef USE_FLASHFS +#ifdef NAZE + if (hardwareRevision == NAZE32_REV5) { + m25p16_init(); + } +#endif +#ifdef SPRACINGF3 + m25p16_init(); +#endif + flashfsInit(); +#endif + #ifdef BLACKBOX initBlackbox(); #endif @@ -404,6 +438,7 @@ void init(void) #ifdef USE_OLED_GPS_DEBUG_PAGE_ONLY displayShowFixedPage(PAGE_GPS); #else + displayResetPageCycling(); displayEnablePageCycling(); #endif } @@ -412,6 +447,8 @@ void init(void) #ifdef CJMCU LED2_ON; #endif + + systemState |= SYSTEM_STATE_READY; } #ifdef SOFTSERIAL_LOOPBACK @@ -440,6 +477,9 @@ int main(void) { void HardFault_Handler(void) { // 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); } diff --git a/src/main/mw.c b/src/main/mw.c index dd3066aa87..12bbf377a2 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -725,7 +725,12 @@ void loop(void) ); mixTable(); + +#ifdef USE_SERVOS + filterServos(); writeServos(); +#endif + writeMotors(); #ifdef BLACKBOX diff --git a/src/main/sensors/acceleration.h b/src/main/sensors/acceleration.h index 5b925b01d4..198b12778a 100644 --- a/src/main/sensors/acceleration.h +++ b/src/main/sensors/acceleration.h @@ -20,15 +20,15 @@ // Type of accelerometer used/detected typedef enum { ACC_DEFAULT = 0, - ACC_ADXL345 = 1, - ACC_MPU6050 = 2, - ACC_MMA8452 = 3, - ACC_BMA280 = 4, - ACC_LSM303DLHC = 5, - ACC_SPI_MPU6000 = 6, - ACC_SPI_MPU6500 = 7, - ACC_FAKE = 8, - ACC_NONE = 9 + ACC_NONE = 1, + ACC_ADXL345 = 2, + ACC_MPU6050 = 3, + ACC_MMA8452 = 4, + ACC_BMA280 = 5, + ACC_LSM303DLHC = 6, + ACC_SPI_MPU6000 = 7, + ACC_SPI_MPU6500 = 8, + ACC_FAKE = 9, } accelerationSensor_e; extern sensor_align_e accAlign; diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c index cd90c28f36..d3bf32a0ef 100644 --- a/src/main/sensors/initialisation.c +++ b/src/main/sensors/initialisation.c @@ -110,7 +110,7 @@ const mpu6050Config_t *selectMPU6050Config(void) #ifdef USE_FAKE_GYRO static void fakeGyroInit(void) {} static void fakeGyroRead(int16_t *gyroData) { - UNUSED(gyroData); + memset(gyroData, 0, sizeof(int16_t[XYZ_AXIS_COUNT])); } static void fakeGyroReadTemp(int16_t *tempData) { UNUSED(tempData); @@ -129,7 +129,7 @@ bool fakeGyroDetect(gyro_t *gyro, uint16_t lpf) #ifdef USE_FAKE_ACC static void fakeAccInit(void) {} static void fakeAccRead(int16_t *accData) { - UNUSED(accData); + memset(accData, 0, sizeof(int16_t[XYZ_AXIS_COUNT])); } bool fakeAccDetect(acc_t *acc) @@ -267,14 +267,6 @@ retry: switch (accHardwareToUse) { case ACC_DEFAULT: ; // fallthrough - case ACC_FAKE: -#ifdef USE_FAKE_ACC - if (fakeAccDetect(&acc)) { - accHardware = ACC_FAKE; - break; - } -#endif - ; // fallthrough case ACC_ADXL345: // ADXL345 #ifdef USE_ACC_ADXL345 acc_params.useFifo = false; @@ -365,6 +357,14 @@ retry: accHardware = ACC_SPI_MPU6500; break; } +#endif + ; // fallthrough + case ACC_FAKE: +#ifdef USE_FAKE_ACC + if (fakeAccDetect(&acc)) { + accHardware = ACC_FAKE; + break; + } #endif ; // fallthrough case ACC_NONE: // disable ACC diff --git a/src/main/target/ALIENWIIF3/target.h b/src/main/target/ALIENWIIF3/target.h index fbd7ac2990..09fbe80a99 100644 --- a/src/main/target/ALIENWIIF3/target.h +++ b/src/main/target/ALIENWIIF3/target.h @@ -92,6 +92,7 @@ #define GPS //#define DISPLAY #define AUTOTUNE +#define USE_SERVOS #define SPEKTRUM_BIND diff --git a/src/main/target/CC3D/target.h b/src/main/target/CC3D/target.h index b0e1a2f7a2..ed0a7fca83 100644 --- a/src/main/target/CC3D/target.h +++ b/src/main/target/CC3D/target.h @@ -106,6 +106,7 @@ #define TELEMETRY #define SERIAL_RX #define AUTOTUNE +#define USE_SERVOS #define SPEKTRUM_BIND // USART3, PB11 (Flexport) diff --git a/src/main/target/CHEBUZZF3/target.h b/src/main/target/CHEBUZZF3/target.h index d8a0f704a5..4739829eaa 100644 --- a/src/main/target/CHEBUZZF3/target.h +++ b/src/main/target/CHEBUZZF3/target.h @@ -35,10 +35,18 @@ #define USABLE_TIMER_CHANNEL_COUNT 18 +#define USE_SPI +#define USE_SPI_DEVICE_1 + #define GYRO #define USE_GYRO_L3GD20 #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_MPU6050_ALIGN CW0_DEG @@ -114,3 +122,4 @@ #define TELEMETRY #define SERIAL_RX #define AUTOTUNE +#define USE_SERVOS diff --git a/src/main/target/CJMCU/target.h b/src/main/target/CJMCU/target.h index ac4088e083..71249f58cc 100644 --- a/src/main/target/CJMCU/target.h +++ b/src/main/target/CJMCU/target.h @@ -61,6 +61,8 @@ // #define SOFT_I2C_PB67 #define SERIAL_RX +//#define BLACKBOX +//#define USE_SERVOS #define SPEKTRUM_BIND // USART2, PA3 diff --git a/src/main/target/EUSTM32F103RC/target.h b/src/main/target/EUSTM32F103RC/target.h index 48ac2fd273..05b304ccd2 100644 --- a/src/main/target/EUSTM32F103RC/target.h +++ b/src/main/target/EUSTM32F103RC/target.h @@ -124,6 +124,7 @@ #define TELEMETRY #define SERIAL_RX #define AUTOTUNE +#define USE_SERVOS #define SPEKTRUM_BIND // USART2, PA3 diff --git a/src/main/target/NAZE/hardware_revision.c b/src/main/target/NAZE/hardware_revision.c index 3726c79e4b..bed18b72f1 100644 --- a/src/main/target/NAZE/hardware_revision.c +++ b/src/main/target/NAZE/hardware_revision.c @@ -58,7 +58,7 @@ void detectHardwareRevision(void) #define SPI_DEVICE_MPU (2) #define M25P16_INSTRUCTION_RDID 0x9F -#define FLASH_M25P16 (0x202015) +#define FLASH_M25P16_ID (0x202015) uint8_t detectSpiDevice(void) { @@ -74,7 +74,7 @@ uint8_t detectSpiDevice(void) DISABLE_SPI_CS; 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; // try autodetect MPU diff --git a/src/main/target/NAZE/target.h b/src/main/target/NAZE/target.h index 1e1fe772d8..c19d4f2583 100644 --- a/src/main/target/NAZE/target.h +++ b/src/main/target/NAZE/target.h @@ -56,10 +56,19 @@ #define NAZE_SPI_CS_GPIO GPIOB #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_PIN NAZE_SPI_CS_PIN #define MPU6500_SPI_INSTANCE NAZE_SPI_INSTANCE +#define USE_FLASHFS + +#define USE_FLASH_M25P16 + #define GYRO #define USE_GYRO_MPU3050 #define USE_GYRO_MPU6050 @@ -144,6 +153,7 @@ #define TELEMETRY #define SERIAL_RX #define AUTOTUNE +#define USE_SERVOS #define SPEKTRUM_BIND // USART2, PA3 diff --git a/src/main/target/NAZE32PRO/target.h b/src/main/target/NAZE32PRO/target.h index 7435a8e991..da072a3bdb 100644 --- a/src/main/target/NAZE32PRO/target.h +++ b/src/main/target/NAZE32PRO/target.h @@ -45,6 +45,7 @@ #define TELEMETRY #define SERIAL_RX #define AUTOTUNE +#define USE_SERVOS #define SPEKTRUM_BIND // USART2, PA3 diff --git a/src/main/target/OLIMEXINO/target.h b/src/main/target/OLIMEXINO/target.h index 6288c6b40b..963de3c1a9 100644 --- a/src/main/target/OLIMEXINO/target.h +++ b/src/main/target/OLIMEXINO/target.h @@ -111,3 +111,4 @@ #define SERIAL_RX #define AUTOTUNE #define BLACKBOX +#define USE_SERVOS diff --git a/src/main/target/PORT103R/target.h b/src/main/target/PORT103R/target.h index f3af089d0a..1ba71e90fd 100644 --- a/src/main/target/PORT103R/target.h +++ b/src/main/target/PORT103R/target.h @@ -135,4 +135,4 @@ #define TELEMETRY #define SERIAL_RX #define AUTOTUNE - +#define USE_SERVOS diff --git a/src/main/target/SPARKY/target.h b/src/main/target/SPARKY/target.h index 2c6101d7cf..3feb28a7e0 100644 --- a/src/main/target/SPARKY/target.h +++ b/src/main/target/SPARKY/target.h @@ -90,6 +90,7 @@ #define SERIAL_RX #define GPS #define DISPLAY +#define USE_SERVOS #define LED_STRIP #if 1 diff --git a/src/main/target/SPRACINGF3/target.h b/src/main/target/SPRACINGF3/target.h index 4af92665a3..0114f9c43b 100644 --- a/src/main/target/SPRACINGF3/target.h +++ b/src/main/target/SPRACINGF3/target.h @@ -45,6 +45,9 @@ #define USE_MAG_HMC5883 #define MAG_HMC5883_ALIGN CW270_DEG +#define USE_FLASHFS +#define USE_FLASH_M25P16 + #define BEEPER #define LED0 @@ -81,8 +84,23 @@ #define USE_I2C #define I2C_DEVICE (I2CDEV_1) // PB6/SCL, PB7/SDA -//#define USE_SPI -//#define USE_SPI_DEVICE_2 // PB12,13,14,15 on AF5 +#define USE_SPI +#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 @@ -126,3 +144,4 @@ #define SERIAL_RX #define AUTOTUNE #define DISPLAY +#define USE_SERVOS diff --git a/src/main/target/STM32F3DISCOVERY/target.h b/src/main/target/STM32F3DISCOVERY/target.h index a98f03d84e..b1d1c2cc02 100644 --- a/src/main/target/STM32F3DISCOVERY/target.h +++ b/src/main/target/STM32F3DISCOVERY/target.h @@ -36,9 +36,17 @@ #define BEEPER_INVERTED +#define USE_SPI +#define USE_SPI_DEVICE_1 + #define GYRO #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 ACC @@ -88,3 +96,4 @@ #define TELEMETRY #define SERIAL_RX #define AUTOTUNE +#define USE_SERVOS diff --git a/src/test/Makefile b/src/test/Makefile index 9f3c5db21e..6575c48a1a 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -52,7 +52,9 @@ TESTS = \ telemetry_hott_unittest \ rc_controls_unittest \ ledstrip_unittest \ - ws2811_unittest + ws2811_unittest \ + encoding_unittest \ + lowpass_unittest # All Google Test headers. Usually you shouldn't change this # definition. @@ -129,6 +131,25 @@ battery_unittest : \ $(OBJECT_DIR)/gtest_main.a $(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 : \ $(USER_DIR)/flight/imu.c \ @@ -319,6 +340,31 @@ ws2811_unittest : \ $(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) set -e && for test in $(TESTS) ; do \ $(OBJECT_DIR)/$$test; \ diff --git a/src/test/unit/encoding_unittest.cc b/src/test/unit/encoding_unittest.cc new file mode 100644 index 0000000000..bf2c70b916 --- /dev/null +++ b/src/test/unit/encoding_unittest.cc @@ -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 . + */ +#include + +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" { +} diff --git a/src/test/unit/lowpass_unittest.cc b/src/test/unit/lowpass_unittest.cc new file mode 100644 index 0000000000..1b07987b79 --- /dev/null +++ b/src/test/unit/lowpass_unittest.cc @@ -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 . + */ +#include +#include + +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" { + +} + + diff --git a/src/test/unit/platform.h b/src/test/unit/platform.h index f74b9be29d..3d0a20faf5 100644 --- a/src/test/unit/platform.h +++ b/src/test/unit/platform.h @@ -22,6 +22,7 @@ #define GPS #define TELEMETRY #define LED_STRIP +#define USE_SERVOS #define SERIAL_PORT_COUNT 4 diff --git a/src/test/unit/rc_controls_unittest.cc b/src/test/unit/rc_controls_unittest.cc index 0c3580f06e..eeb1b16ec7 100644 --- a/src/test/unit/rc_controls_unittest.cc +++ b/src/test/unit/rc_controls_unittest.cc @@ -705,6 +705,9 @@ void mwArm(void) {} void feature(uint32_t) {} void sensors(uint32_t) {} void mwDisarm(void) {} +void displayDisablePageCycling() {} +void displayEnablePageCycling() {} + uint8_t getCurrentControlRateProfile(void) { return 0; }