diff --git a/docs/OSD.md b/docs/OSD.md index 5787496687a..b492e537cef 100644 --- a/docs/OSD.md +++ b/docs/OSD.md @@ -177,7 +177,18 @@ Here are the OSD Elements provided by INAV. | 144 | OSD_MULTI_FUNCTION | 7.0.0 | | | 145 | OSD_ODOMETER | 7.0.0 | For this to work correctly, stats must be enabled (`set stats=ON`). Otherwise, this will show the total flight distance. | | 146 | OSD_PILOT_LOGO | 7.0.0 | | -| 147 | OSD_BLACKBOX | 8.0.0 | The element will be hidden unless blackbox recording is attempted. | +| 147 | OSD_CUSTOM_ELEMENT_1 | 7.0.0 | | +| 148 | OSD_CUSTOM_ELEMENT_2 | 7.0.0 | | +| 149 | OSD_CUSTOM_ELEMENT_3 | 7.0.0 | | +| 150 | OSD_ADSB_WARNING | 7.0.0 | | +| 151 | OSD_ADSB_INFO | 7.0.0 | | +| 152 | OSD_BLACKBOX | 8.0.0 | The element will be hidden unless blackbox recording is attempted. | +| 153 | OSD_FORMATION_FLIGHT | 8.0.0 | | +| 154 | OSD_CUSTOM_ELEMENT_4 | 8.0.0 | | +| 155 | OSD_CUSTOM_ELEMENT_5 | 8.0.0 | | +| 156 | OSD_CUSTOM_ELEMENT_6 | 8.0.0 | | +| 157 | OSD_CUSTOM_ELEMENT_7 | 8.0.0 | | +| 158 | OSD_CUSTOM_ELEMENT_8 | 8.0.0 | | # Pilot Logos diff --git a/docs/Programming Framework.md b/docs/Programming Framework.md index c305fd27888..4d1e47e1d51 100644 --- a/docs/Programming Framework.md +++ b/docs/Programming Framework.md @@ -24,6 +24,8 @@ INAV Programming Framework consists of: IPF can be edited using INAV Configurator user interface, or via CLI. To use COnfigurator, click the tab labeled "Programming". The various options shown in Configurator are described below. +**Note:** IPF uses integer math. If your programming line returns a decimal, it will be truncated to an integer. So if your math is `1` / `3` = , IPF will truncate the decimal and return `0`. + ## Logic Conditions ### CLI @@ -61,9 +63,8 @@ IPF can be edited using INAV Configurator user interface, or via CLI. To use COn | 14 | Basic: Add | Add `Operand A` to `Operand B` and returns the result | | 15 | Basic: Subtract | Substract `Operand B` from `Operand A` and returns the result | | 16 | Basic: Multiply | Multiply `Operand A` by `Operand B` and returns the result | -| 17 | Basic: Divide | Divide `Operand A` by `Operand B` and returns the result | -| 18 | Set GVAR | Store value from `Operand B` into the Global Variable addressed by -`Operand A`. Bear in mind, that operand `Global Variable` means: Value stored in Global Variable of an index! To store in GVAR 1 use `Value 1` not `Global Variable 1` | +| 17 | Basic: Divide | Divide `Operand A` by `Operand B` and returns the result. NOTE: If `Operand B` = `0`, the `Divide` operation will simply return `Operand A`| +| 18 | Set GVAR | Store value from `Operand B` into the Global Variable addressed by `Operand A`. Bear in mind, that operand `Global Variable` means: Value stored in Global Variable of an index! To store in GVAR 1 use `Value 1` not `Global Variable 1` | | 19 | Increase GVAR | Increase the GVAR indexed by `Operand A` (use `Value 1` for Global Variable 1) with value from `Operand B` | | 20 | Decrease GVAR | Decrease the GVAR indexed by `Operand A` (use `Value 1` for Global Variable 1) with value from `Operand B` | | 21 | Set IO Port | Set I2C IO Expander pin `Operand A` to value of `Operand B`. `Operand A` accepts values `0-7` and `Operand B` accepts `0` and `1` | @@ -233,8 +234,15 @@ All flags are reseted on ARM and DISARM event. `gvar ` +**Note:** Global Variables (GVARs) are limited to integers between negative `-32768` and positive `32767`. + ## Programming PID +IPF makes a set of general user PIDFF controllers avaliable for use in your program. These PIDFF controllers are not tied to any roll/pitch/yaw profiles or other controls. +The output of these controllers can be used in an IPF program by using the `Programming PID` operand. +The `` of the controller is the target value for the controller to hit. The `` is the measurement of the current value. For instance, `` could be the speed you want to go, and `` is the current speed. +P, I, D, and FF values will need to be manually adjusted to determine the appropriate value for the program and controller. + `pid

` * `` - ID of PID Controller, starting from `0` diff --git a/docs/Settings.md b/docs/Settings.md index d6a240c5a6b..fad1f15582f 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -1172,6 +1172,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`. @@ -2004,7 +2014,7 @@ Uncertainty value for barometric sensor [cm] ### inav_default_alt_sensor -Sets the default altitude sensor to use. Settings GPS and BARO always use both sensors with priority given to the selected sensor when the altitude error between the sensors exceeds a set limit. Only the selected sensor will be used in this case. GPS error limit = 2 * inav_max_eph_epv. BARO error limit = 4 * inav_baro_epv. Settings GPS_ONLY and BARO_ONLY will use only the selected sensor even if the other sensor is working. The other sensor will only be used as a backup if the selected sensor is no longer available to use. + | Default | Min | Max | | --- | --- | --- | @@ -2572,6 +2582,16 @@ Rate of the extra3 message for MAVLink telemetry --- +### mavlink_min_txbuffer + +Minimum percent of TX buffer space free, before attempting to transmit telemetry. Requuires RADIO_STATUS messages to be processed. 0 = always transmits. + +| Default | Min | Max | +| --- | --- | --- | +| 33 | 0 | 100 | + +--- + ### mavlink_pos_rate Rate of the position message for MAVLink telemetry @@ -2582,6 +2602,16 @@ Rate of the position message for MAVLink telemetry --- +### mavlink_radio_type + +Mavlink radio type. Affects how RSSI and LQ are reported on OSD. + +| Default | Min | Max | +| --- | --- | --- | +| GENERIC | | | + +--- + ### mavlink_rc_chan_rate Rate of the RC channels message for MAVLink telemetry @@ -3704,7 +3734,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 | | --- | --- | --- | @@ -5044,7 +5074,7 @@ Number of leading digits removed from plus code. Removing 2, 4 and 6 digits requ ### osd_radar_peers_display_time -Time in seconds to display next peer +Time in seconds to display next peer | 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/src/main/drivers/serial_uart_at32f43x.c b/src/main/drivers/serial_uart_at32f43x.c index 90ebe05ef82..ad4b4e0c458 100644 --- a/src/main/drivers/serial_uart_at32f43x.c +++ b/src/main/drivers/serial_uart_at32f43x.c @@ -54,7 +54,11 @@ 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 @@ -75,7 +79,11 @@ 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 @@ -96,7 +104,11 @@ 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 @@ -117,7 +129,11 @@ 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 @@ -138,7 +154,11 @@ 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 @@ -159,7 +179,11 @@ 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 @@ -180,7 +204,11 @@ 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, @@ -198,7 +226,11 @@ 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, diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index ee5cb289a94..5aed69c1d13 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -3714,6 +3714,17 @@ static void cliSet(char *cmdline) } if (changeValue) { + // If changing the battery capacity unit, update the osd stats energy unit to match + if (strcmp(name, "battery_capacity_unit") == 0) { + if (batteryMetersConfig()->capacity_unit != (uint8_t)tmp.int_value) { + if (tmp.int_value == BAT_CAPACITY_UNIT_MAH) { + osdConfigMutable()->stats_energy_unit = OSD_STATS_ENERGY_UNIT_MAH; + } else { + osdConfigMutable()->stats_energy_unit = OSD_STATS_ENERGY_UNIT_WH; + } + } + } + cliSetIntFloatVar(val, tmp); cliPrintf("%s set to ", name); diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index f9c43be7864..cb6086ebfd6 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -1710,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) @@ -2091,6 +2082,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) currentBatteryProfileMutable->capacity.value = sbufReadU32(src); currentBatteryProfileMutable->capacity.warning = sbufReadU32(src); currentBatteryProfileMutable->capacity.critical = sbufReadU32(src); + uint8_t currentCapacityUnit = batteryMetersConfigMutable()->capacity_unit; batteryMetersConfigMutable()->capacity_unit = sbufReadU8(src); if ((batteryMetersConfig()->voltageSource != BAT_VOLTAGE_RAW) && (batteryMetersConfig()->voltageSource != BAT_VOLTAGE_SAG_COMP)) { batteryMetersConfigMutable()->voltageSource = BAT_VOLTAGE_RAW; @@ -2099,6 +2091,12 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) if ((batteryMetersConfig()->capacity_unit != BAT_CAPACITY_UNIT_MAH) && (batteryMetersConfig()->capacity_unit != BAT_CAPACITY_UNIT_MWH)) { batteryMetersConfigMutable()->capacity_unit = BAT_CAPACITY_UNIT_MAH; return MSP_RESULT_ERROR; + } else if (currentCapacityUnit != batteryMetersConfig()->capacity_unit) { + if (batteryMetersConfig()->capacity_unit == BAT_CAPACITY_UNIT_MAH) { + osdConfigMutable()->stats_energy_unit = OSD_STATS_ENERGY_UNIT_MAH; + } else { + osdConfigMutable()->stats_energy_unit = OSD_STATS_ENERGY_UNIT_WH; + } } } else return MSP_RESULT_ERROR; @@ -2130,6 +2128,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) currentBatteryProfileMutable->capacity.value = sbufReadU32(src); currentBatteryProfileMutable->capacity.warning = sbufReadU32(src); currentBatteryProfileMutable->capacity.critical = sbufReadU32(src); + uint8_t currentCapacityUnit = batteryMetersConfigMutable()->capacity_unit; batteryMetersConfigMutable()->capacity_unit = sbufReadU8(src); if ((batteryMetersConfig()->voltageSource != BAT_VOLTAGE_RAW) && (batteryMetersConfig()->voltageSource != BAT_VOLTAGE_SAG_COMP)) { batteryMetersConfigMutable()->voltageSource = BAT_VOLTAGE_RAW; @@ -2138,6 +2137,12 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) if ((batteryMetersConfig()->capacity_unit != BAT_CAPACITY_UNIT_MAH) && (batteryMetersConfig()->capacity_unit != BAT_CAPACITY_UNIT_MWH)) { batteryMetersConfigMutable()->capacity_unit = BAT_CAPACITY_UNIT_MAH; return MSP_RESULT_ERROR; + } else if (currentCapacityUnit != batteryMetersConfig()->capacity_unit) { + if (batteryMetersConfig()->capacity_unit == BAT_CAPACITY_UNIT_MAH) { + osdConfigMutable()->stats_energy_unit = OSD_STATS_ENERGY_UNIT_MAH; + } else { + osdConfigMutable()->stats_energy_unit = OSD_STATS_ENERGY_UNIT_WH; + } } } else return MSP_RESULT_ERROR; @@ -3351,7 +3356,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); @@ -3367,7 +3372,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) { @@ -3394,7 +3399,6 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) } return MSP_RESULT_ACK; } -#endif static const setting_t *mspReadSetting(sbuf_t *src) { @@ -3853,6 +3857,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: diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 58f8ac7ba9d..edd423607ac 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -198,7 +198,10 @@ tables: - name: headtracker_dev_type values: ["NONE", "SERIAL", "MSP"] enum: headTrackerDevType_e - - name: default_altitude_source # CR131 + - name: mavlink_radio_type + values: ["GENERIC", "ELRS", "SIK"] + enum: mavlinkRadio_e + - name: default_altitude_source values: ["GPS", "BARO", "GPS ONLY", "BARO ONLY"] enum: navDefaultAltitudeSensor_e @@ -2444,7 +2447,6 @@ groups: min: 0 max: 10 default_value: 0.35 - # CR131 - name: inav_w_z_baro_v description: "Weight of barometer climb rate measurements in estimated climb rate. Setting is used on both airplanes and multirotors." field: w_z_baro_v @@ -2505,13 +2507,12 @@ groups: field: baro_epv min: 0 max: 9999 - # CR131 - name: inav_default_alt_sensor - description: "Sets the default altitude sensor to use. Settings GPS and BARO always use both sensors with priority given to the selected sensor when the altitude error between the sensors exceeds a set limit. Only the selected sensor will be used in this case. GPS error limit = 2 * inav_max_eph_epv. BARO error limit = 4 * inav_baro_epv. Settings GPS_ONLY and BARO_ONLY will use only the selected sensor even if the other sensor is working. The other sensor will only be used as a backup if the selected sensor is no longer available to use." + description: "Sets the default altitude sensor to use. Settings GPS and BARO always use both sensors unless there is an altitude error between the sensors that exceeds a set limit. In this case only the selected sensor will be used while the altitude error limit is exceeded. GPS error limit = 2 * inav_max_eph_epv. BARO error limit = 4 * inav_baro_epv. Settings GPS_ONLY and BARO_ONLY will use only the selected sensor even if the other sensor is working. The other sensor will only be used as a backup if the selected sensor is no longer available to use." default_value: "GPS" field: default_alt_sensor table: default_altitude_source - # CR131 + - name: PG_NAV_CONFIG type: navConfig_t headers: ["navigation/navigation.h"] @@ -2742,7 +2743,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 @@ -3109,6 +3110,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 @@ -3248,6 +3253,18 @@ groups: min: 1 max: 2 default_value: 2 + - name: mavlink_min_txbuffer + field: mavlink.min_txbuff + description: "Minimum percent of TX buffer space free, before attempting to transmit telemetry. Requuires RADIO_STATUS messages to be processed. 0 = always transmits." + default_value: 33 + min: 0 + max: 100 + - name: mavlink_radio_type + description: "Mavlink radio type. Affects how RSSI and LQ are reported on OSD." + field: mavlink.radio_type + table: mavlink_radio_type + default_value: "GENERIC" + type: uint8_t - name: PG_OSD_CONFIG type: osdConfig_t @@ -3764,6 +3781,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 @@ -3817,17 +3845,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/io/osd.c b/src/main/io/osd.c index a53ef5e547b..594d7cbe996 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -226,7 +226,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; @@ -1672,6 +1672,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(); @@ -2884,7 +2909,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; @@ -5011,7 +5035,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 8b53a3b30ea..8b52aa660ea 100644 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -288,7 +288,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; @@ -446,7 +451,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/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/navigation/navigation.h b/src/main/navigation/navigation.h index 8f688e94782..dbe29d2da89 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -239,7 +239,7 @@ typedef struct positionEstimationConfig_s { uint16_t max_surface_altitude; float w_z_baro_p; // Weight (cutoff frequency) for barometer altitude measurements - float w_z_baro_v; // Weight (cutoff frequency) for barometer climb rate measurements CR131_X + float w_z_baro_v; // Weight (cutoff frequency) for barometer climb rate measurements float w_z_surface_p; // Weight (cutoff frequency) for surface altitude measurements float w_z_surface_v; // Weight (cutoff frequency) for surface velocity measurements @@ -261,7 +261,7 @@ typedef struct positionEstimationConfig_s { float max_eph_epv; // Max estimated position error acceptable for estimation (cm) float baro_epv; // Baro position error - uint8_t default_alt_sensor; // default altitude sensor source // CR131 + uint8_t default_alt_sensor; // default altitude sensor source #ifdef USE_GPS_FIX_ESTIMATION uint8_t allow_gps_fix_estimation; #endif diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index 59f99883071..f8f3572a525 100644 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -58,7 +58,7 @@ navigationPosEstimator_t posEstimator; static float initialBaroAltitudeOffset = 0.0f; -PG_REGISTER_WITH_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig, PG_POSITION_ESTIMATION_CONFIG, 8); // CR131 +PG_REGISTER_WITH_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig, PG_POSITION_ESTIMATION_CONFIG, 8); PG_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig, // Inertial position estimator parameters @@ -71,7 +71,7 @@ PG_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig, .max_surface_altitude = SETTING_INAV_MAX_SURFACE_ALTITUDE_DEFAULT, .w_z_baro_p = SETTING_INAV_W_Z_BARO_P_DEFAULT, - .w_z_baro_v = SETTING_INAV_W_Z_BARO_V_DEFAULT, // CR131_X + .w_z_baro_v = SETTING_INAV_W_Z_BARO_V_DEFAULT, .w_z_surface_p = SETTING_INAV_W_Z_SURFACE_P_DEFAULT, .w_z_surface_v = SETTING_INAV_W_Z_SURFACE_V_DEFAULT, @@ -92,7 +92,8 @@ PG_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig, .max_eph_epv = SETTING_INAV_MAX_EPH_EPV_DEFAULT, .baro_epv = SETTING_INAV_BARO_EPV_DEFAULT, - .default_alt_sensor = SETTING_INAV_DEFAULT_ALT_SENSOR_DEFAULT, // CR131 + + .default_alt_sensor = SETTING_INAV_DEFAULT_ALT_SENSOR_DEFAULT, #ifdef USE_GPS_FIX_ESTIMATION .allow_gps_fix_estimation = SETTING_INAV_ALLOW_GPS_FIX_ESTIMATION_DEFAULT #endif @@ -352,6 +353,7 @@ static bool gravityCalibrationIsValid(void) return gravityCalibrationComplete() || posEstimator.imu.calibratedGravityCMSS; } // CR131 + #define ACC_VIB_FACTOR_S 1.0f #define ACC_VIB_FACTOR_E 3.0f static void updateIMUEstimationWeight(const float dt) @@ -446,7 +448,7 @@ static void updateIMUTopic(timeUs_t currentTimeUs) posEstimator.imu.accelNEU.z -= posEstimator.imu.calibratedGravityCMSS; posEstimator.imu.accelNEU.z += applySensorTempCompensation(10 * gyroGetTemperature(), imuMeasuredAccelBF.z, SENSOR_INDEX_ACC); // CR134 } - else { /* If calibration is incomplete - report zero acceleration */ + else { // If calibration is incomplete - report zero acceleration posEstimator.imu.accelNEU.x = 0.0f; posEstimator.imu.accelNEU.y = 0.0f; posEstimator.imu.accelNEU.z = 0.0f; @@ -490,7 +492,7 @@ static uint32_t calculateCurrentValidityFlags(timeUs_t currentTimeUs) /* Figure out if we have valid position data from our data sources */ uint32_t newFlags = 0; - const float max_eph_epv = positionEstimationConfig()->max_eph_epv; // CR131_X + const float max_eph_epv = positionEstimationConfig()->max_eph_epv; if ((sensors(SENSOR_GPS) #ifdef USE_GPS_FIX_ESTIMATION @@ -572,32 +574,28 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx) DEBUG_SET(DEBUG_ALTITUDE, 7, accGetClipCount()); // Clip count bool correctOK = false; - - // float wBaro = 1.0f; - // float wGps = 1.0f; // CR131 - const uint8_t defaultAltitudeSource = positionEstimationConfig()->default_alt_sensor; float wGps = defaultAltitudeSource == ALTITUDE_SOURCE_BARO_ONLY && ctx->newFlags & EST_BARO_VALID ? 0.0f : 1.0f; float wBaro = defaultAltitudeSource == ALTITUDE_SOURCE_GPS_ONLY && ctx->newFlags & EST_GPS_Z_VALID ? 0.0f : 1.0f; if (wBaro && ctx->newFlags & EST_BARO_VALID && wGps && ctx->newFlags & EST_GPS_Z_VALID) { - // if (ctx->newFlags & EST_BARO_VALID && ctx->newFlags & EST_GPS_Z_VALID) { - const float gpsBaroResidual = fabsf(posEstimator.gps.pos.z - posEstimator.baro.alt); - // Fade out the non default sensor to prevent sudden jump INAV_GPS_DEFAULT_EPV + // Fade out the non default sensor to prevent sudden jump uint16_t residualErrorEpvLimit = defaultAltitudeSource == ALTITUDE_SOURCE_BARO ? 2 * positionEstimationConfig()->baro_epv : positionEstimationConfig()->max_eph_epv; const float start_epv = residualErrorEpvLimit; const float end_epv = residualErrorEpvLimit * 2.0f; + + // Calculate residual gps/baro sensor weighting based on assumed default altitude source = GPS wBaro = scaleRangef(constrainf(gpsBaroResidual, start_epv, end_epv), start_epv, end_epv, 1.0f, 0.0f); - if (defaultAltitudeSource == ALTITUDE_SOURCE_BARO) { + if (defaultAltitudeSource == ALTITUDE_SOURCE_BARO) { // flip residual sensor weighting if default = BARO wGps = wBaro; wBaro = 1.0f; } } - // CR131 - if (ctx->newFlags & EST_BARO_VALID && wBaro) { // CR131 + + if (ctx->newFlags & EST_BARO_VALID && wBaro) { timeUs_t currentTimeUs = micros(); if (!ARMING_FLAG(ARMED)) { @@ -620,14 +618,13 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx) ((ctx->newFlags & EST_BARO_VALID) && posEstimator.state.isBaroGroundValid && posEstimator.baro.alt < posEstimator.state.baroGroundAlt)); // Altitude - // CR131 const float baroAltResidual = wBaro * ((isAirCushionEffectDetected ? posEstimator.state.baroGroundAlt : posEstimator.baro.alt) - posEstimator.est.pos.z); const float baroVelZResidual = isAirCushionEffectDetected ? 0.0f : wBaro * (posEstimator.baro.baroAltRate - posEstimator.est.vel.z); - const float w_z_baro_p = positionEstimationConfig()->w_z_baro_p; // CR131_X + const float w_z_baro_p = positionEstimationConfig()->w_z_baro_p; ctx->estPosCorr.z += baroAltResidual * w_z_baro_p * ctx->dt; ctx->estVelCorr.z += baroAltResidual * sq(w_z_baro_p) * ctx->dt; - ctx->estVelCorr.z += baroVelZResidual * positionEstimationConfig()->w_z_baro_v * ctx->dt; // CR131_X + ctx->estVelCorr.z += baroVelZResidual * positionEstimationConfig()->w_z_baro_v * ctx->dt; ctx->newEPV = updateEPE(posEstimator.est.epv, ctx->dt, posEstimator.baro.epv, w_z_baro_p); @@ -636,11 +633,10 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx) ctx->accBiasCorr.z -= baroAltResidual * sq(w_z_baro_p); } - correctOK = ARMING_FLAG(WAS_EVER_ARMED); - // CR131 + correctOK = ARMING_FLAG(WAS_EVER_ARMED); // No correction until first armed } DEBUG_SET(DEBUG_ALWAYS, 4, posEstimator.baro.baroAltRate); - if (ctx->newFlags & EST_GPS_Z_VALID && wGps) { // CR131 + if (ctx->newFlags & EST_GPS_Z_VALID && wGps) { // Reset current estimate to GPS altitude if estimate not valid if (!(ctx->newFlags & EST_Z_VALID)) { ctx->estPosCorr.z += posEstimator.gps.pos.z - posEstimator.est.pos.z; @@ -649,10 +645,9 @@ DEBUG_SET(DEBUG_ALWAYS, 4, posEstimator.baro.baroAltRate); } else { // Altitude - // CR131 const float gpsAltResidual = wGps * (posEstimator.gps.pos.z - posEstimator.est.pos.z); const float gpsVelZResidual = wGps * (posEstimator.gps.vel.z - posEstimator.est.vel.z); - const float w_z_gps_p = positionEstimationConfig()->w_z_gps_p; // CR131_X + const float w_z_gps_p = positionEstimationConfig()->w_z_gps_p; ctx->estPosCorr.z += gpsAltResidual * w_z_gps_p * ctx->dt; ctx->estVelCorr.z += gpsAltResidual * sq(w_z_gps_p) * ctx->dt; @@ -664,8 +659,7 @@ DEBUG_SET(DEBUG_ALWAYS, 4, posEstimator.baro.baroAltRate); ctx->accBiasCorr.z -= gpsAltResidual * sq(w_z_gps_p); } - correctOK = ARMING_FLAG(WAS_EVER_ARMED); - // CR131 + correctOK = ARMING_FLAG(WAS_EVER_ARMED); // No correction until first armed } // DEBUG_SET(DEBUG_ALWAYS, 0, wBaro * 100); // DEBUG_SET(DEBUG_ALWAYS, 1, wGps * 100); @@ -743,6 +737,8 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs) { estimationContext_t ctx; + const float max_eph_epv = positionEstimationConfig()->max_eph_epv; + /* Calculate dT */ ctx.dt = US2S(currentTimeUs - posEstimator.est.lastUpdateTime); posEstimator.est.lastUpdateTime = currentTimeUs; @@ -798,7 +794,7 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs) vectorAdd(&posEstimator.est.vel, &posEstimator.est.vel, &ctx.estVelCorr); /* Correct accelerometer bias */ - const float w_acc_bias = positionEstimationConfig()->w_acc_bias; // CR131_X + const float w_acc_bias = positionEstimationConfig()->w_acc_bias; if (w_acc_bias > 0.0f) { const float accelBiasCorrMagnitudeSq = sq(ctx.accBiasCorr.x) + sq(ctx.accBiasCorr.y) + sq(ctx.accBiasCorr.z); if (accelBiasCorrMagnitudeSq < sq(INAV_ACC_BIAS_ACCEPTANCE_VALUE)) { diff --git a/src/main/navigation/navigation_pos_estimator_private.h b/src/main/navigation/navigation_pos_estimator_private.h index 360d9176bb2..5e3727620c8 100644 --- a/src/main/navigation/navigation_pos_estimator_private.h +++ b/src/main/navigation/navigation_pos_estimator_private.h @@ -145,14 +145,14 @@ typedef enum { EST_XY_VALID = (1 << 5), EST_Z_VALID = (1 << 6), } navPositionEstimationFlags_e; -// CR131 + typedef enum { ALTITUDE_SOURCE_GPS, ALTITUDE_SOURCE_BARO, ALTITUDE_SOURCE_GPS_ONLY, ALTITUDE_SOURCE_BARO_ONLY, } navDefaultAltitudeSensor_e; -// CR131 + typedef struct { timeUs_t baroGroundTimeout; float baroGroundAlt; 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/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/TBS_LUCID_FC/target.h b/src/main/target/TBS_LUCID_FC/target.h index 7a9ed60fb52..6e755b9a1a5 100644 --- a/src/main/target/TBS_LUCID_FC/target.h +++ b/src/main/target/TBS_LUCID_FC/target.h @@ -41,6 +41,7 @@ #define UART1_RX_PIN PA10 #define USE_UART2 +#define UART2_AF 6 #define UART2_TX_PIN NONE #define UART2_RX_PIN PB0 @@ -120,13 +121,15 @@ #define USE_PINIO #define USE_PINIOBOX -#define PINIO1_PIN PC5 // Camera Control +#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 diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c index 4e8d54535a3..92ddeb7a990 100644 --- a/src/main/telemetry/mavlink.c +++ b/src/main/telemetry/mavlink.c @@ -1126,11 +1126,39 @@ static bool handleIncoming_PARAM_REQUEST_LIST(void) { return true; } +static void mavlinkParseRxStats(const mavlink_radio_status_t *msg) { + switch(telemetryConfig()->mavlink.radio_type) { + case MAVLINK_RADIO_SIK: + // rssi scaling info from: https://ardupilot.org/rover/docs/common-3dr-radio-advanced-configuration-and-technical-information.html + rxLinkStatistics.uplinkRSSI = (msg->rssi / 1.9) - 127; + rxLinkStatistics.uplinkSNR = msg->noise / 1.9; + rxLinkStatistics.uplinkLQ = msg->rssi != 255 ? scaleRange(msg->rssi, 0, 254, 0, 100) : 0; + break; + case MAVLINK_RADIO_ELRS: + rxLinkStatistics.uplinkRSSI = -msg->remrssi; + rxLinkStatistics.uplinkSNR = msg->noise; + rxLinkStatistics.uplinkLQ = scaleRange(msg->rssi, 0, 255, 0, 100); + break; + case MAVLINK_RADIO_GENERIC: + default: + rxLinkStatistics.uplinkRSSI = msg->rssi; + rxLinkStatistics.uplinkSNR = msg->noise; + rxLinkStatistics.uplinkLQ = msg->rssi != 255 ? scaleRange(msg->rssi, 0, 254, 0, 100) : 0; + break; + } +} + static bool handleIncoming_RADIO_STATUS(void) { mavlink_radio_status_t msg; mavlink_msg_radio_status_decode(&mavRecvMsg, &msg); txbuff_valid = true; txbuff_free = msg.txbuf; + + if (rxConfig()->receiverType == RX_TYPE_SERIAL && + rxConfig()->serialrx_provider == SERIALRX_MAVLINK) { + mavlinkParseRxStats(&msg); + } + return true; } @@ -1227,7 +1255,7 @@ static bool processMAVLinkIncomingTelemetry(void) #endif case MAVLINK_MSG_ID_RADIO_STATUS: handleIncoming_RADIO_STATUS(); - // Don't set that we handled a message, otherwise radio status packets will block telemetry messages + // Don't set that we handled a message, otherwise radio status packets will block telemetry messages. return false; default: return false; @@ -1260,7 +1288,7 @@ void handleMAVLinkTelemetry(timeUs_t currentTimeUs) // Determine whether to send telemetry back based on flow control / pacing if (txbuff_valid) { // Use flow control if available - shouldSendTelemetry = txbuff_free >= 33; + shouldSendTelemetry = txbuff_free >= telemetryConfig()->mavlink.min_txbuff; } else { // If not, use blind frame pacing - and back off for collision avoidance if half-duplex bool halfDuplexBackoff = (isMAVLinkTelemetryHalfDuplex() && receivedMessage); 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..e0820234e65 100644 --- a/src/main/telemetry/telemetry.c +++ b/src/main/telemetry/telemetry.c @@ -56,12 +56,13 @@ #include "telemetry/ghst.h" -PG_REGISTER_WITH_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig, PG_TELEMETRY_CONFIG, 6); +PG_REGISTER_WITH_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig, PG_TELEMETRY_CONFIG, 8); 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, @@ -92,7 +93,9 @@ PG_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig, .extra1_rate = SETTING_MAVLINK_EXTRA1_RATE_DEFAULT, .extra2_rate = SETTING_MAVLINK_EXTRA2_RATE_DEFAULT, .extra3_rate = SETTING_MAVLINK_EXTRA3_RATE_DEFAULT, - .version = SETTING_MAVLINK_VERSION_DEFAULT + .version = SETTING_MAVLINK_VERSION_DEFAULT, + .min_txbuff = SETTING_MAVLINK_MIN_TXBUFFER_DEFAULT, + .radio_type = SETTING_MAVLINK_RADIO_TYPE_DEFAULT } ); diff --git a/src/main/telemetry/telemetry.h b/src/main/telemetry/telemetry.h index c7edad973b8..c8808959277 100644 --- a/src/main/telemetry/telemetry.h +++ b/src/main/telemetry/telemetry.h @@ -36,6 +36,12 @@ typedef enum { LTM_RATE_SLOW } ltmUpdateRate_e; +typedef enum { + MAVLINK_RADIO_GENERIC, + MAVLINK_RADIO_ELRS, + MAVLINK_RADIO_SIK, +} mavlinkRadio_e; + typedef enum { SMARTPORT_FUEL_UNIT_PERCENT, SMARTPORT_FUEL_UNIT_MAH, @@ -46,6 +52,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; @@ -72,6 +79,8 @@ typedef struct telemetryConfig_s { uint8_t extra2_rate; uint8_t extra3_rate; uint8_t version; + uint8_t min_txbuff; + uint8_t radio_type; } mavlink; } telemetryConfig_t;