`
* `` - ID of PID Controller, starting from `0`
diff --git a/docs/Screenshots/ADSB_TTSC01_settings.png b/docs/Screenshots/ADSB_TTSC01_settings.png
new file mode 100644
index 00000000000..b98b73c4453
Binary files /dev/null and b/docs/Screenshots/ADSB_TTSC01_settings.png differ
diff --git a/docs/Settings.md b/docs/Settings.md
index 817a6e2da28..34af8698bf4 100644
--- a/docs/Settings.md
+++ b/docs/Settings.md
@@ -1162,6 +1162,16 @@ S.Port telemetry: Send pitch and roll degrees*10 instead of raw accelerometer da
---
+### frsky_use_legacy_gps_mode_sensor_ids
+
+S.Port telemetry: If `ON`, send the legacy telemetry IDs for modes (Tmp1) and GNSS (Tmp2). These are old IDs, deprecated, and will be removed in INAV 10.0. Tools and scripts using these IDs should be updated to use the new IDs of **470** for Modes and **480** for GNSS. Default: 'OFF'
+
+| Default | Min | Max |
+| --- | --- | --- |
+| OFF | OFF | ON |
+
+---
+
### fw_autotune_max_rate_deflection
The target percentage of maximum mixer output used for determining the rates in `AUTO` and `LIMIT`.
@@ -3544,7 +3554,7 @@ Pitch Angle deadband when soaring mode enabled (deg). Angle mode inactive within
### nav_fw_wp_tracking_accuracy
-Waypoint tracking accuracy forces the craft to quickly head toward and track along the waypoint course line as closely as possible. Settings 1 to 10 adjust the course tracking response. Higher values dampen the response reducing possible overshoot. A value of 5 is a good starting point. Set to 0 to disable.
+Waypoint tracking accuracy forces the craft to quickly head toward and track along the waypoint course line as closely as possible. Setting adjusts tracking deadband distance fom waypoint courseline [m]. Tracking isn't actively controlled within the deadband providing smoother flight adjustments but less accurate tracking. A 2m deadband should work OK in most cases. Setting to 0 disables waypoint tracking accuracy.
| Default | Min | Max |
| --- | --- | --- |
@@ -3674,7 +3684,7 @@ Maximum speed allowed in fully autonomous modes (RTH, WP) [cm/s] [Multirotor onl
### nav_max_terrain_follow_alt
-Max allowed above the ground altitude for terrain following mode
+Max allowed above the ground altitude for terrain following mode [cm]
| Default | Min | Max |
| --- | --- | --- |
diff --git a/docs/Telemetry.md b/docs/Telemetry.md
index a33aa4ee527..d5d448f55c6 100644
--- a/docs/Telemetry.md
+++ b/docs/Telemetry.md
@@ -102,17 +102,21 @@ The following sensors are transmitted
* **VSpd** : vertical speed, unit is cm/s.
* **Hdg** : heading, North is 0°, South is 180°.
* **AccX,Y,Z** : accelerometer values (not sent if `frsky_pitch_roll = ON`).
-* **Tmp1** : flight mode, sent as 5 digits. Number is sent as **ABCDE** detailed below. The numbers are additives (for example: if digit C is 6, it means both position hold and altitude hold are active) :
+* **470** : flight mode, sent as 5 digits. Number is sent as **ABCDE** detailed below. The numbers are additives (for example: if digit C is 6, it means both position hold and altitude hold are active) :
* **A** : 1 = flaperon mode, 2 = auto tune mode, 4 = failsafe mode
* **B** : 1 = return to home, 2 = waypoint mode, 4 = headfree mode
* **C** : 1 = heading hold, 2 = altitude hold, 4 = position hold
* **D** : 1 = angle mode, 2 = horizon mode, 4 = passthru mode
* **E** : 1 = ok to arm, 2 = arming is prevented, 4 = armed
-* **Tmp2** : GPS lock status, accuracy, home reset trigger, and number of satellites. Number is sent as **ABCD** detailed below. Typical minimum GPS 3D lock value is 3906 (GPS locked and home fixed, HDOP highest accuracy, 6 satellites).
+
+ _NOTE_ This sensor used to be **Tmp1**. The ID has been reassigned in INAV 8.0. The old ID of **Tmp1** can still be used, by using `set frsky_use_legacy_gps_mode_sensor_ids = ON`. This is deprecated and will be removed in INAV 10.0. All tools and scripts using the old IDs should be updated to use the new ID.
+* **480** : GPS lock status, accuracy, home reset trigger, and number of satellites. Number is sent as **ABCD** detailed below. Typical minimum GPS 3D lock value is 3906 (GPS locked and home fixed, HDOP highest accuracy, 6 satellites).
* **A** : 1 = GPS fix, 2 = GPS home fix, 4 = home reset (numbers are additive)
* **B** : GPS accuracy based on HDOP (0 = lowest to 9 = highest accuracy)
* **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)
+
+ _NOTE_ This sensor used to be **Tmp2**. The ID has been reassigned in INAV 8.0. The old ID of **Tmp2** can still be used, by using `set frsky_use_legacy_gps_mode_sensor_ids = ON`. This is deprecated and will be removed in INAV 10.0. All tools and scripts using the old IDs should be updated to use the new ID.
* **GAlt** : GPS altitude, sea level is zero.
* **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)
diff --git a/docs/USB Flashing.md b/docs/USB Flashing.md
index 4b885ef02eb..97787c7fb48 100644
--- a/docs/USB Flashing.md
+++ b/docs/USB Flashing.md
@@ -49,14 +49,19 @@ With the board connected and in bootloader mode (reset it by sending the charact
* Choose Options > List All Devices
* Select `STM32 BOOTLOADER` in the device list
* Choose `WinUSB (v6.x.x.x)` in the right hand box
-
+
![Zadig Driver Procedure](assets/images/zadig-dfu.png)
* Click Replace Driver
* Restart the Configurator (make sure it is completely closed, logout and login if unsure)
* Now the DFU device should be seen by Configurator
+## While Using USB-C cables
+* If you are using a device with only USB-C ports such as a Mac-OS device, you will need a dongle.
+ * A USB-C to USB-C cable is identical on both ends and thus requires extra hardware to let them be auto detected as devices instead of hosts.
+ * Using either a hub with USB-A ports, or a USB-A to C cable or dongle is usually the easiest way to get a working connection but an USB-OTG adapter also works.
+
## Using `dfu-util`
`dfu-util` is a command line tool to flash ARM devices via DFU. It is available via the package manager on most Linux systems or from [source forge](http://sourceforge.net/p/dfu-util).
diff --git a/docs/VTOL.md b/docs/VTOL.md
index 64afb333d11..3873eeb4e3f 100644
--- a/docs/VTOL.md
+++ b/docs/VTOL.md
@@ -186,7 +186,8 @@ Add new servo mixer rules, and select 'Mixer Transition' in input. Set the weigh
## Motor 'Transition Mixing': Dedicated forward motor configuration
In motor mixer set:
- -2.0 < throttle < -1.0: The motor will spin regardless of the radio's throttle position at a speed of `abs(throttle) - 1` multiplied by throttle range only when Mixer Transition is activated.
-
+- Airmode type should be set to "STICK_CENTER". Airmode type must NOT be set to "THROTTLE_THRESHOLD". If set to throttle threshold the (-) motor will spin till throttle threshold is passed.
+
![Alt text](Screenshots/mixerprofile_4puls1_mix.png)
## TailSitter 'Transition Mixing':
@@ -233,4 +234,4 @@ If you set `mixer_automated_switch` to `OFF` for all mixer profiles (the default
2. In addition to 1. Add a little yaw mixing(about 0.2) in tilt motors.
- There will be a time window that tilting motors is providing up lift but rear motor isn't. Result in a sudden pitch raise on the entering of the mode. Use the max speed or faster speed in tiling servo to reduce the time window. OR lower the throttle on the entering of the FW mode to mitigate the effect.
## Dedicated forward motor
-- Easiest way to setup a vtol. and efficiency can be improved by using different motor/prop for hover and forward flight
\ No newline at end of file
+- Easiest way to setup a vtol. and efficiency can be improved by using different motor/prop for hover and forward flight
diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c
index 324b41a1095..4341ea0c34d 100644
--- a/src/main/blackbox/blackbox.c
+++ b/src/main/blackbox/blackbox.c
@@ -99,7 +99,7 @@
#define BLACKBOX_INVERTED_CARD_DETECTION 0
#endif
-PG_REGISTER_WITH_RESET_TEMPLATE(blackboxConfig_t, blackboxConfig, PG_BLACKBOX_CONFIG, 2);
+PG_REGISTER_WITH_RESET_TEMPLATE(blackboxConfig_t, blackboxConfig, PG_BLACKBOX_CONFIG, 3);
PG_RESET_TEMPLATE(blackboxConfig_t, blackboxConfig,
.device = DEFAULT_BLACKBOX_DEVICE,
@@ -108,7 +108,8 @@ PG_RESET_TEMPLATE(blackboxConfig_t, blackboxConfig,
.invertedCardDetection = BLACKBOX_INVERTED_CARD_DETECTION,
.includeFlags = BLACKBOX_FEATURE_NAV_PID | BLACKBOX_FEATURE_NAV_POS |
BLACKBOX_FEATURE_MAG | BLACKBOX_FEATURE_ACC | BLACKBOX_FEATURE_ATTITUDE |
- BLACKBOX_FEATURE_RC_DATA | BLACKBOX_FEATURE_RC_COMMAND | BLACKBOX_FEATURE_MOTORS,
+ BLACKBOX_FEATURE_RC_DATA | BLACKBOX_FEATURE_RC_COMMAND |
+ BLACKBOX_FEATURE_MOTORS | BLACKBOX_FEATURE_SERVOS,
);
void blackboxIncludeFlagSet(uint32_t mask)
@@ -334,22 +335,43 @@ static const blackboxDeltaFieldDefinition_t blackboxMainFields[] = {
{"motor", 7, UNSIGNED, .Ipredict = PREDICT(MOTOR_0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_MOTORS_8)},
/* servos */
- {"servo", 0, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(SERVOS)},
- {"servo", 1, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(SERVOS)},
- {"servo", 2, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(SERVOS)},
- {"servo", 3, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(SERVOS)},
- {"servo", 4, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(SERVOS)},
- {"servo", 5, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(SERVOS)},
- {"servo", 6, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(SERVOS)},
- {"servo", 7, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(SERVOS)},
- {"servo", 8, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(SERVOS)},
- {"servo", 9, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(SERVOS)},
- {"servo", 10, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(SERVOS)},
- {"servo", 11, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(SERVOS)},
- {"servo", 12, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(SERVOS)},
- {"servo", 13, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(SERVOS)},
- {"servo", 14, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(SERVOS)},
- {"servo", 15, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(SERVOS)},
+ {"servo", 0, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_1)},
+ {"servo", 1, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_2)},
+ {"servo", 2, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_3)},
+ {"servo", 3, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_4)},
+ {"servo", 4, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_5)},
+ {"servo", 5, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_6)},
+ {"servo", 6, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_7)},
+ {"servo", 7, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_8)},
+ {"servo", 8, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_9)},
+ {"servo", 9, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_10)},
+ {"servo", 10, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_11)},
+ {"servo", 11, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_12)},
+ {"servo", 12, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_13)},
+ {"servo", 13, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_14)},
+ {"servo", 14, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_15)},
+ {"servo", 15, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_16)},
+ {"servo", 16, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_17)},
+ {"servo", 17, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_18)},
+ {"servo", 18, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_19)},
+ {"servo", 19, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_20)},
+ {"servo", 20, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_21)},
+ {"servo", 21, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_22)},
+ {"servo", 22, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_23)},
+ {"servo", 23, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_24)},
+ {"servo", 24, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_25)},
+ {"servo", 25, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_26)},
+ /*
+ {"servo", 26, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_27)},
+ {"servo", 27, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_28)},
+ {"servo", 27, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_29)},
+ {"servo", 28, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_30)},
+ {"servo", 29, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_31)},
+ {"servo", 30, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_32)},
+ {"servo", 31, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_33)},
+ {"servo", 32, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_34)},
+ {"servo", 33, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_35)},
+ */
{"navState", -1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
{"navFlags", -1, UNSIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
@@ -406,7 +428,7 @@ static const blackboxSimpleFieldDefinition_t blackboxSlowFields[] = {
* but name kept for external compatibility reasons.
* "activeFlightModeFlags" logs actual active flight modes rather than rc boxmodes.
* 'active' should at least distinguish it from the existing "flightModeFlags" */
-
+
{"activeWpNumber", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)},
{"flightModeFlags", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)},
{"flightModeFlags2", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)},
@@ -652,7 +674,45 @@ static bool testBlackboxConditionUncached(FlightLogFieldCondition condition)
return (getMotorCount() >= condition - FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_1 + 1) && blackboxIncludeFlag(BLACKBOX_FEATURE_MOTORS);
case FLIGHT_LOG_FIELD_CONDITION_SERVOS:
- return isMixerUsingServos();
+ return blackboxIncludeFlag(BLACKBOX_FEATURE_SERVOS) && isMixerUsingServos();
+
+ case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_1:
+ case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_2:
+ case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_3:
+ case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_4:
+ case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_5:
+ case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_6:
+ case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_7:
+ case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_8:
+ case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_9:
+ case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_10:
+ case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_11:
+ case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_12:
+ case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_13:
+ case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_14:
+ case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_15:
+ case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_16:
+ case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_17:
+ case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_18:
+ case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_19:
+ case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_20:
+ case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_21:
+ case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_22:
+ case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_23:
+ case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_24:
+ case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_25:
+ case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_26:
+ /*
+ case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_27:
+ case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_28:
+ case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_29:
+ case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_30:
+ case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_31:
+ case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_32:
+ case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_33:
+ case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_34:
+ */
+ return ((FlightLogFieldCondition)getServoCount() >= condition - FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_1 + 1) && blackboxIncludeFlag(BLACKBOX_FEATURE_SERVOS);
case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0:
case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_1:
@@ -953,7 +1013,8 @@ static void writeIntraframe(void)
}
if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_SERVOS)) {
- for (int x = 0; x < MAX_SUPPORTED_SERVOS; x++) {
+ const int servoCount = getServoCount();
+ for (int x = 0; x < servoCount; x++) {
//Assume that servos spends most of its time around the center
blackboxWriteSignedVB(blackboxCurrent->servo[x] - 1500);
}
@@ -1212,7 +1273,7 @@ static void writeInterframe(void)
}
if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_SERVOS)) {
- blackboxWriteArrayUsingAveragePredictor16(offsetof(blackboxMainState_t, servo), MAX_SUPPORTED_SERVOS);
+ blackboxWriteArrayUsingAveragePredictor16(offsetof(blackboxMainState_t, servo), getServoCount());
}
blackboxWriteSignedVB(blackboxCurrent->navState - blackboxLast->navState);
@@ -1680,7 +1741,8 @@ static void loadMainState(timeUs_t currentTimeUs)
blackboxCurrent->rssi = getRSSI();
- for (int i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
+ const int servoCount = getServoCount();
+ for (int i = 0; i < servoCount; i++) {
blackboxCurrent->servo[i] = servo[i];
}
diff --git a/src/main/blackbox/blackbox.h b/src/main/blackbox/blackbox.h
index ea7482cbcd5..a329c11df12 100644
--- a/src/main/blackbox/blackbox.h
+++ b/src/main/blackbox/blackbox.h
@@ -35,6 +35,7 @@ typedef enum {
BLACKBOX_FEATURE_GYRO_PEAKS_ROLL = 1 << 10,
BLACKBOX_FEATURE_GYRO_PEAKS_PITCH = 1 << 11,
BLACKBOX_FEATURE_GYRO_PEAKS_YAW = 1 << 12,
+ BLACKBOX_FEATURE_SERVOS = 1 << 13,
} blackboxFeatureMask_e;
typedef struct blackboxConfig_s {
uint16_t rate_num;
@@ -55,4 +56,4 @@ void blackboxFinish(void);
bool blackboxMayEditConfig(void);
void blackboxIncludeFlagSet(uint32_t mask);
void blackboxIncludeFlagClear(uint32_t mask);
-bool blackboxIncludeFlag(uint32_t mask);
\ No newline at end of file
+bool blackboxIncludeFlag(uint32_t mask);
diff --git a/src/main/blackbox/blackbox_fielddefs.h b/src/main/blackbox/blackbox_fielddefs.h
index 1e5615e3ec4..17595157dd2 100644
--- a/src/main/blackbox/blackbox_fielddefs.h
+++ b/src/main/blackbox/blackbox_fielddefs.h
@@ -33,7 +33,44 @@ typedef enum FlightLogFieldCondition {
FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_6,
FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_7,
FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_8,
+
FLIGHT_LOG_FIELD_CONDITION_SERVOS,
+ FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_1,
+ FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_2,
+ FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_3,
+ FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_4,
+ FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_5,
+ FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_6,
+ FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_7,
+ FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_8,
+ FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_9,
+ FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_10,
+ FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_11,
+ FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_12,
+ FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_13,
+ FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_14,
+ FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_15,
+ FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_16,
+ FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_17,
+ FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_18,
+ FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_19,
+ FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_20,
+ FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_21,
+ FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_22,
+ FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_23,
+ FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_24,
+ FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_25,
+ FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_26,
+ /*
+ FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_27,
+ FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_28,
+ FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_29,
+ FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_30,
+ FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_31,
+ FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_32,
+ FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_33,
+ FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_34,
+ */
FLIGHT_LOG_FIELD_CONDITION_MAG,
FLIGHT_LOG_FIELD_CONDITION_BARO,
diff --git a/src/main/drivers/headtracker_common.c b/src/main/drivers/headtracker_common.c
index feec581da8a..8ea71cd2f66 100644
--- a/src/main/drivers/headtracker_common.c
+++ b/src/main/drivers/headtracker_common.c
@@ -157,10 +157,6 @@ void taskUpdateHeadTracker(timeUs_t currentTimeUs)
#else
void taskUpdateHeadTracker(timeUs_t currentTimeUs)
{
- if (cliMode) {
- return;
- }
-
headTrackerDevice_t *headTrackerDevice = headTrackerCommonDevice();
if(headTrackerDevice) {
diff --git a/src/main/drivers/headtracker_common.h b/src/main/drivers/headtracker_common.h
index 8c177e3e0f0..92dac7e61e6 100644
--- a/src/main/drivers/headtracker_common.h
+++ b/src/main/drivers/headtracker_common.h
@@ -23,7 +23,6 @@
#define HEADTRACKER_RANGE_MIN -2048
#define HEADTRACKER_RANGE_MAX 2047
-#ifdef USE_HEADTRACKER
#include
@@ -81,6 +80,8 @@ typedef struct headTrackerConfig_s {
float roll_ratio;
} headTrackerConfig_t;
+#ifdef USE_HEADTRACKER
+
PG_DECLARE(headTrackerConfig_t, headTrackerConfig);
void headTrackerCommonInit(void);
diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c
index 5ed17d7bb9a..2d01127a504 100644
--- a/src/main/drivers/pwm_mapping.c
+++ b/src/main/drivers/pwm_mapping.c
@@ -42,6 +42,7 @@
#include "sensors/rangefinder.h"
#include "io/serial.h"
+#include "io/servo_sbus.h"
enum {
MAP_TO_NONE,
@@ -442,15 +443,21 @@ static void pwmInitServos(timMotorServoHardware_t * timOutputs)
return;
}
+
// If mixer requests more servos than we have timer outputs - throw an error
- if (servoCount > timOutputs->maxTimServoCount) {
+ uint16_t maxServos = timOutputs->maxTimServoCount;
+ if (servoConfig()->servo_protocol == SERVO_TYPE_SBUS_PWM) {
+ maxServos = MAX(SERVO_SBUS_MAX_SERVOS, timOutputs->maxTimServoCount);
+ }
+
+ if (servoCount > maxServos) {
pwmInitError = PWM_INIT_ERROR_NOT_ENOUGH_SERVO_OUTPUTS;
LOG_ERROR(PWM, "Too many servos. Mixer requested %d, timer outputs %d", servoCount, timOutputs->maxTimServoCount);
return;
}
// Configure individual servo outputs
- for (int idx = 0; idx < servoCount; idx++) {
+ for (int idx = 0; idx < MIN(servoCount, timOutputs->maxTimServoCount); idx++) {
const timerHardware_t *timHw = timOutputs->timServos[idx];
if (!pwmServoConfig(timHw, idx, servoConfig()->servoPwmRate, servoConfig()->servoCenterPulse, feature(FEATURE_PWM_OUTPUT_ENABLE))) {
diff --git a/src/main/drivers/pwm_mapping.h b/src/main/drivers/pwm_mapping.h
index 08123130f2c..3f08d9b500a 100644
--- a/src/main/drivers/pwm_mapping.h
+++ b/src/main/drivers/pwm_mapping.h
@@ -21,6 +21,7 @@
#include "flight/mixer.h"
#include "flight/mixer_profile.h"
#include "flight/servos.h"
+#include "common/maths.h"
#if defined(TARGET_MOTOR_COUNT)
#define MAX_MOTORS TARGET_MOTOR_COUNT
diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c
index faa9cd373d9..619f4b95db5 100644
--- a/src/main/drivers/pwm_output.c
+++ b/src/main/drivers/pwm_output.c
@@ -638,7 +638,7 @@ ioTag_t pwmGetMotorPinTag(int motorIndex)
static void pwmServoWriteStandard(uint8_t index, uint16_t value)
{
- if (servos[index]) {
+ if (index < MAX_SERVOS && servos[index]) {
*servos[index]->ccr = value;
}
}
diff --git a/src/main/drivers/serial.c b/src/main/drivers/serial.c
index 165f9c31698..dc625aaa354 100644
--- a/src/main/drivers/serial.c
+++ b/src/main/drivers/serial.c
@@ -86,6 +86,11 @@ void serialSetMode(serialPort_t *instance, portMode_t mode)
instance->vTable->setMode(instance, mode);
}
+void serialSetOptions(serialPort_t *instance, portOptions_t options)
+{
+ instance->vTable->setOptions(instance, options);
+}
+
void serialWriteBufShim(void *instance, const uint8_t *data, int count)
{
serialWriteBuf((serialPort_t *)instance, data, count);
diff --git a/src/main/drivers/serial.h b/src/main/drivers/serial.h
index fcb787ded99..8e66b5f8445 100644
--- a/src/main/drivers/serial.h
+++ b/src/main/drivers/serial.h
@@ -95,6 +95,8 @@ struct serialPortVTable {
void (*setMode)(serialPort_t *instance, portMode_t mode);
+ void (*setOptions)(serialPort_t *instance, portOptions_t options);
+
void (*writeBuf)(serialPort_t *instance, const void *data, int count);
bool (*isConnected)(const serialPort_t *instance);
@@ -113,6 +115,7 @@ void serialWriteBuf(serialPort_t *instance, const uint8_t *data, int count);
uint8_t serialRead(serialPort_t *instance);
void serialSetBaudRate(serialPort_t *instance, uint32_t baudRate);
void serialSetMode(serialPort_t *instance, portMode_t mode);
+void serialSetOptions(serialPort_t *instance, portOptions_t options);
bool isSerialTransmitBufferEmpty(const serialPort_t *instance);
void serialPrint(serialPort_t *instance, const char *str);
uint32_t serialGetBaudRate(serialPort_t *instance);
diff --git a/src/main/drivers/serial_softserial.c b/src/main/drivers/serial_softserial.c
index 7ce128516f0..09352f4d61b 100644
--- a/src/main/drivers/serial_softserial.c
+++ b/src/main/drivers/serial_softserial.c
@@ -623,6 +623,11 @@ void softSerialSetMode(serialPort_t *instance, portMode_t mode)
instance->mode = mode;
}
+void softSerialSetOptions(serialPort_t *instance, portOptions_t options)
+{
+ instance->options = options;
+}
+
bool isSoftSerialTransmitBufferEmpty(const serialPort_t *instance)
{
return instance->txBufferHead == instance->txBufferTail;
@@ -636,6 +641,7 @@ static const struct serialPortVTable softSerialVTable = {
.serialSetBaudRate = softSerialSetBaudRate,
.isSerialTransmitBufferEmpty = isSoftSerialTransmitBufferEmpty,
.setMode = softSerialSetMode,
+ .setOptions = softSerialSetOptions,
.isConnected = NULL,
.writeBuf = NULL,
.beginWrite = NULL,
diff --git a/src/main/drivers/serial_tcp.c b/src/main/drivers/serial_tcp.c
index 765f8308cd3..915f2a53605 100644
--- a/src/main/drivers/serial_tcp.c
+++ b/src/main/drivers/serial_tcp.c
@@ -317,6 +317,12 @@ void tcpSetMode(serialPort_t *instance, portMode_t mode)
UNUSED(mode);
}
+void tcpSetOptions(serialPort_t *instance, portOptions_t options)
+{
+ UNUSED(instance);
+ UNUSED(options);
+}
+
static const struct serialPortVTable tcpVTable[] = {
{
.serialWrite = tcpWrite,
@@ -326,6 +332,7 @@ static const struct serialPortVTable tcpVTable[] = {
.serialSetBaudRate = tcpSetBaudRate,
.isSerialTransmitBufferEmpty = isTcpTransmitBufferEmpty,
.setMode = tcpSetMode,
+ .setOptions = tcpSetOptions,
.isConnected = tcpIsConnected,
.writeBuf = tcpWritBuf,
.beginWrite = NULL,
diff --git a/src/main/drivers/serial_uart.c b/src/main/drivers/serial_uart.c
index 7b9bbd7128e..9307b0cab8b 100644
--- a/src/main/drivers/serial_uart.c
+++ b/src/main/drivers/serial_uart.c
@@ -175,6 +175,13 @@ void uartSetMode(serialPort_t *instance, portMode_t mode)
uartReconfigure(uartPort);
}
+void uartSetOptions(serialPort_t *instance, portOptions_t options)
+{
+ uartPort_t *uartPort = (uartPort_t *)instance;
+ uartPort->port.options = options;
+ uartReconfigure(uartPort);
+}
+
uint32_t uartTotalRxBytesWaiting(const serialPort_t *instance)
{
const uartPort_t *s = (const uartPort_t*)instance;
@@ -255,6 +262,7 @@ const struct serialPortVTable uartVTable[] = {
.serialSetBaudRate = uartSetBaudRate,
.isSerialTransmitBufferEmpty = isUartTransmitBufferEmpty,
.setMode = uartSetMode,
+ .setOptions = uartSetOptions,
.isConnected = NULL,
.writeBuf = NULL,
.beginWrite = NULL,
diff --git a/src/main/drivers/serial_uart.h b/src/main/drivers/serial_uart.h
index b8e72026128..4c1e6d57508 100644
--- a/src/main/drivers/serial_uart.h
+++ b/src/main/drivers/serial_uart.h
@@ -50,7 +50,8 @@ typedef enum {
UARTDEV_5 = 4,
UARTDEV_6 = 5,
UARTDEV_7 = 6,
- UARTDEV_8 = 7
+ UARTDEV_8 = 7,
+ UARTDEV_MAX
} UARTDevice_e;
typedef struct {
@@ -69,6 +70,7 @@ typedef struct {
void uartGetPortPins(UARTDevice_e device, serialPortPins_t * pins);
void uartClearIdleFlag(uartPort_t *s);
+void uartConfigurePinSwap(uartPort_t *uartPort);
#if defined(AT32F43x)
serialPort_t *uartOpen(usart_type *USARTx, serialReceiveCallbackPtr rxCallback, void *rxCallbackData, uint32_t baudRate, portMode_t mode, portOptions_t options);
#else
diff --git a/src/main/drivers/serial_uart_at32f43x.c b/src/main/drivers/serial_uart_at32f43x.c
index 354ea6c9343..ad4b4e0c458 100644
--- a/src/main/drivers/serial_uart_at32f43x.c
+++ b/src/main/drivers/serial_uart_at32f43x.c
@@ -45,6 +45,7 @@ typedef struct uartDevice_s {
uint8_t af;
uint8_t irq;
uint32_t irqPriority;
+ bool pinSwap;
} uartDevice_t;
#ifdef USE_UART1
@@ -53,13 +54,22 @@ static uartDevice_t uart1 =
.dev = USART1,
.rx = IO_TAG(UART1_RX_PIN),
.tx = IO_TAG(UART1_TX_PIN),
+#ifdef UART1_AF
+ .af = CONCAT(GPIO_MUX_, UART1_AF),
+#else
.af = GPIO_MUX_7,
+#endif
#ifdef UART1_AHB1_PERIPHERALS
.rcc_ahb1 = UART1_AHB1_PERIPHERALS,
#endif
.rcc_apb2 = RCC_APB2(USART1),
.irq = USART1_IRQn,
- .irqPriority = NVIC_PRIO_SERIALUART
+ .irqPriority = NVIC_PRIO_SERIALUART,
+#ifdef USE_UART1_PIN_SWAP
+ .pinSwap = true,
+#else
+ .pinSwap = false,
+#endif
};
#endif
@@ -69,13 +79,22 @@ static uartDevice_t uart2 =
.dev = USART2,
.rx = IO_TAG(UART2_RX_PIN),
.tx = IO_TAG(UART2_TX_PIN),
+#ifdef UART2_AF
+ .af = CONCAT(GPIO_MUX_, UART2_AF),
+#else
.af = GPIO_MUX_7,
+#endif
#ifdef UART2_AHB1_PERIPHERALS
.rcc_ahb1 = UART2_AHB1_PERIPHERALS,
#endif
.rcc_apb1 = RCC_APB1(USART2),
.irq = USART2_IRQn,
- .irqPriority = NVIC_PRIO_SERIALUART
+ .irqPriority = NVIC_PRIO_SERIALUART,
+#ifdef USE_UART2_PIN_SWAP
+ .pinSwap = true,
+#else
+ .pinSwap = false,
+#endif
};
#endif
@@ -85,13 +104,22 @@ static uartDevice_t uart3 =
.dev = USART3,
.rx = IO_TAG(UART3_RX_PIN),
.tx = IO_TAG(UART3_TX_PIN),
+#ifdef UART3_AF
+ .af = CONCAT(GPIO_MUX_, UART3_AF),
+#else
.af = GPIO_MUX_7,
+#endif
#ifdef UART3_AHB1_PERIPHERALS
.rcc_ahb1 = UART3_AHB1_PERIPHERALS,
#endif
.rcc_apb1 = RCC_APB1(USART3),
.irq = USART3_IRQn,
- .irqPriority = NVIC_PRIO_SERIALUART
+ .irqPriority = NVIC_PRIO_SERIALUART,
+#ifdef USE_UART3_PIN_SWAP
+ .pinSwap = true,
+#else
+ .pinSwap = false,
+#endif
};
#endif
@@ -101,13 +129,22 @@ static uartDevice_t uart4 =
.dev = UART4,
.rx = IO_TAG(UART4_RX_PIN),
.tx = IO_TAG(UART4_TX_PIN),
+#ifdef UART4_AF
+ .af = CONCAT(GPIO_MUX_, UART4_AF),
+#else
.af = GPIO_MUX_8,
+#endif
#ifdef UART4_AHB1_PERIPHERALS
.rcc_ahb1 = UART4_AHB1_PERIPHERALS,
#endif
.rcc_apb1 = RCC_APB1(UART4),
.irq = UART4_IRQn,
- .irqPriority = NVIC_PRIO_SERIALUART
+ .irqPriority = NVIC_PRIO_SERIALUART,
+#ifdef USE_UART4_PIN_SWAP
+ .pinSwap = true,
+#else
+ .pinSwap = false,
+#endif
};
#endif
@@ -117,13 +154,22 @@ static uartDevice_t uart5 =
.dev = UART5,
.rx = IO_TAG(UART5_RX_PIN),
.tx = IO_TAG(UART5_TX_PIN),
+#ifdef UART5_AF
+ .af = CONCAT(GPIO_MUX_, UART5_AF),
+#else
.af = GPIO_MUX_8,
+#endif
#ifdef UART5_AHB1_PERIPHERALS
.rcc_ahb1 = UART5_AHB1_PERIPHERALS,
#endif
.rcc_apb1 = RCC_APB1(UART5),
.irq = UART5_IRQn,
- .irqPriority = NVIC_PRIO_SERIALUART
+ .irqPriority = NVIC_PRIO_SERIALUART,
+#ifdef USE_UART5_PIN_SWAP
+ .pinSwap = true,
+#else
+ .pinSwap = false,
+#endif
};
#endif
@@ -133,13 +179,22 @@ static uartDevice_t uart6 =
.dev = USART6,
.rx = IO_TAG(UART6_RX_PIN),
.tx = IO_TAG(UART6_TX_PIN),
+#ifdef UART6_AF
+ .af = CONCAT(GPIO_MUX_, UART6_AF),
+#else
.af = GPIO_MUX_8,
+#endif
#ifdef UART6_AHB1_PERIPHERALS
.rcc_ahb1 = UART6_AHB1_PERIPHERALS,
#endif
.rcc_apb2 = RCC_APB2(USART6),
.irq = USART6_IRQn,
- .irqPriority = NVIC_PRIO_SERIALUART
+ .irqPriority = NVIC_PRIO_SERIALUART,
+#ifdef USE_UART6_PIN_SWAP
+ .pinSwap = true,
+#else
+ .pinSwap = false,
+#endif
};
#endif
@@ -149,10 +204,19 @@ static uartDevice_t uart7 =
.dev = UART7,
.rx = IO_TAG(UART7_RX_PIN),
.tx = IO_TAG(UART7_TX_PIN),
+#ifdef UART7_AF
+ .af = CONCAT(GPIO_MUX_, UART7_AF),
+#else
.af = GPIO_MUX_8,
+#endif
.rcc_apb1 = RCC_APB1(UART7),
.irq = UART7_IRQn,
- .irqPriority = NVIC_PRIO_SERIALUART
+ .irqPriority = NVIC_PRIO_SERIALUART,
+#ifdef USE_UART7_PIN_SWAP
+ .pinSwap = true,
+#else
+ .pinSwap = false,
+#endif
};
#endif
@@ -162,10 +226,19 @@ static uartDevice_t uart8 =
.dev = UART8,
.rx = IO_TAG(UART8_RX_PIN),
.tx = IO_TAG(UART8_TX_PIN),
+#ifdef UART8_AF
+ .af = CONCAT(GPIO_MUX_, UART8_AF),
+#else
.af = GPIO_MUX_8,
+#endif
.rcc_apb1 = RCC_APB1(UART8),
.irq = UART8_IRQn,
- .irqPriority = NVIC_PRIO_SERIALUART
+ .irqPriority = NVIC_PRIO_SERIALUART,
+#ifdef USE_UART8_PIN_SWAP
+ .pinSwap = true,
+#else
+ .pinSwap = false,
+#endif
};
#endif
@@ -258,6 +331,30 @@ void uartClearIdleFlag(uartPort_t *s)
(void) s->USARTx->dt;
}
+static uartDevice_t *uartFindDevice(uartPort_t *uartPort)
+{
+ for (uint32_t i = 0; i < UARTDEV_MAX; i++) {
+ uartDevice_t *pDevice = uartHardwareMap[i];
+
+ if (pDevice->dev == uartPort->USARTx) {
+ return pDevice;
+ }
+ }
+ return NULL;
+}
+
+void uartConfigurePinSwap(uartPort_t *uartPort)
+{
+ uartDevice_t *uartDevice = uartFindDevice(uartPort);
+ if (!uartDevice) {
+ return;
+ }
+
+ if (uartDevice->pinSwap) {
+ usart_transmit_receive_pin_swap(uartPort->USARTx, TRUE);
+ }
+}
+
uartPort_t *serialUART(UARTDevice_e device, uint32_t baudRate, portMode_t mode, portOptions_t options)
{
uartPort_t *s;
diff --git a/src/main/drivers/serial_uart_hal.c b/src/main/drivers/serial_uart_hal.c
index b1df6ed7541..cce38422848 100644
--- a/src/main/drivers/serial_uart_hal.c
+++ b/src/main/drivers/serial_uart_hal.c
@@ -185,6 +185,13 @@ void uartSetMode(serialPort_t *instance, portMode_t mode)
uartReconfigure(uartPort);
}
+void uartSetOptions(serialPort_t *instance, portOptions_t options)
+{
+ uartPort_t *uartPort = (uartPort_t *)instance;
+ uartPort->port.options = options;
+ uartReconfigure(uartPort);
+}
+
uint32_t uartTotalRxBytesWaiting(const serialPort_t *instance)
{
uartPort_t *s = (uartPort_t*)instance;
@@ -266,6 +273,7 @@ const struct serialPortVTable uartVTable[] = {
.serialSetBaudRate = uartSetBaudRate,
.isSerialTransmitBufferEmpty = isUartTransmitBufferEmpty,
.setMode = uartSetMode,
+ .setOptions = uartSetOptions,
.isConnected = NULL,
.writeBuf = NULL,
.beginWrite = NULL,
diff --git a/src/main/drivers/serial_uart_hal_at32f43x.c b/src/main/drivers/serial_uart_hal_at32f43x.c
index 0e0f11b1c13..f30726332f4 100644
--- a/src/main/drivers/serial_uart_hal_at32f43x.c
+++ b/src/main/drivers/serial_uart_hal_at32f43x.c
@@ -85,6 +85,7 @@ static void uartReconfigure(uartPort_t *uartPort)
usart_transmitter_enable(uartPort->USARTx, TRUE);
usartConfigurePinInversion(uartPort);
+ uartConfigurePinSwap(uartPort);
if (uartPort->port.options & SERIAL_BIDIR)
usart_single_line_halfduplex_select(uartPort->USARTx, TRUE);
@@ -178,6 +179,13 @@ void uartSetMode(serialPort_t *instance, portMode_t mode)
uartReconfigure(uartPort);
}
+void uartSetOptions(serialPort_t *instance, portOptions_t options)
+{
+ uartPort_t *uartPort = (uartPort_t *)instance;
+ uartPort->port.options = options;
+ uartReconfigure(uartPort);
+}
+
uint32_t uartTotalRxBytesWaiting(const serialPort_t *instance)
{
const uartPort_t *s = (const uartPort_t*)instance;
@@ -260,6 +268,7 @@ const struct serialPortVTable uartVTable[] = {
.serialSetBaudRate = uartSetBaudRate,
.isSerialTransmitBufferEmpty = isUartTransmitBufferEmpty,
.setMode = uartSetMode,
+ .setOptions = uartSetOptions,
.isConnected = NULL,
.writeBuf = NULL,
.beginWrite = NULL,
diff --git a/src/main/drivers/serial_usb_vcp.c b/src/main/drivers/serial_usb_vcp.c
index a90633d6cfd..7fdbad2a114 100644
--- a/src/main/drivers/serial_usb_vcp.c
+++ b/src/main/drivers/serial_usb_vcp.c
@@ -67,6 +67,14 @@ static void usbVcpSetMode(serialPort_t *instance, portMode_t mode)
// TODO implement
}
+static void usbVcpSetOptions(serialPort_t *instance, portOptions_t options)
+{
+ UNUSED(instance);
+ UNUSED(options);
+
+ // TODO implement
+}
+
static bool isUsbVcpTransmitBufferEmpty(const serialPort_t *instance)
{
UNUSED(instance);
@@ -184,6 +192,7 @@ static const struct serialPortVTable usbVTable[] = {
.serialSetBaudRate = usbVcpSetBaudRate,
.isSerialTransmitBufferEmpty = isUsbVcpTransmitBufferEmpty,
.setMode = usbVcpSetMode,
+ .setOptions = usbVcpSetOptions,
.isConnected = usbVcpIsConnected,
.writeBuf = usbVcpWriteBuf,
.beginWrite = usbVcpBeginWrite,
diff --git a/src/main/drivers/serial_usb_vcp_at32f43x.c b/src/main/drivers/serial_usb_vcp_at32f43x.c
index 29b96d1a2b6..96b283ec363 100644
--- a/src/main/drivers/serial_usb_vcp_at32f43x.c
+++ b/src/main/drivers/serial_usb_vcp_at32f43x.c
@@ -308,6 +308,12 @@ static void usbVcpSetMode(serialPort_t *instance, portMode_t mode)
UNUSED(mode);
}
+static void usbVcpSetOptions(serialPort_t *instance, portOptions_t options)
+{
+ UNUSED(instance);
+ UNUSED(options);
+}
+
static bool isUsbVcpTransmitBufferEmpty(const serialPort_t *instance)
{
UNUSED(instance);
@@ -434,6 +440,7 @@ static const struct serialPortVTable usbVTable[] = {
.serialSetBaudRate = usbVcpSetBaudRate,
.isSerialTransmitBufferEmpty = isUsbVcpTransmitBufferEmpty,
.setMode = usbVcpSetMode,
+ .setOptions = usbVcpSetOptions,
.isConnected = usbVcpIsConnected,
.writeBuf = usbVcpWriteBuf,
.beginWrite = usbVcpBeginWrite,
diff --git a/src/main/drivers/time.h b/src/main/drivers/time.h
index 6901d57c19b..ea30592515e 100644
--- a/src/main/drivers/time.h
+++ b/src/main/drivers/time.h
@@ -19,6 +19,9 @@
#include
+#ifdef __cplusplus
+extern "C" {
+#endif
#include "common/time.h"
extern uint32_t usTicks;
@@ -32,3 +35,7 @@ timeUs_t microsISR(void);
timeMs_t millis(void);
uint32_t ticks(void);
+
+#ifdef __cplusplus
+}
+#endif
\ No newline at end of file
diff --git a/src/main/drivers/vtx_common.h b/src/main/drivers/vtx_common.h
index 021d17f24de..80b957c5d13 100644
--- a/src/main/drivers/vtx_common.h
+++ b/src/main/drivers/vtx_common.h
@@ -34,7 +34,7 @@
#if defined(USE_VTX_SMARTAUDIO) || defined(USE_VTX_TRAMP) || defined(USE_VTX_MSP)
-#define VTX_SETTINGS_POWER_COUNT 5
+#define VTX_SETTINGS_POWER_COUNT 8
#define VTX_SETTINGS_DEFAULT_POWER 1
#define VTX_SETTINGS_MIN_POWER 1
#define VTX_SETTINGS_MIN_USER_FREQ 5000
diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c
index 0ea43fd0ce7..ee5cb289a94 100644
--- a/src/main/fc/cli.c
+++ b/src/main/fc/cli.c
@@ -183,6 +183,7 @@ static const char * const blackboxIncludeFlagNames[] = {
"PEAKS_R",
"PEAKS_P",
"PEAKS_Y",
+ "SERVOS",
NULL
};
#endif
@@ -913,6 +914,42 @@ static void cliSerial(char *cmdline)
}
#ifdef USE_SERIAL_PASSTHROUGH
+
+portOptions_t constructPortOptions(char *options) {
+ if (strlen(options) != 3 || options[0] != '8') {
+ // Invalid format
+ return -1;
+ }
+
+ portOptions_t result = 0;
+
+ switch (options[1]) {
+ case 'N':
+ result |= SERIAL_PARITY_NO;
+ break;
+ case 'E':
+ result |= SERIAL_PARITY_EVEN;
+ break;
+ default:
+ // Invalid format
+ return -1;
+ }
+
+ switch (options[2]) {
+ case '1':
+ result |= SERIAL_STOPBITS_1;
+ break;
+ case '2':
+ result |= SERIAL_STOPBITS_2;
+ break;
+ default:
+ // Invalid format
+ return -1;
+ }
+
+ return result;
+}
+
static void cliSerialPassthrough(char *cmdline)
{
char * saveptr;
@@ -925,6 +962,7 @@ static void cliSerialPassthrough(char *cmdline)
int id = -1;
uint32_t baud = 0;
unsigned mode = 0;
+ portOptions_t options = SERIAL_NOT_INVERTED;
char* tok = strtok_r(cmdline, " ", &saveptr);
int index = 0;
@@ -942,6 +980,9 @@ static void cliSerialPassthrough(char *cmdline)
if (strstr(tok, "tx") || strstr(tok, "TX"))
mode |= MODE_TX;
break;
+ case 3:
+ options |= constructPortOptions(tok);
+ break;
}
index++;
tok = strtok_r(NULL, " ", &saveptr);
@@ -959,7 +1000,7 @@ static void cliSerialPassthrough(char *cmdline)
passThroughPort = openSerialPort(id, FUNCTION_NONE, NULL, NULL,
baud, mode,
- SERIAL_NOT_INVERTED);
+ options);
if (!passThroughPort) {
tfp_printf("Port %d could not be opened.\r\n", id);
return;
@@ -975,6 +1016,11 @@ static void cliSerialPassthrough(char *cmdline)
passThroughPort->mode, mode);
serialSetMode(passThroughPort, mode);
}
+ if (options && passThroughPort->options != options) {
+ tfp_printf("Adjusting options from %d to %d.\r\n",
+ passThroughPort->options, options);
+ serialSetOptions(passThroughPort, options);
+ }
// If this port has a rx callback associated we need to remove it now.
// Otherwise no data will be pushed in the serial port buffer!
if (passThroughPort->rxCallback) {
@@ -1087,7 +1133,7 @@ static void cliAdjustmentRange(char *cmdline)
}
static void printMotorMix(uint8_t dumpMask, const motorMixer_t *primaryMotorMixer, const motorMixer_t *defaultprimaryMotorMixer)
-{
+{
const char *format = "mmix %d %s %s %s %s";
char buf0[FTOA_BUFFER_SIZE];
char buf1[FTOA_BUFFER_SIZE];
@@ -1351,7 +1397,7 @@ static void cliTempSensor(char *cmdline)
#endif
#ifdef USE_FW_AUTOLAND
-static void printFwAutolandApproach(uint8_t dumpMask, const navFwAutolandApproach_t *navFwAutolandApproach, const navFwAutolandApproach_t *defaultFwAutolandApproach)
+static void printFwAutolandApproach(uint8_t dumpMask, const navFwAutolandApproach_t *navFwAutolandApproach, const navFwAutolandApproach_t *defaultFwAutolandApproach)
{
const char *format = "fwapproach %u %d %d %u %d %d %u";
for (uint8_t i = 0; i < MAX_FW_LAND_APPOACH_SETTINGS; i++) {
@@ -1398,7 +1444,7 @@ static void cliFwAutolandApproach(char * cmdline)
if ((ptr = nextArg(ptr))) {
landDirection = fastA2I(ptr);
-
+
if (landDirection != 0 && landDirection != 1) {
cliShowParseError();
return;
@@ -1428,7 +1474,7 @@ static void cliFwAutolandApproach(char * cmdline)
validArgumentCount++;
}
-
+
if ((ptr = nextArg(ptr))) {
isSeaLevelRef = fastA2I(ptr);
validArgumentCount++;
@@ -1842,7 +1888,7 @@ static void cliLedPinPWM(char *cmdline)
if (isEmpty(cmdline)) {
ledPinStopPWM();
cliPrintLine("PWM stopped");
- } else {
+ } else {
i = fastA2I(cmdline);
ledPinStartPWM(i);
cliPrintLinef("PWM started: %d%%",i);
@@ -3739,8 +3785,8 @@ static void cliStatus(char *cmdline)
#if defined(AT32F43x)
cliPrintLine("AT32 system clocks:");
crm_clocks_freq_type clocks;
- crm_clocks_freq_get(&clocks);
-
+ crm_clocks_freq_get(&clocks);
+
cliPrintLinef(" SYSCLK = %d MHz", clocks.sclk_freq / 1000000);
cliPrintLinef(" ABH = %d MHz", clocks.ahb_freq / 1000000);
cliPrintLinef(" ABP1 = %d MHz", clocks.apb1_freq / 1000000);
@@ -3848,6 +3894,24 @@ static void cliStatus(char *cmdline)
cliPrintErrorLinef("Invalid setting: %s", buf);
}
}
+
+#if defined(USE_OSD)
+ if (armingFlags & ARMING_DISABLED_NAVIGATION_UNSAFE) {
+ navArmingBlocker_e reason = navigationIsBlockingArming(NULL);
+ if (reason & NAV_ARMING_BLOCKER_JUMP_WAYPOINT_ERROR)
+ cliPrintLinef(" %s", OSD_MSG_JUMP_WP_MISCONFIG);
+ if (reason & NAV_ARMING_BLOCKER_MISSING_GPS_FIX) {
+ cliPrintLinef(" %s", OSD_MSG_WAITING_GPS_FIX);
+ } else {
+ if (reason & NAV_ARMING_BLOCKER_NAV_IS_ALREADY_ACTIVE)
+ cliPrintLinef(" %s", OSD_MSG_DISABLE_NAV_FIRST);
+ if (reason & NAV_ARMING_BLOCKER_FIRST_WAYPOINT_TOO_FAR)
+ cliPrintLinef(" FIRST WP TOO FAR");
+ }
+ }
+#endif
+
+
#else
cliPrintLinef("Arming disabled flags: 0x%lx", armingFlags & ARMING_DISABLED_ALL_FLAGS);
#endif
@@ -4382,7 +4446,7 @@ static const char *_ubloxGetQuality(uint8_t quality)
case UBLOX_SIG_QUALITY_CODE_LOCK_TIME_SYNC: return "Code locked and time sync";
case UBLOX_SIG_QUALITY_CODE_CARRIER_LOCK_TIME_SYNC:
case UBLOX_SIG_QUALITY_CODE_CARRIER_LOCK_TIME_SYNC2:
- case UBLOX_SIG_QUALITY_CODE_CARRIER_LOCK_TIME_SYNC3:
+ case UBLOX_SIG_QUALITY_CODE_CARRIER_LOCK_TIME_SYNC3:
return "Code and carrier locked and time sync";
default: return "Unknown";
}
@@ -4514,7 +4578,7 @@ const clicmd_t cmdTable[] = {
CLI_COMMAND_DEF("save", "save and reboot", NULL, cliSave),
CLI_COMMAND_DEF("serial", "configure serial ports", NULL, cliSerial),
#ifdef USE_SERIAL_PASSTHROUGH
- CLI_COMMAND_DEF("serialpassthrough", "passthrough serial data to port", " [baud] [mode] : passthrough to serial", cliSerialPassthrough),
+ CLI_COMMAND_DEF("serialpassthrough", "passthrough serial data to port", " [baud] [mode] [options]: passthrough to serial", cliSerialPassthrough),
#endif
CLI_COMMAND_DEF("servo", "configure servos", NULL, cliServo),
#ifdef USE_PROGRAMMING_FRAMEWORK
diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c
index c8ae27f270d..f82f3cbaeaf 100644
--- a/src/main/fc/fc_msp.c
+++ b/src/main/fc/fc_msp.c
@@ -956,6 +956,8 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
#ifdef USE_ADSB
sbufWriteU8(dst, MAX_ADSB_VEHICLES);
sbufWriteU8(dst, ADSB_CALL_SIGN_MAX_LENGTH);
+ sbufWriteU32(dst, getAdsbStatus()->vehiclesMessagesTotal);
+ sbufWriteU32(dst, getAdsbStatus()->heartbeatMessagesTotal);
for(uint8_t i = 0; i < MAX_ADSB_VEHICLES; i++){
@@ -977,6 +979,8 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
#else
sbufWriteU8(dst, 0);
sbufWriteU8(dst, 0);
+ sbufWriteU32(dst, 0);
+ sbufWriteU32(dst, 0);
#endif
break;
case MSP_DEBUG:
@@ -1034,7 +1038,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
case MSP_MIXER:
sbufWriteU8(dst, 3); // mixerMode no longer supported, send 3 (QuadX) as fallback
break;
-
+
case MSP_RX_CONFIG:
sbufWriteU8(dst, rxConfig()->serialrx_provider);
@@ -1274,7 +1278,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
sbufWriteU16(dst, accelerometerConfig()->acc_notch_cutoff);
sbufWriteU16(dst, 0); //Was gyroConfig()->gyro_stage2_lowpass_hz
- break;
+ break;
case MSP_PID_ADVANCED:
sbufWriteU16(dst, 0); // pidProfile()->rollPitchItermIgnoreRate
@@ -1618,7 +1622,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
}
}
break;
-
+
case MSP2_INAV_MC_BRAKING:
#ifdef USE_MR_BRAKING_MODE
@@ -1706,28 +1710,19 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
#endif
#ifdef USE_PROGRAMMING_FRAMEWORK
case MSP2_INAV_CUSTOM_OSD_ELEMENTS:
- sbufWriteU8(dst, MAX_CUSTOM_ELEMENTS);
- sbufWriteU8(dst, OSD_CUSTOM_ELEMENT_TEXT_SIZE - 1);
-
- for (int i = 0; i < MAX_CUSTOM_ELEMENTS; i++) {
- const osdCustomElement_t *customElement = osdCustomElements(i);
- for (int ii = 0; ii < CUSTOM_ELEMENTS_PARTS; ii++) {
- sbufWriteU8(dst, customElement->part[ii].type);
- sbufWriteU16(dst, customElement->part[ii].value);
- }
- sbufWriteU8(dst, customElement->visibility.type);
- sbufWriteU16(dst, customElement->visibility.value);
- for (int ii = 0; ii < OSD_CUSTOM_ELEMENT_TEXT_SIZE - 1; ii++) {
- sbufWriteU8(dst, customElement->osdCustomElementText[ii]);
- }
+ {
+ sbufWriteU8(dst, MAX_CUSTOM_ELEMENTS);
+ sbufWriteU8(dst, OSD_CUSTOM_ELEMENT_TEXT_SIZE - 1);
+ sbufWriteU8(dst, CUSTOM_ELEMENTS_PARTS);
}
break;
+#endif
default:
return false;
}
return true;
}
-#endif
+
#ifdef USE_SAFE_HOME
static mspResult_e mspFcSafeHomeOutCommand(sbuf_t *dst, sbuf_t *src)
@@ -2892,7 +2887,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
} else
return MSP_RESULT_ERROR;
break;
-
+
case MSP_SET_FAILSAFE_CONFIG:
if (dataSize == 20) {
failsafeConfigMutable()->failsafe_delay = sbufReadU8(src);
@@ -3291,11 +3286,9 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
#endif
case MSP2_INAV_GPS_UBLOX_COMMAND:
if(dataSize < 8 || !isGpsUblox()) {
- SD(fprintf(stderr, "[GPS] Not ublox!\n"));
return MSP_RESULT_ERROR;
}
- SD(fprintf(stderr, "[GPS] Sending ubx command: %i!\n", dataSize));
gpsUbloxSendCommand(src->ptr, dataSize, 0);
break;
@@ -3349,7 +3342,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
#ifdef USE_PROGRAMMING_FRAMEWORK
case MSP2_INAV_SET_CUSTOM_OSD_ELEMENTS:
sbufReadU8Safe(&tmp_u8, src);
- if ((dataSize == (OSD_CUSTOM_ELEMENT_TEXT_SIZE - 1) + (MAX_CUSTOM_ELEMENTS * 3) + 4) && (tmp_u8 < MAX_CUSTOM_ELEMENTS)) {
+ if ((dataSize == (OSD_CUSTOM_ELEMENT_TEXT_SIZE - 1) + (CUSTOM_ELEMENTS_PARTS * 3) + 4) && (tmp_u8 < MAX_CUSTOM_ELEMENTS)) {
for (int i = 0; i < CUSTOM_ELEMENTS_PARTS; i++) {
osdCustomElementsMutable(tmp_u8)->part[i].type = sbufReadU8(src);
osdCustomElementsMutable(tmp_u8)->part[i].value = sbufReadU16(src);
@@ -3365,7 +3358,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
}
break;
-
+#endif
case MSP2_BETAFLIGHT_BIND:
if (rxConfig()->receiverType == RX_TYPE_SERIAL) {
switch (rxConfig()->serialrx_provider) {
@@ -3392,7 +3385,6 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
}
return MSP_RESULT_ACK;
}
-#endif
static const setting_t *mspReadSetting(sbuf_t *src)
{
@@ -3849,6 +3841,24 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
case MSP2_INAV_LOGIC_CONDITIONS_SINGLE:
*ret = mspFcLogicConditionCommand(dst, src);
break;
+ case MSP2_INAV_CUSTOM_OSD_ELEMENT:
+ {
+ const uint8_t idx = sbufReadU8(src);
+
+ if (idx < MAX_CUSTOM_ELEMENTS) {
+ const osdCustomElement_t *customElement = osdCustomElements(idx);
+ for (int ii = 0; ii < CUSTOM_ELEMENTS_PARTS; ii++) {
+ sbufWriteU8(dst, customElement->part[ii].type);
+ sbufWriteU16(dst, customElement->part[ii].value);
+ }
+ sbufWriteU8(dst, customElement->visibility.type);
+ sbufWriteU16(dst, customElement->visibility.value);
+ for (int ii = 0; ii < OSD_CUSTOM_ELEMENT_TEXT_SIZE - 1; ii++) {
+ sbufWriteU8(dst, customElement->osdCustomElementText[ii]);
+ }
+ }
+ }
+ break;
#endif
#ifdef USE_SAFE_HOME
case MSP2_INAV_SAFEHOME:
@@ -4145,7 +4155,6 @@ mspResult_e mspFcProcessCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostPro
// initialize reply by default
reply->cmd = cmd->cmd;
- SD(fprintf(stderr, "[MSP] CommandId: 0x%04x bytes: %i!\n", cmdMSP, sbufBytesRemaining(src)));
if (MSP2_IS_SENSOR_MESSAGE(cmdMSP)) {
ret = mspProcessSensorCommand(cmdMSP, src);
} else if (mspFcProcessOutCommand(cmdMSP, dst, mspPostProcessFn)) {
diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c
index 107a0e732b6..afb880db526 100755
--- a/src/main/fc/fc_tasks.c
+++ b/src/main/fc/fc_tasks.c
@@ -439,7 +439,7 @@ void fcTasksInit(void)
#endif
#if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_SBUS2)
- setTaskEnabled(TASK_TELEMETRY_SBUS2,rxConfig()->receiverType == RX_TYPE_SERIAL && rxConfig()->serialrx_provider == SERIALRX_SBUS2);
+ setTaskEnabled(TASK_TELEMETRY_SBUS2,feature(FEATURE_TELEMETRY) && rxConfig()->receiverType == RX_TYPE_SERIAL && rxConfig()->serialrx_provider == SERIALRX_SBUS2);
#endif
#ifdef USE_ADAPTIVE_FILTER
diff --git a/src/main/fc/rc_controls.h b/src/main/fc/rc_controls.h
index 3705a29c3a1..f5b96f2d239 100644
--- a/src/main/fc/rc_controls.h
+++ b/src/main/fc/rc_controls.h
@@ -40,13 +40,23 @@ typedef enum rc_alias {
AUX12, // 16
AUX13, // 17
AUX14, // 18
-#ifdef USE_24CHANNELS
+#ifdef USE_34CHANNELS
AUX15, // 19
AUX16, // 20
AUX17, // 21
AUX18, // 22
AUX19, // 23
AUX20, // 24
+ AUX21, // 25
+ AUX22, // 26
+ AUX23, // 27
+ AUX24, // 28
+ AUX25, // 29
+ AUX26, // 30
+ AUX27, // 31
+ AUX28, // 32
+ AUX29, // 33
+ AUX30, // 34
#endif
} rc_alias_e;
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index 486fc54b8c1..1016e26ddab 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -2559,7 +2559,7 @@ groups:
min: 1
max: 9
- name: nav_fw_wp_tracking_accuracy
- description: "Waypoint tracking accuracy forces the craft to quickly head toward and track along the waypoint course line as closely as possible. Settings 1 to 10 adjust the course tracking response. Higher values dampen the response reducing possible overshoot. A value of 5 is a good starting point. Set to 0 to disable."
+ description: "Waypoint tracking accuracy forces the craft to quickly head toward and track along the waypoint course line as closely as possible. Setting adjusts tracking deadband distance fom waypoint courseline [m]. Tracking isn't actively controlled within the deadband providing smoother flight adjustments but less accurate tracking. A 2m deadband should work OK in most cases. Setting to 0 disables waypoint tracking accuracy."
default_value: 0
field: fw.wp_tracking_accuracy
min: 0
@@ -2706,7 +2706,7 @@ groups:
- name: nav_max_terrain_follow_alt
field: general.max_terrain_follow_altitude
default_value: "100"
- description: "Max allowed above the ground altitude for terrain following mode"
+ description: "Max allowed above the ground altitude for terrain following mode [cm]"
max: 1000
default_value: 100
- name: nav_max_altitude
@@ -3066,6 +3066,10 @@ groups:
description: "S.Port telemetry: Send pitch and roll degrees*10 instead of raw accelerometer data"
default_value: OFF
type: bool
+ - name: frsky_use_legacy_gps_mode_sensor_ids
+ description: "S.Port telemetry: If `ON`, send the legacy telemetry IDs for modes (Tmp1) and GNSS (Tmp2). These are old IDs, deprecated, and will be removed in INAV 10.0. Tools and scripts using these IDs should be updated to use the new IDs of **470** for Modes and **480** for GNSS. Default: 'OFF'"
+ default_value: OFF
+ type: bool
- name: report_cell_voltage
description: "S.Port and IBUS telemetry: Send the average cell voltage if set to ON"
default_value: OFF
@@ -3720,6 +3724,17 @@ groups:
min: 1000
max: 5000
default_value: 1500
+ - name: osd_system_msg_display_time
+ description: System message display cycle time for multiple messages (milliseconds).
+ field: system_msg_display_time
+ default_value: 1000
+ min: 500
+ max: 5000
+ - name: osd_highlight_djis_missing_font_symbols
+ description: Show question marks where there is no symbol in the DJI font to represent the INAV OSD element's symbol. When off, blank spaces will be used. Only relevent for DJICOMPAT modes.
+ field: highlight_djis_missing_characters
+ default_value: ON
+ type: bool
- name: osd_switch_indicator_zero_name
description: "Character to use for OSD switch incicator 0."
field: osd_switch_indicator0_name
@@ -3773,17 +3788,6 @@ groups:
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
- default_value: 1000
- min: 500
- max: 5000
- - name: osd_highlight_djis_missing_font_symbols
- description: Show question marks where there is no symbol in the DJI font to represent the INAV OSD element's symbol. When off, blank spaces will be used. Only relevent for DJICOMPAT modes.
- field: highlight_djis_missing_characters
- default_value: ON
- type: bool
- name: PG_OSD_COMMON_CONFIG
type: osdCommonConfig_t
headers: ["io/osd_common.h"]
diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c
index 5ac98f78215..40711706e78 100755
--- a/src/main/flight/servos.c
+++ b/src/main/flight/servos.c
@@ -349,13 +349,23 @@ void servoMixer(float dT)
input[INPUT_RC_CH16] = GET_RX_CHANNEL_INPUT(AUX12);
input[INPUT_RC_CH17] = GET_RX_CHANNEL_INPUT(AUX13);
input[INPUT_RC_CH18] = GET_RX_CHANNEL_INPUT(AUX14);
-#ifdef USE_24CHANNELS
+#ifdef USE_34CHANNELS
input[INPUT_RC_CH19] = GET_RX_CHANNEL_INPUT(AUX15);
input[INPUT_RC_CH20] = GET_RX_CHANNEL_INPUT(AUX16);
input[INPUT_RC_CH21] = GET_RX_CHANNEL_INPUT(AUX17);
input[INPUT_RC_CH22] = GET_RX_CHANNEL_INPUT(AUX18);
input[INPUT_RC_CH23] = GET_RX_CHANNEL_INPUT(AUX19);
input[INPUT_RC_CH24] = GET_RX_CHANNEL_INPUT(AUX20);
+ input[INPUT_RC_CH25] = GET_RX_CHANNEL_INPUT(AUX21);
+ input[INPUT_RC_CH26] = GET_RX_CHANNEL_INPUT(AUX22);
+ input[INPUT_RC_CH27] = GET_RX_CHANNEL_INPUT(AUX23);
+ input[INPUT_RC_CH28] = GET_RX_CHANNEL_INPUT(AUX24);
+ input[INPUT_RC_CH29] = GET_RX_CHANNEL_INPUT(AUX25);
+ input[INPUT_RC_CH30] = GET_RX_CHANNEL_INPUT(AUX26);
+ input[INPUT_RC_CH31] = GET_RX_CHANNEL_INPUT(AUX27);
+ input[INPUT_RC_CH32] = GET_RX_CHANNEL_INPUT(AUX28);
+ input[INPUT_RC_CH33] = GET_RX_CHANNEL_INPUT(AUX29);
+ input[INPUT_RC_CH34] = GET_RX_CHANNEL_INPUT(AUX30);
#endif
#undef GET_RX_CHANNEL_INPUT
diff --git a/src/main/flight/servos.h b/src/main/flight/servos.h
index 4f6e5777967..23ac3fda495 100644
--- a/src/main/flight/servos.h
+++ b/src/main/flight/servos.h
@@ -68,14 +68,22 @@ typedef enum {
INPUT_HEADTRACKER_ROLL = 41,
INPUT_RC_CH17 = 42,
INPUT_RC_CH18 = 43,
-#ifdef USE_24CHANNELS
INPUT_RC_CH19 = 44,
INPUT_RC_CH20 = 45,
INPUT_RC_CH21 = 46,
INPUT_RC_CH22 = 47,
INPUT_RC_CH23 = 48,
INPUT_RC_CH24 = 49,
-#endif
+ INPUT_RC_CH25 = 50,
+ INPUT_RC_CH26 = 51,
+ INPUT_RC_CH27 = 52,
+ INPUT_RC_CH28 = 53,
+ INPUT_RC_CH29 = 54,
+ INPUT_RC_CH30 = 55,
+ INPUT_RC_CH31 = 56,
+ INPUT_RC_CH32 = 57,
+ INPUT_RC_CH33 = 58,
+ INPUT_RC_CH34 = 59,
INPUT_SOURCE_COUNT
} inputSource_e;
diff --git a/src/main/io/adsb.c b/src/main/io/adsb.c
index 573112c2df7..260f89fa6af 100644
--- a/src/main/io/adsb.c
+++ b/src/main/io/adsb.c
@@ -131,6 +131,11 @@ void gpsDistanceCmBearing(int32_t currentLat1, int32_t currentLon1, int32_t dest
*bearing = wrap_36000(*bearing);
};
+bool adsbHeartbeat(void){
+ adsbVehiclesStatus.heartbeatMessagesTotal++;
+ return true;
+}
+
void adsbNewVehicle(adsbVehicleValues_t* vehicleValuesLocal) {
// no valid lat lon or altitude
@@ -139,6 +144,7 @@ void adsbNewVehicle(adsbVehicleValues_t* vehicleValuesLocal) {
}
adsbVehiclesStatus.vehiclesMessagesTotal++;
+
adsbVehicle_t *vehicle = NULL;
vehicle = findVehicleByIcao(vehicleValuesLocal->icao);
diff --git a/src/main/io/adsb.h b/src/main/io/adsb.h
index 86eefd8aac1..77f2e31fa47 100644
--- a/src/main/io/adsb.h
+++ b/src/main/io/adsb.h
@@ -54,9 +54,11 @@ typedef struct {
typedef struct {
uint32_t vehiclesMessagesTotal;
+ uint32_t heartbeatMessagesTotal;
} adsbVehicleStatus_t;
void adsbNewVehicle(adsbVehicleValues_t* vehicleValuesLocal);
+bool adsbHeartbeat(void);
adsbVehicle_t * findVehicleClosest(void);
adsbVehicle_t * findVehicle(uint8_t index);
uint8_t getActiveVehiclesCount(void);
diff --git a/src/main/io/gimbal_serial.c b/src/main/io/gimbal_serial.c
index 4eec2903947..fd6294c9959 100644
--- a/src/main/io/gimbal_serial.c
+++ b/src/main/io/gimbal_serial.c
@@ -63,11 +63,11 @@ static gimbalSerialHtrkState_t headTrackerState = {
.attitude = {},
.state = WAITING_HDR1,
};
+static serialPort_t *headTrackerPort = NULL;
#endif
#endif
-static serialPort_t *headTrackerPort = NULL;
static serialPort_t *gimbalPort = NULL;
gimbalVTable_t gimbalSerialVTable = {
@@ -116,7 +116,9 @@ bool gimbalSerialIsReady(const gimbalDevice_t *gimbalDevice)
bool gimbalSerialHasHeadTracker(const gimbalDevice_t *gimbalDevice)
{
UNUSED(gimbalDevice);
- return headTrackerPort || (gimbalSerialConfig()->singleUart && gimbalPort);
+
+ headTrackerDevice_t *htrk = headTrackerCommonDevice();
+ return htrk != NULL && headTrackerCommonIsReady(htrk);
}
bool gimbalSerialInit(void)
@@ -251,7 +253,7 @@ void gimbalSerialProcess(gimbalDevice_t *gimbalDevice, timeUs_t currentTime)
#else
{
#endif
- DEBUG_SET(DEBUG_HEADTRACKING, 4, 0);
+ DEBUG_SET(DEBUG_HEADTRACKING, 4, 2);
// Radio endpoints may need to be adjusted, as it seems ot go a bit
// bananas at the extremes
attitude.pan = gimbal_scale12(PWM_RANGE_MIN, PWM_RANGE_MAX, panPWM);
diff --git a/src/main/io/headtracker_msp.c b/src/main/io/headtracker_msp.c
index 8a800b3f46e..e53fd3ab207 100644
--- a/src/main/io/headtracker_msp.c
+++ b/src/main/io/headtracker_msp.c
@@ -30,10 +30,11 @@
#include "io/headtracker_msp.h"
+static bool isReady = false;
static headTrackerVTable_t headTrackerMspVTable = {
.process = NULL,
.getDeviceType = heatTrackerMspGetDeviceType,
- .isReady = NULL,
+ .isReady = heatTrackerMspIsReady,
.isValid = NULL,
};
@@ -52,12 +53,18 @@ void mspHeadTrackerInit(void)
}
}
-void mspHeadTrackerReceiverNewData(uint8_t *data, int dataSize)
+void mspHeadTrackerReceiverNewData(uint8_t *data, unsigned int dataSize)
{
if(dataSize != sizeof(headtrackerMspMessage_t)) {
SD(fprintf(stderr, "[headTracker]: invalid data size %d\n", dataSize));
+ static int errorCount = 0;
+ DEBUG_SET(DEBUG_HEADTRACKING, 7, errorCount++);
+ DEBUG_SET(DEBUG_HEADTRACKING, 5, (sizeof(headtrackerMspMessage_t)));
+ DEBUG_SET(DEBUG_HEADTRACKING, 6, dataSize);
return;
}
+ isReady = true;
+ DEBUG_SET(DEBUG_HEADTRACKING, 6, dataSize);
headtrackerMspMessage_t *status = (headtrackerMspMessage_t *)data;
@@ -66,7 +73,10 @@ void mspHeadTrackerReceiverNewData(uint8_t *data, int dataSize)
headTrackerMspDevice.roll = constrain(status->roll, HEADTRACKER_RANGE_MIN, HEADTRACKER_RANGE_MAX);
headTrackerMspDevice.expires = micros() + MAX_HEADTRACKER_DATA_AGE_US;
- UNUSED(status);
+ DEBUG_SET(DEBUG_HEADTRACKING, 0, headTrackerMspDevice.pan);
+ DEBUG_SET(DEBUG_HEADTRACKING, 1, headTrackerMspDevice.tilt);
+ DEBUG_SET(DEBUG_HEADTRACKING, 2, headTrackerMspDevice.roll);
+ DEBUG_SET(DEBUG_HEADTRACKING, 3, headTrackerMspDevice.expires);
}
headTrackerDevType_e heatTrackerMspGetDeviceType(const headTrackerDevice_t *headTrackerDevice) {
@@ -74,4 +84,11 @@ headTrackerDevType_e heatTrackerMspGetDeviceType(const headTrackerDevice_t *head
return HEADTRACKER_MSP;
}
-#endif
\ No newline at end of file
+
+bool heatTrackerMspIsReady(const headTrackerDevice_t *headTrackerDevice)
+{
+ UNUSED(headTrackerDevice);
+ return headTrackerConfig()->devType == HEADTRACKER_MSP && isReady;
+}
+
+#endif
diff --git a/src/main/io/headtracker_msp.h b/src/main/io/headtracker_msp.h
index edcfb465874..e082d0db8ae 100644
--- a/src/main/io/headtracker_msp.h
+++ b/src/main/io/headtracker_msp.h
@@ -37,8 +37,9 @@ typedef struct headtrackerMspMessage_s {
void mspHeadTrackerInit(void);
-void mspHeadTrackerReceiverNewData(uint8_t *data, int dataSize);
+void mspHeadTrackerReceiverNewData(uint8_t *data, unsigned int dataSize);
headTrackerDevType_e heatTrackerMspGetDeviceType(const headTrackerDevice_t *headTrackerDevice);
+bool heatTrackerMspIsReady(const headTrackerDevice_t *headTrackerDevice);
#endif
diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index d682c0299ff..1b88226b9cb 100644
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -225,7 +225,7 @@ static bool osdDisplayHasCanvas;
#define AH_MAX_PITCH_DEFAULT 20 // Specify default maximum AHI pitch value displayed (degrees)
PG_REGISTER_WITH_RESET_TEMPLATE(osdConfig_t, osdConfig, PG_OSD_CONFIG, 12);
-PG_REGISTER_WITH_RESET_FN(osdLayoutsConfig_t, osdLayoutsConfig, PG_OSD_LAYOUTS_CONFIG, 1);
+PG_REGISTER_WITH_RESET_FN(osdLayoutsConfig_t, osdLayoutsConfig, PG_OSD_LAYOUTS_CONFIG, 2);
void osdStartedSaveProcess(void) {
savingSettings = true;
@@ -1671,6 +1671,31 @@ static bool osdDrawSingleElement(uint8_t item)
customElementDrawElement(buff, 2);
break;
}
+ case OSD_CUSTOM_ELEMENT_4:
+ {
+ customElementDrawElement(buff, 3);
+ break;
+ }
+ case OSD_CUSTOM_ELEMENT_5:
+ {
+ customElementDrawElement(buff, 4);
+ break;
+ }
+ case OSD_CUSTOM_ELEMENT_6:
+ {
+ customElementDrawElement(buff, 5);
+ break;
+ }
+ case OSD_CUSTOM_ELEMENT_7:
+ {
+ customElementDrawElement(buff, 6);
+ break;
+ }
+ case OSD_CUSTOM_ELEMENT_8:
+ {
+ customElementDrawElement(buff, 7);
+ break;
+ }
case OSD_RSSI_VALUE:
{
uint16_t osdRssi = osdConvertRSSI();
@@ -2136,7 +2161,7 @@ static bool osdDrawSingleElement(uint8_t item)
case OSD_ADSB_INFO:
{
buff[0] = SYM_ADSB;
- if(getAdsbStatus()->vehiclesMessagesTotal > 0){
+ if(getAdsbStatus()->vehiclesMessagesTotal > 0 || getAdsbStatus()->heartbeatMessagesTotal > 0){
tfp_sprintf(buff + 1, "%2d", getActiveVehiclesCount());
}else{
buff[1] = '-';
@@ -2872,7 +2897,6 @@ static bool osdDrawSingleElement(uint8_t item)
break;
}
#endif
-
case OSD_SWITCH_INDICATOR_0:
osdDisplaySwitchIndicator(osdConfig()->osd_switch_indicator0_name, rxGetChannelValue(osdConfig()->osd_switch_indicator0_channel - 1), buff);
break;
@@ -4998,7 +5022,7 @@ uint8_t drawStat_ESCTemperature(uint8_t col, uint8_t row, uint8_t statValX)
uint8_t drawStat_GForce(uint8_t col, uint8_t row, uint8_t statValX)
{
char buff[12];
- char outBuff[12];
+ char outBuff[20];
const float max_gforce = accGetMeasuredMaxG();
const acc_extremes_t *acc_extremes = accGetMeasuredExtremes();
diff --git a/src/main/io/osd.h b/src/main/io/osd.h
index a73db015793..0cf93b69162 100644
--- a/src/main/io/osd.h
+++ b/src/main/io/osd.h
@@ -286,7 +286,12 @@ typedef enum {
OSD_ADSB_WARNING, //150
OSD_ADSB_INFO,
OSD_BLACKBOX,
- OSD_FORMATION_FLIGHT, //153
+ OSD_FORMATION_FLIGHT,
+ OSD_CUSTOM_ELEMENT_4,
+ OSD_CUSTOM_ELEMENT_5,
+ OSD_CUSTOM_ELEMENT_6,
+ OSD_CUSTOM_ELEMENT_7,
+ OSD_CUSTOM_ELEMENT_8, // 158
OSD_ITEM_COUNT // MUST BE LAST
} osd_items_e;
@@ -444,7 +449,7 @@ typedef struct osdConfig_s {
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_precision; // Number of numbers used for mAh drawn. Plently of packs now are > 9999 mAh
+ uint8_t mAh_precision; // Number of numbers used for mAh drawn. Plently of packs now are > 9999 mAh
uint8_t ahi_pitch_interval; // redraws AHI at set pitch interval (Not pixel OSD)
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.
diff --git a/src/main/io/osd/custom_elements.c b/src/main/io/osd/custom_elements.c
index fbd05e2be61..df84ddd76b0 100644
--- a/src/main/io/osd/custom_elements.c
+++ b/src/main/io/osd/custom_elements.c
@@ -68,31 +68,129 @@ uint8_t customElementDrawPart(char *buff, uint8_t customElementIndex, uint8_t cu
const int customPartValue = osdCustomElements(customElementIndex)->part[customElementItemIndex].value;
switch (customPartType) {
- case CUSTOM_ELEMENT_TYPE_GV:
+ case CUSTOM_ELEMENT_TYPE_GV_1:
{
- osdFormatCentiNumber(buff, (int32_t) gvGet(customPartValue) * (int32_t) 100, 1, 0, 0, 6, false);
- return 6;
+ osdFormatCentiNumber(buff,(int32_t) (constrain(gvGet(customPartValue), -9, 9) * (int32_t) 100), 1, 0, 0, 2, false);
+ return 2;
+ }
+ case CUSTOM_ELEMENT_TYPE_GV_2:
+ {
+ osdFormatCentiNumber(buff, (int32_t) (constrain(gvGet(customPartValue), -99, 99) * (int32_t) 100), 1, 0, 0, 3, false);
+ return 3;
+ }
+ case CUSTOM_ELEMENT_TYPE_GV_3:
+ {
+ osdFormatCentiNumber(buff, (int32_t) (constrain(gvGet(customPartValue), -999, 999) * (int32_t) 100), 1, 0, 0, 4, false);
+ return 4;
}
- case CUSTOM_ELEMENT_TYPE_GV_FLOAT:
+ case CUSTOM_ELEMENT_TYPE_GV_4:
{
- osdFormatCentiNumber(buff, (int32_t) gvGet(customPartValue), 1, 2, 0, 6, false);
+ osdFormatCentiNumber(buff, (int32_t) (constrain(gvGet(customPartValue), -9999, 9999) * (int32_t) 100), 1, 0, 0, 5, false);
+ return 5;
+ }
+ case CUSTOM_ELEMENT_TYPE_GV_5:
+ {
+ osdFormatCentiNumber(buff, (int32_t) (constrain(gvGet(customPartValue), -99999, 99999) * (int32_t) 100), 1, 0, 0, 6, false);
return 6;
}
- case CUSTOM_ELEMENT_TYPE_GV_SMALL:
+ case CUSTOM_ELEMENT_TYPE_GV_FLOAT_1_1:
{
- osdFormatCentiNumber(buff, (int32_t) ((gvGet(customPartValue) % 1000 ) * (int32_t) 100), 1, 0, 0, 3, false);
+ osdFormatCentiNumber(buff, (int32_t) (constrain(gvGet(customPartValue), -99, 99) * (int32_t) 10), 1, 1, 0, 3, false);
return 3;
}
- case CUSTOM_ELEMENT_TYPE_GV_SMALL_FLOAT:
+ case CUSTOM_ELEMENT_TYPE_GV_FLOAT_2_1:
+ {
+ osdFormatCentiNumber(buff, (int32_t) (constrain(gvGet(customPartValue), -999, 999) * (int32_t) 10), 1, 1, 0, 4, false);
+ return 4;
+ }
+ case CUSTOM_ELEMENT_TYPE_GV_FLOAT_2_2:
+ {
+ osdFormatCentiNumber(buff, (int32_t) constrain(gvGet(customPartValue), -9999, 9999), 1, 2, 0, 5, false);
+ return 5;
+ }
+ case CUSTOM_ELEMENT_TYPE_GV_FLOAT_3_1:
{
- osdFormatCentiNumber(buff, (int32_t) ((gvGet(customPartValue) % 100) * (int32_t) 10), 1, 1, 0, 2, false);
+ osdFormatCentiNumber(buff, (int32_t) (constrain(gvGet(customPartValue), -9999, 9999) * (int32_t) 10), 1, 1, 0, 5, false);
+ return 5;
+ }
+ case CUSTOM_ELEMENT_TYPE_GV_FLOAT_3_2:
+ {
+ osdFormatCentiNumber(buff, (int32_t) constrain(gvGet(customPartValue), -99999, 99999), 1, 2, 0, 6, false);
+ return 6;
+ }
+ case CUSTOM_ELEMENT_TYPE_GV_FLOAT_4_1:
+ {
+ osdFormatCentiNumber(buff, (int32_t) (constrain(gvGet(customPartValue), -99999, 99999) * (int32_t) 10), 1, 1, 0, 6, false);
+ return 6;
+ }
+
+ case CUSTOM_ELEMENT_TYPE_LC_1:
+ {
+ osdFormatCentiNumber(buff,(int32_t) (constrain(logicConditionGetValue(customPartValue), -9, 9) * (int32_t) 100), 1, 0, 0, 2, false);
return 2;
}
+ case CUSTOM_ELEMENT_TYPE_LC_2:
+ {
+ osdFormatCentiNumber(buff, (int32_t) (constrain(logicConditionGetValue(customPartValue), -99, 99) * (int32_t) 100), 1, 0, 0, 3, false);
+ return 3;
+ }
+ case CUSTOM_ELEMENT_TYPE_LC_3:
+ {
+ osdFormatCentiNumber(buff, (int32_t) (constrain(logicConditionGetValue(customPartValue), -999, 999) * (int32_t) 100), 1, 0, 0, 4, false);
+ return 4;
+ }
+ case CUSTOM_ELEMENT_TYPE_LC_4:
+ {
+ osdFormatCentiNumber(buff, (int32_t) (constrain(logicConditionGetValue(customPartValue), -9999, 9999) * (int32_t) 100), 1, 0, 0, 5, false);
+ return 5;
+ }
+ case CUSTOM_ELEMENT_TYPE_LC_5:
+ {
+ osdFormatCentiNumber(buff, (int32_t) (constrain(logicConditionGetValue(customPartValue), -99999, 99999) * (int32_t) 100), 1, 0, 0, 6, false);
+ return 6;
+ }
+ case CUSTOM_ELEMENT_TYPE_LC_FLOAT_1_1:
+ {
+ osdFormatCentiNumber(buff, (int32_t) (constrain(logicConditionGetValue(customPartValue), -99, 99) * (int32_t) 10), 1, 1, 0, 3, false);
+ return 3;
+ }
+ case CUSTOM_ELEMENT_TYPE_LC_FLOAT_2_1:
+ {
+ osdFormatCentiNumber(buff, (int32_t) (constrain(logicConditionGetValue(customPartValue), -999, 999) * (int32_t) 10), 1, 1, 0, 4, false);
+ return 4;
+ }
+ case CUSTOM_ELEMENT_TYPE_LC_FLOAT_2_2:
+ {
+ osdFormatCentiNumber(buff, (int32_t) constrain(logicConditionGetValue(customPartValue), -9999, 9999), 1, 2, 0, 5, false);
+ return 5;
+ }
+ case CUSTOM_ELEMENT_TYPE_LC_FLOAT_3_1:
+ {
+ osdFormatCentiNumber(buff, (int32_t) (constrain(logicConditionGetValue(customPartValue), -9999, 9999) * (int32_t) 10), 1, 1, 0, 5, false);
+ return 5;
+ }
+ case CUSTOM_ELEMENT_TYPE_LC_FLOAT_3_2:
+ {
+ osdFormatCentiNumber(buff, (int32_t) constrain(logicConditionGetValue(customPartValue), -99999, 99999), 1, 2, 0, 6, false);
+ return 6;
+ }
+ case CUSTOM_ELEMENT_TYPE_LC_FLOAT_4_1:
+ {
+ osdFormatCentiNumber(buff, (int32_t) (constrain(logicConditionGetValue(customPartValue), -99999, 99999) * (int32_t) 10), 1, 1, 0, 6, false);
+ return 6;
+ }
+
+
case CUSTOM_ELEMENT_TYPE_ICON_GV:
{
*buff = (uint8_t)gvGet(customPartValue);
return 1;
}
+ case CUSTOM_ELEMENT_TYPE_ICON_LC:
+ {
+ *buff = (uint8_t)constrain(logicConditionGetValue(customPartValue), 1, 255);
+ return 1;
+ }
case CUSTOM_ELEMENT_TYPE_ICON_STATIC:
{
*buff = (uint8_t)customPartValue;
@@ -138,4 +236,3 @@ void customElementDrawElement(char *buff, uint8_t customElementIndex){
}
prevLength[customElementIndex] = buffSeek;
}
-
diff --git a/src/main/io/osd/custom_elements.h b/src/main/io/osd/custom_elements.h
index a55b010f01a..9c12ede6734 100644
--- a/src/main/io/osd/custom_elements.h
+++ b/src/main/io/osd/custom_elements.h
@@ -21,17 +21,36 @@
#define OSD_CUSTOM_ELEMENT_TEXT_SIZE 16
#define CUSTOM_ELEMENTS_PARTS 3
-#define MAX_CUSTOM_ELEMENTS 3
+#define MAX_CUSTOM_ELEMENTS 8
typedef enum {
CUSTOM_ELEMENT_TYPE_NONE = 0,
CUSTOM_ELEMENT_TYPE_TEXT = 1,
CUSTOM_ELEMENT_TYPE_ICON_STATIC = 2,
CUSTOM_ELEMENT_TYPE_ICON_GV = 3,
- CUSTOM_ELEMENT_TYPE_GV = 4,
- CUSTOM_ELEMENT_TYPE_GV_FLOAT = 5,
- CUSTOM_ELEMENT_TYPE_GV_SMALL = 6,
- CUSTOM_ELEMENT_TYPE_GV_SMALL_FLOAT = 7,
+ CUSTOM_ELEMENT_TYPE_ICON_LC = 4,
+ CUSTOM_ELEMENT_TYPE_GV_1 = 5,
+ CUSTOM_ELEMENT_TYPE_GV_2 = 6,
+ CUSTOM_ELEMENT_TYPE_GV_3 = 7,
+ CUSTOM_ELEMENT_TYPE_GV_4 = 8,
+ CUSTOM_ELEMENT_TYPE_GV_5 = 9,
+ CUSTOM_ELEMENT_TYPE_GV_FLOAT_1_1 = 10,
+ CUSTOM_ELEMENT_TYPE_GV_FLOAT_2_1 = 11,
+ CUSTOM_ELEMENT_TYPE_GV_FLOAT_2_2 = 12,
+ CUSTOM_ELEMENT_TYPE_GV_FLOAT_3_1 = 13,
+ CUSTOM_ELEMENT_TYPE_GV_FLOAT_3_2 = 14,
+ CUSTOM_ELEMENT_TYPE_GV_FLOAT_4_1 = 15,
+ CUSTOM_ELEMENT_TYPE_LC_1 = 16,
+ CUSTOM_ELEMENT_TYPE_LC_2 = 17,
+ CUSTOM_ELEMENT_TYPE_LC_3 = 18,
+ CUSTOM_ELEMENT_TYPE_LC_4 = 19,
+ CUSTOM_ELEMENT_TYPE_LC_5 = 20,
+ CUSTOM_ELEMENT_TYPE_LC_FLOAT_1_1 = 21,
+ CUSTOM_ELEMENT_TYPE_LC_FLOAT_2_1 = 22,
+ CUSTOM_ELEMENT_TYPE_LC_FLOAT_2_2 = 23,
+ CUSTOM_ELEMENT_TYPE_LC_FLOAT_3_1 = 24,
+ CUSTOM_ELEMENT_TYPE_LC_FLOAT_3_2 = 25,
+ CUSTOM_ELEMENT_TYPE_LC_FLOAT_4_1 = 26,
} osdCustomElementType_e;
typedef enum {
diff --git a/src/main/io/servo_sbus.c b/src/main/io/servo_sbus.c
index bfcd71b1158..121025c71a3 100644
--- a/src/main/io/servo_sbus.c
+++ b/src/main/io/servo_sbus.c
@@ -109,8 +109,8 @@ void sbusServoUpdate(uint8_t index, uint16_t value)
case 13: sbusFrame.channels.chan13 = sbusEncodeChannelValue(value); break;
case 14: sbusFrame.channels.chan14 = sbusEncodeChannelValue(value); break;
case 15: sbusFrame.channels.chan15 = sbusEncodeChannelValue(value); break;
- case 16: sbusFrame.channels.flags = value > PWM_RANGE_MIDDLE ? (sbusFrame.channels.flags | SBUS_FLAG_CHANNEL_17) : (sbusFrame.channels.flags & ~SBUS_FLAG_CHANNEL_17) ; break;
- case 17: sbusFrame.channels.flags = value > PWM_RANGE_MIDDLE ? (sbusFrame.channels.flags | SBUS_FLAG_CHANNEL_18) : (sbusFrame.channels.flags & ~SBUS_FLAG_CHANNEL_18) ; break;
+ case 16: sbusFrame.channels.flags = value > PWM_RANGE_MIDDLE ? (sbusFrame.channels.flags | SBUS_FLAG_CHANNEL_DG1) : (sbusFrame.channels.flags & ~SBUS_FLAG_CHANNEL_DG1) ; break;
+ case 17: sbusFrame.channels.flags = value > PWM_RANGE_MIDDLE ? (sbusFrame.channels.flags | SBUS_FLAG_CHANNEL_DG2) : (sbusFrame.channels.flags & ~SBUS_FLAG_CHANNEL_DG2) ; break;
default:
break;
}
diff --git a/src/main/io/servo_sbus.h b/src/main/io/servo_sbus.h
index 0dcc14ac8de..2def87b7bb7 100644
--- a/src/main/io/servo_sbus.h
+++ b/src/main/io/servo_sbus.h
@@ -24,6 +24,8 @@
#pragma once
+#define SERVO_SBUS_MAX_SERVOS 18
+
bool sbusServoInitialize(void);
void sbusServoUpdate(uint8_t index, uint16_t value);
void sbusServoSendUpdate(void);
diff --git a/src/main/io/vtx_smartaudio.c b/src/main/io/vtx_smartaudio.c
index 9901db409e2..7d5e213cce0 100644
--- a/src/main/io/vtx_smartaudio.c
+++ b/src/main/io/vtx_smartaudio.c
@@ -61,8 +61,8 @@
static serialPort_t *smartAudioSerialPort = NULL;
uint8_t saPowerCount = VTX_SMARTAUDIO_DEFAULT_POWER_COUNT;
-const char * saPowerNames[VTX_SMARTAUDIO_MAX_POWER_COUNT + 1] = {
- "----", "25 ", "200 ", "500 ", "800 ", " "
+const char *saPowerNames[VTX_SMARTAUDIO_MAX_POWER_COUNT + 1] = {
+ "----", "25 ", "200 ", "500 ", "800 ", " ", " ", " ", " "
};
// Save powerlevels reported from SA 2.1 devices here
@@ -73,7 +73,7 @@ static vtxDevice_t vtxSmartAudio = {
.vTable = &saVTable,
.capability.bandCount = VTX_SMARTAUDIO_BAND_COUNT,
.capability.channelCount = VTX_SMARTAUDIO_CHANNEL_COUNT,
- .capability.powerCount = VTX_SMARTAUDIO_MAX_POWER_COUNT,
+ .capability.powerCount = VTX_SMARTAUDIO_MAX_POWER_COUNT, // Should this be VTX_SMARTAUDIO_DEFAULT_POWER_COUNT?
.capability.bandNames = (char **)vtx58BandNames,
.capability.channelNames = (char **)vtx58ChannelNames,
.capability.powerNames = (char**)saPowerNames
@@ -124,7 +124,10 @@ saPowerTable_t saPowerTable[VTX_SMARTAUDIO_MAX_POWER_COUNT] = {
{ 200, 16 },
{ 500, 25 },
{ 800, 40 },
- { 0, 0 } // Placeholder
+ { 0, 0 }, // Placeholders
+ { 0, 0 },
+ { 0, 0 },
+ { 0, 0 }
};
// Last received device ('hard') states
diff --git a/src/main/io/vtx_smartaudio.h b/src/main/io/vtx_smartaudio.h
index c817f05a79f..ba8856b0faf 100644
--- a/src/main/io/vtx_smartaudio.h
+++ b/src/main/io/vtx_smartaudio.h
@@ -33,7 +33,7 @@
#define VTX_SMARTAUDIO_BAND_COUNT (VTX_SMARTAUDIO_MAX_BAND - VTX_SMARTAUDIO_MIN_BAND + 1)
#define VTX_SMARTAUDIO_CHANNEL_COUNT (VTX_SMARTAUDIO_MAX_CHANNEL - VTX_SMARTAUDIO_MIN_CHANNEL + 1)
-#define VTX_SMARTAUDIO_MAX_POWER_COUNT 5
+#define VTX_SMARTAUDIO_MAX_POWER_COUNT 8
#define VTX_SMARTAUDIO_DEFAULT_POWER_COUNT 4
#define VTX_SMARTAUDIO_DEFAULT_POWER 1
diff --git a/src/main/msp/msp_protocol_v2_inav.h b/src/main/msp/msp_protocol_v2_inav.h
index 8b4a09e87af..02c8c979aae 100755
--- a/src/main/msp/msp_protocol_v2_inav.h
+++ b/src/main/msp/msp_protocol_v2_inav.h
@@ -109,7 +109,8 @@
#define MSP2_ADSB_VEHICLE_LIST 0x2090
#define MSP2_INAV_CUSTOM_OSD_ELEMENTS 0x2100
-#define MSP2_INAV_SET_CUSTOM_OSD_ELEMENTS 0x2101
+#define MSP2_INAV_CUSTOM_OSD_ELEMENT 0x2101
+#define MSP2_INAV_SET_CUSTOM_OSD_ELEMENTS 0x2102
#define MSP2_INAV_SERVO_CONFIG 0x2200
#define MSP2_INAV_SET_SERVO_CONFIG 0x2201
\ No newline at end of file
diff --git a/src/main/msp/msp_serial.c b/src/main/msp/msp_serial.c
index 0dbc1768b8a..f72ea35da11 100644
--- a/src/main/msp/msp_serial.c
+++ b/src/main/msp/msp_serial.c
@@ -173,10 +173,8 @@ static bool mspSerialProcessReceivedData(mspPort_t *mspPort, uint8_t c)
case MSP_CHECKSUM_V1:
if (mspPort->checksum1 == c) {
mspPort->c_state = MSP_COMMAND_RECEIVED;
- SD(fprintf(stderr, "[MSPV1] Command received\n"));
} else {
mspPort->c_state = MSP_IDLE;
- SD(fprintf(stderr, "[MSPV1] Checksum error!\n"));
}
break;
@@ -229,7 +227,6 @@ static bool mspSerialProcessReceivedData(mspPort_t *mspPort, uint8_t c)
// Check for potential buffer overflow
if (hdrv2->size > MSP_PORT_INBUF_SIZE) {
mspPort->c_state = MSP_IDLE;
- SD(fprintf(stderr, "[MSPV2] Potential buffer overflow!\n"));
}
else {
mspPort->dataSize = hdrv2->size;
@@ -253,9 +250,7 @@ static bool mspSerialProcessReceivedData(mspPort_t *mspPort, uint8_t c)
case MSP_CHECKSUM_V2_NATIVE:
if (mspPort->checksum2 == c) {
mspPort->c_state = MSP_COMMAND_RECEIVED;
- SD(fprintf(stderr, "[MSPV2] command received!\n"));
} else {
- SD(fprintf(stderr, "[MSPV2] Checksum error!\n"));
mspPort->c_state = MSP_IDLE;
}
break;
diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
index f7c7303d138..4ae4cf228db 100644
--- a/src/main/navigation/navigation.c
+++ b/src/main/navigation/navigation.c
@@ -1339,6 +1339,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_IN_PROGRESS
}
const bool mcRollStickHeadingAdjustmentActive = STATE(MULTIROTOR) && ABS(rcCommand[ROLL]) > rcControlsConfig()->pos_hold_deadband;
+ static bool adjustmentWasActive = false;
// User demanding yaw -> yaw stick on FW, yaw or roll sticks on MR
// We record the desired course and change the desired target in the meanwhile
@@ -1353,9 +1354,19 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_IN_PROGRESS
if (timeDifference > 100) timeDifference = 0; // if adjustment was called long time ago, reset the time difference.
float rateTarget = scaleRangef((float)headingAdjustCommand, -500.0f, 500.0f, -cruiseYawRate, cruiseYawRate);
float centidegsPerIteration = rateTarget * MS2S(timeDifference);
- posControl.cruise.course = wrap_36000(posControl.cruise.course - centidegsPerIteration);
- DEBUG_SET(DEBUG_CRUISE, 1, CENTIDEGREES_TO_DEGREES(posControl.cruise.course));
+
+ if (ABS(wrap_18000(posControl.cruise.course - posControl.actualState.cog)) < fabsf(rateTarget)) {
+ posControl.cruise.course = wrap_36000(posControl.cruise.course - centidegsPerIteration);
+ }
+
posControl.cruise.lastCourseAdjustmentTime = currentTimeMs;
+ adjustmentWasActive = true;
+
+ DEBUG_SET(DEBUG_CRUISE, 1, CENTIDEGREES_TO_DEGREES(posControl.cruise.course));
+ } else if (STATE(AIRPLANE) && adjustmentWasActive) {
+ posControl.cruise.course = posControl.actualState.cog - DEGREES_TO_CENTIDEGREES(gyroRateDps(YAW));
+ resetPositionController();
+ adjustmentWasActive = false;
} else if (currentTimeMs - posControl.cruise.lastCourseAdjustmentTime > 4000) {
posControl.cruise.previousCourse = posControl.cruise.course;
}
@@ -1848,6 +1859,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION(nav
case NAV_WP_ACTION_LAND:
calculateAndSetActiveWaypoint(&posControl.waypointList[posControl.activeWaypointIndex]);
posControl.wpInitialDistance = calculateDistanceToDestination(&posControl.activeWaypoint.pos);
+ posControl.wpInitialAltitude = posControl.actualState.abs.pos.z;
posControl.wpAltitudeReached = false;
return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_IN_PROGRESS
@@ -1911,16 +1923,12 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(na
fpVector3_t tmpWaypoint;
tmpWaypoint.x = posControl.activeWaypoint.pos.x;
tmpWaypoint.y = posControl.activeWaypoint.pos.y;
- setDesiredPosition(&tmpWaypoint, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_BEARING);
-
- // Use linear climb between WPs arriving at WP altitude when within 10% of total distance to WP
- // Update climb rate until within 100cm of total climb xy distance to WP
- float climbRate = 0.0f;
- if (posControl.wpDistance - 0.1f * posControl.wpInitialDistance > 100.0f) {
- climbRate = posControl.actualState.velXY * (posControl.activeWaypoint.pos.z - posControl.actualState.abs.pos.z) /
- (posControl.wpDistance - 0.1f * posControl.wpInitialDistance);
- }
- updateClimbRateToAltitudeController(climbRate, posControl.activeWaypoint.pos.z, ROC_TO_ALT_TARGET);
+ /* Use linear climb/descent between WPs arriving at WP altitude when within 10% of total distance to WP */
+ tmpWaypoint.z = scaleRangef(constrainf(posControl.wpDistance, 0.1f * posControl.wpInitialDistance, posControl.wpInitialDistance),
+ posControl.wpInitialDistance, 0.1f * posControl.wpInitialDistance,
+ posControl.wpInitialAltitude, posControl.activeWaypoint.pos.z);
+
+ setDesiredPosition(&tmpWaypoint, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING);
if(STATE(MULTIROTOR)) {
switch (wpHeadingControl.mode) {
@@ -3692,7 +3700,8 @@ void setWaypoint(uint8_t wpNumber, const navWaypoint_t * wpData)
setDesiredPosition(&wpPos.pos, DEGREES_TO_CENTIDEGREES(wpData->p1), waypointUpdateFlags);
}
// WP #1 - #NAV_MAX_WAYPOINTS - common waypoints - pre-programmed mission
- else if ((wpNumber >= 1) && (wpNumber <= NAV_MAX_WAYPOINTS) && !ARMING_FLAG(ARMED)) {
+ else if ((wpNumber >= 1) && (wpNumber <= NAV_MAX_WAYPOINTS) && !FLIGHT_MODE(NAV_WP_MODE)) {
+ // WP upload is not allowed why WP mode is active
if (wpData->action == NAV_WP_ACTION_WAYPOINT || wpData->action == NAV_WP_ACTION_JUMP || wpData->action == NAV_WP_ACTION_RTH || wpData->action == NAV_WP_ACTION_HOLD_TIME || wpData->action == NAV_WP_ACTION_LAND || wpData->action == NAV_WP_ACTION_SET_POI || wpData->action == NAV_WP_ACTION_SET_HEAD ) {
// Only allow upload next waypoint (continue upload mission) or first waypoint (new mission)
static int8_t nonGeoWaypointCount = 0;
@@ -3714,6 +3723,10 @@ void setWaypoint(uint8_t wpNumber, const navWaypoint_t * wpData)
posControl.geoWaypointCount = posControl.waypointCount - nonGeoWaypointCount;
if (posControl.waypointListValid) {
nonGeoWaypointCount = 0;
+ // If active WP index is bigger than total mission WP number, reset active WP index (Mission Upload mid flight with interrupted mission) if RESUME is enabled
+ if (posControl.activeWaypointIndex > posControl.waypointCount) {
+ posControl.activeWaypointIndex = 0;
+ }
}
}
}
diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c
index 71a7f99fc34..ff38b2e17e5 100755
--- a/src/main/navigation/navigation_fixedwing.c
+++ b/src/main/navigation/navigation_fixedwing.c
@@ -399,38 +399,52 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta
virtualTargetBearing = calculateBearingToDestination(&virtualDesiredPosition);
}
- /* If waypoint tracking enabled quickly force craft toward waypoint course line and closely track along it */
- if (navConfig()->fw.wp_tracking_accuracy && isWaypointNavTrackingActive() && !needToCalculateCircularLoiter) {
- // courseVirtualCorrection initially used to determine current position relative to course line for later use
- int32_t courseVirtualCorrection = wrap_18000(posControl.activeWaypoint.bearing - virtualTargetBearing);
- navCrossTrackError = ABS(posControl.wpDistance * sin_approx(CENTIDEGREES_TO_RADIANS(courseVirtualCorrection)));
-
- // tracking only active when certain distance and heading conditions are met
- if ((ABS(wrap_18000(virtualTargetBearing - posControl.actualState.cog)) < 9000 || posControl.wpDistance < 1000.0f) && navCrossTrackError > 200) {
- int32_t courseHeadingError = wrap_18000(posControl.activeWaypoint.bearing - posControl.actualState.cog);
-
- // captureFactor adjusts distance/heading sensitivity balance when closing in on course line.
- // Closing distance threashold based on speed and an assumed 1 second response time.
- float captureFactor = navCrossTrackError < posControl.actualState.velXY ? constrainf(2.0f - ABS(courseHeadingError) / 500.0f, 0.0f, 2.0f) : 1.0f;
-
- // bias between reducing distance to course line and aligning with course heading adjusted by waypoint_tracking_accuracy
- // initial courseCorrectionFactor based on distance to course line
- float courseCorrectionFactor = constrainf(captureFactor * navCrossTrackError / (1000.0f * navConfig()->fw.wp_tracking_accuracy), 0.0f, 1.0f);
- courseCorrectionFactor = courseVirtualCorrection < 0 ? -courseCorrectionFactor : courseCorrectionFactor;
-
- // course heading alignment factor
- float courseHeadingFactor = constrainf(courseHeadingError / 18000.0f, 0.0f, 1.0f);
- courseHeadingFactor = courseHeadingError < 0 ? -courseHeadingFactor : courseHeadingFactor;
-
- // final courseCorrectionFactor combining distance and heading factors
- courseCorrectionFactor = constrainf(courseCorrectionFactor - courseHeadingFactor, -1.0f, 1.0f);
-
- // final courseVirtualCorrection value
- courseVirtualCorrection = DEGREES_TO_CENTIDEGREES(navConfig()->fw.wp_tracking_max_angle) * courseCorrectionFactor;
- virtualTargetBearing = wrap_36000(posControl.activeWaypoint.bearing - courseVirtualCorrection);
+ if (isWaypointNavTrackingActive()) {
+ /* Calculate cross track error */
+ posControl.wpDistance = calculateDistanceToDestination(&posControl.activeWaypoint.pos);
+
+ fpVector3_t virtualCoursePoint;
+ virtualCoursePoint.x = posControl.activeWaypoint.pos.x -
+ posControl.wpDistance * cos_approx(CENTIDEGREES_TO_RADIANS(posControl.activeWaypoint.bearing));
+ virtualCoursePoint.y = posControl.activeWaypoint.pos.y -
+ posControl.wpDistance * sin_approx(CENTIDEGREES_TO_RADIANS(posControl.activeWaypoint.bearing));
+ navCrossTrackError = calculateDistanceToDestination(&virtualCoursePoint);
+
+ /* If waypoint tracking enabled force craft toward and closely track along waypoint course line */
+ if (navConfig()->fw.wp_tracking_accuracy && !needToCalculateCircularLoiter) {
+ static float crossTrackErrorRate;
+ static timeUs_t previousCrossTrackErrorUpdateTime;
+ static float previousCrossTrackError = 0.0f;
+ static pt1Filter_t fwCrossTrackErrorRateFilterState;
+
+ if ((currentTimeUs - previousCrossTrackErrorUpdateTime) >= HZ2US(20) && fabsf(previousCrossTrackError - navCrossTrackError) > 10.0f) {
+ const float crossTrackErrorDtSec = US2S(currentTimeUs - previousCrossTrackErrorUpdateTime);
+ if (fabsf(previousCrossTrackError - navCrossTrackError) < 500.0f) {
+ crossTrackErrorRate = (previousCrossTrackError - navCrossTrackError) / crossTrackErrorDtSec;
+ }
+ crossTrackErrorRate = pt1FilterApply4(&fwCrossTrackErrorRateFilterState, crossTrackErrorRate, 3.0f, crossTrackErrorDtSec);
+ previousCrossTrackErrorUpdateTime = currentTimeUs;
+ previousCrossTrackError = navCrossTrackError;
+ }
+
+ uint16_t trackingDeadband = METERS_TO_CENTIMETERS(navConfig()->fw.wp_tracking_accuracy);
+
+ if ((ABS(wrap_18000(virtualTargetBearing - posControl.actualState.cog)) < 9000 || posControl.wpDistance < 1000.0f) && navCrossTrackError > trackingDeadband) {
+ float adjustmentFactor = wrap_18000(posControl.activeWaypoint.bearing - virtualTargetBearing);
+ uint16_t angleLimit = DEGREES_TO_CENTIDEGREES(navConfig()->fw.wp_tracking_max_angle);
+
+ /* Apply heading adjustment to match crossTrackErrorRate with fixed convergence speed profile */
+ float maxApproachSpeed = posControl.actualState.velXY * sin_approx(CENTIDEGREES_TO_RADIANS(angleLimit));
+ float desiredApproachSpeed = constrainf(navCrossTrackError / 3.0f, 50.0f, maxApproachSpeed);
+ adjustmentFactor = SIGN(adjustmentFactor) * navCrossTrackError * ((desiredApproachSpeed - crossTrackErrorRate) / desiredApproachSpeed);
+
+ /* Calculate final adjusted virtualTargetBearing */
+ uint16_t limit = constrainf(navCrossTrackError, 1000.0f, angleLimit);
+ adjustmentFactor = constrainf(adjustmentFactor, -limit, limit);
+ virtualTargetBearing = wrap_36000(posControl.activeWaypoint.bearing - adjustmentFactor);
+ }
}
}
-
/*
* Calculate NAV heading error
* Units are centidegrees
diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c
index 8332d8a70aa..e23b6e1f3cf 100755
--- a/src/main/navigation/navigation_pos_estimator.c
+++ b/src/main/navigation/navigation_pos_estimator.c
@@ -637,8 +637,7 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx)
ctx->estPosCorr.z += gpsAltResidual * w_z_gps_p * ctx->dt;
ctx->estVelCorr.z += gpsAltResidual * sq(w_z_gps_p) * ctx->dt;
ctx->estVelCorr.z += gpsVelZResidual * positionEstimationConfig()->w_z_gps_v * ctx->dt;
-
- ctx->newEPV = updateEPE(posEstimator.est.epv, ctx->dt, MAX(posEstimator.gps.epv, gpsAltResidual), w_z_gps_p);
+ ctx->newEPV = updateEPE(posEstimator.est.epv, ctx->dt, MAX(posEstimator.gps.epv, fabsf(gpsAltResidual)), w_z_gps_p);
// Accelerometer bias
ctx->accBiasCorr.z -= gpsAltResidual * sq(w_z_gps_p);
diff --git a/src/main/rx/jetiexbus.c b/src/main/rx/jetiexbus.c
index 35564833994..9e3c04d2c12 100644
--- a/src/main/rx/jetiexbus.c
+++ b/src/main/rx/jetiexbus.c
@@ -64,7 +64,7 @@
#define JETIEXBUS_OPTIONS (SERIAL_STOPBITS_1 | SERIAL_PARITY_NO)
#define JETIEXBUS_MIN_FRAME_GAP 1000
-#ifdef USE_24CHANNELS
+#ifdef USE_34CHANNELS
#define JETIEXBUS_CHANNEL_COUNT 24
#else
#define JETIEXBUS_CHANNEL_COUNT 16
diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h
index 9e4816ec64c..c841838a5ea 100644
--- a/src/main/rx/rx.h
+++ b/src/main/rx/rx.h
@@ -84,11 +84,7 @@ typedef enum {
SERIALRX_SBUS2,
} rxSerialReceiverType_e;
-#ifdef USE_24CHANNELS
-#define MAX_SUPPORTED_RC_CHANNEL_COUNT 26
-#else
-#define MAX_SUPPORTED_RC_CHANNEL_COUNT 18
-#endif
+#define MAX_SUPPORTED_RC_CHANNEL_COUNT 34
#define NON_AUX_CHANNEL_COUNT 4
#define MAX_AUX_CHANNEL_COUNT (MAX_SUPPORTED_RC_CHANNEL_COUNT - NON_AUX_CHANNEL_COUNT)
diff --git a/src/main/rx/sbus.c b/src/main/rx/sbus.c
index 9c0fa30bd11..f08267eeb1d 100644
--- a/src/main/rx/sbus.c
+++ b/src/main/rx/sbus.c
@@ -54,13 +54,16 @@
typedef enum {
STATE_SBUS_SYNC = 0,
STATE_SBUS_PAYLOAD,
+ STATE_SBUS26_PAYLOAD,
STATE_SBUS_WAIT_SYNC
} sbusDecoderState_e;
typedef struct sbusFrameData_s {
sbusDecoderState_e state;
volatile sbusFrame_t frame;
+ volatile sbusFrame_t frameHigh;
volatile bool frameDone;
+ volatile bool is26channels;
uint8_t buffer[SBUS_FRAME_SIZE];
uint8_t position;
timeUs_t lastActivityTimeUs;
@@ -68,6 +71,7 @@ typedef struct sbusFrameData_s {
static uint8_t sbus2ActiveTelemetryPage = 0;
static uint8_t sbus2ActiveTelemetrySlot = 0;
+static uint8_t sbus2ShortFrameInterval = 0;
timeUs_t frameTime = 0;
// Receive ISR callback
@@ -78,8 +82,17 @@ static void sbusDataReceive(uint16_t c, void *data)
const timeDelta_t timeSinceLastByteUs = cmpTimeUs(currentTimeUs, sbusFrameData->lastActivityTimeUs);
sbusFrameData->lastActivityTimeUs = currentTimeUs;
+ const int32_t syncInterval = sbus2ShortFrameInterval
+ ? ((6300 - SBUS_BYTE_TIME_US(25)) / 2)
+ : rxConfig()->sbusSyncInterval;
+
+
// Handle inter-frame gap. We dwell in STATE_SBUS_WAIT_SYNC state ignoring all incoming bytes until we get long enough quite period on the wire
- if (sbusFrameData->state == STATE_SBUS_WAIT_SYNC && timeSinceLastByteUs >= rxConfig()->sbusSyncInterval) {
+ if ((sbusFrameData->state == STATE_SBUS_WAIT_SYNC && timeSinceLastByteUs >= syncInterval)
+ || (rxConfig()->serialrx_provider == SERIALRX_SBUS2 && timeSinceLastByteUs >= SBUS_BYTE_TIME_US(3))) {
+ sbusFrameData->state = STATE_SBUS_SYNC;
+ } else if ((sbusFrameData->state == STATE_SBUS_PAYLOAD || sbusFrameData->state == STATE_SBUS26_PAYLOAD) && timeSinceLastByteUs >= SBUS_BYTE_TIME_US(3)) {
+ // payload is pausing too long, possible if some telemetry have been sent between frames, or false positves mid frame
sbusFrameData->state = STATE_SBUS_SYNC;
}
@@ -89,6 +102,10 @@ static void sbusDataReceive(uint16_t c, void *data)
sbusFrameData->position = 0;
sbusFrameData->buffer[sbusFrameData->position++] = (uint8_t)c;
sbusFrameData->state = STATE_SBUS_PAYLOAD;
+ } else if (c == SBUS2_HIGHFRAME_BEGIN_BYTE) {
+ sbusFrameData->position = 0;
+ sbusFrameData->buffer[sbusFrameData->position++] = (uint8_t)c;
+ sbusFrameData->state = STATE_SBUS26_PAYLOAD;
}
break;
@@ -103,19 +120,21 @@ static void sbusDataReceive(uint16_t c, void *data)
switch (frame->endByte) {
case 0x00: // This is S.BUS 1
case 0x04: // S.BUS 2 telemetry page 1
+ case 0x08: // S.BUS 2 fast frame pace, not telemetry.
case 0x14: // S.BUS 2 telemetry page 2
case 0x24: // S.BUS 2 telemetry page 3
case 0x34: // S.BUS 2 telemetry page 4
if(frame->endByte & 0x4) {
sbus2ActiveTelemetryPage = (frame->endByte >> 4) & 0xF;
frameTime = currentTimeUs;
+ } else if(frame->endByte == 0x08) {
+ sbus2ShortFrameInterval = 1;
} else {
sbus2ActiveTelemetryPage = 0;
sbus2ActiveTelemetrySlot = 0;
frameTime = -1;
}
-
frameValid = true;
sbusFrameData->state = STATE_SBUS_WAIT_SYNC;
break;
@@ -134,6 +153,40 @@ static void sbusDataReceive(uint16_t c, void *data)
}
break;
+ case STATE_SBUS26_PAYLOAD:
+ sbusFrameData->buffer[sbusFrameData->position++] = (uint8_t)c;
+
+ if (sbusFrameData->position == SBUS_FRAME_SIZE) {
+ const sbusFrame_t * frame = (sbusFrame_t *)&sbusFrameData->buffer[0];
+ bool frameValid = false;
+
+ // Do some sanity check
+ switch (frame->endByte) {
+ case 0x00:
+ case 0x04: // S.BUS 2 telemetry page 1
+ case 0x14: // S.BUS 2 telemetry page 2
+ case 0x24: // S.BUS 2 telemetry page 3
+ case 0x34: // S.BUS 2 telemetry page 4
+ frameTime = -1; // ignore this one, as you can't fit telemetry between this and the next frame.
+ frameValid = true;
+ sbusFrameData->state = STATE_SBUS_SYNC; // Next piece of data should be a sync byte
+ break;
+
+ default: // Failed end marker
+ frameValid = false;
+ sbusFrameData->state = STATE_SBUS_WAIT_SYNC;
+ break;
+ }
+
+ // Frame seems sane, pass data to decoder
+ if (!sbusFrameData->frameDone && frameValid) {
+ memcpy((void *)&sbusFrameData->frameHigh, (void *)&sbusFrameData->buffer[0], SBUS_FRAME_SIZE);
+ sbusFrameData->frameDone = true;
+ sbusFrameData->is26channels = true;
+ }
+ }
+ break;
+
case STATE_SBUS_WAIT_SYNC:
// Stay at this state and do nothing. Exit will be handled before byte is processed if the
// inter-frame gap is long enough
@@ -149,14 +202,20 @@ static uint8_t sbusFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
return RX_FRAME_PENDING;
}
+ uint8_t retValue = 0;
// Decode channel data and store return value
- const uint8_t retValue = sbusChannelsDecode(rxRuntimeConfig, (void *)&sbusFrameData->frame.channels);
+ if (sbusFrameData->is26channels)
+ {
+ retValue = sbus26ChannelsDecode(rxRuntimeConfig, (void *)&sbusFrameData->frame.channels, false);
+ retValue |= sbus26ChannelsDecode(rxRuntimeConfig, (void *)&sbusFrameData->frameHigh.channels, true);
+
+ } else {
+ retValue = sbusChannelsDecode(rxRuntimeConfig, (void *)&sbusFrameData->frame.channels);
+ }
// Reset the frameDone flag - tell ISR that we're ready to receive next frame
sbusFrameData->frameDone = false;
- //taskSendSbus2Telemetry(micros());
-
// Calculate "virtual link quality based on packet loss metric"
if (retValue & RX_FRAME_COMPLETE) {
lqTrackerAccumulate(rxRuntimeConfig->lqTracker, ((retValue & RX_FRAME_DROPPED) || (retValue & RX_FRAME_FAILSAFE)) ? 0 : RSSI_MAX_VALUE);
@@ -168,7 +227,7 @@ static uint8_t sbusFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
static bool sbusInitEx(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, uint32_t sbusBaudRate)
{
static uint16_t sbusChannelData[SBUS_MAX_CHANNEL];
- static sbusFrameData_t sbusFrameData;
+ static sbusFrameData_t sbusFrameData = { .is26channels = false};
rxRuntimeConfig->channelData = sbusChannelData;
rxRuntimeConfig->frameData = &sbusFrameData;
diff --git a/src/main/rx/sbus.h b/src/main/rx/sbus.h
index aa550b618e1..20ed82e2dae 100644
--- a/src/main/rx/sbus.h
+++ b/src/main/rx/sbus.h
@@ -24,6 +24,8 @@
bool sbusInit(const rxConfig_t *initialRxConfig, rxRuntimeConfig_t *rxRuntimeConfig);
bool sbusInitFast(const rxConfig_t *initialRxConfig, rxRuntimeConfig_t *rxRuntimeConfig);
+#define SBUS_BYTE_TIME_US(bytes) MS2US(10 * 12 * bytes) // 10us per bit * (1 start + 8 data + 1 parity + 2 stop) * number of bytes
+
#ifdef USE_TELEMETRY_SBUS2
uint8_t sbusGetCurrentTelemetryPage(void);
uint8_t sbusGetCurrentTelemetryNextSlot(void);
diff --git a/src/main/rx/sbus_channels.c b/src/main/rx/sbus_channels.c
index 99ca76393d6..4794df2f7db 100644
--- a/src/main/rx/sbus_channels.c
+++ b/src/main/rx/sbus_channels.c
@@ -26,6 +26,8 @@
#include "common/utils.h"
#include "common/maths.h"
+#include "build/debug.h"
+
#include "rx/sbus_channels.h"
@@ -34,6 +36,55 @@
STATIC_ASSERT(SBUS_FRAME_SIZE == sizeof(sbusFrame_t), SBUS_FRAME_SIZE_doesnt_match_sbusFrame_t);
+uint8_t sbus26ChannelsDecode(rxRuntimeConfig_t *rxRuntimeConfig, const sbusChannels_t *channels, bool highChannels)
+{
+ uint8_t offset = highChannels ? 16 : 0;
+ uint16_t *sbusChannelData = rxRuntimeConfig->channelData;
+ sbusChannelData[0 + offset] = channels->chan0;
+ sbusChannelData[1 + offset] = channels->chan1;
+ sbusChannelData[2 + offset] = channels->chan2;
+ sbusChannelData[3 + offset] = channels->chan3;
+ sbusChannelData[4 + offset] = channels->chan4;
+ sbusChannelData[5 + offset] = channels->chan5;
+ sbusChannelData[6 + offset] = channels->chan6;
+ sbusChannelData[7 + offset] = channels->chan7;
+ sbusChannelData[8 + offset] = channels->chan8;
+ sbusChannelData[9 + offset] = channels->chan9;
+ sbusChannelData[10 + offset] = channels->chan10;
+ sbusChannelData[11 + offset] = channels->chan11;
+ sbusChannelData[12 + offset] = channels->chan12;
+ sbusChannelData[13 + offset] = channels->chan13;
+ sbusChannelData[14 + offset] = channels->chan14;
+ sbusChannelData[15 + offset] = channels->chan15;
+
+ if (!highChannels) {
+ if (channels->flags & SBUS_FLAG_CHANNEL_DG1) {
+ sbusChannelData[32] = SBUS_DIGITAL_CHANNEL_MAX;
+ } else {
+ sbusChannelData[32] = SBUS_DIGITAL_CHANNEL_MIN;
+ }
+
+ if (channels->flags & SBUS_FLAG_CHANNEL_DG2) {
+ sbusChannelData[33] = SBUS_DIGITAL_CHANNEL_MAX;
+ } else {
+ sbusChannelData[33] = SBUS_DIGITAL_CHANNEL_MIN;
+ }
+ }
+
+ if (channels->flags & SBUS_FLAG_FAILSAFE_ACTIVE) {
+ // internal failsafe enabled and rx failsafe flag set
+ // RX *should* still be sending valid channel data, so use it.
+ return RX_FRAME_COMPLETE | RX_FRAME_FAILSAFE;
+ }
+
+ if (channels->flags & SBUS_FLAG_SIGNAL_LOSS) {
+ // The received data is a repeat of the last valid data so can be considered complete.
+ return RX_FRAME_COMPLETE | RX_FRAME_DROPPED;
+ }
+
+ return RX_FRAME_COMPLETE;
+}
+
uint8_t sbusChannelsDecode(rxRuntimeConfig_t *rxRuntimeConfig, const sbusChannels_t *channels)
{
uint16_t *sbusChannelData = rxRuntimeConfig->channelData;
@@ -54,13 +105,13 @@ uint8_t sbusChannelsDecode(rxRuntimeConfig_t *rxRuntimeConfig, const sbusChannel
sbusChannelData[14] = channels->chan14;
sbusChannelData[15] = channels->chan15;
- if (channels->flags & SBUS_FLAG_CHANNEL_17) {
+ if (channels->flags & SBUS_FLAG_CHANNEL_DG1) {
sbusChannelData[16] = SBUS_DIGITAL_CHANNEL_MAX;
} else {
sbusChannelData[16] = SBUS_DIGITAL_CHANNEL_MIN;
}
- if (channels->flags & SBUS_FLAG_CHANNEL_18) {
+ if (channels->flags & SBUS_FLAG_CHANNEL_DG2) {
sbusChannelData[17] = SBUS_DIGITAL_CHANNEL_MAX;
} else {
sbusChannelData[17] = SBUS_DIGITAL_CHANNEL_MIN;
diff --git a/src/main/rx/sbus_channels.h b/src/main/rx/sbus_channels.h
index 467fdda5afc..a0276ac005c 100644
--- a/src/main/rx/sbus_channels.h
+++ b/src/main/rx/sbus_channels.h
@@ -20,17 +20,18 @@
#include
#include "rx/rx.h"
-#define SBUS_MAX_CHANNEL 18
+#define SBUS_MAX_CHANNEL 34
-#define SBUS_FLAG_CHANNEL_17 (1 << 0)
-#define SBUS_FLAG_CHANNEL_18 (1 << 1)
+#define SBUS_FLAG_CHANNEL_DG1 (1 << 0)
+#define SBUS_FLAG_CHANNEL_DG2 (1 << 1)
#define SBUS_FLAG_SIGNAL_LOSS (1 << 2)
#define SBUS_FLAG_FAILSAFE_ACTIVE (1 << 3)
#define SBUS_CHANNEL_DATA_LENGTH sizeof(sbusChannels_t)
#define SBUS_FRAME_SIZE (SBUS_CHANNEL_DATA_LENGTH + 2)
-#define SBUS_FRAME_BEGIN_BYTE 0x0F
+#define SBUS_FRAME_BEGIN_BYTE ((uint8_t)0x0F)
+#define SBUS2_HIGHFRAME_BEGIN_BYTE ((uint8_t)0x2F)
#define SBUS_BAUDRATE 100000
#define SBUS_BAUDRATE_FAST 200000
@@ -78,5 +79,6 @@ uint16_t sbusDecodeChannelValue(uint16_t sbusValue, bool safeValuesOnly);
uint16_t sbusEncodeChannelValue(uint16_t rcValue);
uint8_t sbusChannelsDecode(rxRuntimeConfig_t *rxRuntimeConfig, const sbusChannels_t *channels);
+uint8_t sbus26ChannelsDecode(rxRuntimeConfig_t *rxRuntimeConfig, const sbusChannels_t *channels, bool highChannels);
void sbusChannelsInit(rxRuntimeConfig_t *rxRuntimeConfig);
diff --git a/src/main/target/AIKONF7/CMakeLists.txt b/src/main/target/AIKONF7/CMakeLists.txt
new file mode 100644
index 00000000000..86c842526d6
--- /dev/null
+++ b/src/main/target/AIKONF7/CMakeLists.txt
@@ -0,0 +1 @@
+target_stm32f722xe(AIKONF7 SKIP_RELEASES)
diff --git a/src/main/target/AIKONF7/target.c b/src/main/target/AIKONF7/target.c
new file mode 100644
index 00000000000..7f5d8453e14
--- /dev/null
+++ b/src/main/target/AIKONF7/target.c
@@ -0,0 +1,60 @@
+/*
+ * 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 .
+ *
+ * This target has been autgenerated by bf2inav.py
+ */
+
+#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, CH1, PC6, TIM_USE_OUTPUT_AUTO, 0, 0),
+ //DEF_TIM(TIM8, CH1, PC6, TIM_USE_OUTPUT_AUTO, 0, 0),
+
+ DEF_TIM(TIM3, CH2, PC7, TIM_USE_OUTPUT_AUTO, 0, 0),
+ //DEF_TIM(TIM8, CH2, PC7, TIM_USE_OUTPUT_AUTO, 0, 0),
+
+ DEF_TIM(TIM3, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 0),
+ //DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 0),
+
+ DEF_TIM(TIM3, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0),
+ //DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0),
+
+ DEF_TIM(TIM4, CH1, PB6, TIM_USE_OUTPUT_AUTO, 0, 0),
+
+ DEF_TIM(TIM4, CH3, PB8, TIM_USE_OUTPUT_AUTO, 0, 0),
+ //DEF_TIM(TIM10, CH1, PB8, TIM_USE_OUTPUT_AUTO, 0, 0),
+
+ DEF_TIM(TIM4, CH2, PB7, TIM_USE_OUTPUT_AUTO, 0, 0),
+
+ DEF_TIM(TIM1, CH3N, PB1, TIM_USE_OUTPUT_AUTO, 0, 0),
+ //DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0),
+ //DEF_TIM(TIM8, CH3N, PB1, TIM_USE_OUTPUT_AUTO, 0, 0),
+
+ DEF_TIM(TIM2, CH1, PA15, TIM_USE_LED, 0, 0),
+
+};
+
+const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]);
diff --git a/src/main/target/AIKONF7/target.h b/src/main/target/AIKONF7/target.h
new file mode 100644
index 00000000000..f738d688a88
--- /dev/null
+++ b/src/main/target/AIKONF7/target.h
@@ -0,0 +1,144 @@
+/*
+ * 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 .
+ *
+ * This target has been autgenerated by bf2inav.py
+ */
+
+#pragma once
+
+//#define USE_TARGET_CONFIG
+
+#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TELEMETRY )
+
+
+
+#define TARGET_BOARD_IDENTIFIER "RPTY"
+#define USBD_PRODUCT_STRING "AIKONF7"
+// Beeper
+#define USE_BEEPER
+#define BEEPER PC15
+#define BEEPER_INVERTED
+// Leds
+#define USE_LED_STRIP
+#define WS2811_PIN PA15
+#define LED0 PC13
+// UARTs
+#define USB_IO
+#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 PC11
+#define UART3_TX_PIN PC10
+#define USE_UART4
+#define UART4_RX_PIN PA1
+#define UART4_TX_PIN PA0
+#define USE_UART5
+#define UART5_RX_PIN PD2
+#define UART5_TX_PIN PC12
+#define SERIAL_PORT_COUNT 6
+#define DEFAULT_RX_TYPE RX_TYPE_SERIAL
+#define SERIALRX_PROVIDER SERIALRX_CRSF
+// SPI
+#define USE_SPI
+#define USE_SPI_DEVICE_1
+#define SPI1_SCK_PIN PA5
+#define SPI1_MISO_PIN PA7
+#define SPI1_MOSI_PIN PA6
+#define USE_SPI_DEVICE_2
+#define SPI2_SCK_PIN PB13
+#define SPI2_MISO_PIN PB15
+#define SPI2_MOSI_PIN PC2
+#define USE_SPI_DEVICE_3
+#define SPI3_SCK_PIN PB3
+#define SPI3_MISO_PIN PB5
+#define SPI3_MOSI_PIN PB4
+// I2C
+#define USE_I2C
+#define USE_I2C_DEVICE_2
+#define I2C2_SCL PB10
+#define I2C2_SDA PB11
+// ADC
+#define ADC_CHANNEL_1_PIN PC0
+#define VBAT_ADC_CHANNEL ADC_CHN_1
+#define ADC_CHANNEL_2_PIN PC1
+#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2
+#define ADC_CHANNEL_3_PIN PC3
+#define RSSI_ADC_CHANNEL ADC_CHN_3
+#define USE_ADC
+#define ADC_INSTANCE ADC1
+// Gyro & ACC
+#define USE_IMU_BMI270
+#define BMI270_CS_PIN PA4
+#define BMI270_SPI_BUS BUS_SPI1
+#define IMU_BMI270_ALIGN CW0_DEG
+#define USE_IMU_MPU6000
+#define MPU6000_CS_PIN PA4
+#define MPU6000_SPI_BUS BUS_SPI1
+#define IMU_MPU6000_ALIGN CW0_DEG
+// BARO
+#define USE_BARO
+#define USE_BARO_BMP280
+#define USE_BARO_SPI_BMP280
+#define BMP280_SPI_BUS BUS_SPI3
+#define BMP280_CS_PIN PB2
+// OSD
+#define USE_MAX7456
+#define MAX7456_CS_PIN PB12
+#define MAX7456_SPI_BUS BUS_SPI2
+// Blackbox
+#define USE_FLASHFS
+#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT
+#define USE_FLASH_M25P16
+#define M25P16_SPI_BUS BUS_SPI3
+#define M25P16_CS_PIN PB0
+#define USE_FLASH_W25M
+#define W25M_SPI_BUS BUS_SPI3
+#define W25M_CS_PIN PB0
+#define USE_FLASH_W25M02G
+#define W25M02G_SPI_BUS BUS_SPI3
+#define W25M02G_CS_PIN PB0
+#define USE_FLASH_W25M512
+#define W25M512_SPI_BUS BUS_SPI3
+#define W25M512_CS_PIN PB0
+#define USE_FLASH_W25N01G
+#define W25N01G_SPI_BUS BUS_SPI3
+#define W25N01G_CS_PIN PB0
+
+// PINIO
+
+#define USE_PINIO
+#define USE_PINIOBOX
+#define PINIO1_PIN PC14
+
+
+// Others
+
+#define MAX_PWM_OUTPUT_PORTS 8
+#define USE_SERIAL_4WAY_BLHELI_INTERFACE
+#define USE_DSHOT
+#define USE_ESC_SENSOR
+
+#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 TARGET_IO_PORTF 0xffff
diff --git a/src/main/target/AOCODARCF7MINI/target.c b/src/main/target/AOCODARCF7MINI/target.c
index f01d2c73e81..72140f50fac 100644
--- a/src/main/target/AOCODARCF7MINI/target.c
+++ b/src/main/target/AOCODARCF7MINI/target.c
@@ -47,11 +47,13 @@ timerHardware_t timerHardware[] = {
#if defined(AOCODARCF7MINI_V2)
DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 0), // S5 D(2, 4, 7)
DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), // S6 D(2, 7, 7)
+ DEF_TIM(TIM4, CH1, PB6, TIM_USE_OUTPUT_AUTO, 0, 0), // S7 D(1, 0, 2)
#else
+ DEF_TIM(TIM4, CH1, PB6, TIM_USE_OUTPUT_AUTO, 0, 0), // S5
DEF_TIM(TIM2, CH2, PB3, TIM_USE_OUTPUT_AUTO, 0, 0), // S6 D(1, 6, 3)
+ DEF_TIM(TIM2, CH1, PA15, TIM_USE_OUTPUT_AUTO, 0, 0), // S7
#endif
- DEF_TIM(TIM4, CH1, PB6, TIM_USE_OUTPUT_AUTO, 0, 0), // S7 D(1, 0, 2)
DEF_TIM(TIM4, CH2, PB7, TIM_USE_OUTPUT_AUTO, 0, 0), // S8 D(1, 3, 2)
DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 0), // LED
diff --git a/src/main/target/AOCODARCF7MINI/target.h b/src/main/target/AOCODARCF7MINI/target.h
index 9f628963d85..4a5f5068746 100644
--- a/src/main/target/AOCODARCF7MINI/target.h
+++ b/src/main/target/AOCODARCF7MINI/target.h
@@ -163,3 +163,7 @@
#define USE_DSHOT
#define USE_SERIALSHOT
#define USE_ESC_SENSOR
+
+#if defined(AOCODARCF7MINI_V1)
+#define USE_DSHOT_DMAR
+#endif
diff --git a/src/main/target/FLYWOOF745/config.c b/src/main/target/FLYWOOF745/config.c
index 7dfe40728dd..16cadc0231b 100644
--- a/src/main/target/FLYWOOF745/config.c
+++ b/src/main/target/FLYWOOF745/config.c
@@ -32,6 +32,5 @@ void targetConfiguration(void)
// Make sure S1-S4 default to Motors
timerOverridesMutable(timer2id(TIM3))->outputMode = OUTPUT_MODE_MOTORS;
- timerOverridesMutable(timer2id(TIM8))->outputMode = OUTPUT_MODE_MOTORS;
timerOverridesMutable(timer2id(TIM1))->outputMode = OUTPUT_MODE_MOTORS;
}
diff --git a/src/main/target/FLYWOOH743PRO/CMakeLists.txt b/src/main/target/FLYWOOH743PRO/CMakeLists.txt
new file mode 100644
index 00000000000..0a1aaea5145
--- /dev/null
+++ b/src/main/target/FLYWOOH743PRO/CMakeLists.txt
@@ -0,0 +1 @@
+target_stm32h743xi(FLYWOOH743PRO)
diff --git a/src/main/target/FLYWOOH743PRO/config.c b/src/main/target/FLYWOOH743PRO/config.c
new file mode 100644
index 00000000000..0f1fec5a816
--- /dev/null
+++ b/src/main/target/FLYWOOH743PRO/config.c
@@ -0,0 +1,31 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight. If not, see .
+ */
+
+#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/FLYWOOH743PRO/target.c b/src/main/target/FLYWOOH743PRO/target.c
new file mode 100644
index 00000000000..ef25c2f8eb9
--- /dev/null
+++ b/src/main/target/FLYWOOH743PRO/target.c
@@ -0,0 +1,48 @@
+/*
+ * 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_icm42688_1, DEVHW_ICM42605, ICM42605_1_SPI_BUS, ICM42605_1_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU_ICM42605_ALIGN);
+BUSDEV_REGISTER_SPI_TAG(busdev_icm42688_2, DEVHW_ICM42605, ICM42605_2_SPI_BUS, ICM42605_2_CS_PIN, NONE, 1, DEVFLAGS_NONE, IMU_ICM42605_ALIGN);
+
+timerHardware_t timerHardware[] = {
+ DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S1
+ DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 1), // S2
+ DEF_TIM(TIM5, CH1, PA0, TIM_USE_OUTPUT_AUTO, 0, 2), // S3
+ DEF_TIM(TIM5, CH2, PA1, TIM_USE_OUTPUT_AUTO, 0, 3), // S4
+ DEF_TIM(TIM5, CH3, PA2, TIM_USE_OUTPUT_AUTO, 0, 4), // S5
+ DEF_TIM(TIM5, CH4, PA3, TIM_USE_OUTPUT_AUTO, 0, 5), // S6
+ DEF_TIM(TIM4, CH1, PD12, TIM_USE_OUTPUT_AUTO, 0, 6), // S7
+ DEF_TIM(TIM4, CH2, PD13, TIM_USE_OUTPUT_AUTO, 0, 7), // S8
+ DEF_TIM(TIM4, CH3, PD14, TIM_USE_OUTPUT_AUTO, 0, 0), // S9
+ DEF_TIM(TIM4, CH4, PD15, TIM_USE_OUTPUT_AUTO, 0, 0), // S10
+ DEF_TIM(TIM15, CH1, PE5, TIM_USE_OUTPUT_AUTO, 0, 0), // S11
+ DEF_TIM(TIM15, CH2, PE6, TIM_USE_OUTPUT_AUTO, 0, 0), // S12
+ 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/FLYWOOH743PRO/target.h b/src/main/target/FLYWOOH743PRO/target.h
new file mode 100644
index 00000000000..47419dc78ff
--- /dev/null
+++ b/src/main/target/FLYWOOH743PRO/target.h
@@ -0,0 +1,189 @@
+/*
+ * 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 "FWH7"
+#define USBD_PRODUCT_STRING "FLYWOOH743PRO"
+
+#define USE_TARGET_CONFIG
+
+#define LED0 PE3
+#define LED1 PE4
+
+#define BEEPER PA15
+#define BEEPER_INVERTED
+
+// *************** IMU generic ***********************
+#define USE_DUAL_GYRO
+#define USE_TARGET_IMU_HARDWARE_DESCRIPTORS
+
+#define USE_IMU_ICM42605
+// *************** SPI1 IMU0 ICM42605_1 ****************
+#define USE_SPI
+#define USE_SPI_DEVICE_1
+#define SPI1_SCK_PIN PA5
+#define SPI1_MISO_PIN PA6
+#define SPI1_MOSI_PIN PD7
+
+#define IMU_ICM42605_ALIGN CW270_DEG
+#define ICM42605_1_SPI_BUS BUS_SPI1
+#define ICM42605_1_CS_PIN PC15
+
+// *************** SPI4 IMU1 ICM42605_2 **************
+#define USE_SPI_DEVICE_4
+#define SPI4_SCK_PIN PE12
+#define SPI4_MISO_PIN PE13
+#define SPI4_MOSI_PIN PE14
+
+#define IMU_ICM42605_ALIGN CW270_DEG
+#define ICM42605_2_SPI_BUS BUS_SPI4
+#define ICM42605_2_CS_PIN PE11
+
+// *************** SPI2 OSD ***********************
+ #define USE_SPI_DEVICE_2
+ #define SPI2_SCK_PIN PB13
+ #define SPI2_MISO_PIN PB14
+ #define SPI2_MOSI_PIN PB15
+
+ #define USE_MAX7456
+ #define MAX7456_SPI_BUS BUS_SPI2
+ #define MAX7456_CS_PIN PB12
+
+// *************** SPI3 SPARE for external RM3100 ***********
+#define USE_SPI_DEVICE_3
+#define SPI3_SCK_PIN PB3
+#define SPI3_MISO_PIN PB4
+#define SPI3_MOSI_PIN PB5
+
+#define USE_MAG_RM3100
+#define RM3100_CS_PIN PE2 //CS2 pad
+#define RM3100_SPI_BUS BUS_SPI3
+
+// *************** I2C /Baro/Mag *********************
+#define USE_I2C
+#define USE_I2C_DEVICE_1
+#define I2C1_SCL PB6
+#define I2C1_SDA PB7
+
+#define USE_I2C_DEVICE_2
+#define I2C2_SCL PB10
+#define I2C2_SDA PB11
+
+#define USE_BARO
+#define BARO_I2C_BUS BUS_I2C2
+#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_ALL
+
+#define TEMPERATURE_I2C_BUS BUS_I2C2
+#define PITOT_I2C_BUS BUS_I2C2
+
+#define USE_RANGEFINDER
+#define RANGEFINDER_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 PD5
+#define UART2_RX_PIN PD6
+
+#define USE_UART3
+#define UART3_TX_PIN PD8
+#define UART3_RX_PIN PD9
+
+#define USE_UART4
+#define UART4_TX_PIN PB9
+#define UART4_RX_PIN PB8
+
+#define USE_UART6
+#define UART6_TX_PIN PC6
+#define UART6_RX_PIN PC7
+
+#define USE_UART7
+#define UART7_TX_PIN PE8
+#define UART7_RX_PIN PE7
+
+#define USE_UART8
+#define UART8_TX_PIN PE1
+#define UART8_RX_PIN PE0
+
+#define SERIAL_PORT_COUNT 8
+
+#define DEFAULT_RX_TYPE RX_TYPE_SERIAL
+#define SERIALRX_PROVIDER SERIALRX_CRSF
+#define SERIALRX_UART SERIAL_PORT_USART6
+
+// *************** SDIO SD BLACKBOX*******************
+#define USE_SDCARD
+#define USE_SDCARD_SDIO
+#define SDCARD_SDIO_DEVICE SDIODEV_1
+#define SDCARD_SDIO_4BIT
+
+#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT
+
+// *************** ADC *****************************
+#define USE_ADC
+#define ADC_INSTANCE ADC1
+
+#define ADC_CHANNEL_1_PIN PC0 //ADC123 VBAT1
+#define ADC_CHANNEL_2_PIN PC1 //ADC123 CURR1
+#define ADC_CHANNEL_3_PIN PC5 //ADC12 RSSI
+#define ADC_CHANNEL_4_PIN PC4 //ADC12 AirS
+#define ADC_CHANNEL_6_PIN PA7 //ADC12 CU2
+
+#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 PD10 // VTX power switcher
+#define PINIO2_PIN PD11 // 2xCamera switcher
+
+// *************** 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 250
+
+#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 12
+#define USE_DSHOT
+#define USE_ESC_SENSOR
+
diff --git a/src/main/target/GEPRC_F722_AIO/CMakeLists.txt b/src/main/target/GEPRC_F722_AIO/CMakeLists.txt
index 51e664a2637..9116889a26e 100644
--- a/src/main/target/GEPRC_F722_AIO/CMakeLists.txt
+++ b/src/main/target/GEPRC_F722_AIO/CMakeLists.txt
@@ -1 +1,2 @@
target_stm32f722xe(GEPRC_F722_AIO)
+target_stm32f722xe(GEPRC_F722_AIO_UART3)
diff --git a/src/main/target/GEPRC_F722_AIO/target.h b/src/main/target/GEPRC_F722_AIO/target.h
index 5dede599778..ee2b9330552 100644
--- a/src/main/target/GEPRC_F722_AIO/target.h
+++ b/src/main/target/GEPRC_F722_AIO/target.h
@@ -17,7 +17,11 @@
#pragma once
+#ifdef GEPRC_F722_AIO_UART3
+#define TARGET_BOARD_IDENTIFIER "GEP3"
+#else
#define TARGET_BOARD_IDENTIFIER "GEPR"
+#endif
#define USBD_PRODUCT_STRING "GEPRC_F722_AIO"
@@ -52,6 +56,7 @@
#define ICM42605_SPI_BUS BUS_SPI1
// *************** I2C/Baro/Mag *********************
+#ifndef GEPRC_F722_AIO_UART3
#define USE_I2C
#define USE_I2C_DEVICE_2
#define I2C2_SCL PB10
@@ -74,6 +79,7 @@
#define USE_RANGEFINDER
#define RANGEFINDER_I2C_BUS BUS_I2C2
#define BNO055_I2C_BUS BUS_I2C2
+#endif
// *************** FLASH **************************
#define M25P16_CS_PIN PB9
@@ -108,9 +114,11 @@
#define UART2_RX_PIN PA3
#define UART2_TX_PIN PA2
+#ifdef GEPRC_F722_AIO_UART3
#define USE_UART3
#define UART3_RX_PIN PB11
#define UART3_TX_PIN PB10
+#endif
#define USE_UART4
#define UART4_RX_PIN PC11
@@ -120,7 +128,11 @@
#define UART5_RX_PIN PD2
#define UART5_TX_PIN PC12
+#ifdef GEPRC_F722_AIO_UART3
#define SERIAL_PORT_COUNT 6
+#else
+#define SERIAL_PORT_COUNT 5
+#endif
#define DEFAULT_RX_TYPE RX_TYPE_SERIAL
#define SERIALRX_PROVIDER SERIALRX_SBUS
diff --git a/src/main/target/IFLIGHT_BLITZ_H7_PRO/CMakeLists.txt b/src/main/target/IFLIGHT_BLITZ_H7_PRO/CMakeLists.txt
index b8fc79235d4..c4bd2c43840 100644
--- a/src/main/target/IFLIGHT_BLITZ_H7_PRO/CMakeLists.txt
+++ b/src/main/target/IFLIGHT_BLITZ_H7_PRO/CMakeLists.txt
@@ -1 +1,2 @@
-target_stm32h743xi(IFLIGHT_BLITZ_H7_PRO)
\ No newline at end of file
+target_stm32h743xi(IFLIGHT_BLITZ_H7_PRO)
+target_stm32h743xi(IFLIGHT_BLITZ_H7_WING)
\ No newline at end of file
diff --git a/src/main/target/IFLIGHT_BLITZ_H7_PRO/target.h b/src/main/target/IFLIGHT_BLITZ_H7_PRO/target.h
index 23fde63466c..7d93ec91ce6 100644
--- a/src/main/target/IFLIGHT_BLITZ_H7_PRO/target.h
+++ b/src/main/target/IFLIGHT_BLITZ_H7_PRO/target.h
@@ -20,7 +20,11 @@
#define TARGET_BOARD_IDENTIFIER "IB7P"
+#ifdef IFLIGHT_BLITZ_H7_PRO
#define USBD_PRODUCT_STRING "IFLIGHT_BLITZ_H7_PRO"
+#else
+#define USBD_PRODUCT_STRING "IFLIGHT_BLITZ_H7_WING"
+#endif
#define USE_TARGET_CONFIG
@@ -161,7 +165,12 @@
#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_TELEMETRY | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TX_PROF_SEL | FEATURE_BLACKBOX)
#define CURRENT_METER_SCALE 212
-#define VBAT_SCALE_DEFAULT 1135
+
+#ifdef IFLIGHT_BLITZ_H7_PRO
+#define VBAT_SCALE_DEFAULT 2100
+#else
+#define VBAT_SCALE_DEFAULT 1100
+#endif
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
diff --git a/src/main/target/MAMBAF722_2022A/target.h b/src/main/target/MAMBAF722_2022A/target.h
index 4ab5a75bbc8..cf834b11ea6 100644
--- a/src/main/target/MAMBAF722_2022A/target.h
+++ b/src/main/target/MAMBAF722_2022A/target.h
@@ -23,6 +23,7 @@
#define TARGET_BOARD_IDENTIFIER "M72B"
#define USBD_PRODUCT_STRING "MAMBAF722_2022B"
+#define USE_DSHOT_DMAR
#else
diff --git a/src/main/target/MICOAIR743/CMakeLists.txt b/src/main/target/MICOAIR743/CMakeLists.txt
index 1d381638780..6847f3daefe 100644
--- a/src/main/target/MICOAIR743/CMakeLists.txt
+++ b/src/main/target/MICOAIR743/CMakeLists.txt
@@ -1 +1,2 @@
-target_stm32h743xi(MICOAIR743)
\ No newline at end of file
+target_stm32h743xi(MICOAIR743)
+target_stm32h743xi(MICOAIR743_EXTMAG)
diff --git a/src/main/target/MICOAIR743/target.h b/src/main/target/MICOAIR743/target.h
index 4ba27f3447e..526cf438d08 100644
--- a/src/main/target/MICOAIR743/target.h
+++ b/src/main/target/MICOAIR743/target.h
@@ -100,7 +100,14 @@
#define BARO_I2C_BUS BUS_I2C2
#define USE_MAG
+
+#ifdef MICOAIR743_EXTMAG
+// External compass
#define MAG_I2C_BUS BUS_I2C1
+#else
+// Onboard compass
+#define MAG_I2C_BUS BUS_I2C2
+#endif
#define USE_MAG_ALL
// *************** ENABLE OPTICAL FLOW & RANGEFINDER *****************************
diff --git a/src/main/target/NEUTRONRCF435MINI/target.h b/src/main/target/NEUTRONRCF435MINI/target.h
index 2e90f48a69d..6ef53abf64d 100644
--- a/src/main/target/NEUTRONRCF435MINI/target.h
+++ b/src/main/target/NEUTRONRCF435MINI/target.h
@@ -144,8 +144,9 @@
#define UART2_TX_PIN PA2
#define USE_UART3
-#define UART3_RX_PIN PB11
-#define UART3_TX_PIN PB10
+#define USE_UART3_PIN_SWAP
+#define UART3_RX_PIN PB10
+#define UART3_TX_PIN PB11
#define USE_UART5
#define UART5_RX_PIN PB8
diff --git a/src/main/target/TBS_LUCID_FC/CMakeLists.txt b/src/main/target/TBS_LUCID_FC/CMakeLists.txt
new file mode 100644
index 00000000000..0f7f65f2625
--- /dev/null
+++ b/src/main/target/TBS_LUCID_FC/CMakeLists.txt
@@ -0,0 +1 @@
+target_at32f43x_xMT7(TBS_LUCID_FC)
\ No newline at end of file
diff --git a/src/main/target/TBS_LUCID_FC/target.c b/src/main/target/TBS_LUCID_FC/target.c
new file mode 100644
index 00000000000..e6c7a3830ab
--- /dev/null
+++ b/src/main/target/TBS_LUCID_FC/target.c
@@ -0,0 +1,46 @@
+/*
+ * This file is part of INAV Project.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this file,
+ * You can obtain one at http://mozilla.org/MPL/2.0/.
+ *
+ * Alternatively, the contents of this file may be used under the terms
+ * of the GNU General Public License Version 3, as described below:
+ *
+ * This file is free software: you may copy, redistribute 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.
+ *
+ * This file 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 this program. If not, see http://www.gnu.org/licenses/.
+ */
+
+#include
+
+#include "drivers/bus.h"
+#include "drivers/io.h"
+#include "drivers/pwm_mapping.h"
+#include "drivers/timer.h"
+#include "platform.h"
+
+timerHardware_t timerHardware[] = {
+
+ DEF_TIM(TMR3, CH1, PC6, TIM_USE_OUTPUT_AUTO, 0, 1), // S1
+ DEF_TIM(TMR3, CH2, PB5, TIM_USE_OUTPUT_AUTO, 0, 2), // S2
+ DEF_TIM(TMR3, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 3), // S3
+ DEF_TIM(TMR3, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 4), // S4
+
+ DEF_TIM(TMR4, CH2, PB7, TIM_USE_OUTPUT_AUTO, 0, 8), // S5
+ DEF_TIM(TMR4, CH3, PB8, TIM_USE_OUTPUT_AUTO, 0, 9), // S6
+
+ DEF_TIM(TMR1, CH1, PA8, TIM_USE_LED, 0, 5), // LED Strip
+};
+
+const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]);
diff --git a/src/main/target/TBS_LUCID_FC/target.h b/src/main/target/TBS_LUCID_FC/target.h
new file mode 100644
index 00000000000..6e755b9a1a5
--- /dev/null
+++ b/src/main/target/TBS_LUCID_FC/target.h
@@ -0,0 +1,154 @@
+/*
+ * This file is part of INAV Project.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this file,
+ * You can obtain one at http://mozilla.org/MPL/2.0/.
+ *
+ * Alternatively, the contents of this file may be used under the terms
+ * of the GNU General Public License Version 3, as described below:
+ *
+ * This file is free software: you may copy, redistribute 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.
+ *
+ * This file 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 this program. If not, see http://www.gnu.org/licenses/.
+ */
+
+#pragma once
+
+#define TARGET_BOARD_IDENTIFIER "LUFC"
+
+#define USBD_PRODUCT_STRING "TBS Lucid FC"
+
+#define LED0 PC14
+#define LED1 PC15
+
+#define BEEPER_INVERTED
+#define BEEPER PC13
+
+#define USE_VCP
+
+#define USE_UART1
+#define UART1_TX_PIN PA9
+#define UART1_RX_PIN PA10
+
+#define USE_UART2
+#define UART2_AF 6
+#define UART2_TX_PIN NONE
+#define UART2_RX_PIN PB0
+
+#define USE_UART3
+#define UART3_TX_PIN PB11
+#define UART3_RX_PIN PB10
+
+#define USE_UART4
+#define UART4_TX_PIN PH3
+#define UART4_RX_PIN PH2
+
+#define USE_UART5
+#define UART5_TX_PIN PB9
+#define UART5_RX_PIN PD2
+
+#define USE_UART7
+#define UART7_TX_PIN PB4
+#define UART7_RX_PIN PB3
+
+#define USE_UART8
+#define UART8_TX_PIN PC2
+#define UART8_RX_PIN PC3
+
+#define SERIAL_PORT_COUNT 8
+
+#define USE_SPI
+
+#define USE_SPI_DEVICE_1
+#define SPI1_SCK_PIN PA5
+#define SPI1_MISO_PIN PA6
+#define SPI1_MOSI_PIN PA7
+
+#define USE_SPI_DEVICE_2
+#define SPI2_SCK_PIN PB13
+#define SPI2_MISO_PIN PB14
+#define SPI2_MOSI_PIN PB15
+
+#define USE_SPI_DEVICE_3
+#define SPI3_SCK_PIN PC10
+#define SPI3_MISO_PIN PC11
+#define SPI3_MOSI_PIN PC12
+
+#define USE_IMU_MPU6000
+#define IMU_MPU6000_ALIGN CW0_DEG_FLIP
+#define MPU6000_SPI_BUS BUS_SPI1
+#define MPU6000_CS_PIN PA4
+
+#define USE_IMU_ICM42605
+#define IMU_ICM42605_ALIGN CW270_DEG_FLIP
+#define ICM42605_CS_PIN PA4
+#define ICM42605_SPI_BUS BUS_SPI1
+
+#define USE_MAX7456
+#define MAX7456_SPI_BUS BUS_SPI2
+#define MAX7456_CS_PIN PB12
+
+#define USE_FLASHFS
+#define USE_FLASH_M25P16
+#define M25P16_SPI_BUS BUS_SPI3
+#define M25P16_CS_PIN PA15
+
+#define USE_I2C
+#define USE_I2C_DEVICE_1
+#define I2C1_SCL PB6
+#define I2C1_SDA PC7
+
+#define USE_BARO
+#define USE_BARO_BMP388
+#define BARO_I2C_BUS BUS_I2C1
+
+#define USE_MAG
+#define USE_MAG_ALL
+#define MAG_I2C_BUS BUS_I2C1
+
+#define USE_LED_STRIP
+#define WS2811_PIN PA8
+
+#define USE_PINIO
+#define USE_PINIOBOX
+#define PINIO1_PIN PA13
+#define PINIO2_PIN PA14
+
+#define USE_ADC
+#define ADC_INSTANCE ADC1
+#define ADC1_DMA_STREAM DMA2_CHANNEL7
+#define ADC_CHANNEL_1_PIN PC0
+#define ADC_CHANNEL_2_PIN PC1
+#define ADC_CHANNEL_3_PIN PC5
+#define VBAT_ADC_CHANNEL ADC_CHN_1
+#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2
+
+#define TARGET_IO_PORTA 0xffff
+#define TARGET_IO_PORTB 0xffff
+#define TARGET_IO_PORTC 0xffff
+#define TARGET_IO_PORTD (BIT(2) | BIT(12) | BIT(13))
+#define TARGET_IO_PORTH (BIT(2) | BIT(3))
+
+#define USE_DSHOT
+#define USE_ESC_SENSOR
+#define USE_SERIAL_4WAY_BLHELI_INTERFACE
+#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT
+#define MAX_PWM_OUTPUT_PORTS 6
+
+#define DEFAULT_FEATURES \
+ (FEATURE_VBAT | FEATURE_TELEMETRY | FEATURE_CURRENT_METER | FEATURE_OSD | \
+ FEATURE_LED_STRIP)
+
+#define DEFAULT_RX_TYPE RX_TYPE_SERIAL
+#define SERIALRX_PROVIDER SERIALRX_CRSF
+#define SERIALRX_UART SERIAL_PORT_USART5
\ No newline at end of file
diff --git a/src/main/target/common.h b/src/main/target/common.h
index 51f1fbf6ea6..f7cc3f747ce 100644
--- a/src/main/target/common.h
+++ b/src/main/target/common.h
@@ -206,7 +206,7 @@
#define USE_SERIALRX_SUMD
#define USE_TELEMETRY_HOTT
#define USE_HOTT_TEXTMODE
-#define USE_24CHANNELS
+#define USE_34CHANNELS
#define MAX_MIXER_PROFILE_COUNT 2
#define USE_SMARTPORT_MASTER
#elif !defined(STM32F7)
diff --git a/src/main/target/common_post.h b/src/main/target/common_post.h
index d3ccf280b17..fddd8b2a906 100644
--- a/src/main/target/common_post.h
+++ b/src/main/target/common_post.h
@@ -29,6 +29,41 @@ extern uint8_t __config_end;
# undef USE_OLED_UG2864
#endif
+
+// Make sure DEFAULT_I2C_BUS is valid
+#ifndef DEFAULT_I2C_BUS
+
+#if defined(USE_I2C_DEVICE_1)
+#define DEFAULT_I2C_BUS BUS_I2C1
+#elif defined(USE_I2C_DEVICE_2)
+#define DEFAULT_I2C_BUS BUS_I2C2
+#elif defined(USE_I2C_DEVICE_3)
+#define DEFAULT_I2C_BUS BUS_I2C3
+#elif defined(USE_I2C_DEVICE_4)
+#define DEFAULT_I2C_BUS BUS_I2C4
+#endif
+
+#endif
+
+// Airspeed sensors
+#if defined(USE_PITOT) && defined(DEFAULT_I2C_BUS)
+
+#ifndef PITOT_I2C_BUS
+#define PITOT_I2C_BUS DEFAULT_I2C_BUS
+#endif
+
+#endif
+
+// Temperature sensors
+#if !defined(TEMPERATURE_I2C_BUS) && defined(DEFAULT_I2C_BUS)
+#define TEMPERATURE_I2C_BUS DEFAULT_I2C_BUS
+#endif
+
+// Rangefinder sensors
+#if !defined(RANGEFINDER_I2C_BUS) && defined(DEFAULT_I2C_BUS)
+#define RANGEFINDER_I2C_BUS DEFAULT_I2C_BUS
+#endif
+
// Enable MSP_DISPLAYPORT for F3 targets without builtin OSD,
// since it's used to display CMS on MWOSD
#if !defined(USE_MSP_DISPLAYPORT) && !defined(USE_OSD)
@@ -67,6 +102,10 @@ extern uint8_t __config_end;
#endif // USE_MAG_ALL
+#if defined(DEFAULT_I2C_BUS) && !defined(MAG_I2C_BUS)
+#define MAG_I2C_BUS DEFAULT_I2C_BUS
+#endif
+
#endif // USE_MAG
#if defined(USE_BARO)
@@ -84,6 +123,10 @@ extern uint8_t __config_end;
#define USE_BARO_SPL06
#endif
+#if defined(DEFAULT_I2C_BUS) && !defined(BARO_I2C_BUS)
+#define BARO_I2C_BUS DEFAULT_I2C_BUS
+#endif
+
#endif
#ifdef USE_ESC_SENSOR
diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c
index 8931ed83c83..4e8d54535a3 100644
--- a/src/main/telemetry/mavlink.c
+++ b/src/main/telemetry/mavlink.c
@@ -184,7 +184,7 @@ static mavlink_message_t mavRecvMsg;
static mavlink_status_t mavRecvStatus;
static uint8_t mavSystemId = 1;
-static uint8_t mavComponentId = MAV_COMP_ID_SYSTEM_CONTROL;
+static uint8_t mavComponentId = MAV_COMP_ID_AUTOPILOT1;
static APM_COPTER_MODE inavToArduCopterMap(flightModeForTelemetry_e flightMode)
{
@@ -1134,6 +1134,22 @@ static bool handleIncoming_RADIO_STATUS(void) {
return true;
}
+static bool handleIncoming_HEARTBEAT(void) {
+ mavlink_heartbeat_t msg;
+ mavlink_msg_heartbeat_decode(&mavRecvMsg, &msg);
+
+ switch (msg.type) {
+#ifdef USE_ADSB
+ case MAV_TYPE_ADSB:
+ return adsbHeartbeat();
+#endif
+ default:
+ break;
+ }
+
+ return false;
+}
+
#ifdef USE_ADSB
static bool handleIncoming_ADSB_VEHICLE(void) {
mavlink_adsb_vehicle_t msg;
@@ -1188,7 +1204,7 @@ static bool processMAVLinkIncomingTelemetry(void)
if (result == MAVLINK_FRAMING_OK) {
switch (mavRecvMsg.msgid) {
case MAVLINK_MSG_ID_HEARTBEAT:
- break;
+ return handleIncoming_HEARTBEAT();
case MAVLINK_MSG_ID_PARAM_REQUEST_LIST:
return handleIncoming_PARAM_REQUEST_LIST();
case MAVLINK_MSG_ID_MISSION_CLEAR_ALL:
diff --git a/src/main/telemetry/sbus2.c b/src/main/telemetry/sbus2.c
index dc5fb81d77d..1e800f2f098 100644
--- a/src/main/telemetry/sbus2.c
+++ b/src/main/telemetry/sbus2.c
@@ -25,6 +25,10 @@
#include "common/time.h"
#include "common/axis.h"
+#include "config/feature.h"
+
+#include "fc/config.h"
+
#include "telemetry/telemetry.h"
#include "telemetry/sbus2.h"
#include "telemetry/sbus2_sensors.h"
@@ -164,7 +168,7 @@ void sbus2IncrementTelemetrySlot(timeUs_t currentTimeUs)
FAST_CODE void taskSendSbus2Telemetry(timeUs_t currentTimeUs)
{
- if (!telemetrySharedPort || rxConfig()->receiverType != RX_TYPE_SERIAL ||
+ if (!feature(FEATURE_TELEMETRY) || !telemetrySharedPort || rxConfig()->receiverType != RX_TYPE_SERIAL ||
rxConfig()->serialrx_provider != SERIALRX_SBUS2) {
return;
}
diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c
index 9d804be52d8..94e318e8a7a 100755
--- a/src/main/telemetry/smartport.c
+++ b/src/main/telemetry/smartport.c
@@ -63,66 +63,68 @@
// these data identifiers are obtained from https://github.com/opentx/opentx/blob/2.3/radio/src/telemetry/frsky.h
enum
{
- FSSP_DATAID_SPEED = 0x0830 ,
- FSSP_DATAID_VFAS = 0x0210 ,
- FSSP_DATAID_CURRENT = 0x0200 ,
- FSSP_DATAID_RPM = 0x050F ,
- FSSP_DATAID_ALTITUDE = 0x0100 ,
- FSSP_DATAID_FUEL = 0x0600 ,
- FSSP_DATAID_ADC1 = 0xF102 ,
- FSSP_DATAID_ADC2 = 0xF103 ,
- FSSP_DATAID_LATLONG = 0x0800 ,
- FSSP_DATAID_CAP_USED = 0x0600 ,
- FSSP_DATAID_VARIO = 0x0110 ,
- FSSP_DATAID_CELLS = 0x0300 ,
- FSSP_DATAID_CELLS_LAST = 0x030F ,
- FSSP_DATAID_HEADING = 0x0840 ,
- FSSP_DATAID_FPV = 0x0450 ,
- FSSP_DATAID_PITCH = 0x0430 ,
- FSSP_DATAID_ROLL = 0x0440 ,
- FSSP_DATAID_ACCX = 0x0700 ,
- FSSP_DATAID_ACCY = 0x0710 ,
- FSSP_DATAID_ACCZ = 0x0720 ,
- FSSP_DATAID_T1 = 0x0400 ,
- FSSP_DATAID_T2 = 0x0410 ,
- FSSP_DATAID_HOME_DIST = 0x0420 ,
- FSSP_DATAID_GPS_ALT = 0x0820 ,
- FSSP_DATAID_ASPD = 0x0A00 ,
- FSSP_DATAID_A3 = 0x0900 ,
- FSSP_DATAID_A4 = 0x0910 ,
- FSSP_DATAID_AZIMUTH = 0x0460
+ FSSP_DATAID_SPEED = 0x0830,
+ FSSP_DATAID_VFAS = 0x0210,
+ FSSP_DATAID_CURRENT = 0x0200,
+ FSSP_DATAID_RPM = 0x050F,
+ FSSP_DATAID_ALTITUDE = 0x0100,
+ FSSP_DATAID_FUEL = 0x0600,
+ FSSP_DATAID_ADC1 = 0xF102,
+ FSSP_DATAID_ADC2 = 0xF103,
+ FSSP_DATAID_LATLONG = 0x0800,
+ FSSP_DATAID_CAP_USED = 0x0600,
+ FSSP_DATAID_VARIO = 0x0110,
+ FSSP_DATAID_CELLS = 0x0300,
+ FSSP_DATAID_CELLS_LAST = 0x030F,
+ FSSP_DATAID_HEADING = 0x0840,
+ FSSP_DATAID_FPV = 0x0450,
+ FSSP_DATAID_PITCH = 0x0430,
+ FSSP_DATAID_ROLL = 0x0440,
+ FSSP_DATAID_ACCX = 0x0700,
+ FSSP_DATAID_ACCY = 0x0710,
+ FSSP_DATAID_ACCZ = 0x0720,
+ FSSP_DATAID_LEGACY_MODES = 0x0400, // Deprecated. Should be removed in INAV 10.0
+ FSSP_DATAID_LEGACY_GNSS = 0x0410, // Deprecated. Should be removed in INAV 10.0
+ FSSP_DATAID_HOME_DIST = 0x0420,
+ FSSP_DATAID_GPS_ALT = 0x0820,
+ FSSP_DATAID_ASPD = 0x0A00,
+ FSSP_DATAID_A3 = 0x0900,
+ FSSP_DATAID_A4 = 0x0910,
+ FSSP_DATAID_AZIMUTH = 0x0460,
+ FSSP_DATAID_MODES = 0x0470,
+ FSSP_DATAID_GNSS = 0x0480,
};
const uint16_t frSkyDataIdTable[] = {
- FSSP_DATAID_SPEED ,
- FSSP_DATAID_VFAS ,
- FSSP_DATAID_CURRENT ,
- //FSSP_DATAID_RPM ,
- FSSP_DATAID_ALTITUDE ,
- FSSP_DATAID_FUEL ,
- //FSSP_DATAID_ADC1 ,
- //FSSP_DATAID_ADC2 ,
- FSSP_DATAID_LATLONG ,
- FSSP_DATAID_LATLONG , // twice
- //FSSP_DATAID_CAP_USED ,
- FSSP_DATAID_VARIO ,
- //FSSP_DATAID_CELLS ,
+ FSSP_DATAID_SPEED,
+ FSSP_DATAID_VFAS,
+ FSSP_DATAID_CURRENT,
+ //FSSP_DATAID_RPM,
+ FSSP_DATAID_ALTITUDE,
+ FSSP_DATAID_FUEL,
+ //FSSP_DATAID_ADC1,
+ //FSSP_DATAID_ADC2,
+ FSSP_DATAID_LATLONG,
+ FSSP_DATAID_LATLONG, // twice
+ //FSSP_DATAID_CAP_USED,
+ FSSP_DATAID_VARIO,
+ //FSSP_DATAID_CELLS,
//FSSP_DATAID_CELLS_LAST,
- FSSP_DATAID_HEADING ,
- FSSP_DATAID_FPV ,
- FSSP_DATAID_PITCH ,
- FSSP_DATAID_ROLL ,
- FSSP_DATAID_ACCX ,
- FSSP_DATAID_ACCY ,
- FSSP_DATAID_ACCZ ,
- FSSP_DATAID_T1 ,
- FSSP_DATAID_T2 ,
- FSSP_DATAID_HOME_DIST ,
- FSSP_DATAID_GPS_ALT ,
- FSSP_DATAID_ASPD ,
- // FSSP_DATAID_A3 ,
- FSSP_DATAID_A4 ,
- FSSP_DATAID_AZIMUTH ,
+ FSSP_DATAID_HEADING,
+ FSSP_DATAID_FPV,
+ FSSP_DATAID_PITCH,
+ FSSP_DATAID_ROLL,
+ FSSP_DATAID_ACCX,
+ FSSP_DATAID_ACCY,
+ FSSP_DATAID_ACCZ,
+ FSSP_DATAID_MODES,
+ FSSP_DATAID_GNSS,
+ FSSP_DATAID_HOME_DIST,
+ FSSP_DATAID_GPS_ALT,
+ FSSP_DATAID_ASPD,
+ // FSSP_DATAID_A3,
+ FSSP_DATAID_A4,
+ FSSP_DATAID_AZIMUTH,
0
};
@@ -467,27 +469,27 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear
smartPortIdCnt++;
switch (id) {
- case FSSP_DATAID_VFAS :
+ case FSSP_DATAID_VFAS:
if (isBatteryVoltageConfigured()) {
uint16_t vfasVoltage = telemetryConfig()->report_cell_voltage ? getBatteryAverageCellVoltage() : getBatteryVoltage();
smartPortSendPackage(id, vfasVoltage);
*clearToSend = false;
}
break;
- case FSSP_DATAID_CURRENT :
+ case FSSP_DATAID_CURRENT:
if (isAmperageConfigured()) {
smartPortSendPackage(id, getAmperage() / 10); // given in 10mA steps, unknown requested unit
*clearToSend = false;
}
break;
- //case FSSP_DATAID_RPM :
- case FSSP_DATAID_ALTITUDE :
+ //case FSSP_DATAID_RPM:
+ case FSSP_DATAID_ALTITUDE:
if (sensors(SENSOR_BARO)) {
smartPortSendPackage(id, getEstimatedActualPosition(Z)); // unknown given unit, requested 100 = 1 meter
*clearToSend = false;
}
break;
- case FSSP_DATAID_FUEL :
+ case FSSP_DATAID_FUEL:
if (telemetryConfig()->smartportFuelUnit == SMARTPORT_FUEL_UNIT_PERCENT) {
smartPortSendPackage(id, calculateBatteryPercentage()); // Show remaining battery % if smartport_fuel_percent=ON
*clearToSend = false;
@@ -496,63 +498,71 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear
*clearToSend = false;
}
break;
- //case FSSP_DATAID_ADC1 :
- //case FSSP_DATAID_ADC2 :
- //case FSSP_DATAID_CAP_USED :
- case FSSP_DATAID_VARIO :
+ //case FSSP_DATAID_ADC1:
+ //case FSSP_DATAID_ADC2:
+ //case FSSP_DATAID_CAP_USED:
+ case FSSP_DATAID_VARIO:
if (sensors(SENSOR_BARO)) {
smartPortSendPackage(id, lrintf(getEstimatedActualVelocity(Z))); // unknown given unit but requested in 100 = 1m/s
*clearToSend = false;
}
break;
- case FSSP_DATAID_HEADING :
+ case FSSP_DATAID_HEADING:
smartPortSendPackage(id, attitude.values.yaw * 10); // given in 10*deg, requested in 10000 = 100 deg
*clearToSend = false;
break;
- case FSSP_DATAID_PITCH :
+ case FSSP_DATAID_PITCH:
if (telemetryConfig()->frsky_pitch_roll) {
smartPortSendPackage(id, attitude.values.pitch); // given in 10*deg
*clearToSend = false;
}
break;
- case FSSP_DATAID_ROLL :
+ case FSSP_DATAID_ROLL:
if (telemetryConfig()->frsky_pitch_roll) {
smartPortSendPackage(id, attitude.values.roll); // given in 10*deg
*clearToSend = false;
}
break;
- case FSSP_DATAID_ACCX :
+ case FSSP_DATAID_ACCX:
if (!telemetryConfig()->frsky_pitch_roll) {
smartPortSendPackage(id, lrintf(100 * acc.accADCf[X]));
*clearToSend = false;
}
break;
- case FSSP_DATAID_ACCY :
+ case FSSP_DATAID_ACCY:
if (!telemetryConfig()->frsky_pitch_roll) {
smartPortSendPackage(id, lrintf(100 * acc.accADCf[Y]));
*clearToSend = false;
}
break;
- case FSSP_DATAID_ACCZ :
+ case FSSP_DATAID_ACCZ:
if (!telemetryConfig()->frsky_pitch_roll) {
smartPortSendPackage(id, lrintf(100 * acc.accADCf[Z]));
*clearToSend = false;
}
break;
- case FSSP_DATAID_T1 :
+ case FSSP_DATAID_MODES:
{
+ if (telemetryConfig()->frsky_use_legacy_gps_mode_sensor_ids)
+ id = FSSP_DATAID_LEGACY_MODES;
+
smartPortSendPackage(id, frskyGetFlightMode());
*clearToSend = false;
break;
}
#ifdef USE_GPS
- case FSSP_DATAID_T2 :
- if (smartPortShouldSendGPSData()) {
- smartPortSendPackage(id, frskyGetGPSState());
- *clearToSend = false;
+ case FSSP_DATAID_GNSS:
+ {
+ if (telemetryConfig()->frsky_use_legacy_gps_mode_sensor_ids)
+ id = FSSP_DATAID_LEGACY_GNSS;
+
+ if (smartPortShouldSendGPSData()) {
+ smartPortSendPackage(id, frskyGetGPSState());
+ *clearToSend = false;
+ }
+ break;
}
- break;
- case FSSP_DATAID_SPEED :
+ case FSSP_DATAID_SPEED:
if (smartPortShouldSendGPSData()) {
//convert to knots: 1cm/s = 0.0194384449 knots
//Speed should be sent in knots/1000 (GPS speed is in cm/s)
@@ -561,7 +571,7 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear
*clearToSend = false;
}
break;
- case FSSP_DATAID_LATLONG :
+ case FSSP_DATAID_LATLONG:
if (smartPortShouldSendGPSData()) {
uint32_t tmpui = 0;
// the same ID is sent twice, one for longitude, one for latitude
@@ -581,25 +591,25 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear
*clearToSend = false;
}
break;
- case FSSP_DATAID_HOME_DIST :
+ case FSSP_DATAID_HOME_DIST:
if (smartPortShouldSendGPSData()) {
smartPortSendPackage(id, GPS_distanceToHome);
*clearToSend = false;
}
break;
- case FSSP_DATAID_GPS_ALT :
+ case FSSP_DATAID_GPS_ALT:
if (smartPortShouldSendGPSData()) {
smartPortSendPackage(id, gpsSol.llh.alt); // cm
*clearToSend = false;
}
break;
- case FSSP_DATAID_FPV :
+ case FSSP_DATAID_FPV:
if (smartPortShouldSendGPSData()) {
smartPortSendPackage(id, gpsSol.groundCourse); // given in 10*deg
*clearToSend = false;
}
break;
- case FSSP_DATAID_AZIMUTH :
+ case FSSP_DATAID_AZIMUTH:
if (smartPortShouldSendGPSData()) {
int16_t h = GPS_directionToHome;
if (h < 0) {
@@ -614,13 +624,13 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear
}
break;
#endif
- case FSSP_DATAID_A4 :
+ case FSSP_DATAID_A4:
if (isBatteryVoltageConfigured()) {
smartPortSendPackage(id, getBatteryAverageCellVoltage());
*clearToSend = false;
}
break;
- case FSSP_DATAID_ASPD :
+ case FSSP_DATAID_ASPD:
#ifdef USE_PITOT
if (sensors(SENSOR_PITOT) && pitotIsHealthy()) {
smartPortSendPackage(id, getAirspeedEstimate() * 0.194384449f); // cm/s to knots*1
diff --git a/src/main/telemetry/telemetry.c b/src/main/telemetry/telemetry.c
index 56c18ca159b..82fc2d2d5fb 100644
--- a/src/main/telemetry/telemetry.c
+++ b/src/main/telemetry/telemetry.c
@@ -62,6 +62,7 @@ PG_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig,
.telemetry_switch = SETTING_TELEMETRY_SWITCH_DEFAULT,
.telemetry_inverted = SETTING_TELEMETRY_INVERTED_DEFAULT,
.frsky_pitch_roll = SETTING_FRSKY_PITCH_ROLL_DEFAULT,
+ .frsky_use_legacy_gps_mode_sensor_ids = SETTING_FRSKY_USE_LEGACY_GPS_MODE_SENSOR_IDS_DEFAULT,
.report_cell_voltage = SETTING_REPORT_CELL_VOLTAGE_DEFAULT,
.hottAlarmSoundInterval = SETTING_HOTT_ALARM_SOUND_INTERVAL_DEFAULT,
.halfDuplex = SETTING_TELEMETRY_HALFDUPLEX_DEFAULT,
diff --git a/src/main/telemetry/telemetry.h b/src/main/telemetry/telemetry.h
index c7edad973b8..7be5e50a9ce 100644
--- a/src/main/telemetry/telemetry.h
+++ b/src/main/telemetry/telemetry.h
@@ -46,6 +46,7 @@ typedef struct telemetryConfig_s {
uint8_t telemetry_switch; // Use aux channel to change serial output & baudrate( MSP / Telemetry ). It disables automatic switching to Telemetry when armed.
uint8_t telemetry_inverted; // Flip the default inversion of the protocol - Same as serialrx_inverted in rx.c, but for telemetry.
uint8_t frsky_pitch_roll;
+ bool frsky_use_legacy_gps_mode_sensor_ids;
uint8_t report_cell_voltage;
uint8_t hottAlarmSoundInterval;
uint8_t halfDuplex;
diff --git a/src/test/unit/CMakeLists.txt b/src/test/unit/CMakeLists.txt
index 300721b8f53..2cb53b64ecb 100644
--- a/src/test/unit/CMakeLists.txt
+++ b/src/test/unit/CMakeLists.txt
@@ -40,8 +40,8 @@ set_property(SOURCE osd_unittest.cc PROPERTY definitions OSD_UNIT_TEST USE_MSP_D
set_property(SOURCE gps_ublox_unittest.cc PROPERTY depends "io/gps_ublox_utils.c")
set_property(SOURCE gps_ublox_unittest.cc PROPERTY definitions GPS_UBLOX_UNIT_TEST)
-set_property(SOURCE gimbal_serial_unittest.cc PROPERTY depends "io/gimbal_serial.c" "drivers/gimbal_common.c" "common/maths.c")
-set_property(SOURCE gimbal_serial_unittest.cc PROPERTY definitions USE_SERIAL_GIMBAL GIMBAL_UNIT_TEST)
+set_property(SOURCE gimbal_serial_unittest.cc PROPERTY depends "io/gimbal_serial.c" "drivers/gimbal_common.c" "common/maths.c" "drivers/headtracker_common.c")
+set_property(SOURCE gimbal_serial_unittest.cc PROPERTY definitions USE_SERIAL_GIMBAL GIMBAL_UNIT_TEST USE_HEADTRACKER)
function(unit_test src)
get_filename_component(basename ${src} NAME)
diff --git a/src/test/unit/gimbal_serial_unittest.cc b/src/test/unit/gimbal_serial_unittest.cc
index 1f0c47231c6..4ca55e17ee2 100644
--- a/src/test/unit/gimbal_serial_unittest.cc
+++ b/src/test/unit/gimbal_serial_unittest.cc
@@ -34,6 +34,12 @@ void dumpMemory(uint8_t *mem, int size)
printf("\n");
}
+extern "C" {
+timeUs_t micros(void) {
+ return 10;
+}
+}
+
TEST(GimbalSerialTest, TestGimbalSerialScale)
{
int16_t res16 = gimbal_scale12(1000, 2000, 2000);