From 965793263d30450f324a25c411abbc2e5ee13b5b Mon Sep 17 00:00:00 2001
From: victor <victorkrook96@gmail.com>
Date: Fri, 9 Dec 2022 16:10:26 +0100
Subject: [PATCH 1/7] Started on implementation for BLHeli config support

---
 src/modules/src/msp.c                 | 224 ++++++++++++++++++++------
 src/modules/src/vcp_esc_passthrough.c |  37 ++++-
 2 files changed, 215 insertions(+), 46 deletions(-)

diff --git a/src/modules/src/msp.c b/src/modules/src/msp.c
index 828fc29a05..81fa98cdc0 100644
--- a/src/modules/src/msp.c
+++ b/src/modules/src/msp.c
@@ -36,10 +36,22 @@
 #include "commander.h"
 
 // MSP command IDs
-#define MSP_STATUS    101
-#define MSP_RC        105
-#define MSP_ATTITUDE  108
-#define MSP_BOXIDS    119
+typedef enum {
+  MSP_API_VERSION = 1,
+  MSP_FC_VARIANT  = 2,
+  MSP_FC_VERSION  = 3,
+  MSP_BOARD_INFO  = 4,
+  MSP_BUILD_INFO  = 5,
+
+  MSP_STATUS      = 101,
+  MSP_RC          = 105,
+  MSP_ATTITUDE    = 108,
+  MSP_BOXIDS      = 119,
+
+  MSP_UID         = 160, // Unique device ID
+
+  MSP_SET_4WAY_IF = 245
+} msp_command_t;
 
 // Misc MSP header defines
 #define MSP_PREAMBLE_0    '$'
@@ -90,19 +102,60 @@ typedef struct _MspRc
   uint16_t roll;      // Range [1000,2000]
   uint16_t pitch;     // Range [1000,2000]
   uint16_t yaw;       // Range [1000,2000]
-  uint16_t throttle;  // Range [1000,2000] 
+  uint16_t throttle;  // Range [1000,2000]
 }__attribute__((packed)) MspRc;
 
+typedef struct _MspApiVersion
+{
+  uint8_t protocolVersion;
+  uint8_t apiVersion[2];
+}__attribute__((packed)) MspApiVersion;
+
+typedef struct _MspFcVariant
+{
+  char variant[4];
+}__attribute__((packed)) MspFcVariant;
+
+typedef struct _MspFcVersion
+{
+  uint8_t version[3];
+}__attribute__((packed)) MspFcVersion;
+
+typedef struct _MspBoardInfo
+{
+  char board_info[4];
+  uint8_t board_version[2];
+}__attribute__((packed)) MspBoardInfo;
+
+typedef struct _MspBuildInfo
+{
+  char date[11];
+  char time[8];
+}__attribute__((packed)) MspBuildInfo;
+
+typedef struct _MspUid
+{
+  uint32_t uid[3];
+}__attribute__((packed)) MspUid;
+
 // Helpers
 static uint8_t mspComputeCrc(uint8_t* pBuffer, uint32_t bufferLen);
 static bool mspIsRequestValid(MspObject* pMspObject);
 static void mspProcessRequest(MspObject* pMspObject);
+static void mspMakeTxPacket(MspObject* pMspObject, const msp_command_t command, const uint8_t* data, uint8_t dataLen);
+static void mspTxPacketToBuf(uint8_t* buf, const MspObject* pMspObject);
 
 // Request handlers
 static void mspHandleRequestMspStatus(MspObject* pMspObject);
 static void mspHandleRequestMspRc(MspObject* pMspObject);
 static void mspHandleRequestMspAttitude(MspObject* pMspObject);
 static void mspHandleRequestMspBoxIds(MspObject* pMspObject);
+static void mspHandleRequestsApiVersion(MspObject* pMspObject);
+static void mspHandleRequestsFcVariant(MspObject* pMspObject);
+static void mspHandleRequestsFcVersion(MspObject* pMspObject);
+static void mspHandleRequestsBoardInfo(MspObject* pMspObject);
+static void mspHandleRequestsBuildInfo(MspObject* pMspObject);
+static void mspHandleRequestsUiid(MspObject* pMspObject);
 
 void mspInit(MspObject* pMspObject, const MspResponseCallback callback)
 {
@@ -229,48 +282,51 @@ void mspProcessRequest(MspObject* pMspObject)
     return;
   }
 
+  DEBUG_PRINT("Request: %d\n", pMspObject->requestHeader.command);
+
   switch(pMspObject->requestHeader.command)
   {
-  case MSP_STATUS:
-    mspHandleRequestMspStatus(pMspObject);
-
-    if(pMspObject->responseCallback)
-    {
-      pMspObject->responseCallback(pMspObject->mspResponse, pMspObject->mspResponseSize);
-    }
-    break;
-
-  case MSP_RC:
-    mspHandleRequestMspRc(pMspObject);
-
-    if(pMspObject->responseCallback)
-    {
-      pMspObject->responseCallback(pMspObject->mspResponse, pMspObject->mspResponseSize);
-    }
-    break;
-
-  case MSP_ATTITUDE:
-    mspHandleRequestMspAttitude(pMspObject);
-
-    if(pMspObject->responseCallback)
-    {
-      pMspObject->responseCallback(pMspObject->mspResponse, pMspObject->mspResponseSize);
-    }
-    break;
-
-  case MSP_BOXIDS:
-    mspHandleRequestMspBoxIds(pMspObject);
-    
-    if(pMspObject->responseCallback)
-    {
-      pMspObject->responseCallback(pMspObject->mspResponse, pMspObject->mspResponseSize);
-    }
-    break;
+    case MSP_STATUS:
+      mspHandleRequestMspStatus(pMspObject);
+      break;
+    case MSP_RC:
+      mspHandleRequestMspRc(pMspObject);
+      break;
+    case MSP_ATTITUDE:
+      mspHandleRequestMspAttitude(pMspObject);
+      break;
+    case MSP_BOXIDS:
+      mspHandleRequestMspBoxIds(pMspObject);
+      break;
+    case MSP_API_VERSION:
+      mspHandleRequestsApiVersion(pMspObject);
+      break;
+    case MSP_FC_VARIANT:
+      mspHandleRequestsFcVariant(pMspObject);
+      break;
+    case MSP_FC_VERSION:
+      mspHandleRequestsFcVersion(pMspObject);
+      break;
+    case MSP_BOARD_INFO:
+      mspHandleRequestsBoardInfo(pMspObject);
+      break;
+    case MSP_BUILD_INFO:
+      mspHandleRequestsBuildInfo(pMspObject);
+      break;
+    case MSP_UID:
+      mspHandleRequestsUiid(pMspObject);
+      break;
+    default:
+      DEBUG_PRINT("Received unsupported MSP request: %d\n", pMspObject->requestHeader.command);
+      return;
+      break;
+  }
 
-  default:
-    DEBUG_PRINT("Received unsupported MSP request: %d\n", pMspObject->requestHeader.command);
-    break;
+  if(pMspObject->responseCallback)
+  {
+    pMspObject->responseCallback(pMspObject->mspResponse, pMspObject->mspResponseSize);
   }
+
 }
 
 void mspHandleRequestMspStatus(MspObject* pMspObject)
@@ -368,9 +424,9 @@ static void mspHandleRequestMspBoxIds(MspObject* pMspObject)
   pHeader->direction = MSP_DIRECTION_OUT;
   pHeader->size = sizeof(*pData);
   pHeader->command = MSP_BOXIDS;
-  
+
   // TODO: Data - this needs to be properly implemented
-  // For now, we just return byte 0 = 0 which tells 
+  // For now, we just return byte 0 = 0 which tells
   // the client to use box ID 0 for the ARMED box
   pData[0] = 0x00;
 
@@ -380,3 +436,81 @@ static void mspHandleRequestMspBoxIds(MspObject* pMspObject)
   // Update total response size
   pMspObject->mspResponseSize = sizeof(MspHeader) + sizeof(*pData) + 1;
 }
+
+static void mspHandleRequestsApiVersion(MspObject* pMspObject)
+{
+  MspApiVersion* apiVersion = (MspApiVersion*)(pMspObject->mspResponse + sizeof(MspHeader));
+  apiVersion->protocolVersion = 2;
+  apiVersion->apiVersion[0] = 3;
+  mspMakeTxPacket(pMspObject, MSP_API_VERSION, apiVersion, sizeof(MspApiVersion));
+}
+
+static void mspHandleRequestsFcVariant(MspObject* pMspObject)
+{
+  MspFcVariant* fcVariant = (MspFcVariant*)(pMspObject->mspResponse + sizeof(MspHeader));
+  fcVariant->variant[0] = 'C';
+  fcVariant->variant[1] = 'F';
+  fcVariant->variant[2] = '2';
+  mspMakeTxPacket(pMspObject, MSP_FC_VARIANT, fcVariant, sizeof(MspFcVariant));
+}
+
+static void mspHandleRequestsFcVersion(MspObject* pMspObject)
+{
+  MspFcVersion* fcVersion = (MspFcVersion*)(pMspObject->mspResponse + sizeof(MspHeader));
+  mspMakeTxPacket(pMspObject, MSP_FC_VERSION, fcVersion, sizeof(MspFcVersion));
+}
+
+static void mspHandleRequestsBoardInfo(MspObject* pMspObject)
+{
+  MspBoardInfo* boardInfo = (MspBoardInfo*)(pMspObject->mspResponse + sizeof(MspHeader));
+  mspMakeTxPacket(pMspObject, MSP_BOARD_INFO, boardInfo, sizeof(MspBoardInfo));
+}
+
+static void mspHandleRequestsBuildInfo(MspObject* pMspObject)
+{
+  MspBuildInfo* buildInfo = (MspBuildInfo*)(pMspObject->mspResponse + sizeof(MspHeader));
+  mspMakeTxPacket(pMspObject, MSP_BUILD_INFO, buildInfo, sizeof(MspBuildInfo));
+}
+
+static void mspHandleRequestsUiid(MspObject* pMspObject)
+{
+  MspUid* uuid = (MspUid*)(pMspObject->mspResponse + sizeof(MspHeader));
+  mspMakeTxPacket(pMspObject, MSP_UID, uuid, sizeof(MspUid));
+}
+
+static void mspMakeTxPacket(MspObject* pMspObject, const msp_command_t command, const uint8_t* data, uint8_t dataLen) {
+  MspHeader* pHeader = (MspHeader*)pMspObject->mspResponse;
+  uint8_t* pData = (uint8_t*)(pMspObject->mspResponse + sizeof(MspHeader));
+  uint8_t* pCrc = (uint8_t*)(pMspObject->mspResponse + sizeof(MspHeader) + dataLen);
+
+  pHeader->preamble[0] = '$';
+  pHeader->preamble[1] = 'M';
+  pHeader->direction = '>';
+  pHeader->size = dataLen;
+  pHeader->command = command;
+  memcpy(pData, data, dataLen);
+
+  uint8_t checksum = pHeader->size ^ pHeader->command;
+  for (int i = 0; i < dataLen; i++) {
+      checksum ^= pData[i];
+  }
+  *pCrc = checksum;
+
+  // Update the packets entire size. +1 is the CRC
+  pMspObject->mspResponseSize = sizeof(MspHeader) + dataLen + 1;
+}
+
+static void mspTxPacketToBuf(uint8_t* buf, const MspObject* pMspObject) {
+  uint8_t* pData = (uint8_t*)(pMspObject->mspResponse + sizeof(MspHeader));
+
+  // Header: <preamble>,<direction>,<size>,<command>
+  buf[0] = pMspObject->requestHeader.preamble[0];
+  buf[1] = pMspObject->requestHeader.preamble[1];
+  buf[2] = pMspObject->requestHeader.direction;
+  buf[3] = pMspObject->requestHeader.size;
+  buf[4] = pMspObject->requestHeader.command;
+  // Data
+  memcpy(&buf[5], pData, pMspObject->requestHeader.size);
+  // CRC
+  buf[5 + pMspObject->requestHeader.size] = pMspObject->requestCrc;
+}
diff --git a/src/modules/src/vcp_esc_passthrough.c b/src/modules/src/vcp_esc_passthrough.c
index 906f3bd9b7..172cc41882 100644
--- a/src/modules/src/vcp_esc_passthrough.c
+++ b/src/modules/src/vcp_esc_passthrough.c
@@ -38,8 +38,10 @@
 #include "usb.h"
 #include "motors.h"
 #include "serial_4way.h"
+#include "msp.h"
 #include "uart_syslink.h"
 #include "sensors.h"
+#include "debug.h"
 
 static TaskHandle_t passthroughTaskHandle;
 STATIC_MEM_TASK_ALLOC(passthroughTask, PASSTHROUGH_TASK_STACKSIZE);
@@ -102,17 +104,48 @@ void passthroughVcpTxSend(uint8_t Ch)
   ASSERT(xQueueSend(ptTxQueue, &Ch, 0) == pdTRUE);
 }
 
-int  passthroughVcpTxReceiveFromISR(uint8_t* receiveChPtr)
+void passthroughVcpTxSendBlock(uint8_t Ch)
+{
+  ASSERT(xQueueSend(ptTxQueue, &Ch, portMAX_DELAY) == pdTRUE);
+}
+
+int passthroughVcpTxReceiveFromISR(uint8_t* receiveChPtr)
 {
   BaseType_t xHigherPriorityTaskWoken;
   return xQueueReceiveFromISR(ptTxQueue, receiveChPtr, &xHigherPriorityTaskWoken);
 }
 
+// MSP STUFF
+static uint8_t ReadByte(void)
+{
+    uint8_t byte;
+    passthroughVcpRxReceiveBlock(&byte);
+    return byte;
+}
+void mspCb(uint8_t* pBuffer, uint32_t bufferLen) {
+  // Sent all data through serial
+  DEBUG_PRINT("TX: ");
+  for (int i = 0; i < bufferLen; i++) {
+    uint8_t byte = pBuffer[i];
+    DEBUG_PRINT("%02d ", byte);
+    passthroughVcpTxSendBlock(byte);
+  }
+  DEBUG_PRINT("\n");
+}
+
+static MspObject pMspObject;
 
 void passthroughTask(void *param)
 {
   systemWaitStart();
 
+  mspInit(&pMspObject, mspCb);
+  while (true) {
+    uint8_t byte = ReadByte();
+    DEBUG_PRINT("%02d ", byte);
+    mspProcessByte(&pMspObject, byte);
+  }
+
   while (true)
   {
     // Wait for interface to be activated, typically when ACM or COM port control message is sent
@@ -122,6 +155,8 @@ void passthroughTask(void *param)
     uartslkPauseRx();
     sensorsSuspend();
 
+    //mspInit();
+
     // Suspend motor signal output and init them as normal GPIO
     esc4wayInit();
     // Run the 4way process

From d81461a07757dcc858dda78806577bcc767790f8 Mon Sep 17 00:00:00 2001
From: victor <victorkrook96@gmail.com>
Date: Mon, 12 Dec 2022 14:35:26 +0100
Subject: [PATCH 2/7] Made it possible to connect to BLHeli configurator, and
 configure ESCS!

---
 src/modules/interface/msp.h           |  4 ++
 src/modules/src/msp.c                 | 34 ++++++++++---
 src/modules/src/vcp_esc_passthrough.c | 73 +++++++++++++++++----------
 3 files changed, 77 insertions(+), 34 deletions(-)

diff --git a/src/modules/interface/msp.h b/src/modules/interface/msp.h
index 875899c2c0..c8d75f2d58 100644
--- a/src/modules/interface/msp.h
+++ b/src/modules/interface/msp.h
@@ -93,4 +93,8 @@ void mspInit(MspObject* pMspObject, const MspResponseCallback callback);
  */
 void mspProcessByte(MspObject* pMspObject, const uint8_t data);
 
+/*
+*/
+bool mspHasSet4WayIf();
+
 #endif /* MSP_H_ */
diff --git a/src/modules/src/msp.c b/src/modules/src/msp.c
index 81fa98cdc0..67e76ff1e6 100644
--- a/src/modules/src/msp.c
+++ b/src/modules/src/msp.c
@@ -138,6 +138,13 @@ typedef struct _MspUid
   uint32_t uid[3];
 }__attribute__((packed)) MspUid;
 
+typedef struct _MspSet4WayIf
+{
+  uint8_t connectedEscs;
+}__attribute__((packed)) MspSet4WayIf;
+
+static bool _hasSet4WayIf = false;
+
 // Helpers
 static uint8_t mspComputeCrc(uint8_t* pBuffer, uint32_t bufferLen);
 static bool mspIsRequestValid(MspObject* pMspObject);
@@ -156,7 +163,9 @@ static void mspHandleRequestsFcVersion(MspObject* pMspObject);
 static void mspHandleRequestsBoardInfo(MspObject* pMspObject);
 static void mspHandleRequestsBuildInfo(MspObject* pMspObject);
 static void mspHandleRequestsUiid(MspObject* pMspObject);
+static void mspHandleRequestsSet4WayIf(MspObject* pMspObject);
 
+// Public API
 void mspInit(MspObject* pMspObject, const MspResponseCallback callback)
 {
   pMspObject->requestState = MSP_REQUEST_STATE_WAIT_FOR_START;
@@ -211,7 +220,12 @@ void mspProcessByte(MspObject* pMspObject, const uint8_t data)
   }
 }
 
-uint8_t mspComputeCrc(uint8_t* pBuffer, uint32_t bufferLen)
+bool mspHasSet4WayIf()
+{
+  return _hasSet4WayIf;
+}
+
+// Private
 {
   uint8_t crc = 0;
 
@@ -299,6 +313,7 @@ void mspProcessRequest(MspObject* pMspObject)
       mspHandleRequestMspBoxIds(pMspObject);
       break;
     case MSP_API_VERSION:
+      _hasSet4WayIf = false;
       mspHandleRequestsApiVersion(pMspObject);
       break;
     case MSP_FC_VARIANT:
@@ -316,6 +331,10 @@ void mspProcessRequest(MspObject* pMspObject)
     case MSP_UID:
       mspHandleRequestsUiid(pMspObject);
       break;
+    case MSP_SET_4WAY_IF:
+      mspHandleRequestsSet4WayIf(pMspObject);
+      _hasSet4WayIf = true;
+      break;
     default:
       DEBUG_PRINT("Received unsupported MSP request: %d\n", pMspObject->requestHeader.command);
       return;
@@ -478,6 +497,13 @@ static void mspHandleRequestsUiid(MspObject* pMspObject)
   mspMakeTxPacket(pMspObject, MSP_UID, uuid, sizeof(MspUid));
 }
 
+static void mspHandleRequestsSet4WayIf(MspObject* pMspObject)
+{
+  MspSet4WayIf* set4WayIf = (MspSet4WayIf*)(pMspObject->mspResponse + sizeof(MspHeader));
+  set4WayIf->connectedEscs = 4;
+  mspMakeTxPacket(pMspObject, MSP_SET_4WAY_IF, set4WayIf, sizeof(MspSet4WayIf));
+}
+
 static void mspMakeTxPacket(MspObject* pMspObject, const msp_command_t command, const uint8_t* data, uint8_t dataLen) {
   MspHeader* pHeader = (MspHeader*)pMspObject->mspResponse;
   uint8_t* pData = (uint8_t*)(pMspObject->mspResponse + sizeof(MspHeader));
@@ -490,11 +516,7 @@ static void mspMakeTxPacket(MspObject* pMspObject, const msp_command_t command,
   pHeader->command = command;
   memcpy(pData, data, dataLen);
 
-  uint8_t checksum = pHeader->size ^ pHeader->command;
-  for (int i = 0; i < dataLen; i++) {
-      checksum ^= pData[i];
-  }
-  *pCrc = checksum;
+  *pCrc = mspComputeCrc(pMspObject->mspResponse, sizeof(pMspObject->mspResponse));
 
   // Update the packets entire size. +1 is the CRC
   pMspObject->mspResponseSize = sizeof(MspHeader) + dataLen + 1;
diff --git a/src/modules/src/vcp_esc_passthrough.c b/src/modules/src/vcp_esc_passthrough.c
index 172cc41882..ebd70ba310 100644
--- a/src/modules/src/vcp_esc_passthrough.c
+++ b/src/modules/src/vcp_esc_passthrough.c
@@ -115,48 +115,24 @@ int passthroughVcpTxReceiveFromISR(uint8_t* receiveChPtr)
   return xQueueReceiveFromISR(ptTxQueue, receiveChPtr, &xHigherPriorityTaskWoken);
 }
 
-// MSP STUFF
-static uint8_t ReadByte(void)
-{
-    uint8_t byte;
-    passthroughVcpRxReceiveBlock(&byte);
-    return byte;
-}
-void mspCb(uint8_t* pBuffer, uint32_t bufferLen) {
-  // Sent all data through serial
-  DEBUG_PRINT("TX: ");
-  for (int i = 0; i < bufferLen; i++) {
-    uint8_t byte = pBuffer[i];
-    DEBUG_PRINT("%02d ", byte);
-    passthroughVcpTxSendBlock(byte);
-  }
-  DEBUG_PRINT("\n");
-}
-
-static MspObject pMspObject;
+static void blHeliConfigHandshake();
 
 void passthroughTask(void *param)
 {
   systemWaitStart();
 
-  mspInit(&pMspObject, mspCb);
-  while (true) {
-    uint8_t byte = ReadByte();
-    DEBUG_PRINT("%02d ", byte);
-    mspProcessByte(&pMspObject, byte);
-  }
-
   while (true)
   {
     // Wait for interface to be activated, typically when ACM or COM port control message is sent
     ulTaskNotifyTake(pdTRUE, portMAX_DELAY);
 
+    // Before we start the 4way process, we must perform a "handshake" with the blheli configurator.
+    blHeliConfigHandshake();
+
     // ESC 1-wire interface is bit-banging so some interrupts must be suspended
     uartslkPauseRx();
     sensorsSuspend();
 
-    //mspInit();
-
     // Suspend motor signal output and init them as normal GPIO
     esc4wayInit();
     // Run the 4way process
@@ -171,3 +147,44 @@ void passthroughTask(void *param)
   }
 }
 
+
+static uint8_t ReadByte()
+{
+    uint8_t byte;
+    passthroughVcpRxReceiveBlock(&byte);
+    return byte;
+}
+
+static void mspCallback(uint8_t* pBuffer, uint32_t bufferLen)
+{
+  // Sent all data through serial
+  for (int i = 0; i < bufferLen; i++)
+  {
+    uint8_t byte = pBuffer[i];
+    passthroughVcpTxSendBlock(byte);
+  }
+}
+
+/*
+ * Performs a "handshake" that BLHeli Configurator uses during the connection.
+ * This "handshake" is done by sending some special MSP commands and responses.
+ * This method blocks until the handshake is complete.
+ */
+static void blHeliConfigHandshake()
+{
+  static bool isInit = false;
+  static MspObject pMspObject;
+
+  if (!isInit)
+  {
+    isInit = true;
+    mspInit(&pMspObject, mspCallback);
+  }
+
+  while (!mspHasSet4WayIf())
+  {
+    uint8_t byte = ReadByte();
+    mspProcessByte(&pMspObject, byte);
+    vTaskDelay(M2T(2));
+  }
+}

From 4038acae2fe7fe97b3ed418eab6694d286309883 Mon Sep 17 00:00:00 2001
From: victor <victorkrook96@gmail.com>
Date: Mon, 12 Dec 2022 15:03:11 +0100
Subject: [PATCH 3/7] Added some more appropiate information to send during
 handshake.

---
 src/modules/src/msp.c | 37 +++++++++++++++++--------------------
 1 file changed, 17 insertions(+), 20 deletions(-)

diff --git a/src/modules/src/msp.c b/src/modules/src/msp.c
index 67e76ff1e6..a1be29ae0c 100644
--- a/src/modules/src/msp.c
+++ b/src/modules/src/msp.c
@@ -34,6 +34,8 @@
 #include "debug.h"
 #include "sensfusion6.h"
 #include "commander.h"
+#include "version.h"
+#include "motors.h"
 
 // MSP command IDs
 typedef enum {
@@ -150,7 +152,6 @@ static uint8_t mspComputeCrc(uint8_t* pBuffer, uint32_t bufferLen);
 static bool mspIsRequestValid(MspObject* pMspObject);
 static void mspProcessRequest(MspObject* pMspObject);
 static void mspMakeTxPacket(MspObject* pMspObject, const msp_command_t command, const uint8_t* data, uint8_t dataLen);
-static void mspTxPacketToBuf(uint8_t* buf, const MspObject* pMspObject);
 
 // Request handlers
 static void mspHandleRequestMspStatus(MspObject* pMspObject);
@@ -458,6 +459,7 @@ static void mspHandleRequestMspBoxIds(MspObject* pMspObject)
 
 static void mspHandleRequestsApiVersion(MspObject* pMspObject)
 {
+  // TODO: Not sure what version we're really using... Most of the protocol was simply reverse engineered from BLHeli Configurator.
   MspApiVersion* apiVersion = (MspApiVersion*)(pMspObject->mspResponse + sizeof(MspHeader));
   apiVersion->protocolVersion = 2;
   apiVersion->apiVersion[0] = 3;
@@ -467,44 +469,54 @@ static void mspHandleRequestsApiVersion(MspObject* pMspObject)
 static void mspHandleRequestsFcVariant(MspObject* pMspObject)
 {
   MspFcVariant* fcVariant = (MspFcVariant*)(pMspObject->mspResponse + sizeof(MspHeader));
-  fcVariant->variant[0] = 'C';
-  fcVariant->variant[1] = 'F';
-  fcVariant->variant[2] = '2';
+  memcpy(fcVariant->variant, "CF2 ", 4);
   mspMakeTxPacket(pMspObject, MSP_FC_VARIANT, fcVariant, sizeof(MspFcVariant));
 }
 
 static void mspHandleRequestsFcVersion(MspObject* pMspObject)
 {
   MspFcVersion* fcVersion = (MspFcVersion*)(pMspObject->mspResponse + sizeof(MspHeader));
+  fcVersion->version[0] = 2;
+  fcVersion->version[1] = 1;
+  fcVersion->version[2] = 0;
   mspMakeTxPacket(pMspObject, MSP_FC_VERSION, fcVersion, sizeof(MspFcVersion));
 }
 
 static void mspHandleRequestsBoardInfo(MspObject* pMspObject)
 {
   MspBoardInfo* boardInfo = (MspBoardInfo*)(pMspObject->mspResponse + sizeof(MspHeader));
+  memcpy(boardInfo->board_info, platformConfigGetDeviceTypeName(), 4);
+  boardInfo->board_version[0] = 2;
+  boardInfo->board_version[1] = 1;
   mspMakeTxPacket(pMspObject, MSP_BOARD_INFO, boardInfo, sizeof(MspBoardInfo));
 }
 
 static void mspHandleRequestsBuildInfo(MspObject* pMspObject)
 {
   MspBuildInfo* buildInfo = (MspBuildInfo*)(pMspObject->mspResponse + sizeof(MspHeader));
+  memcpy(buildInfo->date, V_STAG, 11);
+  memset(buildInfo->time, 0, 8);
   mspMakeTxPacket(pMspObject, MSP_BUILD_INFO, buildInfo, sizeof(MspBuildInfo));
 }
 
 static void mspHandleRequestsUiid(MspObject* pMspObject)
 {
   MspUid* uuid = (MspUid*)(pMspObject->mspResponse + sizeof(MspHeader));
+  uuid->uid[0] = *((int*)(MCU_ID_ADDRESS+8));
+  uuid->uid[1] = *((int*)(MCU_ID_ADDRESS+4));
+  uuid->uid[2] = *((int*)(MCU_ID_ADDRESS+0));
   mspMakeTxPacket(pMspObject, MSP_UID, uuid, sizeof(MspUid));
 }
 
 static void mspHandleRequestsSet4WayIf(MspObject* pMspObject)
 {
   MspSet4WayIf* set4WayIf = (MspSet4WayIf*)(pMspObject->mspResponse + sizeof(MspHeader));
-  set4WayIf->connectedEscs = 4;
+  set4WayIf->connectedEscs = NBR_OF_MOTORS;
   mspMakeTxPacket(pMspObject, MSP_SET_4WAY_IF, set4WayIf, sizeof(MspSet4WayIf));
 }
 
 static void mspMakeTxPacket(MspObject* pMspObject, const msp_command_t command, const uint8_t* data, uint8_t dataLen) {
+  // Packet structure: http://www.multiwii.com/wiki/index.php?title=Multiwii_Serial_Protocol
   MspHeader* pHeader = (MspHeader*)pMspObject->mspResponse;
   uint8_t* pData = (uint8_t*)(pMspObject->mspResponse + sizeof(MspHeader));
   uint8_t* pCrc = (uint8_t*)(pMspObject->mspResponse + sizeof(MspHeader) + dataLen);
@@ -521,18 +533,3 @@ static void mspMakeTxPacket(MspObject* pMspObject, const msp_command_t command,
   // Update the packets entire size. +1 is the CRC
   pMspObject->mspResponseSize = sizeof(MspHeader) + dataLen + 1;
 }
-
-static void mspTxPacketToBuf(uint8_t* buf, const MspObject* pMspObject) {
-  uint8_t* pData = (uint8_t*)(pMspObject->mspResponse + sizeof(MspHeader));
-
-  // Header: <preamble>,<direction>,<size>,<command>
-  buf[0] = pMspObject->requestHeader.preamble[0];
-  buf[1] = pMspObject->requestHeader.preamble[1];
-  buf[2] = pMspObject->requestHeader.direction;
-  buf[3] = pMspObject->requestHeader.size;
-  buf[4] = pMspObject->requestHeader.command;
-  // Data
-  memcpy(&buf[5], pData, pMspObject->requestHeader.size);
-  // CRC
-  buf[5 + pMspObject->requestHeader.size] = pMspObject->requestCrc;
-}

From c372a248d394b1bb705d290e4d1b2c7a91d07bb8 Mon Sep 17 00:00:00 2001
From: victor <victorkrook96@gmail.com>
Date: Mon, 12 Dec 2022 15:03:33 +0100
Subject: [PATCH 4/7] Added "static" to some functions that were only used
 privately.

---
 src/modules/src/msp.c | 9 +++++----
 1 file changed, 5 insertions(+), 4 deletions(-)

diff --git a/src/modules/src/msp.c b/src/modules/src/msp.c
index a1be29ae0c..12b26ed8ec 100644
--- a/src/modules/src/msp.c
+++ b/src/modules/src/msp.c
@@ -227,6 +227,7 @@ bool mspHasSet4WayIf()
 }
 
 // Private
+static uint8_t mspComputeCrc(uint8_t* pBuffer, uint32_t bufferLen)
 {
   uint8_t crc = 0;
 
@@ -254,7 +255,7 @@ bool mspHasSet4WayIf()
   return crc;
 }
 
-bool mspIsRequestValid(MspObject* pMspObject)
+static bool mspIsRequestValid(MspObject* pMspObject)
 {
   if(pMspObject->requestHeader.preamble[0] != MSP_PREAMBLE_0 ||
       pMspObject->requestHeader.preamble[1] != MSP_PREAMBLE_1)
@@ -289,7 +290,7 @@ bool mspIsRequestValid(MspObject* pMspObject)
   return true;
 }
 
-void mspProcessRequest(MspObject* pMspObject)
+static void mspProcessRequest(MspObject* pMspObject)
 {
   if(!mspIsRequestValid(pMspObject))
   {
@@ -349,7 +350,7 @@ void mspProcessRequest(MspObject* pMspObject)
 
 }
 
-void mspHandleRequestMspStatus(MspObject* pMspObject)
+static void mspHandleRequestMspStatus(MspObject* pMspObject)
 {
   MspHeader* pHeader = (MspHeader*)pMspObject->mspResponse;
   MspStatus* pData = (MspStatus*)(pMspObject->mspResponse + sizeof(MspHeader));
@@ -376,7 +377,7 @@ void mspHandleRequestMspStatus(MspObject* pMspObject)
   pMspObject->mspResponseSize = sizeof(MspHeader) + sizeof(*pData) + 1;
 }
 
-void mspHandleRequestMspRc(MspObject* pMspObject)
+static void mspHandleRequestMspRc(MspObject* pMspObject)
 {
   MspHeader* pHeader = (MspHeader*)pMspObject->mspResponse;
   MspRc* pData = (MspRc*)(pMspObject->mspResponse + sizeof(MspHeader));

From faec74c5e4a6a6b99e64e00452b59eb85a8d6400 Mon Sep 17 00:00:00 2001
From: victor <victorkrook96@gmail.com>
Date: Mon, 12 Dec 2022 15:18:14 +0100
Subject: [PATCH 5/7] Did minor clean up, eg namechange. Added missing include
 Added correct type-cast

---
 src/modules/interface/msp.h           |  4 +++-
 src/modules/src/msp.c                 | 30 ++++++++++++++-------------
 src/modules/src/vcp_esc_passthrough.c | 21 +++++++++----------
 3 files changed, 29 insertions(+), 26 deletions(-)

diff --git a/src/modules/interface/msp.h b/src/modules/interface/msp.h
index c8d75f2d58..6096d4f015 100644
--- a/src/modules/interface/msp.h
+++ b/src/modules/interface/msp.h
@@ -94,7 +94,9 @@ void mspInit(MspObject* pMspObject, const MspResponseCallback callback);
 void mspProcessByte(MspObject* pMspObject, const uint8_t data);
 
 /*
-*/
+ * Returns true if the MSP_SET_4WAY_IF request has been answered.
+ * This information can be useful to know whether the connection phase of BLHeli Configurator has been completed.
+ */
 bool mspHasSet4WayIf();
 
 #endif /* MSP_H_ */
diff --git a/src/modules/src/msp.c b/src/modules/src/msp.c
index 12b26ed8ec..fad80921e9 100644
--- a/src/modules/src/msp.c
+++ b/src/modules/src/msp.c
@@ -36,6 +36,8 @@
 #include "commander.h"
 #include "version.h"
 #include "motors.h"
+#include "string.h"
+#include "platform.h"
 
 // MSP command IDs
 typedef enum {
@@ -145,7 +147,7 @@ typedef struct _MspSet4WayIf
   uint8_t connectedEscs;
 }__attribute__((packed)) MspSet4WayIf;
 
-static bool _hasSet4WayIf = false;
+static bool hasSet4WayIf = false;
 
 // Helpers
 static uint8_t mspComputeCrc(uint8_t* pBuffer, uint32_t bufferLen);
@@ -223,7 +225,7 @@ void mspProcessByte(MspObject* pMspObject, const uint8_t data)
 
 bool mspHasSet4WayIf()
 {
-  return _hasSet4WayIf;
+  return hasSet4WayIf;
 }
 
 // Private
@@ -315,7 +317,7 @@ static void mspProcessRequest(MspObject* pMspObject)
       mspHandleRequestMspBoxIds(pMspObject);
       break;
     case MSP_API_VERSION:
-      _hasSet4WayIf = false;
+      hasSet4WayIf = false;
       mspHandleRequestsApiVersion(pMspObject);
       break;
     case MSP_FC_VARIANT:
@@ -335,7 +337,7 @@ static void mspProcessRequest(MspObject* pMspObject)
       break;
     case MSP_SET_4WAY_IF:
       mspHandleRequestsSet4WayIf(pMspObject);
-      _hasSet4WayIf = true;
+      hasSet4WayIf = true;
       break;
     default:
       DEBUG_PRINT("Received unsupported MSP request: %d\n", pMspObject->requestHeader.command);
@@ -464,14 +466,14 @@ static void mspHandleRequestsApiVersion(MspObject* pMspObject)
   MspApiVersion* apiVersion = (MspApiVersion*)(pMspObject->mspResponse + sizeof(MspHeader));
   apiVersion->protocolVersion = 2;
   apiVersion->apiVersion[0] = 3;
-  mspMakeTxPacket(pMspObject, MSP_API_VERSION, apiVersion, sizeof(MspApiVersion));
+  mspMakeTxPacket(pMspObject, MSP_API_VERSION, (uint8_t*) apiVersion, sizeof(MspApiVersion));
 }
 
 static void mspHandleRequestsFcVariant(MspObject* pMspObject)
 {
   MspFcVariant* fcVariant = (MspFcVariant*)(pMspObject->mspResponse + sizeof(MspHeader));
   memcpy(fcVariant->variant, "CF2 ", 4);
-  mspMakeTxPacket(pMspObject, MSP_FC_VARIANT, fcVariant, sizeof(MspFcVariant));
+  mspMakeTxPacket(pMspObject, MSP_FC_VARIANT, (uint8_t*) fcVariant, sizeof(MspFcVariant));
 }
 
 static void mspHandleRequestsFcVersion(MspObject* pMspObject)
@@ -480,7 +482,7 @@ static void mspHandleRequestsFcVersion(MspObject* pMspObject)
   fcVersion->version[0] = 2;
   fcVersion->version[1] = 1;
   fcVersion->version[2] = 0;
-  mspMakeTxPacket(pMspObject, MSP_FC_VERSION, fcVersion, sizeof(MspFcVersion));
+  mspMakeTxPacket(pMspObject, MSP_FC_VERSION, (uint8_t*) fcVersion, sizeof(MspFcVersion));
 }
 
 static void mspHandleRequestsBoardInfo(MspObject* pMspObject)
@@ -489,7 +491,7 @@ static void mspHandleRequestsBoardInfo(MspObject* pMspObject)
   memcpy(boardInfo->board_info, platformConfigGetDeviceTypeName(), 4);
   boardInfo->board_version[0] = 2;
   boardInfo->board_version[1] = 1;
-  mspMakeTxPacket(pMspObject, MSP_BOARD_INFO, boardInfo, sizeof(MspBoardInfo));
+  mspMakeTxPacket(pMspObject, MSP_BOARD_INFO, (uint8_t*) boardInfo, sizeof(MspBoardInfo));
 }
 
 static void mspHandleRequestsBuildInfo(MspObject* pMspObject)
@@ -497,7 +499,7 @@ static void mspHandleRequestsBuildInfo(MspObject* pMspObject)
   MspBuildInfo* buildInfo = (MspBuildInfo*)(pMspObject->mspResponse + sizeof(MspHeader));
   memcpy(buildInfo->date, V_STAG, 11);
   memset(buildInfo->time, 0, 8);
-  mspMakeTxPacket(pMspObject, MSP_BUILD_INFO, buildInfo, sizeof(MspBuildInfo));
+  mspMakeTxPacket(pMspObject, MSP_BUILD_INFO, (uint8_t*) buildInfo, sizeof(MspBuildInfo));
 }
 
 static void mspHandleRequestsUiid(MspObject* pMspObject)
@@ -506,14 +508,14 @@ static void mspHandleRequestsUiid(MspObject* pMspObject)
   uuid->uid[0] = *((int*)(MCU_ID_ADDRESS+8));
   uuid->uid[1] = *((int*)(MCU_ID_ADDRESS+4));
   uuid->uid[2] = *((int*)(MCU_ID_ADDRESS+0));
-  mspMakeTxPacket(pMspObject, MSP_UID, uuid, sizeof(MspUid));
+  mspMakeTxPacket(pMspObject, MSP_UID, (uint8_t*) uuid, sizeof(MspUid));
 }
 
 static void mspHandleRequestsSet4WayIf(MspObject* pMspObject)
 {
   MspSet4WayIf* set4WayIf = (MspSet4WayIf*)(pMspObject->mspResponse + sizeof(MspHeader));
   set4WayIf->connectedEscs = NBR_OF_MOTORS;
-  mspMakeTxPacket(pMspObject, MSP_SET_4WAY_IF, set4WayIf, sizeof(MspSet4WayIf));
+  mspMakeTxPacket(pMspObject, MSP_SET_4WAY_IF, (uint8_t*) set4WayIf, sizeof(MspSet4WayIf));
 }
 
 static void mspMakeTxPacket(MspObject* pMspObject, const msp_command_t command, const uint8_t* data, uint8_t dataLen) {
@@ -522,9 +524,9 @@ static void mspMakeTxPacket(MspObject* pMspObject, const msp_command_t command,
   uint8_t* pData = (uint8_t*)(pMspObject->mspResponse + sizeof(MspHeader));
   uint8_t* pCrc = (uint8_t*)(pMspObject->mspResponse + sizeof(MspHeader) + dataLen);
 
-  pHeader->preamble[0] = '$';
-  pHeader->preamble[1] = 'M';
-  pHeader->direction = '>';
+  pHeader->preamble[0] = MSP_PREAMBLE_0;
+  pHeader->preamble[1] = MSP_PREAMBLE_1;
+  pHeader->direction = MSP_DIRECTION_OUT;
   pHeader->size = dataLen;
   pHeader->command = command;
   memcpy(pData, data, dataLen);
diff --git a/src/modules/src/vcp_esc_passthrough.c b/src/modules/src/vcp_esc_passthrough.c
index ebd70ba310..61b753fdd7 100644
--- a/src/modules/src/vcp_esc_passthrough.c
+++ b/src/modules/src/vcp_esc_passthrough.c
@@ -41,7 +41,6 @@
 #include "msp.h"
 #include "uart_syslink.h"
 #include "sensors.h"
-#include "debug.h"
 
 static TaskHandle_t passthroughTaskHandle;
 STATIC_MEM_TASK_ALLOC(passthroughTask, PASSTHROUGH_TASK_STACKSIZE);
@@ -54,6 +53,14 @@ STATIC_MEM_QUEUE_ALLOC(ptRxQueue, 512, sizeof(uint8_t));
 static xQueueHandle  ptTxQueue;
 STATIC_MEM_QUEUE_ALLOC(ptTxQueue, 512, sizeof(uint8_t));
 
+// Helper
+/*
+ * Performs a "handshake" that BLHeli Configurator uses during the connection.
+ * This "handshake" is done by sending some special MSP commands and responses.
+ * This method blocks until the handshake is complete.
+ */
+static void blHeliConfigHandshake();
+
 void passthroughTask(void *param);
 
 void passthroughInit()
@@ -115,8 +122,6 @@ int passthroughVcpTxReceiveFromISR(uint8_t* receiveChPtr)
   return xQueueReceiveFromISR(ptTxQueue, receiveChPtr, &xHigherPriorityTaskWoken);
 }
 
-static void blHeliConfigHandshake();
-
 void passthroughTask(void *param)
 {
   systemWaitStart();
@@ -147,8 +152,7 @@ void passthroughTask(void *param)
   }
 }
 
-
-static uint8_t ReadByte()
+static uint8_t readByteBlocking()
 {
     uint8_t byte;
     passthroughVcpRxReceiveBlock(&byte);
@@ -165,11 +169,6 @@ static void mspCallback(uint8_t* pBuffer, uint32_t bufferLen)
   }
 }
 
-/*
- * Performs a "handshake" that BLHeli Configurator uses during the connection.
- * This "handshake" is done by sending some special MSP commands and responses.
- * This method blocks until the handshake is complete.
- */
 static void blHeliConfigHandshake()
 {
   static bool isInit = false;
@@ -183,7 +182,7 @@ static void blHeliConfigHandshake()
 
   while (!mspHasSet4WayIf())
   {
-    uint8_t byte = ReadByte();
+    uint8_t byte = readByteBlocking();
     mspProcessByte(&pMspObject, byte);
     vTaskDelay(M2T(2));
   }

From bb7e2530de55ca5b15daace5c9bc6d9ce189e6df Mon Sep 17 00:00:00 2001
From: victor <victorkrook96@gmail.com>
Date: Fri, 16 Dec 2022 15:00:40 +0100
Subject: [PATCH 6/7] Extended the MSP protocol to also accept some commands
 that are sent from ESC Configurator.

I also had to tweak the state-machine that processes incoming bytes.

The added request handlers needs to be filled in with correct data.
---
 src/modules/src/msp.c | 108 +++++++++++++++++++++++++++++++++++-------
 1 file changed, 90 insertions(+), 18 deletions(-)

diff --git a/src/modules/src/msp.c b/src/modules/src/msp.c
index fad80921e9..8babec5548 100644
--- a/src/modules/src/msp.c
+++ b/src/modules/src/msp.c
@@ -41,20 +41,20 @@
 
 // MSP command IDs
 typedef enum {
-  MSP_API_VERSION = 1,
-  MSP_FC_VARIANT  = 2,
-  MSP_FC_VERSION  = 3,
-  MSP_BOARD_INFO  = 4,
-  MSP_BUILD_INFO  = 5,
-
-  MSP_STATUS      = 101,
-  MSP_RC          = 105,
-  MSP_ATTITUDE    = 108,
-  MSP_BOXIDS      = 119,
-
-  MSP_UID         = 160, // Unique device ID
-
-  MSP_SET_4WAY_IF = 245
+  MSP_API_VERSION    = 1,
+  MSP_FC_VARIANT     = 2,
+  MSP_FC_VERSION     = 3,
+  MSP_BOARD_INFO     = 4,
+  MSP_BUILD_INFO     = 5,
+  MSP_FEATURE_CONFIG = 36,
+  MSP_STATUS         = 101,
+  MSP_MOTOR          = 104,
+  MSP_RC             = 105,
+  MSP_ATTITUDE       = 108,
+  MSP_BOXIDS         = 119,
+  MSP_BATTERY_STATE  = 130,
+  MSP_UID            = 160, // Unique device ID
+  MSP_SET_4WAY_IF    = 245
 } msp_command_t;
 
 // Misc MSP header defines
@@ -147,6 +147,28 @@ typedef struct _MspSet4WayIf
   uint8_t connectedEscs;
 }__attribute__((packed)) MspSet4WayIf;
 
+typedef struct _MspFeatures
+{
+  uint32_t featureBits;
+}__attribute__((packed)) MspFeatures;
+
+typedef struct _MspMotors
+{
+  uint16_t data[NBR_OF_MOTORS];
+}__attribute__((packed)) MspMotors;
+
+typedef struct _MspBatteryState
+{
+  uint8_t cellCount;
+  uint16_t capacity; // mAh
+  uint8_t voltage;   // V
+  uint16_t drawn;    // mAh
+  uint16_t amps;     // A
+  // '1.41.0',
+  uint8_t state;
+  uint16_t voltage2;
+}__attribute__((packed)) MspBatteryState;
+
 static bool hasSet4WayIf = false;
 
 // Helpers
@@ -167,6 +189,9 @@ static void mspHandleRequestsBoardInfo(MspObject* pMspObject);
 static void mspHandleRequestsBuildInfo(MspObject* pMspObject);
 static void mspHandleRequestsUiid(MspObject* pMspObject);
 static void mspHandleRequestsSet4WayIf(MspObject* pMspObject);
+static void mspHandleRequestMotor(MspObject* pMspObject);
+static void mspHandleRequestFeaturesConfig(MspObject* pMspObject);
+static void mspHandleRequestBatteryState(MspObject* pMspObject);
 
 // Public API
 void mspInit(MspObject* pMspObject, const MspResponseCallback callback)
@@ -177,9 +202,10 @@ void mspInit(MspObject* pMspObject, const MspResponseCallback callback)
 
 void mspProcessByte(MspObject* pMspObject, const uint8_t data)
 {
-  // If the start token is received at any time,
-  // immediately transition back to the first state
-  if(data == MSP_PREAMBLE_0)
+  // If the start token is received when we're in idle state,
+  // we transition to the first state
+  if((pMspObject->requestState == MSP_REQUEST_STATE_WAIT_FOR_START) &&
+     (data == MSP_PREAMBLE_0))
   {
     pMspObject->requestState = MSP_REQUEST_STATE_PREAMBLE;
     pMspObject->requestHeader.preamble[0] = data;
@@ -218,7 +244,7 @@ void mspProcessByte(MspObject* pMspObject, const uint8_t data)
     // Have a completed request
     mspProcessRequest(pMspObject);
 
-    pMspObject->requestState = MSP_REQUEST_STATE_WAIT_FOR_START;
+    //pMspObject->requestState = MSP_REQUEST_STATE_WAIT_FOR_START;
     break;
   }
 }
@@ -339,6 +365,15 @@ static void mspProcessRequest(MspObject* pMspObject)
       mspHandleRequestsSet4WayIf(pMspObject);
       hasSet4WayIf = true;
       break;
+    case MSP_MOTOR:
+      mspHandleRequestMotor(pMspObject);
+      break;
+    case MSP_FEATURE_CONFIG:
+      mspHandleRequestFeaturesConfig(pMspObject);
+      break;
+    case MSP_BATTERY_STATE:
+      mspHandleRequestBatteryState(pMspObject);
+      break;
     default:
       DEBUG_PRINT("Received unsupported MSP request: %d\n", pMspObject->requestHeader.command);
       return;
@@ -350,6 +385,8 @@ static void mspProcessRequest(MspObject* pMspObject)
     pMspObject->responseCallback(pMspObject->mspResponse, pMspObject->mspResponseSize);
   }
 
+  // Once we've responded, we'll go back to idle state, waiting for start.
+  pMspObject->requestState = MSP_REQUEST_STATE_WAIT_FOR_START;
 }
 
 static void mspHandleRequestMspStatus(MspObject* pMspObject)
@@ -460,6 +497,10 @@ static void mspHandleRequestMspBoxIds(MspObject* pMspObject)
   pMspObject->mspResponseSize = sizeof(MspHeader) + sizeof(*pData) + 1;
 }
 
+// Note: All request-handlers below have been reverese-engineered from BLHeli Configurator: https://github.com/stylesuxx/esc-configurator
+// and ESC Configurator: https://github.com/blheli-configurator/blheli-configurator
+// since there seems to be no good and valid specification for MSP extensions.
+
 static void mspHandleRequestsApiVersion(MspObject* pMspObject)
 {
   // TODO: Not sure what version we're really using... Most of the protocol was simply reverse engineered from BLHeli Configurator.
@@ -518,6 +559,37 @@ static void mspHandleRequestsSet4WayIf(MspObject* pMspObject)
   mspMakeTxPacket(pMspObject, MSP_SET_4WAY_IF, (uint8_t*) set4WayIf, sizeof(MspSet4WayIf));
 }
 
+static void mspHandleRequestMotor(MspObject* pMspObject)
+{
+  MspMotors* motorData = (MspMotors*)(pMspObject->mspResponse + sizeof(MspHeader));
+  for (int i = 0; i < NBR_OF_MOTORS; i++)
+  {
+    motorData->data[i] = 1000;
+  }
+  mspMakeTxPacket(pMspObject, MSP_MOTOR, (uint8_t*) motorData, sizeof(MspMotors));
+}
+
+static void mspHandleRequestFeaturesConfig(MspObject* pMspObject)
+{
+  MspFeatures* features = (MspFeatures*)(pMspObject->mspResponse + sizeof(MspHeader));
+  features->featureBits = 0;
+  mspMakeTxPacket(pMspObject, MSP_FEATURE_CONFIG, (uint8_t*) features, sizeof(MspFeatures));
+}
+
+static void mspHandleRequestBatteryState(MspObject* pMspObject)
+{
+  MspBatteryState* batteryState = (MspBatteryState*)(pMspObject->mspResponse + sizeof(MspHeader));
+  batteryState->cellCount = 1;
+  batteryState->capacity = 350;
+  batteryState->voltage = 34;
+  batteryState->drawn = 0;
+  batteryState->amps = 0;
+
+  batteryState->state = 0;
+  batteryState->voltage2 = 0;
+  mspMakeTxPacket(pMspObject, MSP_BATTERY_STATE, (uint8_t*) batteryState, sizeof(MspBatteryState));
+}
+
 static void mspMakeTxPacket(MspObject* pMspObject, const msp_command_t command, const uint8_t* data, uint8_t dataLen) {
   // Packet structure: http://www.multiwii.com/wiki/index.php?title=Multiwii_Serial_Protocol
   MspHeader* pHeader = (MspHeader*)pMspObject->mspResponse;

From 2d43a4d53ca18109f4a574653c10b1527f941318 Mon Sep 17 00:00:00 2001
From: victor <victorkrook96@gmail.com>
Date: Fri, 16 Dec 2022 15:01:21 +0100
Subject: [PATCH 7/7] Added reset API to msp state to machine

---
 src/modules/interface/msp.h           | 3 +++
 src/modules/src/msp.c                 | 5 +++++
 src/modules/src/vcp_esc_passthrough.c | 3 ++-
 3 files changed, 10 insertions(+), 1 deletion(-)

diff --git a/src/modules/interface/msp.h b/src/modules/interface/msp.h
index 6096d4f015..acfd3c30a2 100644
--- a/src/modules/interface/msp.h
+++ b/src/modules/interface/msp.h
@@ -99,4 +99,7 @@ void mspProcessByte(MspObject* pMspObject, const uint8_t data);
  */
 bool mspHasSet4WayIf();
 
+
+void mspResetSet4WayIf();
+
 #endif /* MSP_H_ */
diff --git a/src/modules/src/msp.c b/src/modules/src/msp.c
index 8babec5548..35a8702307 100644
--- a/src/modules/src/msp.c
+++ b/src/modules/src/msp.c
@@ -254,6 +254,11 @@ bool mspHasSet4WayIf()
   return hasSet4WayIf;
 }
 
+void mspResetSet4WayIf()
+{
+  hasSet4WayIf = false;
+}
+
 // Private
 static uint8_t mspComputeCrc(uint8_t* pBuffer, uint32_t bufferLen)
 {
diff --git a/src/modules/src/vcp_esc_passthrough.c b/src/modules/src/vcp_esc_passthrough.c
index 61b753fdd7..b9e3cb9036 100644
--- a/src/modules/src/vcp_esc_passthrough.c
+++ b/src/modules/src/vcp_esc_passthrough.c
@@ -184,6 +184,7 @@ static void blHeliConfigHandshake()
   {
     uint8_t byte = readByteBlocking();
     mspProcessByte(&pMspObject, byte);
-    vTaskDelay(M2T(2));
   }
+
+  mspResetSet4WayIf();
 }