Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-15 04:15:44 +03:00
Code Issues Wiki Activity

Motor output reordering for CLI and MSP

Browse source 
Fixed some code formatting

+fixReorderingArray() for EEPROM load/save and renamings

fix brace new line

moving MOTOR_OUTPUT_REORDERING to MSP2 betaflight specific

validateAndfixConfig now resets reordering motor array to default in case it is invalid
This commit is contained in:
limonspb 2020-06-23 18:46:11 -05:00
parent 1d78f97846
commit 464919c416
17 changed files with 185 additions and 21 deletions
Download patch file Download diff file Expand all files Collapse all files

3
src/main/cli/settings.c
View file

@ -827,7 +827,8 @@ const clivalue_t valueTable[] = {
{ "motor_pwm_protocol", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_MOTOR_PWM_PROTOCOL }, PG_MOTOR_CONFIG, offsetof(motorConfig_t, dev.motorPwmProtocol) },
{ "motor_pwm_rate", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 200, 32000 }, PG_MOTOR_CONFIG, offsetof(motorConfig_t, dev.motorPwmRate) },
{ "motor_pwm_inversion", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_MOTOR_CONFIG, offsetof(motorConfig_t, dev.motorPwmInversion) },
{ "motor_poles", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 4, UINT8_MAX }, PG_MOTOR_CONFIG, offsetof(motorConfig_t, motorPoleCount) },
{ "motor_poles", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 4, UINT8_MAX }, PG_MOTOR_CONFIG, offsetof(motorConfig_t, motorPoleCount) },
{ "motor_output_reordering", VAR_UINT8 | MASTER_VALUE | MODE_ARRAY, .config.array.length = MAX_SUPPORTED_MOTORS, PG_MOTOR_CONFIG, offsetof(motorConfig_t, dev.motorOutputReordering)},
// PG_THROTTLE_CORRECTION_CONFIG
{ "thr_corr_value", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 150 }, PG_THROTTLE_CORRECTION_CONFIG, offsetof(throttleCorrectionConfig_t, throttle_correction_value) },

5
src/main/config/config.c
View file

@ -89,6 +89,8 @@
#include "config.h"
#include "drivers/dshot.h"
static bool configIsDirty; /* someone indicated that the config is modified and it is not yet saved */
static bool rebootRequired = false; // set if a config change requires a reboot to take effect
@ -596,6 +598,7 @@ static void validateAndFixConfig(void)
}
}
}
#if defined(USE_RX_MSP_OVERRIDE)
for (int i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) {
const modeActivationCondition_t *mac = modeActivationConditions(i);
@ -604,6 +607,8 @@ static void validateAndFixConfig(void)
}
}
#endif
validateAndfixMotorOutputReordering(motorConfigMutable()->dev.motorOutputReordering, MAX_SUPPORTED_MOTORS);
}
void validateAndFixGyroConfig(void)

36
src/main/drivers/dshot.c
View file

@ -164,3 +164,39 @@ void updateDshotTelemetryQuality(dshotTelemetryQuality_t *qualityStats, bool pac
#endif // USE_DSHOT_TELEMETRY_STATS
#endif // USE_DSHOT
// temporarly here, needs to be moved during refactoring
void validateAndfixMotorOutputReordering(uint8_t *array, const unsigned size)
{
bool invalid = false;
for (unsigned i = 0; i < size; i++) {
if (array[i] >= size) {
invalid = true;
break;
}
}
int valuesAsIndexes[size];
for (unsigned i = 0; i < size; i++) {
valuesAsIndexes[i] = -1;
}
if (!invalid) {
for (unsigned i = 0; i < size; i++) {
if (-1 != valuesAsIndexes[array[i]]) {
invalid = true;
break;
}
valuesAsIndexes[array[i]] = array[i];
}
}
if (invalid) {
for (unsigned i = 0; i < size; i++) {
array[i] = i;
}
}
}

2
src/main/drivers/dshot.h
View file

@ -90,3 +90,5 @@ bool isDshotMotorTelemetryActive(uint8_t motorIndex);
bool isDshotTelemetryActive(void);
int16_t getDshotTelemetryMotorInvalidPercent(uint8_t motorIndex);
void validateAndfixMotorOutputReordering(uint8_t *array, const unsigned size);

5
src/main/drivers/dshot_bitbang.c
View file

@ -650,8 +650,9 @@ motorDevice_t *dshotBitbangDevInit(const motorDevConfig_t *motorConfig, uint8_t
#endif
for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) {
const timerHardware_t *timerHardware = timerGetByTag(motorConfig->ioTags[motorIndex]);
const IO_t io = IOGetByTag(motorConfig->ioTags[motorIndex]);
const unsigned reorderedMotorIndex = motorConfig->motorOutputReordering[motorIndex];
const timerHardware_t *timerHardware = timerGetByTag(motorConfig->ioTags[reorderedMotorIndex]);
const IO_t io = IOGetByTag(motorConfig->ioTags[reorderedMotorIndex]);
uint8_t output = motorConfig->motorPwmInversion ? timerHardware->output ^ TIMER_OUTPUT_INVERTED : timerHardware->output;
bbPuPdMode = (output & TIMER_OUTPUT_INVERTED) ? BB_GPIO_PULLDOWN : BB_GPIO_PULLUP;

8
src/main/drivers/dshot_dpwm.c
View file

@ -179,15 +179,17 @@ motorDevice_t *dshotPwmDevInit(const motorDevConfig_t *motorConfig, uint16_t idl
}
for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) {
const ioTag_t tag = motorConfig->ioTags[motorIndex];
const timerHardware_t *timerHardware = timerAllocate(tag, OWNER_MOTOR, RESOURCE_INDEX(motorIndex));
const unsigned reorderedMotorIndex = motorConfig->motorOutputReordering[motorIndex];
const ioTag_t tag = motorConfig->ioTags[reorderedMotorIndex];
const timerHardware_t *timerHardware = timerAllocate(tag, OWNER_MOTOR, RESOURCE_INDEX(reorderedMotorIndex));
if (timerHardware != NULL) {
motors[motorIndex].io = IOGetByTag(tag);
IOInit(motors[motorIndex].io, OWNER_MOTOR, RESOURCE_INDEX(motorIndex));
IOInit(motors[motorIndex].io, OWNER_MOTOR, RESOURCE_INDEX(reorderedMotorIndex));
if (pwmDshotMotorHardwareConfig(timerHardware,
motorIndex,
reorderedMotorIndex,
motorConfig->motorPwmProtocol,
motorConfig->motorPwmInversion ? timerHardware->output ^ TIMER_OUTPUT_INVERTED : timerHardware->output)) {
motors[motorIndex].enabled = true;

2
src/main/drivers/dshot_dpwm.h
View file

@ -157,7 +157,7 @@ typedef struct motorDmaOutput_s {
motorDmaOutput_t *getMotorDmaOutput(uint8_t index);
void pwmWriteDshotInt(uint8_t index, uint16_t value);
bool pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output);
bool pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint8_t reorderedMotorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output);
#ifdef USE_DSHOT_TELEMETRY
bool pwmStartDshotMotorUpdate(void);
#endif

7
src/main/drivers/pwm_output.c
View file

@ -219,8 +219,9 @@ motorDevice_t *motorPwmDevInit(const motorDevConfig_t *motorConfig, uint16_t idl
motorPwmDevice.vTable.updateComplete = useUnsyncedPwm ? motorUpdateCompleteNull : pwmCompleteOneshotMotorUpdate;
for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) {
const ioTag_t tag = motorConfig->ioTags[motorIndex];
const timerHardware_t *timerHardware = timerAllocate(tag, OWNER_MOTOR, RESOURCE_INDEX(motorIndex));
const unsigned reorderedMotorIndex = motorConfig->motorOutputReordering[motorIndex];
const ioTag_t tag = motorConfig->ioTags[reorderedMotorIndex];
const timerHardware_t *timerHardware = timerAllocate(tag, OWNER_MOTOR, RESOURCE_INDEX(reorderedMotorIndex));
if (timerHardware == NULL) {
/* not enough motors initialised for the mixer or a break in the motors */
@ -231,7 +232,7 @@ motorDevice_t *motorPwmDevInit(const motorDevConfig_t *motorConfig, uint16_t idl
}
motors[motorIndex].io = IOGetByTag(tag);
IOInit(motors[motorIndex].io, OWNER_MOTOR, RESOURCE_INDEX(motorIndex));
IOInit(motors[motorIndex].io, OWNER_MOTOR, RESOURCE_INDEX(reorderedMotorIndex));
#if defined(STM32F1)
IOConfigGPIO(motors[motorIndex].io, IOCFG_AF_PP);

4
src/main/drivers/pwm_output_dshot.c
View file

@ -224,7 +224,7 @@ static void motor_DMA_IRQHandler(dmaChannelDescriptor_t *descriptor)
}
}
bool pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output)
bool pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint8_t reorderedMotorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output)
{
#ifdef USE_DSHOT_TELEMETRY
#define OCINIT motor->ocInitStruct
@ -377,7 +377,7 @@ bool pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t m
} else
#endif
{
dmaInit(dmaGetIdentifier(dmaRef), OWNER_MOTOR, RESOURCE_INDEX(motorIndex));
dmaInit(dmaGetIdentifier(dmaRef), OWNER_MOTOR, RESOURCE_INDEX(reorderedMotorIndex));
motor->dmaBuffer = &dshotDmaBuffer[motorIndex][0];

4
src/main/drivers/pwm_output_dshot_hal.c
View file

@ -197,7 +197,7 @@ FAST_CODE static void motor_DMA_IRQHandler(dmaChannelDescriptor_t* descriptor)
}
}
bool pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output)
bool pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint8_t reorderedMotorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output)
{
#ifdef USE_DSHOT_TELEMETRY
#define OCINIT motor->ocInitStruct
@ -347,7 +347,7 @@ bool pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t m
} else
#endif
{
dmaInit(dmaGetIdentifier(dmaRef), OWNER_MOTOR, RESOURCE_INDEX(motorIndex));
dmaInit(dmaGetIdentifier(dmaRef), OWNER_MOTOR, RESOURCE_INDEX(reorderedMotorIndex));
motor->dmaBuffer = &dshotDmaBuffer[motorIndex][0];

6
src/main/drivers/pwm_output_dshot_hal_hal.c
View file

@ -233,7 +233,7 @@ static void motor_DMA_IRQHandler(dmaChannelDescriptor_t* descriptor)
}
}
bool pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output)
bool pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint8_t reorderedMotorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output)
{
dmaResource_t *dmaRef = NULL;
uint32_t dmaChannel;
@ -285,7 +285,7 @@ bool pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t m
const uint8_t timerIndex = getTimerIndex(timer);
const bool configureTimer = (timerIndex == dmaMotorTimerCount - 1);
IOInit(motorIO, OWNER_MOTOR, RESOURCE_INDEX(motorIndex));
IOInit(motorIO, OWNER_MOTOR, RESOURCE_INDEX(reorderedMotorIndex));
IOConfigGPIOAF(motorIO, motor->iocfg, timerHardware->alternateFunction);
// Configure time base
@ -373,7 +373,7 @@ P - High - High -
} else
#endif
{
dmaInit(identifier, OWNER_MOTOR, RESOURCE_INDEX(motorIndex));
dmaInit(identifier, OWNER_MOTOR, RESOURCE_INDEX(reorderedMotorIndex));
dmaSetHandler(identifier, motor_DMA_IRQHandler, NVIC_PRIO_DSHOT_DMA, motorIndex);
}

26
src/main/msp/msp.c
View file

@ -1190,6 +1190,16 @@ static bool mspProcessOutCommand(int16_t cmdMSP, sbuf_t *dst)
}
break;
case MSP2_MOTOR_OUTPUT_REORDERING:
{
sbufWriteU8(dst, MAX_SUPPORTED_MOTORS);
for (unsigned i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
sbufWriteU8(dst, motorConfig()->dev.motorOutputReordering[i]);
}
}
break;
case MSP_RC:
for (int i = 0; i < rxRuntimeState.channelCount; i++) {
sbufWriteU16(dst, rcData[i]);
@ -2964,6 +2974,22 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
break;
#endif
case MSP2_SET_MOTOR_OUTPUT_REORDERING:
{
const uint8_t arraySize = sbufReadU8(src);
for (unsigned i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
uint8_t value = i;
if (i < arraySize) {
value = sbufReadU8(src);
}
motorConfigMutable()->dev.motorOutputReordering[i] = value;
}
}
break;
#ifdef USE_CAMERA_CONTROL
case MSP_CAMERA_CONTROL:
{

4
src/main/msp/msp_protocol_v2_betaflight.h
View file

@ -18,4 +18,6 @@
* If not, see <http://www.gnu.org/licenses/>.
*/
#define MSP2_BETAFLIGHT_BIND 0x3000
#define MSP2_BETAFLIGHT_BIND 0x3000
#define MSP2_MOTOR_OUTPUT_REORDERING 0x3001
#define MSP2_SET_MOTOR_OUTPUT_REORDERING 0x3002

4
src/main/pg/motor.c
View file

@ -74,6 +74,10 @@ void pgResetFn_motorConfig(motorConfig_t *motorConfig)
motorConfig->motorPoleCount = 14; // Most brushes motors that we use are 14 poles
for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
motorConfig->dev.motorOutputReordering[i] = i;
}
#ifdef USE_DSHOT_BITBANG
motorConfig->dev.useDshotBitbang = DSHOT_BITBANG_DEFAULT;
motorConfig->dev.useDshotBitbangedTimer = DSHOT_BITBANGED_TIMER_DEFAULT;

1
src/main/pg/motor.h
View file

@ -48,6 +48,7 @@ typedef struct motorDevConfig_s {
uint8_t motorTransportProtocol;
uint8_t useDshotBitbang;
uint8_t useDshotBitbangedTimer;
uint8_t motorOutputReordering[MAX_SUPPORTED_MOTORS]; // Reindexing motors for "remap motors" feature in Configurator
} motorDevConfig_t;
typedef struct motorConfig_s {

10
src/test/Makefile
View file

@ -174,13 +174,17 @@ ledstrip_unittest_SRC := \
$(USER_DIR)/io/ledstrip.c
ledstrip_unittest_DEFINES := \
USE_LED_STRIP=
USE_LED_STRIP=
maths_unittest_SRC := \
$(USER_DIR)/common/maths.c
motor_output_unittest_SRC := \
$(USER_DIR)/drivers/dshot.c
osd_unittest_SRC := \
$(USER_DIR)/osd/osd.c \
$(USER_DIR)/osd/osd_elements.c \

79
src/test/unit/motor_output_unittest.cc Normal file
View file

@ -0,0 +1,79 @@
/*
* This file is part of Betaflight.
*
* Cleanflight and Betaflight are free software. You can redistribute
* this software and/or modify this software under the terms of the
* GNU General Public License as published by the Free Software
* Foundation, either version 3 of the License, or (at your option)
* any later version.
*
* Cleanflight and Betaflight are distributed in the hope that they
* will be useful, but WITHOUT ANY WARRANTY; without even the implied
* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this software.
*
* If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdint.h>
#include <iostream>
extern "C" {
#include "drivers/dshot.h"
}
#include "unittest_macros.h"
#include "gtest/gtest.h"
TEST(MotorOutputUnittest, TestFixMotorOutputReordering)
{
const unsigned size = 8;
uint8_t a1_initial[size] = {0, 1, 2, 3, 4, 5, 6, 7};
uint8_t a1_expected[size] = {0, 1, 2, 3, 4, 5, 6, 7};
validateAndfixMotorOutputReordering(a1_initial, size);
EXPECT_TRUE( 0 == memcmp(a1_expected, a1_initial, sizeof(a1_expected)));
uint8_t a2_initial[size] = {3, 2, 1, 4, 5, 6, 7, 0};
uint8_t a2_expected[size] = {3, 2, 1, 4, 5, 6, 7, 0};
validateAndfixMotorOutputReordering(a2_initial, size);
EXPECT_TRUE( 0 == memcmp(a2_expected, a2_initial, sizeof(a2_expected)));
uint8_t a3_initial[size] = {3, 2, 1, 100, 5, 6, 7, 0};
uint8_t a3_expected[size] = {0, 1, 2, 3, 4, 5, 6, 7};
validateAndfixMotorOutputReordering(a3_initial, size);
EXPECT_TRUE( 0 == memcmp(a3_expected, a3_initial, sizeof(a3_expected)));
uint8_t a4_initial[size] = {3, 2, 1, 100, 5, 6, 200, 0};
uint8_t a4_expected[size] = {0, 1, 2, 3, 4, 5, 6, 7};
validateAndfixMotorOutputReordering(a4_initial, size);
EXPECT_TRUE( 0 == memcmp(a4_expected, a4_initial, sizeof(a4_expected)));
uint8_t a5_initial[size] = {0, 0, 0, 0, 0, 0, 0, 0};
uint8_t a5_expected[size] = {0, 1, 2, 3, 4, 5, 6, 7};
validateAndfixMotorOutputReordering(a5_initial, size);
EXPECT_TRUE( 0 == memcmp(a5_expected, a5_initial, sizeof(a5_expected)));
uint8_t a6_initial[size] = {0, 0, 0, 1, 0, 99, 0, 0};
uint8_t a6_expected[size] = {0, 1, 2, 3, 4, 5, 6, 7};
validateAndfixMotorOutputReordering(a6_initial, size);
EXPECT_TRUE( 0 == memcmp(a6_expected, a6_initial, sizeof(a5_expected)));
uint8_t a7_initial[size] = {1, 5, 3, 4, 5, 6, 7, 0};
uint8_t a7_expected[size] = {0, 1, 2, 3, 4, 5, 6, 7};
validateAndfixMotorOutputReordering(a7_initial, size);
EXPECT_TRUE( 0 == memcmp(a7_expected, a7_initial, sizeof(a7_expected)));
uint8_t a8_initial[size] = {1, 5, 1, 5, 3, 3, 3, 3};
uint8_t a8_expected[size] = {0, 1, 2, 3, 4, 5, 6, 7};
validateAndfixMotorOutputReordering(a8_initial, size);
EXPECT_TRUE( 0 == memcmp(a8_expected, a8_initial, sizeof(a8_expected)));
uint8_t a9_initial[size] = {7, 6, 5, 4, 3, 2, 1, 0};
uint8_t a9_expected[size] = {7, 6, 5, 4, 3, 2, 1, 0};
validateAndfixMotorOutputReordering(a9_initial, size);
EXPECT_TRUE( 0 == memcmp(a9_expected, a9_initial, sizeof(a9_expected)));
}