Merge remote-tracking branch 'origin' into dzikuvx-pcf8574-expander

docs/Global Functions.md

@@ -26,8 +26,9 @@ _Global Functions_ (abbr. GF) are a mechanism allowing to override certain fligh
 | 5             | INVERT_PITCH                  | Inverts PITCH axis input for PID/PIFF controller  |
 | 6             | INVERT_YAW                    | Inverts YAW axis input for PID/PIFF controller |
 | 7             | OVERRIDE_THROTTLE             | Override throttle value that is fed to the motors by mixer. Operand is scaled in us. `1000` means throttle cut, `1500` means half throttle |
-| 8             | SET_VTX_BAND           | Sets VTX band. Accepted values are `1-5` |
+| 8             | SET_VTX_BAND                  | Sets VTX band. Accepted values are `1-5` |
-| 8             | SET_VTX_CHANNEL           | Sets VTX channel. Accepted values are `1-8` |
+| 9             | SET_VTX_CHANNEL               | Sets VTX channel. Accepted values are `1-8` |
+| 10            | SET_OSD_LAYOUT                | Sets OSD layout. Accepted values are `0-3` |
 
 ## Flags
 

make/source.mk

@@ -204,6 +204,7 @@ COMMON_SRC = \
             io/osd_common.c \
             io/osd_grid.c \
             io/osd_hud.c \
+            io/smartport_master.c \
             navigation/navigation.c \
             navigation/navigation_fixedwing.c \
             navigation/navigation_fw_launch.c \

src/main/build/debug.h

@@ -73,6 +73,9 @@ typedef enum {
     DEBUG_IRLOCK,
     DEBUG_CD,
     DEBUG_KALMAN,
+    DEBUG_SPM_CELLS,    // Smartport master FLVSS
+    DEBUG_SPM_VS600,    // Smartport master VS600 VTX
+    DEBUG_SPM_VARIO,    // Smartport master variometer
     DEBUG_PCF8574,
     DEBUG_COUNT
 } debugType_e;

src/main/common/utils.h

@@ -52,6 +52,7 @@
 #define BUILD_BUG_ON(condition) ((void)sizeof(char[1 - 2*!!(condition)]))
 
 #define BIT(x) (1 << (x))
+#define GET_BIT(value, bit) ((value >> bit) & 1)
 
 #define STATIC_ASSERT(condition, name) \
     typedef char assert_failed_ ## name [(condition) ? 1 : -1 ] __attribute__((unused))
@@ -112,3 +113,5 @@ void * memcpy_fn ( void * destination, const void * source, size_t num ) asm("me
 #define UNREACHABLE() __builtin_unreachable()
 
 #define ALIGNED(x) __attribute__ ((aligned(x)))
+
+#define PACKED __attribute__((packed))

src/main/config/parameter_group_ids.h

@@ -111,7 +111,8 @@
 #define PG_ESC_SENSOR_CONFIG 1021
 #define PG_RPM_FILTER_CONFIG 1022
 #define PG_GLOBAL_VARIABLE_CONFIG 1023
-#define PG_INAV_END 1023
+#define PG_SMARTPORT_MASTER_CONFIG 1024
+#define PG_INAV_END 1024
 
 // OSD configuration (subject to change)
 //#define PG_OSD_FONT_CONFIG 2047

src/main/fc/fc_init.c

@@ -115,6 +115,7 @@
 #include "io/rcdevice_cam.h"
 #include "io/serial.h"
 #include "io/displayport_msp.h"
+#include "io/smartport_master.h"
 #include "io/vtx.h"
 #include "io/vtx_control.h"
 #include "io/vtx_smartaudio.h"
@@ -281,6 +282,10 @@ void init(void)
     djiOsdSerialInit();
 #endif
 
+#if defined(USE_SMARTPORT_MASTER)
+    smartportMasterInit();
+#endif
+
 #if defined(USE_LOG)
     // LOG might use serial output, so we only can init it after serial port is ready
     // From this point on we can use LOG_*() to produce real-time debugging information

src/main/fc/fc_tasks.c

@@ -61,6 +61,7 @@
 #include "io/pwmdriver_i2c.h"
 #include "io/serial.h"
 #include "io/rcdevice_cam.h"
+#include "io/smartport_master.h"
 #include "io/vtx.h"
 #include "io/osd_dji_hd.h"
 #include "io/servo_sbus.h"
@@ -251,6 +252,13 @@ void taskTelemetry(timeUs_t currentTimeUs)
 }
 #endif
 
+#if defined(USE_SMARTPORT_MASTER)
+void taskSmartportMaster(timeUs_t currentTimeUs)
+{
+    smartportMasterHandle(currentTimeUs);
+}
+#endif
+
 #ifdef USE_LED_STRIP
 void taskLedStrip(timeUs_t currentTimeUs)
 {
@@ -360,6 +368,9 @@ void fcTasksInit(void)
 #ifdef USE_IRLOCK
     setTaskEnabled(TASK_IRLOCK, irlockHasBeenDetected());
 #endif
+#if defined(USE_SMARTPORT_MASTER)
+    setTaskEnabled(TASK_SMARTPORT_MASTER, true);
+#endif
 }
 
 cfTask_t cfTasks[TASK_COUNT] = {
@@ -494,6 +505,15 @@ cfTask_t cfTasks[TASK_COUNT] = {
     },
 #endif
 
+#if defined(USE_SMARTPORT_MASTER)
+    [TASK_SMARTPORT_MASTER] = {
+        .taskName = "SPORT MASTER",
+        .taskFunc = taskSmartportMaster,
+        .desiredPeriod = TASK_PERIOD_HZ(500),         // 500 Hz
+        .staticPriority = TASK_PRIORITY_IDLE,
+    },
+#endif
+
 #ifdef USE_LED_STRIP
     [TASK_LEDSTRIP] = {
         .taskName = "LEDSTRIP",

src/main/fc/settings.yaml

@@ -25,7 +25,7 @@ tables:
     values: ["NONE", "PPM", "SERIAL", "MSP", "SPI", "UIB"]
     enum: rxReceiverType_e
   - name: serial_rx
-    values: ["SPEK1024", "SPEK2048", "SBUS", "SUMD", "SUMH", "XB-B", "XB-B-RJ01", "IBUS", "JETIEXBUS", "CRSF", "FPORT", "SBUS_FAST"]
+    values: ["SPEK1024", "SPEK2048", "SBUS", "SUMD", "SUMH", "XB-B", "XB-B-RJ01", "IBUS", "JETIEXBUS", "CRSF", "FPORT", "FPORT2", "SBUS_FAST"]
   - name: rx_spi_protocol
     values: ["V202_250K", "V202_1M", "SYMA_X", "SYMA_X5C", "CX10", "CX10A", "H8_3D", "INAV", "ELERES"]
     enum: rx_spi_protocol_e
@@ -84,7 +84,7 @@ tables:
       "FLOW", "SBUS", "FPORT", "ALWAYS", "SAG_COMP_VOLTAGE",
       "VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO", "ACC", "ITERM_RELAX",
       "ERPM", "RPM_FILTER", "RPM_FREQ", "NAV_YAW", "DYNAMIC_FILTER", "DYNAMIC_FILTER_FREQUENCY",
-      "IRLOCK", "CD", "KALMAN", "PCF8574"]
+      "IRLOCK", "CD", "KALMAN", "SPM_CELLS", "SPM_VS600", "SPM_VARIO", "PCF8574"]
   - name: async_mode
     values: ["NONE", "GYRO", "ALL"]
   - name: aux_operator
@@ -3005,3 +3005,15 @@ groups:
         - name: esc_sensor_listen_only
           field: listenOnly
           type: bool
+
+  - name: PG_SMARTPORT_MASTER_CONFIG
+    type: smartportMasterConfig_t
+    headers: ["io/smartport_master.h"]
+    condition: USE_SMARTPORT_MASTER
+    members:
+      - name: smartport_master_halfduplex
+        field: halfDuplex
+        type: bool
+      - name: smartport_master_inverted
+        field: inverted
+        type: bool

src/main/io/osd.c

@@ -52,6 +52,7 @@ FILE_COMPILE_FOR_SPEED
 #include "common/time.h"
 #include "common/typeconversion.h"
 #include "common/utils.h"
+#include "programming/global_functions.h"
 
 #include "config/feature.h"
 #include "config/parameter_group.h"
@@ -3241,6 +3242,11 @@ void osdUpdate(timeUs_t currentTimeUs)
         if (IS_RC_MODE_ACTIVE(BOXOSDALT1))
             activeLayout = 1;
         else
+#ifdef USE_PROGRAMMING_FRAMEWORK
+        if (GLOBAL_FUNCTION_FLAG(GLOBAL_FUNCTION_FLAG_OVERRIDE_OSD_LAYOUT))
+            activeLayout = constrain(globalFunctionValues[GLOBAL_FUNCTION_ACTION_SET_OSD_LAYOUT], 0, OSD_ALTERNATE_LAYOUT_COUNT); 
+        else
+#endif
             activeLayout = 0;
     }
     if (currentLayout != activeLayout) {

src/main/io/serial.h

@@ -30,30 +30,31 @@ typedef enum {
 } portSharing_e;
 
 typedef enum {
-    FUNCTION_NONE                = 0,
+    FUNCTION_NONE                       = 0,
-    FUNCTION_MSP                 = (1 << 0), // 1
+    FUNCTION_MSP                        = (1 << 0), // 1
-    FUNCTION_GPS                 = (1 << 1), // 2
+    FUNCTION_GPS                        = (1 << 1), // 2
-    FUNCTION_TELEMETRY_FRSKY     = (1 << 2), // 4
+    FUNCTION_TELEMETRY_FRSKY            = (1 << 2), // 4
-    FUNCTION_TELEMETRY_HOTT      = (1 << 3), // 8
+    FUNCTION_TELEMETRY_HOTT             = (1 << 3), // 8
-    FUNCTION_TELEMETRY_LTM       = (1 << 4), // 16
+    FUNCTION_TELEMETRY_LTM              = (1 << 4), // 16
-    FUNCTION_TELEMETRY_SMARTPORT = (1 << 5), // 32
+    FUNCTION_TELEMETRY_SMARTPORT        = (1 << 5), // 32
-    FUNCTION_RX_SERIAL           = (1 << 6), // 64
+    FUNCTION_RX_SERIAL                  = (1 << 6), // 64
-    FUNCTION_BLACKBOX            = (1 << 7), // 128
+    FUNCTION_BLACKBOX                   = (1 << 7), // 128
-    FUNCTION_TELEMETRY_MAVLINK   = (1 << 8), // 256
+    FUNCTION_TELEMETRY_MAVLINK          = (1 << 8), // 256
-    FUNCTION_TELEMETRY_IBUS      = (1 << 9), // 512
+    FUNCTION_TELEMETRY_IBUS             = (1 << 9), // 512
-    FUNCTION_RCDEVICE            = (1 << 10), // 1024
+    FUNCTION_RCDEVICE                   = (1 << 10), // 1024
-    FUNCTION_VTX_SMARTAUDIO      = (1 << 11), // 2048
+    FUNCTION_VTX_SMARTAUDIO             = (1 << 11), // 2048
-    FUNCTION_VTX_TRAMP           = (1 << 12), // 4096
+    FUNCTION_VTX_TRAMP                  = (1 << 12), // 4096
-    FUNCTION_UAV_INTERCONNECT    = (1 << 13), // 8192
+    FUNCTION_UAV_INTERCONNECT           = (1 << 13), // 8192
-    FUNCTION_OPTICAL_FLOW        = (1 << 14), // 16384
+    FUNCTION_OPTICAL_FLOW               = (1 << 14), // 16384
-    FUNCTION_LOG                 = (1 << 15), // 32768
+    FUNCTION_LOG                        = (1 << 15), // 32768
-    FUNCTION_RANGEFINDER         = (1 << 16), // 65536
+    FUNCTION_RANGEFINDER                = (1 << 16), // 65536
-    FUNCTION_VTX_FFPV            = (1 << 17), // 131072
+    FUNCTION_VTX_FFPV                   = (1 << 17), // 131072
-    FUNCTION_ESCSERIAL           = (1 << 18), // 262144: this is used for both SERIALSHOT and ESC_SENSOR telemetry
+    FUNCTION_ESCSERIAL                  = (1 << 18), // 262144: this is used for both SERIALSHOT and ESC_SENSOR telemetry
-    FUNCTION_TELEMETRY_SIM       = (1 << 19), // 524288
+    FUNCTION_TELEMETRY_SIM              = (1 << 19), // 524288
-    FUNCTION_FRSKY_OSD           = (1 << 20), // 1048576
+    FUNCTION_FRSKY_OSD                  = (1 << 20), // 1048576
-    FUNCTION_DJI_HD_OSD          = (1 << 21), // 2097152
+    FUNCTION_DJI_HD_OSD                 = (1 << 21), // 2097152
-    FUNCTION_SERVO_SERIAL        = (1 << 22), // 4194304
+    FUNCTION_SERVO_SERIAL               = (1 << 22), // 4194304
+    FUNCTION_TELEMETRY_SMARTPORT_MASTER = (1 << 23), // 8388608
 } serialPortFunction_e;
 
 typedef enum {

src/main/io/smartport_master.c

@@ -0,0 +1,591 @@
+/*
+ * 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 <string.h>
+
+#include "platform.h"
+FILE_COMPILE_FOR_SPEED
+
+#if defined(USE_SMARTPORT_MASTER)
+
+#include "build/debug.h"
+
+#include "common/utils.h"
+
+#include "config/parameter_group.h"
+#include "config/parameter_group_ids.h"
+
+#include "drivers/serial.h"
+#include "drivers/time.h"
+
+#include "io/serial.h"
+#include "io/smartport_master.h"
+
+#include "rx/frsky_crc.h"
+
+enum {
+    PRIM_DISCARD_FRAME = 0x00,
+    PRIM_DATA_FRAME = 0x10,
+    PRIM_WORKING_STATE = 0x20,
+    PRIM_IDLE_STATE = 0x21,
+    PRIM_CONFIG_READ = 0x30,
+    PRIM_CONFIG_WRITE = 0x31,
+    PRIM_CONFIG_RESPONSE = 0x32,
+    PRIM_DIAG_READ = 0X40,
+    PRIM_DIAG_WRITE = 0X41,
+    PRIM_ENABLE_APP_NODE = 0x70,
+    PRIM_DISABLE_APP_NODE = 0x71,
+};
+
+enum
+{
+    DATAID_SPEED      = 0x0830,
+    DATAID_VFAS       = 0x0210,
+    DATAID_CURRENT    = 0x0200,
+    DATAID_RPM        = 0x050F,
+    DATAID_ALTITUDE   = 0x0100,
+    DATAID_FUEL       = 0x0600,
+    DATAID_ADC1       = 0xF102,
+    DATAID_ADC2       = 0xF103,
+    DATAID_LATLONG    = 0x0800,
+    DATAID_CAP_USED   = 0x0600,
+    DATAID_VARIO      = 0x0110,
+    DATAID_CELLS      = 0x0300,
+    DATAID_CELLS_LAST = 0x030F,
+    DATAID_HEADING    = 0x0840,
+    DATAID_FPV        = 0x0450,
+    DATAID_PITCH      = 0x0430,
+    DATAID_ROLL       = 0x0440,
+    DATAID_ACCX       = 0x0700,
+    DATAID_ACCY       = 0x0710,
+    DATAID_ACCZ       = 0x0720,
+    DATAID_T1         = 0x0400,
+    DATAID_T2         = 0x0410,
+    DATAID_HOME_DIST  = 0x0420,
+    DATAID_GPS_ALT    = 0x0820,
+    DATAID_ASPD       = 0x0A00,
+    DATAID_A3         = 0x0900,
+    DATAID_A4         = 0x0910,
+    DATAID_VS600      = 0x0E10
+};
+
+#define SMARTPORT_BAUD 57600
+#define SMARTPORT_UART_MODE MODE_RXTX
+
+#define SMARTPORT_PHYID_MAX 0x1B
+#define SMARTPORT_PHYID_COUNT (SMARTPORT_PHYID_MAX + 1)
+
+#define SMARTPORT_POLLING_INTERVAL 12 // ms
+
+#define SMARTPORT_FRAME_START 0x7E
+#define SMARTPORT_BYTESTUFFING_MARKER 0x7D
+#define SMARTPORT_BYTESTUFFING_XOR_VALUE 0x20
+
+#define SMARTPORT_SENSOR_DATA_TIMEOUT 500 // ms
+
+#define SMARTPORT_FORWARD_REQUESTS_MAX 10
+
+typedef enum {
+    PT_ACTIVE_ID,
+    PT_INACTIVE_ID
+} pollType_e;
+
+typedef struct smartPortMasterFrame_s {
+    uint8_t magic;
+    uint8_t phyID;
+    smartPortPayload_t payload;
+    uint8_t crc;
+} PACKED smartportFrame_t;
+
+typedef union {
+    smartportFrame_t frame;
+    uint8_t bytes[sizeof(smartportFrame_t)];
+} smartportFrameBuffer_u;
+
+typedef struct {
+    cellsData_t cells;
+    timeUs_t cellsTimestamp;
+    vs600Data_t vs600;
+    timeUs_t vs600Timestamp;
+    int32_t altitude;
+    timeUs_t altitudeTimestamp;
+    int16_t vario;
+    timeUs_t varioTimestamp;
+} smartportSensorsData_t;
+
+typedef struct {
+    uint8_t phyID;
+    smartPortPayload_t payload;
+} smartportForwardData_t;
+
+PG_REGISTER_WITH_RESET_TEMPLATE(smartportMasterConfig_t, smartportMasterConfig, PG_SMARTPORT_MASTER_CONFIG, 0);
+
+PG_RESET_TEMPLATE(smartportMasterConfig_t, smartportMasterConfig,
+    .halfDuplex = true,
+    .inverted = false
+);
+
+static serialPort_t *smartportMasterSerialPort = NULL;
+static serialPortConfig_t *portConfig;
+static int8_t currentPolledPhyID = -1;
+static int8_t forcedPolledPhyID = -1;
+static uint8_t rxBufferLen = 0;
+
+static uint32_t activePhyIDs = 0;
+static smartPortPayload_t sensorPayloadCache[SMARTPORT_PHYID_COUNT];
+
+static uint8_t forwardRequestsStart = 0;
+static uint8_t forwardRequestsEnd = 0;
+static smartportForwardData_t forwardRequests[SMARTPORT_FORWARD_REQUESTS_MAX]; // Forward requests' circular buffer
+
+static uint8_t forwardResponsesCount = 0;
+static smartportForwardData_t forwardResponses[SMARTPORT_FORWARD_REQUESTS_MAX]; // Forward responses' buffer
+
+static smartportSensorsData_t sensorsData;
+
+
+bool smartportMasterInit(void)
+{
+    portConfig = findSerialPortConfig(FUNCTION_TELEMETRY_SMARTPORT_MASTER);
+    if (!portConfig) {
+        return false;
+    }
+
+    portOptions_t portOptions = (smartportMasterConfig()->halfDuplex ? SERIAL_BIDIR : SERIAL_UNIDIR) | (smartportMasterConfig()->inverted ? SERIAL_NOT_INVERTED : SERIAL_INVERTED);
+    smartportMasterSerialPort = openSerialPort(portConfig->identifier, FUNCTION_TELEMETRY_SMARTPORT_MASTER, NULL, NULL, SMARTPORT_BAUD, SMARTPORT_UART_MODE, portOptions);
+
+    memset(&sensorsData, 0, sizeof(sensorsData));
+
+    return true;
+}
+
+static void phyIDSetActive(uint8_t phyID, bool active)
+{
+    uint32_t mask = 1 << phyID;
+    if (active) {
+        activePhyIDs |= mask;
+    } else {
+        activePhyIDs &= ~mask;
+    }
+}
+
+static uint32_t phyIDAllActiveMask(void)
+{
+    uint32_t mask = 0;
+    for (uint8_t i = 0; i < SMARTPORT_PHYID_COUNT; ++i) {
+        mask |= 1 << i;
+    }
+    return mask;
+}
+
+static int8_t phyIDNext(uint8_t start, bool active, bool loop)
+{
+    for (uint8_t i = start; i < ((loop ? start : 0) + SMARTPORT_PHYID_COUNT); ++i) {
+        uint8_t phyID = i % SMARTPORT_PHYID_COUNT;
+        uint32_t mask = 1 << phyID;
+        uint32_t phyIDMasked = activePhyIDs & mask;
+        if ((active && phyIDMasked) || !(active || phyIDMasked)) {
+            return phyID;
+        }
+    }
+    return -1;
+}
+
+static bool phyIDIsActive(uint8_t id)
+{
+    return !!(activePhyIDs & (1 << id));
+}
+
+static bool phyIDNoneActive(void)
+{
+    return activePhyIDs == 0;
+}
+
+static bool phyIDAllActive(void)
+{
+    static uint32_t allActiveMask = 0;
+
+    if (!allActiveMask) {
+        allActiveMask = phyIDAllActiveMask();
+    }
+
+    return !!((activePhyIDs & allActiveMask) == allActiveMask);
+}
+
+static bool phyIDAnyActive(void)
+{
+    return !!activePhyIDs;
+}
+
+static void smartportMasterSendByte(uint8_t byte)
+{
+    serialWrite(smartportMasterSerialPort, byte);
+}
+
+static void smartportMasterPhyIDFillCheckBits(uint8_t *phyIDByte)
+{
+    *phyIDByte |= (GET_BIT(*phyIDByte, 0) ^ GET_BIT(*phyIDByte, 1) ^ GET_BIT(*phyIDByte, 2)) << 5;
+    *phyIDByte |= (GET_BIT(*phyIDByte, 2) ^ GET_BIT(*phyIDByte, 3) ^ GET_BIT(*phyIDByte, 4)) << 6;
+    *phyIDByte |= (GET_BIT(*phyIDByte, 0) ^ GET_BIT(*phyIDByte, 2) ^ GET_BIT(*phyIDByte, 4)) << 7;
+}
+
+static void smartportMasterPoll(void)
+{
+    static pollType_e nextPollType = PT_INACTIVE_ID;
+    static uint8_t nextActivePhyID = 0, nextInactivePhyID = 0;
+    uint8_t phyIDToPoll;
+
+    if (currentPolledPhyID != -1) {
+        // currentPolledPhyID hasn't been reset by smartportMasterReceive so we didn't get valid data for this PhyID (inactive)
+        phyIDSetActive(currentPolledPhyID, false);
+    }
+
+    if (forcedPolledPhyID != -1) {
+
+        phyIDToPoll = forcedPolledPhyID;
+        forcedPolledPhyID = -1;
+
+    } else {
+
+        if (phyIDNoneActive()) {
+            nextPollType = PT_INACTIVE_ID;
+        } else if (phyIDAllActive()) {
+            nextPollType = PT_ACTIVE_ID;
+        }
+
+        switch (nextPollType) {
+
+            case PT_ACTIVE_ID: {
+                int8_t activePhyIDToPoll = phyIDNext(nextActivePhyID, true, false);
+                if (activePhyIDToPoll == -1) {
+                    nextActivePhyID = 0;
+                    nextPollType = PT_INACTIVE_ID;
+                } else {
+                    phyIDToPoll = activePhyIDToPoll;
+                    if (phyIDToPoll == SMARTPORT_PHYID_MAX) {
+                        nextActivePhyID = 0;
+                        if (!phyIDAllActive()) {
+                            nextPollType = PT_INACTIVE_ID;
+                        }
+                    } else {
+                        nextActivePhyID = phyIDToPoll + 1;
+                    }
+                    break;
+                }
+                FALLTHROUGH;
+            }
+
+            case PT_INACTIVE_ID: {
+                phyIDToPoll = phyIDNext(nextInactivePhyID, false, true);
+                nextInactivePhyID = (phyIDToPoll == SMARTPORT_PHYID_MAX ? 0 : phyIDToPoll + 1);
+                if (phyIDAnyActive()) {
+                    nextPollType = PT_ACTIVE_ID;
+                }
+                break;
+            }
+
+        }
+
+    }
+
+    currentPolledPhyID = phyIDToPoll;
+    smartportMasterPhyIDFillCheckBits(&phyIDToPoll);
+
+    // poll
+    smartportMasterSendByte(SMARTPORT_FRAME_START);
+    smartportMasterSendByte(phyIDToPoll);
+
+    rxBufferLen = 0; // discard incomplete frames received during previous poll
+}
+
+static void smartportMasterForwardNextPayload(void)
+{
+    smartportForwardData_t *request = forwardRequests + forwardRequestsStart;
+
+    /*forcedPolledPhyID = request->phyID; // force next poll to the request's phyID          XXX disabled, doesn't seem necessary */
+
+    smartportMasterPhyIDFillCheckBits(&request->phyID);
+    smartportMasterSendByte(SMARTPORT_FRAME_START);
+    smartportMasterSendByte(request->phyID);
+
+    uint16_t checksum = 0;
+    uint8_t *payloadPtr = (uint8_t *)&request->payload;
+    for (uint8_t i = 0; i < sizeof(request->payload); ++i) {
+        uint8_t c = *payloadPtr++;
+        if ((c == SMARTPORT_FRAME_START) || (c == SMARTPORT_BYTESTUFFING_MARKER)) {
+            smartportMasterSendByte(SMARTPORT_BYTESTUFFING_MARKER);
+            smartportMasterSendByte(c ^ SMARTPORT_BYTESTUFFING_XOR_VALUE);
+        } else {
+            smartportMasterSendByte(c);
+        }
+        checksum += c;
+    }
+    checksum = 0xff - ((checksum & 0xff) + (checksum >> 8));
+    smartportMasterSendByte(checksum);
+
+    forwardRequestsStart = (forwardRequestsStart + 1) % SMARTPORT_FORWARD_REQUESTS_MAX;
+}
+
+static void decodeCellsData(uint32_t sdata)
+{
+    uint8_t voltageStartIndex = sdata & 0xF;
+    uint8_t count = sdata >> 4 & 0xF;
+    uint16_t voltage1 = (sdata >> 8 & 0xFFF) * 2;
+    uint16_t voltage2 = (sdata >> 20 & 0xFFF) * 2;
+    if ((voltageStartIndex <= 4) && (count <= 6)) { // sanity check
+        cellsData_t *cd = &sensorsData.cells;
+        cd->count = count;
+        cd->voltage[voltageStartIndex] = voltage1;
+        cd->voltage[voltageStartIndex+1] = voltage2;
+
+        DEBUG_SET(DEBUG_SPM_CELLS, 0, cd->count);
+        for (uint8_t i = 0; i < 6; ++i) {
+            DEBUG_SET(DEBUG_SPM_CELLS, i+1, cd->voltage[i]);
+        }
+    }
+}
+
+static void decodeVS600Data(uint32_t sdata)
+{
+    vs600Data_t *vs600 = &sensorsData.vs600;
+    vs600->power = (sdata >> 8) & 0xFF;
+    vs600->band = (sdata >> 16) & 0xFF;
+    vs600->channel = (sdata >> 24) & 0xFF;
+    DEBUG_SET(DEBUG_SPM_VS600, 0, sdata);
+    DEBUG_SET(DEBUG_SPM_VS600, 1, vs600->channel);
+    DEBUG_SET(DEBUG_SPM_VS600, 2, vs600->band);
+    DEBUG_SET(DEBUG_SPM_VS600, 3, vs600->power);
+}
+
+static void decodeAltitudeData(uint32_t sdata)
+{
+    sensorsData.altitude = sdata * 5; // cm
+    DEBUG_SET(DEBUG_SPM_VARIO, 0, sensorsData.altitude);
+}
+
+static void decodeVarioData(uint32_t sdata)
+{
+    sensorsData.vario = sdata * 2; // mm/s
+    DEBUG_SET(DEBUG_SPM_VARIO, 1, sensorsData.vario);
+}
+
+static void processSensorPayload(smartPortPayload_t *payload, timeUs_t currentTimeUs)
+{
+    switch (payload->valueId) {
+        case DATAID_CELLS:
+            decodeCellsData(payload->data);
+            sensorsData.cellsTimestamp = currentTimeUs;
+            break;
+
+        case DATAID_VS600:
+            decodeVS600Data(payload->data);
+            sensorsData.vs600Timestamp = currentTimeUs;
+            break;
+
+        case DATAID_ALTITUDE:
+            decodeAltitudeData(payload->data);
+            sensorsData.altitudeTimestamp = currentTimeUs;
+            break;
+
+        case DATAID_VARIO:
+            decodeVarioData(payload->data);
+            sensorsData.varioTimestamp = currentTimeUs;
+            break;
+    }
+    sensorPayloadCache[currentPolledPhyID] = *payload;
+}
+
+static void processPayload(smartPortPayload_t *payload, timeUs_t currentTimeUs)
+{
+    switch (payload->frameId) {
+
+        case PRIM_DISCARD_FRAME:
+            break;
+
+        case PRIM_DATA_FRAME:
+            processSensorPayload(payload, currentTimeUs);
+            break;
+
+        default:
+            if (forwardResponsesCount < SMARTPORT_FORWARD_REQUESTS_MAX) {
+                smartportForwardData_t *response = forwardResponses + forwardResponsesCount;
+                response->phyID = currentPolledPhyID;
+                response->payload = *payload;
+                forwardResponsesCount += 1;
+            }
+            break;
+    }
+}
+
+static void smartportMasterReceive(timeUs_t currentTimeUs)
+{
+    static smartportFrameBuffer_u buffer;
+    static bool processByteStuffing = false;
+
+    if (!rxBufferLen) {
+        processByteStuffing = false;
+    }
+
+    while (serialRxBytesWaiting(smartportMasterSerialPort)) {
+
+        uint8_t c = serialRead(smartportMasterSerialPort);
+
+        if (currentPolledPhyID == -1) { // We did not poll a sensor or a packet has already been received and processed
+            continue;
+        }
+
+        if (processByteStuffing) {
+            c ^= SMARTPORT_BYTESTUFFING_XOR_VALUE;
+            processByteStuffing = false;
+        } else if (c == SMARTPORT_BYTESTUFFING_MARKER) {
+            processByteStuffing = true;
+            continue;
+        }
+
+        buffer.bytes[rxBufferLen] = c;
+        rxBufferLen += 1;
+
+        if (rxBufferLen == sizeof(buffer)) { // frame complete
+
+            uint8_t calcCRC = frskyCheckSum((uint8_t *)&buffer.frame.payload, sizeof(buffer.frame.payload));
+
+            if (buffer.frame.crc == calcCRC) {
+                phyIDSetActive(currentPolledPhyID, true);
+                processPayload(&buffer.frame.payload, currentTimeUs);
+            }
+
+            currentPolledPhyID = -1; // previously polled PhyID has answered, not expecting more data until next poll
+            rxBufferLen = 0; // reset buffer
+
+        }
+
+    }
+}
+
+bool smartportMasterGetSensorPayload(uint8_t phyID, smartPortPayload_t *payload)
+{
+    if ((phyID > SMARTPORT_PHYID_MAX) || !phyIDIsActive(phyID)) {
+        return false;
+    }
+
+    *payload = sensorPayloadCache[phyID];
+    return true;
+}
+
+bool smartportMasterHasForwardResponse(uint8_t phyID)
+{
+    for (uint8_t i = 0; i < forwardResponsesCount; ++i) {
+        smartportForwardData_t *response = forwardResponses + i;
+        if (response->phyID == phyID) {
+            return true;
+        }
+    }
+
+    return false;
+}
+
+bool smartportMasterNextForwardResponse(uint8_t phyID, smartPortPayload_t *payload)
+{
+    for (uint8_t i = 0; i < forwardResponsesCount; ++i) {
+        smartportForwardData_t *response = forwardResponses + i;
+        if (response->phyID == phyID) {
+            *payload = response->payload;
+            forwardResponsesCount -= 1;
+            memmove(response, response + 1, (forwardResponsesCount - i) * sizeof(*response));
+            return true;
+        }
+    }
+
+    return false;
+}
+
+static uint8_t forwardRequestCount(void)
+{
+    if (forwardRequestsStart > forwardRequestsEnd) {
+        return SMARTPORT_FORWARD_REQUESTS_MAX - forwardRequestsStart + forwardRequestsEnd;
+    } else {
+        return forwardRequestsEnd - forwardRequestsStart;
+    }
+}
+
+bool smartportMasterForward(uint8_t phyID, smartPortPayload_t *payload)
+{
+    if (forwardRequestCount() == SMARTPORT_FORWARD_REQUESTS_MAX) {
+        return false;
+    }
+
+    smartportForwardData_t *request = forwardRequests + forwardRequestsEnd;
+    request->phyID = phyID;
+    request->payload = *payload;
+    forwardRequestsEnd = (forwardRequestsEnd + 1) % SMARTPORT_FORWARD_REQUESTS_MAX;
+    return true;
+}
+
+void smartportMasterHandle(timeUs_t currentTimeUs)
+{
+    static timeUs_t pollTimestamp = 0;
+
+    if (!smartportMasterSerialPort) {
+        return;
+    }
+
+    if (!pollTimestamp || (cmpTimeUs(currentTimeUs, pollTimestamp) > SMARTPORT_POLLING_INTERVAL * 1000)) {
+        if (forwardRequestCount() && (forcedPolledPhyID == -1)) { // forward next payload if there is one in queue and we are not waiting from the response of the previous one
+            smartportMasterForwardNextPayload();
+        } else {
+            smartportMasterPoll();
+        }
+        pollTimestamp = currentTimeUs;
+    } else {
+        smartportMasterReceive(currentTimeUs);
+    }
+}
+
+cellsData_t *smartportMasterGetCellsData(void)
+{
+    if (micros() - sensorsData.cellsTimestamp > SMARTPORT_SENSOR_DATA_TIMEOUT) {
+        return NULL;
+    }
+
+    return &sensorsData.cells;
+}
+
+vs600Data_t *smartportMasterGetVS600Data(void)
+{
+    if (micros() - sensorsData.vs600Timestamp > SMARTPORT_SENSOR_DATA_TIMEOUT) {
+        return NULL;
+    }
+
+    return &sensorsData.vs600;
+}
+
+bool smartportMasterPhyIDIsActive(uint8_t phyID)
+{
+    return phyIDIsActive(phyID);
+}
+
+int8_t smartportMasterStripPhyIDCheckBits(uint8_t phyID)
+{
+    uint8_t smartportPhyID = phyID & 0x1F;
+    uint8_t phyIDCheck = smartportPhyID;
+    smartportMasterPhyIDFillCheckBits(&phyIDCheck);
+    return phyID == phyIDCheck ? smartportPhyID : -1;
+}
+
+#endif

src/main/io/smartport_master.h

@@ -0,0 +1,87 @@
+/*
+ * This file is part of iNav
+ *
+ * iNav free software. You can redistribute
+ * this software and/or modify this software 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 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 software.
+ *
+ * If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#include <common/time.h>
+
+#include "config/parameter_group.h"
+
+#include <telemetry/smartport.h>
+
+#if defined(USE_SMARTPORT_MASTER)
+
+typedef struct {
+    bool halfDuplex;
+    bool inverted;
+} smartportMasterConfig_t;
+
+PG_DECLARE(smartportMasterConfig_t, smartportMasterConfig);
+
+typedef struct {
+    int8_t count;
+    int16_t voltage[6];
+} cellsData_t;
+
+typedef enum {
+    VS600_BAND_A,
+    VS600_BAND_B,
+    VS600_BAND_C,
+    VS600_BAND_D,
+    VS600_BAND_E,
+    VS600_BAND_F,
+} vs600Band_e;
+
+typedef enum {
+    VS600_POWER_PIT,
+    VS600_POWER_25MW,
+    VS600_POWER_200MW,
+    VS600_POWER_600MW,
+} vs600Power_e;
+
+typedef struct {
+    vs600Band_e band;
+    uint8_t channel;
+    vs600Power_e power;
+} vs600Data_t;
+
+
+bool smartportMasterInit(void);
+void smartportMasterHandle(timeUs_t currentTimeUs);
+
+bool smartportMasterPhyIDIsActive(uint8_t phyID);
+int8_t smartportMasterStripPhyIDCheckBits(uint8_t phyID);
+
+// Returns latest received SmartPort payload for phyID
+bool smartportMasterGetSensorPayload(uint8_t phyID, smartPortPayload_t *payload);
+
+// Forwarding
+bool smartportMasterForward(uint8_t phyID, smartPortPayload_t *payload);
+bool smartportMasterHasForwardResponse(uint8_t phyID);
+bool smartportMasterNextForwardResponse(uint8_t phyID, smartPortPayload_t *payload);
+
+// Returns latest Cells data or NULL if the data is too old
+cellsData_t *smartportMasterGetCellsData(void);
+vs600Data_t *smartportMasterGetVS600Data(void);
+
+#endif /* USE_SMARTPORT_MASTER */

src/main/programming/global_functions.c

@@ -135,6 +135,12 @@ void globalFunctionsProcess(int8_t functionId) {
                     GLOBAL_FUNCTION_FLAG_ENABLE(GLOBAL_FUNCTION_FLAG_OVERRIDE_THROTTLE);
                 }
                 break;
+            case GLOBAL_FUNCTION_ACTION_SET_OSD_LAYOUT:
+                if(conditionValue){
+                    globalFunctionValues[GLOBAL_FUNCTION_ACTION_SET_OSD_LAYOUT] = globalFunctionsStates[functionId].value;
+                    GLOBAL_FUNCTION_FLAG_ENABLE(GLOBAL_FUNCTION_FLAG_OVERRIDE_OSD_LAYOUT);
+                }
+                break;
         }
     }
 }

src/main/programming/global_functions.h

@@ -39,6 +39,7 @@ typedef enum {
     GLOBAL_FUNCTION_ACTION_OVERRIDE_THROTTLE,               // 7
     GLOBAL_FUNCTION_ACTION_SET_VTX_BAND,                    // 8
     GLOBAL_FUNCTION_ACTION_SET_VTX_CHANNEL,                 // 9
+    GLOBAL_FUNCTION_ACTION_SET_OSD_LAYOUT,                  // 10
     GLOBAL_FUNCTION_ACTION_LAST
 } globalFunctionActions_e;
 
@@ -50,6 +51,7 @@ typedef enum {
     GLOBAL_FUNCTION_FLAG_OVERRIDE_INVERT_PITCH = (1 << 4),
     GLOBAL_FUNCTION_FLAG_OVERRIDE_INVERT_YAW = (1 << 5),
     GLOBAL_FUNCTION_FLAG_OVERRIDE_THROTTLE = (1 << 6),
+    GLOBAL_FUNCTION_FLAG_OVERRIDE_OSD_LAYOUT = (1 << 7),
 } globalFunctionFlags_t;
 
 typedef struct globalFunction_s {

src/main/scheduler/scheduler.h

@@ -116,6 +116,9 @@ typedef enum {
 #ifdef USE_RPM_FILTER
     TASK_RPM_FILTER,
 #endif
+#if defined(USE_SMARTPORT_MASTER)
+    TASK_SMARTPORT_MASTER,
+#endif
 #ifdef USE_IRLOCK
     TASK_IRLOCK,
 #endif

src/main/telemetry/mavlink.c

@@ -82,6 +82,60 @@
 #define TELEMETRY_MAVLINK_MAXRATE       50
 #define TELEMETRY_MAVLINK_DELAY         ((1000 * 1000) / TELEMETRY_MAVLINK_MAXRATE)
 
+
+/** @brief A mapping of plane flight modes for custom_mode field of heartbeat. */
+typedef enum APM_PLANE_MODE
+{
+   PLANE_MODE_MANUAL=0,
+   PLANE_MODE_CIRCLE=1,
+   PLANE_MODE_STABILIZE=2,
+   PLANE_MODE_TRAINING=3,
+   PLANE_MODE_ACRO=4,
+   PLANE_MODE_FLY_BY_WIRE_A=5,
+   PLANE_MODE_FLY_BY_WIRE_B=6,
+   PLANE_MODE_CRUISE=7,
+   PLANE_MODE_AUTOTUNE=8,
+   PLANE_MODE_AUTO=10,
+   PLANE_MODE_RTL=11,
+   PLANE_MODE_LOITER=12,
+   PLANE_MODE_TAKEOFF=13,
+   PLANE_MODE_AVOID_ADSB=14,
+   PLANE_MODE_GUIDED=15,
+   PLANE_MODE_INITIALIZING=16,
+   PLANE_MODE_QSTABILIZE=17,
+   PLANE_MODE_QHOVER=18,
+   PLANE_MODE_QLOITER=19,
+   PLANE_MODE_QLAND=20,
+   PLANE_MODE_QRTL=21,
+   PLANE_MODE_QAUTOTUNE=22,
+   PLANE_MODE_ENUM_END=23,
+} APM_PLANE_MODE;
+
+/** @brief A mapping of copter flight modes for custom_mode field of heartbeat. */
+typedef enum APM_COPTER_MODE
+{
+   COPTER_MODE_STABILIZE=0,
+   COPTER_MODE_ACRO=1,
+   COPTER_MODE_ALT_HOLD=2,
+   COPTER_MODE_AUTO=3,
+   COPTER_MODE_GUIDED=4,
+   COPTER_MODE_LOITER=5,
+   COPTER_MODE_RTL=6,
+   COPTER_MODE_CIRCLE=7,
+   COPTER_MODE_LAND=9,
+   COPTER_MODE_DRIFT=11,
+   COPTER_MODE_SPORT=13,
+   COPTER_MODE_FLIP=14,
+   COPTER_MODE_AUTOTUNE=15,
+   COPTER_MODE_POSHOLD=16,
+   COPTER_MODE_BRAKE=17,
+   COPTER_MODE_THROW=18,
+   COPTER_MODE_AVOID_ADSB=19,
+   COPTER_MODE_GUIDED_NOGPS=20,
+   COPTER_MODE_SMART_RTL=21,
+   COPTER_MODE_ENUM_END=22,
+} APM_COPTER_MODE;
+
 static serialPort_t *mavlinkPort = NULL;
 static serialPortConfig_t *portConfig;
 
@@ -108,9 +162,45 @@ static mavlink_status_t mavRecvStatus;
 static uint8_t mavSystemId = 1;
 static uint8_t mavComponentId = MAV_COMP_ID_SYSTEM_CONTROL;
 
-// MANUAL, ACRO, ANGLE, HRZN, ALTHOLD, POSHOLD, RTH, WP, LAUNCH, FAILSAFE
+APM_COPTER_MODE inavToArduCopterMap(flightModeForTelemetry_e flightMode)
-static uint8_t inavToArduCopterMap[FLM_COUNT] = { 1,  1,  0,  0,  2, 16,  6,  3, 18,  0 };
+{
-static uint8_t inavToArduPlaneMap[FLM_COUNT]  = { 0,  4,  2,  2,  5,  1, 11, 10, 15,  2 };
+    switch (flightMode)
+    {
+        case FLM_ACRO:          return COPTER_MODE_ACRO;
+        case FLM_ACRO_AIR:      return COPTER_MODE_ACRO;
+        case FLM_ANGLE:         return COPTER_MODE_STABILIZE;
+        case FLM_HORIZON:       return COPTER_MODE_STABILIZE;
+        case FLM_ALTITUDE_HOLD: return COPTER_MODE_ALT_HOLD;
+        case FLM_POSITION_HOLD: return COPTER_MODE_POSHOLD;
+        case FLM_RTH:           return COPTER_MODE_RTL;
+        case FLM_MISSION:       return COPTER_MODE_AUTO;
+        case FLM_LAUNCH:        return COPTER_MODE_THROW;
+        case FLM_FAILSAFE:      return COPTER_MODE_RTL;
+        default:                return COPTER_MODE_ENUM_END;
+    }
+}
+
+APM_PLANE_MODE inavToArduPlaneMap(flightModeForTelemetry_e flightMode)
+{
+    switch (flightMode)
+    {
+        case FLM_MANUAL:        return PLANE_MODE_MANUAL;
+        case FLM_ACRO:          return PLANE_MODE_ACRO;
+        case FLM_ACRO_AIR:      return PLANE_MODE_ACRO;
+        case FLM_ANGLE:         return PLANE_MODE_FLY_BY_WIRE_A;
+        case FLM_HORIZON:       return PLANE_MODE_STABILIZE;
+        case FLM_ALTITUDE_HOLD: return PLANE_MODE_FLY_BY_WIRE_B;
+        case FLM_POSITION_HOLD: return PLANE_MODE_LOITER;
+        case FLM_RTH:           return PLANE_MODE_RTL;
+        case FLM_MISSION:       return PLANE_MODE_AUTO;
+        case FLM_CRUISE:        return PLANE_MODE_CRUISE;
+        case FLM_LAUNCH:        return PLANE_MODE_TAKEOFF;
+        case FLM_FAILSAFE:      return PLANE_MODE_RTL;
+        default:                return PLANE_MODE_ENUM_END;
+    }
+}
+
+
 
 static int mavlinkStreamTrigger(enum MAV_DATA_STREAM streamNum)
 {
@@ -468,10 +558,10 @@ void mavlinkSendHUDAndHeartbeat(void)
     uint8_t mavCustomMode;
 
     if (STATE(FIXED_WING_LEGACY)) {
-        mavCustomMode = inavToArduPlaneMap[flm];
+        mavCustomMode = (uint8_t)inavToArduPlaneMap(flm);
     }
     else {
-        mavCustomMode = inavToArduCopterMap[flm];
+        mavCustomMode = (uint8_t)inavToArduCopterMap(flm);
     }
 
     if (flm != FLM_MANUAL) {