CF/BF - First cut of MSP current sensor.

How it works:
1. Current Meter Source on FC is set to MSP/OSD Slave by user.
2. On reboot FC sends MSP_ANALOG to OSD Slave.
3. OSD Slave listens for incoming MSP as usual.
4. OSD responds to MSP as usual.
6. The FC recevies the data from the OSD Slave and updates the MSP
Current Meter.

src/main/fc/fc_msp.c

@@ -792,7 +792,7 @@ static bool mspOsdSlaveProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostPro
 
     switch (cmdMSP) {
     case MSP_STATUS_EX:
-        sbufWriteU16(dst, 0); // task delta
+        sbufWriteU16(dst, getTaskDeltaTime(TASK_SERIAL));
 #ifdef USE_I2C
         sbufWriteU16(dst, i2cGetErrorCounter());
 #else
@@ -807,7 +807,7 @@ static bool mspOsdSlaveProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostPro
         break;
 
     case MSP_STATUS:
-        sbufWriteU16(dst, 0); // task delta
+        sbufWriteU16(dst, getTaskDeltaTime(TASK_SERIAL));
 #ifdef USE_I2C
         sbufWriteU16(dst, i2cGetErrorCounter());
 #else
@@ -1375,7 +1375,6 @@ static void mspFcDataFlashReadCommand(sbuf_t *dst, sbuf_t *src)
 static mspResult_e mspOsdSlaveProcessInCommand(uint8_t cmdMSP, sbuf_t *src) {
     UNUSED(cmdMSP);
     UNUSED(src);
-    // Nothing OSD SLAVE specific yet.
     return MSP_RESULT_ERROR;
 }
 #endif
@@ -2145,8 +2144,31 @@ void mspFcProcessReply(mspPacket_t *reply)
     UNUSED(src); // potentially unused depending on compile options.
 
     switch (reply->cmd) {
-        case MSP_DISPLAYPORT: {
+#ifndef OSD_SLAVE
+    case MSP_ANALOG:
+        {
+            uint8_t batteryVoltage = sbufReadU8(src);
+            uint16_t mAhDrawn = sbufReadU16(src);
+            uint16_t rssi = sbufReadU16(src);
+            uint16_t amperage = sbufReadU16(src);
+
+            UNUSED(rssi);
+            UNUSED(batteryVoltage);
+            UNUSED(amperage);
+            UNUSED(mAhDrawn);
+
+#ifdef USE_MSP_CURRENT_METER
+            currentMeterMSPSet(amperage, mAhDrawn);
+#endif
+            break;
+        }
+#endif
+
 #ifdef USE_OSD_SLAVE
+    case MSP_DISPLAYPORT:
+        {
+            osdSlaveIsLocked = true; // lock it as soon as a MSP_DISPLAYPORT message is received to prevent accidental CLI/DFU mode.
+
             int subCmd = sbufReadU8(src);
 
             switch (subCmd) {
@@ -2184,9 +2206,9 @@ void mspFcProcessReply(mspPacket_t *reply)
                     osdSlaveDrawScreen();
                 }
             }
-#endif
             break;
         }
+#endif
     }
 }
 

src/main/fc/fc_msp.h

@@ -33,3 +33,5 @@ void mspFcInit(void);
 void mspOsdSlaveInit(void);
 mspResult_e mspFcProcessCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostProcessFnPtr *mspPostProcessFn);
 void mspFcProcessReply(mspPacket_t *reply);
+
+void mspSerialProcessStreamSchedule(void);

src/main/fc/fc_tasks.c

@@ -23,6 +23,8 @@
 
 #include "cms/cms.h"
 
+#include "build/debug.h"
+
 #include "common/axis.h"
 #include "common/color.h"
 #include "common/utils.h"
@@ -81,6 +83,8 @@
 
 #include "telemetry/telemetry.h"
 
+#include "io/osd_slave.h"
+
 #ifdef USE_BST
 void taskBstMasterProcess(timeUs_t currentTimeUs);
 #endif
@@ -115,7 +119,12 @@ static void taskHandleSerial(timeUs_t currentTimeUs)
         return;
     }
 #endif
-    mspSerialProcess(ARMING_FLAG(ARMED) ? MSP_SKIP_NON_MSP_DATA : MSP_EVALUATE_NON_MSP_DATA, mspFcProcessCommand, mspFcProcessReply);
+#ifndef OSD_SLAVE
+    bool evaluateMspData = ARMING_FLAG(ARMED) ? MSP_SKIP_NON_MSP_DATA : MSP_EVALUATE_NON_MSP_DATA;
+#else
+    bool evaluateMspData = osdSlaveIsLocked ?  MSP_SKIP_NON_MSP_DATA : MSP_EVALUATE_NON_MSP_DATA;;
+#endif
+    mspSerialProcess(evaluateMspData, mspFcProcessCommand, mspFcProcessReply);
 }
 
 void taskBatteryAlerts(timeUs_t currentTimeUs)

src/main/io/displayport_msp.c

@@ -58,7 +58,7 @@ static int output(displayPort_t *displayPort, uint8_t cmd, uint8_t *buf, int len
         return 0;
     }
 #endif
-    return mspSerialPush(cmd, buf, len);
+    return mspSerialPush(cmd, buf, len, MSP_DIRECTION_REPLY);
 }
 
 static int heartbeat(displayPort_t *displayPort)

src/main/io/osd_slave.c

@@ -40,6 +40,9 @@
 
 //#define OSD_SLAVE_DEBUG
 
+// when locked the system ignores requests to enter cli or bootloader mode via serial connection.
+bool osdSlaveIsLocked = false;
+
 static displayPort_t *osdDisplayPort;
 
 static void osdDrawLogo(int x, int y)

src/main/io/osd_slave.h

@@ -22,6 +22,8 @@
 
 struct displayPort_s;
 
+extern bool osdSlaveIsLocked;
+
 // init
 void osdSlaveInit(struct displayPort_s *osdDisplayPort);
 

src/main/msp/msp.h

@@ -26,10 +26,16 @@ typedef enum {
     MSP_RESULT_NO_REPLY = 0
 } mspResult_e;
 
+typedef enum {
+    MSP_DIRECTION_REPLY = 0,
+    MSP_DIRECTION_REQUEST = 1
+} mspDirection_e;
+
 typedef struct mspPacket_s {
     sbuf_t buf;
     int16_t cmd;
     int16_t result;
+    uint8_t direction;
 } mspPacket_t;
 
 struct serialPort_s;

src/main/msp/msp_protocol.h

@@ -101,7 +101,6 @@
 #define MSP_NAME                        10   //out message          Returns user set board name - betaflight
 #define MSP_SET_NAME                    11   //in message           Sets board name - betaflight
 
-
 //
 // MSP commands for Cleanflight original features
 //
@@ -312,3 +311,4 @@
 #define MSP_SERVO_MIX_RULES      241    //out message         Returns servo mixer configuration
 #define MSP_SET_SERVO_MIX_RULE   242    //in message          Sets servo mixer configuration
 #define MSP_SET_4WAY_IF          245    //in message          Sets 4way interface
+

src/main/msp/msp_serial.c

@@ -27,12 +27,13 @@
 
 #include "io/serial.h"
 
+#include "drivers/system.h"
+
 #include "msp/msp.h"
 #include "msp/msp_serial.h"
 
 static mspPort_t mspPorts[MAX_MSP_PORT_COUNT];
 
-
 static void resetMspPort(mspPort_t *mspPortToReset, serialPort_t *serialPort)
 {
     memset(mspPortToReset, 0, sizeof(mspPort_t));
@@ -138,7 +139,13 @@ static int mspSerialEncode(mspPort_t *msp, mspPacket_t *packet)
     serialBeginWrite(msp->port);
     const int len = sbufBytesRemaining(&packet->buf);
     const int mspLen = len < JUMBO_FRAME_SIZE_LIMIT ? len : JUMBO_FRAME_SIZE_LIMIT;
-    uint8_t hdr[8] = {'$', 'M', packet->result == MSP_RESULT_ERROR ? '!' : '>', mspLen, packet->cmd};
+    uint8_t hdr[8] = {
+        '$',
+        'M',
+        packet->result == MSP_RESULT_ERROR ? '!' : packet->direction == MSP_DIRECTION_REPLY ? '>' : '<',
+        mspLen,
+        packet->cmd
+    };
     int hdrLen = 5;
 #define CHECKSUM_STARTPOS 3  // checksum starts from mspLen field
     if (len >= JUMBO_FRAME_SIZE_LIMIT) {
@@ -165,6 +172,7 @@ static mspPostProcessFnPtr mspSerialProcessReceivedCommand(mspPort_t *msp, mspPr
         .buf = { .ptr = outBuf, .end = ARRAYEND(outBuf), },
         .cmd = -1,
         .result = 0,
+        .direction = MSP_DIRECTION_REPLY,
     };
     uint8_t *outBufHead = reply.buf.ptr;
 
@@ -172,6 +180,7 @@ static mspPostProcessFnPtr mspSerialProcessReceivedCommand(mspPort_t *msp, mspPr
         .buf = { .ptr = msp->inBuf, .end = msp->inBuf + msp->dataSize, },
         .cmd = msp->cmdMSP,
         .result = 0,
+        .direction = MSP_DIRECTION_REQUEST,
     };
 
     mspPostProcessFnPtr mspPostProcessFn = NULL;
@@ -182,7 +191,6 @@ static mspPostProcessFnPtr mspSerialProcessReceivedCommand(mspPort_t *msp, mspPr
         mspSerialEncode(msp, &reply);
     }
 
-    msp->c_state = MSP_IDLE;
     return mspPostProcessFn;
 }
 
@@ -215,7 +223,9 @@ void mspSerialProcess(mspEvaluateNonMspData_e evaluateNonMspData, mspProcessComm
         if (!mspPort->port) {
             continue;
         }
+
         mspPostProcessFnPtr mspPostProcessFn = NULL;
+
         while (serialRxBytesWaiting(mspPort->port)) {
 
             const uint8_t c = serialRead(mspPort->port);
@@ -231,9 +241,12 @@ void mspSerialProcess(mspEvaluateNonMspData_e evaluateNonMspData, mspProcessComm
                 } else if (mspPort->packetType == MSP_PACKET_REPLY) {
                     mspSerialProcessReceivedReply(mspPort, mspProcessReplyFn);
                 }
+
+                mspPort->c_state = MSP_IDLE;
                 break; // process one command at a time so as not to block.
             }
         }
+
         if (mspPostProcessFn) {
             waitForSerialPortToFinishTransmitting(mspPort->port);
             mspPostProcessFn(mspPort->port);
@@ -262,7 +275,7 @@ void mspSerialInit(void)
     mspSerialAllocatePorts();
 }
 
-int mspSerialPush(uint8_t cmd, uint8_t *data, int datalen)
+int mspSerialPush(uint8_t cmd, uint8_t *data, int datalen, mspDirection_e direction)
 {
     int ret = 0;
 
@@ -281,6 +294,7 @@ int mspSerialPush(uint8_t cmd, uint8_t *data, int datalen)
             .buf = { .ptr = data, .end = data + datalen, },
             .cmd = cmd,
             .result = 0,
+            .direction = direction,
         };
 
         ret = mspSerialEncode(mspPort, &push);
@@ -288,6 +302,7 @@ int mspSerialPush(uint8_t cmd, uint8_t *data, int datalen)
     return ret; // return the number of bytes written
 }
 
+
 uint32_t mspSerialTxBytesFree()
 {
     uint32_t ret = UINT32_MAX;

src/main/msp/msp_serial.h

@@ -67,11 +67,10 @@ typedef struct mspPort_s {
     uint8_t inBuf[MSP_PORT_INBUF_SIZE];
 } mspPort_t;
 
-
 void mspSerialInit(void);
 bool mspSerialWaiting(void);
 void mspSerialProcess(mspEvaluateNonMspData_e evaluateNonMspData, mspProcessCommandFnPtr mspProcessCommandFn, mspProcessReplyFnPtr mspProcessReplyFn);
 void mspSerialAllocatePorts(void);
 void mspSerialReleasePortIfAllocated(struct serialPort_s *serialPort);
-int mspSerialPush(uint8_t cmd, uint8_t *data, int datalen);
+int mspSerialPush(uint8_t cmd, uint8_t *data, int datalen, mspDirection_e direction);
 uint32_t mspSerialTxBytesFree(void);

src/main/sensors/battery.c

@@ -76,9 +76,13 @@ static batteryState_e consumptionState;
 #ifdef USE_VIRTUAL_CURRENT_METER
 #define DEFAULT_CURRENT_METER_SOURCE CURRENT_METER_VIRTUAL
 #else
+#ifdef USE_MSP_CURRENT_METER
+#define DEFAULT_CURRENT_METER_SOURCE CURRENT_METER_MSP
+#else
 #define DEFAULT_CURRENT_METER_SOURCE CURRENT_METER_NONE
 #endif
 #endif
+#endif
 
 #ifdef BOARD_HAS_VOLTAGE_DIVIDER
 #define DEFAULT_VOLTAGE_METER_SOURCE VOLTAGE_METER_ADC
@@ -302,6 +306,11 @@ void batteryInit(void)
         case CURRENT_METER_ESC:
 #ifdef ESC_SENSOR
             currentMeterESCInit();
+#endif
+            break;
+        case CURRENT_METER_MSP:
+#ifdef USE_MSP_CURRENT_METER
+            currentMeterMSPInit();
 #endif
             break;
 
@@ -362,6 +371,12 @@ void batteryUpdateCurrentMeter(timeUs_t currentTimeUs)
                 currentMeterESCRefresh(lastUpdateAt);
                 currentMeterESCReadCombined(&currentMeter);
             }
+#endif
+            break;
+        case CURRENT_METER_MSP:
+#ifdef USE_MSP_CURRENT_METER
+            currentMeterMSPRefresh(currentTimeUs);
+            currentMeterMSPRead(&currentMeter);
 #endif
             break;
 

src/main/sensors/current.c

@@ -56,12 +56,15 @@ const uint8_t currentMeterIds[] = {
     CURRENT_METER_ID_ESC_MOTOR_11,
     CURRENT_METER_ID_ESC_MOTOR_12,
 #endif
+#ifdef USE_MSP_CURRENT_METER
+    CURRENT_METER_ID_MSP_1,
+#endif
 };
 
 const uint8_t supportedCurrentMeterCount = ARRAYLEN(currentMeterIds);
 
 //
-// ADC/Virtual/ESC shared
+// ADC/Virtual/ESC/MSP shared
 //
 
 void currentMeterReset(currentMeter_t *meter)
@@ -225,6 +228,45 @@ void currentMeterESCReadMotor(uint8_t motorNumber, currentMeter_t *meter)
 }
 #endif
 
+
+#ifdef USE_MSP_CURRENT_METER
+#include "common/streambuf.h"
+#include "msp/msp_protocol.h"
+#include "msp/msp_serial.h"
+
+currentMeterMSPState_t currentMeterMSPState;
+
+void currentMeterMSPSet(uint16_t amperage, uint16_t mAhDrawn)
+{
+    // We expect the FC's MSP_ANALOG response handler to call this function
+    currentMeterMSPState.amperage = amperage;
+    currentMeterMSPState.mAhDrawn = mAhDrawn;
+}
+
+void currentMeterMSPInit(void)
+{
+    memset(&currentMeterMSPState, 0, sizeof(currentMeterMSPState_t));
+}
+
+void currentMeterMSPRefresh(timeUs_t currentTimeUs)
+{
+    // periodically request MSP_ANALOG
+    static timeUs_t streamRequestAt = 0;
+    if (cmp32(currentTimeUs, streamRequestAt) > 0) {
+        streamRequestAt = currentTimeUs + ((1000 * 1000) / 10); // 10hz
+
+        mspSerialPush(MSP_ANALOG, NULL, 0, MSP_DIRECTION_REQUEST);
+    }
+}
+
+void currentMeterMSPRead(currentMeter_t *meter)
+{
+    meter->amperageLatest = currentMeterMSPState.amperage;
+    meter->amperage = currentMeterMSPState.amperage;
+    meter->mAhDrawn = currentMeterMSPState.mAhDrawn;
+}
+#endif
+
 //
 // API for current meters using IDs
 //
@@ -241,6 +283,11 @@ void currentMeterRead(currentMeterId_e id, currentMeter_t *meter)
         currentMeterVirtualRead(meter);
     }
 #endif
+#ifdef USE_MSP_CURRENT_METER
+    else if (id == CURRENT_METER_ID_MSP_1) {
+        currentMeterMSPRead(meter);
+    }
+#endif
 #ifdef USE_ESC_SENSOR
     else if (id == CURRENT_METER_ID_ESC_COMBINED_1) {
         currentMeterESCReadCombined(meter);

src/main/sensors/current.h

@@ -17,6 +17,7 @@
 
 #pragma once
 
+#include "common/time.h"
 #include "current_ids.h"
 
 typedef enum {
@@ -24,6 +25,7 @@ typedef enum {
     CURRENT_METER_ADC,
     CURRENT_METER_VIRTUAL,
     CURRENT_METER_ESC,
+    CURRENT_METER_MSP,
     CURRENT_METER_MAX = CURRENT_METER_ESC
 } currentMeterSource_e;
 
@@ -48,6 +50,7 @@ typedef enum {
     CURRENT_SENSOR_VIRTUAL = 0,
     CURRENT_SENSOR_ADC,
     CURRENT_SENSOR_ESC,
+    CURRENT_SENSOR_MSP
 } currentSensor_e;
 
 
@@ -93,6 +96,21 @@ typedef struct currentMeterESCState_s {
     int32_t amperage;           // current read by current sensor in centiampere (1/100th A)
 } currentMeterESCState_t;
 
+
+//
+// MSP
+//
+
+typedef struct currentMeterMSPState_s {
+    int32_t mAhDrawn;           // milliampere hours drawn from the battery since start
+    int32_t amperage;           // current read by current sensor in centiampere (1/100th A)
+} currentMeterMSPState_t;
+
+
+//
+// Current Meter API
+//
+
 void currentMeterReset(currentMeter_t *meter);
 
 void currentMeterADCInit(void);
@@ -108,6 +126,11 @@ void currentMeterESCRefresh(int32_t lastUpdateAt);
 void currentMeterESCReadCombined(currentMeter_t *meter);
 void currentMeterESCReadMotor(uint8_t motorNumber, currentMeter_t *meter);
 
+void currentMeterMSPInit(void);
+void currentMeterMSPRefresh(timeUs_t currentTimeUs);
+void currentMeterMSPRead(currentMeter_t *meter);
+void currentMeterMSPSet(uint16_t amperage, uint16_t mAhDrawn);
+
 //
 // API for reading current meters by id.
 //

src/main/sensors/current_ids.h

@@ -66,4 +66,7 @@ typedef enum {
     CURRENT_METER_ID_VIRTUAL_1 = 80,       // 80-89 for virtual meters
     CURRENT_METER_ID_VIRTUAL_2,
 
+    CURRENT_METER_ID_MSP_1 = 90,       // 90-99 for MSP meters
+    CURRENT_METER_ID_MSP_2,
+
 } currentMeterId_e;

src/main/target/SPRACINGF3/target.h

@@ -180,6 +180,8 @@
 #define USE_OSD_OVER_MSP_DISPLAYPORT
 #define USE_SLOW_MSP_DISPLAYPORT_RATE_WHEN_UNARMED
 
+#define USE_MSP_CURRENT_METER
+
 #define USE_ESC_SENSOR
 #define REMAP_TIM17_DMA
 

src/main/target/SPRACINGF3EVO/target.h

@@ -163,6 +163,8 @@
 
 #define OSD
 #define USE_OSD_OVER_MSP_DISPLAYPORT
+#define USE_MSP_CURRENT_METER
+
 #undef USE_DASHBOARD
 
 #define TRANSPONDER

src/main/target/SPRACINGF4EVO/target.h

@@ -162,6 +162,7 @@
 
 #define OSD
 #define USE_OSD_OVER_MSP_DISPLAYPORT
+#define USE_MSP_CURRENT_METER
 
 #define LED_STRIP
 #define TRANSPONDER