Changed the power-set functionality to use the internal param API ins…

…tead of the public functions in motors.h.

Note that this requires the Crazyflie to be connected to the cfclient when setting motor power, OR the build flag CONFIG_PARAM_SILENT_UPDATES must be set.

src/drivers/interface/motors.h

@@ -315,19 +315,6 @@ 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'
  */

src/drivers/src/motors.c

@@ -46,7 +46,7 @@
 #include "log.h"
 #include "param.h"
 
-static bool motorPowerSetIsEnabled = false;
+static bool motorSetEnable = 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,16 +454,6 @@ 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)
 {
@@ -472,7 +462,7 @@ void motorsSetRatio(uint32_t id, uint16_t ithrust)
 
     uint16_t ratio = ithrust;
 
-    if (motorPowerSetIsEnabled) {
+    if (motorSetEnable) {
       ratio = motorPowerSet[id];
     }
 
@@ -690,7 +680,7 @@ PARAM_GROUP_START(motorPowerSet)
 /**
  * @brief Nonzero to override controller with set values
  */
-PARAM_ADD_CORE(PARAM_UINT8, enable, &motorPowerSetIsEnabled)
+PARAM_ADD_CORE(PARAM_UINT8, enable, &motorSetEnable)
 
 /**
  * @brief motor power for m1: `0 - UINT16_MAX`

src/modules/src/msp.c

@@ -38,6 +38,7 @@
 #include "motors.h"
 #include "string.h"
 #include "platform.h"
+#include "param.h"
 
 // MSP command IDs
 typedef enum {
@@ -177,6 +178,8 @@ typedef struct _MspSetMotors
 }__attribute__((packed)) MspSetMotors;
 
 static bool hasSet4WayIf = false;
+static paramVarId_t motorPowerSetEnableParam;
+static paramVarId_t motorParams[NBR_OF_MOTORS];
 
 // Helpers
 static uint8_t mspComputeCrc(const MspHeader* header, const uint8_t* data, const uint16_t dataLen);
@@ -207,6 +210,12 @@ void mspInit(MspObject* pMspObject, const MspResponseCallback callback)
 {
   pMspObject->requestState = MSP_REQUEST_STATE_WAIT_FOR_START;
   pMspObject->responseCallback = callback;
+  // Get params from internal parameter API, which we need to enable and set the motor PWM.
+  motorPowerSetEnableParam = paramGetVarId("motorPowerSet", "enable");
+  motorParams[0] = paramGetVarId("motorPowerSet", "m1");
+  motorParams[1] = paramGetVarId("motorPowerSet", "m2");
+  motorParams[2] = paramGetVarId("motorPowerSet", "m3");
+  motorParams[3] = paramGetVarId("motorPowerSet", "m4");
 }
 
 void mspProcessByte(MspObject* pMspObject, const uint8_t data)
@@ -547,13 +556,13 @@ static void mspHandleRequestBatteryState(MspObject* pMspObject)
 static void mspHandleRequestSetMotor(MspObject* pMspObject)
 {
   // Ensure that the motorPowerSet functionality is first enabled
-  motorSetPowerEnabled(true);
+  paramSetInt(motorPowerSetEnableParam, 1);
 
   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);
+    paramSetInt(motorParams[motor], motorSpeed);
   }
 
   mspMakeTxPacket(pMspObject, MSP_SET_MOTOR, 0, 0);

src/modules/src/vcp_esc_passthrough.c

@@ -41,11 +41,13 @@
 #include "msp.h"
 #include "uart_syslink.h"
 #include "sensors.h"
+#include "param.h"
 
 static TaskHandle_t passthroughTaskHandle;
 STATIC_MEM_TASK_ALLOC(passthroughTask, PASSTHROUGH_TASK_STACKSIZE);
 
 static bool isInit;
+static paramVarId_t motorPowerSetEnableParam;
 
 // Passthorugh queues to handle VCP data.
 static xQueueHandle  ptRxQueue;
@@ -73,6 +75,8 @@ void passthroughInit()
   ptTxQueue = STATIC_MEM_QUEUE_CREATE(ptTxQueue);
   DEBUG_QUEUE_MONITOR_REGISTER(ptRxQueue);
 
+  motorPowerSetEnableParam = paramGetVarId("motorPowerSet", "enable");
+
   passthroughTaskHandle = STATIC_MEM_TASK_CREATE(passthroughTask, passthroughTask, PASSTHROUGH_TASK_NAME, NULL, PASSTHROUGH_TASK_PRI);
 }
 
@@ -150,7 +154,7 @@ void passthroughTask(void *param)
     // 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);
+    paramSetInt(motorPowerSetEnableParam, 0);
 
     // Clear any notifications that was queued during 4way process.
     ulTaskNotifyValueClear(NULL, 0xFFFFFFFF);