diff --git a/docs/Blackbox.md b/docs/Blackbox.md index b3b4a2197a..c26fa53631 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, or to a dataflash chip which is present on some flight controllers. +logging device to be recorded, SD card, or to a dataflash chip which is present on some flight controllers. After your flight, you can view the resulting logs using the interactive log viewer: @@ -61,11 +61,11 @@ will work with the Blackbox, in order to reduce the number of dropped frames it higher performance [OpenLog Blackbox firmware][]. The special Blackbox variant of the OpenLog firmware also ensures that the OpenLog is using INAV compatible settings, and defaults to 115200 baud. -You can find the Blackbox version of the OpenLog firmware [here](https://github.com/cleanflight/blackbox-firmware), +You can find the Blackbox version of the OpenLog firmware [here](https://github.com/iNavFlight/openlog-blackbox-firmware), along with instructions for installing it onto your OpenLog. [OpenLog serial data logger]: https://www.sparkfun.com/products/9530 -[OpenLog Blackbox firmware]: https://github.com/cleanflight/blackbox-firmware +[OpenLog Blackbox firmware]: https://github.com/iNavFlight/openlog-blackbox-firmware #### microSDHC @@ -155,8 +155,6 @@ tubing instead. Some flight controllers have an onboard SPI NOR dataflash chip which can be used to store flight logs instead of using an OpenLog. -The SPRacingF3 has a 8 megabyte dataflash chip onboard which allows for longer recording times. - These chips are also supported: * Micron/ST M25P16 - 16 Mbit / 2 MByte @@ -206,6 +204,25 @@ would result in a logging rate of about 10-20Hz and about 650 bytes/second of da dataflash 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. +The CLI command `blackbox` allows setting which Blackbox fields are recorded to conserve space and bandwidth. Possible fields are: + +* NAV_ACC - Navigation accelerometer readouts +* NAV_PID - Navigation PID debug +* NAV_POS - Current and target position and altitude +* MAG - Magnetometer raw values +* ACC - Accelerometer raw values +* ATTI - Attitude as computed by INAV position estimator +* RC_DATA - RC channels 1-4 as returned by the radio receiver +* RC_COMMAND - RC_DATA converted to [-500:500] scale (for A,E,R) with expo and deadband +* MOTORS - motor output + +Usage: + +* `blackbox` currently enabled Blackbox fields +* `blackbox list` all available fields +* `blackbox -MOTORS` disable MOTORS logging +* `blackbox MOTOR` enable MOTORS logging + ## Usage The Blackbox starts recording data as soon as you arm your craft, and stops when you disarm. @@ -259,4 +276,4 @@ another software package. You'll find those tools along with instructions for using them in this repository: -https://github.com/iNavFlight/blackbox-log-viewer +https://github.com/iNavFlight/blackbox-tools diff --git a/docs/Boards.md b/docs/Boards.md index 28e54bbdb5..9440058758 100644 --- a/docs/Boards.md +++ b/docs/Boards.md @@ -1,31 +1,15 @@ # Flight controller hardware -### Sponsored and recommended boards - -These boards come from companies that support INAV development. Buying one of these boards you make your small contribution for improving INAV as well. - -Also these boards are tested by INAV development team and usually flown on daily basis. - -| Board name | CPU Family | Target name(s) | GPS | Compass | Barometer | Telemetry | RX | Blackbox | -|---------------------------|:----------:|:-------------------------:|:----:|:-------:|:--------------:|:---------:|:------------------------------:|:--------------------:| -| [Airbot OMNIBUS F4 PRO](https://inavflight.com/shop/p/OMNIBUSF4PRO)| F4 | OMNIBUSF4PRO | All | All | All | All | All | SERIAL, SD | -| [Airbot OMNIBUS F4](https://inavflight.com/shop/s/bg/1319176)| F4 | OMNIBUSF4 | All | All | All | All | All | SERIAL, SD | - -Note: In the above and following tables, the sensor columns indicates firmware support for the sensor category; it does not necessarily mean there is an on-board sensor. - ### Recommended boards These boards are well tested with INAV and are known to be of good quality and reliability. | Board name | CPU Family | Target name(s) | GPS | Compass | Barometer | Telemetry | RX | Blackbox | |---------------------------|:----------:|:-------------------------:|:----:|:-------:|:--------------:|:---------:|:------------------------------:|:--------------------:| -| [Matek F405-CTR](https://inavflight.com/shop/p/MATEKF405CTR) | F4 | MATEKF405 | All | All | All | All | All | SERIAL, SD | -| [Matek F405-STD](https://inavflight.com/shop/p/MATEKF405STD) | F4 | MATEKF405 | All | All | All | All | All | SERIAL, SD | -| [Matek F405-WING](https://inavflight.com/shop/p/MATEKF405WING) | F4 | MATEKF405SE | All | All | All | All | All | SERIAL, SD | -| [Matek F722 WING](https://inavflight.com/shop/p/MATEKF722WING) | F7 | MATEKF722SE | All | All | All | All | All | SERIAL, SD | -| [Matek F722-SE](https://inavflight.com/shop/p/MATEKF722SE) | F7 | MATEKF722SE | All | All | All | All | All | SERIAL, SD | -| [Matek F722-STD](https://inavflight.com/shop/p/MATEKF722STD) | F7 | MATEKF722 | All | All | All | All | All | SERIAL, SD | -| [Matek F722-MINI](https://inavflight.com/shop/p/MATEKF722MINI) | F7 | MATEKF722SE | All | All | All | All | All | SPIFLASH | +| [Diatone Mamba H743](https://inavflight.com/shop/s/bg/1929033) | H7 | MAMBAH743 | All | All | All | All | All | SERIAL, SD | +| [Matek F765-WSE](https://inavflight.com/shop/s/bg/1890404) | F7 | MATEKF765SE | All | All | All | All | All | SERIAL, SD | +| [Matek F722-SE](https://inavflight.com/shop/p/MATEKF722SE) | F7 | MATEKF722SE | All | All | All | All | All | SERIAL, SD | +| [Holybro Kakute H7](https://inavflight.com/shop/s/bg/1914066) | H7 | KAKUTEH7 | All | All | All | All | All | SERIAL, SD | It's possible to find more supported and tested boards [here](https://github.com/iNavFlight/inav/wiki/Welcome-to-INAV,-useful-links-and-products) ### Boards documentation diff --git a/docs/ESC and servo outputs.md b/docs/ESC and servo outputs.md index 09f9f835bf..78a5056590 100644 --- a/docs/ESC and servo outputs.md +++ b/docs/ESC and servo outputs.md @@ -2,13 +2,14 @@ ## ESC protocols -INAV support following ESC protocols: +INAV support the following ESC protocols: * "standard" PWM with 50-400Hz update rate * OneShot125 * OneShot42 * Multishot * Brushed motors +* DSHOT150, DSHOT300, DSHOT600 ESC protocol can be selected in Configurator. No special configuration is required. @@ -23,4 +24,12 @@ higher update rates. Only high end digital servos are capable of handling 200Hz Not all outputs on a flight controller can be used for servo outputs. It is a hardware thing. Always check flight controller documentation. -While motors are usually ordered sequentially, here is no standard output layout for servos! Some boards might not be supporting servos in _Multirotor_ configuration at all! \ No newline at end of file +While motors are usually ordered sequentially, here is no standard output layout for servos! Some boards might not be supporting servos in _Multirotor_ configuration at all! + +## Modifying output mapping + +INAV 5 allows the limited output type mapping by allowing to change the function of *ALL* outputs at the same time. It can be done with the `output_mode` CLI setting. Allowed values: + +* `AUTO` assigns outputs according to the default mapping +* `SERVOS` assigns all outputs to servos +* `MOTORS` assigns all outputs to motors \ No newline at end of file diff --git a/docs/Inflight Adjustments.md b/docs/Inflight Adjustments.md index 422ba71eb5..37102cebf0 100644 --- a/docs/Inflight Adjustments.md +++ b/docs/Inflight Adjustments.md @@ -158,6 +158,8 @@ this reason ensure that you define enough ranges to cover the range channel's us | 54 | TPA | | 55 | TPA_BREAKPOINT | | 56 | NAV_FW_CONTROL_SMOOTHNESS | +| 57 | FW_TPA_TIME_CONSTANT | +| 58 | FW_LEVEL_TRIM | ## Examples diff --git a/docs/Programming Framework.md b/docs/Programming Framework.md index 3052b4f8ef..f51f841bb9 100644 --- a/docs/Programming Framework.md +++ b/docs/Programming Framework.md @@ -31,30 +31,30 @@ IPF can be edited using INAV Configurator user interface, of via CLI ### Operations -| Operation ID | Name | Notes | -|---- |---- |---- | -| 0 | TRUE | Always evaluates as true | -| 1 | EQUAL | Evaluates `false` if `false` or `0` | -| 2 | GREATER_THAN | | -| 3 | LOWER_THAN | | -| 4 | LOW | `true` if `<1333` | -| 5 | MID | `true` if `>=1333 and <=1666` | -| 6 | HIGH | `true` if `>1666` | -| 7 | AND | | -| 8 | OR | | -| 9 | XOR | | -| 10 | NAND | | -| 11 | NOR | | -| 12 | NOT | | -| 13 | STICKY | `Operand A` is activation operator, `Operand B` is deactivation operator. After activation, operator will return `true` until Operand B is evaluated as `true`| -| 14 | ADD | Add `Operand A` to `Operand B` and returns the result | -| 15 | SUB | Substract `Operand B` from `Operand A` and returns the result | -| 16 | MUL | Multiply `Operand A` by `Operand B` and returns the result | -| 17 | DIV | Divide `Operand A` by `Operand B` and returns the result | -| 18 | GVAR SET | Store value from `Operand B` into the Global Variable addressed by `Operand B`. Bear in mind, that operand `Global Variable` means: Value stored in Global Variable of an index! To store in GVAR 1 use `Value 1` not `Global Variable 1` | -| 19 | GVAR INC | Increase the GVAR indexed by `Operand A` with value from `Operand B` | -| 20 | GVAR DEC | Decrease the GVAR indexed by `Operand A` with value from `Operand B` | -| 21 | IO PORT SET | Set I2C IO Expander pin `Operand A` to value of `Operand B`. `Operand A` accepts values `0-7` and `Operand B` accepts `0` and `1` | +| Operation ID | Name | Notes | +|---------------|-------------------------------|-------| +| 0 | TRUE | Always evaluates as true | +| 1 | EQUAL | Evaluates `false` if `false` or `0` | +| 2 | GREATER_THAN | `true` if `Operand A` is a higher value than `Operand B` | +| 3 | LOWER_THAN | `true` if `Operand A` is a lower value than `Operand B` | +| 4 | LOW | `true` if `<1333` | +| 5 | MID | `true` if `>=1333 and <=1666` | +| 6 | HIGH | `true` if `>1666` | +| 7 | AND | `true` if `Operand A` and `Operand B` are the same value or both `true` | +| 8 | OR | `true` if `Operand A` and/or `OperandB` is `true` | +| 9 | XOR | `true` if `Operand A` or `Operand B` is `true`, but not both | +| 10 | NAND | `false` if `Operand A` and `Operand B` are both `true`| +| 11 | NOR | `true` if `Operand A` and `Operand B` are both `false` | +| 12 | NOT | The boolean opposite to `Operand A` | +| 13 | STICKY | `Operand A` is activation operator, `Operand B` is deactivation operator. After activation, operator will return `true` until Operand B is evaluated as `true`| +| 14 | ADD | Add `Operand A` to `Operand B` and returns the result | +| 15 | SUB | Substract `Operand B` from `Operand A` and returns the result | +| 16 | MUL | Multiply `Operand A` by `Operand B` and returns the result | +| 17 | DIV | Divide `Operand A` by `Operand B` and returns the result | +| 18 | GVAR SET | Store value from `Operand B` into the Global Variable addressed by `Operand B`. Bear in mind, that operand `Global Variable` means: Value stored in Global Variable of an index! To store in GVAR 1 use `Value 1` not `Global Variable 1` | +| 19 | GVAR INC | Increase the GVAR indexed by `Operand A` with value from `Operand B` | +| 20 | GVAR DEC | Decrease the GVAR indexed by `Operand A` with value from `Operand B` | +| 21 | IO PORT SET | Set I2C IO Expander pin `Operand A` to value of `Operand B`. `Operand A` accepts values `0-7` and `Operand B` accepts `0` and `1` | | 22 | OVERRIDE_ARMING_SAFETY | Allows to arm on any angle even without GPS fix | | 23 | OVERRIDE_THROTTLE_SCALE | Override throttle scale to the value defined by operand. Operand type `0` and value `50` means throttle will be scaled by 50%. | | 24 | SWAP_ROLL_YAW | basically, when activated, yaw stick will control roll and roll stick will control yaw. Required for tail-sitters VTOL during vertical-horizonral transition when body frame changes | @@ -66,109 +66,114 @@ IPF can be edited using INAV Configurator user interface, of via CLI | 30 | SET_VTX_BAND | Sets VTX band. Accepted values are `1-5` | | 31 | SET_VTX_CHANNEL | Sets VTX channel. Accepted values are `1-8` | | 32 | SET_OSD_LAYOUT | Sets OSD layout. Accepted values are `0-3` | -| 33 | SIN | Computes SIN of `Operand A` value in degrees. Output is multiplied by `Operand B` value. If `Operand B` is `0`, result is multiplied by `500` | -| 34 | COS | Computes COS of `Operand A` value in degrees. Output is multiplied by `Operand B` value. If `Operand B` is `0`, result is multiplied by `500` | -| 35 | TAN | Computes TAN of `Operand A` value in degrees. Output is multiplied by `Operand B` value. If `Operand B` is `0`, result is multiplied by `500` | -| 36 | MAP_INPUT | Scales `Operand A` from [`0` : `Operand B`] to [`0` : `1000`]. Note: input will be constrained and then scaled | -| 37 | MAP_OUTPUT | Scales `Operand A` from [`0` : `1000`] to [`0` : `Operand B`]. Note: input will be constrained and then scaled | -| 38 | RC_CHANNEL_OVERRIDE | Overrides channel set by `Operand A` to value of `Operand B` | -| 39 | SET_HEADING_TARGET | Sets heading-hold target to `Operand A`, in degrees. Value wraps-around. | -| 40 | MOD | Divide `Operand A` by `Operand B` and returns the remainder | -| 41 | LOITER_RADIUS_OVERRIDE | Sets the loiter radius to `Operand A` [`0` : `100000`] in cm. If the value is lower than the loiter radius set in the **Advanced Tuning**, that will be used. | -| 42 | SET_PROFILE | Sets the active config profile (PIDFF/Rates/Filters/etc) to `Operand A`. `Operand A` must be a valid profile number, currently from 1 to 3. If not, the profile will not change | -| 43 | MIN | Finds the lowest value of `Operand A` and `Operand B` | -| 44 | MAX | Finds the highest value of `Operand A` and `Operand B` | - +| 33 | SIN | Computes SIN of `Operand A` value in degrees. Output is multiplied by `Operand B` value. If `Operand B` is `0`, result is multiplied by `500` | +| 34 | COS | Computes COS of `Operand A` value in degrees. Output is multiplied by `Operand B` value. If `Operand B` is `0`, result is multiplied by `500` | +| 35 | TAN | Computes TAN of `Operand A` value in degrees. Output is multiplied by `Operand B` value. If `Operand B` is `0`, result is multiplied by `500` | +| 36 | MAP_INPUT | Scales `Operand A` from [`0` : `Operand B`] to [`0` : `1000`]. Note: input will be constrained and then scaled | +| 37 | MAP_OUTPUT | Scales `Operand A` from [`0` : `1000`] to [`0` : `Operand B`]. Note: input will be constrained and then scaled | +| 38 | RC_CHANNEL_OVERRIDE | Overrides channel set by `Operand A` to value of `Operand B` | +| 39 | SET_HEADING_TARGET | Sets heading-hold target to `Operand A`, in degrees. Value wraps-around. | +| 40 | MOD | Divide `Operand A` by `Operand B` and returns the remainder | +| 41 | LOITER_RADIUS_OVERRIDE | Sets the loiter radius to `Operand A` [`0` : `100000`] in cm. If the value is lower than the loiter radius set in the **Advanced Tuning**, that will be used. | +| 42 | SET_PROFILE | Sets the active config profile (PIDFF/Rates/Filters/etc) to `Operand A`. `Operand A` must be a valid profile number, currently from 1 to 3. If not, the profile will not change | +| 43 | MIN | Finds the lowest value of `Operand A` and `Operand B` | +| 44 | MAX | Finds the highest value of `Operand A` and `Operand B` | +| 45 | FLIGHT_AXIS_ANGLE_OVERRIDE | Sets the target attitude angle for axis. In other words, when active, it enforces Angle mode (Heading Hold for Yaw) on this axis (Angle mode does not have to be active). `Operand A` defines the axis: `0` - Roll, `1` - Pitch, `2` - Yaw. `Operand B` defines the angle in degrees | +| 46 | FLIGHT_AXIS_RATE_OVERRIDE | Sets the target rate (rotation speed) for axis. `Operand A` defines the axis: `0` - Roll, `1` - Pitch, `2` - Yaw. `Operand B` defines the rate in degrees per second | ### Operands -| Operand Type | Name | Notes | -|---- |---- |---- | -| 0 | VALUE | Value derived from `value` field | -| 1 | GET_RC_CHANNEL | `value` points to RC channel number, indexed from 1 | -| 2 | FLIGHT | `value` points to flight parameter table | -| 3 | FLIGHT_MODE | `value` points to flight modes table | -| 4 | LC | `value` points to other logic condition ID | -| 5 | GVAR | Value stored in Global Variable indexed by `value`. `GVAR 1` means: value in GVAR 1 | -| 5 | PID | Output of a Programming PID indexed by `value`. `PID 1` means: value in PID 1 | +| Operand Type | Name | Notes | +|---------------|-----------------------|-------| +| 0 | VALUE | Value derived from `value` field | +| 1 | GET_RC_CHANNEL | `value` points to RC channel number, indexed from 1 | +| 2 | FLIGHT | `value` points to flight parameter table | +| 3 | FLIGHT_MODE | `value` points to flight modes table | +| 4 | LC | `value` points to other logic condition ID | +| 5 | GVAR | Value stored in Global Variable indexed by `value`. `GVAR 1` means: value in GVAR 1 | +| 5 | PID | Output of a Programming PID indexed by `value`. `PID 1` means: value in PID 1 | #### FLIGHT -| Operand Value | Name | Notes | -|---- |---- |---- | -| 0 | ARM_TIMER | in `seconds` | -| 1 | HOME_DISTANCE | in `meters` | -| 2 | TRIP_DISTANCE | in `meters` | -| 3 | RSSI | | -| 4 | VBAT | in `Volts * 100`, eg. `12.1V` is `1210` | -| 5 | CELL_VOLTAGE | in `Volts * 100`, eg. `12.1V` is `1210` | -| 6 | CURRENT | in `Amps * 100`, eg. `9A` is `900` | -| 7 | MAH_DRAWN | in `mAh` | -| 8 | GPS_SATS | | -| 9 | GROUD_SPEED | in `cm/s` | -| 10 | 3D_SPEED | in `cm/s` | -| 11 | AIR_SPEED | in `cm/s` | -| 12 | ALTITUDE | in `cm` | -| 13 | VERTICAL_SPEED | in `cm/s` | -| 14 | TROTTLE_POS | in `%` | -| 15 | ATTITUDE_ROLL | in `degrees` | -| 16 | ATTITUDE_PITCH | in `degrees` | -| 17 | IS_ARMED | boolean `0`/`1` | -| 18 | IS_AUTOLAUNCH | boolean `0`/`1` | -| 19 | IS_ALTITUDE_CONTROL | boolean `0`/`1` | -| 20 | IS_POSITION_CONTROL | boolean `0`/`1` | -| 21 | IS_EMERGENCY_LANDING | boolean `0`/`1` | -| 22 | IS_RTH | boolean `0`/`1` | -| 23 | IS_WP | boolean `0`/`1` | -| 24 | IS_LANDING | boolean `0`/`1` | -| 25 | IS_FAILSAFE | boolean `0`/`1` | -| 26 | STABILIZED_ROLL | Roll PID controller output `[-500:500]` | -| 27 | STABILIZED_PITCH | Pitch PID controller output `[-500:500]` | -| 28 | STABILIZED_YAW | Yaw PID controller output `[-500:500]` | -| 29 | ACTIVE_WAYPOINT_INDEX | Indexed from `1`. To verify WP is in progress, use `IS_WP` | -| 30 | ACTIVE_WAYPOINT_ACTION | See ACTIVE_WAYPOINT_ACTION paragraph | -| 31 | 3D HOME_DISTANCE | in `meters`, calculated from HOME_DISTANCE and ALTITUDE using Pythagorean theorem | -| 32 | CROSSFIRE LQ | Crossfire Link quality as returned by the CRSF protocol | -| 33 | CROSSFIRE SNR | Crossfire SNR as returned by the CRSF protocol | -| 34 | GPS_VALID | boolean `0`/`1`. True when the GPS has a valid 3D Fix | -| 35 | LOITER_RADIUS | The current loiter radius in cm. | -| 36 | ACTIVE_PROFILE | integer for the active config profile `[1..MAX_PROFILE_COUNT]` | +| Operand Value | Name | Notes | +|---------------|-------------------------------|-------| +| 0 | ARM_TIMER | in `seconds` | +| 1 | HOME_DISTANCE | in `meters` | +| 2 | TRIP_DISTANCE | in `meters` | +| 3 | RSSI | | +| 4 | VBAT | in `Volts * 100`, eg. `12.1V` is `1210` | +| 5 | CELL_VOLTAGE | in `Volts * 100`, eg. `12.1V` is `1210` | +| 6 | CURRENT | in `Amps * 100`, eg. `9A` is `900` | +| 7 | MAH_DRAWN | in `mAh` | +| 8 | GPS_SATS | | +| 9 | GROUD_SPEED | in `cm/s` | +| 10 | 3D_SPEED | in `cm/s` | +| 11 | AIR_SPEED | in `cm/s` | +| 12 | ALTITUDE | in `cm` | +| 13 | VERTICAL_SPEED | in `cm/s` | +| 14 | TROTTLE_POS | in `%` | +| 15 | ATTITUDE_ROLL | in `degrees` | +| 16 | ATTITUDE_PITCH | in `degrees` | +| 17 | IS_ARMED | boolean `0`/`1` | +| 18 | IS_AUTOLAUNCH | boolean `0`/`1` | +| 19 | IS_ALTITUDE_CONTROL | boolean `0`/`1` | +| 20 | IS_POSITION_CONTROL | boolean `0`/`1` | +| 21 | IS_EMERGENCY_LANDING | boolean `0`/`1` | +| 22 | IS_RTH | boolean `0`/`1` | +| 23 | IS_WP | boolean `0`/`1` | +| 24 | IS_LANDING | boolean `0`/`1` | +| 25 | IS_FAILSAFE | boolean `0`/`1` | +| 26 | STABILIZED_ROLL | Roll PID controller output `[-500:500]` | +| 27 | STABILIZED_PITCH | Pitch PID controller output `[-500:500]` | +| 28 | STABILIZED_YAW | Yaw PID controller output `[-500:500]` | +| 29 | ACTIVE_WAYPOINT_INDEX | Indexed from `1`. To verify WP is in progress, use `IS_WP` | +| 30 | ACTIVE_WAYPOINT_ACTION | See ACTIVE_WAYPOINT_ACTION paragraph | +| 31 | 3D HOME_DISTANCE | in `meters`, calculated from HOME_DISTANCE and ALTITUDE using Pythagorean theorem | +| 32 | CROSSFIRE LQ | Crossfire Link quality as returned by the CRSF protocol | +| 33 | CROSSFIRE SNR | Crossfire SNR as returned by the CRSF protocol | +| 34 | GPS_VALID | boolean `0`/`1`. True when the GPS has a valid 3D Fix | +| 35 | LOITER_RADIUS | The current loiter radius in cm. | +| 36 | ACTIVE_PROFILE | integer for the active config profile `[1..MAX_PROFILE_COUNT]` | +| 37 | BATT_CELLS | Number of battery cells detected | +| 38 | AGL_STATUS | boolean `1` when AGL can be trusted, `0` when AGL estimate can not be trusted | +| 39 | AGL | integer Above The Groud Altitude in `cm` | +| 40 | RANGEFINDER_RAW | integer raw distance provided by the rangefinder in `cm` | #### ACTIVE_WAYPOINT_ACTION -| Action | Value | -|---- |---- | -| WAYPOINT | 1 | -| HOLD_TIME | 3 | -| RTH | 4 | -| SET_POI | 5 | -| JUMP | 6 | -| SET_HEAD | 7 | -| LAND | 8 | +| Action | Value | +|---------------|-------| +| WAYPOINT | 1 | +| HOLD_TIME | 3 | +| RTH | 4 | +| SET_POI | 5 | +| JUMP | 6 | +| SET_HEAD | 7 | +| LAND | 8 | #### FLIGHT_MODE -| Operand Value | Name | Notes | -|---- |---- |---- | -| 0 | FAILSAFE | | -| 1 | MANUAL | | -| 2 | RTH | | -| 3 | POSHOLD | | -| 4 | CRUISE | | -| 5 | ALTHOLD | | -| 6 | ANGLE | | -| 7 | HORIZON | | -| 8 | AIR | | -| 9 | USER1 | | -| 10 | USER2 | | +| Operand Value | Name | Notes | +|---------------|-----------|-------| +| 0 | FAILSAFE | | +| 1 | MANUAL | | +| 2 | RTH | | +| 3 | POSHOLD | | +| 4 | CRUISE | | +| 5 | ALTHOLD | | +| 6 | ANGLE | | +| 7 | HORIZON | | +| 8 | AIR | | +| 9 | USER1 | | +| 10 | USER2 | | ### Flags All flags are reseted on ARM and DISARM event. -| bit | Decimal | Function | -|---- |---- |---- | +| bit | Decimal | Function | +|-------|-----------|-----------| | 0 | 1 | Latch - after activation LC will stay active until LATCH flag is reseted | ## Global variables @@ -221,13 +226,13 @@ Inverts ROLL and PITCH input when Logic Condition `0` evaluates as `true`. Movin `logic 0 1 0 29 0 1000 0 0 0` -Sets Thhrottle output to `0%` when Logic Condition `0` evaluates as `true` +Sets throttle output to `0%` when Logic Condition `0` evaluates as `true` ### Set throttle to 50% and keep other throttle bindings active `logic 0 1 0 29 0 1500 0 0 0` -Sets Thhrottle output to about `50%` when Logic Condition `0` evaluates as `true` +Sets throttle output to about `50%` when Logic Condition `0` evaluates as `true` ### Set throttle control to different RC channel diff --git a/docs/Rx.md b/docs/Rx.md index 586e6ece42..b51b6d94f3 100644 --- a/docs/Rx.md +++ b/docs/Rx.md @@ -45,7 +45,7 @@ http://www.lemon-rx.com/shop/index.php?route=product/product&product_id=118 #### Spektrum pesudo RSSI -As of iNav 1.6, a pseudo RSSI, based on satellite fade count is supported and reported as normal iNav RSSI (0-1023 range). In order to use this feature, the following is necessary:obj/inav_2.2.2_SPRACINGF3EVO.hex +As of iNav 1.6, a pseudo RSSI, based on satellite fade count is supported and reported as normal iNav RSSI (0-1023 range). In order to use this feature, the following is necessary: * Bind the satellite receiver using a physical RX; the bind function provided by the flight controller is not sufficient. * The CLI variable `rssi_channel` is set to channel 9: @@ -59,7 +59,7 @@ This pseudo-RSSI should work on all makes of Spektrum satellite RX; it is tested 16 channels via serial currently supported. See below how to set up your transmitter. * You probably need an inverter between the receiver output and the flight controller. However, some flight controllers have this built in and doesn't need one. -* Some OpenLRS receivers produce obj/inav_2.2.2_SPRACINGF3EVO.hexa non-inverted SBUS signal. It is possible to switch SBUS inversion off using CLI command `set sbus_inversion = OFF` when using an F3 based flight controller. +* Some OpenLRS receivers produce a non-inverted SBUS signal. It is possible to switch SBUS inversion off using CLI command `set sbus_inversion = OFF` when using an F3 based flight controller. * Softserial ports cannot be used with SBUS because it runs at too high of a bitrate (1Mbps). Refer to the chapter specific to your board to determine which port(s) may be used. * You will need to configure the channel mapping in the GUI (Receiver tab) or CLI (`map` command). Note that channels above 8 are mapped "straight", with no remapping. @@ -170,7 +170,7 @@ The flash is avaliable here: https://github.com/benb0jangles/FlySky-i6-Mod- ``` After flash "10ch Timer Mod i6 Updater", it is passible to get RSSI signal on selected Aux channel from FS-i6 Err sensor. -It is possible to use IBUS RX and IBUS telemetry on only one port of the hardware UART. More information in Telemetry.md.obj/inav_2.2.2_SPRACINGF3EVO.hex +It is possible to use IBUS RX and IBUS telemetry on only one port of the hardware UART. More information in Telemetry.md. ### SRXL2 diff --git a/docs/Settings.md b/docs/Settings.md index 33aee7cacc..5d43cfe98b 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -212,16 +212,6 @@ ADC channel to use for analog pitot tube (airspeed) sensor. If board doesn't hav --- -### align_acc - -When running on non-default hardware or adding support for new sensors/sensor boards, these values are used for sensor orientation. When carefully understood, these values can also be used to rotate (in 90deg steps) or flip the board. Possible values are: DEFAULT, CW0_DEG, CW90_DEG, CW180_DEG, CW270_DEG, CW0_DEG_FLIP, CW90_DEG_FLIP, CW180_DEG_FLIP, CW270_DEG_FLIP. - -| Default | Min | Max | -| --- | --- | --- | -| DEFAULT | | | - ---- - ### align_board_pitch Arbitrary board rotation in deci-degrees (0.1 degree), to allow mounting it sideways / upside down / rotated etc @@ -252,16 +242,6 @@ Arbitrary board rotation in deci-degrees (0.1 degree), to allow mounting it side --- -### align_gyro - -When running on non-default hardware or adding support for new sensors/sensor boards, these values are used for sensor orientation. When carefully understood, these values can also be used to rotate (in 90deg steps) or flip the board. Possible values are: DEFAULT, CW0_DEG, CW90_DEG, CW180_DEG, CW270_DEG, CW0_DEG_FLIP, CW90_DEG_FLIP, CW180_DEG_FLIP, CW270_DEG_FLIP. - -| Default | Min | Max | -| --- | --- | --- | -| DEFAULT | | | - ---- - ### align_mag When running on non-default hardware or adding support for new sensors/sensor boards, these values are used for sensor orientation. When carefully understood, these values can also be used to rotate (in 90deg steps) or flip the board. Possible values are: DEFAULT, CW0_DEG, CW90_DEG, CW180_DEG, CW270_DEG, CW0_DEG_FLIP, CW90_DEG_FLIP, CW180_DEG_FLIP, CW270_DEG_FLIP. @@ -798,7 +778,7 @@ Minimum frequency for dynamic notches. Default value of `150` works best with 5" | Default | Min | Max | | --- | --- | --- | -| 50 | 30 | 1000 | +| 50 | 30 | 250 | --- @@ -1394,11 +1374,11 @@ Reference airspeed. Set this to airspeed at which PIDs were tuned. Usually shoul ### fw_tpa_time_constant -TPA smoothing and delay time constant to reflect non-instant speed/throttle response of the plane. Planes with low thrust/weight ratio generally need higher time constant. Default is zero for compatibility with old setups +TPA smoothing and delay time constant to reflect non-instant speed/throttle response of the plane. See **PID Attenuation and scaling** Wiki for full details. | Default | Min | Max | | --- | --- | --- | -| 0 | 0 | 5000 | +| 1500 | 0 | 5000 | --- @@ -4362,6 +4342,16 @@ Top and bottom margins for the hud area --- +### osd_hud_radar_alt_difference_display_time + +Time in seconds to display the altitude difference in radar + +| Default | Min | Max | +| --- | --- | --- | +| 3 | 0 | 10 | + +--- + ### osd_hud_radar_disp Maximum count of nearby aircrafts or points of interest to display in the hud, as sent from an ESP32 LoRa module. Set to 0 to disable (show nothing). The nearby aircrafts will appear as markers A, B, C, etc @@ -4372,6 +4362,16 @@ Maximum count of nearby aircrafts or points of interest to display in the hud, a --- +### osd_hud_radar_distance_display_time + +Time in seconds to display the distance in radar + +| Default | Min | Max | +| --- | --- | --- | +| 3 | 1 | 10 | + +--- + ### osd_hud_radar_range_max In meters, radar aircrafts further away than this will not be displayed in the hud @@ -4452,6 +4452,16 @@ LQ % indicator blinks below this value. For Crossfire use 70%, for Tracer use 50 --- +### osd_mah_used_precision + +Number of digits used to display mAh used. + +| Default | Min | Max | +| --- | --- | --- | +| 4 | 4 | 6 | + +--- + ### osd_main_voltage_decimals Number of decimals for the battery voltages displayed in the OSD [1-2]. @@ -4662,6 +4672,96 @@ Auto swap display time interval between disarm stats pages (seconds). Reverts to --- +### osd_switch_indicator_one_channel + +RC Channel to use for OSD switch indicator 1. + +| Default | Min | Max | +| --- | --- | --- | +| 5 | 5 | MAX_SUPPORTED_RC_CHANNEL_COUNT | + +--- + +### osd_switch_indicator_one_name + +Character to use for OSD switch incicator 1. + +| Default | Min | Max | +| --- | --- | --- | +| GEAR | | 5 | + +--- + +### osd_switch_indicator_three_channel + +RC Channel to use for OSD switch indicator 3. + +| Default | Min | Max | +| --- | --- | --- | +| 5 | 5 | MAX_SUPPORTED_RC_CHANNEL_COUNT | + +--- + +### osd_switch_indicator_three_name + +Character to use for OSD switch incicator 3. + +| Default | Min | Max | +| --- | --- | --- | +| LIGT | | 5 | + +--- + +### osd_switch_indicator_two_channel + +RC Channel to use for OSD switch indicator 2. + +| Default | Min | Max | +| --- | --- | --- | +| 5 | 5 | MAX_SUPPORTED_RC_CHANNEL_COUNT | + +--- + +### osd_switch_indicator_two_name + +Character to use for OSD switch incicator 2. + +| Default | Min | Max | +| --- | --- | --- | +| CAM | | 5 | + +--- + +### osd_switch_indicator_zero_channel + +RC Channel to use for OSD switch indicator 0. + +| Default | Min | Max | +| --- | --- | --- | +| 5 | 5 | MAX_SUPPORTED_RC_CHANNEL_COUNT | + +--- + +### osd_switch_indicator_zero_name + +Character to use for OSD switch incicator 0. + +| Default | Min | Max | +| --- | --- | --- | +| FLAP | | 5 | + +--- + +### osd_switch_indicators_align_left + +Align text to left of switch indicators + +| Default | Min | Max | +| --- | --- | --- | +| ON | OFF | ON | + +--- + ### osd_system_msg_display_time System message display cycle time for multiple messages (milliseconds). @@ -5294,7 +5394,7 @@ Selects the servo PWM output cutoff frequency. Value is in [Hz] ### servo_protocol -An option to chose the protocol/option that would be used to output servo data. Possible options `PWM` (FC servo outputs), `SERVO_DRIVER` (I2C PCA9685 peripheral), `SBUS` (S.Bus protocol output via a configured serial port) +An option to chose the protocol/option that would be used to output servo data. Possible options `PWM` (FC servo outputs), `SBUS` (S.Bus protocol output via a configured serial port) | Default | Min | Max | | --- | --- | --- | @@ -5328,7 +5428,7 @@ Quality factor of the setpoint Kalman filter. Higher values means less filtering | Default | Min | Max | | --- | --- | --- | -| 100 | 1 | 16000 | +| 100 | 1 | 1000 | --- @@ -5758,7 +5858,7 @@ Configure the VTX band. Set to zero to use `vtx_freq`. Bands: 1: A, 2: B, 3: E, | Default | Min | Max | | --- | --- | --- | -| 4 | VTX_SETTINGS_NO_BAND | VTX_SETTINGS_MAX_BAND | +| 1 | VTX_SETTINGS_NO_BAND | VTX_SETTINGS_MAX_BAND | --- @@ -5772,6 +5872,16 @@ Channel to use within the configured `vtx_band`. Valid values are [1, 8]. --- +### vtx_frequency_group + +VTx Frequency group to use. Frequency groups: FREQUENCYGROUP_5G8: 5.8GHz, FREQUENCYGROUP_2G4: 2.4GHz, FREQUENCYGROUP_1G3: 1.3GHz. + +| Default | Min | Max | +| --- | --- | --- | +| FREQUENCYGROUP_5G8 | 0 | 2 | + +--- + ### vtx_halfduplex Use half duplex UART to communicate with the VTX, using only a TX pin in the FC. @@ -5842,6 +5952,26 @@ Enable workaround for early AKK SAudio-enabled VTX bug. --- +### vtx_smartaudio_stopbits + +Set stopbit count for serial (TBS Sixty9 SmartAudio 2.1 require value of 1 bit) + +| Default | Min | Max | +| --- | --- | --- | +| 2 | 1 | 2 | + +--- + +### vtx_softserial_shortstop + +Enable the 3x shorter stopbit on softserial. Need for some IRC Tramp VTXes. + +| Default | Min | Max | +| --- | --- | --- | +| OFF | OFF | ON | + +--- + ### yaw_deadband These are values (in us) by how much RC input can be different before it's considered valid. For transmitters with jitter on outputs, this value can be increased. Defaults are zero, but can be increased up to 10 or so if rc inputs twitch while idle. diff --git a/docs/Telemetry.md b/docs/Telemetry.md index af8bb35962..930e9bf9ae 100644 --- a/docs/Telemetry.md +++ b/docs/Telemetry.md @@ -10,10 +10,9 @@ Telemetry is enabled using the 'TELEMETRY' feature. feature TELEMETRY ``` -Multiple telemetry providers are currently supported, FrSky, Graupner -HoTT V4, SmartPort (S.Port) and LightTelemetry (LTM) +Multiple telemetry providers are currently supported, FrSky, Graupner HoTT V4, SmartPort (S.Port), LightTelemetry (LTM). MAVLink, IBUS, Crossfire and GSM SMS. -All telemetry systems use serial ports, configure serial ports to use the telemetry system required. +All telemetry systems use serial ports, configure serial ports to use the telemetry system required. Multiple telemetry streams may be enabled, but only one of each type, e.g. Smartport + LTM or MAVLink + CRSF. ## SmartPort (S.Port) telemetry @@ -115,7 +114,7 @@ The following sensors are transmitted * **C** : number of satellites locked (digit C & D are the number of locked satellites) * **D** : number of satellites locked (if 14 satellites are locked, C = 1 & D = 4) * **GAlt** : GPS altitude, sea level is zero. -* **ASpd** : true air speed, from pitot sensor. +* **ASpd** : true air speed, from pitot sensor. This is _Knots * 10_ * **A4** : average cell value. Warning : unlike FLVSS and MLVSS sensors, you do not get actual lowest value of a cell, but an average : (total lipo voltage) / (number of cells) * **0420** : distance to GPS home fix, in meters * **0430** : if `frsky_pitch_roll = ON` set this will be pitch degrees*10 @@ -204,50 +203,38 @@ Note: The SoftSerial ports may not be 5V tolerant on your board. Verify if you ## LightTelemetry (LTM) -LTM is a lightweight streaming telemetry protocol supported by a -number of OSDs, ground stations and antenna trackers. +LTM is a lightweight streaming telemetry protocol supported by a number of OSDs, ground stations and antenna trackers. The INAV implementation of LTM implements the following frames: -* G-FRAME: GPS information (lat, long, ground speed, altitude, sat - info) +* G-FRAME: GPS information (lat, long, ground speed, altitude, sat info) * A-FRAME: Attitude (pitch, roll, heading) -* S-FRAME: Status (voltage, current+, RSSI, airspeed+, status). Item - suffixed '+' not implemented in INAV. +* S-FRAME: Status (voltage, current+, RSSI, airspeed+, status). Item suffixed '+' not implemented in INAV. * O-FRAME: Origin (home position, lat, long, altitude, fix) In addition, in iNav: -* N-FRAME: Navigation information (GPS mode, Nav mode, Nav action, - Waypoint number, Nav Error, Nav Flags). +* N-FRAME: Navigation information (GPS mode, Nav mode, Nav action, Waypoint number, Nav Error, Nav Flags). * X-FRAME: Extra information. Currently HDOP is reported. -LTM is transmit only, and can work at any supported baud rate. It is -designed to operate over 2400 baud (9600 in INAV) and does not -benefit from higher rates. It is thus usable on soft serial. +LTM is transmit only, and can work at any supported baud rate. It is designed to operate over 2400 baud (9600 in INAV) and does not benefit from higher rates. It is thus usable on soft serial. -A CLI variable `ltm_update_rate` may be used to configure the update -rate and hence band-width used by LTM, with the following enumerations: +A CLI variable `ltm_update_rate` may be used to configure the update rate and hence band-width used by LTM, with the following enumerations: * NORMAL: Legacy rate, currently 303 bytes/second (requires 4800 bps) * MEDIUM: 164 bytes/second (requires 2400 bps) * SLOW: 105 bytes/second (requires 1200 bps) -For many telemetry devices, there is direction correlation between the -air-speed of the radio link and range; thus a lower value may -facilitate longer range links. +For many telemetry devices, there is direction correlation between the air-speed of the radio link and range; thus a lower value may facilitate longer range links. -More information about the fields, encoding and enumerations may be -found at https://github.com/iNavFlight/inav/wiki/Lightweight-Telemetry-(LTM). +More information about the fields, encoding and enumerations may be found [on the wiki](https://github.com/iNavFlight/inav/wiki/Lightweight-Telemetry-(LTM)). ## MAVLink telemetry -MAVLink is a very lightweight, header-only message marshalling library for micro air vehicles. -INAV supports MAVLink for compatibility with ground stations, OSDs and antenna trackers built -for PX4, PIXHAWK, APM and Parrot AR.Drone platforms. +MAVLink is a lightweight header-only message marshalling library for micro air vehicles. INAV supports MAVLink for compatibility with ground stations, OSDs and antenna trackers built for PX4, PIXHAWK, APM and Parrot AR.Drone platforms. -MAVLink implementation in INAV is transmit-only and usable on low baud rates and can be used over soft serial. +MAVLink implementation in INAV is transmit-only and usable on low baud rates and can be used over soft serial (requires 19200 baud). MAVLink V1 and V2 are supported. ## Cellular telemetry via text messages @@ -375,7 +362,7 @@ sensor 10 is of type GALT, sensor 12 is of type GPS_LON, sensor 13 is of type GPS_LAT, sensor 14 is of type ACC_X, -sensor 15 is of type ACC_Y, +sensor 15 is of type ACC_Y, sensor 16 is of type SPEED. 4.This same as 3, but support 4 byte sensors. (fix_updater_03_16_21_33_1 from https://github.com/qba667/FlySkyI6/tree/master/release): diff --git a/docs/VTx.md b/docs/VTx.md new file mode 100644 index 0000000000..8244ec4e85 --- /dev/null +++ b/docs/VTx.md @@ -0,0 +1,19 @@ +## VTx Setup + +### IRC Tramp + +#### Matek 1G3SE Setup + +To use the Matek 1G3SE with IRC Tramp. You will need to enter the CLI command `set vtx_frequency_group = FREQUENCYGROUP_1G3`. You must also make sure that the initial VTx settings in the configuration tab are in a valid range. They are: +- `vtx_band` 1 or 2 +- `vtx_channel` between 1 and 9 + +### Team BlackSheep SmartAudio + +If you have problems getting SmartAudio working. There are a couple of CLI parameters you can try changing to see if they help. + +- There is a workaround for early AKK VTx modules. This is enabled by default. You could try disabling this setting [`vtx_smartaudio_early_akk_workaround`](https://github.com/iNavFlight/inav/blob/master/docs/Settings.md#vtx_smartaudio_early_akk_workaround) to OFF. + +- If you are using softserial, you can try using the alternate method by setting [`vtx_smartaudio_alternate_softserial_method`](https://github.com/iNavFlight/inav/blob/master/docs/Settings.md#vtx_smartaudio_alternate_softserial_method) to OFF. + +- If you are using TBS Sixty9 VTX you may consider to set count of stop bits to 1, using [`set vtx_smartaudio_stopbits = 1`](https://github.com/iNavFlight/inav/blob/master/docs/Settings.md#vtx_smartaudio_stopbits) diff --git a/docs/Wireless Connections (BLE, TCP and UDP).md b/docs/Wireless Connections (BLE, TCP and UDP).md new file mode 100644 index 0000000000..e0c4189b0d --- /dev/null +++ b/docs/Wireless Connections (BLE, TCP and UDP).md @@ -0,0 +1,53 @@ +# Wireless connections + +From iNav 5 onwards, the Configurator supports wireless connections via Bluetooth Low Energy (BLE) and Wifi (UDP and TCP). + +## BLE + +The following adapters are supported: + +- CC2541 based modules (HM1X, HC08/09) +- Nordic Semiconductor NRF5340 (Adafruit BLE Shield) +- SpeedyBee adapter + +Flightcontrollers with BLE should also work, if you have an adapter/FC that doesn't work, open an issue here on Github and we will add it. + +### Configuring the BLE modules +Activate MSP in iNav on a free UART port and set the Bluetooth module to the appropriate baud rate. + +Example for a HM-10 module: + +Connect the module to a USB/UART adapter (remember: RX to TX, TX to RX), and connect it to a serial terminal (e.g. from the Arduino IDE), +Standard baud rate is 115200 baud, CR+LF + +``` +AT+BAUD4 +AT+NAMEiNav +``` + +The baud rate values: +| Value | Baud | +|------|------| +| 1 | 9600 | +| 2 | 19200 | +| 3 | 38400 | +| 4 | 115200 | + +There are many counterfeits of the HC08/09 modules on the market, which work unreliably at high baud rates. +However, it is recommended to avoid these modules and to use an original HM-10. + +### SpeedyBee adapter + +Just connect it to the USB port, no further configuration needed. + +## TCP and UDP + +Allows connections via Wifi. + +Hardware: +- DIY, ESP8266 based: + This project can be used to make iNav Wifi enabled: https://github.com/Scavanger/MSPWifiBridge + A small ESP01S module should still fit anywhere. + +- ExpressLRS Wifi: + Should work (via TCP, port 5761), but untested due to lack of hardware from the developer. CLI and presets do not work here, problem in ELRS, not in iNav. diff --git a/docs/assets/images/StickPositions.png b/docs/assets/images/StickPositions.png index 8ecf4ca4af..68e9105d80 100644 Binary files a/docs/assets/images/StickPositions.png and b/docs/assets/images/StickPositions.png differ diff --git a/docs/assets/images/StickPositions_trans.png b/docs/assets/images/StickPositions_trans.png index 4f7365952d..8ecf4ca4af 100644 Binary files a/docs/assets/images/StickPositions_trans.png and b/docs/assets/images/StickPositions_trans.png differ diff --git a/docs/boards/Omnibus F4.md b/docs/boards/Omnibus F4.md index be323faccc..3c6b78f10a 100644 --- a/docs/boards/Omnibus F4.md +++ b/docs/boards/Omnibus F4.md @@ -71,6 +71,7 @@ More target options: * Integrated current meter * Uses target **OMNIBUSF4PRO** * Omnibus F4 Pro clones (Banggood, AliExpress, eBay, etc.) use **OMNIBUSF4PRO_LEDSTRIPM5** target (LED strip on M5 pin instead of incorrectly wired dedicated connection) +* If you want to power your servos from the servo rail you will have to bridge the 5v to another 5v pad from the board or take it from the esc ### Omnibus F4 Pro Corner diff --git a/docs/development/Development.md b/docs/development/Development.md index 9be10a3da4..03dbc5b095 100755 --- a/docs/development/Development.md +++ b/docs/development/Development.md @@ -46,21 +46,45 @@ Note: Tests are written in C++ and linked with with firmware's C code. ### Running the tests. -The tests and test build system is very simple and based off the googletest example files, it will be improved in due course. From the root folder of the project simply do: +The tests and test build system is very simple and based off the googletest example files, it may be improved in due course. From the root folder of the project simply do: + +Test are configured from the top level directory. It is recommended to use a separate test directory, here named `testing`. ``` -make test +mkdir testing +cd testing +# define NULL toolchain ... +cmake -DTOOLCHAIN= .. +# Run the tests +make check ``` -This will build a set of executable files in the `obj/test` folder, one for each `*_unittest.cc` file. +This will build a set of executable files in the `src/test/unit` folder (below `testing`), one for each `*_unittest.cc` file. -After they have been executed by the make invocation, you can still run them on the command line to execute the tests and to see the test report. +After they have been executed by the make invocation, you can still run them on the command line to execute the tests and to see the test report, for example: -You can also step-debug the tests in eclipse and you can use the GoogleTest test runner to make building and re-running the tests simple. +``` +src/test/unit/time_unittest +Running main() from /home/jrh/Projects/fc/inav/testing/src/test/googletest-src/googletest/src/gtest_main.cc +[==========] Running 2 tests from 1 test suite. +[----------] Global test environment set-up. +[----------] 2 tests from TimeUnittest +[ RUN ] TimeUnittest.TestMillis +[ OK ] TimeUnittest.TestMillis (0 ms) +[ RUN ] TimeUnittest.TestMicros +[ OK ] TimeUnittest.TestMicros (0 ms) +[----------] 2 tests from TimeUnittest (0 ms total) + +[----------] Global test environment tear-down +[==========] 2 tests from 1 test suite ran. (0 ms total) +[ PASSED ] 2 tests. +``` + +You can also step-debug the tests in `gdb` (or IDE debugger). 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. +Tests are verified and working with (native) GCC 11.20. ## Using git and github diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index b74b0ba391..1c968c8cf5 100644 --- a/src/main/CMakeLists.txt +++ b/src/main/CMakeLists.txt @@ -180,8 +180,6 @@ main_sources(COMMON_SRC drivers/flash_m25p16.h drivers/io.c drivers/io.h - drivers/io_pca9685.c - drivers/io_pca9685.h drivers/io_pcf8574.c drivers/io_pcf8574.h drivers/io_port_expander.c @@ -354,8 +352,6 @@ main_sources(COMMON_SRC io/lights.h io/piniobox.c io/piniobox.h - io/pwmdriver_i2c.c - io/pwmdriver_i2c.h io/serial.c io/serial.h io/serial_4way.c diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c index 60105587ba..4a88776267 100644 --- a/src/main/cms/cms_menu_imu.c +++ b/src/main/cms/cms_menu_imu.c @@ -304,11 +304,6 @@ static const OSD_Entry cmsx_menuRateProfileEntries[] = { OSD_LABEL_DATA_ENTRY("-- RATE --", profileIndexString), -#if 0 - { "RC RATE", OME_FLOAT, NULL, &(OSD_FLOAT_t){ &rateProfile.rcRate8, 0, 255, 1, 10 }, 0 }, - { "RC YAW RATE", OME_FLOAT, NULL, &(OSD_FLOAT_t){ &rateProfile.rcYawRate8, 0, 255, 1, 10 }, 0 }, -#endif - OSD_SETTING_ENTRY_TYPE("ROLL RATE", SETTING_ROLL_RATE, CMS_DATA_TYPE_ANGULAR_RATE), OSD_SETTING_ENTRY_TYPE("PITCH RATE", SETTING_PITCH_RATE, CMS_DATA_TYPE_ANGULAR_RATE), OSD_SETTING_ENTRY_TYPE("YAW RATE", SETTING_YAW_RATE, CMS_DATA_TYPE_ANGULAR_RATE), diff --git a/src/main/common/maths.h b/src/main/common/maths.h index 3a3d9b86cc..e17281017f 100644 --- a/src/main/common/maths.h +++ b/src/main/common/maths.h @@ -71,6 +71,12 @@ #define C_TO_KELVIN(temp) (temp + 273.15f) +// Standard Sea Level values +// Ref:https://en.wikipedia.org/wiki/Standard_sea_level +#define SSL_AIR_DENSITY 1.225f // kg/m^3 +#define SSL_AIR_PRESSURE 101325.01576f // Pascal +#define SSL_AIR_TEMPERATURE 288.15f // K + // copied from https://code.google.com/p/cxutil/source/browse/include/cxutil/utility.h#70 #define _CHOOSE2(binoper, lexpr, lvar, rexpr, rvar) \ ( __extension__ ({ \ diff --git a/src/main/common/utils.h b/src/main/common/utils.h index eaef1f1efa..e585d96277 100644 --- a/src/main/common/utils.h +++ b/src/main/common/utils.h @@ -78,11 +78,6 @@ http://resnet.uoregon.edu/~gurney_j/jmpc/bitwise.html + LOG2_32BIT((v)*1L >>16*((v)/2L>>31 > 0) \ >>16*((v)/2L>>31 > 0))) -#if 0 -// ISO C version, but no type checking -#define container_of(ptr, type, member) \ - ((type *) ((char *)(ptr) - offsetof(type, member))) -#else // non ISO variant from linux kernel; checks ptr type, but triggers 'ISO C forbids braced-groups within expressions [-Wpedantic]' // __extension__ is here to disable this warning #define container_of(ptr, type, member) ( __extension__ ({ \ @@ -102,9 +97,6 @@ static inline void memcpy_fn ( void * destination, const void * source, size_t void * memcpy_fn ( void * destination, const void * source, size_t num ) asm("memcpy"); #endif - -#endif - #if __GNUC__ > 6 #define FALLTHROUGH __attribute__ ((fallthrough)) #else diff --git a/src/main/config/parameter_group.h b/src/main/config/parameter_group.h index 1c53225cb4..9594ad96c4 100644 --- a/src/main/config/parameter_group.h +++ b/src/main/config/parameter_group.h @@ -183,14 +183,6 @@ extern const uint8_t __pg_resetdata_end[]; PG_REGISTER_ARRAY_I(_type, _size, _name, _pgn, _version, .reset = {.fn = (pgResetFunc*)&pgResetFn_ ## _name}) \ /**/ -#if 0 -// ARRAY reset mechanism is not implemented yet, only few places in code would benefit from it - See pgResetInstance -#define PG_REGISTER_ARRAY_WITH_RESET_TEMPLATE(_type, _size, _name, _pgn, _version) \ - extern const _type pgResetTemplate_ ## _name; \ - PG_REGISTER_ARRAY_I(_type, _size, _name, _pgn, _version, .reset = {.ptr = (void*)&pgResetTemplate_ ## _name}) \ - /**/ -#endif - #ifdef UNIT_TEST # define _PG_PROFILE_CURRENT_DECL(_type, _name) \ _type *_name ## _ProfileCurrent = &_name ## _Storage[0]; diff --git a/src/main/drivers/bus.h b/src/main/drivers/bus.h index 8b48855905..91d4077bf4 100755 --- a/src/main/drivers/bus.h +++ b/src/main/drivers/bus.h @@ -139,7 +139,6 @@ typedef enum { /* Other hardware */ DEVHW_MS4525, // Pitot meter - DEVHW_PCA9685, // PWM output device DEVHW_M25P16, // SPI NOR flash DEVHW_UG2864, // I2C OLED display DEVHW_SDCARD, // Generic SD-Card diff --git a/src/main/drivers/bus_spi.h b/src/main/drivers/bus_spi.h index 2f5f8e0be7..ad78f1ff5c 100644 --- a/src/main/drivers/bus_spi.h +++ b/src/main/drivers/bus_spi.h @@ -69,7 +69,13 @@ typedef struct SPIDevice_s { ioTag_t mosi; ioTag_t miso; rccPeriphTag_t rcc; +#if defined(STM32F7) || defined(STM32H7) + uint8_t sckAF; + uint8_t misoAF; + uint8_t mosiAF; +#else uint8_t af; +#endif const uint32_t * divisorMap; volatile uint16_t errorCount; bool initDone; diff --git a/src/main/drivers/bus_spi_hal_ll.c b/src/main/drivers/bus_spi_hal_ll.c index 25662f2f2d..fac8a82761 100644 --- a/src/main/drivers/bus_spi_hal_ll.c +++ b/src/main/drivers/bus_spi_hal_ll.c @@ -91,22 +91,53 @@ static const uint32_t spiDivisorMapSlow[] = { #if defined(STM32H7) static spiDevice_t spiHardwareMap[SPIDEV_COUNT] = { #ifdef USE_SPI_DEVICE_1 - { .dev = SPI1, .nss = IO_TAG(SPI1_NSS_PIN), .sck = IO_TAG(SPI1_SCK_PIN), .miso = IO_TAG(SPI1_MISO_PIN), .mosi = IO_TAG(SPI1_MOSI_PIN), .rcc = RCC_APB2(SPI1), .af = GPIO_AF5_SPI1, .divisorMap = spiDivisorMapFast }, +#if defined(SPI1_SCK_AF) || defined(SPI1_MISO_AF) || defined(SPI1_MOSI_AF) +#if !defined(SPI1_SCK_AF) || !defined(SPI1_MISO_AF) || !defined(SPI1_MOSI_AF) +#error SPI1: SCK, MISO and MOSI AFs should be defined together in target.h! +#endif + { .dev = SPI1, .nss = IO_TAG(SPI1_NSS_PIN), .sck = IO_TAG(SPI1_SCK_PIN), .miso = IO_TAG(SPI1_MISO_PIN), .mosi = IO_TAG(SPI1_MOSI_PIN), .rcc = RCC_APB2(SPI1), .sckAF = SPI1_SCK_AF, .misoAF = SPI1_MISO_AF, .mosiAF = SPI1_MOSI_AF, .divisorMap = spiDivisorMapFast }, +#else + { .dev = SPI1, .nss = IO_TAG(SPI1_NSS_PIN), .sck = IO_TAG(SPI1_SCK_PIN), .miso = IO_TAG(SPI1_MISO_PIN), .mosi = IO_TAG(SPI1_MOSI_PIN), .rcc = RCC_APB2(SPI1), .sckAF = GPIO_AF5_SPI1, .misoAF = GPIO_AF5_SPI1, .mosiAF = GPIO_AF5_SPI1, .divisorMap = spiDivisorMapFast }, +#endif #else { .dev = NULL }, // No SPI1 #endif + #ifdef USE_SPI_DEVICE_2 - { .dev = SPI2, .nss = IO_TAG(SPI2_NSS_PIN), .sck = IO_TAG(SPI2_SCK_PIN), .miso = IO_TAG(SPI2_MISO_PIN), .mosi = IO_TAG(SPI2_MOSI_PIN), .rcc = RCC_APB1L(SPI2), .af = GPIO_AF5_SPI2, .divisorMap = spiDivisorMapSlow }, +#if defined(SPI2_SCK_AF) || defined(SPI2_MISO_AF) || defined(SPI2_MOSI_AF) +#if !defined(SPI2_SCK_AF) || !defined(SPI2_MISO_AF) || !defined(SPI2_MOSI_AF) +#error SPI2: SCK, MISO and MOSI AFs should be defined together in target.h! +#endif + { .dev = SPI2, .nss = IO_TAG(SPI2_NSS_PIN), .sck = IO_TAG(SPI2_SCK_PIN), .miso = IO_TAG(SPI2_MISO_PIN), .mosi = IO_TAG(SPI2_MOSI_PIN), .rcc = RCC_APB1L(SPI2), .sckAF = SPI2_SCK_AF, .misoAF = SPI2_MISO_AF, .mosiAF = SPI2_MOSI_AF, .divisorMap = spiDivisorMapSlow }, +#else + { .dev = SPI2, .nss = IO_TAG(SPI2_NSS_PIN), .sck = IO_TAG(SPI2_SCK_PIN), .miso = IO_TAG(SPI2_MISO_PIN), .mosi = IO_TAG(SPI2_MOSI_PIN), .rcc = RCC_APB1L(SPI2), .sckAF = GPIO_AF5_SPI2, .misoAF = GPIO_AF5_SPI2, .mosiAF = GPIO_AF5_SPI2, .divisorMap = spiDivisorMapSlow }, +#endif #else { .dev = NULL }, // No SPI2 #endif + #ifdef USE_SPI_DEVICE_3 - { .dev = SPI3, .nss = IO_TAG(SPI3_NSS_PIN), .sck = IO_TAG(SPI3_SCK_PIN), .miso = IO_TAG(SPI3_MISO_PIN), .mosi = IO_TAG(SPI3_MOSI_PIN), .rcc = RCC_APB1L(SPI3), .af = GPIO_AF6_SPI3, .divisorMap = spiDivisorMapSlow }, +#if defined(SPI3_SCK_AF) || defined(SPI3_MISO_AF) || defined(SPI3_MOSI_AF) +#if !defined(SPI3_SCK_AF) || !defined(SPI3_MISO_AF) || !defined(SPI3_MOSI_AF) +#error SPI3: SCK, MISO and MOSI AFs should be defined together in target.h! +#endif + { .dev = SPI3, .nss = IO_TAG(SPI3_NSS_PIN), .sck = IO_TAG(SPI3_SCK_PIN), .miso = IO_TAG(SPI3_MISO_PIN), .mosi = IO_TAG(SPI3_MOSI_PIN), .rcc = RCC_APB1L(SPI3), .sckAF = SPI3_SCK_AF, .misoAF = SPI3_MISO_AF, .mosiAF = SPI3_MOSI_AF, .divisorMap = spiDivisorMapSlow }, +#else + { .dev = SPI3, .nss = IO_TAG(SPI3_NSS_PIN), .sck = IO_TAG(SPI3_SCK_PIN), .miso = IO_TAG(SPI3_MISO_PIN), .mosi = IO_TAG(SPI3_MOSI_PIN), .rcc = RCC_APB1L(SPI3), .sckAF = GPIO_AF6_SPI3, .misoAF = GPIO_AF6_SPI3, .mosiAF = GPIO_AF6_SPI3, .divisorMap = spiDivisorMapSlow }, +#endif #else { .dev = NULL }, // No SPI3 #endif + #ifdef USE_SPI_DEVICE_4 - { .dev = SPI4, .nss = IO_TAG(SPI4_NSS_PIN), .sck = IO_TAG(SPI4_SCK_PIN), .miso = IO_TAG(SPI4_MISO_PIN), .mosi = IO_TAG(SPI4_MOSI_PIN), .rcc = RCC_APB2(SPI4), .af = GPIO_AF5_SPI4, .divisorMap = spiDivisorMapSlow } +#if defined(SPI4_SCK_AF) || defined(SPI4_MISO_AF) || defined(SPI4_MOSI_AF) +#if !defined(SPI4_SCK_AF) || !defined(SPI4_MISO_AF) || !defined(SPI4_MOSI_AF) +#error SPI4: SCK, MISO and MOSI AFs should be defined together in target.h! +#endif + { .dev = SPI4, .nss = IO_TAG(SPI4_NSS_PIN), .sck = IO_TAG(SPI4_SCK_PIN), .miso = IO_TAG(SPI4_MISO_PIN), .mosi = IO_TAG(SPI4_MOSI_PIN), .rcc = RCC_APB2(SPI4), .sckAF = SPI4_SCK_AF, .misoAF = SPI4_MISO_AF, .mosiAF = SPI4_MOSI_AF, .divisorMap = spiDivisorMapSlow } +#else + { .dev = SPI4, .nss = IO_TAG(SPI4_NSS_PIN), .sck = IO_TAG(SPI4_SCK_PIN), .miso = IO_TAG(SPI4_MISO_PIN), .mosi = IO_TAG(SPI4_MOSI_PIN), .rcc = RCC_APB2(SPI4), .sckAF = GPIO_AF5_SPI4, .misoAF = GPIO_AF5_SPI4, .mosiAF = GPIO_AF5_SPI4, .divisorMap = spiDivisorMapSlow } +#endif #else { .dev = NULL } // No SPI4 #endif @@ -114,22 +145,53 @@ static spiDevice_t spiHardwareMap[SPIDEV_COUNT] = { #else static spiDevice_t spiHardwareMap[] = { #ifdef USE_SPI_DEVICE_1 - { .dev = SPI1, .nss = IO_TAG(SPI1_NSS_PIN), .sck = IO_TAG(SPI1_SCK_PIN), .miso = IO_TAG(SPI1_MISO_PIN), .mosi = IO_TAG(SPI1_MOSI_PIN), .rcc = RCC_APB2(SPI1), .af = GPIO_AF5_SPI1, .divisorMap = spiDivisorMapFast }, +#if defined(SPI1_SCK_AF) || defined(SPI1_MISO_AF) || defined(SPI1_MOSI_AF) +#if !defined(SPI1_SCK_AF) || !defined(SPI1_MISO_AF) || !defined(SPI1_MOSI_AF) +#error SPI1: SCK, MISO and MOSI AFs should be defined together in target.h! +#endif + { .dev = SPI1, .nss = IO_TAG(SPI1_NSS_PIN), .sck = IO_TAG(SPI1_SCK_PIN), .miso = IO_TAG(SPI1_MISO_PIN), .mosi = IO_TAG(SPI1_MOSI_PIN), .rcc = RCC_APB2(SPI1), .sckAF = SPI1_SCK_AF, .misoAF = SPI1_MISO_AF, .mosiAF = SPI1_MOSI_AF, .divisorMap = spiDivisorMapFast }, +#else + { .dev = SPI1, .nss = IO_TAG(SPI1_NSS_PIN), .sck = IO_TAG(SPI1_SCK_PIN), .miso = IO_TAG(SPI1_MISO_PIN), .mosi = IO_TAG(SPI1_MOSI_PIN), .rcc = RCC_APB2(SPI1), .sckAF = GPIO_AF5_SPI1, .misoAF = GPIO_AF5_SPI1, .mosiAF = GPIO_AF5_SPI1, .divisorMap = spiDivisorMapFast }, +#endif #else { .dev = NULL }, // No SPI1 #endif + #ifdef USE_SPI_DEVICE_2 - { .dev = SPI2, .nss = IO_TAG(SPI2_NSS_PIN), .sck = IO_TAG(SPI2_SCK_PIN), .miso = IO_TAG(SPI2_MISO_PIN), .mosi = IO_TAG(SPI2_MOSI_PIN), .rcc = RCC_APB1(SPI2), .af = GPIO_AF5_SPI2, .divisorMap = spiDivisorMapSlow }, +#if defined(SPI2_SCK_AF) || defined(SPI2_MISO_AF) || defined(SPI2_MOSI_AF) +#if !defined(SPI2_SCK_AF) || !defined(SPI2_MISO_AF) || !defined(SPI2_MOSI_AF) +#error SPI2: SCK, MISO and MOSI AFs should be defined together in target.h! +#endif + { .dev = SPI2, .nss = IO_TAG(SPI2_NSS_PIN), .sck = IO_TAG(SPI2_SCK_PIN), .miso = IO_TAG(SPI2_MISO_PIN), .mosi = IO_TAG(SPI2_MOSI_PIN), .rcc = RCC_APB1(SPI2), .sckAF = SPI2_SCK_AF, .misoAF = SPI2_MISO_AF, .mosiAF = SPI2_MOSI_AF, .divisorMap = spiDivisorMapSlow }, +#else + { .dev = SPI2, .nss = IO_TAG(SPI2_NSS_PIN), .sck = IO_TAG(SPI2_SCK_PIN), .miso = IO_TAG(SPI2_MISO_PIN), .mosi = IO_TAG(SPI2_MOSI_PIN), .rcc = RCC_APB1(SPI2), .sckAF = GPIO_AF5_SPI2, .misoAF = GPIO_AF5_SPI2, .mosiAF = GPIO_AF5_SPI2, .divisorMap = spiDivisorMapSlow }, +#endif #else { .dev = NULL }, // No SPI2 #endif + #ifdef USE_SPI_DEVICE_3 - { .dev = SPI3, .nss = IO_TAG(SPI3_NSS_PIN), .sck = IO_TAG(SPI3_SCK_PIN), .miso = IO_TAG(SPI3_MISO_PIN), .mosi = IO_TAG(SPI3_MOSI_PIN), .rcc = RCC_APB1(SPI3), .af = GPIO_AF6_SPI3, .divisorMap = spiDivisorMapSlow }, +#if defined(SPI3_SCK_AF) || defined(SPI3_MISO_AF) || defined(SPI3_MOSI_AF) +#if !defined(SPI3_SCK_AF) || !defined(SPI3_MISO_AF) || !defined(SPI3_MOSI_AF) +#error SPI3: SCK, MISO and MOSI AFs should be defined together in target.h! +#endif + { .dev = SPI3, .nss = IO_TAG(SPI3_NSS_PIN), .sck = IO_TAG(SPI3_SCK_PIN), .miso = IO_TAG(SPI3_MISO_PIN), .mosi = IO_TAG(SPI3_MOSI_PIN), .rcc = RCC_APB1(SPI3), .sckAF = SPI3_SCK_AF, .misoAF = SPI3_MISO_AF, .mosiAF = SPI3_MOSI_AF, .divisorMap = spiDivisorMapSlow }, +#else + { .dev = SPI3, .nss = IO_TAG(SPI3_NSS_PIN), .sck = IO_TAG(SPI3_SCK_PIN), .miso = IO_TAG(SPI3_MISO_PIN), .mosi = IO_TAG(SPI3_MOSI_PIN), .rcc = RCC_APB1(SPI3), .sckAF = GPIO_AF6_SPI3, .misoAF = GPIO_AF6_SPI3, .mosiAF = GPIO_AF6_SPI3, .divisorMap = spiDivisorMapSlow }, +#endif #else { .dev = NULL }, // No SPI3 #endif + #ifdef USE_SPI_DEVICE_4 - { .dev = SPI4, .nss = IO_TAG(SPI4_NSS_PIN), .sck = IO_TAG(SPI4_SCK_PIN), .miso = IO_TAG(SPI4_MISO_PIN), .mosi = IO_TAG(SPI4_MOSI_PIN), .rcc = RCC_APB2(SPI4), .af = GPIO_AF5_SPI4, .divisorMap = spiDivisorMapSlow } +#if defined(SPI4_SCK_AF) || defined(SPI4_MISO_AF) || defined(SPI4_MOSI_AF) +#if !defined(SPI4_SCK_AF) || !defined(SPI4_MISO_AF) || !defined(SPI4_MOSI_AF) +#error SPI3: SCK, MISO and MOSI AFs should be defined together in target.h! +#endif + { .dev = SPI4, .nss = IO_TAG(SPI4_NSS_PIN), .sck = IO_TAG(SPI4_SCK_PIN), .miso = IO_TAG(SPI4_MISO_PIN), .mosi = IO_TAG(SPI4_MOSI_PIN), .rcc = RCC_APB2(SPI4), .sckAF = SPI4_SCK_AF, .misoAF = SPI4_MISO_AF, .mosiAF = SPI4_MOSI_AF, .divisorMap = spiDivisorMapSlow } +#else + { .dev = SPI4, .nss = IO_TAG(SPI4_NSS_PIN), .sck = IO_TAG(SPI4_SCK_PIN), .miso = IO_TAG(SPI4_MISO_PIN), .mosi = IO_TAG(SPI4_MOSI_PIN), .rcc = RCC_APB2(SPI4), .sckAF = GPIO_AF5_SPI4, .misoAF = GPIO_AF5_SPI4, .mosiAF = GPIO_AF5_SPI4, .divisorMap = spiDivisorMapSlow } +#endif #else { .dev = NULL } // No SPI4 #endif @@ -184,12 +246,13 @@ bool spiInitDevice(SPIDevice device, bool leadingEdge) IOInit(IOGetByTag(spi->mosi), OWNER_SPI, RESOURCE_SPI_MOSI, device + 1); if (leadingEdge) { - IOConfigGPIOAF(IOGetByTag(spi->sck), SPI_IO_AF_SCK_CFG_LOW, spi->af); + IOConfigGPIOAF(IOGetByTag(spi->sck), SPI_IO_AF_SCK_CFG_LOW, spi->sckAF); } else { - IOConfigGPIOAF(IOGetByTag(spi->sck), SPI_IO_AF_SCK_CFG_HIGH, spi->af); + IOConfigGPIOAF(IOGetByTag(spi->sck), SPI_IO_AF_SCK_CFG_HIGH, spi->sckAF); } - IOConfigGPIOAF(IOGetByTag(spi->miso), SPI_IO_AF_MISO_CFG, spi->af); - IOConfigGPIOAF(IOGetByTag(spi->mosi), SPI_IO_AF_CFG, spi->af); + IOConfigGPIOAF(IOGetByTag(spi->miso), SPI_IO_AF_MISO_CFG, spi->misoAF); + + IOConfigGPIOAF(IOGetByTag(spi->mosi), SPI_IO_AF_CFG, spi->mosiAF); if (spi->nss) { IOInit(IOGetByTag(spi->nss), OWNER_SPI, RESOURCE_SPI_CS, device + 1); diff --git a/src/main/drivers/flash.c b/src/main/drivers/flash.c index 647ac675a0..dd961fc5f5 100644 --- a/src/main/drivers/flash.c +++ b/src/main/drivers/flash.c @@ -90,29 +90,6 @@ void flashEraseCompletely(void) #endif } -#if 0 -void flashPageProgramBegin(uint32_t address) -{ -#ifdef USE_FLASH_M25P16 - return m25p16_pageProgramBegin(address); -#endif -} - -void flashPageProgramContinue(const uint8_t *data, int length) -{ -#ifdef USE_FLASH_M25P16 - return m25p16_pageProgramContinue(data, length); -#endif -} - -void flashPageProgramFinish(void) -{ -#ifdef USE_FLASH_M25P16 - return m25p16_pageProgramFinish(); -#endif -} -#endif - uint32_t flashPageProgram(uint32_t address, const uint8_t *data, int length) { #ifdef USE_FLASH_M25P16 diff --git a/src/main/drivers/flash.h b/src/main/drivers/flash.h index 9548a8e1ed..e554810705 100644 --- a/src/main/drivers/flash.h +++ b/src/main/drivers/flash.h @@ -42,11 +42,6 @@ bool flashIsReady(void); bool flashWaitForReady(timeMs_t timeoutMillis); void flashEraseSector(uint32_t address); void flashEraseCompletely(void); -#if 0 -void flashPageProgramBegin(uint32_t address); -void flashPageProgramContinue(const uint8_t *data, int length); -void flashPageProgramFinish(void); -#endif uint32_t flashPageProgram(uint32_t address, const uint8_t *data, int length); int flashReadBytes(uint32_t address, uint8_t *buffer, int length); void flashFlush(void); diff --git a/src/main/drivers/flash_m25p16.c b/src/main/drivers/flash_m25p16.c index ea22ffddde..7896abd44b 100644 --- a/src/main/drivers/flash_m25p16.c +++ b/src/main/drivers/flash_m25p16.c @@ -56,6 +56,8 @@ #define JEDEC_ID_SPANSION_S25FL116 0x014015 #define JEDEC_ID_EON_W25Q64 0x1C3017 #define JEDEC_ID_CYPRESS_S25FL128L 0x016018 +#define JEDEC_ID_WINBOND_W25Q128_2 0xEF7018 + // The timeout we expect between being able to issue page program instructions #define DEFAULT_TIMEOUT_MILLIS 6 @@ -190,7 +192,8 @@ static bool m25p16_readIdentification(void) case JEDEC_ID_MICRON_N25Q128: case JEDEC_ID_WINBOND_W25Q128: - case JEDEC_ID_CYPRESS_S25FL128L: + case JEDEC_ID_CYPRESS_S25FL128L: + case JEDEC_ID_WINBOND_W25Q128_2: geometry.sectors = 256; geometry.pagesPerSector = 256; break; diff --git a/src/main/drivers/io_pca9685.c b/src/main/drivers/io_pca9685.c deleted file mode 100644 index 1038496228..0000000000 --- a/src/main/drivers/io_pca9685.c +++ /dev/null @@ -1,147 +0,0 @@ -#include -#include - -#include "platform.h" - -#include "build/build_config.h" - -#ifdef USE_PWM_DRIVER_PCA9685 - -#include "common/time.h" -#include "common/maths.h" - -#include "drivers/io.h" -#include "drivers/time.h" -#include "drivers/bus.h" -#include "drivers/bus_i2c.h" - -#define PCA9685_MODE1 0x00 -#define PCA9685_PRESCALE 0xFE - -#define LED0_ON_L 0x06 -#define LED0_ON_H 0x07 -#define LED0_OFF_L 0x08 -#define LED0_OFF_H 0x09 - -#define PCA9685_SERVO_FREQUENCY 60 -#define PCA9685_SERVO_COUNT 16 -#define PCA9685_SYNC_THRESHOLD 5 - -static busDevice_t * busDev; -static uint16_t currentOutputState[PCA9685_SERVO_COUNT] = {0}; -static uint16_t temporaryOutputState[PCA9685_SERVO_COUNT] = {0}; - -void pca9685setPWMOn(uint8_t servoIndex, uint16_t on) { - if (servoIndex < PCA9685_SERVO_COUNT) { - busWrite(busDev, LED0_ON_L + (servoIndex * 4), on); - busWrite(busDev, LED0_ON_H + (servoIndex * 4), on>>8); - } -} - -void pca9685setPWMOff(uint8_t servoIndex, uint16_t off) { - if (servoIndex < PCA9685_SERVO_COUNT) { - busWrite(busDev, LED0_OFF_L + (servoIndex * 4), off); - busWrite(busDev, LED0_OFF_H + (servoIndex * 4), off>>8); - } -} - -/* -Writing new state every cycle for each servo is extremely time consuming -and does not makes sense. -Trying to sync 5 servos every 2000us extends looptime -to 3500us. Very, very bad... -Instead of that, write desired values to temporary -table and write it to PCA9685 only when there a need. -Also, because of resultion of PCA9685 internal counter of about 5us, do not write -small changes, since thwy will only clog the bandwidch and will not -be represented on output -PWM Driver runs at 200Hz, every cycle every 4th servo output is updated: -cycle 0: servo 0, 4, 8, 12 -cycle 1: servo 1, 5, 9, 13 -cycle 2: servo 2, 6, 10, 14 -cycle 3: servo3, 7, 11, 15 -*/ -void pca9685sync(uint8_t cycleIndex) { - uint8_t i; - - for (i = 0; i < PCA9685_SERVO_COUNT; i++) { - if (cycleIndex == i % 4 && ABS(temporaryOutputState[i] - currentOutputState[i]) > PCA9685_SYNC_THRESHOLD) { - pca9685setPWMOff(i, temporaryOutputState[i]); - currentOutputState[i] = temporaryOutputState[i]; - } - } -} - -void pca9685setServoPulse(uint8_t servoIndex, uint16_t pulse) { - - static double pulselength = 0; - - if (pulselength == 0) { - pulselength = 1000000; // 1,000,000 us per second - pulselength /= PCA9685_SERVO_FREQUENCY; - pulselength /= 4096; // 12 bits of resolution - } - pulse /= pulselength; - - temporaryOutputState[servoIndex] = pulse; -} - -void pca9685setPWMFreq(uint16_t freq) { - - uint32_t prescaleval = 25000000; - prescaleval /= 4096; - prescaleval /= freq; - prescaleval -= 1; - - busWrite(busDev, PCA9685_MODE1, 16); - delay(1); - busWrite(busDev, PCA9685_PRESCALE, (uint8_t) prescaleval); - delay(1); - busWrite(busDev, PCA9685_MODE1, 128); -} - -static bool deviceDetect(busDevice_t * busDev) -{ - for (int retry = 0; retry < 5; retry++) { - uint8_t sig; - - delay(150); - - bool ack = busRead(busDev, PCA9685_MODE1, &sig); - if (ack) { - return true; - } - }; - - return false; -} - -bool pca9685Initialize(void) -{ - busDev = busDeviceInit(BUSTYPE_I2C, DEVHW_PCA9685, 0, 0); - if (busDev == NULL) { - return false; - } - - if (!deviceDetect(busDev)) { - busDeviceDeInit(busDev); - return false; - } - - /* Reset device */ - busWrite(busDev, PCA9685_MODE1, 0x00); - - /* Set refresh rate */ - pca9685setPWMFreq(PCA9685_SERVO_FREQUENCY); - - delay(1); - - for (uint8_t i = 0; i < PCA9685_SERVO_COUNT; i++) { - pca9685setPWMOn(i, 0); - pca9685setPWMOff(i, 1500); - } - - return true; -} - -#endif diff --git a/src/main/drivers/io_pca9685.h b/src/main/drivers/io_pca9685.h deleted file mode 100644 index 2ab7b0477e..0000000000 --- a/src/main/drivers/io_pca9685.h +++ /dev/null @@ -1,6 +0,0 @@ -bool pca9685Initialize(void); -void pca9685setPWMOn(uint8_t servoIndex, uint16_t on); -void pca9685setPWMOff(uint8_t servoIndex, uint16_t off); -void pca9685setPWMFreq(uint16_t freq); -void pca9685setServoPulse(uint8_t servoIndex, uint16_t pulse); -void pca9685sync(uint8_t cycleIndex); diff --git a/src/main/drivers/lights_io.c b/src/main/drivers/lights_io.c index b6f453686e..a495a32493 100644 --- a/src/main/drivers/lights_io.c +++ b/src/main/drivers/lights_io.c @@ -3,11 +3,10 @@ #ifdef USE_LIGHTS -#if (!defined(LIGHTS_USE_PCA9685_OUTPUT)) && (!defined(LIGHTS_OUTPUT_MODE)) +#ifndef LIGHTS_OUTPUT_MODE #define LIGHTS_OUTPUT_MODE IOCFG_OUT_PP #endif - static IO_t lightsIO = DEFIO_IO(NONE); bool lightsHardwareInit(void) diff --git a/src/main/drivers/osd_symbols.h b/src/main/drivers/osd_symbols.h index 229ba680a2..841404dbce 100644 --- a/src/main/drivers/osd_symbols.h +++ b/src/main/drivers/osd_symbols.h @@ -161,7 +161,10 @@ #define SYM_HEADING_LINE 0xCD // 205 Heading Graphic #define SYM_MAX 0xCE // 206 MAX symbol #define SYM_PROFILE 0xCF // 207 Profile symbol - +#define SYM_SWITCH_INDICATOR_LOW 0xD0 // 208 Switch High +#define SYM_SWITCH_INDICATOR_MID 0xD1 // 209 Switch Mid +#define SYM_SWITCH_INDICATOR_HIGH 0xD2 // 210 Switch Low +#define SYM_AH 0xD3 // 211 Amphours symbol #define SYM_LOGO_START 0x101 // 257 to 280, INAV logo #define SYM_LOGO_WIDTH 6 diff --git a/src/main/drivers/pitotmeter/pitotmeter_adc.c b/src/main/drivers/pitotmeter/pitotmeter_adc.c index da68570ca9..4d0237b3a9 100644 --- a/src/main/drivers/pitotmeter/pitotmeter_adc.c +++ b/src/main/drivers/pitotmeter/pitotmeter_adc.c @@ -22,6 +22,7 @@ #include "build/build_config.h" +#include "common/maths.h" #include "common/utils.h" #include "drivers/pitotmeter/pitotmeter.h" @@ -34,6 +35,7 @@ * NXP MPXV7002DP differential pressure sensor * */ + #define PITOT_ADC_VOLTAGE_SCALER (2.0f / 1.0f) // MPXV7002DP is 5V device, assumed resistive divider 1K:1K #define PITOT_ADC_VOLTAGE_ZERO (2.5f) // Pressure offset is 2.5V #define PITOT_ADC_VOLTAGE_TO_PRESSURE (1000.0f) // 1V/kPa = 1000 Pa/V @@ -59,7 +61,7 @@ static void adcPitotCalculate(pitotDev_t *pitot, float *pressure, float *tempera if (pressure) *pressure = (voltage * PITOT_ADC_VOLTAGE_SCALER - PITOT_ADC_VOLTAGE_ZERO) * PITOT_ADC_VOLTAGE_TO_PRESSURE; if (temperature) - *temperature = 288.15f; // Temperature at standard sea level (288.15 K) + *temperature = SSL_AIR_TEMPERATURE; // Temperature at standard sea level } bool adcPitotDetect(pitotDev_t *pitot) diff --git a/src/main/drivers/pitotmeter/pitotmeter_virtual.c b/src/main/drivers/pitotmeter/pitotmeter_virtual.c index 3f72828517..b8af2df33a 100644 --- a/src/main/drivers/pitotmeter/pitotmeter_virtual.c +++ b/src/main/drivers/pitotmeter/pitotmeter_virtual.c @@ -74,10 +74,9 @@ static void virtualPitotCalculate(pitotDev_t *pitot, float *pressure, float *tem } } if (pressure) - //*pressure = sq(airSpeed / 100) * AIR_DENSITY_SEA_LEVEL_15C / 2 + P0; - *pressure = sq(airSpeed) * AIR_DENSITY_SEA_LEVEL_15C / 20000.0f + P0; + *pressure = sq(airSpeed) * SSL_AIR_DENSITY / 20000.0f + SSL_AIR_PRESSURE; if (temperature) - *temperature = 288.15f; // Temperature at standard sea level (288.15 K) + *temperature = SSL_AIR_TEMPERATURE; // Temperature at standard sea level } bool virtualPitotDetect(pitotDev_t *pitot) diff --git a/src/main/drivers/pwm_mapping.h b/src/main/drivers/pwm_mapping.h index 51ec456a77..14c3f9293b 100644 --- a/src/main/drivers/pwm_mapping.h +++ b/src/main/drivers/pwm_mapping.h @@ -48,7 +48,6 @@ typedef enum { typedef enum { SERVO_TYPE_PWM = 0, - SERVO_TYPE_SERVO_DRIVER, SERVO_TYPE_SBUS, SERVO_TYPE_SBUS_PWM } servoProtocolType_e; diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index f084c19a21..e04f90c232 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -34,9 +34,6 @@ FILE_COMPILE_FOR_SPEED #include "drivers/timer.h" #include "drivers/pwm_mapping.h" #include "drivers/pwm_output.h" -#include "drivers/io_pca9685.h" - -#include "io/pwmdriver_i2c.h" #include "io/servo_sbus.h" #include "sensors/esc_sensor.h" @@ -60,7 +57,7 @@ FILE_COMPILE_FOR_SPEED #define DSHOT_DMA_BUFFER_SIZE 18 /* resolution + frame reset (2us) */ -#define DSHOT_COMMAND_INTERVAL_US 1000 +#define DSHOT_COMMAND_INTERVAL_US 10000 #define DSHOT_COMMAND_QUEUE_LENGTH 8 #define DHSOT_COMMAND_QUEUE_SIZE DSHOT_COMMAND_QUEUE_LENGTH * sizeof(dshotCommands_e) #endif @@ -98,11 +95,6 @@ static pwmWriteFuncPtr motorWritePtr = NULL; // Function to write val static pwmOutputPort_t * servos[MAX_SERVOS]; static pwmWriteFuncPtr servoWritePtr = NULL; // Function to write value to motors -#if defined(USE_DSHOT) -static timeUs_t digitalMotorUpdateIntervalUs = 0; -static timeUs_t digitalMotorLastUpdateUs; -#endif - static pwmOutputPort_t beeperPwmPort; static pwmOutputPort_t *beeperPwm; static uint16_t beeperFrequency = 0; @@ -112,6 +104,10 @@ static uint8_t allocatedOutputPortCount = 0; static bool pwmMotorsEnabled = true; #ifdef USE_DSHOT +static timeUs_t digitalMotorUpdateIntervalUs = 0; +static timeUs_t digitalMotorLastUpdateUs; +static timeUs_t lastCommandSent = 0; + static circularBuffer_t commandsCircularBuffer; static uint8_t commandsBuff[DHSOT_COMMAND_QUEUE_SIZE]; static currentExecutingCommand_t currentExecutingCommand; @@ -367,29 +363,27 @@ static int getDShotCommandRepeats(dshotCommands_e cmd) { } static void executeDShotCommands(void){ + + timeUs_t tNow = micros(); if(currentExecutingCommand.remainingRepeats == 0) { - - const int isTherePendingCommands = !circularBufferIsEmpty(&commandsCircularBuffer); - - if (isTherePendingCommands) { + const int isTherePendingCommands = !circularBufferIsEmpty(&commandsCircularBuffer); + if (isTherePendingCommands && (tNow - lastCommandSent > DSHOT_COMMAND_INTERVAL_US)){ //Load the command dshotCommands_e cmd; circularBufferPopHead(&commandsCircularBuffer, (uint8_t *) &cmd); - currentExecutingCommand.cmd = cmd; - currentExecutingCommand.remainingRepeats = getDShotCommandRepeats(cmd); - } - else { - return; - } + currentExecutingCommand.remainingRepeats = getDShotCommandRepeats(cmd); + } else { + return; + } } - for (uint8_t i = 0; i < getMotorCount(); i++) { - motors[i].requestTelemetry = true; - motors[i].value = currentExecutingCommand.cmd; + motors[i].requestTelemetry = true; + motors[i].value = currentExecutingCommand.cmd; } - currentExecutingCommand.remainingRepeats--; + currentExecutingCommand.remainingRepeats--; + lastCommandSent = tNow; } #endif @@ -538,16 +532,6 @@ static void sbusPwmWriteStandard(uint8_t index, uint16_t value) } #endif -#ifdef USE_PWM_SERVO_DRIVER -static void pwmServoWriteExternalDriver(uint8_t index, uint16_t value) -{ - // If PCA9685 is not detected, we do not want to write servo output anywhere - if (STATE(PWM_DRIVER_AVAILABLE)) { - pwmDriverSetPulse(index, value); - } -} -#endif - void pwmServoPreconfigure(void) { // Protocol-specific configuration @@ -557,13 +541,6 @@ void pwmServoPreconfigure(void) servoWritePtr = pwmServoWriteStandard; break; -#ifdef USE_PWM_SERVO_DRIVER - case SERVO_TYPE_SERVO_DRIVER: - pwmDriverInitialize(); - servoWritePtr = pwmServoWriteExternalDriver; - break; -#endif - #ifdef USE_SERVO_SBUS case SERVO_TYPE_SBUS: sbusServoInitialize(); diff --git a/src/main/drivers/serial.h b/src/main/drivers/serial.h index f9a5b263fa..05d31fb024 100644 --- a/src/main/drivers/serial.h +++ b/src/main/drivers/serial.h @@ -38,6 +38,8 @@ typedef enum portOptions_t { SERIAL_PARITY_EVEN = 1 << 2, SERIAL_UNIDIR = 0 << 3, SERIAL_BIDIR = 1 << 3, + SERIAL_LONGSTOP = 0 << 4, + SERIAL_SHORTSTOP = 1 << 4, /* * Note on SERIAL_BIDIR_PP diff --git a/src/main/drivers/serial_softserial.c b/src/main/drivers/serial_softserial.c index 255087f004..92d89c4c12 100644 --- a/src/main/drivers/serial_softserial.c +++ b/src/main/drivers/serial_softserial.c @@ -381,7 +381,9 @@ void processTxState(softSerial_t *softSerial) serialOutputPortActivate(softSerial); } - return; + if ((softSerial->port.options & SERIAL_SHORTSTOP) == 0) { + return; + } } if (softSerial->bitsLeftToTransmit) { @@ -390,7 +392,10 @@ void processTxState(softSerial_t *softSerial) setTxSignal(softSerial, mask); softSerial->bitsLeftToTransmit--; - return; + + if (((softSerial->port.options & SERIAL_SHORTSTOP) == 0) || softSerial->bitsLeftToTransmit) { + return; + } } softSerial->isTransmittingData = false; diff --git a/src/main/drivers/vtx_common.h b/src/main/drivers/vtx_common.h index c83b0aeab5..1910075a83 100644 --- a/src/main/drivers/vtx_common.h +++ b/src/main/drivers/vtx_common.h @@ -66,6 +66,12 @@ typedef enum { VTXDEV_UNKNOWN = 0xFF, } vtxDevType_e; +typedef enum { + FREQUENCYGROUP_5G8 = 0, + FREQUENCYGROUP_2G4 = 1, + FREQUENCYGROUP_1G3 = 2, +} vtxFrequencyGroups_e; + struct vtxVTable_s; typedef struct vtxDeviceCapability_s { diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 2dc35e5272..3c355f7814 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -140,6 +140,7 @@ static uint8_t cliWriteBuffer[sizeof(*cliWriter) + 128]; static char cliBuffer[64]; static uint32_t bufferIndex = 0; +static uint16_t cliDelayMs = 0; #if defined(USE_ASSERT) static void cliAssert(char *cmdline); @@ -222,6 +223,9 @@ static void cliPrint(const char *str) static void cliPrintLinefeed(void) { cliPrint("\r\n"); + if (cliDelayMs) { + delay(cliDelayMs); + } } static void cliPrintLine(const char *str) @@ -1672,6 +1676,25 @@ static void cliModeColor(char *cmdline) } #endif +static void cliDelay(char* cmdLine) { + int ms = 0; + if (isEmpty(cmdLine)) { + cliDelayMs = 0; + cliPrintLine("CLI delay deactivated"); + return; + } + + ms = fastA2I(cmdLine); + if (ms) { + cliDelayMs = ms; + cliPrintLinef("CLI delay set to %d ms", ms); + + } else { + cliShowParseError(); + } + +} + static void printServo(uint8_t dumpMask, const servoParam_t *servoParam, const servoParam_t *defaultServoParam) { // print out servo settings @@ -3869,6 +3892,7 @@ const clicmd_t cmdTable[] = { CLI_COMMAND_DEF("color", "configure colors", NULL, cliColor), CLI_COMMAND_DEF("mode_color", "configure mode and special colors", NULL, cliModeColor), #endif + CLI_COMMAND_DEF("cli_delay", "CLI Delay", "Delay in ms", cliDelay), CLI_COMMAND_DEF("defaults", "reset to defaults and reboot", NULL, cliDefaults), CLI_COMMAND_DEF("dfu", "DFU mode on reboot", NULL, cliDfu), CLI_COMMAND_DEF("diff", "list configuration changes from default", diff --git a/src/main/fc/config.c b/src/main/fc/config.c index efd1a3129d..205289a3ca 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -217,12 +217,6 @@ void validateAndFixConfig(void) } #endif -#ifndef USE_PWM_SERVO_DRIVER - if (servoConfig()->servo_protocol == SERVO_TYPE_SERVO_DRIVER) { - servoConfigMutable()->servo_protocol = SERVO_TYPE_PWM; - } -#endif - #ifndef USE_SERVO_SBUS if (servoConfig()->servo_protocol == SERVO_TYPE_SBUS || servoConfig()->servo_protocol == SERVO_TYPE_SBUS_PWM) { servoConfigMutable()->servo_protocol = SERVO_TYPE_PWM; @@ -239,7 +233,7 @@ void validateAndFixConfig(void) // Limitations of different protocols #if !defined(USE_DSHOT) if (motorConfig()->motorPwmProtocol > PWM_TYPE_BRUSHED) { - motorConfigMutable()->motorPwmProtocol = PWM_TYPE_STANDARD; + motorConfigMutable()->motorPwmProtocol = PWM_TYPE_MULTISHOT; } #endif diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index 4bf83d3d05..df55b33d62 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -53,7 +53,6 @@ #include "drivers/exti.h" #include "drivers/flash_m25p16.h" #include "drivers/io.h" -#include "drivers/io_pca9685.h" #include "drivers/flash.h" #include "drivers/light_led.h" #include "drivers/nvic.h" @@ -75,7 +74,6 @@ #include "drivers/uart_inverter.h" #include "drivers/io.h" #include "drivers/exti.h" -#include "drivers/io_pca9685.h" #include "drivers/vtx_common.h" #ifdef USE_USB_MSC #include "drivers/usb_msc.h" @@ -114,7 +112,6 @@ #include "io/flashfs.h" #include "io/gps.h" #include "io/ledstrip.h" -#include "io/pwmdriver_i2c.h" #include "io/osd.h" #include "io/osd_dji_hd.h" #include "io/rcdevice_cam.h" diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 0a15a1b620..9b8bcc2360 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -1159,8 +1159,8 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF break; case MSP_SENSOR_ALIGNMENT: - sbufWriteU8(dst, gyroConfig()->gyro_align); - sbufWriteU8(dst, accelerometerConfig()->acc_align); + sbufWriteU8(dst, 0); // was gyroConfig()->gyro_align + sbufWriteU8(dst, 0); // was accelerometerConfig()->acc_align #ifdef USE_MAG sbufWriteU8(dst, compassConfig()->mag_align); #else @@ -1553,6 +1553,23 @@ static mspResult_e mspFcSafeHomeOutCommand(sbuf_t *dst, sbuf_t *src) } } +static mspResult_e mspFcLogicConditionCommand(sbuf_t *dst, sbuf_t *src) { + const uint8_t idx = sbufReadU8(src); + if (idx < MAX_LOGIC_CONDITIONS) { + sbufWriteU8(dst, logicConditions(idx)->enabled); + sbufWriteU8(dst, logicConditions(idx)->activatorId); + sbufWriteU8(dst, logicConditions(idx)->operation); + sbufWriteU8(dst, logicConditions(idx)->operandA.type); + sbufWriteU32(dst, logicConditions(idx)->operandA.value); + sbufWriteU8(dst, logicConditions(idx)->operandB.type); + sbufWriteU32(dst, logicConditions(idx)->operandB.value); + sbufWriteU8(dst, logicConditions(idx)->flags); + return MSP_RESULT_ACK; + } else { + return MSP_RESULT_ERROR; + } +} + static void mspFcWaypointOutCommand(sbuf_t *dst, sbuf_t *src) { const uint8_t msp_wp_no = sbufReadU8(src); // get the wp number @@ -2049,8 +2066,8 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) case MSP_SET_SENSOR_ALIGNMENT: if (dataSize == 4) { - gyroConfigMutable()->gyro_align = sbufReadU8(src); - accelerometerConfigMutable()->acc_align = sbufReadU8(src); + sbufReadU8(src); // was gyroConfigMutable()->gyro_align + sbufReadU8(src); // was accelerometerConfigMutable()->acc_align #ifdef USE_MAG compassConfigMutable()->mag_align = sbufReadU8(src); #else @@ -3237,6 +3254,11 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu break; #endif +#ifdef USE_PROGRAMMING_FRAMEWORK + case MSP2_INAV_LOGIC_CONDITIONS_SINGLE: + *ret = mspFcLogicConditionCommand(dst, src); + break; +#endif case MSP2_INAV_SAFEHOME: *ret = mspFcSafeHomeOutCommand(dst, src); break; diff --git a/src/main/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c index 1b1ac0793e..8da37bebf3 100644 --- a/src/main/fc/fc_msp_box.c +++ b/src/main/fc/fc_msp_box.c @@ -49,13 +49,13 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = { { BOXARM, "ARM", 0 }, { BOXANGLE, "ANGLE", 1 }, { BOXHORIZON, "HORIZON", 2 }, - { BOXNAVALTHOLD, "NAV ALTHOLD", 3 }, // old BARO + { BOXNAVALTHOLD, "NAV ALTHOLD", 3 }, { BOXHEADINGHOLD, "HEADING HOLD", 5 }, { BOXHEADFREE, "HEADFREE", 6 }, { BOXHEADADJ, "HEADADJ", 7 }, { BOXCAMSTAB, "CAMSTAB", 8 }, - { BOXNAVRTH, "NAV RTH", 10 }, // old GPS HOME - { BOXNAVPOSHOLD, "NAV POSHOLD", 11 }, // old GPS HOLD + { BOXNAVRTH, "NAV RTH", 10 }, + { BOXNAVPOSHOLD, "NAV POSHOLD", 11 }, { BOXMANUAL, "MANUAL", 12 }, { BOXBEEPERON, "BEEPER", 13 }, { BOXLEDLOW, "LEDS OFF", 15 }, @@ -368,14 +368,12 @@ void packBoxModeFlags(boxBitmask_t * mspBoxModeFlags) CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(NAV_ALTHOLD_MODE)), BOXNAVALTHOLD); CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(NAV_POSHOLD_MODE)), BOXNAVPOSHOLD); CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(NAV_COURSE_HOLD_MODE)), BOXNAVCOURSEHOLD); - CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) && IS_ENABLED(FLIGHT_MODE(NAV_ALTHOLD_MODE)), BOXNAVCRUISE); + CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) && IS_ENABLED(FLIGHT_MODE(NAV_ALTHOLD_MODE)), BOXNAVCRUISE); CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(NAV_RTH_MODE)), BOXNAVRTH); CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(NAV_WP_MODE)), BOXNAVWP); CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXAIRMODE)), BOXAIRMODE); CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGCSNAV)), BOXGCSNAV); -#ifdef USE_FLM_FLAPERON CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(FLAPERON)), BOXFLAPERON); -#endif CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(TURN_ASSISTANT)), BOXTURNASSIST); CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(NAV_LAUNCH_MODE)), BOXNAVLAUNCH); CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(AUTO_TUNE)), BOXAUTOTUNE); diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index 40c8f3fbac..e255813140 100755 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -61,7 +61,6 @@ #include "io/gps.h" #include "io/ledstrip.h" #include "io/osd.h" -#include "io/pwmdriver_i2c.h" #include "io/serial.h" #include "io/rcdevice_cam.h" #include "io/smartport_master.h" @@ -294,9 +293,6 @@ void taskSyncServoDriver(timeUs_t currentTimeUs) sbusServoSendUpdate(); #endif -#ifdef USE_PWM_SERVO_DRIVER - pwmDriverSync(); -#endif } #ifdef USE_OSD @@ -370,8 +366,8 @@ void fcTasksInit(void) #ifdef STACK_CHECK setTaskEnabled(TASK_STACK_CHECK, true); #endif -#if defined(USE_PWM_SERVO_DRIVER) || defined(USE_SERVO_SBUS) - setTaskEnabled(TASK_PWMDRIVER, (servoConfig()->servo_protocol == SERVO_TYPE_SERVO_DRIVER) || (servoConfig()->servo_protocol == SERVO_TYPE_SBUS) || (servoConfig()->servo_protocol == SERVO_TYPE_SBUS_PWM)); +#if defined(USE_SERVO_SBUS) + setTaskEnabled(TASK_PWMDRIVER, (servoConfig()->servo_protocol == SERVO_TYPE_SBUS) || (servoConfig()->servo_protocol == SERVO_TYPE_SBUS_PWM)); #endif #ifdef USE_CMS #ifdef USE_MSP_DISPLAYPORT @@ -561,7 +557,7 @@ cfTask_t cfTasks[TASK_COUNT] = { }, #endif -#if defined(USE_PWM_SERVO_DRIVER) || defined(USE_SERVO_SBUS) +#if defined(USE_SERVO_SBUS) [TASK_PWMDRIVER] = { .taskName = "SERVOS", .taskFunc = taskSyncServoDriver, diff --git a/src/main/fc/rc_adjustments.c b/src/main/fc/rc_adjustments.c index 610e62f16e..d1edcd20b5 100644 --- a/src/main/fc/rc_adjustments.c +++ b/src/main/fc/rc_adjustments.c @@ -289,6 +289,14 @@ static const adjustmentConfig_t defaultAdjustmentConfigs[ADJUSTMENT_FUNCTION_COU .adjustmentFunction = ADJUSTMENT_NAV_FW_CONTROL_SMOOTHNESS, .mode = ADJUSTMENT_MODE_STEP, .data = { .stepConfig = { .step = 1 }} + }, { + .adjustmentFunction = ADJUSTMENT_FW_TPA_TIME_CONSTANT, + .mode = ADJUSTMENT_MODE_STEP, + .data = { .stepConfig = { .step = 5 }} + }, { + .adjustmentFunction = ADJUSTMENT_FW_LEVEL_TRIM, + .mode = ADJUSTMENT_MODE_STEP, + .data = { .stepConfig = { .step = 1 }} } }; @@ -327,24 +335,6 @@ static void blackboxLogInflightAdjustmentEvent(adjustmentFunction_e adjustmentFu #endif } -#if 0 -static void blackboxLogInflightAdjustmentEventFloat(adjustmentFunction_e adjustmentFunction, float newFloatValue) -{ -#ifndef USE_BLACKBOX - UNUSED(adjustmentFunction); - UNUSED(newFloatValue); -#else - if (feature(FEATURE_BLACKBOX)) { - flightLogEvent_inflightAdjustment_t eventData; - eventData.adjustmentFunction = adjustmentFunction; - eventData.newFloatValue = newFloatValue; - eventData.floatFlag = true; - blackboxLogEvent(FLIGHT_LOG_EVENT_INFLIGHT_ADJUSTMENT, (flightLogEventData_t*)&eventData); - } -#endif -} -#endif - static void applyAdjustmentU8(adjustmentFunction_e adjustmentFunction, uint8_t *val, int delta, int low, int high) { int newValue = constrain((int)(*val) + delta, low, high); @@ -597,9 +587,21 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t case ADJUSTMENT_TPA_BREAKPOINT: applyAdjustmentU16(ADJUSTMENT_TPA_BREAKPOINT, &controlRateConfig->throttle.pa_breakpoint, delta, PWM_RANGE_MIN, PWM_RANGE_MAX); break; + case ADJUSTMENT_FW_TPA_TIME_CONSTANT: + applyAdjustmentU16(ADJUSTMENT_FW_TPA_TIME_CONSTANT, &controlRateConfig->throttle.fixedWingTauMs, delta, SETTING_FW_TPA_TIME_CONSTANT_MIN, SETTING_FW_TPA_TIME_CONSTANT_MAX); + break; case ADJUSTMENT_NAV_FW_CONTROL_SMOOTHNESS: applyAdjustmentU8(ADJUSTMENT_NAV_FW_CONTROL_SMOOTHNESS, &navConfigMutable()->fw.control_smoothness, delta, SETTING_NAV_FW_CONTROL_SMOOTHNESS_MIN, SETTING_NAV_FW_CONTROL_SMOOTHNESS_MAX); break; + case ADJUSTMENT_FW_LEVEL_TRIM: + { + float newValue = pidProfileMutable()->fixedWingLevelTrim + (delta / 10.0f); + if (newValue > SETTING_FW_LEVEL_PITCH_TRIM_MAX) {newValue = (float)SETTING_FW_LEVEL_PITCH_TRIM_MAX;} + else if (newValue < SETTING_FW_LEVEL_PITCH_TRIM_MIN) {newValue = (float)SETTING_FW_LEVEL_PITCH_TRIM_MIN;} + pidProfileMutable()->fixedWingLevelTrim = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_FW_LEVEL_TRIM, (int)(newValue * 10.0f)); + break; + } default: break; }; diff --git a/src/main/fc/rc_adjustments.h b/src/main/fc/rc_adjustments.h index 93dd3d7e61..30f25340bd 100644 --- a/src/main/fc/rc_adjustments.h +++ b/src/main/fc/rc_adjustments.h @@ -25,63 +25,65 @@ //#define USE_INFLIGHT_PROFILE_ADJUSTMENT - not currently enabled typedef enum { - ADJUSTMENT_NONE = 0, - ADJUSTMENT_RC_RATE = 1, - ADJUSTMENT_RC_EXPO = 2, - ADJUSTMENT_THROTTLE_EXPO = 3, - ADJUSTMENT_PITCH_ROLL_RATE = 4, - ADJUSTMENT_YAW_RATE = 5, - ADJUSTMENT_PITCH_ROLL_P = 6, - ADJUSTMENT_PITCH_ROLL_I = 7, - ADJUSTMENT_PITCH_ROLL_D = 8, - ADJUSTMENT_PITCH_ROLL_FF = 9, - ADJUSTMENT_PITCH_P = 10, - ADJUSTMENT_PITCH_I = 11, - ADJUSTMENT_PITCH_D = 12, - ADJUSTMENT_PITCH_FF = 13, - ADJUSTMENT_ROLL_P = 14, - ADJUSTMENT_ROLL_I = 15, - ADJUSTMENT_ROLL_D = 16, - ADJUSTMENT_ROLL_FF = 17, - ADJUSTMENT_YAW_P = 18, - ADJUSTMENT_YAW_I = 19, - ADJUSTMENT_YAW_D = 20, - ADJUSTMENT_YAW_FF = 21, - ADJUSTMENT_RATE_PROFILE = 22, // Unused, placeholder for compatibility - ADJUSTMENT_PITCH_RATE = 23, - ADJUSTMENT_ROLL_RATE = 24, - ADJUSTMENT_RC_YAW_EXPO = 25, - ADJUSTMENT_MANUAL_RC_EXPO = 26, - ADJUSTMENT_MANUAL_RC_YAW_EXPO = 27, - ADJUSTMENT_MANUAL_PITCH_ROLL_RATE = 28, - ADJUSTMENT_MANUAL_ROLL_RATE = 29, - ADJUSTMENT_MANUAL_PITCH_RATE = 30, - ADJUSTMENT_MANUAL_YAW_RATE = 31, - ADJUSTMENT_NAV_FW_CRUISE_THR = 32, - ADJUSTMENT_NAV_FW_PITCH2THR = 33, - ADJUSTMENT_ROLL_BOARD_ALIGNMENT = 34, - ADJUSTMENT_PITCH_BOARD_ALIGNMENT = 35, - ADJUSTMENT_LEVEL_P = 36, - ADJUSTMENT_LEVEL_I = 37, - ADJUSTMENT_LEVEL_D = 38, - ADJUSTMENT_POS_XY_P = 39, - ADJUSTMENT_POS_XY_I = 40, - ADJUSTMENT_POS_XY_D = 41, - ADJUSTMENT_POS_Z_P = 42, - ADJUSTMENT_POS_Z_I = 43, - ADJUSTMENT_POS_Z_D = 44, - ADJUSTMENT_HEADING_P = 45, - ADJUSTMENT_VEL_XY_P = 46, - ADJUSTMENT_VEL_XY_I = 47, - ADJUSTMENT_VEL_XY_D = 48, - ADJUSTMENT_VEL_Z_P = 49, - ADJUSTMENT_VEL_Z_I = 50, - ADJUSTMENT_VEL_Z_D = 51, + ADJUSTMENT_NONE = 0, + ADJUSTMENT_RC_RATE = 1, + ADJUSTMENT_RC_EXPO = 2, + ADJUSTMENT_THROTTLE_EXPO = 3, + ADJUSTMENT_PITCH_ROLL_RATE = 4, + ADJUSTMENT_YAW_RATE = 5, + ADJUSTMENT_PITCH_ROLL_P = 6, + ADJUSTMENT_PITCH_ROLL_I = 7, + ADJUSTMENT_PITCH_ROLL_D = 8, + ADJUSTMENT_PITCH_ROLL_FF = 9, + ADJUSTMENT_PITCH_P = 10, + ADJUSTMENT_PITCH_I = 11, + ADJUSTMENT_PITCH_D = 12, + ADJUSTMENT_PITCH_FF = 13, + ADJUSTMENT_ROLL_P = 14, + ADJUSTMENT_ROLL_I = 15, + ADJUSTMENT_ROLL_D = 16, + ADJUSTMENT_ROLL_FF = 17, + ADJUSTMENT_YAW_P = 18, + ADJUSTMENT_YAW_I = 19, + ADJUSTMENT_YAW_D = 20, + ADJUSTMENT_YAW_FF = 21, + ADJUSTMENT_RATE_PROFILE = 22, // Unused, placeholder for compatibility + ADJUSTMENT_PITCH_RATE = 23, + ADJUSTMENT_ROLL_RATE = 24, + ADJUSTMENT_RC_YAW_EXPO = 25, + ADJUSTMENT_MANUAL_RC_EXPO = 26, + ADJUSTMENT_MANUAL_RC_YAW_EXPO = 27, + ADJUSTMENT_MANUAL_PITCH_ROLL_RATE = 28, + ADJUSTMENT_MANUAL_ROLL_RATE = 29, + ADJUSTMENT_MANUAL_PITCH_RATE = 30, + ADJUSTMENT_MANUAL_YAW_RATE = 31, + ADJUSTMENT_NAV_FW_CRUISE_THR = 32, + ADJUSTMENT_NAV_FW_PITCH2THR = 33, + ADJUSTMENT_ROLL_BOARD_ALIGNMENT = 34, + ADJUSTMENT_PITCH_BOARD_ALIGNMENT = 35, + ADJUSTMENT_LEVEL_P = 36, + ADJUSTMENT_LEVEL_I = 37, + ADJUSTMENT_LEVEL_D = 38, + ADJUSTMENT_POS_XY_P = 39, + ADJUSTMENT_POS_XY_I = 40, + ADJUSTMENT_POS_XY_D = 41, + ADJUSTMENT_POS_Z_P = 42, + ADJUSTMENT_POS_Z_I = 43, + ADJUSTMENT_POS_Z_D = 44, + ADJUSTMENT_HEADING_P = 45, + ADJUSTMENT_VEL_XY_P = 46, + ADJUSTMENT_VEL_XY_I = 47, + ADJUSTMENT_VEL_XY_D = 48, + ADJUSTMENT_VEL_Z_P = 49, + ADJUSTMENT_VEL_Z_I = 50, + ADJUSTMENT_VEL_Z_D = 51, ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE = 52, - ADJUSTMENT_VTX_POWER_LEVEL = 53, - ADJUSTMENT_TPA = 54, - ADJUSTMENT_TPA_BREAKPOINT = 55, - ADJUSTMENT_NAV_FW_CONTROL_SMOOTHNESS = 56, + ADJUSTMENT_VTX_POWER_LEVEL = 53, + ADJUSTMENT_TPA = 54, + ADJUSTMENT_TPA_BREAKPOINT = 55, + ADJUSTMENT_NAV_FW_CONTROL_SMOOTHNESS = 56, + ADJUSTMENT_FW_TPA_TIME_CONSTANT = 57, + ADJUSTMENT_FW_LEVEL_TRIM = 58, ADJUSTMENT_FUNCTION_COUNT // must be last } adjustmentFunction_e; diff --git a/src/main/fc/runtime_config.h b/src/main/fc/runtime_config.h index 79f49dacd9..a8bfe62980 100644 --- a/src/main/fc/runtime_config.h +++ b/src/main/fc/runtime_config.h @@ -89,14 +89,14 @@ typedef enum { ANGLE_MODE = (1 << 0), HORIZON_MODE = (1 << 1), HEADING_MODE = (1 << 2), - NAV_ALTHOLD_MODE = (1 << 3), // old BARO - NAV_RTH_MODE = (1 << 4), // old GPS_HOME - NAV_POSHOLD_MODE = (1 << 5), // old GPS_HOLD + NAV_ALTHOLD_MODE = (1 << 3), + NAV_RTH_MODE = (1 << 4), + NAV_POSHOLD_MODE = (1 << 5), HEADFREE_MODE = (1 << 6), NAV_LAUNCH_MODE = (1 << 7), MANUAL_MODE = (1 << 8), FAILSAFE_MODE = (1 << 9), - AUTO_TUNE = (1 << 10), // old G-Tune + AUTO_TUNE = (1 << 10), NAV_WP_MODE = (1 << 11), NAV_COURSE_HOLD_MODE = (1 << 12), FLAPERON = (1 << 13), @@ -122,7 +122,6 @@ typedef enum { NAV_MOTOR_STOP_OR_IDLE = (1 << 7), // navigation requests MOTOR_STOP or motor idle regardless of throttle stick, will only activate if MOTOR_STOP feature is available COMPASS_CALIBRATED = (1 << 8), ACCELEROMETER_CALIBRATED = (1 << 9), - PWM_DRIVER_AVAILABLE = (1 << 10), NAV_CRUISE_BRAKING = (1 << 11), NAV_CRUISE_BRAKING_BOOST = (1 << 12), NAV_CRUISE_BRAKING_LOCKED = (1 << 13), diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index fe30ea3f2a..3d8b3cb546 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -37,7 +37,7 @@ tables: - name: motor_pwm_protocol values: ["STANDARD", "ONESHOT125", "MULTISHOT", "BRUSHED", "DSHOT150", "DSHOT300", "DSHOT600"] - name: servo_protocol - values: ["PWM", "SERVO_DRIVER", "SBUS", "SBUS_PWM"] + values: ["PWM", "SBUS", "SBUS_PWM"] - name: failsafe_procedure values: ["LAND", "DROP", "RTH", "NONE"] - name: current_sensor @@ -47,7 +47,7 @@ tables: values: ["NONE", "ADC", "ESC"] enum: voltageSensor_e - name: gps_provider - values: ["NMEA", "UBLOX", "UNUSED", "NAZA", "UBLOX7", "MTK", "MSP"] + values: ["NMEA", "UBLOX", "NAZA", "UBLOX7", "MTK", "MSP"] enum: gpsProvider_e - name: gps_sbas_mode values: ["AUTO", "EGNOS", "WAAS", "MSAS", "GAGAN", "NONE"] @@ -131,6 +131,9 @@ tables: - name: vtx_low_power_disarm values: ["OFF", "ON", "UNTIL_FIRST_ARM"] enum: vtxLowerPowerDisarm_e + - name: vtx_frequency_groups + values: ["FREQUENCYGROUP_5G8", "FREQUENCYGROUP_2G4", "FREQUENCYGROUP_1G3"] + enum: vtxFrequencyGroups_e - name: filter_type values: ["PT1", "BIQUAD"] - name: filter_type_full @@ -213,12 +216,6 @@ groups: description: "This is the main loop time (in us). Changing this affects PID effect with some PID controllers (see PID section for details). A very conservative value of 3500us/285Hz should work for everyone. Setting it to zero does not limit loop time, so it will go as fast as possible." default_value: 1000 max: 9000 - - name: align_gyro - description: "When running on non-default hardware or adding support for new sensors/sensor boards, these values are used for sensor orientation. When carefully understood, these values can also be used to rotate (in 90deg steps) or flip the board. Possible values are: DEFAULT, CW0_DEG, CW90_DEG, CW180_DEG, CW270_DEG, CW0_DEG_FLIP, CW90_DEG_FLIP, CW180_DEG_FLIP, CW270_DEG_FLIP." - default_value: "DEFAULT" - field: gyro_align - type: uint8_t - table: alignment - name: gyro_hardware_lpf description: "Hardware lowpass filter for gyro. This value should never be changed without a very strong reason! If you have to set gyro lpf below 256HZ, it means the frame is vibrating too much, and that should be fixed first." default_value: "256HZ" @@ -292,7 +289,7 @@ groups: field: dynamicGyroNotchMinHz condition: USE_DYNAMIC_FILTERS min: 30 - max: 1000 + max: 250 - name: gyro_to_use condition: USE_DUAL_GYRO min: 0 @@ -310,7 +307,7 @@ groups: field: kalman_q condition: USE_GYRO_KALMAN min: 1 - max: 16000 + max: 1000 - name: init_gyro_cal description: "If defined to 'OFF', it will ignore the gyroscope calibration done at each startup. Instead, the gyroscope last calibration from when you calibrated will be used. It also means you don't have to keep the UAV stationary during a startup." default_value: ON @@ -383,12 +380,6 @@ groups: min: 1 max: 255 default_value: 1 - - name: align_acc - description: "When running on non-default hardware or adding support for new sensors/sensor boards, these values are used for sensor orientation. When carefully understood, these values can also be used to rotate (in 90deg steps) or flip the board. Possible values are: DEFAULT, CW0_DEG, CW90_DEG, CW180_DEG, CW270_DEG, CW0_DEG_FLIP, CW90_DEG_FLIP, CW180_DEG_FLIP, CW270_DEG_FLIP." - default_value: "DEFAULT" - field: acc_align - type: uint8_t - table: alignment - name: acc_hardware description: "Selection of acc hardware. See Wiki Sensor auto detect and hardware failure detection for more info" default_value: "AUTO" @@ -1297,7 +1288,7 @@ groups: headers: ["flight/servos.h"] members: - name: servo_protocol - description: "An option to chose the protocol/option that would be used to output servo data. Possible options `PWM` (FC servo outputs), `SERVO_DRIVER` (I2C PCA9685 peripheral), `SBUS` (S.Bus protocol output via a configured serial port)" + description: "An option to chose the protocol/option that would be used to output servo data. Possible options `PWM` (FC servo outputs), `SBUS` (S.Bus protocol output via a configured serial port)" default_value: "PWM" field: servo_protocol table: servo_protocol @@ -1364,8 +1355,8 @@ groups: min: PWM_RANGE_MIN max: PWM_RANGE_MAX - name: fw_tpa_time_constant - description: "TPA smoothing and delay time constant to reflect non-instant speed/throttle response of the plane. Planes with low thrust/weight ratio generally need higher time constant. Default is zero for compatibility with old setups" - default_value: 0 + description: "TPA smoothing and delay time constant to reflect non-instant speed/throttle response of the plane. See **PID Attenuation and scaling** Wiki for full details." + default_value: 1500 field: throttle.fixedWingTauMs min: 0 max: 5000 @@ -3370,6 +3361,18 @@ groups: field: hud_radar_range_max min: 100 max: 9990 + - name: osd_hud_radar_alt_difference_display_time + description: "Time in seconds to display the altitude difference in radar" + field: hud_radar_alt_difference_display_time + min: 0 + max: 10 + default_value: 3 + - name: osd_hud_radar_distance_display_time + description: "Time in seconds to display the distance in radar" + field: hud_radar_distance_display_time + min: 1 + max: 10 + default_value: 3 - name: osd_hud_wp_disp description: "How many navigation waypoints are displayed, set to 0 (zero) to disable. As sample, if set to 2, and you just passed the 3rd waypoint of the mission, you'll see markers for the 4th waypoint (marked 1) and the 5th waypoint (marked 2)" default_value: 0 @@ -3516,6 +3519,66 @@ groups: max: 6 default_value: 3 + - name: osd_mah_used_precision + description: Number of digits used to display mAh used. + field: mAh_used_precision + min: 4 + max: 6 + default_value: 4 + + - name: osd_switch_indicator_zero_name + description: "Character to use for OSD switch incicator 0." + field: osd_switch_indicator0_name + type: string + max: 5 + default_value: "FLAP" + - name: osd_switch_indicator_one_name + description: "Character to use for OSD switch incicator 1." + field: osd_switch_indicator1_name + type: string + max: 5 + default_value: "GEAR" + - name: osd_switch_indicator_two_name + description: "Character to use for OSD switch incicator 2." + field: osd_switch_indicator2_name + type: string + max: 5 + default_value: "CAM" + - name: osd_switch_indicator_three_name + description: "Character to use for OSD switch incicator 3." + field: osd_switch_indicator3_name + type: string + max: 5 + default_value: "LIGT" + - name: osd_switch_indicator_zero_channel + description: "RC Channel to use for OSD switch indicator 0." + field: osd_switch_indicator0_channel + min: 5 + max: MAX_SUPPORTED_RC_CHANNEL_COUNT + default_value: 5 + - name: osd_switch_indicator_one_channel + description: "RC Channel to use for OSD switch indicator 1." + field: osd_switch_indicator1_channel + min: 5 + max: MAX_SUPPORTED_RC_CHANNEL_COUNT + default_value: 5 + - name: osd_switch_indicator_two_channel + description: "RC Channel to use for OSD switch indicator 2." + field: osd_switch_indicator2_channel + min: 5 + max: MAX_SUPPORTED_RC_CHANNEL_COUNT + default_value: 5 + - name: osd_switch_indicator_three_channel + description: "RC Channel to use for OSD switch indicator 3." + field: osd_switch_indicator3_channel + min: 5 + max: MAX_SUPPORTED_RC_CHANNEL_COUNT + default_value: 5 + - name: osd_switch_indicators_align_left + description: "Align text to left of switch indicators" + field: osd_switch_indicators_align_left + type: bool + default_value: ON - name: osd_system_msg_display_time description: System message display cycle time for multiple messages (milliseconds). field: system_msg_display_time @@ -3651,6 +3714,17 @@ groups: default_value: ON field: smartAudioAltSoftSerialMethod type: bool + - name: vtx_softserial_shortstop + description: "Enable the 3x shorter stopbit on softserial. Need for some IRC Tramp VTXes." + default_value: OFF + field: softSerialShortStop + type: bool + - name: vtx_smartaudio_stopbits + description: "Set stopbit count for serial (TBS Sixty9 SmartAudio 2.1 require value of 1 bit)" + default_value: 2 + field: smartAudioStopBits + min: 1 + max: 2 - name: PG_VTX_SETTINGS_CONFIG type: vtxSettingsConfig_t @@ -3659,7 +3733,7 @@ groups: members: - name: vtx_band description: "Configure the VTX band. Set to zero to use `vtx_freq`. Bands: 1: A, 2: B, 3: E, 4: F, 5: Race." - default_value: 4 + default_value: 1 field: band min: VTX_SETTINGS_NO_BAND max: VTX_SETTINGS_MAX_BAND @@ -3692,6 +3766,13 @@ groups: field: maxPowerOverride min: 0 max: 10000 + - name: vtx_frequency_group + field: frequencyGroup + description: "VTx Frequency group to use. Frequency groups: FREQUENCYGROUP_5G8: 5.8GHz, FREQUENCYGROUP_2G4: 2.4GHz, FREQUENCYGROUP_1G3: 1.3GHz." + table: vtx_frequency_groups + min: 0 + max: 2 + default_value: FREQUENCYGROUP_5G8 - name: PG_PINIOBOX_CONFIG type: pinioBoxConfig_t diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 3e99afa466..ee4cf4ef54 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -93,16 +93,6 @@ PG_RESET_TEMPLATE(mixerConfig_t, mixerConfig, .outputMode = SETTING_OUTPUT_MODE_DEFAULT, ); -#ifdef BRUSHED_MOTORS -#define DEFAULT_PWM_PROTOCOL PWM_TYPE_BRUSHED -#define DEFAULT_PWM_RATE 16000 -#else -#define DEFAULT_PWM_PROTOCOL PWM_TYPE_ONESHOT125 -#define DEFAULT_PWM_RATE 400 -#endif - -#define DEFAULT_MAX_THROTTLE 1850 - PG_REGISTER_WITH_RESET_TEMPLATE(motorConfig_t, motorConfig, PG_MOTOR_CONFIG, 9); PG_RESET_TEMPLATE(motorConfig_t, motorConfig, diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 804a539a42..7e4608d241 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -591,8 +591,7 @@ int16_t angleFreefloatDeadband(int16_t deadband, flight_dynamics_index_t axis) } } -static void pidLevel(pidState_t *pidState, flight_dynamics_index_t axis, float horizonRateMagnitude, float dT) -{ +static float computePidLevelTarget(flight_dynamics_index_t axis) { // This is ROLL/PITCH, run ANGLE/HORIZON controllers float angleTarget = pidRcCommandToAngle(rcCommand[axis], pidProfile()->max_angle_inclination[axis]); @@ -620,6 +619,12 @@ static void pidLevel(pidState_t *pidState, flight_dynamics_index_t axis, float h DEBUG_SET(DEBUG_AUTOLEVEL, 3, angleTarget * 10); } + return angleTarget; +} + +static void pidLevel(const float angleTarget, pidState_t *pidState, flight_dynamics_index_t axis, float horizonRateMagnitude, float dT) +{ + #ifdef USE_SECONDARY_IMU float actual; if (secondaryImuState.active && secondaryImuConfig()->useForStabilized) { @@ -702,13 +707,13 @@ static float pTermProcess(pidState_t *pidState, float rateError, float dT) { } #ifdef USE_D_BOOST -static float FAST_CODE applyDBoost(pidState_t *pidState, float dT) { +static float FAST_CODE applyDBoost(pidState_t *pidState, float currentRateTarget, float dT) { float dBoost = 1.0f; const float dBoostGyroDelta = (pidState->gyroRate - pidState->previousRateGyro) / dT; const float dBoostGyroAcceleration = fabsf(biquadFilterApply(&pidState->dBoostGyroLpf, dBoostGyroDelta)); - const float dBoostRateAcceleration = fabsf((pidState->rateTarget - pidState->previousRateTarget) / dT); + const float dBoostRateAcceleration = fabsf((currentRateTarget - pidState->previousRateTarget) / dT); if (dBoostGyroAcceleration >= dBoostRateAcceleration) { //Gyro is accelerating faster than setpoint, we want to smooth out @@ -731,7 +736,7 @@ static float applyDBoost(pidState_t *pidState, float dT) { } #endif -static float dTermProcess(pidState_t *pidState, float dT) { +static float dTermProcess(pidState_t *pidState, float currentRateTarget, float dT) { // Calculate new D-term float newDTerm = 0; if (pidState->kD == 0) { @@ -744,7 +749,7 @@ static float dTermProcess(pidState_t *pidState, float dT) { delta = dTermLpf2FilterApplyFn((filter_t *) &pidState->dtermLpf2State, delta); // Calculate derivative - newDTerm = delta * (pidState->kD / dT) * applyDBoost(pidState, dT); + newDTerm = delta * (pidState->kD / dT) * applyDBoost(pidState, currentRateTarget, dT); } return(newDTerm); } @@ -766,10 +771,12 @@ static void nullRateController(pidState_t *pidState, flight_dynamics_index_t axi static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, flight_dynamics_index_t axis, float dT) { - const float rateError = pidState->rateTarget - pidState->gyroRate; + const float rateTarget = getFlightAxisRateOverride(axis, pidState->rateTarget); + + const float rateError = rateTarget - pidState->gyroRate; const float newPTerm = pTermProcess(pidState, rateError, dT); - const float newDTerm = dTermProcess(pidState, dT); - const float newFFTerm = pidState->rateTarget * pidState->kFF; + const float newDTerm = dTermProcess(pidState, rateTarget, dT); + const float newFFTerm = rateTarget * pidState->kFF; /* * Integral should be updated only if axis Iterm is not frozen @@ -794,7 +801,7 @@ static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, fligh #ifdef USE_AUTOTUNE_FIXED_WING if (FLIGHT_MODE(AUTO_TUNE) && !FLIGHT_MODE(MANUAL_MODE)) { - autotuneFixedWingUpdate(axis, pidState->rateTarget, pidState->gyroRate, constrainf(newPTerm + newFFTerm, -pidState->pidSumLimit, +pidState->pidSumLimit)); + autotuneFixedWingUpdate(axis, rateTarget, pidState->gyroRate, constrainf(newPTerm + newFFTerm, -pidState->pidSumLimit, +pidState->pidSumLimit)); } #endif @@ -803,7 +810,7 @@ static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, fligh axisPID_I[axis] = pidState->errorGyroIf; axisPID_D[axis] = newDTerm; axisPID_F[axis] = newFFTerm; - axisPID_Setpoint[axis] = pidState->rateTarget; + axisPID_Setpoint[axis] = rateTarget; #endif pidState->previousRateGyro = pidState->gyroRate; @@ -828,11 +835,14 @@ static float FAST_CODE applyItermRelax(const int axis, float currentPidSetpoint, static void FAST_CODE NOINLINE pidApplyMulticopterRateController(pidState_t *pidState, flight_dynamics_index_t axis, float dT) { - const float rateError = pidState->rateTarget - pidState->gyroRate; - const float newPTerm = pTermProcess(pidState, rateError, dT); - const float newDTerm = dTermProcess(pidState, dT); - const float rateTargetDelta = pidState->rateTarget - pidState->previousRateTarget; + const float rateTarget = getFlightAxisRateOverride(axis, pidState->rateTarget); + + const float rateError = rateTarget - pidState->gyroRate; + const float newPTerm = pTermProcess(pidState, rateError, dT); + const float newDTerm = dTermProcess(pidState, rateTarget, dT); + + const float rateTargetDelta = rateTarget - pidState->previousRateTarget; const float rateTargetDeltaFiltered = pt3FilterApply(&pidState->rateTargetFilter, rateTargetDelta); /* @@ -844,7 +854,7 @@ static void FAST_CODE NOINLINE pidApplyMulticopterRateController(pidState_t *pid const float newOutput = newPTerm + newDTerm + pidState->errorGyroIf + newCDTerm; const float newOutputLimited = constrainf(newOutput, -pidState->pidSumLimit, +pidState->pidSumLimit); - float itermErrorRate = applyItermRelax(axis, pidState->rateTarget, rateError); + float itermErrorRate = applyItermRelax(axis, rateTarget, rateError); #ifdef USE_ANTIGRAVITY itermErrorRate *= iTermAntigravityGain; @@ -863,10 +873,10 @@ static void FAST_CODE NOINLINE pidApplyMulticopterRateController(pidState_t *pid axisPID_I[axis] = pidState->errorGyroIf; axisPID_D[axis] = newDTerm; axisPID_F[axis] = newCDTerm; - axisPID_Setpoint[axis] = pidState->rateTarget; + axisPID_Setpoint[axis] = rateTarget; #endif - pidState->previousRateTarget = pidState->rateTarget; + pidState->previousRateTarget = rateTarget; pidState->previousRateGyro = pidState->gyroRate; } @@ -1088,6 +1098,12 @@ void FAST_CODE pidController(float dT) bool canUseFpvCameraMix = true; uint8_t headingHoldState = getHeadingHoldState(); + // In case Yaw override is active, we engage the Heading Hold state + if (isFlightAxisAngleOverrideActive(FD_YAW)) { + headingHoldState = HEADING_HOLD_ENABLED; + headingHoldTarget = getFlightAxisAngleOverride(FD_YAW, 0); + } + if (headingHoldState == HEADING_HOLD_UPDATE_HEADING) { updateHeadingHoldTarget(DECIDEGREES_TO_DEGREES(attitude.values.yaw)); } @@ -1126,14 +1142,18 @@ void FAST_CODE pidController(float dT) } // Step 3: Run control for ANGLE_MODE, HORIZON_MODE, and HEADING_LOCK - if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) { - const float horizonRateMagnitude = calcHorizonRateMagnitude(); - pidLevel(&pidState[FD_ROLL], FD_ROLL, horizonRateMagnitude, dT); - pidLevel(&pidState[FD_PITCH], FD_PITCH, horizonRateMagnitude, dT); - canUseFpvCameraMix = false; // FPVANGLEMIX is incompatible with ANGLE/HORIZON - levelingEnabled = true; - } else { - levelingEnabled = false; + const float horizonRateMagnitude = calcHorizonRateMagnitude(); + levelingEnabled = false; + for (uint8_t axis = FD_ROLL; axis <= FD_PITCH; axis++) { + if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE) || isFlightAxisAngleOverrideActive(axis)) { + //If axis angle override, get the correct angle from Logic Conditions + float angleTarget = getFlightAxisAngleOverride(axis, computePidLevelTarget(axis)); + + //Apply the Level PID controller + pidLevel(angleTarget, &pidState[axis], axis, horizonRateMagnitude, dT); + canUseFpvCameraMix = false; // FPVANGLEMIX is incompatible with ANGLE/HORIZON + levelingEnabled = true; + } } if ((FLIGHT_MODE(TURN_ASSISTANT) || navigationRequiresTurnAssistance()) && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE) || navigationRequiresTurnAssistance())) { diff --git a/src/main/flight/rth_estimator.c b/src/main/flight/rth_estimator.c index 747f326753..104e795a9c 100644 --- a/src/main/flight/rth_estimator.c +++ b/src/main/flight/rth_estimator.c @@ -127,7 +127,7 @@ static float estimateRTHDistanceAndHeadingAfterAltitudeChange(float altitudeChan float triangleAltitudeToReturnStart = estimatedAltitudeChangeGroundDistance - GPS_distanceToHome * cos_approx(headingDiff); const float reverseHeadingDiff = RADIANS_TO_DEGREES(atan2_approx(triangleAltitude, triangleAltitudeToReturnStart)); *heading = CENTIDEGREES_TO_DEGREES(wrap_36000(DEGREES_TO_CENTIDEGREES(180 + reverseHeadingDiff + DECIDEGREES_TO_DEGREES((float)attitude.values.yaw)))); - return sqrt(sq(triangleAltitude) + sq(triangleAltitudeToReturnStart)); + return calc_length_pythagorean_2D(triangleAltitude, triangleAltitudeToReturnStart); } else { *heading = GPS_directionToHome; return GPS_distanceToHome - estimatedAltitudeChangeGroundDistance; diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c index 067a10e280..5b828d5671 100755 --- a/src/main/flight/servos.c +++ b/src/main/flight/servos.c @@ -58,7 +58,7 @@ #include "sensors/gyro.h" -PG_REGISTER_WITH_RESET_TEMPLATE(servoConfig_t, servoConfig, PG_SERVO_CONFIG, 2); +PG_REGISTER_WITH_RESET_TEMPLATE(servoConfig_t, servoConfig, PG_SERVO_CONFIG, 3); PG_RESET_TEMPLATE(servoConfig_t, servoConfig, .servoCenterPulse = SETTING_SERVO_CENTER_PULSE_DEFAULT, diff --git a/src/main/io/gps.c b/src/main/io/gps.c index 9dcb9471d6..ca71eedeb3 100755 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -91,9 +91,6 @@ static gpsProviderDescriptor_t gpsProviders[GPS_PROVIDER_COUNT] = { { false, 0, false, NULL, NULL }, #endif - /* Stub */ - { false, 0, false, NULL, NULL }, - /* NAZA GPS module */ #ifdef USE_GPS_PROTO_NAZA { false, MODE_RX, true, &gpsRestartNAZA, &gpsHandleNAZA }, @@ -123,7 +120,7 @@ static gpsProviderDescriptor_t gpsProviders[GPS_PROVIDER_COUNT] = { #endif }; -PG_REGISTER_WITH_RESET_TEMPLATE(gpsConfig_t, gpsConfig, PG_GPS_CONFIG, 0); +PG_REGISTER_WITH_RESET_TEMPLATE(gpsConfig_t, gpsConfig, PG_GPS_CONFIG, 1); PG_RESET_TEMPLATE(gpsConfig_t, gpsConfig, .provider = SETTING_GPS_PROVIDER_DEFAULT, diff --git a/src/main/io/gps.h b/src/main/io/gps.h index 6b1e09ad45..7fa018652b 100755 --- a/src/main/io/gps.h +++ b/src/main/io/gps.h @@ -34,7 +34,6 @@ typedef enum { GPS_NMEA = 0, GPS_UBLOX, - GPS_I2CNAV, GPS_NAZA, GPS_UBLOX7PLUS, GPS_MTK, diff --git a/src/main/io/gps_naza.c b/src/main/io/gps_naza.c index 1b1a35e258..345dd2d2b2 100644 --- a/src/main/io/gps_naza.c +++ b/src/main/io/gps_naza.c @@ -194,7 +194,7 @@ static bool NAZA_parse_gps(void) gpsSol.eph = gpsConstrainEPE(h_acc / 10); // hAcc in cm gpsSol.epv = gpsConstrainEPE(v_acc / 10); // vAcc in cm gpsSol.numSat = _buffernaza.nav.satellites; - gpsSol.groundSpeed = fast_fsqrtf(sq(gpsSol.velNED[X]) + sq(gpsSol.velNED[Y])); //cm/s + gpsSol.groundSpeed = calc_length_pythagorean_2D(gpsSol.velNED[X], gpsSol.velNED[Y]); //cm/s // calculate gps heading from VELNE gpsSol.groundCourse = (uint16_t)(fmodf(RADIANS_TO_DECIDEGREES(atan2_approx(gpsSol.velNED[Y], gpsSol.velNED[X])) + 3600.0f, 3600.0f)); diff --git a/src/main/io/gps_ublox.c b/src/main/io/gps_ublox.c index 1dfa702bfa..a2ea21d523 100755 --- a/src/main/io/gps_ublox.c +++ b/src/main/io/gps_ublox.c @@ -961,27 +961,6 @@ STATIC_PROTOTHREAD(gpsProtocolStateThread) // Change baud rate if (gpsState.gpsConfig->autoBaud != GPS_AUTOBAUD_OFF) { -#if 0 - // Autobaud logic: - // 0. Wait for TX buffer to be empty - ptWait(isSerialTransmitBufferEmpty(gpsState.gpsPort)); - - // 1. Set serial port to baud rate specified by [autoBaudrateIndex] - serialSetBaudRate(gpsState.gpsPort, baudRates[gpsToSerialBaudRate[gpsState.autoBaudrateIndex]]); - gpsState.autoBaudrateIndex = (gpsState.autoBaudrateIndex + 1) % GPS_BAUDRATE_COUNT; - - // 2. Send an $UBX command to switch the baud rate specified by portConfig [baudrateIndex] - serialPrint(gpsState.gpsPort, baudInitDataNMEA[gpsState.baudrateIndex]); - - // 3. Wait for command to be received and processed by GPS - ptWait(isSerialTransmitBufferEmpty(gpsState.gpsPort)); - - // 4. Switch to [baudrateIndex] - serialSetBaudRate(gpsState.gpsPort, baudRates[gpsToSerialBaudRate[gpsState.baudrateIndex]]); - - // 5. Attempt to configure the GPS - ptDelayMs(GPS_BAUD_CHANGE_DELAY); -#else // 0. Wait for TX buffer to be empty ptWait(isSerialTransmitBufferEmpty(gpsState.gpsPort)); @@ -998,9 +977,7 @@ STATIC_PROTOTHREAD(gpsProtocolStateThread) // 4. Extra wait to make sure GPS processed the command ptDelayMs(GPS_BAUD_CHANGE_DELAY); } - serialSetBaudRate(gpsState.gpsPort, baudRates[gpsToSerialBaudRate[gpsState.baudrateIndex]]); -#endif } else { // No auto baud - set port baud rate to [baudrateIndex] diff --git a/src/main/io/osd.c b/src/main/io/osd.c index c517671b9d..06430a6b67 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -1506,6 +1506,40 @@ int8_t getGeoWaypointNumber(int8_t waypointIndex) return geoWaypointIndex + 1; } +void osdDisplaySwitchIndicator(const char *swName, int rcValue, char *buff) { + int8_t ptr = 0; + + if (osdConfig()->osd_switch_indicators_align_left) { + for (ptr = 0; ptr < constrain(strlen(swName), 0, OSD_SWITCH_INDICATOR_NAME_LENGTH); ptr++) { + buff[ptr] = swName[ptr]; + } + + if ( rcValue < 1333) { + buff[ptr++] = SYM_SWITCH_INDICATOR_LOW; + } else if ( rcValue > 1666) { + buff[ptr++] = SYM_SWITCH_INDICATOR_HIGH; + } else { + buff[ptr++] = SYM_SWITCH_INDICATOR_MID; + } + } else { + if ( rcValue < 1333) { + buff[ptr++] = SYM_SWITCH_INDICATOR_LOW; + } else if ( rcValue > 1666) { + buff[ptr++] = SYM_SWITCH_INDICATOR_HIGH; + } else { + buff[ptr++] = SYM_SWITCH_INDICATOR_MID; + } + + for (ptr = 1; ptr < constrain(strlen(swName), 0, OSD_SWITCH_INDICATOR_NAME_LENGTH) + 1; ptr++) { + buff[ptr] = swName[ptr-1]; + } + + ptr++; + } + + buff[ptr] = '\0'; +} + static bool osdDrawSingleElement(uint8_t item) { uint16_t pos = osdLayoutsConfig()->item_pos[currentLayout][item]; @@ -1548,13 +1582,18 @@ static bool osdDrawSingleElement(uint8_t item) } break; - case OSD_MAH_DRAWN: - tfp_sprintf(buff, "%4d", (int)getMAhDrawn()); - buff[4] = SYM_MAH; - buff[5] = '\0'; + case OSD_MAH_DRAWN: { + if (osdFormatCentiNumber(buff, getMAhDrawn() * 100, 1000, 0, (osdConfig()->mAh_used_precision - 2), osdConfig()->mAh_used_precision)) { + // Shown in mAh + buff[osdConfig()->mAh_used_precision] = SYM_AH; + } else { + // Shown in Ah + buff[osdConfig()->mAh_used_precision] = SYM_MAH; + } + buff[(osdConfig()->mAh_used_precision + 1)] = '\0'; osdUpdateBatteryCapacityOrVoltageTextAttributes(&elemAttr); break; - + } case OSD_WH_DRAWN: osdFormatCentiNumber(buff, getMWhDrawn() / 10, 0, 2, 0, 3); osdUpdateBatteryCapacityOrVoltageTextAttributes(&elemAttr); @@ -2274,6 +2313,22 @@ static bool osdDrawSingleElement(uint8_t item) } #endif + case OSD_SWITCH_INDICATOR_0: + osdDisplaySwitchIndicator(osdConfig()->osd_switch_indicator0_name, rxGetChannelValue(osdConfig()->osd_switch_indicator0_channel - 1), buff); + break; + + case OSD_SWITCH_INDICATOR_1: + osdDisplaySwitchIndicator(osdConfig()->osd_switch_indicator1_name, rxGetChannelValue(osdConfig()->osd_switch_indicator1_channel - 1), buff); + break; + + case OSD_SWITCH_INDICATOR_2: + osdDisplaySwitchIndicator(osdConfig()->osd_switch_indicator2_name, rxGetChannelValue(osdConfig()->osd_switch_indicator2_channel - 1), buff); + break; + + case OSD_SWITCH_INDICATOR_3: + osdDisplaySwitchIndicator(osdConfig()->osd_switch_indicator3_name, rxGetChannelValue(osdConfig()->osd_switch_indicator3_channel - 1), buff); + break; + case OSD_ACTIVE_PROFILE: tfp_sprintf(buff, "%c%u", SYM_PROFILE, (getConfigProfile() + 1)); displayWrite(osdDisplayPort, elemPosX, elemPosY, buff); @@ -2964,6 +3019,16 @@ static bool osdDrawSingleElement(uint8_t item) return true; } + case OSD_TPA_TIME_CONSTANT: + { + osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "TPA TC", 0, currentControlRateProfile->throttle.fixedWingTauMs, 4, 0, ADJUSTMENT_FW_TPA_TIME_CONSTANT); + return true; + } + case OSD_FW_LEVEL_TRIM: + { + osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "LEVEL", 0, pidProfileMutable()->fixedWingLevelTrim, 3, 1, ADJUSTMENT_FW_LEVEL_TRIM); + return true; + } case OSD_NAV_FW_CONTROL_SMOOTHNESS: osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "CTL S", 0, navConfig()->fw.control_smoothness, 1, 0, ADJUSTMENT_NAV_FW_CONTROL_SMOOTHNESS); @@ -3206,6 +3271,8 @@ PG_RESET_TEMPLATE(osdConfig_t, osdConfig, .hud_radar_disp = SETTING_OSD_HUD_RADAR_DISP_DEFAULT, .hud_radar_range_min = SETTING_OSD_HUD_RADAR_RANGE_MIN_DEFAULT, .hud_radar_range_max = SETTING_OSD_HUD_RADAR_RANGE_MAX_DEFAULT, + .hud_radar_alt_difference_display_time = SETTING_OSD_HUD_RADAR_ALT_DIFFERENCE_DISPLAY_TIME_DEFAULT, + .hud_radar_distance_display_time = SETTING_OSD_HUD_RADAR_DISTANCE_DISPLAY_TIME_DEFAULT, .hud_wp_disp = SETTING_OSD_HUD_WP_DISP_DEFAULT, .left_sidebar_scroll = SETTING_OSD_LEFT_SIDEBAR_SCROLL_DEFAULT, .right_sidebar_scroll = SETTING_OSD_RIGHT_SIDEBAR_SCROLL_DEFAULT, @@ -3218,6 +3285,16 @@ PG_RESET_TEMPLATE(osdConfig_t, osdConfig, .pan_servo_index = SETTING_OSD_PAN_SERVO_INDEX_DEFAULT, .pan_servo_pwm2centideg = SETTING_OSD_PAN_SERVO_PWM2CENTIDEG_DEFAULT, .esc_rpm_precision = SETTING_OSD_ESC_RPM_PRECISION_DEFAULT, + .mAh_used_precision = SETTING_OSD_MAH_USED_PRECISION_DEFAULT, + .osd_switch_indicator0_name = SETTING_OSD_SWITCH_INDICATOR_ZERO_NAME_DEFAULT, + .osd_switch_indicator0_channel = SETTING_OSD_SWITCH_INDICATOR_ZERO_CHANNEL_DEFAULT, + .osd_switch_indicator1_name = SETTING_OSD_SWITCH_INDICATOR_ONE_NAME_DEFAULT, + .osd_switch_indicator1_channel = SETTING_OSD_SWITCH_INDICATOR_ONE_CHANNEL_DEFAULT, + .osd_switch_indicator2_name = SETTING_OSD_SWITCH_INDICATOR_TWO_NAME_DEFAULT, + .osd_switch_indicator2_channel = SETTING_OSD_SWITCH_INDICATOR_TWO_CHANNEL_DEFAULT, + .osd_switch_indicator3_name = SETTING_OSD_SWITCH_INDICATOR_THREE_NAME_DEFAULT, + .osd_switch_indicator3_channel = SETTING_OSD_SWITCH_INDICATOR_THREE_CHANNEL_DEFAULT, + .osd_switch_indicators_align_left = SETTING_OSD_SWITCH_INDICATORS_ALIGN_LEFT_DEFAULT, .system_msg_display_time = SETTING_OSD_SYSTEM_MSG_DISPLAY_TIME_DEFAULT, .units = SETTING_OSD_UNITS_DEFAULT, .main_voltage_decimals = SETTING_OSD_MAIN_VOLTAGE_DECIMALS_DEFAULT, @@ -3380,6 +3457,11 @@ void pgResetFn_osdLayoutsConfig(osdLayoutsConfig_t *osdLayoutsConfig) osdLayoutsConfig->item_pos[0][OSD_GVAR_2] = OSD_POS(1, 3); osdLayoutsConfig->item_pos[0][OSD_GVAR_3] = OSD_POS(1, 4); + osdLayoutsConfig->item_pos[0][OSD_SWITCH_INDICATOR_0] = OSD_POS(2, 7); + osdLayoutsConfig->item_pos[0][OSD_SWITCH_INDICATOR_1] = OSD_POS(2, 8); + osdLayoutsConfig->item_pos[0][OSD_SWITCH_INDICATOR_2] = OSD_POS(2, 9); + osdLayoutsConfig->item_pos[0][OSD_SWITCH_INDICATOR_3] = OSD_POS(2, 10); + #if defined(USE_ESC_SENSOR) osdLayoutsConfig->item_pos[0][OSD_ESC_RPM] = OSD_POS(1, 2); osdLayoutsConfig->item_pos[0][OSD_ESC_TEMPERATURE] = OSD_POS(1, 3); diff --git a/src/main/io/osd.h b/src/main/io/osd.h index 45579d1bed..53814003ba 100644 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -253,6 +253,12 @@ typedef enum { OSD_AIR_MAX_SPEED, OSD_ACTIVE_PROFILE, OSD_MISSION, + OSD_SWITCH_INDICATOR_0, + OSD_SWITCH_INDICATOR_1, + OSD_SWITCH_INDICATOR_2, + OSD_SWITCH_INDICATOR_3, + OSD_TPA_TIME_CONSTANT, + OSD_FW_LEVEL_TRIM, OSD_ITEM_COUNT // MUST BE LAST } osd_items_e; @@ -318,6 +324,8 @@ typedef struct osdLayoutsConfig_s { PG_DECLARE(osdLayoutsConfig_t, osdLayoutsConfig); +#define OSD_SWITCH_INDICATOR_NAME_LENGTH 4 + typedef struct osdConfig_s { // Alarms uint8_t rssi_alarm; // rssi % @@ -372,6 +380,8 @@ typedef struct osdConfig_s { uint8_t hud_radar_disp; uint16_t hud_radar_range_min; uint16_t hud_radar_range_max; + uint8_t hud_radar_alt_difference_display_time; + uint8_t hud_radar_distance_display_time; uint8_t hud_wp_disp; uint8_t left_sidebar_scroll; // from osd_sidebar_scroll_e @@ -389,27 +399,36 @@ typedef struct osdConfig_s { uint8_t coordinate_digits; - bool osd_failsafe_switch_layout; + bool osd_failsafe_switch_layout; uint8_t plus_code_digits; // Number of digits to use in OSD_PLUS_CODE uint8_t plus_code_short; uint8_t ahi_style; - uint8_t force_grid; // Force a pixel based OSD to use grid mode. - uint8_t ahi_bordered; // Only used by the AHI widget - uint8_t ahi_width; // In pixels, only used by the AHI widget - uint8_t ahi_height; // In pixels, only used by the AHI widget - int8_t ahi_vertical_offset; // Offset from center in pixels. Positive moves the AHI down. Widget only. - int8_t sidebar_horizontal_offset; // Horizontal offset from default position. Units are grid slots for grid OSDs, pixels for pixel based OSDs. Positive values move sidebars closer to the edges. - uint8_t left_sidebar_scroll_step; // How many units each sidebar step represents. 0 means the default value for the scroll type. - uint8_t right_sidebar_scroll_step; // Same as left_sidebar_scroll_step, but for the right sidebar. - bool osd_home_position_arm_screen; - uint8_t pan_servo_index; // Index of the pan servo used for home direction offset - int8_t pan_servo_pwm2centideg; // Centidegrees of servo rotation per us pwm + uint8_t force_grid; // Force a pixel based OSD to use grid mode. + uint8_t ahi_bordered; // Only used by the AHI widget + uint8_t ahi_width; // In pixels, only used by the AHI widget + uint8_t ahi_height; // In pixels, only used by the AHI widget + int8_t ahi_vertical_offset; // Offset from center in pixels. Positive moves the AHI down. Widget only. + int8_t sidebar_horizontal_offset; // Horizontal offset from default position. Units are grid slots for grid OSDs, pixels for pixel based OSDs. Positive values move sidebars closer to the edges. + uint8_t left_sidebar_scroll_step; // How many units each sidebar step represents. 0 means the default value for the scroll type. + uint8_t right_sidebar_scroll_step; // Same as left_sidebar_scroll_step, but for the right sidebar. + bool osd_home_position_arm_screen; + uint8_t pan_servo_index; // Index of the pan servo used for home direction offset + int8_t pan_servo_pwm2centideg; // Centidegrees of servo rotation per us pwm uint8_t crsf_lq_format; - uint16_t system_msg_display_time; // system message display time for multiple messages (ms) - uint8_t sidebar_height; // sidebar height in rows, 0 turns off sidebars leaving only level indicator arrows - uint8_t telemetry; // use telemetry on displayed pixel line 0 - uint8_t esc_rpm_precision; // Number of characters used for the RPM numbers. - + uint8_t sidebar_height; // sidebar height in rows, 0 turns off sidebars leaving only level indicator arrows + uint8_t telemetry; // use telemetry on displayed pixel line 0 + uint8_t esc_rpm_precision; // Number of characters used for the RPM numbers. + uint16_t system_msg_display_time; // system message display time for multiple messages (ms) + uint8_t mAh_used_precision; // Number of numbers used for mAh drawn. Plently of packs now are > 9999 mAh + char osd_switch_indicator0_name[OSD_SWITCH_INDICATOR_NAME_LENGTH + 1]; // Name to use for switch indicator 0. + uint8_t osd_switch_indicator0_channel; // RC Channel to use for switch indicator 0. + char osd_switch_indicator1_name[OSD_SWITCH_INDICATOR_NAME_LENGTH + 1]; // Name to use for switch indicator 1. + uint8_t osd_switch_indicator1_channel; // RC Channel to use for switch indicator 1. + char osd_switch_indicator2_name[OSD_SWITCH_INDICATOR_NAME_LENGTH + 1]; // Name to use for switch indicator 2. + uint8_t osd_switch_indicator2_channel; // RC Channel to use for switch indicator 2. + char osd_switch_indicator3_name[OSD_SWITCH_INDICATOR_NAME_LENGTH + 1]; // Name to use for switch indicator 3. + uint8_t osd_switch_indicator3_channel; // RC Channel to use for switch indicator 3. + bool osd_switch_indicators_align_left; // Align switch indicator name to left of the switch. } osdConfig_t; PG_DECLARE(osdConfig_t, osdConfig); diff --git a/src/main/io/osd_hud.c b/src/main/io/osd_hud.c index 62c8187c6b..7c900c7e6b 100644 --- a/src/main/io/osd_hud.c +++ b/src/main/io/osd_hud.c @@ -202,17 +202,48 @@ void osdHudDrawPoi(uint32_t poiDistance, int16_t poiDirection, int32_t poiAltitu // Distance - if (((millis() / 1000) % 6 == 0) && poiType > 0) { // For Radar and WPs, display the difference in altitude - altc = ((osd_unit_e)osdConfig()->units == OSD_UNIT_IMPERIAL) ? constrain(CENTIMETERS_TO_FEET(poiAltitude * 100), -99, 99) : constrain(poiAltitude, -99 , 99); - tfp_sprintf(buff, "%3d", altc); - buff[0] = (poiAltitude >= 0) ? SYM_HUD_ARROWS_U3 : SYM_HUD_ARROWS_D3; - } - else { // Display the distance by default - if ((osd_unit_e)osdConfig()->units == OSD_UNIT_IMPERIAL) { - osdFormatCentiNumber(buff, CENTIMETERS_TO_CENTIFEET(poiDistance * 100), FEET_PER_MILE, 0, 3, 3); + if (poiType > 0 && + ((millis() / 1000) % (osdConfig()->hud_radar_alt_difference_display_time + osdConfig()->hud_radar_distance_display_time) < (osdConfig()->hud_radar_alt_difference_display_time % (osdConfig()->hud_radar_alt_difference_display_time + osdConfig()->hud_radar_distance_display_time))) + ) { // For Radar and WPs, display the difference in altitude, then distance. Time is pilot defined + altc = constrain(poiAltitude, -99 , 99); + + switch ((osd_unit_e)osdConfig()->units) { + case OSD_UNIT_UK: + FALLTHROUGH; + case OSD_UNIT_GA: + FALLTHROUGH; + case OSD_UNIT_IMPERIAL: + // Convert to feet + altc = constrain(CENTIMETERS_TO_FEET(poiAltitude * 100), -99, 99); + break; + default: + FALLTHROUGH; + case OSD_UNIT_METRIC_MPH: + FALLTHROUGH; + case OSD_UNIT_METRIC: + // Already in metres + break; } - else { - osdFormatCentiNumber(buff, poiDistance * 100, METERS_PER_KILOMETER, 0, 3, 3); + + tfp_sprintf(buff, "%3d", altc); + buff[0] = (poiAltitude >= 0) ? SYM_DIRECTION : SYM_DIRECTION+4; + } else { // Display the distance by default + switch ((osd_unit_e)osdConfig()->units) { + case OSD_UNIT_UK: + FALLTHROUGH; + case OSD_UNIT_IMPERIAL: + osdFormatCentiNumber(buff, CENTIMETERS_TO_CENTIFEET(poiDistance * 100), FEET_PER_MILE, 0, 3, 3); + break; + case OSD_UNIT_GA: + osdFormatCentiNumber(buff, CENTIMETERS_TO_CENTIFEET(poiDistance * 100), FEET_PER_NAUTICALMILE, 0, 3, 3); + break; + default: + FALLTHROUGH; + case OSD_UNIT_METRIC_MPH: + FALLTHROUGH; + case OSD_UNIT_METRIC: + osdFormatCentiNumber(buff, poiDistance * 100, METERS_PER_KILOMETER, 0, 3, 3); + break; } } diff --git a/src/main/io/pwmdriver_i2c.c b/src/main/io/pwmdriver_i2c.c deleted file mode 100644 index 0103a29102..0000000000 --- a/src/main/io/pwmdriver_i2c.c +++ /dev/null @@ -1,70 +0,0 @@ -#include -#include - -#include "drivers/io_pca9685.h" - -#include "fc/config.h" -#include "fc/runtime_config.h" - -#include "config/feature.h" -#include "platform.h" - -#ifdef USE_PWM_SERVO_DRIVER - -#define PWM_DRIVER_IMPLEMENTATION_COUNT 1 -#define PWM_DRIVER_MAX_CYCLE 4 - -static bool driverEnabled = false; -static uint8_t driverImplementationIndex = 0; - -typedef struct { - bool (*initFunction)(void); - void (*writeFunction)(uint8_t servoIndex, uint16_t off); - void (*setFrequencyFunction)(uint16_t freq); - void (*syncFunction)(uint8_t cycleIndex); -} pwmDriverDriver_t; - -pwmDriverDriver_t pwmDrivers[PWM_DRIVER_IMPLEMENTATION_COUNT] = { - [0] = { - .initFunction = pca9685Initialize, - .writeFunction = pca9685setServoPulse, - .setFrequencyFunction = pca9685setPWMFreq, - .syncFunction = pca9685sync - } -}; - -bool isPwmDriverEnabled(void) { - return driverEnabled; -} - -void pwmDriverSetPulse(uint8_t servoIndex, uint16_t length) { - (pwmDrivers[driverImplementationIndex].writeFunction)(servoIndex, length); -} - -void pwmDriverInitialize(void) { - driverEnabled = (pwmDrivers[driverImplementationIndex].initFunction)(); - - if (driverEnabled) { - ENABLE_STATE(PWM_DRIVER_AVAILABLE); - } else { - DISABLE_STATE(PWM_DRIVER_AVAILABLE); - } - -} - -void pwmDriverSync(void) { - if (!STATE(PWM_DRIVER_AVAILABLE)) { - return; - } - - static uint8_t cycle = 0; - - (pwmDrivers[driverImplementationIndex].syncFunction)(cycle); - - cycle++; - if (cycle == PWM_DRIVER_MAX_CYCLE) { - cycle = 0; - } -} - -#endif \ No newline at end of file diff --git a/src/main/io/pwmdriver_i2c.h b/src/main/io/pwmdriver_i2c.h deleted file mode 100644 index 1170c87109..0000000000 --- a/src/main/io/pwmdriver_i2c.h +++ /dev/null @@ -1,4 +0,0 @@ - -void pwmDriverInitialize(void); -void pwmDriverSync(void); -void pwmDriverSetPulse(uint8_t servoIndex, uint16_t length); diff --git a/src/main/io/vtx.c b/src/main/io/vtx.c index eabe0d1f00..c7891ce6bd 100644 --- a/src/main/io/vtx.c +++ b/src/main/io/vtx.c @@ -53,6 +53,7 @@ PG_RESET_TEMPLATE(vtxSettingsConfig_t, vtxSettingsConfig, .pitModeChan = SETTING_VTX_PIT_MODE_CHAN_DEFAULT, .lowPowerDisarm = SETTING_VTX_LOW_POWER_DISARM_DEFAULT, .maxPowerOverride = SETTING_VTX_MAX_POWER_OVERRIDE_DEFAULT, + .frequencyGroup = SETTING_VTX_FREQUENCY_GROUP_DEFAULT, ); typedef enum { diff --git a/src/main/io/vtx.h b/src/main/io/vtx.h index 4bff70af54..1a0fcaac48 100644 --- a/src/main/io/vtx.h +++ b/src/main/io/vtx.h @@ -41,6 +41,7 @@ typedef struct vtxSettingsConfig_s { uint16_t pitModeChan; // sets out-of-range pitmode frequency uint8_t lowPowerDisarm; // min power while disarmed, from vtxLowerPowerDisarm_e uint16_t maxPowerOverride; // for VTX drivers that are polling VTX capabilities - override what VTX is reporting + uint8_t frequencyGroup; // Frequencies being used, i.e. 5.8, 2.4, or 1.3 GHz } vtxSettingsConfig_t; PG_DECLARE(vtxSettingsConfig_t, vtxSettingsConfig); diff --git a/src/main/io/vtx_control.c b/src/main/io/vtx_control.c index 516b78ab06..02fc8010d0 100644 --- a/src/main/io/vtx_control.c +++ b/src/main/io/vtx_control.c @@ -40,12 +40,14 @@ #if defined(USE_VTX_CONTROL) -PG_REGISTER_WITH_RESET_TEMPLATE(vtxConfig_t, vtxConfig, PG_VTX_CONFIG, 3); +PG_REGISTER_WITH_RESET_TEMPLATE(vtxConfig_t, vtxConfig, PG_VTX_CONFIG, 4); PG_RESET_TEMPLATE(vtxConfig_t, vtxConfig, .halfDuplex = SETTING_VTX_HALFDUPLEX_DEFAULT, .smartAudioEarlyAkkWorkaroundEnable = SETTING_VTX_SMARTAUDIO_EARLY_AKK_WORKAROUND_DEFAULT, .smartAudioAltSoftSerialMethod = SETTING_VTX_SMARTAUDIO_ALTERNATE_SOFTSERIAL_METHOD_DEFAULT, + .softSerialShortStop = SETTING_VTX_SOFTSERIAL_SHORTSTOP_DEFAULT, + .smartAudioStopBits = SETTING_VTX_SMARTAUDIO_STOPBITS_DEFAULT, ); static uint8_t locked = 0; diff --git a/src/main/io/vtx_control.h b/src/main/io/vtx_control.h index b474a4bc0b..83a2b1d428 100644 --- a/src/main/io/vtx_control.h +++ b/src/main/io/vtx_control.h @@ -33,6 +33,8 @@ typedef struct vtxConfig_s { uint8_t halfDuplex; uint8_t smartAudioEarlyAkkWorkaroundEnable; bool smartAudioAltSoftSerialMethod; + bool softSerialShortStop; + uint8_t smartAudioStopBits; } vtxConfig_t; PG_DECLARE(vtxConfig_t, vtxConfig); diff --git a/src/main/io/vtx_smartaudio.c b/src/main/io/vtx_smartaudio.c index 3ee412ff27..d498f3e816 100644 --- a/src/main/io/vtx_smartaudio.c +++ b/src/main/io/vtx_smartaudio.c @@ -236,11 +236,6 @@ static void saAutobaud(void) return; } -#if 0 - LOG_D(VTX, "autobaud: %d rcvd %d/%d (%d)", - sa_smartbaud, saStat.pktrcvd, saStat.pktsent, ((saStat.pktrcvd * 100) / saStat.pktsent))); -#endif - if (((saStat.pktrcvd * 100) / saStat.pktsent) >= 70) { // This is okay saStat.pktsent = 0; // Should be more moderate? @@ -642,13 +637,6 @@ void saSetPitFreq(uint16_t freq) saSetFreq(freq | SA_FREQ_SETPIT); } -#if 0 -static void saGetPitFreq(void) -{ - saDoDevSetFreq(SA_FREQ_GETPIT); -} -#endif - static bool saValidateBandAndChannel(uint8_t band, uint8_t channel) { return (band >= VTX_SMARTAUDIO_MIN_BAND && band <= VTX_SMARTAUDIO_MAX_BAND && @@ -689,7 +677,7 @@ bool vtxSmartAudioInit(void) { serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_VTX_SMARTAUDIO); if (portConfig) { - portOptions_t portOptions = SERIAL_STOPBITS_2 | SERIAL_BIDIR_NOPULL; + portOptions_t portOptions = (vtxConfig()->smartAudioStopBits == 2 ? SERIAL_STOPBITS_2 : SERIAL_STOPBITS_1) | SERIAL_BIDIR_NOPULL; portOptions = portOptions | (vtxConfig()->halfDuplex ? SERIAL_BIDIR | SERIAL_BIDIR_PP : SERIAL_UNIDIR); smartAudioSerialPort = openSerialPort(portConfig->identifier, FUNCTION_VTX_SMARTAUDIO, NULL, NULL, 4800, MODE_RXTX, portOptions); } diff --git a/src/main/io/vtx_string.c b/src/main/io/vtx_string.c index ed75c981bd..b86a7ba027 100644 --- a/src/main/io/vtx_string.c +++ b/src/main/io/vtx_string.c @@ -25,11 +25,15 @@ #include "platform.h" #include "build/debug.h" -#define VTX_STRING_BAND_COUNT 5 -#define VTX_STRING_CHAN_COUNT 8 -#define VTX_STRING_POWER_COUNT 5 +#define VTX_STRING_5G8_BAND_COUNT 5 +#define VTX_STRING_5G8_CHAN_COUNT 8 +#define VTX_STRING_5G8_POWER_COUNT 5 -const uint16_t vtx58frequencyTable[VTX_STRING_BAND_COUNT][VTX_STRING_CHAN_COUNT] = +#define VTX_STRING_1G3_BAND_COUNT 2 +#define VTX_STRING_1G3_CHAN_COUNT 8 +#define VTX_STRING_1G3_POWER_COUNT 3 + +const uint16_t vtx58frequencyTable[VTX_STRING_5G8_BAND_COUNT][VTX_STRING_5G8_CHAN_COUNT] = { { 5865, 5845, 5825, 5805, 5785, 5765, 5745, 5725 }, // A { 5733, 5752, 5771, 5790, 5809, 5828, 5847, 5866 }, // B @@ -38,7 +42,7 @@ const uint16_t vtx58frequencyTable[VTX_STRING_BAND_COUNT][VTX_STRING_CHAN_COUNT] { 5658, 5695, 5732, 5769, 5806, 5843, 5880, 5917 }, // R }; -const char * const vtx58BandNames[VTX_STRING_BAND_COUNT + 1] = { +const char * const vtx58BandNames[VTX_STRING_5G8_BAND_COUNT + 1] = { "-", "A", "B", @@ -47,16 +51,38 @@ const char * const vtx58BandNames[VTX_STRING_BAND_COUNT + 1] = { "R", }; -const char vtx58BandLetter[VTX_STRING_BAND_COUNT + 1] = "-ABEFR"; +const char vtx58BandLetter[VTX_STRING_5G8_BAND_COUNT + 1] = "-ABEFR"; -const char * const vtx58ChannelNames[VTX_STRING_CHAN_COUNT + 1] = { +const char * const vtx58ChannelNames[VTX_STRING_5G8_CHAN_COUNT + 1] = { "-", "1", "2", "3", "4", "5", "6", "7", "8", }; -const char * const vtx58DefaultPowerNames[VTX_STRING_POWER_COUNT + 1] = { +const char * const vtx58DefaultPowerNames[VTX_STRING_5G8_POWER_COUNT + 1] = { "---", "PL1", "PL2", "PL3", "PL4", "PL5" }; +const uint16_t vtx1G3frequencyTable[VTX_STRING_1G3_BAND_COUNT][VTX_STRING_1G3_CHAN_COUNT] = +{ + { 1080, 1120, 1160, 1200, 1240, 1280, 1320, 1360 }, // A + { 1080, 1120, 1160, 1200, 1258, 1280, 1320, 1360 }, // B +}; + +const char * const vtx1G3BandNames[VTX_STRING_1G3_BAND_COUNT + 1] = { + "-", + "A", + "B", +}; + +const char vtx1G3BandLetter[VTX_STRING_1G3_BAND_COUNT + 1] = "-AB"; + +const char * const vtx1G3ChannelNames[VTX_STRING_1G3_CHAN_COUNT + 1] = { + "-", "1", "2", "3", "4", "5", "6", "7", "8", +}; + +const char * const vtx1G3DefaultPowerNames[VTX_STRING_1G3_POWER_COUNT + 1] = { + "---", "PL1", "PL2", "PL3" +}; + bool vtx58_Freq2Bandchan(uint16_t freq, uint8_t *pBand, uint8_t *pChannel) { int8_t band; @@ -80,15 +106,28 @@ bool vtx58_Freq2Bandchan(uint16_t freq, uint8_t *pBand, uint8_t *pChannel) return false; } -//Converts band and channel values to a frequency (in MHz) value. +// Converts band and channel values to a frequency (in MHz) value. // band: Band value (1 to 5). // channel: Channel value (1 to 8). // Returns frequency value (in MHz), or 0 if band/channel out of range. uint16_t vtx58_Bandchan2Freq(uint8_t band, uint8_t channel) { - if (band > 0 && band <= VTX_STRING_BAND_COUNT && - channel > 0 && channel <= VTX_STRING_CHAN_COUNT) { + if (band > 0 && band <= VTX_STRING_5G8_BAND_COUNT && + channel > 0 && channel <= VTX_STRING_5G8_CHAN_COUNT) { return vtx58frequencyTable[band - 1][channel - 1]; } return 0; } + +// Converts band and channel values to a frequency (in MHz) value. +// band: Band value (1 to 2). +// channel: Channel value (1 to 8). +// Returns frequency value (in MHz), or 0 if band/channel out of range. +uint16_t vtx1G3_Bandchan2Freq(uint8_t band, uint8_t channel) +{ + if (band > 0 && band <= VTX_STRING_1G3_BAND_COUNT && + channel > 0 && channel <= VTX_STRING_1G3_CHAN_COUNT) { + return vtx1G3frequencyTable[band - 1][channel - 1]; + } + return 0; +} diff --git a/src/main/io/vtx_string.h b/src/main/io/vtx_string.h index cb63cbe7ae..e23990ec77 100644 --- a/src/main/io/vtx_string.h +++ b/src/main/io/vtx_string.h @@ -8,5 +8,12 @@ extern const char * const vtx58ChannelNames[]; extern const char * const vtx58DefaultPowerNames[]; extern const char vtx58BandLetter[]; +extern const uint16_t vtx1G3frequencyTable[2][8]; +extern const char * const vtx1G3BandNames[]; +extern const char * const vtx1G3ChannelNames[]; +extern const char * const vtx1G3DefaultPowerNames[]; +extern const char vtx51G3BandLetter[]; + bool vtx58_Freq2Bandchan(uint16_t freq, uint8_t *pBand, uint8_t *pChannel); uint16_t vtx58_Bandchan2Freq(uint8_t band, uint8_t channel); +uint16_t vtx1G3_Bandchan2Freq(uint8_t band, uint8_t channel); diff --git a/src/main/io/vtx_tramp.c b/src/main/io/vtx_tramp.c index 7e50122113..2791ada294 100644 --- a/src/main/io/vtx_tramp.c +++ b/src/main/io/vtx_tramp.c @@ -407,8 +407,12 @@ static void impl_SetBandAndChannel(vtxDevice_t * vtxDevice, uint8_t band, uint8_ return; } - // TRAMP is 5.8 GHz only - uint16_t newFreqMhz = vtx58_Bandchan2Freq(band, channel); + // Default to 5.8 GHz + uint16_t newFreqMhz = vtx58_Bandchan2Freq(band, channel); + + if (vtxSettingsConfig()->frequencyGroup == FREQUENCYGROUP_1G3) { + newFreqMhz = vtx1G3_Bandchan2Freq(band, channel); + } if (newFreqMhz < vtxState.capabilities.freqMin || newFreqMhz > vtxState.capabilities.freqMax) { return; @@ -512,9 +516,20 @@ static bool impl_GetOsdInfo(const vtxDevice_t *vtxDevice, vtxDeviceOsdInfo_t * pOsdInfo->frequency = vtxState.request.freq; pOsdInfo->powerIndex = vtxState.request.powerIndex; pOsdInfo->powerMilliwatt = vtxState.request.power; - pOsdInfo->bandLetter = vtx58BandNames[vtxState.request.band][0]; - pOsdInfo->bandName = vtx58BandNames[vtxState.request.band]; - pOsdInfo->channelName = vtx58ChannelNames[vtxState.request.channel]; + + switch (vtxSettingsConfig()->frequencyGroup) { + case FREQUENCYGROUP_1G3: + pOsdInfo->bandLetter = vtx1G3BandNames[vtxState.request.band][0]; + pOsdInfo->bandName = vtx1G3BandNames[vtxState.request.band]; + pOsdInfo->channelName = vtx1G3ChannelNames[vtxState.request.channel]; + break; + default: // Currently all except 1.3GHz + pOsdInfo->bandLetter = vtx58BandNames[vtxState.request.band][0]; + pOsdInfo->bandName = vtx58BandNames[vtxState.request.band]; + pOsdInfo->channelName = vtx58ChannelNames[vtxState.request.channel]; + break; + } + pOsdInfo->powerIndexLetter = '0' + vtxState.request.powerIndex; return true; } @@ -539,67 +554,85 @@ static vtxDevice_t impl_vtxDevice = { .vTable = &impl_vtxVTable, .capability.bandCount = VTX_TRAMP_5G8_BAND_COUNT, .capability.channelCount = VTX_TRAMP_5G8_CHANNEL_COUNT, - .capability.powerCount = VTX_TRAMP_MAX_POWER_COUNT, + .capability.powerCount = VTX_TRAMP_5G8_MAX_POWER_COUNT, .capability.bandNames = (char **)vtx58BandNames, .capability.channelNames = (char **)vtx58ChannelNames, .capability.powerNames = NULL, }; -const uint16_t trampPowerTable_200[VTX_TRAMP_MAX_POWER_COUNT] = { 25, 100, 200, 200, 200 }; -const char * const trampPowerNames_200[VTX_TRAMP_MAX_POWER_COUNT + 1] = { "---", "25 ", "100", "200", "200", "200" }; +const uint16_t trampPowerTable_5G8_200[VTX_TRAMP_5G8_MAX_POWER_COUNT] = { 25, 100, 200, 200, 200 }; +const char * const trampPowerNames_5G8_200[VTX_TRAMP_5G8_MAX_POWER_COUNT + 1] = { "---", "25 ", "100", "200", "200", "200" }; -const uint16_t trampPowerTable_400[VTX_TRAMP_MAX_POWER_COUNT] = { 25, 100, 200, 400, 400 }; -const char * const trampPowerNames_400[VTX_TRAMP_MAX_POWER_COUNT + 1] = { "---", "25 ", "100", "200", "400", "400" }; +const uint16_t trampPowerTable_5G8_400[VTX_TRAMP_5G8_MAX_POWER_COUNT] = { 25, 100, 200, 400, 400 }; +const char * const trampPowerNames_5G8_400[VTX_TRAMP_5G8_MAX_POWER_COUNT + 1] = { "---", "25 ", "100", "200", "400", "400" }; -const uint16_t trampPowerTable_600[VTX_TRAMP_MAX_POWER_COUNT] = { 25, 100, 200, 400, 600 }; -const char * const trampPowerNames_600[VTX_TRAMP_MAX_POWER_COUNT + 1] = { "---", "25 ", "100", "200", "400", "600" }; +const uint16_t trampPowerTable_5G8_600[VTX_TRAMP_5G8_MAX_POWER_COUNT] = { 25, 100, 200, 400, 600 }; +const char * const trampPowerNames_5G8_600[VTX_TRAMP_5G8_MAX_POWER_COUNT + 1] = { "---", "25 ", "100", "200", "400", "600" }; -const uint16_t trampPowerTable_800[VTX_TRAMP_MAX_POWER_COUNT] = { 25, 100, 200, 500, 800 }; -const char * const trampPowerNames_800[VTX_TRAMP_MAX_POWER_COUNT + 1] = { "---", "25 ", "100", "200", "500", "800" }; +const uint16_t trampPowerTable_5G8_800[VTX_TRAMP_5G8_MAX_POWER_COUNT] = { 25, 100, 200, 500, 800 }; +const char * const trampPowerNames_5G8_800[VTX_TRAMP_5G8_MAX_POWER_COUNT + 1] = { "---", "25 ", "100", "200", "500", "800" }; + +const uint16_t trampPowerTable_1G3_800[VTX_TRAMP_1G3_MAX_POWER_COUNT] = { 25, 200, 800 }; +const char * const trampPowerNames_1G3_800[VTX_TRAMP_1G3_MAX_POWER_COUNT + 1] = { "---", "25 ", "200", "800" }; static void vtxProtoUpdatePowerMetadata(uint16_t maxPower) { - if (maxPower >= 800) { - // Max power 800mW: Use 25, 100, 200, 500, 800 table - vtxState.metadata.powerTablePtr = trampPowerTable_800; - vtxState.metadata.powerTableCount = VTX_TRAMP_MAX_POWER_COUNT; - - impl_vtxDevice.capability.powerNames = (char **)trampPowerNames_800; - impl_vtxDevice.capability.powerCount = VTX_TRAMP_MAX_POWER_COUNT; - } - else if (maxPower >= 600) { - // Max power 600mW: Use 25, 100, 200, 400, 600 table - vtxState.metadata.powerTablePtr = trampPowerTable_600; - vtxState.metadata.powerTableCount = VTX_TRAMP_MAX_POWER_COUNT; + switch (vtxSettingsConfig()->frequencyGroup) { + case FREQUENCYGROUP_1G3: + vtxState.metadata.powerTablePtr = trampPowerTable_1G3_800; + vtxState.metadata.powerTableCount = VTX_TRAMP_1G3_MAX_POWER_COUNT; + + impl_vtxDevice.capability.powerNames = (char **)trampPowerNames_1G3_800; + impl_vtxDevice.capability.powerCount = VTX_TRAMP_1G3_MAX_POWER_COUNT; + + impl_vtxDevice.capability.bandCount = VTX_TRAMP_1G3_BAND_COUNT; + impl_vtxDevice.capability.channelCount = VTX_TRAMP_1G3_CHANNEL_COUNT; + impl_vtxDevice.capability.bandNames = (char **)vtx1G3BandNames; + impl_vtxDevice.capability.channelNames = (char **)vtx1G3ChannelNames; + break; + default: + if (maxPower >= 800) { + // Max power 800mW: Use 25, 100, 200, 500, 800 table + vtxState.metadata.powerTablePtr = trampPowerTable_5G8_800; + vtxState.metadata.powerTableCount = VTX_TRAMP_5G8_MAX_POWER_COUNT; + + impl_vtxDevice.capability.powerNames = (char **)trampPowerNames_5G8_800; + impl_vtxDevice.capability.powerCount = VTX_TRAMP_5G8_MAX_POWER_COUNT; + } + else if (maxPower >= 600) { + // Max power 600mW: Use 25, 100, 200, 400, 600 table + vtxState.metadata.powerTablePtr = trampPowerTable_5G8_600; + vtxState.metadata.powerTableCount = VTX_TRAMP_5G8_MAX_POWER_COUNT; - impl_vtxDevice.capability.powerNames = (char **)trampPowerNames_600; - impl_vtxDevice.capability.powerCount = VTX_TRAMP_MAX_POWER_COUNT; - } - else if (maxPower >= 400) { - // Max power 400mW: Use 25, 100, 200, 400 table - vtxState.metadata.powerTablePtr = trampPowerTable_400; - vtxState.metadata.powerTableCount = 4; + impl_vtxDevice.capability.powerNames = (char **)trampPowerNames_5G8_600; + impl_vtxDevice.capability.powerCount = VTX_TRAMP_5G8_MAX_POWER_COUNT; + } + else if (maxPower >= 400) { + // Max power 400mW: Use 25, 100, 200, 400 table + vtxState.metadata.powerTablePtr = trampPowerTable_5G8_400; + vtxState.metadata.powerTableCount = 4; - impl_vtxDevice.capability.powerNames = (char **)trampPowerNames_400; - impl_vtxDevice.capability.powerCount = 4; - } - else if (maxPower >= 200) { - // Max power 200mW: Use 25, 100, 200 table - vtxState.metadata.powerTablePtr = trampPowerTable_200; - vtxState.metadata.powerTableCount = 3; + impl_vtxDevice.capability.powerNames = (char **)trampPowerNames_5G8_400; + impl_vtxDevice.capability.powerCount = 4; + } + else if (maxPower >= 200) { + // Max power 200mW: Use 25, 100, 200 table + vtxState.metadata.powerTablePtr = trampPowerTable_5G8_200; + vtxState.metadata.powerTableCount = 3; - impl_vtxDevice.capability.powerNames = (char **)trampPowerNames_200; - impl_vtxDevice.capability.powerCount = 3; - } - else { - // Default to standard TRAMP 600mW VTX - vtxState.metadata.powerTablePtr = trampPowerTable_600; - vtxState.metadata.powerTableCount = VTX_TRAMP_MAX_POWER_COUNT; + impl_vtxDevice.capability.powerNames = (char **)trampPowerNames_5G8_200; + impl_vtxDevice.capability.powerCount = 3; + } + else { + // Default to standard TRAMP 600mW VTX + vtxState.metadata.powerTablePtr = trampPowerTable_5G8_600; + vtxState.metadata.powerTableCount = VTX_TRAMP_5G8_MAX_POWER_COUNT; - impl_vtxDevice.capability.powerNames = (char **)trampPowerNames_600; - impl_vtxDevice.capability.powerCount = VTX_TRAMP_MAX_POWER_COUNT; + impl_vtxDevice.capability.powerNames = (char **)trampPowerNames_5G8_600; + impl_vtxDevice.capability.powerCount = VTX_TRAMP_5G8_MAX_POWER_COUNT; + } + break; } - } bool vtxTrampInit(void) @@ -609,6 +642,7 @@ bool vtxTrampInit(void) if (portConfig) { portOptions_t portOptions = 0; portOptions = portOptions | (vtxConfig()->halfDuplex ? SERIAL_BIDIR : SERIAL_UNIDIR); + portOptions = portOptions | (vtxConfig()->softSerialShortStop ? SERIAL_SHORTSTOP : SERIAL_LONGSTOP); vtxState.port = openSerialPort(portConfig->identifier, FUNCTION_VTX_TRAMP, NULL, NULL, 9600, MODE_RXTX, portOptions); } diff --git a/src/main/io/vtx_tramp.h b/src/main/io/vtx_tramp.h index 3d2643c194..1f44201853 100644 --- a/src/main/io/vtx_tramp.h +++ b/src/main/io/vtx_tramp.h @@ -19,13 +19,24 @@ #include +// 5.8 GHz #define VTX_TRAMP_5G8_BAND_COUNT 5 #define VTX_TRAMP_5G8_CHANNEL_COUNT 8 -#define VTX_TRAMP_MAX_POWER_COUNT 5 -#define VTX_TRAMP_DEFAULT_POWER 1 +#define VTX_TRAMP_5G8_MAX_POWER_COUNT 5 +#define VTX_TRAMP_5G8_DEFAULT_POWER 1 -#define VTX_TRAMP_MIN_FREQUENCY_MHZ 5000 //min freq in MHz -#define VTX_TRAMP_MAX_FREQUENCY_MHZ 5999 //max freq in MHz +#define VTX_TRAMP_5G8_MIN_FREQUENCY_MHZ 5000 //min freq in MHz +#define VTX_TRAMP_5G8_MAX_FREQUENCY_MHZ 5999 //max freq in MHz + +// 1.3 GHz +#define VTX_TRAMP_1G3_BAND_COUNT 2 +#define VTX_TRAMP_1G3_CHANNEL_COUNT 8 + +#define VTX_TRAMP_1G3_MAX_POWER_COUNT 3 +#define VTX_TRAMP_1G3_DEFAULT_POWER 1 + +#define VTX_TRAMP_1G3_MIN_FREQUENCY_MHZ 1000 +#define VTX_TRAMP_1G3_MAX_FREQUENCY_MHZ 1399 bool vtxTrampInit(void); diff --git a/src/main/msp/msp_protocol_v2_inav.h b/src/main/msp/msp_protocol_v2_inav.h index d3e7d14585..51311f4557 100755 --- a/src/main/msp/msp_protocol_v2_inav.h +++ b/src/main/msp/msp_protocol_v2_inav.h @@ -80,3 +80,4 @@ #define MSP2_INAV_SET_SAFEHOME 0x2039 #define MSP2_INAV_MISC2 0x203A +#define MSP2_INAV_LOGIC_CONDITIONS_SINGLE 0x203B diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 9abf846e6f..f5201ccf89 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -3200,13 +3200,9 @@ static void processNavigationRCAdjustments(void) } if (navStateFlags & NAV_RC_POS) { - if (!FLIGHT_MODE(FAILSAFE_MODE)) { - posControl.flags.isAdjustingPosition = adjustPositionFromRCInput(); - } - else { - if (!STATE(FIXED_WING_LEGACY)) { - resetMulticopterBrakingMode(); - } + posControl.flags.isAdjustingPosition = adjustPositionFromRCInput() && !FLIGHT_MODE(FAILSAFE_MODE); + if (STATE(MULTIROTOR) && FLIGHT_MODE(FAILSAFE_MODE)) { + resetMulticopterBrakingMode(); } } else { diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 16c00439ee..ac91fe79f7 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -584,6 +584,9 @@ int32_t getCruiseHeadingAdjustment(void); bool isAdjustingPosition(void); bool isAdjustingHeading(void); +float getEstimatedAglPosition(void); +bool isEstimatedAglTrusted(void); + /* Returns the heading recorded when home position was acquired. * Note that the navigation system uses deg*100 as unit and angles * are in the [0, 360 * 100) interval. diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c index 74c550fdd9..d74b8e34b6 100644 --- a/src/main/navigation/navigation_multicopter.c +++ b/src/main/navigation/navigation_multicopter.c @@ -533,7 +533,7 @@ static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxA const float setpointX = posControl.desiredState.vel.x; const float setpointY = posControl.desiredState.vel.y; - const float setpointXY = fast_fsqrtf(sq(setpointX) + sq(setpointY)); + const float setpointXY = calc_length_pythagorean_2D(setpointX, setpointY); // Calculate velocity error const float velErrorX = setpointX - measurementX; @@ -554,7 +554,7 @@ static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxA // Apply additional jerk limiting of 1700 cm/s^3 (~100 deg/s), almost any copter should be able to achieve this rate // This will assure that we wont't saturate out LEVEL and RATE PID controller - float maxAccelChange = US2S(deltaMicros) * 1700.0f; + float maxAccelChange = US2S(deltaMicros) * MC_POS_CONTROL_JERK_LIMIT_CMSSS; //When braking, raise jerk limit even if we are not boosting acceleration #ifdef USE_MR_BRAKING_MODE if (STATE(NAV_CRUISE_BRAKING)) { diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index 94eee78dd1..c351cfec4e 100755 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -815,6 +815,14 @@ bool isGPSGlitchDetected(void) } #endif +float getEstimatedAglPosition(void) { + return posEstimator.est.aglAlt; +} + +bool isEstimatedAglTrusted(void) { + return (posEstimator.est.aglQual == SURFACE_QUAL_HIGH) ? true : false; +} + /** * Initialize position estimator * Should be called once before any update occurs diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h index aa5fc76d1e..902bf283f9 100644 --- a/src/main/navigation/navigation_private.h +++ b/src/main/navigation/navigation_private.h @@ -37,6 +37,8 @@ #define INAV_SURFACE_MAX_DISTANCE 40 +#define MC_POS_CONTROL_JERK_LIMIT_CMSSS 1700.0f // jerk limit on horizontal acceleration (cm/s^3) + #define MC_LAND_CHECK_VEL_XY_MOVING 100.0f // cm/s #define MC_LAND_CHECK_VEL_Z_MOVING 25.0f // cm/s #define MC_LAND_THR_STABILISE_DELAY 1 // seconds diff --git a/src/main/programming/logic_condition.c b/src/main/programming/logic_condition.c index 6af6858904..a74837e316 100644 --- a/src/main/programming/logic_condition.c +++ b/src/main/programming/logic_condition.c @@ -42,6 +42,7 @@ #include "navigation/navigation.h" #include "sensors/battery.h" #include "sensors/pitotmeter.h" +#include "sensors/rangefinder.h" #include "flight/imu.h" #include "flight/pid.h" #include "drivers/io_port_expander.h" @@ -54,11 +55,12 @@ #include "io/vtx.h" #include "drivers/vtx_common.h" -PG_REGISTER_ARRAY_WITH_RESET_FN(logicCondition_t, MAX_LOGIC_CONDITIONS, logicConditions, PG_LOGIC_CONDITIONS, 2); +PG_REGISTER_ARRAY_WITH_RESET_FN(logicCondition_t, MAX_LOGIC_CONDITIONS, logicConditions, PG_LOGIC_CONDITIONS, 3); EXTENDED_FASTRAM uint64_t logicConditionsGlobalFlags; EXTENDED_FASTRAM int logicConditionValuesByType[LOGIC_CONDITION_LAST]; EXTENDED_FASTRAM rcChannelOverride_t rcChannelOverrides[MAX_SUPPORTED_RC_CHANNEL_COUNT]; +EXTENDED_FASTRAM flightAxisOverride_t flightAxisOverride[XYZ_AXIS_COUNT]; void pgResetFn_logicConditions(logicCondition_t *instance) { @@ -363,7 +365,42 @@ static int logicConditionCompute( LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_LOITER_RADIUS); return true; break; - + + case LOGIC_CONDITION_FLIGHT_AXIS_ANGLE_OVERRIDE: + if (operandA >= 0 && operandA <= 2) { + + flightAxisOverride[operandA].angleTargetActive = true; + int target = DEGREES_TO_DECIDEGREES(operandB); + if (operandA == 0) { + //ROLL + target = constrain(target, -pidProfile()->max_angle_inclination[FD_ROLL], pidProfile()->max_angle_inclination[FD_ROLL]); + } else if (operandA == 1) { + //PITCH + target = constrain(target, -pidProfile()->max_angle_inclination[FD_PITCH], pidProfile()->max_angle_inclination[FD_PITCH]); + } else if (operandA == 2) { + //YAW + target = (constrain(target, 0, 3600)); + } + flightAxisOverride[operandA].angleTarget = target; + LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_FLIGHT_AXIS); + + return true; + } else { + return false; + } + break; + + case LOGIC_CONDITION_FLIGHT_AXIS_RATE_OVERRIDE: + if (operandA >= 0 && operandA <= 2) { + flightAxisOverride[operandA].rateTargetActive = true; + flightAxisOverride[operandA].rateTarget = constrain(operandB, -2000, 2000); + LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_FLIGHT_AXIS); + return true; + } else { + return false; + } + break; + default: return false; break; @@ -576,6 +613,18 @@ static int logicConditionGetFlightOperandValue(int operand) { case LOGIC_CONDITION_OPERAND_FLIGHT_LOITER_RADIUS: return getLoiterRadius(navConfig()->fw.loiter_radius); + case LOGIC_CONDITION_OPERAND_FLIGHT_AGL_STATUS: + return isEstimatedAglTrusted(); + break; + + case LOGIC_CONDITION_OPERAND_FLIGHT_AGL: + return getEstimatedAglPosition(); + break; + + case LOGIC_CONDITION_OPERAND_FLIGHT_RANGEFINDER_RAW: + return rangefinderGetLatestRawAltitude(); + break; + default: return 0; break; @@ -710,6 +759,11 @@ void logicConditionUpdateTask(timeUs_t currentTimeUs) { rcChannelOverrides[i].active = false; } + for (uint8_t i = 0; i < XYZ_AXIS_COUNT; i++) { + flightAxisOverride[i].rateTargetActive = false; + flightAxisOverride[i].angleTargetActive = false; + } + for (uint8_t i = 0; i < MAX_LOGIC_CONDITIONS; i++) { logicConditionProcess(i); } @@ -776,3 +830,35 @@ uint32_t getLoiterRadius(uint32_t loiterRadius) { return loiterRadius; #endif } + +float getFlightAxisAngleOverride(uint8_t axis, float angle) { + if (flightAxisOverride[axis].angleTargetActive) { + return flightAxisOverride[axis].angleTarget; + } else { + return angle; + } +} + +float getFlightAxisRateOverride(uint8_t axis, float rate) { + if (flightAxisOverride[axis].rateTargetActive) { + return flightAxisOverride[axis].rateTarget; + } else { + return rate; + } +} + +bool isFlightAxisAngleOverrideActive(uint8_t axis) { + if (flightAxisOverride[axis].angleTargetActive) { + return true; + } else { + return false; + } +} + +bool isFlightAxisRateOverrideActive(uint8_t axis) { + if (flightAxisOverride[axis].rateTargetActive) { + return true; + } else { + return false; + } +} \ No newline at end of file diff --git a/src/main/programming/logic_condition.h b/src/main/programming/logic_condition.h index 69f3d9c7d9..54ef2e3025 100644 --- a/src/main/programming/logic_condition.h +++ b/src/main/programming/logic_condition.h @@ -26,7 +26,7 @@ #include "config/parameter_group.h" #include "common/time.h" -#define MAX_LOGIC_CONDITIONS 32 +#define MAX_LOGIC_CONDITIONS 64 typedef enum { LOGIC_CONDITION_TRUE = 0, @@ -74,7 +74,9 @@ typedef enum { LOGIC_CONDITION_SET_PROFILE = 42, LOGIC_CONDITION_MIN = 43, LOGIC_CONDITION_MAX = 44, - LOGIC_CONDITION_LAST = 45, + LOGIC_CONDITION_FLIGHT_AXIS_ANGLE_OVERRIDE = 45, + LOGIC_CONDITION_FLIGHT_AXIS_RATE_OVERRIDE = 46, + LOGIC_CONDITION_LAST = 47, } logicOperation_e; typedef enum logicOperandType_s { @@ -127,7 +129,9 @@ typedef enum { LOGIC_CONDITION_OPERAND_FLIGHT_LOITER_RADIUS, // 35 LOGIC_CONDITION_OPERAND_FLIGHT_ACTIVE_PROFILE, //int // 36 LOGIC_CONDITION_OPERAND_FLIGHT_BATT_CELLS, // 37 - + LOGIC_CONDITION_OPERAND_FLIGHT_AGL_STATUS, //0,1,2 // 38 + LOGIC_CONDITION_OPERAND_FLIGHT_AGL, //0,1,2 // 39 + LOGIC_CONDITION_OPERAND_FLIGHT_RANGEFINDER_RAW, //int // 40 } logicFlightOperands_e; typedef enum { @@ -156,6 +160,7 @@ typedef enum { LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_OSD_LAYOUT = (1 << 7), LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_RC_CHANNEL = (1 << 8), LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_LOITER_RADIUS = (1 << 9), + LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_FLIGHT_AXIS = (1 << 10), } logicConditionsGlobalFlags_t; typedef enum { @@ -188,6 +193,13 @@ typedef struct rcChannelOverride_s { int value; } rcChannelOverride_t; +typedef struct flightAxisOverride_s { + uint8_t rateTargetActive; + uint8_t angleTargetActive; + int angleTarget; + int rateTarget; +} flightAxisOverride_t; + extern int logicConditionValuesByType[LOGIC_CONDITION_LAST]; extern uint64_t logicConditionsGlobalFlags; @@ -207,3 +219,7 @@ float getThrottleScale(float globalThrottleScale); int16_t getRcCommandOverride(int16_t command[], uint8_t axis); int16_t getRcChannelOverride(uint8_t channel, int16_t originalValue); uint32_t getLoiterRadius(uint32_t loiterRadius); +float getFlightAxisAngleOverride(uint8_t axis, float angle); +float getFlightAxisRateOverride(uint8_t axis, float rate); +bool isFlightAxisAngleOverrideActive(uint8_t axis); +bool isFlightAxisRateOverrideActive(uint8_t axis); diff --git a/src/main/scheduler/scheduler.h b/src/main/scheduler/scheduler.h index 4efa96e17d..98aadbcb44 100755 --- a/src/main/scheduler/scheduler.h +++ b/src/main/scheduler/scheduler.h @@ -87,7 +87,7 @@ typedef enum { #ifdef USE_LED_STRIP TASK_LEDSTRIP, #endif -#if defined(USE_PWM_SERVO_DRIVER) || defined(USE_SERVO_SBUS) +#if defined(USE_SERVO_SBUS) TASK_PWMDRIVER, #endif #ifdef STACK_CHECK diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c index 99360c2475..68bca9d6a4 100644 --- a/src/main/sensors/acceleration.c +++ b/src/main/sensors/acceleration.c @@ -84,12 +84,11 @@ static EXTENDED_FASTRAM pt1Filter_t accVibeFilter[XYZ_AXIS_COUNT]; static EXTENDED_FASTRAM filterApplyFnPtr accNotchFilterApplyFn; static EXTENDED_FASTRAM void *accNotchFilter[XYZ_AXIS_COUNT]; -PG_REGISTER_WITH_RESET_FN(accelerometerConfig_t, accelerometerConfig, PG_ACCELEROMETER_CONFIG, 4); +PG_REGISTER_WITH_RESET_FN(accelerometerConfig_t, accelerometerConfig, PG_ACCELEROMETER_CONFIG, 5); void pgResetFn_accelerometerConfig(accelerometerConfig_t *instance) { RESET_CONFIG_2(accelerometerConfig_t, instance, - .acc_align = SETTING_ALIGN_ACC_DEFAULT, .acc_hardware = SETTING_ACC_HARDWARE_DEFAULT, .acc_lpf_hz = SETTING_ACC_LPF_HZ_DEFAULT, .acc_notch_hz = SETTING_ACC_NOTCH_HZ_DEFAULT, @@ -291,11 +290,6 @@ bool accInit(uint32_t targetLooptime) acc.extremes[axis].max = -100; } - // At this poinrt acc.dev.accAlign was set up by the driver from the busDev record - // If configuration says different - override - if (accelerometerConfig()->acc_align != ALIGN_DEFAULT) { - acc.dev.accAlign = accelerometerConfig()->acc_align; - } return true; } diff --git a/src/main/sensors/acceleration.h b/src/main/sensors/acceleration.h index 76f3a6ea5c..88893a0ac7 100644 --- a/src/main/sensors/acceleration.h +++ b/src/main/sensors/acceleration.h @@ -67,7 +67,6 @@ typedef struct acc_s { extern acc_t acc; typedef struct accelerometerConfig_s { - sensor_align_e acc_align; // acc alignment uint8_t acc_hardware; // Which acc hardware to use on boards with more than one device uint16_t acc_lpf_hz; // cutoff frequency for the low pass filter used on the acc z-axis for althold in Hz flightDynamicsTrims_t accZero; // Accelerometer offset diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 2a88a4cadf..0847b0cec5 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -97,13 +97,12 @@ EXTENDED_FASTRAM dynamicGyroNotchState_t dynamicGyroNotchState; #endif -PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 3); +PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 4); PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig, .gyro_lpf = SETTING_GYRO_HARDWARE_LPF_DEFAULT, .gyro_anti_aliasing_lpf_hz = SETTING_GYRO_ANTI_ALIASING_LPF_HZ_DEFAULT, .gyro_anti_aliasing_lpf_type = SETTING_GYRO_ANTI_ALIASING_LPF_TYPE_DEFAULT, - .gyro_align = SETTING_ALIGN_GYRO_DEFAULT, .gyroMovementCalibrationThreshold = SETTING_MORON_THRESHOLD_DEFAULT, .looptime = SETTING_LOOPTIME_DEFAULT, #ifdef USE_DUAL_GYRO @@ -304,13 +303,7 @@ bool gyroInit(void) // initFn will initialize sampleRateIntervalUs to actual gyro sampling rate (if driver supports it). Calculate target looptime using that value gyro.targetLooptime = gyroDev[0].sampleRateIntervalUs; - - // At this poinrt gyroDev[0].gyroAlign was set up by the driver from the busDev record - // If configuration says different - override - if (gyroConfig()->gyro_align != ALIGN_DEFAULT) { - gyroDev[0].gyroAlign = gyroConfig()->gyro_align; - } - + gyroInitFilters(); #ifdef USE_DYNAMIC_FILTERS diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 3bb99772cc..88e15db8c8 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -53,7 +53,6 @@ typedef struct gyro_s { extern gyro_t gyro; typedef struct gyroConfig_s { - sensor_align_e gyro_align; // gyro alignment uint8_t gyroMovementCalibrationThreshold; // people keep forgetting that moving model while init results in wrong gyro offsets. and then they never reset gyro. so this is now on by default. uint16_t looptime; // imu loop time in us uint8_t gyro_lpf; // gyro LPF setting - values are driver specific, in case of invalid number, a reasonable default ~30-40HZ is chosen. diff --git a/src/main/sensors/pitotmeter.c b/src/main/sensors/pitotmeter.c index 332f98e02e..c50449c59a 100644 --- a/src/main/sensors/pitotmeter.c +++ b/src/main/sensors/pitotmeter.c @@ -167,7 +167,7 @@ bool pitotIsCalibrationComplete(void) void pitotStartCalibration(void) { - zeroCalibrationStartS(&pitot.zeroCalibration, CALIBRATING_PITOT_TIME_MS, P0 * 0.00001f, false); + zeroCalibrationStartS(&pitot.zeroCalibration, CALIBRATING_PITOT_TIME_MS, SSL_AIR_PRESSURE * 0.00001f, false); } static void performPitotCalibrationCycle(void) @@ -223,7 +223,7 @@ STATIC_PROTOTHREAD(pitotThread) // // Therefore we shouldn't care about CAS/TAS and only calculate IAS since it's more indicative to the pilot and more useful in calculations // It also allows us to use pitot_scale to calibrate the dynamic pressure sensor scale - pitot.airSpeed = pitotmeterConfig()->pitot_scale * fast_fsqrtf(2.0f * fabsf(pitot.pressure - pitot.pressureZero) / AIR_DENSITY_SEA_LEVEL_15C) * 100; + pitot.airSpeed = pitotmeterConfig()->pitot_scale * fast_fsqrtf(2.0f * fabsf(pitot.pressure - pitot.pressureZero) / SSL_AIR_DENSITY) * 100; } else { performPitotCalibrationCycle(); pitot.airSpeed = 0; diff --git a/src/main/sensors/pitotmeter.h b/src/main/sensors/pitotmeter.h index b7fd3c4ca7..fffc5a9f1b 100644 --- a/src/main/sensors/pitotmeter.h +++ b/src/main/sensors/pitotmeter.h @@ -59,9 +59,6 @@ typedef struct pito_s { #ifdef USE_PITOT -#define AIR_DENSITY_SEA_LEVEL_15C 1.225f // Air density at sea level and 15 degrees Celsius -#define P0 101325.0f // standard pressure [Pa] - extern pitot_t pitot; bool pitotInit(void); diff --git a/src/main/target/AIKONF4/CMakeLists.txt b/src/main/target/AIKONF4/CMakeLists.txt index 4f415fd1af..8a1cfd7a69 100644 --- a/src/main/target/AIKONF4/CMakeLists.txt +++ b/src/main/target/AIKONF4/CMakeLists.txt @@ -1 +1 @@ -target_stm32f405xg(AIKONF4) +target_stm32f405xg(AIKONF4 SKIP_RELEASES) diff --git a/src/main/target/AIKONF4/target.h b/src/main/target/AIKONF4/target.h index 28ee33cf22..33310b3fc6 100644 --- a/src/main/target/AIKONF4/target.h +++ b/src/main/target/AIKONF4/target.h @@ -102,7 +102,6 @@ #define RANGEFINDER_I2C_BUS DEFAULT_I2C_BUS #define TEMPERATURE_I2C_BUS DEFAULT_I2C_BUS #define PITOT_I2C_BUS DEFAULT_I2C_BUS -#define PCA9685_I2C_BUS DEFAULT_I2C_BUS #define BNO055_I2C_BUS DEFAULT_I2C_BUS #define DEFAULT_RX_TYPE RX_TYPE_SERIAL diff --git a/src/main/target/AIRBOTF4/target.h b/src/main/target/AIRBOTF4/target.h index 4dbd8476c8..81c525c82f 100644 --- a/src/main/target/AIRBOTF4/target.h +++ b/src/main/target/AIRBOTF4/target.h @@ -144,7 +144,5 @@ #define TARGET_IO_PORTC 0xffff #define TARGET_IO_PORTD 0xffff -#define PCA9685_I2C_BUS BUS_I2C2 - #define USE_DSHOT #define USE_ESC_SENSOR \ No newline at end of file diff --git a/src/main/target/AIRBOTF7/CMakeLists.txt b/src/main/target/AIRBOTF7/CMakeLists.txt index 1915ffc91c..3d0e5bf1b8 100644 --- a/src/main/target/AIRBOTF7/CMakeLists.txt +++ b/src/main/target/AIRBOTF7/CMakeLists.txt @@ -1,2 +1,2 @@ -target_stm32f722xe(AIRBOTF7) -target_stm32f722xe(OMNIBUSF7NANOV7) +target_stm32f722xe(AIRBOTF7 SKIP_RELEASES) +target_stm32f722xe(OMNIBUSF7NANOV7 SKIP_RELEASES) diff --git a/src/main/target/ALIENFLIGHTF3/target.h b/src/main/target/ALIENFLIGHTF3/target.h index 7b5349533c..c5c76a45aa 100644 --- a/src/main/target/ALIENFLIGHTF3/target.h +++ b/src/main/target/ALIENFLIGHTF3/target.h @@ -131,5 +131,3 @@ #define TARGET_IO_PORTC 0xffff #define TARGET_IO_PORTD (BIT(2)) #define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)) - -#define PCA9685_I2C_BUS BUS_I2C2 \ No newline at end of file diff --git a/src/main/target/ALIENFLIGHTNGF7/CMakeLists.txt b/src/main/target/ALIENFLIGHTNGF7/CMakeLists.txt index 16288f4b84..ae68fc6fc9 100644 --- a/src/main/target/ALIENFLIGHTNGF7/CMakeLists.txt +++ b/src/main/target/ALIENFLIGHTNGF7/CMakeLists.txt @@ -1 +1 @@ -target_stm32f722xe(ALIENFLIGHTNGF7) +target_stm32f722xe(ALIENFLIGHTNGF7 SKIP_RELEASES) diff --git a/src/main/target/ANYFC/CMakeLists.txt b/src/main/target/ANYFC/CMakeLists.txt index 4a04e0f69b..82d68535fa 100644 --- a/src/main/target/ANYFC/CMakeLists.txt +++ b/src/main/target/ANYFC/CMakeLists.txt @@ -1 +1 @@ -target_stm32f405xg(ANYFC) +target_stm32f405xg(ANYFC SKIP_RELEASES) \ No newline at end of file diff --git a/src/main/target/ANYFC/target.h b/src/main/target/ANYFC/target.h index dbc23b09d7..765fcbf0b6 100644 --- a/src/main/target/ANYFC/target.h +++ b/src/main/target/ANYFC/target.h @@ -127,5 +127,3 @@ #define TARGET_IO_PORTB 0xffff #define TARGET_IO_PORTC 0xffff #define TARGET_IO_PORTD 0xffff - -#define PCA9685_I2C_BUS BUS_I2C2 diff --git a/src/main/target/ANYFCF7/target.h b/src/main/target/ANYFCF7/target.h index 6a75ae5624..4c0a8b25ca 100644 --- a/src/main/target/ANYFCF7/target.h +++ b/src/main/target/ANYFCF7/target.h @@ -168,5 +168,3 @@ #define TARGET_IO_PORTC 0xffff #define TARGET_IO_PORTD 0xffff #define TARGET_IO_PORTE 0xffff - -#define PCA9685_I2C_BUS BUS_I2C2 diff --git a/src/main/target/ANYFCM7/target.h b/src/main/target/ANYFCM7/target.h index 2ba9486297..1cbf321885 100644 --- a/src/main/target/ANYFCM7/target.h +++ b/src/main/target/ANYFCM7/target.h @@ -135,5 +135,3 @@ #define TARGET_IO_PORTC 0xffff #define TARGET_IO_PORTD 0xffff #define TARGET_IO_PORTE 0xffff - -#define PCA9685_I2C_BUS BUS_I2C2 diff --git a/src/main/target/AOCODARCF7DUAL/CMakeLists.txt b/src/main/target/AOCODARCF7DUAL/CMakeLists.txt new file mode 100644 index 0000000000..ca0b580d53 --- /dev/null +++ b/src/main/target/AOCODARCF7DUAL/CMakeLists.txt @@ -0,0 +1 @@ +target_stm32f722xe(AOCODARCF7DUAL) diff --git a/src/main/target/AOCODARCF7DUAL/target.c b/src/main/target/AOCODARCF7DUAL/target.c new file mode 100644 index 0000000000..d4c4143bec --- /dev/null +++ b/src/main/target/AOCODARCF7DUAL/target.c @@ -0,0 +1,50 @@ +/* + * This file is part of INAV. + * + * INAV 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. + * + * INAV 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 INAV. If not, see . + */ + +#include + +#include "platform.h" + +#include "drivers/bus.h" +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" +#include "drivers/timer.h" +#include "drivers/pinio.h" +#include "drivers/sensor.h" + +BUSDEV_REGISTER_SPI_TAG(busdev_mpu6000, DEVHW_MPU6000, MPU6000_SPI_BUS, MPU6000_CS_PIN, MPU6000_EXTI_PIN, 0, DEVFLAGS_NONE, IMU_MPU6000_ALIGN); +BUSDEV_REGISTER_SPI_TAG(busdev_mpu6500, DEVHW_MPU6500, MPU6000_SPI_BUS, MPU6500_CS_PIN, MPU6500_EXTI_PIN, 0, DEVFLAGS_NONE, IMU_MPU6500_ALIGN); +BUSDEV_REGISTER_SPI_TAG(busdev_bmi270, DEVHW_BMI270, BMI270_SPI_BUS, BMI270_CS_PIN, BMI270_EXTI_PIN, 1, DEVFLAGS_NONE, IMU_BMI270_ALIGN); + + +timerHardware_t timerHardware[] = { + DEF_TIM(TIM1, CH3, PA10, TIM_USE_PPM, 0, 0), //PPM + + DEF_TIM(TIM8, CH1, PC6, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 1), // S1 D(2, 2, 7) + DEF_TIM(TIM8, CH2, PC7, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 1), // S2 D(2, 3, 7) + DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 1), // S3 D(2, 4, 7) + DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S4 D(2, 7, 7) + + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR |TIM_USE_FW_SERVO, 0, 0), // S5 D(1, 7, 5) + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR |TIM_USE_FW_SERVO, 0, 0), // S6 D(1, 2, 5) + DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_MOTOR |TIM_USE_FW_SERVO, 0, 0), // S7 D(1, 0, 2) + DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR |TIM_USE_FW_SERVO, 0, 0), // S8 D(1, 3, 2) + + DEF_TIM(TIM2, CH1, PA0, TIM_USE_LED, 0, 0), // LED_TRIP +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/AOCODARCF7DUAL/target.h b/src/main/target/AOCODARCF7DUAL/target.h new file mode 100644 index 0000000000..25d618c5a4 --- /dev/null +++ b/src/main/target/AOCODARCF7DUAL/target.h @@ -0,0 +1,182 @@ +/* + * This file is part of INAV. + * + * INAV 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. + * + * INAV 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 INAV. If not, see . + */ + + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "AO7D" +#define USBD_PRODUCT_STRING "AocodaRCF7Dual" + + +#define LED0 PC14 //Blue + +#define BEEPER PC13 +#define BEEPER_INVERTED + +// *************** SPI1 Gyro & ACC ******************* +#define USE_SPI +#define USE_SPI_DEVICE_2 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 + +#define USE_EXTI +#define USE_DUAL_GYRO +#define USE_MPU_DATA_READY_SIGNAL +#define ENSURE_MPU_DATA_READY_IS_LOW +#define USE_TARGET_IMU_HARDWARE_DESCRIPTORS + +#define USE_IMU_MPU6000 +#define IMU_MPU6000_ALIGN CW270_DEG +#define MPU6000_CS_PIN PB12 +#define MPU6000_SPI_BUS BUS_SPI2 +#define MPU6000_EXTI_PIN PC4 + +#define USE_IMU_MPU6500 +#define IMU_MPU6500_ALIGN CW270_DEG +#define MPU6500_CS_PIN PB12 +#define MPU6500_SPI_BUS BUS_SPI2 +#define MPU6500_EXTI_PIN PC4 + +#define USE_IMU_BMI270 +#define IMU_BMI270_ALIGN CW180_DEG +#define BMI270_CS_PIN PA13 +#define BMI270_SPI_BUS BUS_SPI2 +#define BMI270_EXTI_PIN PA8 + +// *************** I2C /Baro/Mag ********************* +#define USE_I2C +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB8 +#define I2C1_SDA PB9 + +#define USE_BARO +#define BARO_I2C_BUS BUS_I2C1 +#define USE_BARO_BMP280 +#define USE_BARO_MS5611 +#define USE_BARO_MS5607 +#define USE_BARO_DPS310 + +#define USE_I2C_DEVICE_2 +#define I2C2_SCL PB10 +#define I2C2_SDA PB11 + +#define USE_MAG +#define MAG_I2C_BUS BUS_I2C2 +#define USE_MAG_AK8975 +#define USE_MAG_HMC5883 +#define USE_MAG_QMC5883 +#define USE_MAG_IST8310 +#define USE_MAG_IST8308 +#define USE_MAG_MAG3110 +#define USE_MAG_LIS3MDL + +#define TEMPERATURE_I2C_BUS BUS_I2C2 + +#define PITOT_I2C_BUS BUS_I2C2 + +#define USE_RANGEFINDER +#define RANGEFINDER_I2C_BUS BUS_I2C2 + +// *************** SPI2 OSD *********************** +#define USE_SPI_DEVICE_1 +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 + +#define USE_OSD +#define USE_MAX7456 +#define MAX7456_SPI_BUS BUS_SPI1 +#define MAX7456_CS_PIN PA4 + +// *************** SPI3 FLASH BLACKBOX******************* +#define USE_SPI_DEVICE_3 +#define SPI3_SCK_PIN PB3 +#define SPI3_MISO_PIN PB4 +#define SPI3_MOSI_PIN PB5 + + +#define USE_FLASHFS +#define USE_FLASH_M25P16 +#define M25P16_SPI_BUS BUS_SPI3 +#define M25P16_CS_PIN PC0 +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT + + +// *************** UART ***************************** +#define USE_VCP +#define USB_DETECT_PIN PC15 +#define USE_USB_DETECT + +#define USE_UART1 +#define UART1_TX_PIN PA9 +#define UART1_RX_PIN PA10 + +#define USE_UART2 +#define UART2_TX_PIN PA2 +#define UART2_RX_PIN PA3 + +#define USE_UART3 +#define UART3_TX_PIN PC10 +#define UART3_RX_PIN PC11 + +#define USE_UART4 +#define UART4_TX_PIN NONE +#define UART4_RX_PIN PA1 + +#define USE_UART5 +#define UART5_TX_PIN PC12 +#define UART5_RX_PIN PD2 + +#define SERIAL_PORT_COUNT 6 + +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_SBUS +#define SERIALRX_UART SERIAL_PORT_USART1 + +// *************** ADC ***************************** +#define USE_ADC +#define ADC_INSTANCE ADC1 +#define ADC1_DMA_STREAM DMA2_Stream0 + +#define ADC_CHANNEL_1_PIN PC1 +#define ADC_CHANNEL_2_PIN PC2 +#define ADC_CHANNEL_3_PIN PC3 + + +#define VBAT_ADC_CHANNEL ADC_CHN_2 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_1 +#define RSSI_ADC_CHANNEL ADC_CHN_3 + +// *************** LEDSTRIP ************************ +#define USE_LED_STRIP +#define WS2811_PIN PA0 + +#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_TELEMETRY | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TX_PROF_SEL | FEATURE_BLACKBOX) +#define CURRENT_METER_SCALE 650 + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD 0xffff + +#define MAX_PWM_OUTPUT_PORTS 10 +#define USE_DSHOT +#define USE_SERIALSHOT +#define USE_ESC_SENSOR diff --git a/src/main/target/ASGARD32F4/CMakeLists.txt b/src/main/target/ASGARD32F4/CMakeLists.txt index 4a2d3bbed2..7f0487e89d 100644 --- a/src/main/target/ASGARD32F4/CMakeLists.txt +++ b/src/main/target/ASGARD32F4/CMakeLists.txt @@ -1 +1 @@ -target_stm32f405xg(ASGARD32F4) +target_stm32f405xg(ASGARD32F4 SKIP_RELEASES) diff --git a/src/main/target/ASGARD32F4/target.h b/src/main/target/ASGARD32F4/target.h index ad671e8065..53631c9169 100644 --- a/src/main/target/ASGARD32F4/target.h +++ b/src/main/target/ASGARD32F4/target.h @@ -161,6 +161,5 @@ #define TARGET_IO_PORTD 0xffff #define PITOT_I2C_BUS BUS_I2C2 -#define PCA9685_I2C_BUS BUS_I2C2 #define TEMPERATURE_I2C_BUS BUS_I2C2 #define BNO055_I2C_BUS BUS_I2C2 diff --git a/src/main/target/ASGARD32F7/CMakeLists.txt b/src/main/target/ASGARD32F7/CMakeLists.txt index 5e7f889d6b..c589aff5cf 100644 --- a/src/main/target/ASGARD32F7/CMakeLists.txt +++ b/src/main/target/ASGARD32F7/CMakeLists.txt @@ -1 +1 @@ -target_stm32f722xe(ASGARD32F7) +target_stm32f722xe(ASGARD32F7 SKIP_RELEASES) \ No newline at end of file diff --git a/src/main/target/ASGARD32F7/target.h b/src/main/target/ASGARD32F7/target.h index 0ac92994f9..7e2841c3e9 100644 --- a/src/main/target/ASGARD32F7/target.h +++ b/src/main/target/ASGARD32F7/target.h @@ -165,6 +165,5 @@ #define TARGET_IO_PORTC 0xffff #define TARGET_IO_PORTD 0xffff -#define PCA9685_I2C_BUS BUS_I2C2 #define PITOT_I2C_BUS BUS_I2C2 #define BNO055_I2C_BUS BUS_I2C2 diff --git a/src/main/target/BETAFLIGHTF4/target.h b/src/main/target/BETAFLIGHTF4/target.h index e9b44bb480..89e6db9d51 100755 --- a/src/main/target/BETAFLIGHTF4/target.h +++ b/src/main/target/BETAFLIGHTF4/target.h @@ -171,7 +171,5 @@ #define MAX_PWM_OUTPUT_PORTS 4 -#define PCA9685_I2C_BUS BUS_I2C2 - #define USE_DSHOT #define USE_ESC_SENSOR diff --git a/src/main/target/BLUEJAYF4/CMakeLists.txt b/src/main/target/BLUEJAYF4/CMakeLists.txt index 2c93878cff..dd197a14d0 100644 --- a/src/main/target/BLUEJAYF4/CMakeLists.txt +++ b/src/main/target/BLUEJAYF4/CMakeLists.txt @@ -1 +1 @@ -target_stm32f405xg(BLUEJAYF4) +target_stm32f405xg(BLUEJAYF4 SKIP_RELEASES) diff --git a/src/main/target/BLUEJAYF4/config.c b/src/main/target/BLUEJAYF4/config.c index bde1ed0396..a66b6d443e 100644 --- a/src/main/target/BLUEJAYF4/config.c +++ b/src/main/target/BLUEJAYF4/config.c @@ -27,8 +27,4 @@ // alternative defaults settings for BlueJayF4 targets void targetConfiguration(void) { - if (hardwareRevision == BJF4_REV1 || hardwareRevision == BJF4_REV2) { - gyroConfigMutable()->gyro_align = CW180_DEG; - accelerometerConfigMutable()->acc_align = CW180_DEG; - } } diff --git a/src/main/target/BLUEJAYF4/hardware_revision.c b/src/main/target/BLUEJAYF4/hardware_revision.c index adfb5a81a5..c2cf36c248 100644 --- a/src/main/target/BLUEJAYF4/hardware_revision.c +++ b/src/main/target/BLUEJAYF4/hardware_revision.c @@ -29,16 +29,6 @@ #include "drivers/flash_m25p16.h" #include "hardware_revision.h" -#if 0 -static const char * const hardwareRevisionNames[] = { - "Unknown", - "BlueJay rev1", - "BlueJay rev2", - "BlueJay rev3", - "BlueJay rev3a" -}; -#endif - uint8_t hardwareRevision = UNKNOWN; void detectHardwareRevision(void) diff --git a/src/main/target/CLRACINGF4AIR/CMakeLists.txt b/src/main/target/CLRACINGF4AIR/CMakeLists.txt index 3b4ceb65ad..26cc0addb4 100644 --- a/src/main/target/CLRACINGF4AIR/CMakeLists.txt +++ b/src/main/target/CLRACINGF4AIR/CMakeLists.txt @@ -1,3 +1,3 @@ -target_stm32f405xg(CLRACINGF4AIR) -target_stm32f405xg(CLRACINGF4AIRV2) -target_stm32f405xg(CLRACINGF4AIRV3) +target_stm32f405xg(CLRACINGF4AIR SKIP_RELEASES) +target_stm32f405xg(CLRACINGF4AIRV2 SKIP_RELEASES) +target_stm32f405xg(CLRACINGF4AIRV3 SKIP_RELEASES) diff --git a/src/main/target/COLIBRI/CMakeLists.txt b/src/main/target/COLIBRI/CMakeLists.txt index e8568b8281..ad8fd0d3f7 100644 --- a/src/main/target/COLIBRI/CMakeLists.txt +++ b/src/main/target/COLIBRI/CMakeLists.txt @@ -1,2 +1,2 @@ -target_stm32f405xg(COLIBRI HSE_MHZ 16) -target_stm32f405xg(QUANTON HSE_MHZ 16) +target_stm32f405xg(COLIBRI HSE_MHZ 16 SKIP_RELEASES) +target_stm32f405xg(QUANTON HSE_MHZ 16 SKIP_RELEASES) diff --git a/src/main/target/COLIBRI_RACE/target.h b/src/main/target/COLIBRI_RACE/target.h index 8347b58cd3..205f68f2e5 100755 --- a/src/main/target/COLIBRI_RACE/target.h +++ b/src/main/target/COLIBRI_RACE/target.h @@ -124,5 +124,4 @@ #define TARGET_IO_PORTD (BIT(2)) #define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)) -#define PCA9685_I2C_BUS BUS_I2C2 #define BNO055_I2C_BUS BUS_I2C2 \ No newline at end of file diff --git a/src/main/target/DALRCF722DUAL/target.c b/src/main/target/DALRCF722DUAL/target.c index 3536cba0c8..8ea225e412 100644 --- a/src/main/target/DALRCF722DUAL/target.c +++ b/src/main/target/DALRCF722DUAL/target.c @@ -30,12 +30,12 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM4, CH2, PB7, TIM_USE_PPM ,0, 0), - DEF_TIM(TIM8, CH1, PC6, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR,0,0), //S1 DMA2_ST2 T8CH1 + DEF_TIM(TIM8, CH1, PC6, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO,0,0), //S1 DMA2_ST2 T8CH1 DEF_TIM(TIM8, CH2, PC7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO,0,0), //S2 DMA2_ST3 T8CH2 DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO,0,0), //S3 DMA2_ST4 T8CH3 DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO,0,0), //S4 DMA2_ST7 T8CH4 DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR,0,0), //S5 DMA2_ST1 T1CH1 - DEF_TIM(TIM1, CH2, PA9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO,0,0), //S6 DMA2_ST6 T1CH2 + DEF_TIM(TIM1, CH2, PA9, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR,0,0), //S6 DMA2_ST6 T1CH2 DEF_TIM(TIM2, CH1, PA15,TIM_USE_LED,0,0), //2812 STRIP DMA1_ST5 }; diff --git a/src/main/target/F4BY/CMakeLists.txt b/src/main/target/F4BY/CMakeLists.txt index 09b695702b..809c93b482 100644 --- a/src/main/target/F4BY/CMakeLists.txt +++ b/src/main/target/F4BY/CMakeLists.txt @@ -1 +1 @@ -target_stm32f405xg(F4BY) +target_stm32f405xg(F4BY SKIP_RELEASES) diff --git a/src/main/target/F4BY/target.h b/src/main/target/F4BY/target.h index 03fc7c6749..dff346355a 100644 --- a/src/main/target/F4BY/target.h +++ b/src/main/target/F4BY/target.h @@ -148,5 +148,3 @@ #define TARGET_IO_PORTC 0xffff #define TARGET_IO_PORTD 0xffff #define TARGET_IO_PORTE 0xffff - -#define PCA9685_I2C_BUS BUS_I2C2 diff --git a/src/main/target/FALCORE/target.h b/src/main/target/FALCORE/target.h index 094adb7631..fe35c1b85f 100755 --- a/src/main/target/FALCORE/target.h +++ b/src/main/target/FALCORE/target.h @@ -130,5 +130,4 @@ #define TARGET_IO_PORTD 0xFFFF #define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)) -#define PCA9685_I2C_BUS BUS_I2C2 #define BNO055_I2C_BUS BUS_I2C2 \ No newline at end of file diff --git a/src/main/target/FF_FORTINIF4/CMakeLists.txt b/src/main/target/FF_FORTINIF4/CMakeLists.txt index a78703f69c..3bf60c46d8 100644 --- a/src/main/target/FF_FORTINIF4/CMakeLists.txt +++ b/src/main/target/FF_FORTINIF4/CMakeLists.txt @@ -1 +1 @@ -target_stm32f405xg(FF_FORTINIF4) +target_stm32f405xg(FF_FORTINIF4 SKIP_RELEASES) diff --git a/src/main/target/FF_PIKOF4/CMakeLists.txt b/src/main/target/FF_PIKOF4/CMakeLists.txt index 9a1c349de1..3e98eb2e9a 100644 --- a/src/main/target/FF_PIKOF4/CMakeLists.txt +++ b/src/main/target/FF_PIKOF4/CMakeLists.txt @@ -1,2 +1,2 @@ -target_stm32f405xg(FF_PIKOF4) -target_stm32f405xg(FF_PIKOF4OSD) +target_stm32f405xg(FF_PIKOF4 SKIP_RELEASES) +target_stm32f405xg(FF_PIKOF4OSD SKIP_RELEASES) diff --git a/src/main/target/FIREWORKSV2/target.h b/src/main/target/FIREWORKSV2/target.h index ec20e387ec..faffddf32b 100644 --- a/src/main/target/FIREWORKSV2/target.h +++ b/src/main/target/FIREWORKSV2/target.h @@ -228,9 +228,7 @@ #define TARGET_IO_PORTD 0xffff #if defined(OMNIBUSF4V6) -#define PCA9685_I2C_BUS BUS_I2C1 #define BNO055_I2C_BUS BUS_I2C1 #else -#define PCA9685_I2C_BUS BUS_I2C2 #define BNO055_I2C_BUS BUS_I2C2 #endif diff --git a/src/main/target/FRSKYF4/CMakeLists.txt b/src/main/target/FRSKYF4/CMakeLists.txt index ef64ff2ded..cef488494b 100644 --- a/src/main/target/FRSKYF4/CMakeLists.txt +++ b/src/main/target/FRSKYF4/CMakeLists.txt @@ -1 +1 @@ -target_stm32f405xg(FRSKYF4) +target_stm32f405xg(FRSKYF4 SKIP_RELEASES) diff --git a/src/main/target/FRSKY_ROVERF7/CMakeLists.txt b/src/main/target/FRSKY_ROVERF7/CMakeLists.txt index e220b6642f..7fb4ef13ce 100644 --- a/src/main/target/FRSKY_ROVERF7/CMakeLists.txt +++ b/src/main/target/FRSKY_ROVERF7/CMakeLists.txt @@ -1 +1 @@ -target_stm32f722xe(FRSKY_ROVERF7) +target_stm32f722xe(FRSKY_ROVERF7 SKIP_RELEASES) diff --git a/src/main/target/FURYF4OSD/target.h b/src/main/target/FURYF4OSD/target.h index 9e8283dfff..8eeed9c853 100644 --- a/src/main/target/FURYF4OSD/target.h +++ b/src/main/target/FURYF4OSD/target.h @@ -179,5 +179,3 @@ #define USE_ESC_SENSOR #define MAX_PWM_OUTPUT_PORTS 6 - -#define PCA9685_I2C_BUS DEFAULT_I2C_BUS diff --git a/src/main/target/IFLIGHTF4_TWING/CMakeLists.txt b/src/main/target/IFLIGHTF4_TWING/CMakeLists.txt index a7640915f3..2db541cd9f 100644 --- a/src/main/target/IFLIGHTF4_TWING/CMakeLists.txt +++ b/src/main/target/IFLIGHTF4_TWING/CMakeLists.txt @@ -1 +1 @@ -target_stm32f405xg(IFLIGHTF4_TWING) +target_stm32f405xg(IFLIGHTF4_TWING SKIP_RELEASES) diff --git a/src/main/target/IFLIGHTF4_TWING/target.h b/src/main/target/IFLIGHTF4_TWING/target.h index 5a3943e11a..8d52534d36 100644 --- a/src/main/target/IFLIGHTF4_TWING/target.h +++ b/src/main/target/IFLIGHTF4_TWING/target.h @@ -78,7 +78,6 @@ #define USE_RANGEFINDER #define RANGEFINDER_I2C_BUS BUS_I2C1 -#define PCA9685_I2C_BUS BUS_I2C1 #define BNO055_I2C_BUS BUS_I2C1 // *************** OSD ***************************** diff --git a/src/main/target/IFLIGHT_H743_AIO_V2/CMakeLists.txt b/src/main/target/IFLIGHT_H743_AIO_V2/CMakeLists.txt new file mode 100644 index 0000000000..66f1731aa9 --- /dev/null +++ b/src/main/target/IFLIGHT_H743_AIO_V2/CMakeLists.txt @@ -0,0 +1 @@ +target_stm32h743xi(IFLIGHT_H743_AIO_V2 SKIP_RELEASES) diff --git a/src/main/target/IFLIGHT_H743_AIO_V2/config.c b/src/main/target/IFLIGHT_H743_AIO_V2/config.c new file mode 100644 index 0000000000..8ec9f13847 --- /dev/null +++ b/src/main/target/IFLIGHT_H743_AIO_V2/config.c @@ -0,0 +1,26 @@ +/* + * This file is part of INAV. + * + * INAV 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. + * + * INAV 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 INAV. If not, see . + */ + +#include +#include "platform.h" +#include "fc/fc_msp_box.h" +#include "fc/config.h" +#include "io/piniobox.h" + +void targetConfiguration(void) +{ +} diff --git a/src/main/target/IFLIGHT_H743_AIO_V2/target.c b/src/main/target/IFLIGHT_H743_AIO_V2/target.c new file mode 100644 index 0000000000..3e02bff3e3 --- /dev/null +++ b/src/main/target/IFLIGHT_H743_AIO_V2/target.c @@ -0,0 +1,38 @@ +/* + * This file is part of INAV. + * + * INAV 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. + * + * INAV 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 INAV. If not, see . + */ + +#include + +#include "platform.h" + +#include "drivers/bus.h" +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" +#include "drivers/timer.h" +#include "drivers/pinio.h" +#include "drivers/sensor.h" + +timerHardware_t timerHardware[] = { + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // M1 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 1), // M2 + DEF_TIM(TIM3, CH1, PB4, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 2), // M3 + DEF_TIM(TIM3, CH2, PB5, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 3), // M4 + + DEF_TIM( TIM1, CH1, PA8, TIM_USE_LED, 0, 4) // LED_2812 +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/IFLIGHT_H743_AIO_V2/target.h b/src/main/target/IFLIGHT_H743_AIO_V2/target.h new file mode 100644 index 0000000000..f3447f25dd --- /dev/null +++ b/src/main/target/IFLIGHT_H743_AIO_V2/target.h @@ -0,0 +1,183 @@ +/* + * This file is part of INAV. + * + * INAV 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. + * + * INAV 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 INAV. If not, see . + */ + +#pragma once + +#define USE_TARGET_CONFIG + +#define TARGET_BOARD_IDENTIFIER "IF7A" +#define USBD_PRODUCT_STRING "IFLIGHT_H743_AIO_V2" + +#define LED0 PC13 + +#define BEEPER_PIN PD2 +#define BEEPER_INVERTED + +// *************** SPI1 Gyro & ACC ******************* + +#define USE_SPI +#define USE_SPI_DEVICE_1 +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 +#define SPI1_NSS_PIN PA4 + +#define USE_IMU_MPU6000 +#define IMU_MPU6000_ALIGN CW0_DEG +#define MPU6000_CS_PIN SPI1_NSS_PIN +#define MPU6000_SPI_BUS BUS_SPI1 +#define MPU6000_EXTI_PIN PD0 + +#define USE_IMU_BMI270 +#define IMU_BMI270_ALIGN CW0_DEG +#define BMI270_CS_PIN SPI1_NSS_PIN +#define BMI270_SPI_BUS BUS_SPI1 +#define BMI270_EXTI_PIN + +#define USE_EXTI +#define USE_MPU_DATA_READY_SIGNAL +#define ENSURE_MPU_DATA_READY_IS_LOW + +// *************** I2C1 Baro/Mag ********************* +#define USE_I2C +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB8 +#define I2C1_SDA PB9 + +//UUART3 +#define USE_I2C_DEVICE_2 +#define I2C2_SCL PB10 +#define I2C2_SDA PB11 +// + +#define USE_BARO +#define BARO_I2C_BUS BUS_I2C1 +#define USE_BARO_BMP280 +#define USE_BARO_MS5611 +#define USE_BARO_DPS310 + +#define USE_MAG +#define MAG_I2C_BUS BUS_I2C2 +#define USE_MAG_AK8975 +#define USE_MAG_HMC5883 +#define USE_MAG_QMC5883 +#define USE_MAG_IST8310 +#define USE_MAG_IST8308 +#define USE_MAG_MAG3110 +#define USE_MAG_LIS3MDL + +#define TEMPERATURE_I2C_BUS BUS_I2C2 +#define BNO055_I2C_BUS BUS_I2C2 +#define PITOT_I2C_BUS BUS_I2C2 + +#define USE_RANGEFINDER +#define RANGEFINDER_I2C_BUS BUS_I2C2 + +// *************** SPI2 *********************** +#define USE_SPI_DEVICE_2 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 +#define SPI2_NSS_PIN PB12 + +// *************** SPI3 BLACKBOX ******************* +#define USE_SPI_DEVICE_3 +#define SPI3_SCK_PIN PC10 +#define SPI3_MISO_PIN PC11 +#define SPI3_MOSI_PIN PC12 +#define SPI3_NSS_PIN PA15 + +#define USE_FLASHFS +#define USE_FLASH_M25P16 +#define M25P16_SPI_BUS BUS_SPI3 +#define M25P16_CS_PIN SPI3_NSS_PIN +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT + +// *************** SPI4 OSD ******************* +#define USE_SPI_DEVICE_4 +#define SPI4_SCK_PIN PE2 +#define SPI4_MISO_PIN PE5 +#define SPI4_MOSI_PIN PE6 +#define SPI4_NSS_PIN PE4 + +#define USE_MAX7456 +#define MAX7456_SPI_BUS BUS_SPI4 +#define MAX7456_CS_PIN SPI4_NSS_PIN + +// *************** UART ***************************** +#define USE_VCP + +#define USE_UART1 +#define UART1_RX_PIN PA10 +#define UART1_TX_PIN PA9 + +#define USE_UART2 +#define UART2_RX_PIN PA3 +#define UART2_TX_PIN PA2 + +#define USE_UART4 +#define UART4_RX_PIN PA1 +#define UART4_TX_PIN PA0 + +#define USE_UART6 +#define UART6_RX_PIN PC7 +#define UART6_TX_PIN PC6 + +#define USE_UART7 +#define UART7_RX_PIN PE7 +#define UART7_TX_PIN PE8 + +#define USE_UART8 +#define UART8_RX_PIN PE0 +#define UART8_TX_PIN PE1 + +#define SERIAL_PORT_COUNT 7 + +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_CRSF +#define SERIALRX_UART SERIAL_PORT_USART4 + +// *************** ADC ***************************** +#define USE_ADC +#define ADC_INSTANCE ADC3 + +#define ADC_CHANNEL_1_PIN PC3 +#define ADC_CHANNEL_2_PIN PC2 +#define ADC_CHANNEL_3_PIN PC5 + +#define VBAT_ADC_CHANNEL ADC_CHN_1 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 +#define RSSI_ADC_CHANNEL ADC_CHN_3 + +// *************** LEDSTRIP ************************ +#define USE_LED_STRIP +#define WS2811_PIN PA8 + +#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_TELEMETRY | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TX_PROF_SEL | FEATURE_BLACKBOX) +#define CURRENT_METER_SCALE 100 + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD 0xffff +#define TARGET_IO_PORTE 0xffff + +#define MAX_PWM_OUTPUT_PORTS 4 +#define USE_DSHOT +#define USE_ESC_SENSOR diff --git a/src/main/target/JHEH7AIO/CMakeLists.txt b/src/main/target/JHEH7AIO/CMakeLists.txt new file mode 100644 index 0000000000..b5fc68d4fe --- /dev/null +++ b/src/main/target/JHEH7AIO/CMakeLists.txt @@ -0,0 +1 @@ +target_stm32h743xi(JHEH7AIO SKIP_RELEASES) diff --git a/src/main/target/JHEH7AIO/config.c b/src/main/target/JHEH7AIO/config.c new file mode 100644 index 0000000000..8ec9f13847 --- /dev/null +++ b/src/main/target/JHEH7AIO/config.c @@ -0,0 +1,26 @@ +/* + * This file is part of INAV. + * + * INAV 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. + * + * INAV 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 INAV. If not, see . + */ + +#include +#include "platform.h" +#include "fc/fc_msp_box.h" +#include "fc/config.h" +#include "io/piniobox.h" + +void targetConfiguration(void) +{ +} diff --git a/src/main/target/JHEH7AIO/target.c b/src/main/target/JHEH7AIO/target.c new file mode 100644 index 0000000000..479816acdc --- /dev/null +++ b/src/main/target/JHEH7AIO/target.c @@ -0,0 +1,38 @@ +/* + * This file is part of INAV. + * + * INAV 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. + * + * INAV 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 INAV. If not, see . + */ + +#include + +#include "platform.h" + +#include "drivers/bus.h" +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" +#include "drivers/timer.h" +#include "drivers/pinio.h" +#include "drivers/sensor.h" + +timerHardware_t timerHardware[] = { + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // M1 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 1), // M2 + DEF_TIM(TIM1, CH1, PE9, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 2), // M3 + DEF_TIM(TIM1, CH2, PE11, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 3), // M4 + + DEF_TIM( TIM4, CH1, PD12, TIM_USE_LED, 0, 4) // LED_2812 +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); \ No newline at end of file diff --git a/src/main/target/JHEH7AIO/target.h b/src/main/target/JHEH7AIO/target.h new file mode 100644 index 0000000000..fe16fa07c2 --- /dev/null +++ b/src/main/target/JHEH7AIO/target.h @@ -0,0 +1,175 @@ +/* + * This file is part of INAV. + * + * INAV 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. + * + * INAV 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 INAV. If not, see . + */ + +#pragma once + +#define USE_TARGET_CONFIG + +#define TARGET_BOARD_IDENTIFIER "JH7A" +#define USBD_PRODUCT_STRING "JHEH743AIO" + +#define LED0 PC13 + +#define BEEPER_PIN PD15 +#define BEEPER_INVERTED + +// *************** SPI1 Gyro & ACC ******************* + +#define USE_SPI +#define USE_SPI_DEVICE_1 +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 +#define SPI1_NSS_PIN PA4 + +#define USE_IMU_MPU6000 +#define IMU_MPU6000_ALIGN CW0_DEG +#define MPU6000_CS_PIN SPI1_NSS_PIN +#define MPU6000_SPI_BUS BUS_SPI1 +#define MPU6000_EXTI_PIN PD0 + +#define USE_EXTI +#define USE_MPU_DATA_READY_SIGNAL +#define ENSURE_MPU_DATA_READY_IS_LOW + +// *************** I2C1 Baro/Mag ********************* +#define USE_I2C +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB8 +#define I2C1_SDA PB9 + +#define USE_BARO +#define BARO_I2C_BUS BUS_I2C1 +#define USE_BARO_BMP280 +#define USE_BARO_MS5611 +#define USE_BARO_DPS310 + +#define USE_MAG +#define MAG_I2C_BUS BUS_I2C1 +#define USE_MAG_AK8975 +#define USE_MAG_HMC5883 +#define USE_MAG_QMC5883 +#define USE_MAG_IST8310 +#define USE_MAG_IST8308 +#define USE_MAG_MAG3110 +#define USE_MAG_LIS3MDL + +#define TEMPERATURE_I2C_BUS BUS_I2C1 +#define BNO055_I2C_BUS BUS_I2C1 +#define PITOT_I2C_BUS BUS_I2C1 + +#define USE_RANGEFINDER +#define RANGEFINDER_I2C_BUS BUS_I2C1 + +// *************** SPI2 *********************** +#define USE_SPI_DEVICE_2 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 +#define SPI2_NSS_PIN PB12 + +// *************** SPI3 BLACKBOX ******************* +#define USE_SPI_DEVICE_3 +#define SPI3_SCK_PIN PC10 +#define SPI3_MISO_PIN PC11 +#define SPI3_MOSI_PIN PC12 +#define SPI3_NSS_PIN PA15 + +#define USE_FLASHFS +#define USE_FLASH_M25P16 +#define M25P16_SPI_BUS BUS_SPI3 +#define M25P16_CS_PIN SPI3_NSS_PIN +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT + +// *************** SPI4 OSD ******************* +#define USE_SPI_DEVICE_4 +#define SPI4_SCK_PIN PE2 +#define SPI4_MISO_PIN PE5 +#define SPI4_MOSI_PIN PE6 +#define SPI4_NSS_PIN PE4 + +#define USE_MAX7456 +#define MAX7456_SPI_BUS BUS_SPI4 +#define MAX7456_CS_PIN SPI4_NSS_PIN + +// *************** UART ***************************** +#define USE_VCP + +#define USE_UART1 +#define UART1_RX_PIN PA10 +#define UART1_TX_PIN PA9 + +#define USE_UART2 +#define UART2_RX_PIN PA3 +#define UART2_TX_PIN PA2 + +#define USE_UART3 +#define UART3_RX_PIN PB11 +#define UART3_TX_PIN PB10 + +#define USE_UART4 +#define UART4_RX_PIN PA1 +#define UART4_TX_PIN PA0 + +#define USE_UART6 +#define UART6_RX_PIN PC7 +#define UART6_TX_PIN PC6 + +#define USE_UART7 +#define UART7_RX_PIN PE7 +#define UART7_TX_PIN PE8 + +#define USE_UART8 +#define UART8_RX_PIN PE0 +#define UART8_TX_PIN PE1 + +#define SERIAL_PORT_COUNT 8 + +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_CRSF +#define SERIALRX_UART SERIAL_PORT_USART2 + +// *************** ADC ***************************** +#define USE_ADC +#define ADC_INSTANCE ADC3 + +#define ADC_CHANNEL_1_PIN PC3 +#define ADC_CHANNEL_2_PIN PC2 +#define ADC_CHANNEL_3_PIN PC5 + +#define VBAT_ADC_CHANNEL ADC_CHN_1 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 +#define RSSI_ADC_CHANNEL ADC_CHN_3 + +// *************** LEDSTRIP ************************ +#define USE_LED_STRIP +#define WS2811_PIN PD12 + +#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_TELEMETRY | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TX_PROF_SEL | FEATURE_BLACKBOX) +#define CURRENT_METER_SCALE 100 + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD 0xffff +#define TARGET_IO_PORTE 0xffff + +#define MAX_PWM_OUTPUT_PORTS 4 +#define USE_DSHOT +#define USE_ESC_SENSOR \ No newline at end of file diff --git a/src/main/target/KAKUTEH7/CMakeLists.txt b/src/main/target/KAKUTEH7/CMakeLists.txt index 6c7ffdc31b..f2b739773d 100644 --- a/src/main/target/KAKUTEH7/CMakeLists.txt +++ b/src/main/target/KAKUTEH7/CMakeLists.txt @@ -1 +1,2 @@ target_stm32h743xi(KAKUTEH7) +target_stm32h743xi(KAKUTEH7MINI) diff --git a/src/main/target/KAKUTEH7/config.c b/src/main/target/KAKUTEH7/config.c index 001de76c1a..01391c4bae 100644 --- a/src/main/target/KAKUTEH7/config.c +++ b/src/main/target/KAKUTEH7/config.c @@ -33,8 +33,10 @@ void targetConfiguration(void) serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART1)].functionMask = FUNCTION_MSP; serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART1)].msp_baudrateIndex = BAUD_115200; +#ifdef KAKUTEH7 + //No BT on H7 Mini, do not reserve port serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART2)].functionMask = FUNCTION_MSP; serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART2)].msp_baudrateIndex = BAUD_115200; - +#endif } diff --git a/src/main/target/KAKUTEH7/target.c b/src/main/target/KAKUTEH7/target.c index be6402a639..086d941f1c 100644 --- a/src/main/target/KAKUTEH7/target.c +++ b/src/main/target/KAKUTEH7/target.c @@ -33,12 +33,12 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM2, CH2, PB3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 2), // S3 DEF_TIM(TIM2, CH3, PB10, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 3), // S4 - DEF_TIM(TIM5, CH1, PA0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 4), // S1 - DEF_TIM(TIM5, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 5), // S2 - DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 6), // S3 - DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 7), // S4 + DEF_TIM(TIM5, CH1, PA0, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 4), // S5 + DEF_TIM(TIM5, CH3, PA2, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 5), // S6 + DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 6), // S7 + DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 7), // S8 - DEF_TIM(TIM4, CH1, PD12, TIM_USE_LED, 0, 8), // LED_2812 + DEF_TIM(TIM4, CH1, PD12, TIM_USE_LED, 0, 9), // LED_2812 }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/KAKUTEH7/target.h b/src/main/target/KAKUTEH7/target.h index 9b8727abd1..3b308c844e 100644 --- a/src/main/target/KAKUTEH7/target.h +++ b/src/main/target/KAKUTEH7/target.h @@ -18,8 +18,14 @@ #pragma once +#ifdef KAKUTEH7MINI #define TARGET_BOARD_IDENTIFIER "KTH7" #define USBD_PRODUCT_STRING "KAKUTEH7" +#else +#define TARGET_BOARD_IDENTIFIER "KH7M" +#define USBD_PRODUCT_STRING "KAKUTEH7MINI" + +#endif #define USE_TARGET_CONFIG @@ -41,6 +47,17 @@ #define SPI1_MISO_PIN PA6 #define SPI1_MOSI_PIN PA7 +#ifdef KAKUTEH7MINI + +#define USE_FLASHFS +#define USE_FLASH_M25P16 +#define M25P16_CS_PIN PA4 +#define M25P16_SPI_BUS BUS_SPI1 + +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT + +#else + #define USE_SDCARD #define USE_SDCARD_SPI #define SDCARD_SPI_BUS BUS_SPI1 @@ -50,6 +67,9 @@ #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT +#endif + + // *************** SPI2 *********************** #define USE_SPI_DEVICE_2 #define SPI2_SCK_PIN PB13 @@ -151,7 +171,13 @@ // *************** PINIO *************************** #define USE_PINIO #define USE_PINIOBOX + +#ifdef KAKUTEH7MINI +#define PINIO1_PIN PB11 +#define PINIO1_FLAGS PINIO_FLAGS_INVERTED +#else #define PINIO1_PIN PE13 +#endif // *************** LEDSTRIP ************************ #define USE_LED_STRIP diff --git a/src/main/target/LUX_RACE/target.h b/src/main/target/LUX_RACE/target.h index 45acda286a..1eacd65d91 100644 --- a/src/main/target/LUX_RACE/target.h +++ b/src/main/target/LUX_RACE/target.h @@ -106,5 +106,3 @@ #define TARGET_IO_PORTC 0xffff #define TARGET_IO_PORTD (BIT(2)) #define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)) - -#define PCA9685_I2C_BUS BUS_I2C2 \ No newline at end of file diff --git a/src/main/target/MAMBAF405_2022A/CMakeLists.txt b/src/main/target/MAMBAF405_2022A/CMakeLists.txt new file mode 100644 index 0000000000..418424e653 --- /dev/null +++ b/src/main/target/MAMBAF405_2022A/CMakeLists.txt @@ -0,0 +1 @@ +target_stm32f405xg(MAMBAF405_2022A) diff --git a/src/main/target/MAMBAF405_2022A/config.c b/src/main/target/MAMBAF405_2022A/config.c new file mode 100644 index 0000000000..355605fd90 --- /dev/null +++ b/src/main/target/MAMBAF405_2022A/config.c @@ -0,0 +1,59 @@ +/* + * This file is part of INAV. + * + * INAV 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. + * + * INAV 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 INAV. If not, see . + */ + +#include +#include + +#include + +#include "common/axis.h" + +#include "config/config_master.h" +#include "config/feature.h" + +#include "drivers/sensor.h" +#include "drivers/pwm_esc_detect.h" +#include "drivers/pwm_output.h" +#include "drivers/serial.h" + +#include "fc/rc_controls.h" + +#include "flight/failsafe.h" +#include "flight/mixer.h" +#include "flight/pid.h" + +#include "rx/rx.h" + +#include "io/serial.h" + +#include "sensors/battery.h" +#include "sensors/sensors.h" + +#include "telemetry/telemetry.h" +#include "fc/fc_msp_box.h" +#include "io/piniobox.h" + +void targetConfiguration(void) +{ + pinioBoxConfigMutable()->permanentId[0] = BOX_PERMANENT_ID_USER1; + pinioBoxConfigMutable()->permanentId[1] = BOX_PERMANENT_ID_USER2; + + serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART1)].functionMask = FUNCTION_RX_SERIAL; + + serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART4)].functionMask = FUNCTION_MSP; + serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART4)].msp_baudrateIndex = BAUD_115200; +} diff --git a/src/main/target/MAMBAF405_2022A/target.c b/src/main/target/MAMBAF405_2022A/target.c new file mode 100644 index 0000000000..0e659296e8 --- /dev/null +++ b/src/main/target/MAMBAF405_2022A/target.c @@ -0,0 +1,39 @@ +/* + * This file is part of INAV. + * + * INAV 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. + * + * INAV 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 INAV. If not, see . + */ + + +#include + +#include "platform.h" +#include "drivers/io.h" +#include "drivers/timer.h" +#include "drivers/pwm_mapping.h" +#include "drivers/bus.h" + +timerHardware_t timerHardware[] = { + + DEF_TIM(TIM1, CH2, PA9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1 ), // S1 pin A9: DMA2 Stream 6 Channel 0 + DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1 ), // S2 pin A8: DMA2 Stream 6 Channel 0 + DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), // S3 pin C9: DMA2 Stream 7 Channel 7 + DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1 ), // S4 pin C8: DMA2 Stream 2 Channel 0 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR, 1, 0 ), // S4 pin C8: DMA2 Stream 2 Channel 0 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR, 1, 0 ), // S4 pin C8: DMA2 Stream 2 Channel 0 + + DEF_TIM(TIM2, CH2, PB3, TIM_USE_LED, 0, 0 ), // LED_STRIP – D(1, 6, 3) +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/MAMBAF405_2022A/target.h b/src/main/target/MAMBAF405_2022A/target.h new file mode 100644 index 0000000000..b3d6ad9d44 --- /dev/null +++ b/src/main/target/MAMBAF405_2022A/target.h @@ -0,0 +1,197 @@ +/* + * This file is part of INAV. + * + * INAV 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. + * + * INAV 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 INAV. If not, see . + */ + +#pragma once + +#define USE_TARGET_CONFIG + +#define TARGET_BOARD_IDENTIFIER "M42A" + +#define USBD_PRODUCT_STRING "MAMBAF405_2022A" + +// ******** Board LEDs ********************** +#define LED0 PC15 +#define LED1 PC14 + +// ******* Beeper *********** +#define BEEPER PC13 +#define BEEPER_INVERTED + + +// ******* GYRO and ACC ******** +#define USE_EXTI +#define GYRO_INT_EXTI PC4 +#define USE_MPU_DATA_READY_SIGNAL + +//MPU6000 +#define USE_IMU_MPU6000 +#define IMU_MPU6000_ALIGN CW180_DEG +#define MPU6000_SPI_BUS BUS_SPI1 +#define MPU6000_CS_PIN PA4 + +//MPU6500 +#define USE_IMU_MPU6500 +#define IMU_MPU6500_ALIGN CW180_DEG +#define MPU6500_CS_PIN SPI1_NSS_PIN +#define MPU6500_SPI_BUS BUS_SPI1 + +//BMI270 +#define USE_IMU_BMI270 +#define IMU_BMI270_ALIGN CW180_DEG +#define BMI270_SPI_BUS BUS_SPI1 +#define BMI270_CS_PIN SPI1_NSS_PIN +#define BMI270_EXTI_PIN GYRO_INT_EXTI + +// *************** Baro ************************** +#define USE_I2C + +#define USE_I2C_DEVICE_1 +#define I2C2_SCL PB8 +#define I2C2_SDA PB9 +#define DEFAULT_I2C_BUS BUS_I2C1 + +#define USE_BARO +#define BARO_I2C_BUS DEFAULT_I2C_BUS + +#define USE_BARO_BMP280 +#define USE_BARO_MS5611 +#define USE_BARO_BMP085 +#define USE_BARO_DPS310 +#define USE_BARO_SPL06 + +//*********** Magnetometer / Compass ************* +#define USE_MAG +#define MAG_I2C_BUS DEFAULT_I2C_BUS + +#define USE_MAG_AK8975 +#define USE_MAG_HMC5883 +#define USE_MAG_QMC5883 +#define USE_MAG_IST8310 +#define USE_MAG_IST8308 +#define USE_MAG_MAG3110 +#define USE_MAG_LIS3MDL + +// ******* SERIAL ******** +#define USE_VCP + +#define USE_UART1 +#define UART1_TX_PIN PB6 +#define UART1_RX_PIN PB7 + +#define USE_UART2 +#define UART2_TX_PIN PA2 +#define UART2_RX_PIN PA3 + +#define USE_UART3 +#define UART3_TX_PIN PB10 +#define UART3_RX_PIN PB11 + +#define USE_UART4 +#define UART4_TX_PIN PA0 +#define UART4_RX_PIN PA1 + +#define USE_UART5 +#define UART5_TX_PIN PC12 +#define UART5_RX_PIN PD2 + +#define USE_UART6 +#define UART6_TX_PIN PC6 +#define UART6_RX_PIN PC7 + +#define SERIAL_PORT_COUNT 7 + +#define USE_UART_INVERTER +#define INVERTER_PIN_UART1_RX PC0 + +// ******* SPI ******** +#define USE_SPI + +#define USE_SPI_DEVICE_1 +#define SPI1_NSS_PIN PA4 +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 + +#define USE_SPI_DEVICE_2 +#define SPI2_NSS_PIN PB12 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 + +#define USE_SPI_DEVICE_3 +#define SPI3_NSS_PIN PA15 +#define SPI3_SCK_PIN PC10 +#define SPI3_MISO_PIN PC11 +#define SPI3_MOSI_PIN PB5 + +// ******* ADC ******** +#define USE_ADC +#define ADC_CHANNEL_1_PIN PC1 +#define ADC_CHANNEL_2_PIN PC3 + +#define VBAT_ADC_CHANNEL ADC_CHN_1 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 + +#define VBAT_SCALE_DEFAULT 1100 + +// ******* OSD ******** +#define USE_MAX7456 +#define MAX7456_SPI_BUS BUS_SPI2 +#define MAX7456_CS_PIN PB12 + +//******* FLASH ******** +#define USE_FLASHFS +#define USE_FLASH_M25P16 +#define M25P16_SPI_BUS BUS_SPI3 +#define M25P16_CS_PIN PA15 + +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT + +//************ LEDSTRIP ***************** +#define USE_LED_STRIP +#define WS2811_PIN PB3 + +// ******* FEATURES ******** +#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL +#define SERIALRX_UART SERIAL_PORT_USART1 +#define SERIALRX_PROVIDER SERIALRX_SBUS + +#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_TELEMETRY) + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD (BIT(2)) + +#define MAX_PWM_OUTPUT_PORTS 6 +#define TARGET_MOTOR_COUNT 4 + +// ESC-related features +#define USE_DSHOT +#define USE_ESC_SENSOR +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +#define TEMPERATURE_I2C_BUS DEFAULT_I2C_BUS +#define PITOT_I2C_BUS DEFAULT_I2C_BUS +#define RANGEFINDER_I2C_BUS DEFAULT_I2C_BUS +#define BNO055_I2C_BUS DEFAULT_I2C_BUS + +// *************** PINIO *************************** +#define USE_PINIO +#define USE_PINIOBOX +#define PINIO1_PIN PC2 +#define PINIO2_PIN PC5 \ No newline at end of file diff --git a/src/main/target/MAMBAF722/target.h b/src/main/target/MAMBAF722/target.h index 5f4b9035ae..b6522ee008 100644 --- a/src/main/target/MAMBAF722/target.h +++ b/src/main/target/MAMBAF722/target.h @@ -46,6 +46,12 @@ #define USE_IMU_MPU6000 #define IMU_MPU6000_ALIGN CW180_DEG +// The 2022 variant of F722_I2C with MPU6500 +#define USE_IMU_MPU6500 +#define IMU_MPU6500_ALIGN CW180_DEG +#define MPU6500_CS_PIN SPI1_NSS_PIN +#define MPU6500_SPI_BUS BUS_SPI1 + #define USE_I2C #ifdef MAMBAF722_I2C diff --git a/src/main/target/MAMBAF722_2022A/CMakeLists.txt b/src/main/target/MAMBAF722_2022A/CMakeLists.txt new file mode 100644 index 0000000000..202a13f7cc --- /dev/null +++ b/src/main/target/MAMBAF722_2022A/CMakeLists.txt @@ -0,0 +1 @@ +target_stm32f722xe(MAMBAF722_2022A) \ No newline at end of file diff --git a/src/main/target/MAMBAF722_2022A/config.c b/src/main/target/MAMBAF722_2022A/config.c new file mode 100644 index 0000000000..4b9b627f1b --- /dev/null +++ b/src/main/target/MAMBAF722_2022A/config.c @@ -0,0 +1,65 @@ +/* + * This file is part of INAV. + * + * INAV 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. + * + * INAV 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 INAV. If not, see . + */ + +#include +#include + +#include + +#include "common/axis.h" + +#include "config/config_master.h" +#include "config/feature.h" + +#include "drivers/sensor.h" +#include "drivers/pwm_esc_detect.h" +#include "drivers/pwm_output.h" +#include "drivers/serial.h" + +#include "fc/rc_controls.h" + +#include "flight/failsafe.h" +#include "flight/mixer.h" +#include "flight/pid.h" + +#include "rx/rx.h" + +#include "io/serial.h" + +#include "sensors/battery.h" +#include "sensors/sensors.h" + +#include "telemetry/telemetry.h" +#include "io/piniobox.h" + +void targetConfiguration(void) +{ + + pinioBoxConfigMutable()->permanentId[0] = BOX_PERMANENT_ID_USER1; + pinioBoxConfigMutable()->permanentId[1] = BOX_PERMANENT_ID_USER2; + + /* + * UART1 is SerialRX + */ + serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART1)].functionMask = FUNCTION_RX_SERIAL; + + /* + * Enable MSP at 115200 at UART4 + */ + serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART4)].functionMask = FUNCTION_MSP; + serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART4)].msp_baudrateIndex = BAUD_115200; +} diff --git a/src/main/target/MAMBAF722_2022A/target.c b/src/main/target/MAMBAF722_2022A/target.c new file mode 100644 index 0000000000..56f5483cbd --- /dev/null +++ b/src/main/target/MAMBAF722_2022A/target.c @@ -0,0 +1,43 @@ +/* + * This file is part of INAV. + * + * INAV 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. + * + * INAV 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 INAV. If not, see . + */ + + +#include + +#include "platform.h" +#include "drivers/io.h" +#include "drivers/timer.h" +#include "drivers/pwm_mapping.h" +#include "drivers/bus.h" + +timerHardware_t timerHardware[] = { + DEF_TIM(TIM11, CH1, PB9, TIM_USE_PPM, 0, 0 ), // PPM IN + + DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0 ), // S1 + DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0 ), // S2 + DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1 ), // S3 + DEF_TIM(TIM1, CH2, PA9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1 ), // S4 + + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0 ), // S5 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0 ), // S6 + DEF_TIM(TIM1, CH3, PA10, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0 ), // S7 + DEF_TIM(TIM3, CH1, PB4, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0 ), // S8 + + DEF_TIM(TIM2, CH2, PB3, TIM_USE_LED, 0, 0 ), // LED_STRIP – D(1, 6, 3) +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/MAMBAF722_2022A/target.h b/src/main/target/MAMBAF722_2022A/target.h new file mode 100644 index 0000000000..a55fdb58cb --- /dev/null +++ b/src/main/target/MAMBAF722_2022A/target.h @@ -0,0 +1,190 @@ +/* + * This file is part of INAV. + * + * INAV 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. + * + * INAV 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 INAV. If not, see . + */ + +#pragma once + +#define USE_TARGET_CONFIG + +#define TARGET_BOARD_IDENTIFIER "M72A" +#define USBD_PRODUCT_STRING "MAMBAF722_2022A" + +// ******** Board LEDs ********************** +#define LED0 PC15 +#define LED1 PC14 + +// ******* Beeper *********** +#define BEEPER PB2 +#define BEEPER_INVERTED + +// ******* GYRO and ACC ******** +#define USE_EXTI +#define GYRO_INT_EXTI PC4 +#define USE_MPU_DATA_READY_SIGNAL + +//MPU6000 +#define USE_IMU_MPU6000 +#define IMU_MPU6000_ALIGN CW180_DEG +#define MPU6000_CS_PIN SPI1_NSS_PIN +#define MPU6000_SPI_BUS BUS_SPI1 + +//MPU6500 +#define USE_IMU_MPU6500 +#define IMU_MPU6500_ALIGN CW180_DEG +#define MPU6500_CS_PIN SPI1_NSS_PIN +#define MPU6500_SPI_BUS BUS_SPI1 + +//BMI270 +#define USE_IMU_BMI270 +#define IMU_BMI270_ALIGN CW180_DEG +#define BMI270_SPI_BUS BUS_SPI1 +#define BMI270_CS_PIN SPI1_NSS_PIN +#define BMI270_EXTI_PIN GYRO_INT_EXTI + +#define USE_I2C + +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB8 +#define I2C1_SDA PB9 +#define DEFAULT_I2C_BUS BUS_I2C1 + +// *************** Baro ************************** +#define USE_BARO +#define BARO_I2C_BUS DEFAULT_I2C_BUS +#define USE_BARO_BMP280 +#define USE_BARO_MS5611 +#define USE_BARO_BMP085 +#define USE_BARO_DPS310 +#define USE_BARO_SPL06 + +//*********** Magnetometer / Compass ************* +#define USE_MAG +#define MAG_I2C_BUS DEFAULT_I2C_BUS + +#define USE_MAG_HMC5883 +#define USE_MAG_QMC5883 +#define USE_MAG_IST8310 +#define USE_MAG_MAG3110 +#define USE_MAG_LIS3MDL + +// ******* SERIAL ******** +#define USE_VCP +#define USE_UART1 +#define USE_UART2 +#define USE_UART3 +#define USE_UART4 +#define USE_UART5 +#define USE_UART6 + +#define UART1_TX_PIN PB6 +#define UART1_RX_PIN PB7 + +#define UART2_TX_PIN PA2 +#define UART2_RX_PIN PA3 + +#define UART3_TX_PIN PB10 +#define UART3_RX_PIN PB11 + +#define UART4_TX_PIN PA0 +#define UART4_RX_PIN PA1 + +#define UART5_TX_PIN PC12 +#define UART5_RX_PIN PD2 + +#define UART6_TX_PIN PC6 +#define UART6_RX_PIN PC7 + +#define SERIAL_PORT_COUNT 7 + +// ******* SPI ******** +#define USE_SPI + +#define USE_SPI_DEVICE_1 +#define SPI1_NSS_PIN PA4 +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 + +#define USE_SPI_DEVICE_2 +#define SPI2_NSS_PIN PB12 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 + +#define USE_SPI_DEVICE_3 +#define SPI3_NSS_PIN PA15 +#define SPI3_SCK_PIN PC10 +#define SPI3_MISO_PIN PC11 +#define SPI3_MOSI_PIN PB5 + +// ******* ADC ******** +#define USE_ADC +#define ADC_CHANNEL_1_PIN PC1 +#define ADC_CHANNEL_2_PIN PC3 + +#define VBAT_ADC_CHANNEL ADC_CHN_1 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 + +#define VBAT_SCALE_DEFAULT 1100 + +// ******* OSD ******** +#define USE_MAX7456 +#define MAX7456_SPI_BUS BUS_SPI2 +#define MAX7456_CS_PIN SPI2_NSS_PIN + +//******* FLASH ******** +#define USE_FLASHFS +#define USE_FLASH_M25P16 +#define M25P16_CS_PIN SPI3_NSS_PIN +#define M25P16_SPI_BUS BUS_SPI3 + +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT + +//************ LEDSTRIP ***************** +#define USE_LED_STRIP +#define WS2811_PIN PB3 + +// ******* FEATURES ******** +#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL +#define SERIALRX_UART SERIAL_PORT_USART1 +#define SERIALRX_PROVIDER SERIALRX_SBUS + +#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_TELEMETRY) + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD (BIT(2)) + +#define MAX_PWM_OUTPUT_PORTS 8 +#define TARGET_MOTOR_COUNT 8 + +// ESC-related features +#define USE_DSHOT +#define USE_ESC_SENSOR +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +#define USE_RANGEFINDER +#define RANGEFINDER_I2C_BUS DEFAULT_I2C_BUS +#define TEMPERATURE_I2C_BUS DEFAULT_I2C_BUS +#define PITOT_I2C_BUS DEFAULT_I2C_BUS +#define BNO055_I2C_BUS DEFAULT_I2C_BUS + +// *************** PINIO *************************** +#define USE_PINIO +#define USE_PINIOBOX +#define PINIO1_PIN PC0 // VTX power switcher +#define PINIO2_PIN PC2 // WiFi Switcher \ No newline at end of file diff --git a/src/main/target/MAMBAH743/target.c b/src/main/target/MAMBAH743/target.c index 9802d05814..909df49a07 100644 --- a/src/main/target/MAMBAH743/target.c +++ b/src/main/target/MAMBAH743/target.c @@ -36,12 +36,12 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM5, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 2), // S3 DEF_TIM(TIM5, CH4, PA3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 3), // S4 - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 4), // S5 - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 5), // S6 - DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 6), // S7 - DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 7), // S8 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 4), // S5 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 5), // S6 + DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR, 0, 6), // S7 + DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR, 0, 7), // S8 - DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 8), // LED_2812 + DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 9), // LED_2812 }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/MATEKF405/target.h b/src/main/target/MATEKF405/target.h index 4e66c2fb7a..df9bd6087c 100644 --- a/src/main/target/MATEKF405/target.h +++ b/src/main/target/MATEKF405/target.h @@ -201,5 +201,3 @@ #define USE_ESC_SENSOR #define MAX_PWM_OUTPUT_PORTS 6 - -#define PCA9685_I2C_BUS DEFAULT_I2C_BUS diff --git a/src/main/target/MATEKF405CAN/target.h b/src/main/target/MATEKF405CAN/target.h index 927e8ae3fb..044781b36f 100644 --- a/src/main/target/MATEKF405CAN/target.h +++ b/src/main/target/MATEKF405CAN/target.h @@ -73,7 +73,6 @@ #define PITOT_I2C_BUS BUS_I2C2 #define TEMPERATURE_I2C_BUS BUS_I2C2 -#define PCA9685_I2C_BUS BUS_I2C2 #define BNO055_I2C_BUS BUS_I2C2 diff --git a/src/main/target/MATEKF405SE/target.h b/src/main/target/MATEKF405SE/target.h index 507279bc47..17a0be19a4 100644 --- a/src/main/target/MATEKF405SE/target.h +++ b/src/main/target/MATEKF405SE/target.h @@ -177,5 +177,3 @@ #define TARGET_IO_PORTD (BIT(2)) #define MAX_PWM_OUTPUT_PORTS 9 - -#define PCA9685_I2C_BUS BUS_I2C2 diff --git a/src/main/target/MATEKF405TE/CMakeLists.txt b/src/main/target/MATEKF405TE/CMakeLists.txt new file mode 100644 index 0000000000..ce0150c7d3 --- /dev/null +++ b/src/main/target/MATEKF405TE/CMakeLists.txt @@ -0,0 +1,2 @@ +target_stm32f405xg(MATEKF405TE) +target_stm32f405xg(MATEKF405TE_SD) diff --git a/src/main/target/MATEKF405TE/config.c b/src/main/target/MATEKF405TE/config.c new file mode 100644 index 0000000000..f149b4b8ec --- /dev/null +++ b/src/main/target/MATEKF405TE/config.c @@ -0,0 +1,29 @@ +/* + * This file is part of INAV. + * + * INAV 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. + * + * INAV 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 INAV. If not, see . + */ + +#include +#include "platform.h" +#include "fc/fc_msp_box.h" +#include "fc/config.h" +#include "io/piniobox.h" + +void targetConfiguration(void) +{ + pinioBoxConfigMutable()->permanentId[0] = BOX_PERMANENT_ID_USER1; + pinioBoxConfigMutable()->permanentId[1] = BOX_PERMANENT_ID_USER2; + beeperConfigMutable()->pwmMode = true; +} diff --git a/src/main/target/MATEKF405TE/target.c b/src/main/target/MATEKF405TE/target.c new file mode 100644 index 0000000000..70561e001f --- /dev/null +++ b/src/main/target/MATEKF405TE/target.c @@ -0,0 +1,49 @@ +/* + * This file is part of INAV. + * + * INAV 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. + * + * INAV 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 INAV. If not, see . + */ + +#include +#include +#include "drivers/bus.h" +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" +#include "drivers/timer.h" +#include "drivers/sensor.h" +#include "drivers/timer_def_stm32f4xx.h" + +timerHardware_t timerHardware[] = { + DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1 D(2,7,7) UP217 + DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S2 D(2,2,0) UP217 + DEF_TIM(TIM1, CH3N, PB15, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S3 D(2,6,0) UP256 + DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), // S4 D(2,1,6) UP256 + + DEF_TIM(TIM2, CH4, PB11, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S5 D(1,7,3) UP173 + DEF_TIM(TIM2, CH3, PB10, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S6 D(1,1,3) UP173 + DEF_TIM(TIM2, CH2, PB3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S7 D(1,6,3) UP173 + DEF_TIM(TIM2, CH1, PA15, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S8 D(1,5,3) UP173 + + DEF_TIM(TIM12, CH1, PB14, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S9 DMA NONE + DEF_TIM(TIM13, CH1, PA6, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S10 DMA NONE + DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S11 D(1,0,2) + + DEF_TIM(TIM3, CH4, PB1, TIM_USE_LED, 0, 0), // 2812LED D(1,2,5) + DEF_TIM(TIM11, CH1, PB9, TIM_USE_BEEPER, 0, 0), // BEEPER PWM + + DEF_TIM(TIM9, CH2, PA3, TIM_USE_PPM, 0, 0), //RX2 + DEF_TIM(TIM5, CH3, PA2, TIM_USE_ANY, 0, 0), //TX2 softserial1_Tx +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/MATEKF405TE/target.h b/src/main/target/MATEKF405TE/target.h new file mode 100644 index 0000000000..4ceac5ae97 --- /dev/null +++ b/src/main/target/MATEKF405TE/target.h @@ -0,0 +1,186 @@ +/* + * This file is part of INAV. + * + * INAV 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. + * + * INAV 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 INAV. If not, see . + */ + +#pragma once +#define USE_TARGET_CONFIG + +#if defined(MATEKF405TE_SD) +# define TARGET_BOARD_IDENTIFIER "MF4T" +# define USBD_PRODUCT_STRING "MatekF405TE_SD" +#else +# define TARGET_BOARD_IDENTIFIER "MF4T" +# define USBD_PRODUCT_STRING "MatekF405TE" +#endif + +#define LED0 PA14 //Blue +#define LED1 PA13 //Green + +#define BEEPER PB9 +#define BEEPER_INVERTED +#define BEEPER_PWM_FREQUENCY 2500 + +// *************** SPI1 IMU & OSD ******************* +#define USE_SPI +#define USE_SPI_DEVICE_1 + +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PB4 +#define SPI1_MOSI_PIN PA7 + +#define USE_IMU_ICM42605 +#define IMU_ICM42605_ALIGN CW270_DEG_FLIP +#define ICM42605_SPI_BUS BUS_SPI1 +#define ICM42605_CS_PIN PC14 +#define ICM42605_EXTI_PIN PC15 + +#define USE_IMU_BMI270 +#define IMU_BMI270_ALIGN CW270_DEG_FLIP +#define BMI270_SPI_BUS BUS_SPI1 +#define BMI270_CS_PIN PC14 +#define BMI270_EXTI_PIN PC15 + +#define USE_EXTI +#define USE_MPU_DATA_READY_SIGNAL + +#define USE_MAX7456 +#define MAX7456_SPI_BUS BUS_SPI1 +#define MAX7456_CS_PIN PB12 + +// *************** SPI2 Flash/SD Card **************** +#define USE_SPI_DEVICE_2 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PC2 +#define SPI2_MOSI_PIN PC3 + +#if defined(MATEKF405TE_SD) +# define USE_SDCARD +# define USE_SDCARD_SPI +# define SDCARD_SPI_BUS BUS_SPI2 +# define SDCARD_CS_PIN PC1 +# define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT +#else +# define USE_FLASHFS +# define USE_FLASH_M25P16 +# define M25P16_SPI_BUS BUS_SPI2 +# define M25P16_CS_PIN PC13 +# define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT +#endif + +// *************** I2C /Baro/Mag ********************* +#define USE_I2C +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB8 +#define I2C1_SDA PB7 + +#define USE_BARO +#define BARO_I2C_BUS BUS_I2C1 +#define USE_BARO_BMP280 +#define USE_BARO_MS5611 +#define USE_BARO_DPS310 +#define USE_BARO_SPL06 + +#define USE_MAG +#define MAG_I2C_BUS BUS_I2C1 +#define USE_MAG_HMC5883 +#define USE_MAG_QMC5883 +#define USE_MAG_IST8310 +#define USE_MAG_IST8308 +#define USE_MAG_MAG3110 +#define USE_MAG_LIS3MDL + +#define USE_RANGEFINDER +#define RANGEFINDER_I2C_BUS BUS_I2C1 +#define PITOT_I2C_BUS BUS_I2C1 +#define TEMPERATURE_I2C_BUS BUS_I2C1 +#define BNO055_I2C_BUS BUS_I2C1 + +// *************** UART ***************************** +#define USE_VCP + +#define USE_UART1 +#define UART1_TX_PIN PA9 +#define UART1_RX_PIN PA10 + +#define USE_UART2 +#define UART2_TX_PIN PA2 +#define UART2_RX_PIN PA3 + +#define USE_UART3 +#define UART3_TX_PIN PC10 +#define UART3_RX_PIN PC11 + +#define USE_UART4 +#define UART4_TX_PIN PA0 +#define UART4_RX_PIN PA1 + +#define USE_UART5 +#define UART5_TX_PIN PC12 +#define UART5_RX_PIN PD2 + +#define USE_UART6 +#define UART6_TX_PIN PC6 +#define UART6_RX_PIN PC7 + +#define USE_SOFTSERIAL1 +#define SOFTSERIAL_1_TX_PIN PA2 +#define SOFTSERIAL_1_RX_PIN PA2 + +#define SERIAL_PORT_COUNT 8 + +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_CRSF +#define SERIALRX_UART SERIAL_PORT_USART2 + +// *************** ADC *************************** +#define USE_ADC +#define ADC_INSTANCE ADC1 +#define ADC1_DMA_STREAM DMA2_Stream4 +#define ADC_CHANNEL_1_PIN PC4 +#define ADC_CHANNEL_2_PIN PC5 +#define ADC_CHANNEL_3_PIN PB0 +#define ADC_CHANNEL_4_PIN PC0 +#define VBAT_ADC_CHANNEL ADC_CHN_1 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 +#define RSSI_ADC_CHANNEL ADC_CHN_3 +#define AIRSPEED_ADC_CHANNEL ADC_CHN_4 + +// *************** PINIO *************************** +#define USE_PINIO +#define USE_PINIOBOX +#define PINIO1_PIN PA4 +#define PINIO2_PIN PB5 + +// *************** LEDSTRIP ************************ +#define USE_LED_STRIP +#define WS2811_PIN PB1 + +// *************** others ************************ +#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_TELEMETRY | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TX_PROF_SEL | FEATURE_BLACKBOX) +#define VBAT_SCALE_DEFAULT 2100 +#define CURRENT_METER_SCALE 150 + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD (BIT(2)) + +#define MAX_PWM_OUTPUT_PORTS 11 + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE +#define USE_DSHOT +#define USE_ESC_SENSOR + diff --git a/src/main/target/MATEKF411TE/CMakeLists.txt b/src/main/target/MATEKF411TE/CMakeLists.txt new file mode 100644 index 0000000000..6ca2bd3f7b --- /dev/null +++ b/src/main/target/MATEKF411TE/CMakeLists.txt @@ -0,0 +1 @@ +target_stm32f411xe(MATEKF411TE) diff --git a/src/main/target/MATEKF411TE/config.c b/src/main/target/MATEKF411TE/config.c new file mode 100644 index 0000000000..12a62e7599 --- /dev/null +++ b/src/main/target/MATEKF411TE/config.c @@ -0,0 +1,28 @@ +/* + * 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 "platform.h" +#include "fc/fc_msp_box.h" +#include "fc/config.h" +#include "io/piniobox.h" + +void targetConfiguration(void) +{ + pinioBoxConfigMutable()->permanentId[0] = BOX_PERMANENT_ID_USER1; + pinioBoxConfigMutable()->permanentId[1] = BOX_PERMANENT_ID_USER2; +} diff --git a/src/main/target/MATEKF411TE/target.c b/src/main/target/MATEKF411TE/target.c new file mode 100644 index 0000000000..9bfad340f1 --- /dev/null +++ b/src/main/target/MATEKF411TE/target.c @@ -0,0 +1,43 @@ +/* +* 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 "drivers/bus.h" +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" +#include "drivers/timer.h" +#include "drivers/pinio.h" + +timerHardware_t timerHardware[] = { + + DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1 D(1,0,2) + DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S2 D(1,3,6) + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S3 D(1,7,5) + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4 D(1,2,5) + DEF_TIM(TIM3, CH1, PB4, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S5 D(1,4,5) + DEF_TIM(TIM3, CH2, PB5, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S6 D(1,5,5) + + DEF_TIM(TIM5, CH1, PA0, TIM_USE_ANY, 0, 0), //Softserial1_TX + DEF_TIM(TIM5, CH2, PA1, TIM_USE_ANY, 0, 0), //Softserial1_RX + DEF_TIM(TIM2, CH1, PA15, TIM_USE_ANY, 0, 0), //Softserial2_TX + DEF_TIM(TIM2, CH2, PB3, TIM_USE_ANY, 0, 0), //Softserial2_RX + + DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED | TIM_USE_MC_SERVO, 0, 0), //2812LED D(2,1,6) +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/MATEKF411TE/target.h b/src/main/target/MATEKF411TE/target.h new file mode 100644 index 0000000000..820894acef --- /dev/null +++ b/src/main/target/MATEKF411TE/target.h @@ -0,0 +1,145 @@ +/* + * 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 TARGET_BOARD_IDENTIFIER "M41T" +#define USBD_PRODUCT_STRING "MatekF411TE" + +#define LED0 PA14 +#define LED1 PA13 + +#define BEEPER PB2 +#define BEEPER_INVERTED + +// *************** SPI2 IMU OSD ********************** +#define USE_SPI +#define USE_SPI_DEVICE_2 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 + +#define USE_IMU_BMI270 +#define BMI270_SPI_BUS BUS_SPI2 +#define BMI270_CS_PIN PC13 +#define USE_EXTI +#define BMI270_EXTI_PIN PC14 +#define IMU_BMI270_ALIGN CW270_DEG_FLIP +#define USE_MPU_DATA_READY_SIGNAL + +#define USE_MAX7456 +#define MAX7456_SPI_BUS BUS_SPI2 +#define MAX7456_CS_PIN PB12 + +// *************** UART ***************************** +#define USE_VCP + +#define USE_UART1 +#define UART1_TX_PIN PA9 +#define UART1_RX_PIN PA10 + +#define USE_UART2 +#define UART2_TX_PIN PA2 +#define UART2_RX_PIN PA3 + +#define USE_SOFTSERIAL1 +#define SOFTSERIAL_1_TX_PIN PA0 +#define SOFTSERIAL_1_RX_PIN PA1 + +#define USE_SOFTSERIAL2 +#define SOFTSERIAL_2_TX_PIN PA15 +#define SOFTSERIAL_2_RX_PIN PB3 + +#define SERIAL_PORT_COUNT 5 + +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_CRSF +#define SERIALRX_UART SERIAL_PORT_USART2 + +// *************** I2C /Baro/Mag/Pitot ******************** +#define USE_I2C +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB8 +#define I2C1_SDA PB9 + +#define DEFAULT_I2C_BUS BUS_I2C1 + +#define USE_BARO +#define BARO_I2C_BUS BUS_I2C1 +#define USE_BARO_BMP280 +#define USE_BARO_MS5611 +#define USE_BARO_SPL06 + +#define USE_MAG +#define MAG_I2C_BUS BUS_I2C1 +#define USE_MAG_HMC5883 +#define USE_MAG_QMC5883 +#define USE_MAG_IST8310 +#define USE_MAG_IST8308 +#define USE_MAG_MAG3110 +#define USE_MAG_LIS3MDL +#define USE_MAG_AK8975 + +#define PITOT_I2C_BUS BUS_I2C1 +#define TEMPERATURE_I2C_BUS BUS_I2C1 +#define BNO055_I2C_BUS BUS_I2C1 + +#define USE_RANGEFINDER +#define USE_RANGEFINDER_MSP +#define RANGEFINDER_I2C_BUS BUS_I2C1 + +// *************** ADC ***************************** +#define USE_ADC +#define ADC_INSTANCE ADC1 +#define ADC1_DMA_STREAM DMA2_Stream0 + +#define ADC_CHANNEL_1_PIN PA4 +#define ADC_CHANNEL_2_PIN PA5 +#define ADC_CHANNEL_3_PIN PA6 +#define ADC_CHANNEL_4_PIN PA7 + +#define VBAT_ADC_CHANNEL ADC_CHN_1 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 +#define RSSI_ADC_CHANNEL ADC_CHN_3 +#define AIRSPEED_ADC_CHANNEL ADC_CHN_4 + +// *************** LED2812 ************************ +#define USE_LED_STRIP +#define WS2811_PIN PA8 + +// *************** PINIO *************************** +#define USE_PINIO +#define USE_PINIOBOX +#define PINIO1_PIN PC15 // power switch +#define PINIO2_PIN PB10 // Camera switch + +// *************** OTHERS ************************* +#define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_OSD | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TELEMETRY | FEATURE_SOFTSERIAL ) + +#define VBAT_SCALE_DEFAULT 1100 +#define CURRENT_METER_SCALE 250 //F411-WTE 132A + +#define USE_DSHOT +#define USE_ESC_SENSOR +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD (BIT(2)) + +#define MAX_PWM_OUTPUT_PORTS 6 diff --git a/src/main/target/MATEKH743/target.c b/src/main/target/MATEKH743/target.c index 02c7460e91..d51872b5a9 100644 --- a/src/main/target/MATEKH743/target.c +++ b/src/main/target/MATEKH743/target.c @@ -26,9 +26,10 @@ #include "drivers/pinio.h" #include "drivers/sensor.h" -BUSDEV_REGISTER_SPI_TAG(busdev_imu0, DEVHW_MPU6000, MPU6000_SPI_BUS, MPU6000_CS_PIN, MPU6000_EXTI_PIN, 0, DEVFLAGS_NONE, IMU_MPU6000_ALIGN); -BUSDEV_REGISTER_SPI_TAG(busdev_imu1, DEVHW_MPU6500, MPU6500_SPI_BUS, MPU6500_CS_PIN, MPU6500_EXTI_PIN, 1, DEVFLAGS_NONE, IMU_MPU6500_ALIGN); -BUSDEV_REGISTER_SPI_TAG(busdev_imu2, DEVHW_ICM42605, ICM42605_SPI_BUS, ICM42605_CS_PIN, ICM42605_EXTI_PIN, 2, DEVFLAGS_NONE, IMU_ICM42605_ALIGN); +BUSDEV_REGISTER_SPI_TAG(busdev_mpu6000, DEVHW_MPU6000, MPU6000_SPI_BUS, MPU6000_CS_PIN, MPU6000_EXTI_PIN, 0, DEVFLAGS_NONE, IMU_MPU6000_ALIGN); +BUSDEV_REGISTER_SPI_TAG(busdev_icm42688, DEVHW_ICM42605, MPU6000_SPI_BUS, MPU6000_CS_PIN, MPU6000_EXTI_PIN, 0, DEVFLAGS_NONE, IMU_MPU6000_ALIGN); +BUSDEV_REGISTER_SPI_TAG(busdev_icm20602, DEVHW_MPU6500, MPU6500_SPI_BUS, MPU6500_CS_PIN, MPU6500_EXTI_PIN, 1, DEVFLAGS_NONE, IMU_MPU6500_ALIGN); +BUSDEV_REGISTER_SPI_TAG(busdev_icm42605, DEVHW_ICM42605, ICM42605_SPI_BUS, ICM42605_CS_PIN, ICM42605_EXTI_PIN, 2, DEVFLAGS_NONE, IMU_ICM42605_ALIGN); timerHardware_t timerHardware[] = { DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1 diff --git a/src/main/target/MOTOLAB/target.h b/src/main/target/MOTOLAB/target.h index 81d44eb6a2..f2e2289a2c 100644 --- a/src/main/target/MOTOLAB/target.h +++ b/src/main/target/MOTOLAB/target.h @@ -120,5 +120,4 @@ // !!TODO - check the following line is correct #define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(3)|BIT(4)) -#define PCA9685_I2C_BUS BUS_I2C2 #define BNO055_I2C_BUS BUS_I2C2 \ No newline at end of file diff --git a/src/main/target/OMNIBUSF4/target.h b/src/main/target/OMNIBUSF4/target.h index 96c0de6244..4bb37521a1 100644 --- a/src/main/target/OMNIBUSF4/target.h +++ b/src/main/target/OMNIBUSF4/target.h @@ -281,5 +281,3 @@ #ifdef OMNIBUSF4PRO #define CURRENT_METER_SCALE 265 #endif - -#define PCA9685_I2C_BUS I2C_EXT_BUS diff --git a/src/main/target/OMNIBUSF7/target.h b/src/main/target/OMNIBUSF7/target.h index 65f1fb364d..ab2f85c9c6 100644 --- a/src/main/target/OMNIBUSF7/target.h +++ b/src/main/target/OMNIBUSF7/target.h @@ -196,5 +196,3 @@ #define TARGET_IO_PORTC 0xffff #define TARGET_IO_PORTD 0xffff #define TARGET_IO_PORTE 0xffff - -#define PCA9685_I2C_BUS BUS_I2C2 diff --git a/src/main/target/RCEXPLORERF3/target.h b/src/main/target/RCEXPLORERF3/target.h index 18e6719435..04dc53bbfe 100644 --- a/src/main/target/RCEXPLORERF3/target.h +++ b/src/main/target/RCEXPLORERF3/target.h @@ -110,5 +110,3 @@ #define TARGET_IO_PORTB 0xffff #define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) #define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)) - -#define PCA9685_I2C_BUS BUS_I2C2 diff --git a/src/main/target/REVO/target.h b/src/main/target/REVO/target.h index 4f4befce66..35ae7367b9 100644 --- a/src/main/target/REVO/target.h +++ b/src/main/target/REVO/target.h @@ -144,5 +144,3 @@ #define TARGET_IO_PORTB 0xffff #define TARGET_IO_PORTC 0xffff #define TARGET_IO_PORTD 0xffff - -#define PCA9685_I2C_BUS BUS_I2C2 diff --git a/src/main/target/SKYSTARSF405HD/target.h b/src/main/target/SKYSTARSF405HD/target.h index 544a001487..ec83125d5f 100644 --- a/src/main/target/SKYSTARSF405HD/target.h +++ b/src/main/target/SKYSTARSF405HD/target.h @@ -135,7 +135,6 @@ #define RANGEFINDER_I2C_BUS DEFAULT_I2C_BUS #define PITOT_I2C_BUS DEFAULT_I2C_BUS -#define PCA9685_I2C_BUS DEFAULT_I2C_BUS // *************** ADC ***************************** #define USE_ADC diff --git a/src/main/target/SPARKY/target.h b/src/main/target/SPARKY/target.h index f119da56b1..166ba737f2 100644 --- a/src/main/target/SPARKY/target.h +++ b/src/main/target/SPARKY/target.h @@ -105,5 +105,3 @@ #define TARGET_IO_PORTA (BIT(1)|BIT(2)|BIT(3)|BIT(4)|BIT(5)|BIT(6)|BIT(7)|BIT(8)|BIT(9)|BIT(10)|BIT(11)|BIT(12)|BIT(13)|BIT(14)|BIT(15)) #define TARGET_IO_PORTB (BIT(0)|BIT(1)|BIT(6)|BIT(10)|BIT(11)|BIT(14)|BIT(15)|BIT(3)|BIT(4)|BIT(5)|BIT(6)|BIT(7)|BIT(8)|BIT(9)|BIT(12)|BIT(13)) #define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)) - -#define PCA9685_I2C_BUS BUS_I2C2 \ No newline at end of file diff --git a/src/main/target/SPARKY2/CMakeLists.txt b/src/main/target/SPARKY2/CMakeLists.txt index 12985b381a..dc9ec76308 100644 --- a/src/main/target/SPARKY2/CMakeLists.txt +++ b/src/main/target/SPARKY2/CMakeLists.txt @@ -1 +1 @@ -target_stm32f405xg(SPARKY2) +target_stm32f405xg(SPARKY2 SKIP_RELEASES) diff --git a/src/main/target/SPARKY2/target.h b/src/main/target/SPARKY2/target.h index d5b291b323..c5526d4afd 100644 --- a/src/main/target/SPARKY2/target.h +++ b/src/main/target/SPARKY2/target.h @@ -133,5 +133,3 @@ #define TARGET_IO_PORTA 0xffff #define TARGET_IO_PORTB 0xffff #define TARGET_IO_PORTC 0xffff - -#define PCA9685_I2C_BUS BUS_I2C2 diff --git a/src/main/target/SPRACINGF7DUAL/CMakeLists.txt b/src/main/target/SPRACINGF7DUAL/CMakeLists.txt index 1fc19b7e12..0c87eadd78 100644 --- a/src/main/target/SPRACINGF7DUAL/CMakeLists.txt +++ b/src/main/target/SPRACINGF7DUAL/CMakeLists.txt @@ -1 +1 @@ -target_stm32f722xe(SPRACINGF7DUAL) +target_stm32f722xe(SPRACINGF7DUAL SKIP_RELEASES) diff --git a/src/main/target/TMOTORF7V2/target.h b/src/main/target/TMOTORF7V2/target.h index 6e9fc6dd30..d944121795 100644 --- a/src/main/target/TMOTORF7V2/target.h +++ b/src/main/target/TMOTORF7V2/target.h @@ -86,6 +86,9 @@ #define SPI3_SCK_PIN PC10 #define SPI3_MISO_PIN PC11 #define SPI3_MOSI_PIN PB2 +#define SPI3_SCK_AF GPIO_AF6_SPI3 +#define SPI3_MISO_AF GPIO_AF6_SPI3 +#define SPI3_MOSI_AF GPIO_AF7_SPI3 #define USE_FLASHFS #define USE_FLASH_M25P16 diff --git a/src/main/target/YUPIF4/target.h b/src/main/target/YUPIF4/target.h index 69c5bc3ab1..b514a1fa3e 100644 --- a/src/main/target/YUPIF4/target.h +++ b/src/main/target/YUPIF4/target.h @@ -156,5 +156,3 @@ #define TARGET_IO_PORTB 0xffff #define TARGET_IO_PORTC 0xffff #define TARGET_IO_PORTD (BIT(2)) - -#define PCA9685_I2C_BUS BUS_I2C2 diff --git a/src/main/target/YUPIF7/CMakeLists.txt b/src/main/target/YUPIF7/CMakeLists.txt index b164218ca8..3494bfa453 100644 --- a/src/main/target/YUPIF7/CMakeLists.txt +++ b/src/main/target/YUPIF7/CMakeLists.txt @@ -1 +1 @@ -target_stm32f722xe(YUPIF7) +target_stm32f722xe(YUPIF7 SKIP_RELEASES) diff --git a/src/main/target/common.h b/src/main/target/common.h index 874cdc3a8e..b299e3f3b1 100644 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -220,8 +220,6 @@ #define USE_VTX_FFPV #define USE_PITOT_VIRTUAL -#define USE_PWM_DRIVER_PCA9685 -#define USE_PWM_SERVO_DRIVER #define USE_SERIALRX_SUMD #define USE_SERIALRX_SUMH diff --git a/src/main/target/common_hardware.c b/src/main/target/common_hardware.c index 7ff1444177..0ebbb91df5 100755 --- a/src/main/target/common_hardware.c +++ b/src/main/target/common_hardware.c @@ -388,15 +388,6 @@ BUSDEV_REGISTER_I2C(busdev_ug2864, DEVHW_UG2864, UG2864_I2C_BUS, 0x3C, NONE, DEVFLAGS_NONE, 0); #endif -#if defined(USE_PWM_SERVO_DRIVER) - #if defined(USE_PWM_DRIVER_PCA9685) && defined(USE_I2C) - #if !defined(PCA9685_I2C_BUS) - #define PCA9685_I2C_BUS BUS_I2C1 - #endif - BUSDEV_REGISTER_I2C(busdev_pca9685, DEVHW_PCA9685, PCA9685_I2C_BUS, 0x40, NONE, DEVFLAGS_NONE, 0); - #endif -#endif - #if defined(USE_IRLOCK) && defined(USE_I2C) #if !defined(IRLOCK_I2C_BUS) && defined(EXTERNAL_I2C_BUS) #define IRLOCK_I2C_BUS EXTERNAL_I2C_BUS diff --git a/src/main/telemetry/sim.c b/src/main/telemetry/sim.c index 05a8cfbe87..7fda28cd08 100644 --- a/src/main/telemetry/sim.c +++ b/src/main/telemetry/sim.c @@ -125,14 +125,6 @@ static char* accEventDescriptions[] = { "", "HIT! ", "DROP ", "HIT " }; static char* modeDescriptions[] = { "MAN", "ACR", "AIR", "ANG", "HOR", "ALH", "POS", "RTH", "WP", "CRS", "LAU", "FS" }; static const char gpsFixIndicators[] = { '!', '*', ' ' }; - -// XXX UNUSED -#if 0 -static bool isGroundStationNumberDefined(void) { - return telemetryConfig()->simGroundStationNumber[0] != '\0'; -} -#endif - static bool checkGroundStationNumber(uint8_t* rv) { int i; @@ -465,16 +457,6 @@ void handleSimTelemetry() } } -// XXX UNUSED -#if 0 -static void freeSimTelemetryPort(void) -{ - closeSerialPort(simPort); - simPort = NULL; - simEnabled = false; -} -#endif - void initSimTelemetry(void) { portConfig = findSerialPortConfig(FUNCTION_TELEMETRY_SIM); diff --git a/src/test/unit/ws2811_unittest.cc.txt b/src/test/unit/ws2811_unittest.cc.txt index 5643529474..8b6b256d63 100644 --- a/src/test/unit/ws2811_unittest.cc.txt +++ b/src/test/unit/ws2811_unittest.cc.txt @@ -45,13 +45,7 @@ TEST(WS2812, updateDMABuffer) { dmaBufferOffset = 0; // when -#if 0 - updateLEDDMABuffer(color1.rgb.g); - updateLEDDMABuffer(color1.rgb.r); - updateLEDDMABuffer(color1.rgb.b); -#else fastUpdateLEDDMABuffer(&color1); -#endif // then EXPECT_EQ(24, dmaBufferOffset); diff --git a/src/utils/rename-ifdefs.py b/src/utils/rename-ifdefs.py index 095d1d55e6..314016148e 100644 --- a/src/utils/rename-ifdefs.py +++ b/src/utils/rename-ifdefs.py @@ -37,7 +37,6 @@ RENAMES = [ 'TELEMETRY_MAVLINK', 'TELEMETRY_SMARTPORT', 'TELEMETRY_CRSF', - 'PWM_DRIVER_PCA9685', 'PITOT', 'OSD', 'NAV',