Added support to set motor power through MSP (for instance with ESC C…

…onfigurator) - Two public functions were added to motors.h and motors.c to enable this. - msp.c was also extended to support command MSP_SET_MOTOR - Fixed the msp request state machine to support data in requests, which was not supported prior to this. - Fixed function mspIsRequestValid() to no longer discard packets with payload data, since certain requests (like MSP_SET_MOTOR) DOES contain data in the packet. - Refactored old functions to use helper-function mspMakeTxPacket() instead of manually assembling packets every time.
bitcraze · Feb 5, 2023 · e73dddf · e73dddf
1 parent 8f42a5d
commit e73dddf
Show file tree

Hide file tree

Showing 5 changed files with 103 additions and 99 deletions.
diff --git a/src/drivers/interface/motors.h b/src/drivers/interface/motors.h
@@ -315,6 +315,19 @@ int motorsESCIsLo(uint32_t id);
  */
 void motorsBurstDshot();
 
+/*
+ * Enables or disables the ability to set power levels of the motors directly
+ * which is done by calling motorPowerSetValue(id, value)
+*/
+void motorSetPowerEnabled(bool enable);
+
+/*
+ * Sets the power level for the given motor id.
+ * Note that the powerSetEnable functionality must be set to true
+ * in order for this to have any effect.
+*/
+void motorSetPowerValue(uint32_t id, uint16_t value);
+
 /**
  * Set the PWM ratio of the motor 'id'
  */

diff --git a/src/drivers/src/motors.c b/src/drivers/src/motors.c
@@ -46,7 +46,7 @@
 #include "log.h"
 #include "param.h"
 
-static bool motorSetEnable = false;
+static bool motorPowerSetIsEnabled = false;
 static uint16_t motorPowerSet[] = {0, 0, 0, 0}; // user-requested PWM signals (overrides)
 static uint32_t motor_ratios[] = {0, 0, 0, 0};  // actual PWM signals
 
@@ -454,6 +454,15 @@ void motorsBurstDshot()
 }
 #endif
 
+void motorSetPowerEnabled(bool enable)
+{
+  motorPowerSetIsEnabled = enable;
+}
+
+void motorSetPowerValue(uint32_t id, uint16_t value)
+{
+  motorPowerSet[id] = value;
+}
 
 // Ithrust is thrust mapped for 65536 <==> 60 grams
 void motorsSetRatio(uint32_t id, uint16_t ithrust)
@@ -463,7 +472,7 @@ void motorsSetRatio(uint32_t id, uint16_t ithrust)
 
     uint16_t ratio = ithrust;
 
-    if (motorSetEnable) {
+    if (motorPowerSetIsEnabled) {
       ratio = motorPowerSet[id];
     }
 
@@ -681,7 +690,7 @@ PARAM_GROUP_START(motorPowerSet)
 /**
  * @brief Nonzero to override controller with set values
  */
-PARAM_ADD_CORE(PARAM_UINT8, enable, &motorSetEnable)
+PARAM_ADD_CORE(PARAM_UINT8, enable, &motorPowerSetIsEnabled)
 
 /**
  * @brief motor power for m1: `0 - UINT16_MAX`

diff --git a/src/modules/interface/msp.h b/src/modules/interface/msp.h
@@ -67,6 +67,9 @@ typedef struct
   uint8_t requestCrc;
   uint8_t requestState;
 
+  uint8_t dataRead;
+  uint8_t data[16];
+
   // Response context
   uint8_t mspResponse[256];
   // Size of the complete response message contained

diff --git a/src/modules/src/msp.c b/src/modules/src/msp.c
@@ -54,6 +54,7 @@ typedef enum {
   MSP_BOXIDS         = 119,
   MSP_BATTERY_STATE  = 130,
   MSP_UID            = 160, // Unique device ID
+  MSP_SET_MOTOR      = 214,
   MSP_SET_4WAY_IF    = 245
 } msp_command_t;
 
@@ -76,6 +77,7 @@ typedef enum
   MSP_REQUEST_STATE_DIRECTION,
   MSP_REQUEST_STATE_SIZE,
   MSP_REQUEST_STATE_COMMAND,
+  MSP_REQUEST_STATE_DATA,
   MSP_REQUEST_STATE_CRC
 } MSP_REQUEST_STATE;
 
@@ -169,13 +171,19 @@ typedef struct _MspBatteryState
   uint16_t voltage2;
 }__attribute__((packed)) MspBatteryState;
 
+typedef struct _MspSetMotors
+{
+  uint16_t speed[8];
+}__attribute__((packed)) MspSetMotors;
+
 static bool hasSet4WayIf = false;
 
 // Helpers
-static uint8_t mspComputeCrc(uint8_t* pBuffer, uint32_t bufferLen);
+static uint8_t mspComputeCrc(const MspHeader* header, const uint8_t* data, const uint16_t dataLen);
 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 uint16_t mapMotorRequestSpeed(const uint16_t from);
 
 // Request handlers
 static void mspHandleRequestMspStatus(MspObject* pMspObject);
@@ -192,6 +200,7 @@ static void mspHandleRequestsSet4WayIf(MspObject* pMspObject);
 static void mspHandleRequestMotor(MspObject* pMspObject);
 static void mspHandleRequestFeaturesConfig(MspObject* pMspObject);
 static void mspHandleRequestBatteryState(MspObject* pMspObject);
+static void mspHandleRequestSetMotor(MspObject* pMspObject);
 
 // Public API
 void mspInit(MspObject* pMspObject, const MspResponseCallback callback)
@@ -235,7 +244,25 @@ void mspProcessByte(MspObject* pMspObject, const uint8_t data)
 
   case MSP_REQUEST_STATE_COMMAND:
     pMspObject->requestHeader.command = data;
-    pMspObject->requestState = MSP_REQUEST_STATE_CRC;
+
+    if (pMspObject->requestHeader.size > 0)
+    {
+      pMspObject->dataRead = 0;
+      pMspObject->requestState = MSP_REQUEST_STATE_DATA;
+    }
+    else
+    {
+      pMspObject->requestState = MSP_REQUEST_STATE_CRC;
+    }
+    break;
+
+  case MSP_REQUEST_STATE_DATA:
+    pMspObject->data[pMspObject->dataRead] = data;
+    pMspObject->dataRead++;
+    if (pMspObject->dataRead >= pMspObject->requestHeader.size)
+    {
+      pMspObject->requestState = MSP_REQUEST_STATE_CRC;
+    }
     break;
 
   case MSP_REQUEST_STATE_CRC:
@@ -260,29 +287,15 @@ void mspResetSet4WayIf()
 }
 
 // Private
-static uint8_t mspComputeCrc(uint8_t* pBuffer, uint32_t bufferLen)
+static uint8_t mspComputeCrc(const MspHeader* header, const uint8_t* data, const uint16_t dataLen)
 {
-  uint8_t crc = 0;
+  // The MSP CRC is defined as the XOR of size, command,
+  // and all data bytes into a zeroed sum.
+  uint8_t crc = header->size ^ header->command;
 
-  // Make sure the buffer is at least the size of a header
-  if(bufferLen >= sizeof(MspHeader))
+  for(uint16_t i = 0; i < dataLen; i++)
   {
-    MspHeader* pHeader = (MspHeader*)pBuffer;
-
-    // Make sure the buffer is at least the size of the buffer and data
-    if(bufferLen >= sizeof(MspHeader) + pHeader->size)
-    {
-      // The MSP CRC is defined as the XOR of size, command, and all
-      // data bytes into a zeroed sum.
-
-      crc ^= pHeader->size;
-      crc ^= pHeader->command;
-
-      for(uint16_t i = 0; i < pHeader->size; i++)
-      {
-        crc ^= pBuffer[sizeof(MspHeader) + i];
-      }
-    }
+    crc ^= data[i];
   }
 
   return crc;
@@ -305,18 +318,10 @@ static bool mspIsRequestValid(MspObject* pMspObject)
     return false;
   }
 
-  if(pMspObject->requestHeader.size != 0)
-  {
-    // Requests should not have a payload
-    DEBUG_PRINT("MSP Request has invalid size %d\n", pMspObject->requestHeader.size);
-    return false;
-  }
-
-  if(pMspObject->requestCrc !=
-      mspComputeCrc((uint8_t*)&pMspObject->requestHeader, sizeof(pMspObject->requestHeader)))
+  if(pMspObject->requestCrc != mspComputeCrc(&pMspObject->requestHeader, pMspObject->data, pMspObject->requestHeader.size))
   {
     // CRC does not match
-    DEBUG_PRINT("MSP Request has invalid crc (%d != %d)\n", pMspObject->requestCrc, mspComputeCrc((uint8_t*)&pMspObject->requestHeader, sizeof(pMspObject->requestHeader)));
+    DEBUG_PRINT("MSP Request has invalid crc (%d != %d)\n", pMspObject->requestCrc, mspComputeCrc(&pMspObject->requestHeader, pMspObject->data, pMspObject->requestHeader.size));
     return false;
   }
 
@@ -379,10 +384,12 @@ static void mspProcessRequest(MspObject* pMspObject)
     case MSP_BATTERY_STATE:
       mspHandleRequestBatteryState(pMspObject);
       break;
+    case MSP_SET_MOTOR:
+      mspHandleRequestSetMotor(pMspObject);
+      break;
     default:
       DEBUG_PRINT("Received unsupported MSP request: %d\n", pMspObject->requestHeader.command);
       return;
-      break;
   }
 
   if(pMspObject->responseCallback)
@@ -396,71 +403,31 @@ static void mspProcessRequest(MspObject* pMspObject)
 
 static void mspHandleRequestMspStatus(MspObject* pMspObject)
 {
-  MspHeader* pHeader = (MspHeader*)pMspObject->mspResponse;
   MspStatus* pData = (MspStatus*)(pMspObject->mspResponse + sizeof(MspHeader));
-  uint8_t* pCrc = (uint8_t*)(pMspObject->mspResponse + sizeof(MspHeader) + sizeof(*pData));
-
-  // Header
-  pHeader->preamble[0] = MSP_PREAMBLE_0;
-  pHeader->preamble[1] = MSP_PREAMBLE_1;
-  pHeader->direction = MSP_DIRECTION_OUT;
-  pHeader->size = sizeof(*pData);
-  pHeader->command = MSP_STATUS;
-
-  // Data
   pData->cycleTime = 1000; // TODO: API to query this?
   pData->i2cErrors = 0; // unused
   pData->sensors = 0x0001; // no sensors supported yet, but need to report at least one to get the level bars to show
   pData->flags = 0x00000001; // always report armed (bit zero)
   pData->currentSet = 0x00;
 
-  // CRC
-  *pCrc = mspComputeCrc(pMspObject->mspResponse, sizeof(pMspObject->mspResponse));
-
-  // Update total response size
-  pMspObject->mspResponseSize = sizeof(MspHeader) + sizeof(*pData) + 1;
+  mspMakeTxPacket(pMspObject, MSP_STATUS, (uint8_t*) pData, sizeof(MspStatus));
 }
 
 static void mspHandleRequestMspRc(MspObject* pMspObject)
 {
-  MspHeader* pHeader = (MspHeader*)pMspObject->mspResponse;
   MspRc* pData = (MspRc*)(pMspObject->mspResponse + sizeof(MspHeader));
-  uint8_t* pCrc = (uint8_t*)(pMspObject->mspResponse + sizeof(MspHeader) + sizeof(*pData));
-
-  // Header
-  pHeader->preamble[0] = MSP_PREAMBLE_0;
-  pHeader->preamble[1] = MSP_PREAMBLE_1;
-  pHeader->direction = MSP_DIRECTION_OUT;
-  pHeader->size = sizeof(*pData);
-  pHeader->command = MSP_RC;
-
   // TODO: get actual data - for now hardcode the midpoint
   pData->roll = 1500;
   pData->pitch = 1500;
   pData->yaw = 1500;
   pData->throttle = 1500;
 
-  // CRC
-  *pCrc = mspComputeCrc(pMspObject->mspResponse, sizeof(pMspObject->mspResponse));
-
-  // Update total response size
-  pMspObject->mspResponseSize = sizeof(MspHeader) + sizeof(*pData) + 1;
+  mspMakeTxPacket(pMspObject, MSP_RC, (uint8_t*) pData, sizeof(MspRc));
 }
 
 static void mspHandleRequestMspAttitude(MspObject* pMspObject)
 {
-  MspHeader* pHeader = (MspHeader*)pMspObject->mspResponse;
   MspAttitude* pData = (MspAttitude*)(pMspObject->mspResponse + sizeof(MspHeader));
-  uint8_t* pCrc = (uint8_t*)(pMspObject->mspResponse + sizeof(MspHeader) + sizeof(*pData));
-
-  // Header
-  pHeader->preamble[0] = MSP_PREAMBLE_0;
-  pHeader->preamble[1] = MSP_PREAMBLE_1;
-  pHeader->direction = MSP_DIRECTION_OUT;
-  pHeader->size = sizeof(*pData);
-  pHeader->command = MSP_ATTITUDE;
-
-  // Data
   float roll;
   float pitch;
   float yaw;
@@ -470,36 +437,18 @@ static void mspHandleRequestMspAttitude(MspObject* pMspObject)
   pData->angY = (int16_t)(pitch * 10);
   pData->heading = 0; // TODO: mag support
 
-  // CRC
-  *pCrc = mspComputeCrc(pMspObject->mspResponse, sizeof(pMspObject->mspResponse));
-
-  // Update total response size
-  pMspObject->mspResponseSize = sizeof(MspHeader) + sizeof(*pData) + 1;
+  mspMakeTxPacket(pMspObject, MSP_ATTITUDE, (uint8_t*) pData, sizeof(MspAttitude));
 }
 
 static void mspHandleRequestMspBoxIds(MspObject* pMspObject)
 {
-  MspHeader* pHeader = (MspHeader*)pMspObject->mspResponse;
   uint8_t* pData = (uint8_t*)(pMspObject->mspResponse + sizeof(MspHeader));
-  uint8_t* pCrc = (uint8_t*)(pMspObject->mspResponse + sizeof(MspHeader) + sizeof(*pData));
-
-  // Header
-  pHeader->preamble[0] = MSP_PREAMBLE_0;
-  pHeader->preamble[1] = MSP_PREAMBLE_1;
-  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
   // the client to use box ID 0 for the ARMED box
   pData[0] = 0x00;
 
-  // CRC
-  *pCrc = mspComputeCrc(pMspObject->mspResponse, sizeof(pMspObject->mspResponse));
-
-  // Update total response size
-  pMspObject->mspResponseSize = sizeof(MspHeader) + sizeof(*pData) + 1;
+  mspMakeTxPacket(pMspObject, MSP_BOXIDS, (uint8_t*) pData, 0);
 }
 
 // Note: All request-handlers below have been reverese-engineered from BLHeli Configurator: https://github.com/stylesuxx/esc-configurator
@@ -595,6 +544,31 @@ static void mspHandleRequestBatteryState(MspObject* pMspObject)
   mspMakeTxPacket(pMspObject, MSP_BATTERY_STATE, (uint8_t*) batteryState, sizeof(MspBatteryState));
 }
 
+static void mspHandleRequestSetMotor(MspObject* pMspObject)
+{
+  // Ensure that the motorPowerSet functionality is first enabled
+  motorSetPowerEnabled(true);
+
+  for (int motor = 0; motor < NBR_OF_MOTORS; motor++) {
+    // Set the motor power level for each motor.
+    uint16_t motorSpeed = ((uint16_t*) pMspObject->data)[motor];
+    motorSpeed = mapMotorRequestSpeed(motorSpeed);
+    motorSetPowerValue(motor, motorSpeed);
+  }
+
+  mspMakeTxPacket(pMspObject, MSP_SET_MOTOR, 0, 0);
+}
+
+/*
+ * Maps the motorSpeed from a value between 1000 and 2000 to
+ * a value between 0 and 65535.
+*/
+static uint16_t mapMotorRequestSpeed(const uint16_t from)
+{
+  float perc = (from - 1000) / 1000.0;
+  return perc * 65535;
+}
+
 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;
@@ -608,7 +582,7 @@ static void mspMakeTxPacket(MspObject* pMspObject, const msp_command_t command,
   pHeader->command = command;
   memcpy(pData, data, dataLen);
 
-  *pCrc = mspComputeCrc(pMspObject->mspResponse, sizeof(pMspObject->mspResponse));
+  *pCrc = mspComputeCrc(pHeader, pData, pHeader->size);
 
   // 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
@@ -147,6 +147,11 @@ void passthroughTask(void *param)
     uartslkResumeRx();
     sensorsResume();
 
+    // The ability to set the powers of the motors directly might be changed
+    // during the 4way process (for instance while using the motor sliders in ESC Configurator ).
+    // Here we'll just make sure that the ability is set to false, so we don't accidentally start the motors.
+    motorSetPowerEnabled(false);
+
     // Clear any notifications that was queued during 4way process.
     ulTaskNotifyValueClear(NULL, 0xFFFFFFFF);
   }