From bed4f735a12119c36c542154831d0c441e337d2f Mon Sep 17 00:00:00 2001 From: victor Date: Tue, 14 Feb 2023 22:17:01 +0100 Subject: [PATCH] Changed the power-set functionality to use the internal param API instead 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 | 13 ------------- src/drivers/src/motors.c | 16 +++------------- src/modules/src/msp.c | 13 +++++++++++-- src/modules/src/vcp_esc_passthrough.c | 6 +++++- 4 files changed, 19 insertions(+), 29 deletions(-) diff --git a/src/drivers/interface/motors.h b/src/drivers/interface/motors.h index ca0d4efdb8..614601aa81 100644 --- a/src/drivers/interface/motors.h +++ b/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' */ diff --git a/src/drivers/src/motors.c b/src/drivers/src/motors.c index 3eb209e83c..e920b0c875 100644 --- a/src/drivers/src/motors.c +++ b/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` diff --git a/src/modules/src/msp.c b/src/modules/src/msp.c index a12bbe9a11..f8d5a1624a 100644 --- a/src/modules/src/msp.c +++ b/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); diff --git a/src/modules/src/vcp_esc_passthrough.c b/src/modules/src/vcp_esc_passthrough.c index 45c306eca3..685b25c711 100644 --- a/src/modules/src/vcp_esc_passthrough.c +++ b/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);