diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 7f967bf710..1f06610296 100644 --- a/src/main/fc/fc_msp.c +++ b/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 } } diff --git a/src/main/fc/fc_msp.h b/src/main/fc/fc_msp.h index c5ccb1f1c4..577969c349 100644 --- a/src/main/fc/fc_msp.h +++ b/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); diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index a01e833546..a34c582f8f 100644 --- a/src/main/fc/fc_tasks.c +++ b/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) diff --git a/src/main/io/displayport_msp.c b/src/main/io/displayport_msp.c index d977ca75ca..dc8088ef4c 100644 --- a/src/main/io/displayport_msp.c +++ b/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) diff --git a/src/main/io/osd_slave.c b/src/main/io/osd_slave.c index ff148e83e8..b8a6ff6ca0 100644 --- a/src/main/io/osd_slave.c +++ b/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) diff --git a/src/main/io/osd_slave.h b/src/main/io/osd_slave.h index c7974f84dc..7d9e945afc 100644 --- a/src/main/io/osd_slave.h +++ b/src/main/io/osd_slave.h @@ -22,6 +22,8 @@ struct displayPort_s; +extern bool osdSlaveIsLocked; + // init void osdSlaveInit(struct displayPort_s *osdDisplayPort); diff --git a/src/main/msp/msp.h b/src/main/msp/msp.h index 5e6a14ac58..9a9ddb0f4b 100644 --- a/src/main/msp/msp.h +++ b/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; diff --git a/src/main/msp/msp_protocol.h b/src/main/msp/msp_protocol.h index f8c8eadf18..14d0e606af 100644 --- a/src/main/msp/msp_protocol.h +++ b/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 + diff --git a/src/main/msp/msp_serial.c b/src/main/msp/msp_serial.c index 7a233ca876..08cb7d7e4a 100644 --- a/src/main/msp/msp_serial.c +++ b/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; diff --git a/src/main/msp/msp_serial.h b/src/main/msp/msp_serial.h index e6136420cc..0032937363 100644 --- a/src/main/msp/msp_serial.h +++ b/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); diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index ed289aaecb..a0e7af7956 100644 --- a/src/main/sensors/battery.c +++ b/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(¤tMeter); } +#endif + break; + case CURRENT_METER_MSP: +#ifdef USE_MSP_CURRENT_METER + currentMeterMSPRefresh(currentTimeUs); + currentMeterMSPRead(¤tMeter); #endif break; diff --git a/src/main/sensors/current.c b/src/main/sensors/current.c index 67b170fd32..da7654357c 100644 --- a/src/main/sensors/current.c +++ b/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(¤tMeterMSPState, 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); diff --git a/src/main/sensors/current.h b/src/main/sensors/current.h index 7da02fa6b5..48c0db2bab 100644 --- a/src/main/sensors/current.h +++ b/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. // diff --git a/src/main/sensors/current_ids.h b/src/main/sensors/current_ids.h index 2f366c6f1f..f7bb3029e3 100644 --- a/src/main/sensors/current_ids.h +++ b/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; diff --git a/src/main/target/SPRACINGF3/target.h b/src/main/target/SPRACINGF3/target.h index 12542977eb..c1ab6f9830 100644 --- a/src/main/target/SPRACINGF3/target.h +++ b/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 diff --git a/src/main/target/SPRACINGF3EVO/target.h b/src/main/target/SPRACINGF3EVO/target.h index c9884f42f5..f4acbf5b98 100755 --- a/src/main/target/SPRACINGF3EVO/target.h +++ b/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 diff --git a/src/main/target/SPRACINGF4EVO/target.h b/src/main/target/SPRACINGF4EVO/target.h index f4a0bd2d8a..b3c70e5bac 100644 --- a/src/main/target/SPRACINGF4EVO/target.h +++ b/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