Compare commits

...

35 commits

Author SHA1 Message Date
Dominic Clifton 98f7549aa4 Fix serial command handling of telemetry baud rates. Set min blackbox
bad rate to be 19200 to match the confiurator.
2015-05-31 12:36:10 +01:00
Dominic Clifton ca22fb0ee3 Replace CLI serial port configuration code with a new serial command.
This is done to make it clearer in configuration dumps which ports are
enabled and configured.

There is a nice code size reduction due to less strings being needed.

Serial documentation updated.

Closes #966.
2015-05-31 11:50:58 +01:00
Dominic Clifton 27db9ad0fe Merge pull request #945 from inkonova/v1.8.1-mixerFix
removed multiple declaration of function mixerUseConfigs
2015-05-30 07:27:33 +01:00
Dominic Clifton 6f3c16fe6c Reducing the maximum number of mode ranges from 40 to 20 to free up some
configuration storage space.
2015-05-30 00:03:56 +01:00
Dominic Clifton e6733b4dfc Cleanup status indicator code.
This primarily is to avaoid stalling the main loop when beeping and
flashing.

This is needed because oneshot ESCs do not receive updates when the main
loop is stalled.

Additionally the beeper code for sticks held in disarm position is
changed since it also clashed with profile selection.  Now profile
selections can be seen and heard clearly.

Other subsections of the system that changed the LED0 state while the
main loop is running have been updated to use the status indicator API
instead of blindly hitting the hardware which previously caused lots of
odd LED flashing behaviour - now it is consistent.
2015-05-29 23:37:33 +01:00
Dominic Clifton 22a98af25a Removing unnecessary idlePulse parameter from pwmOneshotMotorConfig. 2015-05-29 20:36:43 +01:00
Dominic Clifton 0c1a6c5c2f Merge branch 'lock_active_features' of https://github.com/ProDrone/cleanflight into ProDrone-lock_active_features
Conflicts:
	src/test/unit/rc_controls_unittest.cc
2015-05-29 20:05:25 +01:00
Dominic Clifton 25fe9f73df Adding note about GCC version for unit tests in the Developer docs. 2015-05-29 19:53:21 +01:00
Dominic Clifton c29900f012 Fix compilation of tests on Cygwin. Tested with GCC 4.9.2. 2015-05-29 19:51:01 +01:00
Dominic Clifton 36c8b482d1 Re-order initialisers to be the same as the structure declation to avoid
GCC compilation error.

Error message is: "sorry, unimplemented: non-trivial designated
initializers not supported"
2015-05-29 19:49:01 +01:00
Dominic Clifton a54b073419 Re-enabling pthread for travis builds. 2015-05-29 19:16:22 +01:00
Dominic Clifton 41d737e29a Merge pull request #869 from SteveAmor/remove_tricopter_yaw_gyro_smoothing
Remove tricopter yaw gyro smoothing from imuUpdate
2015-05-29 15:39:02 +01:00
Dominic Clifton b194288668 Merge pull request #937 from MJ666/ALIENWII32_update
AlienWii32 updates
2015-05-29 15:30:38 +01:00
Dominic Clifton 704dcf46ba Fix all warnings when compiling unit tests.
Use C++11 for the C++ unit test code to make the tests easier to read.
Previously we were using a 17 year old standard (!) (g++98)

GoogleTest is compiled with some additional flags to avoid some specific
warnings.  Consider enabling the warnings when upgrading GoogleTest.

See https://code.google.com/p/googletest/issues/detail?id=433

Closes #941
2015-05-29 15:26:10 +01:00
Dominic Clifton 0df22f8049 Remove some test duplication in RcControls unit rest by using test
fixture classes.
2015-05-29 14:41:27 +01:00
Dominic Clifton dcc54a9aec Remove duplicate const keywords which were generating warnings when
compiling unit tests.
2015-05-29 14:23:12 +01:00
Dominic Clifton 55180070d6 Merge pull request #964 from nzmichaelh/more-const
various: optimise RAM and flash usage
2015-05-29 11:13:41 +01:00
Michael Hope c9349ce83b msp: rework the different size encoders.
Tidy the encoders up.  Reduce the code size stacking the encoders for
different sizes.

Signed-off-by: Michael Hope <mlhx@google.com>
2015-05-29 05:42:22 +02:00
Michael Hope 31f933ac21 various: mark more constant data so it goes into flash instead of RAM.
Signed-off-by: Michael Hope <mlhx@google.com>
2015-05-29 05:42:08 +02:00
Dominic Clifton a7d347689d Merge pull request #953 from enly1/Telemetry_Switch_Fix
Telemetry_Switch TELEMETRY_BOX conditional logic incorrect
2015-05-28 23:36:20 +01:00
Dominic Clifton 70cda73add Update port identifier for Naze32. Add basic documentation of the Paris
Air Hero 32 board.
2015-05-28 14:07:32 +01:00
Dominic Clifton c37454ca3a Add note explaining that the softserial feature must be enabled to allow
configuration of softserial ports.  Fix some typos.
2015-05-28 14:06:37 +01:00
Nicholas Sherlock 3ddea2a849 Fix blackbox baudrate upper bound on CLI
Closes #960
2015-05-28 14:27:49 +12:00
enly1 [Simon Love] 7df9f7d0d5 Telemetry_Switch TELEMETRY_BOX conditional logic incorrect - corrected so TELEMETRY_BOX now available in GUI with telemetry_switch=1 2015-05-26 20:43:05 +01:00
Mauro Mombelli 51a067db6d removed mixerUseConfigs multile declaration 2015-05-23 20:24:07 +02:00
Michael Jakob c5ce2ff8c6 Fix typo in documentation 2015-05-22 12:13:17 +02:00
Michael Jakob 196a0f7369 Fix documentation links 2015-05-22 11:53:42 +02:00
Michael Jakob e3e55c64eb AlienWii32 updates
Updated configuration defaults
Documentation updates AlienWii and Spektrum bind
2015-05-22 11:18:35 +02:00
ProDrone bbb86b30fa Delayed motor PWM enable when using Oneshot 2015-05-22 00:30:29 +02:00
ProDrone d1d73d933e Always stop motor PWM prior to soft restart 2015-05-21 16:02:49 +02:00
ProDrone 655bd574c2 Added missing stuff to unittests 2015-05-21 14:16:22 +02:00
ProDrone a153302b48 Minor comment update. 2015-05-21 02:03:09 +02:00
ProDrone b75de91f35 Instead of trying to latch the desired features...
...and apply them after a soft reset (which also required an additional
write to flash), it is now such that features and settings are modified
and stored in flash as before.

After initialisation completes, the active features are latched and are
not to be modified until the next startup. This guarantees that all
saved modifications are persistent even when power is switched of
(without a reset in between).

When a soft reset is required, the active features and the currently
configured features are used to detect if the oneshot feature has
changed state, in which case motor PWM outputs are stopped and soft
reset is done after a 1.5 second delay.

During normal operation the active features will not change and all
changes to features ordered via MSP commands or the CLI are applied to
the configuration that gets saved to flash.

The required effect of modifying features without changing the actions
in the running mainloop is achieved. The user needs to be aware that
changes to features are not applied immidiatly.
2015-05-21 01:27:45 +02:00
Steveis 6448b4b3d7 Replaced gyroData with gyroADC as they both contain the same value 2015-05-17 07:14:42 +01:00
Steveis 318592b063 Removed tricopter yaw gyro smoothing from imuUpdate 2015-05-10 11:02:44 +01:00
49 changed files with 593 additions and 428 deletions

View file

@ -1,13 +1,13 @@
# Board - AlienWii32 (ALIENWIIF1 and ALIENWIIF3 target)
The AlienWii32 is actually in prototype stage and only a few samples exist. There are two different variants and some more field testing with some users is ongoing. The information below is preliminary and will be updated as needed.
The AlienWii32 is actually in prototype stage and few samples exist. There are some different variants and field testing with some users is ongoing. The information below is preliminary and will be updated as needed.
Here are the hardware specifications:
- STM32F103CBT6 MCU (ALIENWIIF1)
- STM32F303CCT6 MCU (ALIENWIIF3)
- MPU6050 accelerometer/gyro sensor unit
- 8x 4.2A brushed ESCs, integrated, to run the strongest micro motors
- 4-8 x 4.2A brushed ESCs, integrated, to run the strongest micro motors
- extra-wide traces on the PCB, for maximum power throughput
- USB port, integrated
- (*) serial connection for external DSM2/DSMX sat receiver (e.g. Spektrum SAT, OrangeRx R100, Lemon RX or Deltang Rx31)
@ -16,11 +16,16 @@ Here are the hardware specifications:
- motor connections are at the corners for a clean look with reduced wiring
- dimensions: 29x33mm
- direct operation from an single cell lipoly battery
- 3.3V LDO power regulator (ALIENWIIF1 only)
- 3.3V buck-boost power converter (ALIENWIIF3 only)
- battery monitoring with an white LED for buzzer functionality (ALIENWIIF3 only)
- 3.3V LDO power regulator (older prototypes)
- 3.3V buck-boost power converter (newer prototypes and production versions)
- battery monitoring with an LED for buzzer functionality (actualy for an ALIENWIIF3 variant)
(*) Spektrum Compatible DSM2 satellites are supported out of the box. DSMX sat will work with DSM2 protocol with default settings (DSM2, 11bit, 11ms is preset).
(*) Spektrum Compatible DSM2 satellites are supported out of the box. DSMX sat will work with DSM2 protocol with default settings (DSM2, 11bit, 11ms is preset). This is chosen for maximum compatibility. For optimal connection it is recommended to adjust settings to match the capabilities of your transmitter and satellite receiver. If possible it is recommended to use the DSMX protocol since it is known as more reliable. Also to make use of additional channels you should adjust the following two parameters with the Cleanflight Configurator.
set serialrx_provider = 1 (0 for 1024bit, 1 for 2048bit)
set spektrum_sat_bind = 5
For more detail of the different bind modes please refer the [Spektrum Bind](Spektrum bind.md) document
Deltang receivers in serial mode will work like any other Spektrum satellite receiver (10bit, 22ms) only the bind process will be different.
@ -30,4 +35,4 @@ The pin layout for the ALIENWIIF1 is very similar to NAZE32 or the related clone
The AlienWii32 F1 board can be flashed like the Naze board or the related clones. All the different methods will work in the same way.
The AlienWii32 F3 board needs to be flashed via the USB port in DFU mode. Flashing via the Cleanflight GUI is not possible yet. The DFU mode can be activated via setting the BOOT0 jumper during power on of the board. The second method is to connect with an terminal program (i.e. Putty) to the board and enter the character "R" immediately after connecting. Details about the flashing process can be found in the related section of the Sparky documentation. The BOOT0 jumper should be removed and the board needs to be repowerd after firmware flashing. Please be aware, during reboot of the AlienWii F3 board, the GUI will disconnect and an manual reconnect is required.
The AlienWii32 F3 board needs to be flashed via the USB port in DFU mode. Flashing via the Cleanflight GUI is not possible yet. The DFU mode can be activated via setting the BOOT0 jumper during power on of the board. The second method is to connect with an terminal program (i.e. Putty) to the board and enter the character "R" immediately after connecting. Details about the flashing process can be found in the related section of the [Sparky](Board - Sparky.md) documentation. The BOOT0 jumper should be removed and the board needs to be repowerd after firmware flashing. Please be aware, during reboot of the AlienWii F3 board, the GUI will disconnect and an manual reconnect is required.

View file

@ -1,22 +1,22 @@
# Board - Naze32
The Naze32 target supports all Naze hardware revisions. Revison 4 and Revision 5 are used and
The Naze32 target supports all Naze hardware revisions. Revision 4 and 5 are used and
frequently flown by the primary maintainer. Previous Naze hardware revisions may have issues,
if found please report via the [github issue tracker](https://github.com/cleanflight/cleanflight/issues).
# Serial Ports
## Serial Ports
| Value | Identifier | RX | TX | Notes |
| ----- | ------------ | ---------- | ------------------ | ------------------------------------------------------------------------------------------- |
| 1 | USART1 | RX / PA10 | TX / PA9 / TELEM | TELEM output is always inverted (for FrSky). Internally connected to USB port via CP2102 IC |
| 2 | USART2 | RC4 / PA3 | RC3 / PA2 | |
| 3 | SOFTSERIAL1 | RC5 / PA6 | RC6 / PA7 | |
| 4 | SOFTSERIAL2 | RC7 / PB0 | RC8 / PB1 | |
| 4 | SOFTSERIAL1 | RC5 / PA6 | RC6 / PA7 | |
| 5 | SOFTSERIAL2 | RC7 / PB0 | RC8 / PB1 | |
* You cannot use USART1/TX/TELEM pins at the same time.
* You may encounter flashing problems if you have something connected to the RX/TX pins. Try disconnecting RX/TX.
# Pinouts
## Pinouts
The 10 pin RC I/O connector has the following pinouts when used in RX_PPM/RX_SERIAL mode.
@ -42,14 +42,14 @@ When SOFTSERIAL is enabled, LED_STRIP and CURRENT_METER are unavailable, but two
| 9 | 7 | SOFTSERIAL2 RX | |
| 10 | 8 | SOFTSERIAL2 TX | |
# Recovery
## Recovery
## Board
### Board
+ Short the two pads labelled 'Boot' **taking extra care not to touch the 5V pad**
+ Apply power to the board
+ Remove the short on the board
## Cleanflight configurator
### Cleanflight configurator
+ Select the correct hardware and the desired release of the Clearflight firmware
+ Put a check in the "No reboot sequence"
+ Flash firmware

View file

@ -0,0 +1,48 @@
# Board - Paris Air Hero 32 / Acro Naze 32 Mini
This board uses the same firmware as the Naze32 board.
## Sensors
MPU6500 via SPI interface.
## Ports
6 x 3pin ESC / Servo outputs
1 x 8pin JST connector (PPM/PWM/UART2)
1 x 4pin JST connector (UART3/I2C)
## Pinouts
The 10 pin RC I/O connector has the following pinouts when used in RX_PPM/RX_SERIAL mode.
From right to left when looking at the socket from the edge of the board.
| Pin | Function | Notes |
| --- | -------------- | -------------------------------- |
| 1 | Ground | |
| 2 | +5V | |
| 3 | RX_PPM | Enable `feature RX_PPM` |
| 4 | RSSI_ADC | Enable `feature RSSI_ADC`. Connect to the output of a PWM-RSSI conditioner, 0v-3.3v input |
| 5 | USART2 TX | |
| 6 | USART2 RX | Built-in inverter |
| 7 | LED_STRIP | Enable `feature LED_STRIP` |
| 8 | unused | |
When SOFTSERIAL is enabled, LED_STRIP and CURRENT_METER are unavailable, but one SoftSerial port is made available to use instead.
| Pin | Function | Notes |
| --- | -------------- | -------------------------------- |
| 7 | SOFTSERIAL1 RX | Enable `feature SOFTSERIAL` |
| 8 | SOFTSERIAL1 TX | |
## Serial Ports
| Value | Identifier | RX | TX | Notes |
| ----- | ------------ | ---------- | ------------------ | ------------------------------------------------------------------------------------------- |
| 1 | USART1 | RX / PA10 | TX / PA9 / TELEM | TELEM output is always inverted (for FrSky). Internally connected to USB port via CP2102 IC |
| 2 | USART2 | RC4 / PA3 | RC3 / PA2 | |
| 3 | USART3 | F3 / PB11 | F2 / PB10 | Flex port is configured as UART3 when port is configured |
| 4 | SOFTSERIAL1 | RC5 / PA6 | RC6 / PA7 | |

View file

@ -13,11 +13,11 @@ a dedicated USB to UART adapter. VCP does not 'use' a physical UART port.
* UART - A pair of dedicated hardware transmit and receive pins with signal detection and generation done in hardware.
* SoftSerial - A pair of hardware transmit and receive pins with signal detection and generation done in software.
UART is the most efficent in terms of CPU usage.
SoftSerial is the least efficient and slowest, SoftSerial should only be used for low-bandwith usages, such as telemetry transmission.
UART is the most efficient in terms of CPU usage.
SoftSerial is the least efficient and slowest, SoftSerial should only be used for low-bandwidth usages, such as telemetry transmission.
UART ports are sometimes exposed via on-board USB to UART converters, such as the CP2102 as found on the Naze and Flip32 boards.
If the flight controller does not have an onboard USB to UART converter and doesn't support VCP then an external USB to UART board is required.
If the flight controller does not have an on-board USB to UART converter and doesn't support VCP then an external USB to UART board is required.
These are sometimes referred to as FTDI boards. FTDI is just a common manufacturer of a chip (the FT232RL) used on USB to UART boards.
When selecting a USB to UART converter choose one that has DTR exposed as well as a selector for 3.3v and 5v since they are more useful.
@ -31,9 +31,9 @@ Both SoftSerial and UART ports can be connected to your computer via USB to UART
## Serial Configuration
Serial port configuration is best done via the configurator. You can use the CLI too but the commands are reserved for developers and advanced users.
Serial port configuration is best done via the configurator.
Configure serial ports first, then enable/disable features that use the ports.
Configure serial ports first, then enable/disable features that use the ports. To configure SoftSerial ports the SOFTSERIAL feature must be also be enabled.
### Constraints
@ -52,9 +52,23 @@ e.g. after configuring a port for GPS enable the GPS feature.
* You can use as many different telemetry systems as you like at the same time.
* You can only use each telemetry system once. e.g. FrSky telemetry cannot be used on two port, but MSP Telemetry + FrSky on different ports is fine.
### Configuration via CLI
You can use the CLI for configuration but the commands are reserved for developers and advanced users.
The `serial` CLI command takes 6 arguments.
1. Identifier
1. Function bitmask (see serialPortFunction_e in the source)
1. MSP baud rate
1. GPS baud rate
1. Telemetry baud rate (auto baud allowed)
1. Blackbox baud rate
### Baud Rates
Each baud rate is assigned an identifier, they are as follows:
The allowable baud rates are as follows:
| Identifier | Baud rate |
| ---------- | --------- |
@ -66,3 +80,4 @@ Each baud rate is assigned an identifier, they are as follows:
| 5 | 115200 |
| 6 | 230400 |
| 7 | 250000 |

View file

@ -34,12 +34,12 @@ Please refer to the satellite receiver documentation for more details of the spe
## Table with spektrum_sat_bind parameter value
| Value | Receiver mode | Notes |
| ----- | ---------------| -------------------|
| 3 | DSM2 1024/22ms | |
| 5 | DSM2 2048/11ms | default AlienWii32 |
| 7 | DSMX 22ms | |
| 9 | DSMX 11ms | |
| Value | Receiver mode | Notes |
| ----- | ------------------| -------------------|
| 3 | DSM2 1024bit/22ms | |
| 5 | DSM2 2048bit/11ms | default AlienWii32 |
| 7 | DSMX 1024bit/22ms | |
| 9 | DSMX 2048bit/11ms | |
More detailed information regarding the satellite binding process can be found here:
http://wiki.openpilot.org/display/Doc/Spektrum+Satellite
@ -50,15 +50,15 @@ NAZE, NAZE32PRO, CJMCU, SPARKY, EUSTM32F103RC, CC3D targets and ALIENWIIF1, ALIE
### Connecting a Spektrum-compatible satellite to a Flip32+ flight controller
The Flip32+ is wired in a rather strange way, i.e. the dedicated connector for the satellite module uses the same UART pins as the USB adapter. This means that you can't use that connector as it maps to UART1 which you really shouldn't assign to SERIAL_RX as that will break USB functionality.
The Flip32/Flip32+ is wired in a rather strange way, i.e. the dedicated connector for the satellite module uses the same UART pins as the USB adapter. This means that you can't use that connector as it maps to UART1 which you really shouldn't assign to SERIAL_RX as that will break USB functionality. (Looks this problem is fixed in later versions of the Flip32/Flip32+)
In order to connect the satellite to a Flip32+, you have to wire the serial data pin to RC_CH4. This is the fourth pin from the top in the left column of the 3x6 header on the right side of the board. GND and +3.3V may either be obtained from the dedicated SAT connector or from any ground pin and pin 1 of the BOOT connector which also provides 3.3V.
#### Tested satellite transmitter combinations
| Satellite | Remote | Remark |
| -------------------- | -------------- | ------------------------------------------ |
| Orange R100 | Spektrum DX6i | Bind value 3 |
| Lemon RX DSM2/DSMX | Spektrum DX8 | |
| Lemon RX DSMX | Walkera Devo10 | Deviation firmware 4.01 up to 12 channels |
| Lemon RX DSM2 | Walkera Devo7 | Deviation firmware |
| Satellite | Remote | Remark |
| -------------------- | -------------- | -------------------------------------------------------- |
| Orange R100 | Spektrum DX6i | Bind value 3 |
| Lemon RX DSM2/DSMX | Spektrum DX8 | Bind value 5 |
| Lemon RX DSMX | Walkera Devo10 | Bind value 9, Deviation firmware 4.01 up to 12 channels |
| Lemon RX DSM2 | Walkera Devo7 | Bind value 9, Deviation firmware |

View file

@ -61,6 +61,8 @@ You can also step-debug the tests in eclipse and you can use the GoogleTest test
The tests are currently always compiled with debugging information enabled, there may be additional warnings, if you see any warnings please attempt to fix them and submit pull requests with the fixes.
Tests are verified and working with GCC 4.9.2.
## Using git and github
Ensure you understand the github workflow: https://guides.github.com/introduction/flow/index.html

View file

@ -1,8 +1,8 @@
### IO variables
`gyroData/8192*2000 = deg/s`
`gyroADC/8192*2000 = deg/s`
`gyroData/4 ~ deg/s`
`gyroADC/4 ~ deg/s`
`rcCommand` - `<-500 - 500>` nominal, but is scaled with `rcRate/100`, max +-1250
@ -23,7 +23,7 @@ Iacc = intergrate(error, limit +-10000) * I8[PIDLEVEL] / 4096
#### Gyro term
```
Pgyro = rcCommand[axis];
error = rcCommand[axis] * 10 * 8 / pidProfile->P8[axis] - gyroData[axis] / 4; (conversion so that error is in deg/s ?)
error = rcCommand[axis] * 10 * 8 / pidProfile->P8[axis] - gyroADC[axis] / 4; (conversion so that error is in deg/s ?)
Igyro = integrate(error, limit +-16000) / 10 / 8 * I8[axis] / 100 (conversion back to mixer units ?)
```
@ -52,12 +52,12 @@ reset I term if
#### Gyro stabilization
```
P -= gyroData[axis] / 4 * dynP8 / 10 / 8
D = -mean(diff(gyroData[axis] / 4), over 3 samples) * 3 * dynD8 / 32
P -= gyroADC[axis] / 4 * dynP8 / 10 / 8
D = -mean(diff(gyroADC[axis] / 4), over 3 samples) * 3 * dynD8 / 32
[equivalent to :]
D = - (gyroData[axis]/4 - (<3 loops old>gyroData[axis]/4)) * dynD8 / 32
D = - (gyroADC[axis]/4 - (<3 loops old>gyroADC[axis]/4)) * dynD8 / 32
```
This can be seen as sum of
- PI controller (handles rcCommand, HORIZON/ANGLE); `Igyro` is only output based on gyroData
- PD controller(parameters dynP8/dynD8) with zero setpoint acting on gyroData
- PI controller (handles rcCommand, HORIZON/ANGLE); `Igyro` is only output based on gyroADC
- PD controller(parameters dynP8/dynD8) with zero setpoint acting on gyroADC

View file

@ -5,7 +5,7 @@
uint32_t SystemCoreClock = SYSCLK_FREQ_72MHz; /*!< System Clock Frequency (Core Clock) */
__I uint8_t AHBPrescTable[16] = { 0, 0, 0, 0, 0, 0, 0, 0, 1, 2, 3, 4, 6, 7, 8, 9 };
static const uint8_t AHBPrescTable[16] = { 0, 0, 0, 0, 0, 0, 0, 0, 1, 2, 3, 4, 6, 7, 8, 9 };
uint32_t hse_value = 8000000;

View file

@ -190,8 +190,8 @@
* @{
*/
static __I uint8_t APBAHBPrescTable[16] = {0, 0, 0, 0, 1, 2, 3, 4, 1, 2, 3, 4, 6, 7, 8, 9};
static __I uint8_t ADCPrescTable[4] = {2, 4, 6, 8};
static const uint8_t APBAHBPrescTable[16] = {0, 0, 0, 0, 1, 2, 3, 4, 1, 2, 3, 4, 6, 7, 8, 9};
static const uint8_t ADCPrescTable[4] = {2, 4, 6, 8};
extern uint32_t hse_value;
/**

View file

@ -202,9 +202,9 @@ static const blackboxMainFieldDefinition_t blackboxMainFields[] = {
#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)},
{"gyroADC", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
{"gyroADC", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
{"gyroADC", 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)},
@ -486,7 +486,7 @@ static void writeIntraframe(void)
#endif
for (x = 0; x < XYZ_AXIS_COUNT; x++) {
blackboxWriteSignedVB(blackboxCurrent->gyroData[x]);
blackboxWriteSignedVB(blackboxCurrent->gyroADC[x]);
}
for (x = 0; x < XYZ_AXIS_COUNT; x++) {
@ -602,7 +602,7 @@ static void writeInterframe(void)
//Since gyros, accs and motors are noisy, base the prediction on the average of the history:
for (x = 0; x < XYZ_AXIS_COUNT; x++) {
blackboxWriteSignedVB(blackboxHistory[0]->gyroData[x] - (blackboxHistory[1]->gyroData[x] + blackboxHistory[2]->gyroData[x]) / 2);
blackboxWriteSignedVB(blackboxHistory[0]->gyroADC[x] - (blackboxHistory[1]->gyroADC[x] + blackboxHistory[2]->gyroADC[x]) / 2);
}
for (x = 0; x < XYZ_AXIS_COUNT; x++) {
@ -781,7 +781,7 @@ static void loadBlackboxState(void)
}
for (i = 0; i < XYZ_AXIS_COUNT; i++) {
blackboxCurrent->gyroData[i] = gyroData[i];
blackboxCurrent->gyroADC[i] = gyroADC[i];
}
for (i = 0; i < XYZ_AXIS_COUNT; i++) {

View file

@ -29,7 +29,7 @@ typedef struct blackboxValues_t {
int32_t axisPID_P[XYZ_AXIS_COUNT], axisPID_I[XYZ_AXIS_COUNT], axisPID_D[XYZ_AXIS_COUNT];
int16_t rcCommand[4];
int16_t gyroData[XYZ_AXIS_COUNT];
int16_t gyroADC[XYZ_AXIS_COUNT];
int16_t accSmooth[XYZ_AXIS_COUNT];
int16_t motor[MAX_SUPPORTED_MOTORS];
int16_t servo[MAX_SUPPORTED_SERVOS];

View file

@ -44,7 +44,7 @@
#include "sensors/boardalignment.h"
#include "sensors/battery.h"
#include "io/statusindicator.h"
#include "io/beeper.h"
#include "io/serial.h"
#include "io/gimbal.h"
#include "io/escservo.h"
@ -73,17 +73,6 @@
#define BRUSHED_MOTORS_PWM_RATE 16000
#define BRUSHLESS_MOTORS_PWM_RATE 400
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
@ -134,11 +123,12 @@ void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, es
master_t masterConfig; // master config struct with data independent from profiles
profile_t *currentProfile;
static uint32_t activeFeaturesLatch = 0;
static uint8_t currentControlRateProfileIndex = 0;
controlRateConfig_t *currentControlRateProfile;
static const uint8_t EEPROM_CONF_VERSION = 100;
static const uint8_t EEPROM_CONF_VERSION = 101;
static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims)
{
@ -534,6 +524,7 @@ static void resetConf(void)
#else
masterConfig.serialConfig.portConfigs[1].functionMask = FUNCTION_RX_SERIAL;
#endif
masterConfig.rxConfig.serialrx_provider = 1;
masterConfig.rxConfig.spektrum_sat_bind = 5;
masterConfig.escAndServoConfig.minthrottle = 1000;
masterConfig.escAndServoConfig.maxthrottle = 2000;
@ -724,26 +715,26 @@ void activateConfig(void)
void validateAndFixConfig(void)
{
if (!(feature(FEATURE_RX_PARALLEL_PWM) || feature(FEATURE_RX_PPM) || feature(FEATURE_RX_SERIAL) || feature(FEATURE_RX_MSP))) {
if (!(featureConfigured(FEATURE_RX_PARALLEL_PWM) || featureConfigured(FEATURE_RX_PPM) || featureConfigured(FEATURE_RX_SERIAL) || featureConfigured(FEATURE_RX_MSP))) {
featureSet(FEATURE_RX_PARALLEL_PWM); // Consider changing the default to PPM
}
if (feature(FEATURE_RX_PPM)) {
if (featureConfigured(FEATURE_RX_PPM)) {
featureClear(FEATURE_RX_PARALLEL_PWM);
}
if (feature(FEATURE_RX_MSP)) {
if (featureConfigured(FEATURE_RX_MSP)) {
featureClear(FEATURE_RX_SERIAL);
featureClear(FEATURE_RX_PARALLEL_PWM);
featureClear(FEATURE_RX_PPM);
}
if (feature(FEATURE_RX_SERIAL)) {
if (featureConfigured(FEATURE_RX_SERIAL)) {
featureClear(FEATURE_RX_PARALLEL_PWM);
featureClear(FEATURE_RX_PPM);
}
if (feature(FEATURE_RX_PARALLEL_PWM)) {
if (featureConfigured(FEATURE_RX_PARALLEL_PWM)) {
#if defined(STM32F10X)
// rssi adc needs the same ports
featureClear(FEATURE_RSSI_ADC);
@ -764,7 +755,7 @@ void validateAndFixConfig(void)
#if defined(LED_STRIP) && (defined(USE_SOFTSERIAL1) || defined(USE_SOFTSERIAL2))
if (feature(FEATURE_SOFTSERIAL) && (
if (featureConfigured(FEATURE_SOFTSERIAL) && (
0
#ifdef USE_SOFTSERIAL1
|| (LED_STRIP_TIMER == SOFTSERIAL_1_TIMER)
@ -779,7 +770,7 @@ void validateAndFixConfig(void)
#endif
#if defined(NAZE) && defined(SONAR)
if (feature(FEATURE_RX_PARALLEL_PWM) && feature(FEATURE_SONAR) && feature(FEATURE_CURRENT_METER) && masterConfig.batteryConfig.currentMeterType == CURRENT_SENSOR_ADC) {
if (featureConfigured(FEATURE_RX_PARALLEL_PWM) && featureConfigured(FEATURE_SONAR) && featureConfigured(FEATURE_CURRENT_METER) && masterConfig.batteryConfig.currentMeterType == CURRENT_SENSOR_ADC) {
featureClear(FEATURE_CURRENT_METER);
}
#endif
@ -850,7 +841,7 @@ void readEEPROMAndNotify(void)
{
// re-read written data
readEEPROM();
blinkLedAndSoundBeeper(15, 20, 1);
beeperConfirmationBeeps(1);
}
void writeEEPROM(void)
@ -931,7 +922,7 @@ void changeProfile(uint8_t profileIndex)
masterConfig.current_profile_index = profileIndex;
writeEEPROM();
readEEPROM();
blinkLedAndSoundBeeper(2, 40, profileIndex + 1);
beeperConfirmationBeeps(profileIndex + 1);
}
void changeControlRateProfile(uint8_t profileIndex)
@ -943,11 +934,32 @@ void changeControlRateProfile(uint8_t profileIndex)
activateControlRateConfig();
}
bool feature(uint32_t mask)
void handleOneshotFeatureChangeOnRestart(void)
{
// Shutdown PWM on all motors prior to soft restart
StopPwmAllMotors();
delay(50);
// Apply additional delay when OneShot125 feature changed from on to off state
if (feature(FEATURE_ONESHOT125) && !featureConfigured(FEATURE_ONESHOT125)) {
delay(ONESHOT_FEATURE_CHANGED_DELAY_ON_BOOT_MS);
}
}
void latchActiveFeatures()
{
activeFeaturesLatch = masterConfig.enabledFeatures;
}
bool featureConfigured(uint32_t mask)
{
return masterConfig.enabledFeatures & mask;
}
bool feature(uint32_t mask)
{
return activeFeaturesLatch & mask;
}
void featureSet(uint32_t mask)
{
masterConfig.enabledFeatures |= mask;

View file

@ -19,6 +19,7 @@
#define MAX_PROFILE_COUNT 3
#define MAX_CONTROL_RATE_PROFILE_COUNT 3
#define ONESHOT_FEATURE_CHANGED_DELAY_ON_BOOT_MS 1500
typedef enum {
@ -44,6 +45,9 @@ typedef enum {
FEATURE_BLACKBOX = 1 << 19
} features_e;
void handleOneshotFeatureChangeOnRestart(void);
void latchActiveFeatures(void);
bool featureConfigured(uint32_t mask);
bool feature(uint32_t mask);
void featureSet(uint32_t mask);
void featureClear(uint32_t mask);

View file

@ -57,7 +57,7 @@
static uint8_t mpuLowPassFilter = L3G4200D_DLPF_32HZ;
static void l3g4200dInit(void);
static void l3g4200dRead(int16_t *gyroData);
static void l3g4200dRead(int16_t *gyroADC);
bool l3g4200dDetect(gyro_t *gyro, uint16_t lpf)
{
@ -110,13 +110,13 @@ static void l3g4200dInit(void)
}
// Read 3 gyro values into user-provided buffer. No overrun checking is done.
static void l3g4200dRead(int16_t *gyroData)
static void l3g4200dRead(int16_t *gyroADC)
{
uint8_t buf[6];
i2cRead(L3G4200D_ADDRESS, L3G4200D_AUTOINCR | L3G4200D_GYRO_OUT, 6, buf);
gyroData[X] = (int16_t)((buf[0] << 8) | buf[1]);
gyroData[Y] = (int16_t)((buf[2] << 8) | buf[3]);
gyroData[Z] = (int16_t)((buf[4] << 8) | buf[5]);
gyroADC[X] = (int16_t)((buf[0] << 8) | buf[1]);
gyroADC[Y] = (int16_t)((buf[2] << 8) | buf[3]);
gyroADC[Z] = (int16_t)((buf[4] << 8) | buf[5]);
}

View file

@ -120,7 +120,7 @@ void l3gd20GyroInit(void)
delay(100);
}
static void l3gd20GyroRead(int16_t *gyroData)
static void l3gd20GyroRead(int16_t *gyroADC)
{
uint8_t buf[6];
@ -134,9 +134,9 @@ static void l3gd20GyroRead(int16_t *gyroData)
GPIO_SetBits(L3GD20_CS_GPIO, L3GD20_CS_PIN);
gyroData[0] = (int16_t)((buf[0] << 8) | buf[1]);
gyroData[1] = (int16_t)((buf[2] << 8) | buf[3]);
gyroData[2] = (int16_t)((buf[4] << 8) | buf[5]);
gyroADC[0] = (int16_t)((buf[0] << 8) | buf[1]);
gyroADC[1] = (int16_t)((buf[2] << 8) | buf[3]);
gyroADC[2] = (int16_t)((buf[4] << 8) | buf[5]);
#if 0
debug[0] = (int16_t)((buf[1] << 8) | buf[0]);

View file

@ -106,7 +106,7 @@ void lsm303dlhcAccInit(void)
}
// Read 3 gyro values into user-provided buffer. No overrun checking is done.
static void lsm303dlhcAccRead(int16_t *gyroData)
static void lsm303dlhcAccRead(int16_t *gyroADC)
{
uint8_t buf[6];
@ -116,9 +116,9 @@ static void lsm303dlhcAccRead(int16_t *gyroData)
return;
// the values range from -8192 to +8191
gyroData[X] = (int16_t)((buf[1] << 8) | buf[0]) / 2;
gyroData[Y] = (int16_t)((buf[3] << 8) | buf[2]) / 2;
gyroData[Z] = (int16_t)((buf[5] << 8) | buf[4]) / 2;
gyroADC[X] = (int16_t)((buf[1] << 8) | buf[0]) / 2;
gyroADC[Y] = (int16_t)((buf[3] << 8) | buf[2]) / 2;
gyroADC[Z] = (int16_t)((buf[5] << 8) | buf[4]) / 2;
#if 0
debug[0] = (int16_t)((buf[1] << 8) | buf[0]);

View file

@ -58,7 +58,7 @@
static uint8_t mpuLowPassFilter = MPU3050_DLPF_42HZ;
static void mpu3050Init(void);
static void mpu3050Read(int16_t *gyroData);
static void mpu3050Read(int16_t *gyroADC);
static void mpu3050ReadTemp(int16_t *tempData);
bool mpu3050Detect(gyro_t *gyro, uint16_t lpf)
@ -121,15 +121,15 @@ static void mpu3050Init(void)
}
// Read 3 gyro values into user-provided buffer. No overrun checking is done.
static void mpu3050Read(int16_t *gyroData)
static void mpu3050Read(int16_t *gyroADC)
{
uint8_t buf[6];
i2cRead(MPU3050_ADDRESS, MPU3050_GYRO_OUT, 6, buf);
gyroData[0] = (int16_t)((buf[0] << 8) | buf[1]);
gyroData[1] = (int16_t)((buf[2] << 8) | buf[3]);
gyroData[2] = (int16_t)((buf[4] << 8) | buf[5]);
gyroADC[0] = (int16_t)((buf[0] << 8) | buf[1]);
gyroADC[1] = (int16_t)((buf[2] << 8) | buf[3]);
gyroADC[2] = (int16_t)((buf[4] << 8) | buf[5]);
}
static void mpu3050ReadTemp(int16_t *tempData)

View file

@ -175,7 +175,7 @@ static uint8_t mpuLowPassFilter = INV_FILTER_42HZ;
static void mpu6050AccInit(void);
static void mpu6050AccRead(int16_t *accData);
static void mpu6050GyroInit(void);
static void mpu6050GyroRead(int16_t *gyroData);
static void mpu6050GyroRead(int16_t *gyroADC);
typedef enum {
MPU_6050_HALF_RESOLUTION,
@ -439,7 +439,7 @@ static void mpu6050GyroInit(void)
#endif
}
static void mpu6050GyroRead(int16_t *gyroData)
static void mpu6050GyroRead(int16_t *gyroADC)
{
uint8_t buf[6];
@ -447,7 +447,7 @@ static void mpu6050GyroRead(int16_t *gyroData)
return;
}
gyroData[0] = (int16_t)((buf[0] << 8) | buf[1]);
gyroData[1] = (int16_t)((buf[2] << 8) | buf[3]);
gyroData[2] = (int16_t)((buf[4] << 8) | buf[5]);
gyroADC[0] = (int16_t)((buf[0] << 8) | buf[1]);
gyroADC[1] = (int16_t)((buf[2] << 8) | buf[3]);
gyroADC[2] = (int16_t)((buf[4] << 8) | buf[5]);
}

View file

@ -125,8 +125,8 @@ static bool mpuSpi6000InitDone = false;
#define DISABLE_MPU6000 GPIO_SetBits(MPU6000_CS_GPIO, MPU6000_CS_PIN)
#define ENABLE_MPU6000 GPIO_ResetBits(MPU6000_CS_GPIO, MPU6000_CS_PIN)
void mpu6000SpiGyroRead(int16_t *gyroData);
void mpu6000SpiAccRead(int16_t *gyroData);
void mpu6000SpiGyroRead(int16_t *gyroADC);
void mpu6000SpiAccRead(int16_t *gyroADC);
static void mpu6000WriteRegister(uint8_t reg, uint8_t data)
{

View file

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

View file

@ -74,7 +74,7 @@ static uint8_t mpuLowPassFilter = INV_FILTER_42HZ;
static void mpu6500AccInit(void);
static void mpu6500AccRead(int16_t *accData);
static void mpu6500GyroInit(void);
static void mpu6500GyroRead(int16_t *gyroData);
static void mpu6500GyroRead(int16_t *gyroADC);
extern uint16_t acc_1G;
@ -188,13 +188,13 @@ static void mpu6500GyroInit(void)
mpu6500WriteRegister(MPU6500_RA_RATE_DIV, 0); // 1kHz S/R
}
static void mpu6500GyroRead(int16_t *gyroData)
static void mpu6500GyroRead(int16_t *gyroADC)
{
uint8_t buf[6];
mpu6500ReadRegister(MPU6500_RA_GYRO_XOUT_H, buf, 6);
gyroData[X] = (int16_t)((buf[0] << 8) | buf[1]);
gyroData[Y] = (int16_t)((buf[2] << 8) | buf[3]);
gyroData[Z] = (int16_t)((buf[4] << 8) | buf[5]);
gyroADC[X] = (int16_t)((buf[0] << 8) | buf[1]);
gyroADC[Y] = (int16_t)((buf[2] << 8) | buf[3]);
gyroADC[Z] = (int16_t)((buf[4] << 8) | buf[5]);
}

View file

@ -31,7 +31,7 @@
void pwmBrushedMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse);
void pwmBrushlessMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse);
void pwmOneshotMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t idlePulse);
void pwmOneshotMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex);
void pwmServoConfig(const timerHardware_t *timerHardware, uint8_t servoIndex, uint16_t servoPwmRate, uint16_t servoCenterPulse);
/*
@ -543,7 +543,7 @@ pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init)
channelIndex++;
} else if (type == MAP_TO_MOTOR_OUTPUT) {
if (init->useOneshot) {
pwmOneshotMotorConfig(timerHardwarePtr, pwmOutputConfiguration.motorCount, init->idlePulse);
pwmOneshotMotorConfig(timerHardwarePtr, pwmOutputConfiguration.motorCount);
} else if (init->motorPwmRate > 500) {
pwmBrushedMotorConfig(timerHardwarePtr, pwmOutputConfiguration.motorCount, init->motorPwmRate, init->idlePulse);
} else {

View file

@ -142,6 +142,16 @@ void pwmWriteMotor(uint8_t index, uint16_t value)
motors[index]->pwmWritePtr(index, value);
}
void pwmShutdownPulsesForAllMotors(uint8_t motorCount)
{
uint8_t index;
for(index = 0; index < motorCount; index++){
// Set the compare register to 0, which stops the output pulsing if the timer overflows
*motors[index]->ccr = 0;
}
}
void pwmCompleteOneshotMotorUpdate(uint8_t motorCount)
{
uint8_t index;
@ -176,9 +186,9 @@ void pwmBrushlessMotorConfig(const timerHardware_t *timerHardware, uint8_t motor
motors[motorIndex]->pwmWritePtr = pwmWriteStandard;
}
void pwmOneshotMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t idlePulse)
void pwmOneshotMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex)
{
motors[motorIndex] = pwmOutConfig(timerHardware, ONESHOT125_TIMER_MHZ, 0xFFFF, idlePulse);
motors[motorIndex] = pwmOutConfig(timerHardware, ONESHOT125_TIMER_MHZ, 0xFFFF, 0);
motors[motorIndex]->pwmWritePtr = pwmWriteStandard;
}

View file

@ -18,6 +18,7 @@
#pragma once
void pwmWriteMotor(uint8_t index, uint16_t value);
void pwmShutdownPulsesForAllMotors(uint8_t motorCount);
void pwmCompleteOneshotMotorUpdate(uint8_t motorCount);
void pwmWriteServo(uint8_t index, uint16_t value);

View file

@ -48,7 +48,6 @@
int16_t accSmooth[XYZ_AXIS_COUNT];
int32_t accSum[XYZ_AXIS_COUNT];
int16_t gyroData[FLIGHT_DYNAMICS_INDEX_COUNT] = { 0, 0, 0 };
uint32_t accTimeSum = 0; // keep track for integration of acc
int accSumCount = 0;
@ -298,11 +297,10 @@ static void imuCalculateEstimatedAttitude(void)
imuCalculateAcceleration(deltaT); // rotate acc vector into earth frame
}
void imuUpdate(rollAndPitchTrims_t *accelerometerTrims, uint8_t mixerMode)
void imuUpdate(rollAndPitchTrims_t *accelerometerTrims)
{
static int16_t gyroYawSmooth = 0;
gyroUpdate();
if (sensors(SENSOR_ACC)) {
updateAccelerationReadings(accelerometerTrims); // TODO rename to accelerometerUpdate and rename many other 'Acceleration' references to be 'Accelerometer'
imuCalculateEstimatedAttitude();
@ -311,16 +309,6 @@ void imuUpdate(rollAndPitchTrims_t *accelerometerTrims, uint8_t mixerMode)
accADC[Y] = 0;
accADC[Z] = 0;
}
gyroData[FD_ROLL] = gyroADC[FD_ROLL];
gyroData[FD_PITCH] = gyroADC[FD_PITCH];
if (mixerMode == MIXER_TRI) {
gyroData[FD_YAW] = (gyroYawSmooth * 2 + gyroADC[FD_YAW]) / 3;
gyroYawSmooth = gyroData[FD_YAW];
} else {
gyroData[FD_YAW] = gyroADC[FD_YAW];
}
}
int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value)

View file

@ -24,7 +24,6 @@ extern float accVelScale;
extern t_fp_vector EstG;
extern int16_t accSmooth[XYZ_AXIS_COUNT];
extern int32_t accSum[XYZ_AXIS_COUNT];
extern int16_t gyroData[FLIGHT_DYNAMICS_INDEX_COUNT];
extern int16_t smallAngle;
typedef struct rollAndPitchInclination_s {
@ -62,7 +61,7 @@ void imuConfigure(
);
void calculateEstimatedAltitude(uint32_t currentTime);
void imuUpdate(rollAndPitchTrims_t *accelerometerTrims, uint8_t mixerMode);
void imuUpdate(rollAndPitchTrims_t *accelerometerTrims);
float calculateThrottleAngleScale(uint16_t throttle_correction_angle);
int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value);
float calculateAccZLowPassFilterRCTimeConstant(float accz_lpf_cutoff);

View file

@ -513,6 +513,11 @@ void stopMotors(void)
delay(50); // give the timers and ESCs a chance to react.
}
void StopPwmAllMotors()
{
pwmShutdownPulsesForAllMotors(motorCount);
}
#ifndef USE_QUAD_MIXER_ONLY
static void airplaneMixer(void)
{

View file

@ -100,6 +100,10 @@ typedef struct servoParam_t {
int8_t forwardFromChannel; // RX channel index, 0 based. See CHANNEL_FORWARDING_DISABLED
} servoParam_t;
struct gimbalConfig_s;
struct escAndServoConfig_s;
struct rxConfig_s;
extern int16_t servo[MAX_SUPPORTED_SERVOS];
bool isMixerUsingServos(void);
void writeServos(void);
@ -109,9 +113,21 @@ void filterServos(void);
extern int16_t motor[MAX_SUPPORTED_MOTORS];
extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
void mixerUseConfigs(
#ifdef USE_SERVOS
servoParam_t *servoConfToUse,
struct gimbalConfig_s *gimbalConfigToUse,
#endif
flight3DConfig_t *flight3DConfigToUse,
struct escAndServoConfig_s *escAndServoConfigToUse,
mixerConfig_t *mixerConfigToUse,
airplaneConfig_t *airplaneConfigToUse,
struct rxConfig_s *rxConfigToUse);
void writeAllMotors(int16_t mc);
void mixerLoadMix(int index, motorMixer_t *customMixers);
void mixerResetMotors(void);
void mixTable(void);
void writeMotors(void);
void stopMotors(void);
void StopPwmAllMotors(void);

View file

@ -174,7 +174,7 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
}
}
gyroRate = gyroData[axis] * gyro.scale; // gyro output scaled to dps
gyroRate = gyroADC[axis] * gyro.scale; // gyro output scaled to dps
// --------low-level gyro-based PID. ----------
// Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes
@ -258,12 +258,12 @@ static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
}
if (!FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE) || axis == FD_YAW) { // MODE relying on GYRO or YAW axis
error = (int32_t) rcCommand[axis] * 10 * 8 / pidProfile->P8[axis];
error -= gyroData[axis] / 4;
error -= gyroADC[axis] / 4;
PTermGYRO = rcCommand[axis];
errorGyroI[axis] = constrain(errorGyroI[axis] + error, -16000, +16000); // WindUp
if ((ABS(gyroData[axis]) > (640 * 4)) || (axis == FD_YAW && ABS(rcCommand[axis]) > 100))
if ((ABS(gyroADC[axis]) > (640 * 4)) || (axis == FD_YAW && ABS(rcCommand[axis]) > 100))
errorGyroI[axis] = 0;
ITermGYRO = (errorGyroI[axis] / 125 * pidProfile->I8[axis]) / 64;
@ -281,9 +281,9 @@ static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
}
}
PTerm -= ((int32_t)gyroData[axis] / 4) * dynP8[axis] / 10 / 8; // 32 bits is needed for calculation
delta = (gyroData[axis] - lastGyro[axis]) / 4;
lastGyro[axis] = gyroData[axis];
PTerm -= ((int32_t)gyroADC[axis] / 4) * dynP8[axis] / 10 / 8; // 32 bits is needed for calculation
delta = (gyroADC[axis] - lastGyro[axis]) / 4;
lastGyro[axis] = gyroADC[axis];
deltaSum = delta1[axis] + delta2[axis] + delta;
delta2[axis] = delta1[axis];
delta1[axis] = delta;
@ -319,10 +319,10 @@ static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *control
rc = rcCommand[axis] << 1;
error = rc - (gyroData[axis] / 4);
error = rc - (gyroADC[axis] / 4);
errorGyroI[axis] = constrain(errorGyroI[axis] + error, -16000, +16000); // WindUp 16 bits is ok here
if (ABS(gyroData[axis]) > (640 * 4)) {
if (ABS(gyroADC[axis]) > (640 * 4)) {
errorGyroI[axis] = 0;
}
@ -359,10 +359,10 @@ static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *control
PTerm = PTermACC + ((PTerm - PTermACC) * prop >> 9);
}
PTerm -= ((int32_t)(gyroData[axis] / 4) * dynP8[axis]) >> 6; // 32 bits is needed for calculation
PTerm -= ((int32_t)(gyroADC[axis] / 4) * dynP8[axis]) >> 6; // 32 bits is needed for calculation
delta = (gyroData[axis] - lastGyro[axis]) / 4; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
lastGyro[axis] = gyroData[axis];
delta = (gyroADC[axis] - lastGyro[axis]) / 4; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
lastGyro[axis] = gyroADC[axis];
DTerm = delta1[axis] + delta2[axis] + delta;
delta2[axis] = delta1[axis];
delta1[axis] = delta;
@ -381,9 +381,9 @@ static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *control
//YAW
rc = (int32_t)rcCommand[FD_YAW] * (2 * controlRateConfig->rates[FD_YAW] + 30) >> 5;
#ifdef ALIENWII32
error = rc - gyroData[FD_YAW];
error = rc - gyroADC[FD_YAW];
#else
error = rc - (gyroData[FD_YAW] / 4);
error = rc - (gyroADC[FD_YAW] / 4);
#endif
errorGyroI[FD_YAW] += (int32_t)error * pidProfile->I8[FD_YAW];
errorGyroI[FD_YAW] = constrain(errorGyroI[FD_YAW], 2 - ((int32_t)1 << 28), -2 + ((int32_t)1 << 28));
@ -450,12 +450,12 @@ static void pidMultiWiiHybrid(pidProfile_t *pidProfile, controlRateConfig_t *con
}
if (!FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) { // MODE relying on GYRO
error = (int32_t) rcCommand[axis] * 10 * 8 / pidProfile->P8[axis];
error -= gyroData[axis] / 4;
error -= gyroADC[axis] / 4;
PTermGYRO = rcCommand[axis];
errorGyroI[axis] = constrain(errorGyroI[axis] + error, -16000, +16000); // WindUp
if (ABS(gyroData[axis]) > (640 * 4))
if (ABS(gyroADC[axis]) > (640 * 4))
errorGyroI[axis] = 0;
ITermGYRO = (errorGyroI[axis] / 125 * pidProfile->I8[axis]) / 64;
@ -473,9 +473,9 @@ static void pidMultiWiiHybrid(pidProfile_t *pidProfile, controlRateConfig_t *con
}
}
PTerm -= ((int32_t)gyroData[axis] / 4) * dynP8[axis] / 10 / 8; // 32 bits is needed for calculation
delta = (gyroData[axis] - lastGyro[axis]) / 4;
lastGyro[axis] = gyroData[axis];
PTerm -= ((int32_t)gyroADC[axis] / 4) * dynP8[axis] / 10 / 8; // 32 bits is needed for calculation
delta = (gyroADC[axis] - lastGyro[axis]) / 4;
lastGyro[axis] = gyroADC[axis];
deltaSum = delta1[axis] + delta2[axis] + delta;
delta2[axis] = delta1[axis];
delta1[axis] = delta;
@ -491,9 +491,9 @@ static void pidMultiWiiHybrid(pidProfile_t *pidProfile, controlRateConfig_t *con
//YAW
rc = (int32_t)rcCommand[FD_YAW] * (2 * controlRateConfig->rates[FD_YAW] + 30) >> 5;
#ifdef ALIENWII32
error = rc - gyroData[FD_YAW];
error = rc - gyroADC[FD_YAW];
#else
error = rc - (gyroData[FD_YAW] / 4);
error = rc - (gyroADC[FD_YAW] / 4);
#endif
errorGyroI[FD_YAW] += (int32_t)error * pidProfile->I8[FD_YAW];
errorGyroI[FD_YAW] = constrain(errorGyroI[FD_YAW], 2 - ((int32_t)1 << 28), -2 + ((int32_t)1 << 28));
@ -523,7 +523,7 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
{
UNUSED(rxConfig);
float delta, RCfactor, rcCommandAxis, MainDptCut, gyroDataQuant;
float delta, RCfactor, rcCommandAxis, MainDptCut, gyroADCQuant;
float PTerm, ITerm, DTerm, PTermACC = 0.0f, ITermACC = 0.0f, ITermGYRO, error, prop = 0.0f;
static float lastGyro[2] = { 0.0f, 0.0f }, lastDTerm[2] = { 0.0f, 0.0f };
uint8_t axis;
@ -540,8 +540,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)gyroADC[axis] * 0.3125f); // Multiwii masks out the last 2 bits, this has the same idea
gyroADCQuant = (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
@ -563,10 +563,10 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
}
if (!FLIGHT_MODE(ANGLE_MODE)) {
if (ABS((int16_t)gyroData[axis]) > 2560) {
if (ABS((int16_t)gyroADC[axis]) > 2560) {
errorGyroIf[axis] = 0.0f;
} else {
error = (rcCommandAxis * 320.0f / (float)pidProfile->P8[axis]) - gyroDataQuant;
error = (rcCommandAxis * 320.0f / (float)pidProfile->P8[axis]) - gyroADCQuant;
errorGyroIf[axis] = constrainf(errorGyroIf[axis] + error * ACCDeltaTimeINS, -192.0f, +192.0f);
}
@ -584,10 +584,10 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
ITerm = ITermACC;
}
PTerm -= gyroDataQuant * dynP8[axis] * 0.003f;
delta = (gyroDataQuant - lastGyro[axis]) / ACCDeltaTimeINS;
PTerm -= gyroADCQuant * dynP8[axis] * 0.003f;
delta = (gyroADCQuant - lastGyro[axis]) / ACCDeltaTimeINS;
lastGyro[axis] = gyroDataQuant;
lastGyro[axis] = gyroADCQuant;
lastDTerm[axis] += RCfactor * (delta - lastDTerm[axis]);
DTerm = lastDTerm[axis] * dynD8[axis] * 0.00007f;
@ -605,7 +605,7 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
if (OLD_YAW) { // [0/1] 0 = multiwii 2.3 yaw, 1 = older yaw. hardcoded for now
PTerm = ((int32_t)pidProfile->P8[FD_YAW] * (100 - (int32_t)controlRateConfig->rates[FD_YAW] * (int32_t)ABS(rcCommand[FD_YAW]) / 500)) / 100;
int32_t tmp = lrintf(gyroData[FD_YAW] * 0.25f);
int32_t tmp = lrintf(gyroADC[FD_YAW] * 0.25f);
PTerm = rcCommand[FD_YAW] - tmp * PTerm / 80;
if ((ABS(tmp) > 640) || (ABS(rcCommand[FD_YAW]) > 100)) {
errorGyroI[FD_YAW] = 0;
@ -616,7 +616,7 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
}
} else {
int32_t tmp = ((int32_t)rcCommand[FD_YAW] * (((int32_t)controlRateConfig->rates[FD_YAW] << 1) + 40)) >> 5;
error = tmp - lrintf(gyroData[FD_YAW] * 0.25f); // Less Gyrojitter works actually better
error = tmp - lrintf(gyroADC[FD_YAW] * 0.25f); // Less Gyrojitter works actually better
if (ABS(tmp) > 50) {
errorGyroI[FD_YAW] = 0;
@ -716,7 +716,7 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
// Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes
// -----calculate scaled error.AngleRates
// multiplication of rcCommand corresponds to changing the sticks scaling here
RateError = AngleRateTmp - (gyroData[axis] / 4);
RateError = AngleRateTmp - (gyroADC[axis] / 4);
// -----calculate P component
PTerm = (RateError * pidProfile->P8[axis] * PIDweight[axis] / 100) >> 7;

View file

@ -30,6 +30,7 @@
#include "rx/rx.h"
#include "io/rc_controls.h"
#include "io/statusindicator.h"
#ifdef GPS
#include "io/gps.h"
@ -38,6 +39,7 @@
#include "config/runtime_config.h"
#include "config/config.h"
#include "io/beeper.h"
#if FLASH_SIZE > 64
@ -75,7 +77,7 @@ static const uint8_t beep_disarmBeep[] = {
};
// beeps while stick held in disarm position (after pause)
static const uint8_t beep_disarmRepeatBeep[] = {
0, 35, 40, 5, BEEPER_COMMAND_STOP
0, 100, 10, BEEPER_COMMAND_STOP
};
// Long beep and pause after that
static const uint8_t beep_lowBatteryBeep[] = {
@ -110,6 +112,11 @@ static const uint8_t beep_2shortBeeps[] = {
static const uint8_t beep_2longerBeeps[] = {
20, 15, 35, 5, BEEPER_COMMAND_STOP
};
// 3 beeps
static const uint8_t beep_gyroCalibrated[] = {
20, 10, 20, 10, 20, 10, BEEPER_COMMAND_STOP
};
// array used for variable # of beeps (reporting GPS sat count, etc)
static uint8_t beep_multiBeeps[MAX_MULTI_BEEPS + 2];
@ -145,21 +152,22 @@ typedef struct beeperTableEntry_s {
#endif
static const beeperTableEntry_t const beeperTable[] = {
{ BEEPER_ENTRY(BEEPER_RX_LOST_LANDING, 0, beep_sos, "RX_LOST_LANDING") },
{ BEEPER_ENTRY(BEEPER_RX_LOST, 1, beep_txLostBeep, "RX_LOST") },
{ BEEPER_ENTRY(BEEPER_DISARMING, 2, beep_disarmBeep, "DISARMING") },
{ BEEPER_ENTRY(BEEPER_ARMING, 3, beep_armingBeep, "ARMING") },
{ BEEPER_ENTRY(BEEPER_ARMING_GPS_FIX, 4, beep_armedGpsFix, "ARMING_GPS_FIX") },
{ BEEPER_ENTRY(BEEPER_BAT_CRIT_LOW, 5, beep_critBatteryBeep, "BAT_CRIT_LOW") },
{ BEEPER_ENTRY(BEEPER_BAT_LOW, 6, beep_lowBatteryBeep, "BAT_LOW") },
{ BEEPER_ENTRY(BEEPER_GPS_STATUS, 7, beep_multiBeeps, NULL) },
{ BEEPER_ENTRY(BEEPER_RX_SET, 8, beep_shortBeep, "RX_SET") },
{ BEEPER_ENTRY(BEEPER_DISARM_REPEAT, 9, beep_disarmRepeatBeep, "DISARM_REPEAT") },
{ BEEPER_ENTRY(BEEPER_GYRO_CALIBRATED, 0, beep_gyroCalibrated, "GYRO_CALIBRATED") },
{ BEEPER_ENTRY(BEEPER_RX_LOST_LANDING, 1, beep_sos, "RX_LOST_LANDING") },
{ BEEPER_ENTRY(BEEPER_RX_LOST, 2, beep_txLostBeep, "RX_LOST") },
{ BEEPER_ENTRY(BEEPER_DISARMING, 3, beep_disarmBeep, "DISARMING") },
{ BEEPER_ENTRY(BEEPER_ARMING, 4, beep_armingBeep, "ARMING") },
{ BEEPER_ENTRY(BEEPER_ARMING_GPS_FIX, 5, beep_armedGpsFix, "ARMING_GPS_FIX") },
{ BEEPER_ENTRY(BEEPER_BAT_CRIT_LOW, 6, beep_critBatteryBeep, "BAT_CRIT_LOW") },
{ BEEPER_ENTRY(BEEPER_BAT_LOW, 7, beep_lowBatteryBeep, "BAT_LOW") },
{ BEEPER_ENTRY(BEEPER_GPS_STATUS, 8, beep_multiBeeps, NULL) },
{ BEEPER_ENTRY(BEEPER_RX_SET, 9, beep_shortBeep, "RX_SET") },
{ BEEPER_ENTRY(BEEPER_ACC_CALIBRATION, 10, beep_2shortBeeps, "ACC_CALIBRATION") },
{ BEEPER_ENTRY(BEEPER_ACC_CALIBRATION_FAIL, 11, beep_2longerBeeps, "ACC_CALIBRATION_FAIL") },
{ BEEPER_ENTRY(BEEPER_READY_BEEP, 12, beep_readyBeep, "READY_BEEP") },
{ BEEPER_ENTRY(BEEPER_MULTI_BEEPS, 13, beep_multiBeeps, NULL) }, // FIXME having this listed makes no sense since the beep array will not be initialised.
{ BEEPER_ENTRY(BEEPER_ARMED, 14, beep_armedBeep, "ARMED") },
{ BEEPER_ENTRY(BEEPER_DISARM_REPEAT, 14, beep_disarmRepeatBeep, "DISARM_REPEAT") },
{ BEEPER_ENTRY(BEEPER_ARMED, 15, beep_armedBeep, "ARMED") },
};
static const beeperTableEntry_t *currentBeeperEntry = NULL;
@ -209,6 +217,10 @@ void beeper(beeperMode_e mode)
void beeperSilence(void)
{
BEEP_OFF;
warningLedDisable();
warningLedRefresh();
beeperIsOn = 0;
beeperNextToggleTime = 0;
@ -291,7 +303,9 @@ void beeperUpdate(void)
beeperIsOn = 1;
if (currentBeeperEntry->sequence[beeperPos] != 0) {
BEEP_ON;
//if this was arming beep then mark time (for blackbox)
warningLedEnable();
warningLedRefresh();
// if this was arming beep then mark time (for blackbox)
if (
beeperPos == 0
&& (currentBeeperEntry->mode == BEEPER_ARMING || currentBeeperEntry->mode == BEEPER_ARMING_GPS_FIX)
@ -303,6 +317,8 @@ void beeperUpdate(void)
beeperIsOn = 0;
if (currentBeeperEntry->sequence[beeperPos] != 0) {
BEEP_OFF;
warningLedDisable();
warningLedRefresh();
}
}
@ -348,7 +364,7 @@ beeperMode_e beeperModeForTableIndex(int idx)
*/
const char *beeperNameForTableIndex(int idx)
{
#ifndef BEEPER_NAME
#ifndef BEEPER_NAMES
UNUSED(idx);
return NULL;
#else

View file

@ -21,6 +21,7 @@ typedef enum {
// IMPORTANT: these are in priority order, 0 = Highest
BEEPER_SILENCE = 0, // Silence, see beeperSilence()
BEEPER_GYRO_CALIBRATED,
BEEPER_RX_LOST_LANDING, // Beeps SOS when armed and TX is turned off or signal lost (autolanding/autodisarm)
BEEPER_RX_LOST, // Beeps when TX is turned off or signal lost (repeat until TX is okay)
BEEPER_DISARMING, // Beep when disarming the board

View file

@ -83,7 +83,7 @@ static char lineBuffer[SCREEN_CHARACTER_COLUMN_COUNT + 1];
#define HALF_SCREEN_CHARACTER_COLUMN_COUNT (SCREEN_CHARACTER_COLUMN_COUNT / 2)
#define IS_SCREEN_CHARACTER_COLUMN_COUNT_ODD (SCREEN_CHARACTER_COLUMN_COUNT & 1)
const char* pageTitles[] = {
static const char* const pageTitles[] = {
"CLEANFLIGHT",
"ARMED",
"BATTERY",

View file

@ -140,23 +140,23 @@ void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStat
return;
}
if (isUsingSticksToArm) {
// Disarm on throttle down + yaw
if (isUsingSticksToArm) {
// Disarm on throttle down + yaw
if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_CE) {
if (ARMING_FLAG(ARMED)) //board was armed
if (ARMING_FLAG(ARMED))
mwDisarm();
else { //board was not armed
beeper(BEEPER_DISARM_REPEAT); //sound tone while stick held
rcDelayCommand = 0; //reset so disarm tone will repeat
else {
beeper(BEEPER_DISARM_REPEAT); // sound tone while stick held
rcDelayCommand = 0; // reset so disarm tone will repeat
}
}
// Disarm on roll (only when retarded_arm is enabled)
if (retarded_arm && (rcSticks == THR_LO + YAW_CE + PIT_CE + ROL_LO)) {
if (ARMING_FLAG(ARMED)) //board was armed
if (ARMING_FLAG(ARMED))
mwDisarm();
else { //board was not armed
beeper(BEEPER_DISARM_REPEAT); //sound tone while stick held
rcDelayCommand = 0; //reset so disarm tone will repeat
else {
beeper(BEEPER_DISARM_REPEAT); // sound tone while stick held
rcDelayCommand = 0; // reset so disarm tone will repeat
}
}
}

View file

@ -84,12 +84,7 @@ typedef enum {
#define THR_CE (3 << (2 * THROTTLE))
#define THR_HI (2 << (2 * THROTTLE))
#define MAX_MODE_ACTIVATION_CONDITION_COUNT 40
// 40 is enough for 1 mode for each position of 11 * 3 position switches and a 6 pos switch.
// however, that is unlikely because you don't define the 'off' positions, so for a 3 position
// switch it's normal that only 2 values would be configured.
// this leaves plenty of 'slots' free for cases where you enable multiple modes for a switch
// position (like gps rth + horizon + baro + beeper)
#define MAX_MODE_ACTIVATION_CONDITION_COUNT 20
#define CHANNEL_RANGE_MIN 900
#define CHANNEL_RANGE_MAX 2100

View file

@ -53,7 +53,7 @@
static serialConfig_t *serialConfig;
static serialPortUsage_t serialPortUsageList[SERIAL_PORT_COUNT];
const serialPortIdentifier_e const serialPortIdentifiers[SERIAL_PORT_COUNT] = {
const serialPortIdentifier_e serialPortIdentifiers[SERIAL_PORT_COUNT] = {
#ifdef USE_VCP
SERIAL_PORT_USB_VCP,
#endif
@ -76,7 +76,7 @@ const serialPortIdentifier_e const serialPortIdentifiers[SERIAL_PORT_COUNT] = {
static uint8_t serialPortCount;
const uint32_t const baudRates[] = {0, 9600, 19200, 38400, 57600, 115200, 230400, 250000}; // see baudRate_e
const uint32_t baudRates[] = {0, 9600, 19200, 38400, 57600, 115200, 230400, 250000}; // see baudRate_e
#define BAUD_RATE_COUNT (sizeof(baudRates) / sizeof(baudRates[0]))
@ -89,7 +89,7 @@ baudRate_e lookupBaudRateIndex(uint32_t baudRate)
return index;
}
}
return 0;
return BAUD_AUTO;
}
static serialPortUsage_t *findSerialPortUsageByIdentifier(serialPortIdentifier_e identifier)
@ -254,6 +254,13 @@ serialPort_t *openSerialPort(
portMode_t mode,
portOptions_t options)
{
#if (!defined(USE_VCP) && !defined(USE_USART1) && !defined(USE_USART2) && !defined(USE_USART3) && !defined(USE_SOFTSERIAL1) && !defined(USE_SOFTSERIAL1))
UNUSED(callback);
UNUSED(baudRate);
UNUSED(mode);
UNUSED(options);
#endif
serialPortUsage_t *serialPortUsage = findSerialPortUsageByIdentifier(identifier);
if (!serialPortUsage || serialPortUsage->function != FUNCTION_NONE) {
// not available / already in use

View file

@ -46,7 +46,7 @@ typedef enum {
BAUD_250000,
} baudRate_e;
extern const uint32_t const baudRates[];
extern const uint32_t baudRates[];
// serial port identifiers are now fixed, these values are used by MSP commands.
typedef enum {
@ -61,7 +61,7 @@ typedef enum {
SERIAL_PORT_IDENTIFIER_MAX = SERIAL_PORT_SOFTSERIAL2
} serialPortIdentifier_e;
extern const serialPortIdentifier_e const serialPortIdentifiers[SERIAL_PORT_COUNT];
extern const serialPortIdentifier_e serialPortIdentifiers[SERIAL_PORT_COUNT];
//
// runtime

View file

@ -109,6 +109,7 @@ static void cliProfile(char *cmdline);
static void cliRateProfile(char *cmdline);
static void cliReboot(void);
static void cliSave(char *cmdline);
static void cliSerial(char *cmdline);
static void cliServo(char *cmdline);
static void cliSet(char *cmdline);
static void cliGet(char *cmdline);
@ -220,6 +221,7 @@ const clicmd_t cmdTable[] = {
{ "profile", "index (0 to 2)", cliProfile },
{ "rateprofile", "index (0 to 2)", cliRateProfile },
{ "save", "save and reboot", cliSave },
{ "serial", "show/set serial settings", cliSerial },
#ifdef USE_SERVOS
{ "servo", "servo config", cliServo },
#endif
@ -287,40 +289,6 @@ const clivalue_t valueTable[] = {
{ "fixedwing_althold_dir", VAR_INT8 | MASTER_VALUE, &masterConfig.airplaneConfig.fixedwing_althold_dir, -1, 1 },
{ "serial_port_1_functions", VAR_UINT16 | MASTER_VALUE, &masterConfig.serialConfig.portConfigs[0].functionMask, 0, 0xFFFF },
{ "serial_port_1_msp_baudrate", VAR_UINT8 | MASTER_VALUE, &masterConfig.serialConfig.portConfigs[0].msp_baudrateIndex, BAUD_9600, BAUD_115200 },
{ "serial_port_1_telemetry_baudrate", VAR_UINT8 | MASTER_VALUE, &masterConfig.serialConfig.portConfigs[0].telemetry_baudrateIndex, BAUD_AUTO, BAUD_115200 },
{ "serial_port_1_blackbox_baudrate", VAR_UINT8 | MASTER_VALUE, &masterConfig.serialConfig.portConfigs[0].blackbox_baudrateIndex, BAUD_9600, BAUD_115200 },
{ "serial_port_1_gps_baudrate", VAR_UINT8 | MASTER_VALUE, &masterConfig.serialConfig.portConfigs[0].gps_baudrateIndex, BAUD_9600, BAUD_115200 },
#if (SERIAL_PORT_COUNT >= 2)
{ "serial_port_2_functions", VAR_UINT16 | MASTER_VALUE, &masterConfig.serialConfig.portConfigs[1].functionMask, 0, 0xFFFF },
{ "serial_port_2_msp_baudrate", VAR_UINT8 | MASTER_VALUE, &masterConfig.serialConfig.portConfigs[1].msp_baudrateIndex, BAUD_9600, BAUD_115200 },
{ "serial_port_2_telemetry_baudrate", VAR_UINT8 | MASTER_VALUE, &masterConfig.serialConfig.portConfigs[1].telemetry_baudrateIndex, BAUD_AUTO, BAUD_115200 },
{ "serial_port_2_blackbox_baudrate", VAR_UINT8 | MASTER_VALUE, &masterConfig.serialConfig.portConfigs[1].blackbox_baudrateIndex, BAUD_9600, BAUD_115200 },
{ "serial_port_2_gps_baudrate", VAR_UINT8 | MASTER_VALUE, &masterConfig.serialConfig.portConfigs[1].gps_baudrateIndex, BAUD_9600, BAUD_115200 },
#if (SERIAL_PORT_COUNT >= 3)
{ "serial_port_3_functions", VAR_UINT16 | MASTER_VALUE, &masterConfig.serialConfig.portConfigs[2].functionMask, 0, 0xFFFF},
{ "serial_port_3_msp_baudrate", VAR_UINT8 | MASTER_VALUE, &masterConfig.serialConfig.portConfigs[2].msp_baudrateIndex, BAUD_9600, BAUD_115200 },
{ "serial_port_3_telemetry_baudrate", VAR_UINT8 | MASTER_VALUE, &masterConfig.serialConfig.portConfigs[2].telemetry_baudrateIndex, BAUD_AUTO, BAUD_115200 },
{ "serial_port_3_blackbox_baudrate", VAR_UINT8 | MASTER_VALUE, &masterConfig.serialConfig.portConfigs[2].blackbox_baudrateIndex, BAUD_9600, BAUD_115200 },
{ "serial_port_3_gps_baudrate", VAR_UINT8 | MASTER_VALUE, &masterConfig.serialConfig.portConfigs[2].gps_baudrateIndex, BAUD_9600, BAUD_115200 },
#if (SERIAL_PORT_COUNT >= 4)
{ "serial_port_4_functions", VAR_UINT16 | MASTER_VALUE, &masterConfig.serialConfig.portConfigs[3].functionMask, 0, 0xFFFF },
{ "serial_port_4_msp_baudrate", VAR_UINT8 | MASTER_VALUE, &masterConfig.serialConfig.portConfigs[3].msp_baudrateIndex, BAUD_9600, BAUD_115200 },
{ "serial_port_4_telemetry_baudrate", VAR_UINT8 | MASTER_VALUE, &masterConfig.serialConfig.portConfigs[3].telemetry_baudrateIndex, BAUD_AUTO, BAUD_115200 },
{ "serial_port_4_blackbox_baudrate", VAR_UINT8 | MASTER_VALUE, &masterConfig.serialConfig.portConfigs[3].blackbox_baudrateIndex, BAUD_9600, BAUD_115200 },
{ "serial_port_4_gps_baudrate", VAR_UINT8 | MASTER_VALUE, &masterConfig.serialConfig.portConfigs[3].gps_baudrateIndex, BAUD_9600, BAUD_115200 },
#if (SERIAL_PORT_COUNT >= 5)
{ "serial_port_5_functions", VAR_UINT16 | MASTER_VALUE, &masterConfig.serialConfig.portConfigs[4].functionMask, 0, 0xFFFF },
{ "serial_port_5_msp_baudrate", VAR_UINT8 | MASTER_VALUE, &masterConfig.serialConfig.portConfigs[4].msp_baudrateIndex, BAUD_9600, BAUD_115200 },
{ "serial_port_5_telemetry_baudrate", VAR_UINT8 | MASTER_VALUE, &masterConfig.serialConfig.portConfigs[4].telemetry_baudrateIndex, BAUD_AUTO, BAUD_115200 },
{ "serial_port_5_blackbox_baudrate", VAR_UINT8 | MASTER_VALUE, &masterConfig.serialConfig.portConfigs[4].blackbox_baudrateIndex, BAUD_9600, BAUD_115200 },
{ "serial_port_5_gps_baudrate", VAR_UINT8 | MASTER_VALUE, &masterConfig.serialConfig.portConfigs[4].gps_baudrateIndex, BAUD_9600, BAUD_115200 },
#endif
#endif
#endif
#endif
{ "reboot_character", VAR_UINT8 | MASTER_VALUE, &masterConfig.serialConfig.reboot_character, 48, 126 },
#ifdef GPS
@ -591,6 +559,102 @@ static void cliAux(char *cmdline)
}
}
static void cliSerial(char *cmdline)
{
int i, val;
char *ptr;
if (isEmpty(cmdline)) {
for (i = 0; i < SERIAL_PORT_COUNT; i++) {
if (!serialIsPortAvailable(masterConfig.serialConfig.portConfigs[i].identifier)) {
continue;
};
printf("serial %d %d %ld %ld %ld %ld\r\n" ,
masterConfig.serialConfig.portConfigs[i].identifier,
masterConfig.serialConfig.portConfigs[i].functionMask,
baudRates[masterConfig.serialConfig.portConfigs[i].msp_baudrateIndex],
baudRates[masterConfig.serialConfig.portConfigs[i].gps_baudrateIndex],
baudRates[masterConfig.serialConfig.portConfigs[i].telemetry_baudrateIndex],
baudRates[masterConfig.serialConfig.portConfigs[i].blackbox_baudrateIndex]
);
}
return;
}
serialPortConfig_t portConfig;
memset(&portConfig, 0 , sizeof(portConfig));
serialPortConfig_t *currentConfig;
uint8_t validArgumentCount = 0;
ptr = cmdline;
val = atoi(ptr++);
currentConfig = serialFindPortConfiguration(val);
if (currentConfig) {
portConfig.identifier = val;
validArgumentCount++;
}
ptr = strchr(ptr, ' ');
if (ptr) {
val = atoi(++ptr);
portConfig.functionMask = val & 0xFFFF;
validArgumentCount++;
}
for (i = 0; i < 4; i ++) {
ptr = strchr(ptr, ' ');
if (!ptr) {
break;
}
val = atoi(++ptr);
uint8_t baudRateIndex = lookupBaudRateIndex(val);
if (baudRates[baudRateIndex] != (uint32_t) val) {
break;
}
switch(i) {
case 0:
if (baudRateIndex < BAUD_9600 || baudRateIndex > BAUD_115200) {
continue;
}
portConfig.msp_baudrateIndex = baudRateIndex;
break;
case 1:
if (baudRateIndex < BAUD_9600 || baudRateIndex > BAUD_115200) {
continue;
}
portConfig.gps_baudrateIndex = baudRateIndex;
break;
case 2:
if (baudRateIndex != BAUD_AUTO && baudRateIndex > BAUD_115200) {
continue;
}
portConfig.telemetry_baudrateIndex = baudRateIndex;
break;
case 3:
if (baudRateIndex < BAUD_19200 || baudRateIndex > BAUD_250000) {
continue;
}
portConfig.blackbox_baudrateIndex = baudRateIndex;
break;
}
validArgumentCount++;
}
if (validArgumentCount < 6) {
cliPrint("Parse error\r\n");
return;
}
memcpy(currentConfig, &portConfig, sizeof(portConfig));
}
static void cliAdjustmentRange(char *cmdline)
{
@ -1066,6 +1130,9 @@ static void cliDump(char *cmdline)
buf[i] = '\0';
printf("map %s\r\n", buf);
cliPrint("\r\n\r\n# serial\r\n");
cliSerial("");
#ifdef LED_STRIP
cliPrint("\r\n\r\n# led\r\n");
cliLed("");
@ -1410,6 +1477,7 @@ static void cliReboot(void) {
cliPrint("\r\nRebooting");
waitForSerialPortToFinishTransmitting(cliPort);
stopMotors();
handleOneshotFeatureChangeOnRestart();
systemReset();
}

View file

@ -140,12 +140,12 @@ void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, es
#define BASEFLIGHT_IDENTIFIER "BAFL";
#define FLIGHT_CONTROLLER_IDENTIFIER_LENGTH 4
static const char *flightControllerIdentifier = CLEANFLIGHT_IDENTIFIER; // 4 UPPER CASE alpha numeric characters that identify the flight controller.
static const char * const flightControllerIdentifier = CLEANFLIGHT_IDENTIFIER; // 4 UPPER CASE alpha numeric characters that identify the flight controller.
#define FLIGHT_CONTROLLER_VERSION_LENGTH 3
#define FLIGHT_CONTROLLER_VERSION_MASK 0xFFF
const char *boardIdentifier = TARGET_BOARD_IDENTIFIER;
static const char * const boardIdentifier = TARGET_BOARD_IDENTIFIER;
#define BOARD_IDENTIFIER_LENGTH 4 // 4 UPPER CASE alpha numeric characters that identify the board being used.
#define BOARD_HARDWARE_REVISION_LENGTH 2
@ -396,40 +396,24 @@ static mspPort_t mspPorts[MAX_MSP_PORT_COUNT];
static mspPort_t *currentPort;
static void serialize32(uint32_t a)
{
static uint8_t t;
t = a;
serialWrite(mspSerialPort, t);
currentPort->checksum ^= t;
t = a >> 8;
serialWrite(mspSerialPort, t);
currentPort->checksum ^= t;
t = a >> 16;
serialWrite(mspSerialPort, t);
currentPort->checksum ^= t;
t = a >> 24;
serialWrite(mspSerialPort, t);
currentPort->checksum ^= t;
}
static void serialize16(int16_t a)
{
static uint8_t t;
t = a;
serialWrite(mspSerialPort, t);
currentPort->checksum ^= t;
t = a >> 8 & 0xff;
serialWrite(mspSerialPort, t);
currentPort->checksum ^= t;
}
static void serialize8(uint8_t a)
{
serialWrite(mspSerialPort, a);
currentPort->checksum ^= a;
}
static void serialize16(uint16_t a)
{
serialize8((uint8_t)(a >> 0));
serialize8((uint8_t)(a >> 8));
}
static void serialize32(uint32_t a)
{
serialize16((uint16_t)(a >> 0));
serialize16((uint16_t)(a >> 16));
}
static uint8_t read8(void)
{
return currentPort->inBuf[currentPort->indRX++] & 0xff;
@ -682,7 +666,7 @@ void mspInit(serialConfig_t *serialConfig)
activeBoxIds[activeBoxIdCount++] = BOXOSD;
if (feature(FEATURE_TELEMETRY && masterConfig.telemetryConfig.telemetry_switch))
if (feature(FEATURE_TELEMETRY) && masterConfig.telemetryConfig.telemetry_switch)
activeBoxIds[activeBoxIdCount++] = BOXTELEMETRY;
#ifdef AUTOTUNE
@ -833,7 +817,7 @@ static bool processOutCommand(uint8_t cmdMSP)
serialize16(accSmooth[i]);
}
for (i = 0; i < 3; i++)
serialize16(gyroData[i]);
serialize16(gyroADC[i]);
for (i = 0; i < 3; i++)
serialize16(magADC[i]);
break;
@ -1764,6 +1748,7 @@ void mspProcess(void)
delay(50);
}
stopMotors();
handleOneshotFeatureChangeOnRestart();
systemReset();
}
}

View file

@ -27,44 +27,63 @@
#include "statusindicator.h"
void blinkLedAndSoundBeeper(uint8_t num, uint8_t wait, uint8_t repeat)
{
uint8_t i, r;
for (r = 0; r < repeat; r++) {
for (i = 0; i < num; i++) {
LED0_TOGGLE; // switch LEDPIN state
BEEP_ON;
delay(wait);
BEEP_OFF;
}
delay(60);
}
}
static uint32_t warningLedTimer = 0;
void enableWarningLed(uint32_t currentTime)
typedef enum {
WARNING_LED_OFF = 0,
WARNING_LED_ON,
WARNING_LED_FLASH
} warningLedState_e;
static warningLedState_e warningLedState = WARNING_LED_OFF;
void warningLedResetTimer(void) {
uint32_t now = millis();
warningLedTimer = now + 500000;
}
void warningLedEnable(void)
{
if (warningLedTimer != 0) {
return; // already enabled
warningLedState = WARNING_LED_ON;
}
void warningLedDisable(void)
{
warningLedState = WARNING_LED_OFF;
}
void warningLedFlash(void)
{
warningLedState = WARNING_LED_FLASH;
}
void warningLedRefresh(void)
{
switch (warningLedState) {
case WARNING_LED_OFF:
LED0_OFF;
break;
case WARNING_LED_ON:
LED0_ON;
break;
case WARNING_LED_FLASH:
LED0_TOGGLE;
break;
}
warningLedTimer = currentTime + 500000;
LED0_ON;
uint32_t now = micros();
warningLedTimer = now + 500000;
}
void disableWarningLed(void)
void warningLedUpdate(void)
{
warningLedTimer = 0;
LED0_OFF;
}
uint32_t now = micros();
void updateWarningLed(uint32_t currentTime)
{
if (warningLedTimer && (int32_t)(currentTime - warningLedTimer) >= 0) {
LED0_TOGGLE;
warningLedTimer = warningLedTimer + 500000;
if ((int32_t)(now - warningLedTimer) < 0) {
return;
}
warningLedRefresh();
}

View file

@ -17,8 +17,8 @@
#pragma once
void blinkLedAndSoundBeeper(uint8_t num, uint8_t wait, uint8_t repeat);
void enableWarningLed(uint32_t currentTime);
void disableWarningLed(void);
void updateWarningLed(uint32_t currentTime);
void warningLedEnable(void);
void warningLedDisable(void);
void warningLedRefresh(void);
void warningLedUpdate(void);
void warningLedFlash(void);

View file

@ -91,6 +91,7 @@
#include "debug.h"
extern uint32_t previousTime;
extern uint8_t motorControlEnable;
#ifdef SOFTSERIAL_LOOPBACK
serialPort_t *loopbackPort;
@ -171,6 +172,9 @@ void init(void)
systemInit();
// Latch active features to be used for feature() in the remainder of init().
latchActiveFeatures();
ledInit();
#ifdef SPEKTRUM_BIND
@ -256,6 +260,9 @@ void init(void)
mixerUsePWMOutputConfiguration(pwmOutputConfiguration);
if (!feature(FEATURE_ONESHOT125))
motorControlEnable = true;
systemState |= SYSTEM_STATE_MOTORS_READY;
#ifdef BEEPER
@ -481,6 +488,10 @@ void init(void)
LED2_ON;
#endif
// Latch active features AGAIN since some may be modified by init().
latchActiveFeatures();
motorControlEnable = true;
systemState |= SYSTEM_STATE_READY;
}

View file

@ -97,6 +97,8 @@ uint16_t cycleTime = 0; // this is the number in micro second to achieve
int16_t magHold;
int16_t headFreeModeHold;
uint8_t motorControlEnable = false;
int16_t telemTemperature1; // gyro sensor temperature
static uint32_t disarmAt; // Time of automatic disarm when "Don't spin the motors when armed" is enabled and auto_disarm_delay is nonzero
@ -274,11 +276,6 @@ void annexCode(void)
ENABLE_ARMING_FLAG(OK_TO_ARM);
}
if (isCalibrating()) {
LED0_TOGGLE;
DISABLE_ARMING_FLAG(OK_TO_ARM);
}
if (!STATE(SMALL_ANGLE)) {
DISABLE_ARMING_FLAG(OK_TO_ARM);
}
@ -287,13 +284,18 @@ void annexCode(void)
DISABLE_ARMING_FLAG(OK_TO_ARM);
}
if (ARMING_FLAG(OK_TO_ARM)) {
disableWarningLed();
if (isCalibrating()) {
warningLedFlash();
DISABLE_ARMING_FLAG(OK_TO_ARM);
} else {
enableWarningLed(currentTime);
if (ARMING_FLAG(OK_TO_ARM)) {
warningLedDisable();
} else {
warningLedFlash();
}
}
updateWarningLed(currentTime);
warningLedUpdate();
}
#ifdef TELEMETRY
@ -386,7 +388,7 @@ void mwArm(void)
}
if (!ARMING_FLAG(ARMED)) {
blinkLedAndSoundBeeper(2, 255, 1);
beeperConfirmationBeeps(1);
}
}
@ -728,7 +730,7 @@ void loop(void)
if (masterConfig.looptime == 0 || (int32_t)(currentTime - loopTime) >= 0) {
loopTime = currentTime + masterConfig.looptime;
imuUpdate(&currentProfile->accelerometerTrims, masterConfig.mixerMode);
imuUpdate(&currentProfile->accelerometerTrims);
// Measure loop rate just after reading the sensors
currentTime = micros();
@ -801,7 +803,9 @@ void loop(void)
writeServos();
#endif
writeMotors();
if (motorControlEnable) {
writeMotors();
}
#ifdef BLACKBOX
if (!cliMode && feature(FEATURE_BLACKBOX)) {

View file

@ -26,6 +26,7 @@
#include "drivers/sensor.h"
#include "drivers/accgyro.h"
#include "sensors/sensors.h"
#include "io/beeper.h"
#include "io/statusindicator.h"
#include "sensors/boardalignment.h"
@ -95,10 +96,14 @@ static void performAcclerationCalibration(uint8_t gyroMovementCalibrationThresho
return;
}
gyroZero[axis] = (g[axis] + (CALIBRATING_GYRO_CYCLES / 2)) / CALIBRATING_GYRO_CYCLES;
blinkLedAndSoundBeeper(10, 15, 1);
}
}
if (isOnFinalGyroCalibrationCycle()) {
beeper(BEEPER_GYRO_CALIBRATED);
}
calibratingG--;
}
static void applyGyroZero(void)

View file

@ -122,8 +122,8 @@ const mpu6050Config_t *selectMPU6050Config(void)
#ifdef USE_FAKE_GYRO
static void fakeGyroInit(void) {}
static void fakeGyroRead(int16_t *gyroData) {
memset(gyroData, 0, sizeof(int16_t[XYZ_AXIS_COUNT]));
static void fakeGyroRead(int16_t *gyroADC) {
memset(gyroADC, 0, sizeof(int16_t[XYZ_AXIS_COUNT]));
}
static void fakeGyroReadTemp(int16_t *tempData) {
UNUSED(tempData);

View file

@ -15,7 +15,7 @@
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
char *targetName = __TARGET__;
char *shortGitRevision = __REVISION__;
char *buildDate = __DATE__;
char *buildTime = __TIME__;
const char * const targetName = __TARGET__;
const char * const shortGitRevision = __REVISION__;
const char * const buildDate = __DATE__;
const char * const buildTime = __TIME__;

View file

@ -25,13 +25,13 @@
#define MW_VERSION 231
extern char* targetName;
extern const char* const targetName;
#define GIT_SHORT_REVISION_LENGTH 7 // lower case hexadecimal digits.
extern char* shortGitRevision;
extern const char* const shortGitRevision;
#define BUILD_DATE_LENGTH 11
extern char* buildDate; // "MMM DD YYYY" MMM = Jan/Feb/...
extern const char* const buildDate; // "MMM DD YYYY" MMM = Jan/Feb/...
#define BUILD_TIME_LENGTH 8
extern char* buildTime; // "HH:MM:SS"
extern const char* const buildTime; // "HH:MM:SS"

View file

@ -26,8 +26,8 @@ OBJECT_DIR = ../../obj/test
COMMON_FLAGS = \
-g \
-Wall \
-Wextra \
-pthread \
-Wextra \
-ggdb3 \
-O0 \
-DUNIT_TEST \
@ -39,7 +39,7 @@ C_FLAGS = $(COMMON_FLAGS) \
# Flags passed to the C++ compiler.
CXX_FLAGS = $(COMMON_FLAGS) \
-std=gnu++98
-std=gnu++11
# All tests produced by this Makefile. Remember to add new tests you
# created to the list.
@ -83,7 +83,7 @@ GTEST_SRCS_ = $(GTEST_DIR)/src/*.cc $(GTEST_DIR)/inc/gtest/*.h $(GTEST_HEADERS)
# compiles fast and for ordinary users its source rarely changes.
$(OBJECT_DIR)/gtest-all.o : $(GTEST_SRCS_)
@mkdir -p $(dir $@)
$(CXX) $(CXX_FLAGS) -I$(GTEST_DIR) -c \
$(CXX) $(CXX_FLAGS) -I$(GTEST_DIR) -Wno-missing-field-initializers -Wno-unused-const-variable -c \
$(GTEST_DIR)/src/gtest-all.cc -o $@
$(OBJECT_DIR)/gtest_main.o : $(GTEST_SRCS_)
@ -134,7 +134,7 @@ battery_unittest : \
$(OBJECT_DIR)/battery_unittest.o \
$(OBJECT_DIR)/gtest_main.a
$(CXX) $(CXX_FLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@
$(CXX) $(CXX_FLAGS) $^ -o $(OBJECT_DIR)/$@
$(OBJECT_DIR)/common/encoding.o : $(USER_DIR)/common/encoding.c $(USER_DIR)/common/encoding.h $(GTEST_HEADERS)
@mkdir -p $(dir $@)
@ -153,7 +153,7 @@ encoding_unittest : \
$(OBJECT_DIR)/encoding_unittest.o \
$(OBJECT_DIR)/gtest_main.a
$(CXX) $(CXX_FLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@
$(CXX) $(CXX_FLAGS) $^ -o $(OBJECT_DIR)/$@
$(OBJECT_DIR)/flight/imu.o : \
$(USER_DIR)/flight/imu.c \
@ -178,7 +178,7 @@ flight_imu_unittest : \
$(OBJECT_DIR)/common/maths.o \
$(OBJECT_DIR)/gtest_main.a
$(CXX) $(CXX_FLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@
$(CXX) $(CXX_FLAGS) $^ -o $(OBJECT_DIR)/$@
$(OBJECT_DIR)/maths_unittest.o : \
$(TEST_DIR)/maths_unittest.cc \
@ -192,7 +192,7 @@ maths_unittest : \
$(OBJECT_DIR)/common/maths.o \
$(OBJECT_DIR)/gtest_main.a
$(CXX) $(CXX_FLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@
$(CXX) $(CXX_FLAGS) $^ -o $(OBJECT_DIR)/$@
@ -217,7 +217,7 @@ altitude_hold_unittest : \
$(OBJECT_DIR)/altitude_hold_unittest.o \
$(OBJECT_DIR)/gtest_main.a
$(CXX) $(CXX_FLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@
$(CXX) $(CXX_FLAGS) $^ -o $(OBJECT_DIR)/$@
$(OBJECT_DIR)/flight/gps_conversion.o : \
@ -241,7 +241,7 @@ gps_conversion_unittest : \
$(OBJECT_DIR)/gps_conversion_unittest.o \
$(OBJECT_DIR)/gtest_main.a
$(CXX) $(CXX_FLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@
$(CXX) $(CXX_FLAGS) $^ -o $(OBJECT_DIR)/$@
@ -267,7 +267,7 @@ telemetry_hott_unittest : \
$(OBJECT_DIR)/flight/gps_conversion.o \
$(OBJECT_DIR)/gtest_main.a
$(CXX) $(CXX_FLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@
$(CXX) $(CXX_FLAGS) $^ -o $(OBJECT_DIR)/$@
@ -293,7 +293,7 @@ rc_controls_unittest : \
$(OBJECT_DIR)/rc_controls_unittest.o \
$(OBJECT_DIR)/gtest_main.a
$(CXX) $(CXX_FLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@
$(CXX) $(CXX_FLAGS) $^ -o $(OBJECT_DIR)/$@
$(OBJECT_DIR)/io/ledstrip.o : \
@ -317,7 +317,7 @@ ledstrip_unittest : \
$(OBJECT_DIR)/ledstrip_unittest.o \
$(OBJECT_DIR)/gtest_main.a
$(CXX) $(CXX_FLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@
$(CXX) $(CXX_FLAGS) $^ -o $(OBJECT_DIR)/$@
@ -342,7 +342,7 @@ ws2811_unittest : \
$(OBJECT_DIR)/ws2811_unittest.o \
$(OBJECT_DIR)/gtest_main.a
$(CXX) $(CXX_FLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@
$(CXX) $(CXX_FLAGS) $^ -o $(OBJECT_DIR)/$@
$(OBJECT_DIR)/flight/lowpass.o : \
@ -366,7 +366,7 @@ lowpass_unittest : \
$(OBJECT_DIR)/lowpass_unittest.o \
$(OBJECT_DIR)/gtest_main.a
$(CXX) $(CXX_FLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@
$(CXX) $(CXX_FLAGS) $^ -o $(OBJECT_DIR)/$@
$(OBJECT_DIR)/flight/mixer.o : \
$(USER_DIR)/flight/mixer.c \
@ -390,7 +390,7 @@ flight_mixer_unittest : \
$(OBJECT_DIR)/common/maths.o \
$(OBJECT_DIR)/gtest_main.a
$(CXX) $(CXX_FLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@
$(CXX) $(CXX_FLAGS) $^ -o $(OBJECT_DIR)/$@
$(OBJECT_DIR)/flight/failsafe.o : \
$(USER_DIR)/flight/failsafe.c \
@ -414,7 +414,7 @@ flight_failsafe_unittest : \
$(OBJECT_DIR)/common/maths.o \
$(OBJECT_DIR)/gtest_main.a
$(CXX) $(CXX_FLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@
$(CXX) $(CXX_FLAGS) $^ -o $(OBJECT_DIR)/$@
$(OBJECT_DIR)/io/serial.o : \
$(USER_DIR)/io/serial.c \
@ -437,7 +437,7 @@ io_serial_unittest : \
$(OBJECT_DIR)/io_serial_unittest.o \
$(OBJECT_DIR)/gtest_main.a
$(CXX) $(CXX_FLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@
$(CXX) $(CXX_FLAGS) $^ -o $(OBJECT_DIR)/$@
$(OBJECT_DIR)/rx/rx.o : \
$(USER_DIR)/rx/rx.c \
@ -461,7 +461,7 @@ rx_rx_unittest : \
$(OBJECT_DIR)/common/maths.o \
$(OBJECT_DIR)/gtest_main.a
$(CXX) $(CXX_FLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@
$(CXX) $(CXX_FLAGS) $^ -o $(OBJECT_DIR)/$@
test: $(TESTS)
set -e && for test in $(TESTS) ; do \

View file

@ -48,18 +48,6 @@ extern "C" {
extern uint8_t servoCount;
void forwardAuxChannelsToServos(void);
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
);
void mixerInit(mixerMode_e mixerMode, motorMixer_t *initialCustomMixers);
void mixerUsePWMOutputConfiguration(pwmOutputConfiguration_t *pwmOutputConfiguration);
}
@ -315,6 +303,15 @@ void pwmWriteMotor(uint8_t index, uint16_t value) {
motors[index].value = value;
}
void pwmShutdownPulsesForAllMotors(uint8_t motorCount)
{
uint8_t index;
for(index = 0; index < motorCount; index++){
motors[index].value = 0;
}
}
void pwmCompleteOneshotMotorUpdate(uint8_t motorCount) {
lastOneShotUpdateMotorCount = motorCount;
}

View file

@ -48,13 +48,18 @@ extern "C" {
extern void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, escAndServoConfig_t *escAndServoConfig, pidProfile_t *pidProfile);
}
TEST(RcControlsTest, updateActivatedModesWithAllInputsAtMidde)
class RcControlsModesTest : public ::testing::Test {
protected:
modeActivationCondition_t modeActivationConditions[MAX_MODE_ACTIVATION_CONDITION_COUNT];
virtual void SetUp() {
memset(&modeActivationConditions, 0, sizeof(modeActivationConditions));
}
};
TEST_F(RcControlsModesTest, updateActivatedModesWithAllInputsAtMidde)
{
// given
modeActivationCondition_t modeActivationConditions[MAX_MODE_ACTIVATION_CONDITION_COUNT];
memset(&modeActivationConditions, 0, sizeof(modeActivationConditions));
// and
rcModeActivationMask = 0;
// and
@ -79,12 +84,9 @@ TEST(RcControlsTest, updateActivatedModesWithAllInputsAtMidde)
}
}
TEST(RcControlsTest, updateActivatedModesUsingValidAuxConfigurationAndRXValues)
TEST_F(RcControlsModesTest, updateActivatedModesUsingValidAuxConfigurationAndRXValues)
{
// given
modeActivationCondition_t modeActivationConditions[MAX_MODE_ACTIVATION_CONDITION_COUNT];
memset(&modeActivationConditions, 0, sizeof(modeActivationConditions));
modeActivationConditions[0].modeId = (boxId_e)0;
modeActivationConditions[0].auxChannelIndex = AUX1 - NON_AUX_CHANNEL_COUNT;
modeActivationConditions[0].range.startStep = CHANNEL_VALUE_TO_STEP(1700);
@ -232,27 +234,33 @@ static const adjustmentConfig_t rateAdjustmentConfig = {
.data = { { 1 } }
};
TEST(RcControlsTest, processRcAdjustmentsSticksInMiddle)
{
// given
class RcControlsAdjustmentsTest : public ::testing::Test {
protected:
controlRateConfig_t controlRateConfig = {
.rcRate8 = 90,
.rcExpo8 = 0,
.thrMid8 = 0,
.thrExpo8 = 0,
.rates = {0,0,0},
.rates = {0, 0, 0},
.dynThrPID = 0,
.rcYawExpo8 = 0,
.tpa_breakpoint = 0
};
// and
memset(&rxConfig, 0, sizeof (rxConfig));
rxConfig.mincheck = DEFAULT_MIN_CHECK;
rxConfig.maxcheck = DEFAULT_MAX_CHECK;
rxConfig.midrc = 1500;
virtual void SetUp() {
adjustmentStateMask = 0;
memset(&adjustmentStates, 0, sizeof(adjustmentStates));
adjustmentStateMask = 0;
memset(&adjustmentStates, 0, sizeof(adjustmentStates));
memset(&rxConfig, 0, sizeof (rxConfig));
rxConfig.mincheck = DEFAULT_MIN_CHECK;
rxConfig.maxcheck = DEFAULT_MAX_CHECK;
rxConfig.midrc = 1500;
}
};
TEST_F(RcControlsAdjustmentsTest, processRcAdjustmentsSticksInMiddle)
{
// given
configureAdjustment(0, AUX3 - NON_AUX_CHANNEL_COUNT, &rateAdjustmentConfig);
// and
@ -275,28 +283,9 @@ TEST(RcControlsTest, processRcAdjustmentsSticksInMiddle)
EXPECT_EQ(adjustmentStateMask, 0);
}
TEST(RcControlsTest, processRcAdjustmentsWithRcRateFunctionSwitchUp)
TEST_F(RcControlsAdjustmentsTest, processRcAdjustmentsWithRcRateFunctionSwitchUp)
{
// given
controlRateConfig_t controlRateConfig = {
.rcRate8 = 90,
.rcExpo8 = 0,
.thrMid8 = 0,
.thrExpo8 = 0,
.rates = {0,0,0},
.dynThrPID = 0,
.tpa_breakpoint = 0
};
// and
memset(&rxConfig, 0, sizeof (rxConfig));
rxConfig.mincheck = DEFAULT_MIN_CHECK;
rxConfig.maxcheck = DEFAULT_MAX_CHECK;
rxConfig.midrc = 1500;
// and
adjustmentStateMask = 0;
memset(&adjustmentStates, 0, sizeof(adjustmentStates));
configureAdjustment(0, AUX3 - NON_AUX_CHANNEL_COUNT, &rateAdjustmentConfig);
// and
@ -441,28 +430,9 @@ static const adjustmentConfig_t rateProfileAdjustmentConfig = {
.data = { { 3 } }
};
TEST(RcControlsTest, processRcRateProfileAdjustments)
TEST_F(RcControlsAdjustmentsTest, processRcRateProfileAdjustments)
{
// given
controlRateConfig_t controlRateConfig = {
.rcRate8 = 90,
.rcExpo8 = 0,
.thrMid8 = 0,
.thrExpo8 = 0,
.rates = {0,0,0},
.dynThrPID = 0,
.tpa_breakpoint = 0
};
// and
memset(&rxConfig, 0, sizeof (rxConfig));
rxConfig.mincheck = DEFAULT_MIN_CHECK;
rxConfig.maxcheck = DEFAULT_MAX_CHECK;
rxConfig.midrc = 1500;
adjustmentStateMask = 0;
memset(&adjustmentStates, 0, sizeof(adjustmentStates));
int adjustmentIndex = 3;
configureAdjustment(adjustmentIndex, AUX4 - NON_AUX_CHANNEL_COUNT, &rateProfileAdjustmentConfig);
@ -528,7 +498,7 @@ static const adjustmentConfig_t pidYawDAdjustmentConfig = {
.data = { { 1 } }
};
TEST(RcControlsTest, processPIDIncreasePidController0)
TEST_F(RcControlsAdjustmentsTest, processPIDIncreasePidController0)
{
// given
modeActivationCondition_t modeActivationConditions[MAX_MODE_ACTIVATION_CONDITION_COUNT];
@ -554,15 +524,6 @@ TEST(RcControlsTest, processPIDIncreasePidController0)
controlRateConfig_t controlRateConfig;
memset(&controlRateConfig, 0, sizeof (controlRateConfig));
// and
memset(&rxConfig, 0, sizeof (rxConfig));
rxConfig.mincheck = DEFAULT_MIN_CHECK;
rxConfig.maxcheck = DEFAULT_MAX_CHECK;
rxConfig.midrc = 1500;
adjustmentStateMask = 0;
memset(&adjustmentStates, 0, sizeof(adjustmentStates));
configureAdjustment(0, AUX1 - NON_AUX_CHANNEL_COUNT, &pidPitchAndRollPAdjustmentConfig);
configureAdjustment(1, AUX2 - NON_AUX_CHANNEL_COUNT, &pidPitchAndRollIAdjustmentConfig);
configureAdjustment(2, AUX3 - NON_AUX_CHANNEL_COUNT, &pidPitchAndRollDAdjustmentConfig);
@ -614,7 +575,7 @@ TEST(RcControlsTest, processPIDIncreasePidController0)
EXPECT_EQ(28, pidProfile.D8[YAW]);
}
TEST(RcControlsTest, processPIDIncreasePidController2)
TEST_F(RcControlsAdjustmentsTest, processPIDIncreasePidController2)
{
// given
modeActivationCondition_t modeActivationConditions[MAX_MODE_ACTIVATION_CONDITION_COUNT];
@ -640,15 +601,6 @@ TEST(RcControlsTest, processPIDIncreasePidController2)
controlRateConfig_t controlRateConfig;
memset(&controlRateConfig, 0, sizeof (controlRateConfig));
// and
memset(&rxConfig, 0, sizeof (rxConfig));
rxConfig.mincheck = DEFAULT_MIN_CHECK;
rxConfig.maxcheck = DEFAULT_MAX_CHECK;
rxConfig.midrc = 1500;
adjustmentStateMask = 0;
memset(&adjustmentStates, 0, sizeof(adjustmentStates));
configureAdjustment(0, AUX1 - NON_AUX_CHANNEL_COUNT, &pidPitchAndRollPAdjustmentConfig);
configureAdjustment(1, AUX2 - NON_AUX_CHANNEL_COUNT, &pidPitchAndRollIAdjustmentConfig);
configureAdjustment(2, AUX3 - NON_AUX_CHANNEL_COUNT, &pidPitchAndRollDAdjustmentConfig);