Merge remote-tracking branch 'origin/master' into dzikuvx-rework-7804

2025-07-24 16:55:29 +03:00 · 2022-02-22 10:52:30 +01:00 · 2022-02-22 10:52:30 +01:00 · b3a8cc2835
commit b3a8cc2835
parent b920d73c02 864fd42306
53 changed files with 1419 additions and 131 deletions
--- a/build.sh
+++ b/build.sh
@ -1,3 +1,4 @@
+#!/usr/bin/env bash
 set -e

 if [[ $# == 0 ]]; then
@ -6,9 +7,15 @@ Usage syntax: ./build.sh <TARGET>

 Notes:
  * You can specify multiple targets.
-  * If no targets are specified, *all* of them will be built.
+    ./build.sh <TARGET_1> <TARGET_2> <TARGET_N>
+  * To get a list of all targets use \"help\". Hint: pipe the output through a pager.
+    ./build.sh help | less
+  * To build all targets use \"all\"
+    ./build.sh all
  * To clean a target prefix it with \"clean_\".
-  * To clean all targets just use \"clean\"."
+    ./build.sh clean_MATEKF405SE
+  * To clean all targets just use \"clean\".
+    ./build.sh clean"
  exit 1
 fi

--- a/docs/Battery.md
+++ b/docs/Battery.md
@ -195,7 +195,25 @@ Note that in this example even though your warning capacity (`battery_capacity_w

 ## Battery profiles

-Up to 3 battery profiles are supported. You can select the battery profile from the GUI, OSD menu, [stick commands](Controls.md) and CLI command `battery_profile n`. Battery profiles store the following settings (see above for an explanation of each setting): `bat_cells`, `vbat_cell_detect_voltage`, `vbat_max_cell_voltage`, `vbat_warning_cell_voltage`, `vbat_min_cell_voltage`, `battery_capacity_unit`, `battery_capacity`, `battery_capacity_warning`, `battery_capacity_critical`
+Up to 3 battery profiles are supported. You can select the battery profile from the GUI, OSD menu, [stick commands](Controls.md) and CLI command `battery_profile n`. Battery profiles store the following settings (see above for an explanation of each setting): 
+- `bat_cells`
+- `vbat_cell_detect_voltage`
+- `vbat_max_cell_voltage`
+- `vbat_warning_cell_voltage`
+- `vbat_min_cell_voltage`
+- `battery_capacity_unit`
+- `battery_capacity`
+- `battery_capacity_warning`
+- `battery_capacity_critical`
+- `throttle_idle`
+- `fw_min_throttle_down_pitch`
+- `nav_fw_cruise_thr`
+- `nav_fw_min_thr`
+- `nav_fw_pitch2thr`
+- `nav_fw_launch_thr`
+- `nav_fw_launch_idle_thr`
+- `failsafe_throttle`
+- `nav_mc_hover_thr`

 To enable the automatic battery profile switching based on battery voltage enable the `BAT_PROF_AUTOSWITCH` feature. For a profile to be automatically selected the number of cells of the battery needs to be specified (>0).

--- a/docs/Programming
+++ b/docs/Programming
@ -76,6 +76,8 @@ IPF can be edited using INAV Configurator user interface, of via CLI
 | 40            | MOD           | Divide `Operand A` by `Operand B` and returns the remainder |
 | 41			| LOITER_RADIUS_OVERRIDE | Sets the loiter radius to `Operand A` [`0` : `100000`] in cm. If the value is lower than the loiter radius set in the **Advanced Tuning**, that will be used. |
 | 42            | SET_PROFILE   | Sets the active config profile (PIDFF/Rates/Filters/etc) to `Operand A`. `Operand A` must be a valid profile number, currently from 1 to 3. If not, the profile will not change |
+| 43			| MIN				| Finds the lowest value of `Operand A` and `Operand B` |
+| 44			| MAX				| Finds the highest value of `Operand A` and `Operand B` |

 ### Operands

--- a/docs/Settings.md
+++ b/docs/Settings.md
@ -188,7 +188,7 @@ Defines airmode THROTTLE activation threshold when `airmode_type` **THROTTLE_THR

 | Default | Min | Max |
 | --- | --- | --- |
-| 1300 | 1000 | 2000 |
+| 1150 | 1000 | 2000 |

 ---

@ -1602,6 +1602,36 @@ Use Dynamic LPF instead of static gyro stage1 LPF. Dynamic Gyro LPF updates gyro

 ---

+### gyro_zero_x
+
+Calculated gyro zero calibration of axis X
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 0 | -32768 | 32767 |
+
+---
+
+### gyro_zero_y
+
+Calculated gyro zero calibration of axis Y
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 0 | -32768 | 32767 |
+
+---
+
+### gyro_zero_z
+
+Calculated gyro zero calibration of axis Z
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 0 | -32768 | 32767 |
+
+---
+
 ### has_flaps

 Defines is UAV is capable of having flaps. If ON and AIRPLANE `platform_type` is used, **FLAPERON** flight mode will be available for the pilot
@ -2102,6 +2132,26 @@ _// TODO_

 ---

+### init_gyro_cal
+
+If defined to 'OFF', it will ignore the gyroscope calibration done at each startup. Instead, the gyroscope last calibration from when you calibrated will be used. It also means you don't have to keep the UAV stationary during a startup.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| ON | OFF | ON |
+
+---
+
+### ins_gravity_cmss
+
+Calculated 1G of Acc axis Z to use in INS
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 0.0 | 0 | 1000 |
+
+---
+
 ### iterm_windup

 Used to prevent Iterm accumulation on during maneuvers. Iterm will be dampened when motors are reaching it's limit (when requested motor correction range is above percentage specified by this parameter)
@ -2818,7 +2868,7 @@ Craft name

 | Default | Min | Max |
 | --- | --- | --- |
-| _empty_ |  |  |
+| _empty_ |  | MAX_NAME_LENGTH |

 ---

@ -3852,6 +3902,16 @@ Defines how Pitch/Roll input from RC receiver affects flight in POSHOLD mode: AT

 ---

+### nav_wp_enforce_altitude
+
+Forces craft to achieve the set WP altitude as well as position before moving to next WP. Position is held and altitude adjusted as required before moving on.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| OFF | OFF | ON |
+
+---
+
 ### nav_wp_load_on_boot

 If set to ON, waypoints will be automatically loaded from EEPROM to the FC during startup.
@ -4582,6 +4642,16 @@ Auto swap display time interval between disarm stats pages (seconds). Reverts to

 ---

+### osd_system_msg_display_time
+
+System message display cycle time for multiple messages (milliseconds).
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 1000 | 500 | 5000 |
+
+---
+
 ### osd_telemetry

 To enable OSD telemetry for antenna tracker. Possible values are `OFF`, `ON` and `TEST`
@ -4624,7 +4694,7 @@ IMPERIAL, METRIC, UK

 ### osd_video_system

-Video system used. Possible values are `AUTO`, `PAL` and `NTSC`
+Video system used. Possible values are `AUTO`, `PAL`, `NTSC`, and `HD`

 | Default | Min | Max |
 | --- | --- | --- |
@ -5404,7 +5474,7 @@ Total flight distance [in meters]. The value is updated on every disarm when "st

 ### stats_total_energy

-_// TODO_
+Total energy consumption [in mWh]. The value is updated on every disarm when "stats" are enabled.

 | Default | Min | Max |
 | --- | --- | --- |
--- a/docs/development/Building
+++ b/docs/development/Building
@ -20,6 +20,12 @@ Where `<TARGET>` must be replaced with the name of the target that you want to b
 ./build.sh MATEKF405SE
 ```

+Run the script with no arguments to get more details on its usage:
+
+```
+./build.sh
+```
+
 ## Windows 10

 Docker on Windows requires full paths for mounting volumes in `docker run` commands. For example: `c:\Users\pspyc\Documents\Projects\inav` becomes `//c/Users/pspyc/Documents/Projects/inav` .
--- a/src/main/CMakeLists.txt
+++ b/src/main/CMakeLists.txt
@ -496,6 +496,8 @@ main_sources(COMMON_SRC
    io/displayport_msp.h
    io/displayport_oled.c
    io/displayport_oled.h
+    io/displayport_hdzero_osd.c
+    io/displayport_hdzero_osd.h
    io/displayport_srxl.c
    io/displayport_srxl.h
    io/displayport_hott.c
--- a/src/main/cms/cms_menu_navigation.c
+++ b/src/main/cms/cms_menu_navigation.c
@ -192,6 +192,7 @@ static const OSD_Entry cmsx_menuMissionSettingsEntries[] =
    OSD_SETTING_ENTRY("MISSION FAILSAFE", SETTING_FAILSAFE_MISSION),
    OSD_SETTING_ENTRY("WP LOAD ON BOOT", SETTING_NAV_WP_LOAD_ON_BOOT),
    OSD_SETTING_ENTRY("WP REACHED RADIUS", SETTING_NAV_WP_RADIUS),
+    OSD_SETTING_ENTRY("WP ENFORCE ALTITUDE", SETTING_NAV_WP_ENFORCE_ALTITUDE),
    OSD_SETTING_ENTRY("WP SAFE DISTANCE", SETTING_NAV_WP_SAFE_DISTANCE),
 #ifdef USE_MULTI_MISSION
    OSD_SETTING_ENTRY("MULTI MISSION NUMBER", SETTING_NAV_WP_MULTI_MISSION_INDEX),
--- a/src/main/common/maths.h
+++ b/src/main/common/maths.h
@ -66,6 +66,8 @@
 #define CMSEC_TO_CENTIKPH(cms)      (cms * 3.6)
 #define CMSEC_TO_CENTIKNOTS(cms)    (cms * 1.943845)

+#define C_TO_KELVIN(temp) (temp + 273.15f)
+
 // copied from https://code.google.com/p/cxutil/source/browse/include/cxutil/utility.h#70
 #define _CHOOSE2(binoper, lexpr, lvar, rexpr, rvar)         \
    ( __extension__ ({                                      \
--- a/src/main/drivers/io_port_expander.c
+++ b/src/main/drivers/io_port_expander.c
@ -60,7 +60,7 @@ void ioPortExpanderSync(void)
 {
    if (ioPortExpanderState.active && ioPortExpanderState.shouldSync) {
        pcf8574Write(ioPortExpanderState.state);
-        ioPortExpanderState.shouldSync = false;;
+        ioPortExpanderState.shouldSync = false;
    }
 }

--- a/src/main/drivers/pitotmeter/pitotmeter_ms4525.c
+++ b/src/main/drivers/pitotmeter/pitotmeter_ms4525.c
@ -97,7 +97,7 @@ static void ms4525_calculate(pitotDev_t * pitot, float *pressure, float *tempera
    //float dP = ctx->ms4525_up * 10.0f * 0.1052120688f;
    const float dP_psi = -((ctx->ms4525_up - 0.1f * 16383) * (P_max - P_min) / (0.8f * 16383) + P_min);
    float dP = dP_psi * PSI_to_Pa;
-    float T  = (float)(200.0f * (int32_t)ctx->ms4525_ut) / 2047.0f - 50.0f + 273.15f;
+    float T  = C_TO_KELVIN((float)(200.0f * (int32_t)ctx->ms4525_ut) / 2047.0f - 50.0f);

    if (pressure) {
        *pressure = dP;    // Pa
--- a/src/main/drivers/serial_softserial.c
+++ b/src/main/drivers/serial_softserial.c
@ -572,7 +572,7 @@ uint32_t softSerialTxBytesFree(const serialPort_t *instance)

    softSerial_t *s = (softSerial_t *)instance;

-    uint8_t bytesUsed = (s->port.txBufferHead - s->port.txBufferTail) & (s->port.txBufferSize - 1);
+    uint32_t bytesUsed = (s->port.txBufferHead - s->port.txBufferTail) & (s->port.txBufferSize - 1);

    return (s->port.txBufferSize - 1) - bytesUsed;
 }
--- a/src/main/fc/cli.c
+++ b/src/main/fc/cli.c
@ -2249,6 +2249,11 @@ static void cliFlashInfo(char *cmdline)

    const flashGeometry_t *layout = flashGetGeometry();
    
+    if (layout->totalSize == 0) {
+        cliPrintLine("Flash not available");
+        return;
+    }
+
    cliPrintLinef("Flash sectors=%u, sectorSize=%u, pagesPerSector=%u, pageSize=%u, totalSize=%u",
            layout->sectors, layout->sectorSize, layout->pagesPerSector, layout->pageSize, layout->totalSize);

@ -2277,6 +2282,13 @@ static void cliFlashErase(char *cmdline)
 {
    UNUSED(cmdline);

+    const flashGeometry_t *layout = flashGetGeometry();
+    
+    if (layout->totalSize == 0) {
+        cliPrintLine("Flash not available");
+        return;
+    }
+    
    cliPrintLine("Erasing...");
    flashfsEraseCompletely();

@ -3344,7 +3356,11 @@ static void cliStatus(char *cmdline)
        hardwareSensorStatusNames[getHwRangefinderStatus()],
        hardwareSensorStatusNames[getHwOpticalFlowStatus()],
        hardwareSensorStatusNames[getHwGPSStatus()],
+#ifdef USE_SECONDARY_IMU
        hardwareSensorStatusNames[getHwSecondaryImuStatus()]
+#else
+        hardwareSensorStatusNames[0]
+#endif
    );

 #ifdef USE_ESC_SENSOR
--- a/src/main/fc/config.c
+++ b/src/main/fc/config.c
@ -459,6 +459,22 @@ void setConfigBatteryProfileAndWriteEEPROM(uint8_t profileIndex)
    beeperConfirmationBeeps(profileIndex + 1);
 }

+void setGyroCalibrationAndWriteEEPROM(int16_t getGyroZero[XYZ_AXIS_COUNT]) {
+    gyroConfigMutable()->gyro_zero_cal[X] = getGyroZero[X];
+    gyroConfigMutable()->gyro_zero_cal[Y] = getGyroZero[Y];
+    gyroConfigMutable()->gyro_zero_cal[Z] = getGyroZero[Z];
+    // save the calibration
+    writeEEPROM();
+    readEEPROM();
+}
+
+void setGravityCalibrationAndWriteEEPROM(float getGravity) {
+    gyroConfigMutable()->gravity_cmss_cal = getGravity;
+    // save the calibration
+    writeEEPROM();
+    readEEPROM();
+}
+
 void beeperOffSet(uint32_t mask)
 {
    beeperConfigMutable()->beeper_off_flags |= mask;
--- a/src/main/fc/config.h
+++ b/src/main/fc/config.h
@ -19,6 +19,7 @@

 #include <stdint.h>
 #include <stdbool.h>
+#include "common/axis.h"
 #include "common/time.h"
 #include "config/parameter_group.h"
 #include "drivers/adc.h"
@ -29,6 +30,7 @@
 #define MAX_NAME_LENGTH 16

 #define TASK_GYRO_LOOPTIME 250 // Task gyro always runs at 4kHz
+
 typedef enum {
    FEATURE_THR_VBAT_COMP = 1 << 0,
    FEATURE_VBAT = 1 << 1,
@ -130,6 +132,9 @@ uint8_t getConfigBatteryProfile(void);
 bool setConfigBatteryProfile(uint8_t profileIndex);
 void setConfigBatteryProfileAndWriteEEPROM(uint8_t profileIndex);

+void setGyroCalibrationAndWriteEEPROM(int16_t getGyroZero[XYZ_AXIS_COUNT]);
+void setGravityCalibrationAndWriteEEPROM(float getGravity);
+
 bool canSoftwareSerialBeUsed(void);
 void applyAndSaveBoardAlignmentDelta(int16_t roll, int16_t pitch);

--- a/src/main/fc/fc_core.c
+++ b/src/main/fc/fc_core.c
@ -978,12 +978,12 @@ void taskUpdateRxMain(timeUs_t currentTimeUs)
 // returns seconds
 float getFlightTime()
 {
-    return (float)(flightTime / 1000) / 1000;
+    return US2S(flightTime);
 }

 float getArmTime()
 {
-    return (float)(armTime / 1000) / 1000;
+    return US2S(armTime);
 }

 void fcReboot(bool bootLoader)
--- a/src/main/fc/fc_init.c
+++ b/src/main/fc/fc_init.c
@ -109,6 +109,7 @@
 #include "io/displayport_frsky_osd.h"
 #include "io/displayport_msp.h"
 #include "io/displayport_max7456.h"
+#include "io/displayport_hdzero_osd.h"
 #include "io/displayport_srxl.h"
 #include "io/flashfs.h"
 #include "io/gps.h"
@ -553,6 +554,11 @@ void init(void)
            osdDisplayPort = frskyOSDDisplayPortInit(osdConfig()->video_system);
        }
 #endif
+#ifdef USE_HDZERO_OSD
+        if (!osdDisplayPort) {
+            osdDisplayPort = hdzeroOsdDisplayPortInit();
+        }
+#endif
 #if defined(USE_MAX7456)
        // If there is a max7456 chip for the OSD and we have no
        // external OSD initialized, use it.
--- a/src/main/fc/fc_msp.c
+++ b/src/main/fc/fc_msp.c
@ -425,7 +425,11 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
        sbufWriteU8(dst, getHwRangefinderStatus());
        sbufWriteU8(dst, getHwPitotmeterStatus());
        sbufWriteU8(dst, getHwOpticalFlowStatus());
+#ifdef USE_SECONDARY_IMU
        sbufWriteU8(dst, getHwSecondaryImuStatus());
+#else
+        sbufWriteU8(dst, 0);
+#endif
        break;

    case MSP_ACTIVEBOXES:
--- a/src/main/fc/fc_msp_box.c
+++ b/src/main/fc/fc_msp_box.c
@ -188,48 +188,49 @@ void initActiveBoxIds(void)

    ADD_ACTIVE_BOX(BOXHEADINGHOLD);

-    if ((sensors(SENSOR_ACC) || sensors(SENSOR_MAG)) && STATE(MULTIROTOR)) {
-        ADD_ACTIVE_BOX(BOXHEADFREE);
-        ADD_ACTIVE_BOX(BOXHEADADJ);
-    }
-
-    if (STATE(ALTITUDE_CONTROL) && STATE(MULTIROTOR)) {
-        ADD_ACTIVE_BOX(BOXFPVANGLEMIX);
-    }
-
    //Camstab mode is enabled always
    ADD_ACTIVE_BOX(BOXCAMSTAB);

-#ifdef USE_GPS
-    if (STATE(ALTITUDE_CONTROL) && (sensors(SENSOR_BARO) || (feature(FEATURE_GPS) && (STATE(AIRPLANE) || positionEstimationConfig()->use_gps_no_baro)))) {
-        ADD_ACTIVE_BOX(BOXNAVALTHOLD);
    if (STATE(MULTIROTOR)) {
+        if ((sensors(SENSOR_ACC) || sensors(SENSOR_MAG))) {
+            ADD_ACTIVE_BOX(BOXHEADFREE);
+            ADD_ACTIVE_BOX(BOXHEADADJ);
+        }
+        if (sensors(SENSOR_BARO) && sensors(SENSOR_RANGEFINDER) && sensors(SENSOR_OPFLOW)) {
            ADD_ACTIVE_BOX(BOXSURFACE);
        }
+        ADD_ACTIVE_BOX(BOXFPVANGLEMIX);
    }

-    const bool navReadyMultirotor = STATE(MULTIROTOR) && (getHwCompassStatus() != HW_SENSOR_NONE) && sensors(SENSOR_ACC) && feature(FEATURE_GPS);
-    const bool navReadyOther = !STATE(MULTIROTOR) && sensors(SENSOR_ACC) && feature(FEATURE_GPS);
+    bool navReadyAltControl = sensors(SENSOR_BARO);
+#ifdef USE_GPS
+    navReadyAltControl = navReadyAltControl || (feature(FEATURE_GPS) && (STATE(AIRPLANE) || positionEstimationConfig()->use_gps_no_baro));
+
    const bool navFlowDeadReckoning = sensors(SENSOR_OPFLOW) && sensors(SENSOR_ACC) && positionEstimationConfig()->allow_dead_reckoning;
-    if (navFlowDeadReckoning || navReadyMultirotor || navReadyOther) {
-        if (!STATE(ROVER) && !STATE(BOAT)) {
-            ADD_ACTIVE_BOX(BOXNAVPOSHOLD);
+    bool navReadyPosControl = sensors(SENSOR_ACC) && feature(FEATURE_GPS);
+    if (STATE(MULTIROTOR)) {
+        navReadyPosControl = navReadyPosControl && getHwCompassStatus() != HW_SENSOR_NONE;
    }
+
+    if (STATE(ALTITUDE_CONTROL) && navReadyAltControl && (navReadyPosControl || navFlowDeadReckoning)) {
+        ADD_ACTIVE_BOX(BOXNAVPOSHOLD);
        if (STATE(AIRPLANE)) {
            ADD_ACTIVE_BOX(BOXLOITERDIRCHN);
        }
    }

-    if (navReadyMultirotor || navReadyOther) {
+    if (navReadyPosControl) {
+        if (!STATE(ALTITUDE_CONTROL) || (STATE(ALTITUDE_CONTROL) && navReadyAltControl)) {
            ADD_ACTIVE_BOX(BOXNAVRTH);
            ADD_ACTIVE_BOX(BOXNAVWP);
            ADD_ACTIVE_BOX(BOXHOMERESET);
            ADD_ACTIVE_BOX(BOXGCSNAV);
            ADD_ACTIVE_BOX(BOXPLANWPMISSION);
+        }

        if (STATE(AIRPLANE)) {
-            ADD_ACTIVE_BOX(BOXNAVCOURSEHOLD);
            ADD_ACTIVE_BOX(BOXNAVCRUISE);
+            ADD_ACTIVE_BOX(BOXNAVCOURSEHOLD);
            ADD_ACTIVE_BOX(BOXSOARING);
        }
    }
@ -239,8 +240,10 @@ void initActiveBoxIds(void)
        ADD_ACTIVE_BOX(BOXBRAKING);
    }
 #endif
-
-#endif
+#endif  // GPS
+    if (STATE(ALTITUDE_CONTROL) && navReadyAltControl) {
+        ADD_ACTIVE_BOX(BOXNAVALTHOLD);
+    }

    if (STATE(AIRPLANE) || STATE(ROVER) || STATE(BOAT)) {
        ADD_ACTIVE_BOX(BOXMANUAL);
--- a/src/main/fc/fc_tasks.c
+++ b/src/main/fc/fc_tasks.c
@ -67,6 +67,7 @@
 #include "io/smartport_master.h"
 #include "io/vtx.h"
 #include "io/osd_dji_hd.h"
+#include "io/displayport_hdzero_osd.h"
 #include "io/servo_sbus.h"

 #include "msp/msp_serial.h"
@ -108,6 +109,11 @@ void taskHandleSerial(timeUs_t currentTimeUs)
    // DJI OSD uses a special flavour of MSP (subset of Betaflight 4.1.1 MSP) - process as part of serial task
    djiOsdSerialProcess();
 #endif
+
+#ifdef USE_HDZERO_OSD
+	// Capture HDZero messages to determine if VTX is connected
+    hdzeroOsdSerialProcess(mspFcProcessCommand);
+#endif
 }

 void taskUpdateBattery(timeUs_t currentTimeUs)
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@ -75,7 +75,7 @@ tables:
    values: ["BATTERY", "CELL"]
    enum: osd_stats_min_voltage_unit_e
  - name: osd_video_system
-    values: ["AUTO", "PAL", "NTSC"]
+    values: ["AUTO", "PAL", "NTSC", "HD"]
    enum: videoSystem_e
  - name: osd_telemetry
    values: ["OFF", "ON","TEST"]
@ -309,6 +309,35 @@ groups:
        condition: USE_GYRO_KALMAN
        min: 1
        max: 16000
+      - name: init_gyro_cal
+        description: "If defined to 'OFF', it will ignore the gyroscope calibration done at each startup. Instead, the gyroscope last calibration from when you calibrated will be used. It also means you don't have to keep the UAV stationary during a startup."
+        default_value: ON
+        field: init_gyro_cal_enabled
+        type: bool
+      - name: gyro_zero_x
+        description: "Calculated gyro zero calibration of axis X"
+        default_value: 0
+        field: gyro_zero_cal[X]
+        min: INT16_MIN
+        max: INT16_MAX
+      - name: gyro_zero_y
+        description: "Calculated gyro zero calibration of axis Y"
+        default_value: 0
+        field: gyro_zero_cal[Y]
+        min: INT16_MIN
+        max: INT16_MAX
+      - name: gyro_zero_z
+        description: "Calculated gyro zero calibration of axis Z"
+        default_value: 0
+        field: gyro_zero_cal[Z]
+        min: INT16_MIN
+        max: INT16_MAX
+      - name: ins_gravity_cmss
+        description: "Calculated 1G of Acc axis Z to use in INS"
+        default_value: 0.0
+        field: gravity_cmss_cal
+        min: 0
+        max: 1000

  - name: PG_ADC_CHANNEL_CONFIG
    type: adcChannelConfig_t
@ -1648,7 +1677,7 @@ groups:
        table: airmodeHandlingType
      - name: airmode_throttle_threshold
        description: "Defines airmode THROTTLE activation threshold when `airmode_type` **THROTTLE_THRESHOLD** is used"
-        default_value: 1300
+        default_value: 1150
        field: airmodeThrottleThreshold
        min: 1000
        max: 2000
@ -2392,6 +2421,11 @@ groups:
        field: general.waypoint_radius
        min: 10
        max: 10000
+      - name: nav_wp_enforce_altitude
+        description: "Forces craft to achieve the set WP altitude as well as position before moving to next WP. Position is held and altitude adjusted as required before moving on."
+        default_value: OFF
+        field: general.flags.waypoint_enforce_altitude
+        type: bool
      - name: nav_wp_safe_distance
        description: "First waypoint in the mission should be closer than this value [cm]. A value of 0 disables this check."
        default_value: 10000
@ -3049,7 +3083,7 @@ groups:
        type: uint8_t
        default_value: "OFF"
      - name: osd_video_system
-        description: "Video system used. Possible values are `AUTO`, `PAL` and `NTSC`"
+        description: "Video system used. Possible values are `AUTO`, `PAL`, `NTSC`, and `HD`"
        default_value: "AUTO"
        table: osd_video_system
        field: video_system
@ -3470,6 +3504,13 @@ groups:
        max: 6
        default_value: 3

+      - 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: PG_OSD_COMMON_CONFIG
    type: osdCommonConfig_t
    headers: ["io/osd_common.h"]
@ -3510,6 +3551,7 @@ groups:
      - name: name
        description: "Craft name"
        default_value: ""
+        max: MAX_NAME_LENGTH

  - name: PG_MODE_ACTIVATION_OPERATOR_CONFIG
    type: modeActivationOperatorConfig_t
@ -3541,6 +3583,7 @@ groups:
        default_value: 0
        max: INT32_MAX
      - name: stats_total_energy
+        description: "Total energy consumption [in mWh]. The value is updated on every disarm when \"stats\" are enabled."
        max: INT32_MAX
        condition: USE_ADC
        default_value: 0
--- a/src/main/flight/mixer.c
+++ b/src/main/flight/mixer.c
@ -540,6 +540,17 @@ void FAST_CODE mixTable()

        motorValueWhenStopped = getReversibleMotorsThrottleDeadband();
        mixerThrottleCommand = constrain(rcCommand[THROTTLE], throttleRangeMin, throttleRangeMax);
+
+#ifdef USE_DSHOT
+        if(isMotorProtocolDigital() && feature(FEATURE_REVERSIBLE_MOTORS) && reversibleMotorsThrottleState == MOTOR_DIRECTION_BACKWARD) {
+            /*
+             * We need to start the throttle output from stick input to start in the middle of the stick at the low and.
+             * Without this, it's starting at the high side.
+             */
+            int throttleDistanceToMax = throttleRangeMax - rcCommand[THROTTLE];
+            mixerThrottleCommand = throttleRangeMin + throttleDistanceToMax;
+        }
+#endif
    } else {
        mixerThrottleCommand = rcCommand[THROTTLE];
        throttleRangeMin = throttleIdleValue;
--- a/src/main/flight/pid_autotune.c
+++ b/src/main/flight/pid_autotune.c
@ -198,7 +198,7 @@ void autotuneFixedWingUpdate(const flight_dynamics_index_t axis, float desiredRa
        tuneCurrent[axis].updateCount++;
        tuneCurrent[axis].absDesiredRateAccum += (absDesiredRate - tuneCurrent[axis].absDesiredRateAccum) / MIN(tuneCurrent[axis].updateCount, (uint32_t)AUTOTUNE_FIXED_WING_SAMPLES);
        tuneCurrent[axis].absReachedRateAccum += (absReachedRate - tuneCurrent[axis].absReachedRateAccum) / MIN(tuneCurrent[axis].updateCount, (uint32_t)AUTOTUNE_FIXED_WING_SAMPLES);
-        tuneCurrent[axis].absPidOutputAccum += (absPidOutput - tuneCurrent[axis].absPidOutputAccum) / MIN(tuneCurrent[axis].updateCount, (uint32_t)AUTOTUNE_FIXED_WING_SAMPLES);;
+        tuneCurrent[axis].absPidOutputAccum += (absPidOutput - tuneCurrent[axis].absPidOutputAccum) / MIN(tuneCurrent[axis].updateCount, (uint32_t)AUTOTUNE_FIXED_WING_SAMPLES);

        if ((tuneCurrent[axis].updateCount & 25) == 0 && tuneCurrent[axis].updateCount >= AUTOTUNE_FIXED_WING_MIN_SAMPLES) {
            if (pidAutotuneConfig()->fw_rate_adjustment != FIXED  && !FLIGHT_MODE(ANGLE_MODE)) { // Rate discovery is not possible in ANGLE mode
--- a/src/main/flight/secondary_imu.c
+++ b/src/main/flight/secondary_imu.c
@ -22,6 +22,9 @@
 * along with this program. If not, see http://www.gnu.org/licenses/.
 */

+#include "platform.h"
+#ifdef USE_SECONDARY_IMU
+
 #include "stdint.h"

 #include "build/debug.h"
@ -257,3 +260,5 @@ hardwareSensorStatus_e getHwSecondaryImuStatus(void)
    return HW_SENSOR_NONE;
 #endif
 }
+
+#endif
--- a/src/main/io/displayport_hdzero_osd.c
+++ b/src/main/io/displayport_hdzero_osd.c
@ -0,0 +1,427 @@
+/*
+ * 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 <stdbool.h>
+#include <stdint.h>
+#include <string.h>
+
+#include "platform.h"
+
+FILE_COMPILE_FOR_SPEED
+
+//#define HDZERO_STATS
+
+#if defined(USE_OSD) && defined(USE_HDZERO_OSD)
+
+#include "common/utils.h"
+#include "common/printf.h"
+#include "common/time.h"
+#include "common/bitarray.h"
+
+#include "drivers/display.h"
+#include "drivers/display_font_metadata.h"
+#include "drivers/osd_symbols.h"
+
+#include "msp/msp_protocol.h"
+#include "msp/msp_serial.h"
+
+#include "displayport_hdzero_osd.h"
+
+#define FONT_VERSION 3
+
+#define MSP_CLEAR_SCREEN 2
+#define MSP_WRITE_STRING 3
+#define MSP_DRAW_SCREEN 4
+#define MSP_SET_OPTIONS 5
+#define DRAW_FREQ_DENOM 4 // 60Hz
+#define TX_BUFFER_SIZE 1024
+#define VTX_TIMEOUT 1000 // 1 second timer
+
+static mspProcessCommandFnPtr mspProcessCommand;
+static mspPort_t hdZeroMspPort;
+static displayPort_t hdZeroOsdDisplayPort;
+static bool vtxSeen, vtxActive, vtxReset;
+static timeMs_t vtxHeartbeat;
+
+// HD screen size
+#define ROWS 18
+#define COLS 50
+#define SCREENSIZE (ROWS*COLS)
+static uint8_t screen[SCREENSIZE];
+static BITARRAY_DECLARE(fontPage, SCREENSIZE); // font page for each character on the screen
+static BITARRAY_DECLARE(dirty, SCREENSIZE); // change status for each character on the screen
+static bool screenCleared;
+
+extern uint8_t cliMode;
+
+#ifdef HDZERO_STATS
+static uint32_t dataSent;
+static uint8_t resetCount;
+#endif
+
+static int output(displayPort_t *displayPort, uint8_t cmd, uint8_t *subcmd, int len)
+{
+    UNUSED(displayPort);
+
+    int sent = 0;
+
+    if (!cliMode && vtxActive) {
+        sent = mspSerialPushPort(cmd, subcmd, len, &hdZeroMspPort, MSP_V1);
+    }
+
+#ifdef HDZERO_STATS
+    dataSent += sent;
+#endif
+
+    return sent;
+}
+
+static void checkVtxPresent(void)
+{
+    if (vtxActive && (millis()-vtxHeartbeat) > VTX_TIMEOUT) {
+        vtxActive = false;
+    }
+}
+
+static int setHdMode(displayPort_t *displayPort)
+{
+    checkVtxPresent();
+    uint8_t subcmd[] = { MSP_SET_OPTIONS, 0, 1 }; // font selection, mode (SD/HD)
+    return output(displayPort, MSP_DISPLAYPORT, subcmd, sizeof(subcmd));
+}
+
+static void hdZeroInit(void)
+{
+    memset(screen, SYM_BLANK, sizeof(screen));
+    BITARRAY_CLR_ALL(fontPage);
+    BITARRAY_CLR_ALL(dirty);
+}
+
+static int clearScreen(displayPort_t *displayPort)
+{
+    uint8_t subcmd[] = { MSP_CLEAR_SCREEN };
+
+    hdZeroInit();
+    setHdMode(displayPort);
+    screenCleared = true;
+    return output(displayPort, MSP_DISPLAYPORT, subcmd, sizeof(subcmd));
+}
+
+static bool readChar(displayPort_t *displayPort, uint8_t col, uint8_t row, uint16_t *c, textAttributes_t *attr)
+{
+    UNUSED(displayPort);
+
+    uint16_t pos = (row * COLS) + col;
+    if (pos >= SCREENSIZE) {
+        return false;
+    }
+
+    *c = screen[pos];
+    if (bitArrayGet(fontPage, pos)) {
+        *c |= 0x100;
+    }
+
+    if (attr) {
+        *attr = TEXT_ATTRIBUTES_NONE;
+    }
+
+    return true;
+}
+
+static int setChar(const uint16_t pos, const uint16_t c)
+{
+    if (pos < SCREENSIZE) {
+        uint8_t ch = c & 0xFF;
+        bool page = (c >> 8);
+        if (screen[pos] != ch || bitArrayGet(fontPage, pos) !=  page) {
+            screen[pos] = ch;
+            (page) ? bitArraySet(fontPage, pos) : bitArrayClr(fontPage, pos);
+            bitArraySet(dirty, pos);
+        }
+    }
+    return 0;
+}
+
+static int writeChar(displayPort_t *displayPort, uint8_t col, uint8_t row, uint16_t c, textAttributes_t attr)
+{
+    UNUSED(displayPort);
+    UNUSED(attr);
+
+    return setChar((row * COLS) + col, c);
+}
+
+static int writeString(displayPort_t *displayPort, uint8_t col, uint8_t row, const char *string, textAttributes_t attr)
+{
+    UNUSED(displayPort);
+    UNUSED(attr);
+
+    uint16_t pos = (row * COLS) + col;
+    while (*string) {
+        setChar(pos++, *string++);
+    }
+    return 0;
+}
+
+#ifdef HDZERO_STATS
+static void printStats(displayPort_t *displayPort, uint32_t updates)
+{
+    static timeMs_t lastTime;
+    static uint32_t maxDataSent, maxBufferUsed, maxUpdates;
+    timeMs_t currentTime = millis();
+    char lineBuffer[100];
+
+    if (updates > maxUpdates) {
+        maxUpdates = updates; // updates sent per displayWrite
+    }
+
+    uint32_t bufferUsed = TX_BUFFER_SIZE - serialTxBytesFree(hdZeroMspPort.port);
+    if (bufferUsed > maxBufferUsed) {
+        maxBufferUsed = bufferUsed; // serial buffer used after displayWrite
+    }
+
+    uint32_t diff = (currentTime - lastTime);
+    if (diff > 1000) { // Data sampled in 1 second
+        if (dataSent > maxDataSent) {
+            maxDataSent = dataSent; // bps (max 11520 allowed)
+        }
+
+        dataSent = 0;
+        lastTime = currentTime;
+    }
+
+
+    tfp_sprintf(lineBuffer, "R:%2d %4ld %5ld(%5ld) U:%2ld(%2ld) B:%3ld(%4ld,%4ld)", resetCount, (millis()-vtxHeartbeat),
+            dataSent, maxDataSent, updates, maxUpdates, bufferUsed, maxBufferUsed, hdZeroMspPort.port->txBufferSize);
+    writeString(displayPort, 0, 17, lineBuffer, 0);
+}
+#endif
+
+/**
+ * Write only changed characters to the VTX
+ */
+static int drawScreen(displayPort_t *displayPort) // 250Hz
+{
+    static uint8_t counter = 0;
+
+    if ((counter++ % DRAW_FREQ_DENOM) == 0) {
+        uint8_t subcmd[COLS + 4];
+        uint8_t updateCount = 0;
+        subcmd[0] = MSP_WRITE_STRING;
+
+        int next = BITARRAY_FIND_FIRST_SET(dirty, 0);
+        while (next >= 0) {
+            // Look for sequential dirty characters on the same line for the same font page
+            int pos = next;
+            uint8_t row = pos / COLS;
+            uint8_t col = pos % COLS;
+            int endOfLine = row * COLS + COLS;
+            bool page = bitArrayGet(fontPage, pos);
+
+            uint8_t len = 4;
+            do {
+                bitArrayClr(dirty, pos);
+                subcmd[len++] = screen[pos++];
+
+                if (bitArrayGet(dirty, pos)) {
+                    next = pos;
+                }
+            } while (next == pos && next < endOfLine && bitArrayGet(fontPage, next) == page);
+
+            subcmd[1] = row;
+            subcmd[2] = col;
+            subcmd[3] = page;
+            output(displayPort, MSP_DISPLAYPORT, subcmd, len);
+            updateCount++;
+            next = BITARRAY_FIND_FIRST_SET(dirty, pos);
+        }
+
+        if (updateCount > 0 || screenCleared) {
+            if (screenCleared) {
+                screenCleared = false;
+            }
+            subcmd[0] = MSP_DRAW_SCREEN;
+            output(displayPort, MSP_DISPLAYPORT, subcmd, 1);
+        }
+
+#ifdef HDZERO_STATS
+        printStats(displayPort, updateCount);
+#endif
+        checkVtxPresent();
+
+        if (vtxReset) {
+#ifdef HDZERO_STATS
+            resetCount++;
+#endif
+            clearScreen(displayPort);
+            vtxReset = false;
+        }
+    }
+
+    return 0;
+}
+
+static void resync(displayPort_t *displayPort)
+{
+    displayPort->rows = ROWS;
+    displayPort->cols = COLS;
+    setHdMode(displayPort);
+}
+
+static int screenSize(const displayPort_t *displayPort)
+{
+    UNUSED(displayPort);
+    return SCREENSIZE;
+}
+
+static uint32_t txBytesFree(const displayPort_t *displayPort)
+{
+    UNUSED(displayPort);
+    return mspSerialTxBytesFree();
+}
+
+static bool getFontMetadata(displayFontMetadata_t *metadata, const displayPort_t *displayPort)
+{
+    UNUSED(displayPort);
+    metadata->charCount = 512;
+    metadata->version = FONT_VERSION;
+    return true;
+}
+
+static textAttributes_t supportedTextAttributes(const displayPort_t *displayPort)
+{
+    UNUSED(displayPort);
+    return TEXT_ATTRIBUTES_NONE;
+}
+
+static bool isTransferInProgress(const displayPort_t *displayPort)
+{
+    UNUSED(displayPort);
+    return false;
+}
+
+static bool isReady(displayPort_t *displayPort)
+{
+    UNUSED(displayPort);
+    return vtxActive;
+}
+
+static int grab(displayPort_t *displayPort)
+{
+    UNUSED(displayPort);
+    return 0;
+}
+
+static int heartbeat(displayPort_t *displayPort)
+{
+    UNUSED(displayPort);
+    return 0;
+}
+
+static int release(displayPort_t *displayPort)
+{
+    UNUSED(displayPort);
+    return 0;
+}
+
+static const displayPortVTable_t hdzeroOsdVTable = {
+    .grab = grab,
+    .release = release,
+    .clearScreen = clearScreen,
+    .drawScreen = drawScreen,
+    .screenSize = screenSize,
+    .writeString = writeString,
+    .writeChar = writeChar,
+    .readChar = readChar,
+    .isTransferInProgress = isTransferInProgress,
+    .heartbeat = heartbeat,
+    .resync = resync,
+    .txBytesFree = txBytesFree,
+    .supportedTextAttributes = supportedTextAttributes,
+    .getFontMetadata = getFontMetadata,
+    .isReady = isReady,
+};
+
+bool hdzeroOsdSerialInit(void)
+{
+    static volatile uint8_t txBuffer[TX_BUFFER_SIZE];
+    memset(&hdZeroMspPort, 0, sizeof(mspPort_t));
+
+    serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_HDZERO_OSD);
+    if (portConfig) {
+        serialPort_t *port = openSerialPort(portConfig->identifier, FUNCTION_HDZERO_OSD, NULL, NULL,
+                baudRates[portConfig->peripheral_baudrateIndex], MODE_RXTX, SERIAL_NOT_INVERTED);
+
+        if (port) {
+            // Use a bigger TX buffer size to accommodate the configuration menus
+            port->txBuffer = txBuffer;
+            port->txBufferSize = TX_BUFFER_SIZE;
+            port->txBufferTail = 0;
+            port->txBufferHead = 0;
+
+            resetMspPort(&hdZeroMspPort, port);
+
+            return true;
+        }
+    }
+
+    return false;
+}
+
+displayPort_t* hdzeroOsdDisplayPortInit(void)
+{
+    if (hdzeroOsdSerialInit()) {
+        hdZeroInit();
+        displayInit(&hdZeroOsdDisplayPort, &hdzeroOsdVTable);
+        return &hdZeroOsdDisplayPort;
+    }
+    return NULL;
+}
+
+/*
+ * Intercept MSP processor.
+ * VTX sends an MSP command every 125ms or so.
+ * VTX will have be marked as not ready if no commands received within VTX_TIMEOUT.
+ */
+static mspResult_e hdZeroProcessMspCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostProcessFnPtr *mspPostProcessFn)
+{
+    if (vtxSeen && !vtxActive) {
+        vtxReset = true;
+    }
+
+    vtxSeen = vtxActive = true;
+    vtxHeartbeat = millis();
+
+    // Process MSP command
+    return mspProcessCommand(cmd, reply, mspPostProcessFn);
+}
+
+void hdzeroOsdSerialProcess(mspProcessCommandFnPtr mspProcessCommandFn)
+{
+    if (hdZeroMspPort.port) {
+        mspProcessCommand = mspProcessCommandFn;
+        mspSerialProcessOnePort(&hdZeroMspPort, MSP_SKIP_NON_MSP_DATA, hdZeroProcessMspCommand);
+    }
+}
+
+#endif // USE_HDZERO_OSD
--- a/src/main/io/displayport_hdzero_osd.h
+++ b/src/main/io/displayport_hdzero_osd.h
@ -0,0 +1,33 @@
+/*
+ * 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
+
+#include "drivers/osd.h"
+#include "msp/msp_serial.h"
+
+typedef struct displayPort_s displayPort_t;
+
+displayPort_t *hdzeroOsdDisplayPortInit(void);
+void hdzeroOsdSerialProcess(mspProcessCommandFnPtr mspProcessCommandFn);
--- a/src/main/io/gps_naza.c
+++ b/src/main/io/gps_naza.c
@ -189,17 +189,12 @@ static bool NAZA_parse_gps(void)


        uint16_t pdop = decodeShort(_buffernaza.nav.pdop, mask); // pdop
-        //uint16_t vdop = decodeShort(_buffernaza.nav.vdop, mask); // vdop
-        //uint16_t ndop = decodeShort(_buffernaza.nav.ndop, mask);
-        //uint16_t edop = decodeShort(_buffernaza.nav.edop, mask);
-        //gpsSol.hdop = sqrtf(powf(ndop,2)+powf(edop,2));
-        //gpsSol.vdop = decodeShort(_buffernaza.nav.vdop, mask); // vdop

        gpsSol.hdop = gpsConstrainEPE(pdop);        // PDOP
        gpsSol.eph = gpsConstrainEPE(h_acc / 10);   // hAcc in cm
        gpsSol.epv = gpsConstrainEPE(v_acc / 10);   // vAcc in cm
        gpsSol.numSat = _buffernaza.nav.satellites;
-        gpsSol.groundSpeed = fast_fsqrtf(powf(gpsSol.velNED[X], 2) + powf(gpsSol.velNED[Y], 2)); //cm/s
+        gpsSol.groundSpeed = fast_fsqrtf(sq(gpsSol.velNED[X]) + sq(gpsSol.velNED[Y])); //cm/s

        // calculate gps heading from VELNE
        gpsSol.groundCourse = (uint16_t)(fmodf(RADIANS_TO_DECIDEGREES(atan2_approx(gpsSol.velNED[Y], gpsSol.velNED[X])) + 3600.0f, 3600.0f));
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@ -196,8 +196,8 @@ 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, 5);
-PG_REGISTER_WITH_RESET_FN(osdLayoutsConfig_t, osdLayoutsConfig, PG_OSD_LAYOUTS_CONFIG, 0);
+PG_REGISTER_WITH_RESET_TEMPLATE(osdConfig_t, osdConfig, PG_OSD_CONFIG, 6);
+PG_REGISTER_WITH_RESET_FN(osdLayoutsConfig_t, osdLayoutsConfig, PG_OSD_LAYOUTS_CONFIG, 1);

 static int digitCount(int32_t value)
 {
@ -965,7 +965,7 @@ static void osdFormatMessage(char *buff, size_t size, const char *message, bool
        strncpy(buff + rem / 2, message, MIN((int)size - rem / 2, (int)messageLength));
    }
    // Ensure buff is zero terminated
-    buff[size - 1] = '\0';
+    buff[size] = '\0';
 }

 /**
@ -2015,7 +2015,7 @@ static bool osdDrawSingleElement(uint8_t item)
    case OSD_CRSF_RSSI_DBM:
        {
            int16_t rssi = rxLinkStatistics.uplinkRSSI;
-            buff[0] = (rxLinkStatistics.activeAnt == 0) ? SYM_RSSI : SYM_2RSS; // Separate symbols for each antenna
+            buff[0] = (rxLinkStatistics.activeAntenna == 0) ? SYM_RSSI : SYM_2RSS; // Separate symbols for each antenna
            if (rssi <= -100) {
                tfp_sprintf(buff + 1, "%4d%c", rssi, SYM_DBM);
            } else {
@ -3142,7 +3142,7 @@ void osdDrawNextElement(void)
 		if (osdConfig()->telemetry>0){
 		  osdDisplayTelemetry();
 		}
-}
+    }

 PG_RESET_TEMPLATE(osdConfig_t, osdConfig,
    .rssi_alarm = SETTING_OSD_RSSI_ALARM_DEFAULT,
@ -3208,7 +3208,7 @@ PG_RESET_TEMPLATE(osdConfig_t, osdConfig,
    .pan_servo_index = SETTING_OSD_PAN_SERVO_INDEX_DEFAULT,
    .pan_servo_pwm2centideg = SETTING_OSD_PAN_SERVO_PWM2CENTIDEG_DEFAULT,
    .esc_rpm_precision = SETTING_OSD_ESC_RPM_PRECISION_DEFAULT,
-
+    .system_msg_display_time = SETTING_OSD_SYSTEM_MSG_DISPLAY_TIME_DEFAULT,
    .units = SETTING_OSD_UNITS_DEFAULT,
    .main_voltage_decimals = SETTING_OSD_MAIN_VOLTAGE_DECIMALS_DEFAULT,

@ -3541,7 +3541,7 @@ void osdInit(displayPort_t *osdDisplayPortToUse)
    if (!osdDisplayPortToUse)
        return;

-    BUILD_BUG_ON(OSD_POS_MAX != OSD_POS(31,31));
+    BUILD_BUG_ON(OSD_POS_MAX != OSD_POS(63,63));

    osdDisplayPort = osdDisplayPortToUse;

@ -4102,7 +4102,7 @@ void osdUpdate(timeUs_t currentTimeUs)
        osdUpdateStats();
    }

-    if ((counter & DRAW_FREQ_DENOM) == 0) {
+    if ((counter % DRAW_FREQ_DENOM) == 0) {
        // redraw values in buffer
        osdRefresh(currentTimeUs);
    } else {
@ -4165,6 +4165,18 @@ displayCanvas_t *osdGetDisplayPortCanvas(void)
    return NULL;
 }

+timeMs_t systemMessageCycleTime(unsigned messageCount, const char **messages){
+    uint8_t i = 0;
+    float factor = 1.0f;
+    while (i < messageCount) {
+        if ((float)strlen(messages[i]) / 15.0f > factor) {
+            factor = (float)strlen(messages[i]) / 15.0f;
+        }
+        i++;
+    }
+    return osdConfig()->system_msg_display_time * factor;
+}
+
 textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenteredText)
 {
    textAttributes_t elemAttr = TEXT_ATTRIBUTES_NONE;
@ -4193,13 +4205,17 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
                    tfp_sprintf(messageBuf, "TO WP %u/%u (%s)", getGeoWaypointNumber(posControl.activeWaypointIndex), posControl.geoWaypointCount, buf);
                    messages[messageCount++] = messageBuf;
                } else if (NAV_Status.state == MW_NAV_STATE_HOLD_TIMED) {
+                    if (navConfig()->general.flags.waypoint_enforce_altitude && !posControl.wpAltitudeReached) {
+                        messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_ADJUSTING_WP_ALT);
+                    } else {
                        // WP hold time countdown in seconds
                        timeMs_t currentTime = millis();
-                    int holdTimeRemaining = posControl.waypointList[posControl.activeWaypointIndex].p1 - (int)((currentTime - posControl.wpReachedTime)/1000);
-                    if (holdTimeRemaining >=0) {
+                        int holdTimeRemaining = posControl.waypointList[posControl.activeWaypointIndex].p1 - (int)(MS2S(currentTime - posControl.wpReachedTime));
+                        holdTimeRemaining = holdTimeRemaining >= 0 ? holdTimeRemaining : 0;
                        tfp_sprintf(messageBuf, "HOLDING WP FOR %2u S", holdTimeRemaining);
-                    }
+
                        messages[messageCount++] = messageBuf;
+                    }
                } else {
                    const char *navStateMessage = navigationStateMessage();
                    if (navStateMessage) {
@ -4292,7 +4308,7 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
        }

        if (messageCount > 0) {
-            message = messages[OSD_ALTERNATING_CHOICES(1000, messageCount)];
+            message = messages[OSD_ALTERNATING_CHOICES(systemMessageCycleTime(messageCount, messages), messageCount)];
            if (message == failsafeInfoMessage) {
                // failsafeInfoMessage is not useful for recovering
                // a lost model, but might help avoiding a crash.
--- a/src/main/io/osd.h
+++ b/src/main/io/osd.h
@ -29,12 +29,21 @@
 #endif
 #define OSD_LAYOUT_COUNT (OSD_ALTERNATE_LAYOUT_COUNT + 1)

-#define OSD_VISIBLE_FLAG    0x0800
+// 00vb yyyy yyxx xxxx
+// (visible)(blink)(yCoord)(xCoord)
+
+#define OSD_VISIBLE_FLAG    0x2000
 #define OSD_VISIBLE(x)      ((x) & OSD_VISIBLE_FLAG)
-#define OSD_POS(x,y)        ((x) | ((y) << 5))
-#define OSD_X(x)            ((x) & 0x001F)
-#define OSD_Y(x)            (((x) >> 5) & 0x001F)
-#define OSD_POS_MAX         0x3FF
+
+#define OSD_POS(x,y)        (((x) & 0x3F) | (((y) & 0x3F) << 6))
+#define OSD_X(x)            ((x) & 0x3F)
+#define OSD_Y(x)            (((x) >> 6) & 0x3F)
+#define OSD_POS_MAX         0xFFF
+
+// For DJI compatibility
+#define OSD_VISIBLE_FLAG_SD 0x0800
+#define OSD_POS_SD(x,y)     (((x) & 0x1F) | (((y) & 0x1F) << 5))
+
 #define OSD_POS_MAX_CLI     (OSD_POS_MAX | OSD_VISIBLE_FLAG)

 #define OSD_HOMING_LIM_H1 6
@ -85,6 +94,7 @@
 #define OSD_MSG_HEADING_HOME        "EN ROUTE TO HOME"
 #define OSD_MSG_WP_FINISHED         "WP END>HOLDING POSITION"
 #define OSD_MSG_PREPARE_NEXT_WP     "PREPARING FOR NEXT WAYPOINT"
+#define OSD_MSG_ADJUSTING_WP_ALT    "ADJUSTING WP ALTITUDE"
 #define OSD_MSG_MISSION_PLANNER     "(WP MISSION PLANNER)"
 #define OSD_MSG_WP_RTH_CANCEL       "CANCEL WP TO EXIT RTH"
 #define OSD_MSG_WP_MISSION_LOADED   "* MISSION LOADED *"
@ -391,6 +401,7 @@ typedef struct osdConfig_s {
    uint8_t pan_servo_index;            // Index of the pan servo used for home direction offset
    int8_t pan_servo_pwm2centideg;      // Centidegrees of servo rotation per us pwm
    uint8_t crsf_lq_format;
+    uint16_t system_msg_display_time;   // system message display time for multiple messages (ms)
    uint8_t sidebar_height;             // sidebar height in rows, 0 turns off sidebars leaving only level indicator arrows
    uint8_t telemetry; 				    // use telemetry on displayed pixel line 0
    uint8_t esc_rpm_precision;          // Number of characters used for the RPM numbers.
--- a/src/main/io/osd_dji_hd.c
+++ b/src/main/io/osd_dji_hd.c
@ -357,33 +357,34 @@ static void djiSerializeOSDConfigReply(sbuf_t *dst)
        const bool itemIsSupported = ((djiOSDItemIndexMap[i].depFeature == 0) || feature(djiOSDItemIndexMap[i].depFeature));

        if (inavOSDIdx >= 0 && itemIsSupported) {
-            // Position & visibility encoded in 16 bits. Position encoding is the same between BF/DJI and INAV
-            // However visibility is different. INAV has 3 layouts, while BF only has visibility profiles
-            // Here we use only one OSD layout mapped to first OSD BF profile
+            // Position & visibility are encoded in 16 bits, and is the same between BF/DJI.
+            // However INAV supports co-ords of 0-63 and has 3 layouts, while BF has co-ords 0-31 and visibility profiles.
+            // Re-encode for co-ords of 0-31 and map the layout to all three BF profiles.
            uint16_t itemPos = osdLayoutsConfig()->item_pos[0][inavOSDIdx];
+            uint16_t itemPosSD = OSD_POS_SD(OSD_X(itemPos), OSD_Y(itemPos));

            // Workarounds for certain OSD element positions
            // INAV calculates these dynamically, while DJI expects the config to have defined coordinates
            switch(inavOSDIdx) {
                case OSD_CROSSHAIRS:
-                    itemPos = (itemPos & (~OSD_POS_MAX)) | OSD_POS(13, 6);
+                    itemPosSD = OSD_POS_SD(15, 8);
                    break;

                case OSD_ARTIFICIAL_HORIZON:
-                    itemPos = (itemPos & (~OSD_POS_MAX)) | OSD_POS(14, 2);
+                    itemPosSD = OSD_POS_SD(9, 8);
                    break;

                case OSD_HORIZON_SIDEBARS:
-                    itemPos = (itemPos & (~OSD_POS_MAX)) | OSD_POS(14, 5);
+                    itemPosSD = OSD_POS_SD(16, 7);
                    break;
            }

            // Enforce visibility in 3 BF OSD profiles
            if (OSD_VISIBLE(itemPos)) {
-                itemPos |= 0x3000;
+            	itemPosSD |= (0x3000 | OSD_VISIBLE_FLAG_SD);
            }

-            sbufWriteU16(dst, itemPos);
+            sbufWriteU16(dst, itemPosSD);
        }
        else {
            // Hide OSD elements unsupported by INAV
@ -1051,7 +1052,7 @@ static bool djiFormatMessages(char *buff)
        // Pick one of the available messages. Each message lasts
        // a second.
        if (messageCount > 0) {
-           strcpy(buff, messages[OSD_ALTERNATING_CHOICES(DJI_ALTERNATING_DURATION_SHORT, messageCount)]);;
+           strcpy(buff, messages[OSD_ALTERNATING_CHOICES(DJI_ALTERNATING_DURATION_SHORT, messageCount)]);
           haveMessage = true;
        }
    } else if (ARMING_FLAG(ARMING_DISABLED_ALL_FLAGS)) {
--- a/src/main/io/serial.h
+++ b/src/main/io/serial.h
@ -56,6 +56,7 @@ typedef enum {
    FUNCTION_SERVO_SERIAL               = (1 << 22), // 4194304
    FUNCTION_TELEMETRY_SMARTPORT_MASTER = (1 << 23), // 8388608
    FUNCTION_IMU2                       = (1 << 24), // 16777216
+	FUNCTION_HDZERO_OSD                 = (1 << 25), // 33554432
 } serialPortFunction_e;

 typedef enum {
--- a/src/main/navigation/navigation.c
+++ b/src/main/navigation/navigation.c
@ -60,11 +60,12 @@
 #include "sensors/acceleration.h"
 #include "sensors/boardalignment.h"

-
+#define WP_ALTITUDE_MARGIN_CM   100      // WP enforce altitude tolerance, used when WP altitude setting enforced when WP reached
 // Multirotors:
 #define MR_RTH_CLIMB_OVERSHOOT_CM   100  // target this amount of cm *above* the target altitude to ensure it is actually reached (Vz > 0 at target alt)
 #define MR_RTH_CLIMB_MARGIN_MIN_CM  100  // start cruising home this amount of cm *before* reaching the cruise altitude (while continuing the ascend)
 #define MR_RTH_CLIMB_MARGIN_PERCENT 15   // on high RTH altitudes use even bigger margin - percent of the altitude set
+
 // Planes:
 #define FW_RTH_CLIMB_OVERSHOOT_CM   100
 #define FW_RTH_CLIMB_MARGIN_MIN_CM  100
@ -97,7 +98,7 @@ STATIC_ASSERT(NAV_MAX_WAYPOINTS < 254, NAV_MAX_WAYPOINTS_exceeded_allowable_rang
 PG_REGISTER_ARRAY(navWaypoint_t, NAV_MAX_WAYPOINTS, nonVolatileWaypointList, PG_WAYPOINT_MISSION_STORAGE, 1);
 #endif

-PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 14);
+PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 15);

 PG_RESET_TEMPLATE(navConfig_t, navConfig,
    .general = {
@ -119,6 +120,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
            .mission_planner_reset = SETTING_NAV_MISSION_PLANNER_RESET_DEFAULT,       // Allow mode switch toggle to reset Mission Planner WPs
            .waypoint_mission_restart = SETTING_NAV_WP_MISSION_RESTART_DEFAULT,       // WP mission restart action
            .soaring_motor_stop = SETTING_NAV_FW_SOARING_MOTOR_STOP_DEFAULT,          // stops motor when Saoring mode enabled
+            .waypoint_enforce_altitude = SETTING_NAV_WP_ENFORCE_ALTITUDE_DEFAULT,     // Forces set wp altitude to be achieved
        },

        // General navigation parameters
@ -243,6 +245,7 @@ void calculateInitialHoldPosition(fpVector3_t * pos);
 void calculateFarAwayTarget(fpVector3_t * farAwayPos, int32_t yaw, int32_t distance);
 void calculateNewCruiseTarget(fpVector3_t * origin, int32_t yaw, int32_t distance);
 static bool isWaypointPositionReached(const fpVector3_t * pos, const bool isWaypointHome);
+bool isWaypointAltitudeReached(void);
 static void mapWaypointToLocalPosition(fpVector3_t * localPos, const navWaypoint_t * waypoint, geoAltitudeConversionMode_e altConv);
 static navigationFSMEvent_t nextForNonGeoStates(void);
 static bool isWaypointMissionValid(void);
@ -817,7 +820,6 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
            [NAV_FSM_EVENT_ERROR]                          = NAV_STATE_IDLE,
            [NAV_FSM_EVENT_SWITCH_TO_IDLE]                 = NAV_STATE_IDLE,
            [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD]              = NAV_STATE_ALTHOLD_INITIALIZE,
-            [NAV_FSM_EVENT_SWITCH_TO_RTH]                  = NAV_STATE_RTH_INITIALIZE,
            [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT]             = NAV_STATE_WAYPOINT_INITIALIZE,
        }
    },
@ -835,7 +837,6 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
            [NAV_FSM_EVENT_SUCCESS]                        = NAV_STATE_EMERGENCY_LANDING_FINISHED,
            [NAV_FSM_EVENT_SWITCH_TO_IDLE]                 = NAV_STATE_IDLE,
            [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD]              = NAV_STATE_ALTHOLD_INITIALIZE,
-            [NAV_FSM_EVENT_SWITCH_TO_RTH]                  = NAV_STATE_RTH_INITIALIZE,
            [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT]             = NAV_STATE_WAYPOINT_INITIALIZE,
        }
    },
@ -1482,6 +1483,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION(nav
            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

                // We use p3 as the volatile jump counter (p2 is the static value)
@ -1583,9 +1585,17 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_REACHED(naviga
 {
    UNUSED(previousState);

+    if (navConfig()->general.flags.waypoint_enforce_altitude) {
+        posControl.wpAltitudeReached = isWaypointAltitudeReached();
+    }
+
    switch ((navWaypointActions_e)posControl.waypointList[posControl.activeWaypointIndex].action) {
        case NAV_WP_ACTION_WAYPOINT:
+            if (navConfig()->general.flags.waypoint_enforce_altitude && !posControl.wpAltitudeReached) {
+                return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_HOLD_TIME;
+            } else {
                return NAV_FSM_EVENT_SUCCESS;   // NAV_STATE_WAYPOINT_NEXT
+            }

        case NAV_WP_ACTION_JUMP:
        case NAV_WP_ACTION_SET_HEAD:
@ -1614,6 +1624,24 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_HOLD_TIME(navi
        return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
    }

+    if (navConfig()->general.flags.waypoint_enforce_altitude && !posControl.wpAltitudeReached) {
+        // Adjust altitude to waypoint setting
+        if (STATE(AIRPLANE)) {
+            int8_t altitudeChangeDirection = posControl.activeWaypoint.pos.z > navGetCurrentActualPositionAndVelocity()->pos.z ? 1 : -1;
+            updateClimbRateToAltitudeController(altitudeChangeDirection * navConfig()->general.max_auto_climb_rate, ROC_TO_ALT_NORMAL);
+        } else {
+            setDesiredPosition(&posControl.activeWaypoint.pos, 0, NAV_POS_UPDATE_Z);
+        }
+
+        posControl.wpAltitudeReached = isWaypointAltitudeReached();
+
+        if (posControl.wpAltitudeReached) {
+            posControl.wpReachedTime = millis();
+        } else {
+            return NAV_FSM_EVENT_NONE;
+        }
+    }
+
    timeMs_t currentTime = millis();

    if (posControl.waypointList[posControl.activeWaypointIndex].p1 <= 0 ||
@ -1621,7 +1649,6 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_HOLD_TIME(navi
        return NAV_FSM_EVENT_SUCCESS;
    }

-
    return NAV_FSM_EVENT_NONE;      // will re-process state in >10ms
 }

@ -2132,6 +2159,11 @@ bool isWaypointReached(const navWaypointPosition_t * waypoint, const bool isWayp
    return isWaypointPositionReached(&waypoint->pos, isWaypointHome);
 }

+bool isWaypointAltitudeReached(void)
+{
+    return ABS(navGetCurrentActualPositionAndVelocity()->pos.z - posControl.activeWaypoint.pos.z) < WP_ALTITUDE_MARGIN_CM;
+}
+
 static void updateHomePositionCompatibility(void)
 {
    geoConvertLocalToGeodetic(&GPS_home, &posControl.gpsOrigin, &posControl.rthState.homePosition.pos);
@ -2596,7 +2628,7 @@ void updateClimbRateToAltitudeController(float desiredClimbRate, climbRateToAlti
        lastUpdateTimeUs = currentTimeUs;
        posControl.desiredState.pos.z = altitudeToUse;
    }
-    else {
+    else {      // ROC_TO_ALT_NORMAL

        /*
         * If max altitude is set, reset climb rate if altitude is reached and climb rate is > 0
--- a/src/main/navigation/navigation.h
+++ b/src/main/navigation/navigation.h
@ -222,6 +222,7 @@ typedef struct navConfig_s {
            uint8_t soaring_motor_stop;         // stop motor when Soaring mode enabled
            uint8_t mission_planner_reset;      // Allow WP Mission Planner reset using mode toggle (resets WPs to 0)
            uint8_t waypoint_mission_restart;   // Waypoint mission restart action
+            uint8_t waypoint_enforce_altitude;  // Forces waypoint altitude to be achieved
        } flags;

        uint8_t  pos_failure_timeout;               // Time to wait before switching to emergency landing (0 - disable)
--- a/src/main/navigation/navigation_multicopter.c
+++ b/src/main/navigation/navigation_multicopter.c
@ -493,7 +493,7 @@ static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxA

    const float setpointX = posControl.desiredState.vel.x;
    const float setpointY = posControl.desiredState.vel.y;
-    const float setpointXY = fast_fsqrtf(powf(setpointX, 2)+powf(setpointY, 2));
+    const float setpointXY = fast_fsqrtf(sq(setpointX) + sq(setpointY));

    // Calculate velocity error
    const float velErrorX = setpointX - measurementX;
@ -696,20 +696,20 @@ bool isMulticopterLandingDetected(void)
    }

    // Average climb rate should be low enough
-    bool verticalMovement = fabsf(navGetCurrentActualPositionAndVelocity()->vel.z) > 25.0f;
+    bool verticalMovement = fabsf(navGetCurrentActualPositionAndVelocity()->vel.z) > MC_LAND_CHECK_VEL_Z_MOVING;

    // check if we are moving horizontally
-    bool horizontalMovement = posControl.actualState.velXY > 100.0f;
+    bool horizontalMovement = posControl.actualState.velXY > MC_LAND_CHECK_VEL_XY_MOVING;

    // We have likely landed if throttle is 40 units below average descend throttle
    // We use rcCommandAdjustedThrottle to keep track of NAV corrected throttle (isLandingDetected is executed
    // from processRx() and rcCommand at that moment holds rc input, not adjusted values from NAV core)
    // Wait for 1 second so throttle has stabilized.
    bool isAtMinimalThrust = false;
-    if (currentTimeUs - landingDetectorStartedAt > 1000 * 1000) {
+    if (currentTimeUs - landingDetectorStartedAt > HZ2US(MC_LAND_THR_SUM_RATE)) {
        landingThrSamples += 1;
        landingThrSum += rcCommandAdjustedThrottle;
-        isAtMinimalThrust = rcCommandAdjustedThrottle < (landingThrSum / landingThrSamples - 40);
+        isAtMinimalThrust = rcCommandAdjustedThrottle < (landingThrSum / landingThrSamples - MC_LAND_DESCEND_THROTTLE);
    }

    bool possibleLandingDetected = isAtMinimalThrust && !verticalMovement && !horizontalMovement;
@ -719,7 +719,7 @@ bool isMulticopterLandingDetected(void)
        // TODO: Come up with a clever way to let sonar increase detection performance, not just add extra safety.
        // TODO: Out of range sonar may give reading that looks like we landed, find a way to check if sonar is healthy.
        // surfaceMin is our ground reference. If we are less than 5cm above the ground - we are likely landed
-        possibleLandingDetected = possibleLandingDetected && (posControl.actualState.agl.pos.z <= (posControl.actualState.surfaceMin + 5.0f));
+        possibleLandingDetected = possibleLandingDetected && (posControl.actualState.agl.pos.z <= (posControl.actualState.surfaceMin + MC_LAND_SAFE_SURFACE));
    }

    if (!possibleLandingDetected) {
@ -727,7 +727,7 @@ bool isMulticopterLandingDetected(void)
        return false;
    }
    else {
-        return ((currentTimeUs - landingTimer) > (navConfig()->mc.auto_disarm_delay * 1000)) ? true : false;
+        return ((currentTimeUs - landingTimer) > MS2US(navConfig()->mc.auto_disarm_delay)) ? true : false;
    }
 }

--- a/src/main/navigation/navigation_pos_estimator.c
+++ b/src/main/navigation/navigation_pos_estimator.c
@ -49,6 +49,7 @@
 #include "sensors/acceleration.h"
 #include "sensors/barometer.h"
 #include "sensors/compass.h"
+#include "sensors/gyro.h"
 #include "sensors/pitotmeter.h"
 #include "sensors/opflow.h"

@ -351,11 +352,19 @@ void updatePositionEstimator_PitotTopic(timeUs_t currentTimeUs)
 */
 static void restartGravityCalibration(void)
 {
+    if (!gyroConfig()->init_gyro_cal_enabled) {
+        return;
+    }
+
    zeroCalibrationStartS(&posEstimator.imu.gravityCalibration, CALIBRATING_GRAVITY_TIME_MS, positionEstimationConfig()->gravity_calibration_tolerance, false);
 }

 static bool gravityCalibrationComplete(void)
 { 
+    if (!gyroConfig()->init_gyro_cal_enabled) {
+        return true;
+    }
+    
    return zeroCalibrationIsCompleteS(&posEstimator.imu.gravityCalibration);
 }

@ -391,9 +400,9 @@ static void updateIMUTopic(timeUs_t currentTimeUs)
    posEstimator.imu.lastUpdateTime = currentTimeUs;

    if (!isImuReady()) {
-        posEstimator.imu.accelNEU.x = 0;
-        posEstimator.imu.accelNEU.y = 0;
-        posEstimator.imu.accelNEU.z = 0;
+        posEstimator.imu.accelNEU.x = 0.0f;
+        posEstimator.imu.accelNEU.y = 0.0f;
+        posEstimator.imu.accelNEU.z = 0.0f;

        restartGravityCalibration();
    }
@ -422,23 +431,29 @@ static void updateIMUTopic(timeUs_t currentTimeUs)
        posEstimator.imu.accelNEU.z = accelBF.z;

        /* When unarmed, assume that accelerometer should measure 1G. Use that to correct accelerometer gain */
+        if (gyroConfig()->init_gyro_cal_enabled) {
            if (!ARMING_FLAG(ARMED) && !gravityCalibrationComplete()) {
                zeroCalibrationAddValueS(&posEstimator.imu.gravityCalibration, posEstimator.imu.accelNEU.z);

                if (gravityCalibrationComplete()) {
                    zeroCalibrationGetZeroS(&posEstimator.imu.gravityCalibration, &posEstimator.imu.calibratedGravityCMSS);
+                    setGravityCalibrationAndWriteEEPROM(posEstimator.imu.calibratedGravityCMSS);
                    LOG_D(POS_ESTIMATOR, "Gravity calibration complete (%d)", (int)lrintf(posEstimator.imu.calibratedGravityCMSS));
                }
            }
+        } else {
+            posEstimator.imu.gravityCalibration.params.state = ZERO_CALIBRATION_DONE;
+            posEstimator.imu.calibratedGravityCMSS = gyroConfig()->gravity_cmss_cal;
+        }

        /* If calibration is incomplete - report zero acceleration */
        if (gravityCalibrationComplete()) {
            posEstimator.imu.accelNEU.z -= posEstimator.imu.calibratedGravityCMSS;
        }
        else {
-            posEstimator.imu.accelNEU.x = 0;
-            posEstimator.imu.accelNEU.y = 0;
-            posEstimator.imu.accelNEU.z = 0;
+            posEstimator.imu.accelNEU.x = 0.0f;
+            posEstimator.imu.accelNEU.y = 0.0f;
+            posEstimator.imu.accelNEU.z = 0.0f;
        }

        /* Update blackbox values */
--- a/src/main/navigation/navigation_private.h
+++ b/src/main/navigation/navigation_private.h
@ -35,6 +35,12 @@

 #define INAV_SURFACE_MAX_DISTANCE           40

+#define MC_LAND_CHECK_VEL_XY_MOVING 100.0f // cm/s
+#define MC_LAND_CHECK_VEL_Z_MOVING 25.0f   // cm/s
+#define MC_LAND_THR_SUM_RATE 1             // hz
+#define MC_LAND_DESCEND_THROTTLE 40        // uS
+#define MC_LAND_SAFE_SURFACE 5.0f          // cm
+
 #define MAX_POSITION_UPDATE_INTERVAL_US     HZ2US(MIN_POSITION_UPDATE_RATE_HZ)        // convenience macro
 _Static_assert(MAX_POSITION_UPDATE_INTERVAL_US <= TIMEDELTA_MAX, "deltaMicros can overflow!");

@ -377,6 +383,7 @@ typedef struct {
    float                       wpInitialDistance;          // Distance when starting flight to WP
    float                       wpDistance;                 // Distance to active WP
    timeMs_t                    wpReachedTime;              // Time the waypoint was reached
+    bool                        wpAltitudeReached;          // WP altitude achieved

    /* Internals & statistics */
    int16_t                     rcAdjustment[4];
--- a/src/main/programming/logic_condition.c
+++ b/src/main/programming/logic_condition.c
@ -304,6 +304,14 @@ static int logicConditionCompute(
            return tan_approx(DEGREES_TO_RADIANS(operandA)) * temporaryValue; 
        break;

+        case LOGIC_CONDITION_MIN:
+            return (operandA < operandB) ? operandA : operandB;
+        break;
+
+        case LOGIC_CONDITION_MAX:
+            return (operandA > operandB) ? operandA : operandB;
+        break;
+    
        case LOGIC_CONDITION_MAP_INPUT:
            return scaleRange(constrain(operandA, 0, operandB), 0, operandB, 0, 1000);
        break;
@ -423,6 +431,11 @@ static int logicConditionGetFlightOperandValue(int operand) {
        case LOGIC_CONDITION_OPERAND_FLIGHT_CELL_VOLTAGE: // V / 10
            return getBatteryAverageCellVoltage();
            break;
+
+        case LOGIC_CONDITION_OPERAND_FLIGHT_BATT_CELLS:
+            return getBatteryCellCount();
+            break;
+
        case LOGIC_CONDITION_OPERAND_FLIGHT_CURRENT: // Amp / 100
            return getAmperage();
            break;
--- a/src/main/programming/logic_condition.h
+++ b/src/main/programming/logic_condition.h
@ -72,7 +72,9 @@ typedef enum {
    LOGIC_CONDITION_MODULUS                     = 40,
    LOGIC_CONDITION_LOITER_OVERRIDE             = 41,
    LOGIC_CONDITION_SET_PROFILE                 = 42,
-    LOGIC_CONDITION_LAST                        = 43,
+    LOGIC_CONDITION_MIN                         = 43,
+    LOGIC_CONDITION_MAX                         = 44,
+    LOGIC_CONDITION_LAST                        = 45,
 } logicOperation_e;

 typedef enum logicOperandType_s {
@ -124,6 +126,7 @@ typedef enum {
    LOGIC_CONDITION_OPERAND_FLIGHT_GPS_VALID, // 0/1                        // 34
    LOGIC_CONDITION_OPERAND_FLIGHT_LOITER_RADIUS,                           // 35
    LOGIC_CONDITION_OPERAND_FLIGHT_ACTIVE_PROFILE, //int                    // 36
+    LOGIC_CONDITION_OPERAND_FLIGHT_BATT_CELLS,                              // 37

 } logicFlightOperands_e;

--- a/src/main/rx/crsf.c
+++ b/src/main/rx/crsf.c
@ -48,6 +48,7 @@ FILE_COMPILE_FOR_SPEED
 #define CRSF_DIGITAL_CHANNEL_MIN 172
 #define CRSF_DIGITAL_CHANNEL_MAX 1811
 #define CRSF_PAYLOAD_OFFSET offsetof(crsfFrameDef_t, type)
+#define CRSF_POWER_COUNT 9

 STATIC_UNIT_TESTED bool crsfFrameDone = false;
 STATIC_UNIT_TESTED crsfFrame_t crsfFrame;
@ -59,8 +60,7 @@ static timeUs_t crsfFrameStartAt = 0;
 static uint8_t telemetryBuf[CRSF_FRAME_SIZE_MAX];
 static uint8_t telemetryBufLen = 0;

-// The power levels represented by uplinkTXPower above in mW (250mW added to full TX in v4.00 firmware, 50mW added for ExpressLRS)
-const uint16_t crsfPowerStates[] = {0, 10, 25, 100, 500, 1000, 2000, 250, 50};
+const uint16_t crsfTxPowerStatesmW[CRSF_POWER_COUNT] = {0, 10, 25, 100, 500, 1000, 2000, 250, 50};

 /*
 * CRSF protocol
@ -122,7 +122,6 @@ typedef struct crsfPayloadLinkStatistics_s {
    uint8_t     downlinkRSSI;
    uint8_t     downlinkLQ;
    int8_t      downlinkSNR;
-    uint8_t     activeAnt;
 } __attribute__ ((__packed__)) crsfPayloadLinkStatistics_t;

 typedef struct crsfPayloadLinkStatistics_s crsfPayloadLinkStatistics_t;
@ -234,13 +233,14 @@ STATIC_UNIT_TESTED uint8_t crsfFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
            crsfFrame.frame.frameLength = CRSF_FRAME_LINK_STATISTICS_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC;

            const crsfPayloadLinkStatistics_t* linkStats = (crsfPayloadLinkStatistics_t*)&crsfFrame.frame.payload;
+            const uint8_t crsftxpowerindex = (linkStats->uplinkTXPower < CRSF_POWER_COUNT) ? linkStats->uplinkTXPower : 0;

            rxLinkStatistics.uplinkRSSI = -1* (linkStats->activeAntenna ? linkStats->uplinkRSSIAnt2 : linkStats->uplinkRSSIAnt1);
            rxLinkStatistics.uplinkLQ = linkStats->uplinkLQ;
            rxLinkStatistics.uplinkSNR = linkStats->uplinkSNR;
            rxLinkStatistics.rfMode = linkStats->rfMode;
-            rxLinkStatistics.uplinkTXPower = crsfPowerStates[linkStats->uplinkTXPower];
-            rxLinkStatistics.activeAnt = linkStats->activeAntenna;
+            rxLinkStatistics.uplinkTXPower = crsfTxPowerStatesmW[crsftxpowerindex];
+            rxLinkStatistics.activeAntenna = linkStats->activeAntenna;

            if (rxLinkStatistics.uplinkLQ > 0) {
                int16_t uplinkStrength;   // RSSI dBm converted to %
--- a/src/main/rx/rx.h
+++ b/src/main/rx/rx.h
@ -191,7 +191,7 @@ typedef struct rxLinkStatistics_s {
    int8_t      uplinkSNR;      // The SNR of the uplink in dB
    uint8_t     rfMode;         // A protocol specific measure of the transmission bandwidth [2 = 150Hz, 1 = 50Hz, 0 = 4Hz]
    uint16_t    uplinkTXPower;  // power in mW
-    uint8_t     activeAnt;
+    uint8_t     activeAntenna;
 } rxLinkStatistics_t;

 extern rxRuntimeConfig_t rxRuntimeConfig; //!!TODO remove this extern, only needed once for channelCount
--- a/src/main/sensors/diagnostics.c
+++ b/src/main/sensors/diagnostics.c
@ -227,7 +227,11 @@ bool isHardwareHealthy(void)
    const hardwareSensorStatus_e pitotStatus = getHwPitotmeterStatus();
    const hardwareSensorStatus_e gpsStatus = getHwGPSStatus();
    const hardwareSensorStatus_e opflowStatus = getHwOpticalFlowStatus();
+#ifdef USE_SECONDARY_IMU
    const hardwareSensorStatus_e imu2Status = getHwSecondaryImuStatus();
+#else
+    const hardwareSensorStatus_e imu2Status = HW_SENSOR_NONE;
+#endif

    // Sensor is considered failing if it's either unavailable (selected but not detected) or unhealthy (returning invalid readings)
    if (gyroStatus == HW_SENSOR_UNAVAILABLE || gyroStatus == HW_SENSOR_UNHEALTHY)
--- a/src/main/sensors/gyro.c
+++ b/src/main/sensors/gyro.c
@ -97,7 +97,7 @@ EXTENDED_FASTRAM dynamicGyroNotchState_t dynamicGyroNotchState;

 #endif

-PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 2);
+PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 3);

 PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
    .gyro_lpf = SETTING_GYRO_HARDWARE_LPF_DEFAULT,
@ -124,6 +124,9 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
    .kalman_q = SETTING_SETPOINT_KALMAN_Q_DEFAULT,
    .kalmanEnabled = SETTING_SETPOINT_KALMAN_ENABLED_DEFAULT,
 #endif
+    .init_gyro_cal_enabled = SETTING_INIT_GYRO_CAL_DEFAULT,
+    .gyro_zero_cal = {SETTING_GYRO_ZERO_X_DEFAULT, SETTING_GYRO_ZERO_Y_DEFAULT, SETTING_GYRO_ZERO_Z_DEFAULT},
+    .gravity_cmss_cal = SETTING_INS_GRAVITY_CMSS_DEFAULT,
 );

 STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHardware)
@ -328,6 +331,12 @@ void gyroStartCalibration(void)
        return;
    }

+#ifndef USE_IMU_FAKE // fixes Test Unit compilation error     
+    if (!gyroConfig()->init_gyro_cal_enabled) {
+        return;
+    }
+#endif
+
    zeroCalibrationStartV(&gyroCalibration[0], CALIBRATING_GYRO_TIME_MS, gyroConfig()->gyroMovementCalibrationThreshold, false);
 }

@ -337,6 +346,12 @@ bool gyroIsCalibrationComplete(void)
        return true;
    }
    
+#ifndef USE_IMU_FAKE // fixes Test Unit compilation error
+    if (!gyroConfig()->init_gyro_cal_enabled) {
+        return true;
+    }
+#endif
+
    return zeroCalibrationIsCompleteV(&gyroCalibration[0]) && zeroCalibrationIsSuccessfulV(&gyroCalibration[0]);
 }

@ -358,10 +373,13 @@ STATIC_UNIT_TESTED void performGyroCalibration(gyroDev_t *dev, zeroCalibrationVe
        dev->gyroZero[Y] = v.v[Y];
        dev->gyroZero[Z] = v.v[Z];

+#ifndef USE_IMU_FAKE // fixes Test Unit compilation error
+        setGyroCalibrationAndWriteEEPROM(dev->gyroZero);
+#endif
+
        LOG_D(GYRO, "Gyro calibration complete (%d, %d, %d)", dev->gyroZero[X], dev->gyroZero[Y], dev->gyroZero[Z]);
        schedulerResetTaskStatistics(TASK_SELF); // so calibration cycles do not pollute tasks statistics
-    }
-    else {
+    } else {
        dev->gyroZero[X] = 0;
        dev->gyroZero[Y] = 0;
        dev->gyroZero[Z] = 0;
@ -380,8 +398,21 @@ void gyroGetMeasuredRotationRate(fpVector3_t *measuredRotationRate)

 static bool FAST_CODE NOINLINE gyroUpdateAndCalibrate(gyroDev_t * gyroDev, zeroCalibrationVector_t * gyroCal, float * gyroADCf)
 {
+
    // range: +/- 8192; +/- 2000 deg/sec
    if (gyroDev->readFn(gyroDev)) {
+
+#ifndef USE_IMU_FAKE // fixes Test Unit compilation error
+    if (!gyroConfig()->init_gyro_cal_enabled) {
+        // marks that the gyro calibration has ended
+        gyroCalibration[0].params.state = ZERO_CALIBRATION_DONE;
+        // pass the calibration values
+        gyroDev->gyroZero[X] = gyroConfig()->gyro_zero_cal[X];
+        gyroDev->gyroZero[Y] = gyroConfig()->gyro_zero_cal[Y];
+        gyroDev->gyroZero[Z] = gyroConfig()->gyro_zero_cal[Z];
+    }
+#endif
+
        if (zeroCalibrationIsCompleteV(gyroCal)) {
            int32_t gyroADCtmp[XYZ_AXIS_COUNT];

--- a/src/main/sensors/gyro.h
+++ b/src/main/sensors/gyro.h
@ -77,6 +77,9 @@ typedef struct gyroConfig_s {
    uint16_t kalman_q;
    uint8_t kalmanEnabled;
 #endif
+    bool init_gyro_cal_enabled;
+    int16_t gyro_zero_cal[XYZ_AXIS_COUNT];
+    float gravity_cmss_cal;
 } gyroConfig_t;

 PG_DECLARE(gyroConfig_t, gyroConfig);
--- a/src/main/target/FOXEERF745AIO/CMakeLists.txt
+++ b/src/main/target/FOXEERF745AIO/CMakeLists.txt
@ -0,0 +1 @@
+target_stm32f745xg(FOXEERF745AIO)
--- a/src/main/target/FOXEERF745AIO/target.c
+++ b/src/main/target/FOXEERF745AIO/target.c
@ -0,0 +1,37 @@
+/*
+ * 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 <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdbool.h>
+#include <platform.h>
+#include "drivers/io.h"
+#include "drivers/bus.h"
+#include "drivers/timer.h"
+#include "drivers/sensor.h"
+#include "drivers/pwm_mapping.h"
+
+const timerHardware_t timerHardware[] = {
+
+    DEF_TIM(TIM1,  CH1, PA8, TIM_USE_LED, 0, 0),      // LED STRIP - D(2, 6, 0)
+
+    DEF_TIM(TIM3, CH1, PB4,  TIM_USE_MC_MOTOR, 0, 0), // M1 - D(1, 4, 5)
+    DEF_TIM(TIM3, CH2, PB5,  TIM_USE_MC_MOTOR, 0, 0), // M2 - D(1, 5, 5)
+    DEF_TIM(TIM3, CH4, PB1,  TIM_USE_MC_MOTOR, 0, 0), // M3 - D(1, 2, 5)
+    DEF_TIM(TIM3, CH3, PB0,  TIM_USE_MC_MOTOR, 0, 0), // M4 - D(1, 7, 5)
+
+};
+
+const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]);
--- a/src/main/target/FOXEERF745AIO/target.h
+++ b/src/main/target/FOXEERF745AIO/target.h
@ -0,0 +1,146 @@
+/*
+ * 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 <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#define TARGET_BOARD_IDENTIFIER "FXF5"
+#define USBD_PRODUCT_STRING     "FOXEERF745AIO"
+
+/*** Indicators ***/
+#define LED0                    PC13
+#define BEEPER                  PD2
+#define BEEPER_INVERTED
+
+/*** IMU sensors ***/
+#define USE_EXTI
+
+
+#define USE_MPU_DATA_READY_SIGNAL
+
+// MPU6000
+#define USE_IMU_MPU6000
+#define IMU_MPU6000_ALIGN       CW180_DEG
+#define MPU6000_CS_PIN          PA15
+#define MPU6000_SPI_BUS         BUS_SPI3
+#define MPU6000_EXTI_PIN        PD0
+
+
+/*** SPI/I2C bus ***/
+#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_3
+#define SPI3_SCK_PIN            PC10
+#define SPI3_MISO_PIN           PC11 
+#define SPI3_MOSI_PIN           PC12
+
+#define USE_SPI_DEVICE_4
+#define SPI4_SCK_PIN            PE2
+#define SPI4_MISO_PIN           PE5 
+#define SPI4_MOSI_PIN           PE6
+
+#define USE_I2C
+#define USE_I2C_DEVICE_1
+#define I2C1_SCL                PB8      
+#define I2C1_SDA                PB9 
+
+/*** Onboard flash ***/
+#define USE_FLASHFS
+#define USE_FLASH_M25P16
+#define M25P16_CS_PIN           PE4
+#define M25P16_SPI_BUS          BUS_SPI4
+
+/*** OSD ***/
+#define USE_MAX7456
+#define MAX7456_SPI_BUS         BUS_SPI1
+#define MAX7456_CS_PIN          PA4
+
+/*** Serial ports ***/
+#define USE_VCP
+
+#define USE_UART1
+#define UART1_TX_PIN            PA9
+#define UART1_RX_PIN            PA10
+
+#define USE_UART2
+#define UART2_TX_PIN            PA2
+#define UART2_RX_PIN            PA3
+
+#define USE_UART3
+#define UART3_TX_PIN            PB10
+#define UART3_RX_PIN            PB11
+
+#define USE_UART4
+#define UART4_TX_PIN            PA0
+#define UART4_RX_PIN            PA1
+
+#define USE_UART7   
+#define UART7_TX_PIN            PE8
+#define UART7_RX_PIN            PE7
+
+
+#define SERIAL_PORT_COUNT       6
+
+#define DEFAULT_FEATURES        (FEATURE_OSD | FEATURE_VBAT | FEATURE_CURRENT_METER)
+
+/*** BARO & MAG ***/
+#define USE_BARO
+#define BARO_I2C_BUS            BUS_I2C1
+#define USE_BARO_BMP280
+#define USE_BARO_MS5611
+
+#define USE_MAG
+#define MAG_I2C_BUS             BUS_I2C1
+#define USE_MAG_HMC5883
+#define USE_MAG_QMC5883
+
+/*** ADC ***/
+#define USE_ADC
+#define ADC_CHANNEL_1_PIN               PC3
+#define ADC_CHANNEL_2_PIN               PC5
+#define ADC_CHANNEL_3_PIN               PC2
+
+#define VBAT_ADC_CHANNEL                ADC_CHN_1
+#define RSSI_ADC_CHANNEL                ADC_CHN_2
+#define CURRENT_METER_ADC_CHANNEL       ADC_CHN_3
+
+/*** LED STRIP ***/
+#define USE_LED_STRIP
+#define WS2811_PIN                      PA8
+ 
+/*** Default settings ***/
+#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT
+#define SERIALRX_UART                   SERIAL_PORT_USART1
+#define DEFAULT_RX_TYPE                 RX_TYPE_SERIAL
+#define SERIALRX_PROVIDER               SERIALRX_SBUS
+#define CURRENT_METER_SCALE             100
+
+/*** Timer/PWM output ***/
+#define USE_SERIAL_4WAY_BLHELI_INTERFACE
+#define MAX_PWM_OUTPUT_PORTS            4
+#define USE_DSHOT
+#define USE_ESC_SENSOR
+
+/*** Used pins ***/
+#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
--- a/src/main/target/MAMBAF722_X8/CMakeLists.txt
+++ b/src/main/target/MAMBAF722_X8/CMakeLists.txt
@ -0,0 +1 @@
+target_stm32f722xe(MAMBAF722_X8)
--- a/src/main/target/MAMBAF722_X8/config.c
+++ b/src/main/target/MAMBAF722_X8/config.c
@ -0,0 +1,61 @@
+/*
+ * 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 <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#include <platform.h>
+
+#include "common/axis.h"
+
+#include "config/config_master.h"
+#include "config/feature.h"
+
+#include "drivers/sensor.h"
+#include "drivers/pwm_esc_detect.h"
+#include "drivers/pwm_output.h"
+#include "drivers/serial.h"
+
+#include "fc/rc_controls.h"
+
+#include "flight/failsafe.h"
+#include "flight/mixer.h"
+#include "flight/pid.h"
+
+#include "rx/rx.h"
+
+#include "io/serial.h"
+
+#include "sensors/battery.h"
+#include "sensors/sensors.h"
+
+#include "telemetry/telemetry.h"
+
+void targetConfiguration(void)
+{
+
+    /*
+     * UART1 is SerialRX
+     */
+    serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART1)].functionMask = FUNCTION_RX_SERIAL;
+    
+    /*
+     * Enable MSP at 115200 at UART4
+     */
+    serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART4)].functionMask = FUNCTION_MSP;
+    serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART4)].msp_baudrateIndex = BAUD_115200;
+}
--- a/src/main/target/MAMBAF722_X8/target.c
+++ b/src/main/target/MAMBAF722_X8/target.c
@ -0,0 +1,43 @@
+/*
+ * This file is part of INAV.
+ *
+ * INAV is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * INAV is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with INAV.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+
+#include <stdint.h>
+
+#include "platform.h"
+#include "drivers/io.h"
+#include "drivers/timer.h"
+#include "drivers/pwm_mapping.h"
+#include "drivers/bus.h"
+
+const timerHardware_t timerHardware[] = {
+    DEF_TIM(TIM11,  CH1,  PB9,   TIM_USE_PPM,       0, 0 ),     // PPM IN
+
+    DEF_TIM(TIM8,   CH3,  PC8,   TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR,  0, 0 ),     // S1
+    DEF_TIM(TIM8,   CH4,  PC9,   TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR,  0, 0 ),     // S2
+    DEF_TIM(TIM1,   CH1,  PA8,   TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO,  0, 1 ),     // S3
+    DEF_TIM(TIM1,   CH2,  PA9,   TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO,  0, 1 ),     // S4
+
+    DEF_TIM(TIM3,   CH3,  PB0,   TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO,  0, 0 ),     // S5
+    DEF_TIM(TIM3,   CH4,  PB1,   TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO,  0, 0 ),     // S6
+    DEF_TIM(TIM1,   CH3,  PA10,  TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO,  0, 0 ),     // S7
+    DEF_TIM(TIM3,   CH1,  PB4,   TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO,  0, 0 ),     // S8
+
+    DEF_TIM(TIM2,   CH2,  PB3,   TIM_USE_LED,       0, 0 ),     // LED_STRIP – D(1, 6, 3)
+};
+
+const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]);
--- a/src/main/target/MAMBAF722_X8/target.h
+++ b/src/main/target/MAMBAF722_X8/target.h
@ -0,0 +1,172 @@
+/*
+ * 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 <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#define USE_TARGET_CONFIG
+
+#define TARGET_BOARD_IDENTIFIER         "M7X8"
+#define USBD_PRODUCT_STRING             "MAMBAF722_X8"
+
+// ******** Board LEDs  **********************
+#define LED0                            PC15
+#define LED1                            PC14
+
+// ******* Beeper ***********
+#define BEEPER                          PB2
+#define BEEPER_INVERTED
+
+// ******* GYRO and ACC ********
+#define USE_EXTI
+#define GYRO_INT_EXTI                   PC4
+#define USE_MPU_DATA_READY_SIGNAL
+
+#define MPU6000_CS_PIN                  SPI1_NSS_PIN
+#define MPU6000_SPI_BUS                 BUS_SPI1
+
+#define USE_IMU_MPU6000
+#define IMU_MPU6000_ALIGN               CW180_DEG
+
+#define USE_I2C
+
+#define USE_I2C_DEVICE_1
+#define I2C1_SCL                        PB8
+#define I2C1_SDA                        PB9
+#define DEFAULT_I2C_BUS                 BUS_I2C1
+
+
+// *************** Baro **************************
+#define USE_BARO
+#define BARO_I2C_BUS                    DEFAULT_I2C_BUS
+#define USE_BARO_BMP280
+#define USE_BARO_MS5611
+
+//*********** Magnetometer / Compass *************
+#define USE_MAG
+#define MAG_I2C_BUS                     DEFAULT_I2C_BUS
+
+#define USE_MAG_HMC5883
+#define USE_MAG_QMC5883
+#define USE_MAG_IST8310
+#define USE_MAG_MAG3110
+#define USE_MAG_LIS3MDL
+
+// ******* SERIAL ********
+#define USE_VCP
+#define USE_UART1
+#define USE_UART2
+#define USE_UART3
+#define USE_UART4
+#define USE_UART5
+#define USE_UART6
+
+#define UART1_TX_PIN                    PB6
+#define UART1_RX_PIN                    PB7
+
+#define UART2_TX_PIN                    PA2
+#define UART2_RX_PIN                    PA3
+
+#define UART3_TX_PIN                    PB10
+#define UART3_RX_PIN                    PB11
+
+#define UART4_TX_PIN                    PA0
+#define UART4_RX_PIN                    PA1
+
+#define UART5_TX_PIN                    PC12
+#define UART5_RX_PIN                    PD2
+
+#define UART6_TX_PIN                    PC6
+#define UART6_RX_PIN                    PC7
+
+#define SERIAL_PORT_COUNT               7
+
+// ******* SPI ********
+#define USE_SPI
+
+#define USE_SPI_DEVICE_1
+#define SPI1_NSS_PIN                    PA4
+#define SPI1_SCK_PIN                    PA5
+#define SPI1_MISO_PIN                   PA6
+#define SPI1_MOSI_PIN                   PA7
+
+#define USE_SPI_DEVICE_2
+#define SPI2_NSS_PIN                    PB12
+#define SPI2_SCK_PIN                    PB13
+#define SPI2_MISO_PIN                   PB14
+#define SPI2_MOSI_PIN                   PB15
+
+#define USE_SPI_DEVICE_3
+#define SPI3_NSS_PIN                    PA15
+#define SPI3_SCK_PIN                    PC10
+#define SPI3_MISO_PIN                   PC11
+#define SPI3_MOSI_PIN                   PB5
+
+// ******* ADC ********
+#define USE_ADC
+#define ADC_CHANNEL_1_PIN               PC1
+#define ADC_CHANNEL_2_PIN               PC2
+#define ADC_CHANNEL_3_PIN               PC3
+
+#define VBAT_ADC_CHANNEL                ADC_CHN_1
+#define RSSI_ADC_CHANNEL                ADC_CHN_2
+#define CURRENT_METER_ADC_CHANNEL       ADC_CHN_3
+
+#define VBAT_SCALE_DEFAULT              1100
+
+// ******* OSD ********
+#define USE_MAX7456
+#define MAX7456_SPI_BUS                 BUS_SPI2
+#define MAX7456_CS_PIN                  SPI2_NSS_PIN
+
+//******* FLASH ********
+#define USE_FLASHFS
+#define USE_FLASH_M25P16
+#define M25P16_CS_PIN                   SPI3_NSS_PIN
+#define M25P16_SPI_BUS                  BUS_SPI3
+
+#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT
+
+//************ LEDSTRIP *****************
+#define USE_LED_STRIP
+#define WS2811_PIN                      PB3
+
+// ******* FEATURES ********
+#define DEFAULT_RX_FEATURE              FEATURE_RX_SERIAL
+#define SERIALRX_UART                   SERIAL_PORT_USART1
+#define SERIALRX_PROVIDER               SERIALRX_SBUS
+
+#define DEFAULT_FEATURES                (FEATURE_OSD | FEATURE_TELEMETRY)
+
+#define TARGET_IO_PORTA                 0xffff
+#define TARGET_IO_PORTB                 0xffff
+#define TARGET_IO_PORTC                 0xffff
+#define TARGET_IO_PORTD                 (BIT(2))
+
+#define MAX_PWM_OUTPUT_PORTS            8
+#define TARGET_MOTOR_COUNT              8        
+
+// ESC-related features
+#define USE_DSHOT
+#define USE_ESC_SENSOR
+#define USE_SERIAL_4WAY_BLHELI_INTERFACE
+
+#define USE_RANGEFINDER
+#define RANGEFINDER_I2C_BUS         DEFAULT_I2C_BUS
+#define TEMPERATURE_I2C_BUS         DEFAULT_I2C_BUS
+#define PITOT_I2C_BUS               DEFAULT_I2C_BUS
+
+#define BNO055_I2C_BUS              DEFAULT_I2C_BUS
--- a/src/main/target/MATEKH743/target.c
+++ b/src/main/target/MATEKH743/target.c
@ -47,7 +47,7 @@ const timerHardware_t timerHardware[] = {
    DEF_TIM(TIM15, CH1, PE5, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0),   // S11
    DEF_TIM(TIM15, CH2, PE6, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0),   // S12 DMA_NONE

-    DEF_TIM(TIM1,  CH1, PA8,  TIM_USE_LED, 0, 8),    // LED_2812
+    DEF_TIM(TIM1,  CH1, PA8,  TIM_USE_LED, 0, 9),    // LED_2812
    DEF_TIM(TIM2,  CH1, PA15, TIM_USE_BEEPER, 0, 0),  // BEEPER PWM

    DEF_TIM(TIM8,  CH2, PC7,  TIM_USE_PPM, 0, 0),   // RX6 PPM
--- a/src/main/target/SPEEDYBEEF7V2/target.h
+++ b/src/main/target/SPEEDYBEEF7V2/target.h
@ -137,6 +137,17 @@
 #define CURRENT_METER_ADC_CHANNEL   ADC_CHN_2
 #define RSSI_ADC_CHANNEL            ADC_CHN_3

+// *************** LED *****************************
+#define USE_LED_STRIP
+#define WS2811_PIN PB0
+
+// ********** Optiical Flow adn Lidar **************
+#define USE_RANGEFINDER
+#define USE_RANGEFINDER_MSP
+#define USE_OPFLOW
+#define USE_OPFLOW_MSP
+
+

 #define DEFAULT_FEATURES            (FEATURE_TX_PROF_SEL | FEATURE_CURRENT_METER | FEATURE_TELEMETRY | FEATURE_VBAT | FEATURE_OSD | FEATURE_BLACKBOX)
 #define USE_DSHOT
--- a/src/main/target/common.h
+++ b/src/main/target/common.h
@ -129,6 +129,7 @@
 #define USE_OSD
 #define USE_FRSKYOSD
 #define USE_DJI_HD_OSD
+#define USE_HDZERO_OSD
 #define USE_SMARTPORT_MASTER

 #define NAV_NON_VOLATILE_WAYPOINT_CLI
--- a/src/test/unit/sensor_gyro_unittest.cc
+++ b/src/test/unit/sensor_gyro_unittest.cc
@ -137,7 +137,6 @@ TEST(SensorGyro, Update)

 extern "C" {
 static timeMs_t milliTime = 0;
-
 timeMs_t millis(void) {return milliTime++;}
 uint32_t micros(void) {return 0;}
 void beeper(beeperMode_e) {}