From 22147105fb1ac956b17a9561494a5a6d61b3ed90 Mon Sep 17 00:00:00 2001 From: Hydra Date: Sun, 23 Apr 2017 20:39:24 +0100 Subject: [PATCH] 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 | 32 ++++++++++++++--- src/main/fc/fc_msp.h | 2 ++ src/main/fc/fc_tasks.c | 11 +++++- src/main/io/displayport_msp.c | 2 +- src/main/io/osd_slave.c | 3 ++ src/main/io/osd_slave.h | 2 ++ src/main/msp/msp.h | 6 ++++ src/main/msp/msp_protocol.h | 2 +- src/main/msp/msp_serial.c | 23 +++++++++--- src/main/msp/msp_serial.h | 3 +- src/main/sensors/battery.c | 15 ++++++++ src/main/sensors/current.c | 49 +++++++++++++++++++++++++- src/main/sensors/current.h | 23 ++++++++++++ src/main/sensors/current_ids.h | 3 ++ src/main/target/SPRACINGF3/target.h | 2 ++ src/main/target/SPRACINGF3EVO/target.h | 2 ++ src/main/target/SPRACINGF4EVO/target.h | 1 + 17 files changed, 166 insertions(+), 15 deletions(-) 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