Merge pull request #2983 from betaflight/osd-slave-msp-current-meter

CF/BF - MSP current sensor.

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

@@ -59,7 +59,7 @@
 #define MSP_PROTOCOL_VERSION                0
 
 #define API_VERSION_MAJOR                   1  // increment when major changes are made
-#define API_VERSION_MINOR                   35 // increment after a release, to set the version for all changes to go into the following release (if no changes to MSP are made between the releases, this can be reverted before the release)
+#define API_VERSION_MINOR                   36 // increment after a release, to set the version for all changes to go into the following release (if no changes to MSP are made between the releases, this can be reverted before the release)
 
 #define API_VERSION_LENGTH                  2
 
@@ -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