rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #3242 from betaflight/float_conversion_moved 

Convert motor values at the final stage // Increase resolution
This commit is contained in:
borisbstyle 2017-06-19 09:51:57 +02:00 committed by GitHub
parent c5054f9e9d b21877438e
commit 3575ff4515
9 changed files with 62 additions and 50 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
src/main/blackbox/blackbox.c
View File

@ -323,7 +323,7 @@ typedef struct blackboxSlowState_s {
} __attribute__((__packed__)) blackboxSlowState_t; // We pack this struct so that padding doesn't interfere with memcmp() } __attribute__((__packed__)) blackboxSlowState_t; // We pack this struct so that padding doesn't interfere with memcmp()
//From mixer.c: //From mixer.c:
extern uint16_t motorOutputHigh, motorOutputLow; extern float motorOutputHigh, motorOutputLow;
//From rc_controls.c //From rc_controls.c
extern boxBitmask_t rcModeActivationMask; extern boxBitmask_t rcModeActivationMask;
@ -1188,6 +1188,9 @@ static bool sendFieldDefinition(char mainFrameChar, char deltaFrameChar, const v
*/ */
static bool blackboxWriteSysinfo(void) static bool blackboxWriteSysinfo(void)
{ {
const uint16_t motorOutputLowInt = lrintf(motorOutputLow);
const uint16_t motorOutputHighInt = lrintf(motorOutputHigh);
// Make sure we have enough room in the buffer for our longest line (as of this writing, the "Firmware date" line) // Make sure we have enough room in the buffer for our longest line (as of this writing, the "Firmware date" line)
if (blackboxDeviceReserveBufferSpace(64) != BLACKBOX_RESERVE_SUCCESS) { if (blackboxDeviceReserveBufferSpace(64) != BLACKBOX_RESERVE_SUCCESS) {
return false; return false;
@ -1203,7 +1206,7 @@ static bool blackboxWriteSysinfo(void)
BLACKBOX_PRINT_HEADER_LINE("minthrottle", "%d", motorConfig()->minthrottle); BLACKBOX_PRINT_HEADER_LINE("minthrottle", "%d", motorConfig()->minthrottle);
BLACKBOX_PRINT_HEADER_LINE("maxthrottle", "%d", motorConfig()->maxthrottle); BLACKBOX_PRINT_HEADER_LINE("maxthrottle", "%d", motorConfig()->maxthrottle);
BLACKBOX_PRINT_HEADER_LINE("gyro_scale","0x%x", castFloatBytesToInt(1.0f)); BLACKBOX_PRINT_HEADER_LINE("gyro_scale","0x%x", castFloatBytesToInt(1.0f));
BLACKBOX_PRINT_HEADER_LINE("motorOutput", "%d,%d", motorOutputLow,motorOutputHigh); BLACKBOX_PRINT_HEADER_LINE("motorOutput", "%d,%d", motorOutputLowInt,motorOutputHighInt);
BLACKBOX_PRINT_HEADER_LINE("acc_1G", "%u", acc.dev.acc_1G); BLACKBOX_PRINT_HEADER_LINE("acc_1G", "%u", acc.dev.acc_1G);
BLACKBOX_PRINT_HEADER_LINE_CUSTOM( BLACKBOX_PRINT_HEADER_LINE_CUSTOM(

28
src/main/drivers/pwm_output.c
View File

@ -125,38 +125,38 @@ static void pwmOutConfig(pwmOutputPort_t *port, const timerHardware_t *timerHard
*port->ccr = 0; *port->ccr = 0;
} }
static void pwmWriteUnused(uint8_t index, uint16_t value) static void pwmWriteUnused(uint8_t index, float value)
{ {
UNUSED(index); UNUSED(index);
UNUSED(value); UNUSED(value);
} }
static void pwmWriteBrushed(uint8_t index, uint16_t value) static void pwmWriteBrushed(uint8_t index, float value)
{ {
*motors[index].ccr = (value - 1000) * motors[index].period / 1000; *motors[index].ccr = lrintf((value - 1000) * motors[index].period / 1000);
} }
static void pwmWriteStandard(uint8_t index, uint16_t value) static void pwmWriteStandard(uint8_t index, float value)
{ {
*motors[index].ccr = value; *motors[index].ccr = lrintf(value);
} }
static void pwmWriteOneShot125(uint8_t index, uint16_t value) static void pwmWriteOneShot125(uint8_t index, float value)
{ {
*motors[index].ccr = lrintf((float)(value * ONESHOT125_TIMER_MHZ/8.0f)); *motors[index].ccr = lrintf(value * ONESHOT125_TIMER_MHZ/8.0f);
} }
static void pwmWriteOneShot42(uint8_t index, uint16_t value) static void pwmWriteOneShot42(uint8_t index, float value)
{ {
*motors[index].ccr = lrintf((float)(value * ONESHOT42_TIMER_MHZ/24.0f)); *motors[index].ccr = lrintf(value * ONESHOT42_TIMER_MHZ/24.0f);
} }
static void pwmWriteMultiShot(uint8_t index, uint16_t value) static void pwmWriteMultiShot(uint8_t index, float value)
{ {
*motors[index].ccr = lrintf(((float)(value-1000) * MULTISHOT_20US_MULT) + MULTISHOT_5US_PW); *motors[index].ccr = lrintf(((value-1000) * MULTISHOT_20US_MULT) + MULTISHOT_5US_PW);
} }
void pwmWriteMotor(uint8_t index, uint16_t value) void pwmWriteMotor(uint8_t index, float value)
{ {
if (pwmMotorsEnabled) { if (pwmMotorsEnabled) {
pwmWritePtr(index, value); pwmWritePtr(index, value);
@ -374,10 +374,10 @@ void pwmWriteDshotCommand(uint8_t index, uint8_t command)
#endif #endif
#ifdef USE_SERVOS #ifdef USE_SERVOS
void pwmWriteServo(uint8_t index, uint16_t value) void pwmWriteServo(uint8_t index, float value)
{ {
if (index < MAX_SUPPORTED_SERVOS && servos[index].ccr) { if (index < MAX_SUPPORTED_SERVOS && servos[index].ccr) {
*servos[index].ccr = value; *servos[index].ccr = lrintf(value);
} }
} }

10
src/main/drivers/pwm_output.h
View File

@ -131,7 +131,7 @@ motorDmaOutput_t *getMotorDmaOutput(uint8_t index);
extern bool pwmMotorsEnabled; extern bool pwmMotorsEnabled;
struct timerHardware_s; struct timerHardware_s;
typedef void(*pwmWriteFuncPtr)(uint8_t index, uint16_t value); // function pointer used to write motors typedef void(*pwmWriteFuncPtr)(uint8_t index, float value); // function pointer used to write motors
typedef void(*pwmCompleteWriteFuncPtr)(uint8_t motorCount); // function pointer used after motors are written typedef void(*pwmCompleteWriteFuncPtr)(uint8_t motorCount); // function pointer used after motors are written
typedef struct { typedef struct {
@ -167,8 +167,8 @@ void pwmServoConfig(const struct timerHardware_s *timerHardware, uint8_t servoIn
#ifdef USE_DSHOT #ifdef USE_DSHOT
uint32_t getDshotHz(motorPwmProtocolTypes_e pwmProtocolType); uint32_t getDshotHz(motorPwmProtocolTypes_e pwmProtocolType);
void pwmWriteDshotCommand(uint8_t index, uint8_t command); void pwmWriteDshotCommand(uint8_t index, uint8_t command);
void pwmWriteProShot(uint8_t index, uint16_t value); void pwmWriteProShot(uint8_t index, float value);
void pwmWriteDshot(uint8_t index, uint16_t value); void pwmWriteDshot(uint8_t index, float value);
void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output); void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output);
void pwmCompleteDigitalMotorUpdate(uint8_t motorCount); void pwmCompleteDigitalMotorUpdate(uint8_t motorCount);
#endif #endif
@ -179,11 +179,11 @@ void pwmToggleBeeper(void);
void beeperPwmInit(IO_t io, uint16_t frequency); void beeperPwmInit(IO_t io, uint16_t frequency);
#endif #endif
void pwmWriteMotor(uint8_t index, uint16_t value); void pwmWriteMotor(uint8_t index, float value);
void pwmShutdownPulsesForAllMotors(uint8_t motorCount); void pwmShutdownPulsesForAllMotors(uint8_t motorCount);
void pwmCompleteMotorUpdate(uint8_t motorCount); void pwmCompleteMotorUpdate(uint8_t motorCount);
void pwmWriteServo(uint8_t index, uint16_t value); void pwmWriteServo(uint8_t index, float value);
pwmOutputPort_t *pwmGetMotors(void); pwmOutputPort_t *pwmGetMotors(void);
bool pwmIsSynced(void); bool pwmIsSynced(void);

12
src/main/drivers/pwm_output_dshot.c
View File

@ -54,15 +54,17 @@ uint8_t getTimerIndex(TIM_TypeDef *timer)
return dmaMotorTimerCount-1; return dmaMotorTimerCount-1;
} }
void pwmWriteDshot(uint8_t index, uint16_t value) void pwmWriteDshot(uint8_t index, float value)
{ {
const uint16_t digitalValue = lrintf(value);
motorDmaOutput_t * const motor = &dmaMotors[index]; motorDmaOutput_t * const motor = &dmaMotors[index];
if (!motor->timerHardware || !motor->timerHardware->dmaRef) { if (!motor->timerHardware || !motor->timerHardware->dmaRef) {
return; return;
} }
uint16_t packet = (value << 1) | (motor->requestTelemetry ? 1 : 0); uint16_t packet = (digitalValue << 1) | (motor->requestTelemetry ? 1 : 0);
motor->requestTelemetry = false; // reset telemetry request to make sure it's triggered only once in a row motor->requestTelemetry = false; // reset telemetry request to make sure it's triggered only once in a row
// compute checksum // compute checksum
@ -85,15 +87,17 @@ void pwmWriteDshot(uint8_t index, uint16_t value)
DMA_Cmd(motor->timerHardware->dmaRef, ENABLE); DMA_Cmd(motor->timerHardware->dmaRef, ENABLE);
} }
void pwmWriteProShot(uint8_t index, uint16_t value) void pwmWriteProShot(uint8_t index, float value)
{ {
const uint16_t digitalValue = lrintf(value);
motorDmaOutput_t * const motor = &dmaMotors[index]; motorDmaOutput_t * const motor = &dmaMotors[index];
if (!motor->timerHardware || !motor->timerHardware->dmaRef) { if (!motor->timerHardware || !motor->timerHardware->dmaRef) {
return; return;
} }
uint16_t packet = (value << 1) | (motor->requestTelemetry ? 1 : 0); uint16_t packet = (digitalValue << 1) | (motor->requestTelemetry ? 1 : 0);
motor->requestTelemetry = false; // reset telemetry request to make sure it's triggered only once in a row motor->requestTelemetry = false; // reset telemetry request to make sure it's triggered only once in a row
// compute checksum // compute checksum

12
src/main/drivers/pwm_output_dshot_hal.c
View File

@ -49,15 +49,17 @@ uint8_t getTimerIndex(TIM_TypeDef *timer)
return dmaMotorTimerCount - 1; return dmaMotorTimerCount - 1;
} }
void pwmWriteDshot(uint8_t index, uint16_t value) void pwmWriteDshot(uint8_t index, float value)
{ {
const uint16_t digitalValue = lrintf(value);
motorDmaOutput_t * const motor = &dmaMotors[index]; motorDmaOutput_t * const motor = &dmaMotors[index];
if (!motor->timerHardware || !motor->timerHardware->dmaRef) { if (!motor->timerHardware || !motor->timerHardware->dmaRef) {
return; return;
} }
uint16_t packet = (value << 1) | (motor->requestTelemetry ? 1 : 0); uint16_t packet = (digitalValue << 1) | (motor->requestTelemetry ? 1 : 0);
motor->requestTelemetry = false; // reset telemetry request to make sure it's triggered only once in a row motor->requestTelemetry = false; // reset telemetry request to make sure it's triggered only once in a row
// compute checksum // compute checksum
@ -89,15 +91,17 @@ void pwmWriteDshot(uint8_t index, uint16_t value)
} }
} }
void pwmWriteProShot(uint8_t index, uint16_t value) void pwmWriteProShot(uint8_t index, float value)
{ {
const uint16_t digitalValue = lrintf(value);
motorDmaOutput_t * const motor = &dmaMotors[index]; motorDmaOutput_t * const motor = &dmaMotors[index];
if (!motor->timerHardware || !motor->timerHardware->dmaRef) { if (!motor->timerHardware || !motor->timerHardware->dmaRef) {
return; return;
} }
uint16_t packet = (value << 1) | (motor->requestTelemetry ? 1 : 0); uint16_t packet = (digitalValue << 1) | (motor->requestTelemetry ? 1 : 0);
motor->requestTelemetry = false; // reset telemetry request to make sure it's triggered only once in a row motor->requestTelemetry = false; // reset telemetry request to make sure it's triggered only once in a row
// compute checksum // compute checksum

29
src/main/flight/mixer.c
View File

@ -114,8 +114,8 @@ PG_REGISTER_ARRAY(motorMixer_t, MAX_SUPPORTED_MOTORS, customMotorMixer, PG_MOTOR
static uint8_t motorCount; static uint8_t motorCount;
static float motorMixRange; static float motorMixRange;
int16_t motor[MAX_SUPPORTED_MOTORS]; float motor[MAX_SUPPORTED_MOTORS];
int16_t motor_disarmed[MAX_SUPPORTED_MOTORS]; float motor_disarmed[MAX_SUPPORTED_MOTORS];
mixerMode_e currentMixerMode; mixerMode_e currentMixerMode;
static motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS]; static motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS];
@ -312,8 +312,8 @@ const mixer_t mixers[] = {
}; };
#endif // !USE_QUAD_MIXER_ONLY #endif // !USE_QUAD_MIXER_ONLY
static uint16_t disarmMotorOutput, deadbandMotor3dHigh, deadbandMotor3dLow; static float disarmMotorOutput, deadbandMotor3dHigh, deadbandMotor3dLow;
uint16_t motorOutputHigh, motorOutputLow; float motorOutputHigh, motorOutputLow;
static float rcCommandThrottleRange, rcCommandThrottleRange3dLow, rcCommandThrottleRange3dHigh; static float rcCommandThrottleRange, rcCommandThrottleRange3dLow, rcCommandThrottleRange3dHigh;
uint8_t getMotorCount() uint8_t getMotorCount()
@ -352,17 +352,18 @@ bool isMotorProtocolDshot(void) {
#endif #endif
} }
// Add here scaled ESC outputs for digital protol // All PWM motor scaling is done to standard PWM range of 1000-2000 for easier tick conversion with legacy code / configurator
// DSHOT scaling is done to the actual dshot range
void initEscEndpoints(void) { void initEscEndpoints(void) {
#ifdef USE_DSHOT #ifdef USE_DSHOT
if (isMotorProtocolDshot()) { if (isMotorProtocolDshot()) {
disarmMotorOutput = DSHOT_DISARM_COMMAND; disarmMotorOutput = DSHOT_DISARM_COMMAND;
if (feature(FEATURE_3D)) if (feature(FEATURE_3D))
motorOutputLow = DSHOT_MIN_THROTTLE + lrintf(((DSHOT_3D_DEADBAND_LOW - DSHOT_MIN_THROTTLE) / 100.0f) * CONVERT_PARAMETER_TO_PERCENT(motorConfig()->digitalIdleOffsetValue)); motorOutputLow = DSHOT_MIN_THROTTLE + ((DSHOT_3D_DEADBAND_LOW - DSHOT_MIN_THROTTLE) / 100.0f) * CONVERT_PARAMETER_TO_PERCENT(motorConfig()->digitalIdleOffsetValue);
else else
motorOutputLow = DSHOT_MIN_THROTTLE + lrintf(((DSHOT_MAX_THROTTLE - DSHOT_MIN_THROTTLE) / 100.0f) * CONVERT_PARAMETER_TO_PERCENT(motorConfig()->digitalIdleOffsetValue)); motorOutputLow = DSHOT_MIN_THROTTLE + ((DSHOT_MAX_THROTTLE - DSHOT_MIN_THROTTLE) / 100.0f) * CONVERT_PARAMETER_TO_PERCENT(motorConfig()->digitalIdleOffsetValue);
motorOutputHigh = DSHOT_MAX_THROTTLE; motorOutputHigh = DSHOT_MAX_THROTTLE;
deadbandMotor3dHigh = DSHOT_3D_DEADBAND_HIGH + lrintf(((DSHOT_MAX_THROTTLE - DSHOT_3D_DEADBAND_HIGH) / 100.0f) * CONVERT_PARAMETER_TO_PERCENT(motorConfig()->digitalIdleOffsetValue)); // TODO - Not working yet !! Mixer requires some throttle rescaling changes deadbandMotor3dHigh = DSHOT_3D_DEADBAND_HIGH + ((DSHOT_MAX_THROTTLE - DSHOT_3D_DEADBAND_HIGH) / 100.0f) * CONVERT_PARAMETER_TO_PERCENT(motorConfig()->digitalIdleOffsetValue); // TODO - Not working yet !! Mixer requires some throttle rescaling changes
deadbandMotor3dLow = DSHOT_3D_DEADBAND_LOW; deadbandMotor3dLow = DSHOT_3D_DEADBAND_LOW;
} else } else
#endif #endif
@ -509,7 +510,7 @@ void mixTable(pidProfile_t *pidProfile)
// Scale roll/pitch/yaw uniformly to fit within throttle range // Scale roll/pitch/yaw uniformly to fit within throttle range
// Initial mixer concept by bdoiron74 reused and optimized for Air Mode // Initial mixer concept by bdoiron74 reused and optimized for Air Mode
float throttle = 0, currentThrottleInputRange = 0; float throttle = 0, currentThrottleInputRange = 0;
uint16_t motorOutputMin, motorOutputMax; float motorOutputMin, motorOutputMax;
static uint16_t throttlePrevious = 0; // Store the last throttle direction for deadband transitions static uint16_t throttlePrevious = 0; // Store the last throttle direction for deadband transitions
bool mixerInversion = false; bool mixerInversion = false;
@ -608,7 +609,7 @@ void mixTable(pidProfile_t *pidProfile)
// Now add in the desired throttle, but keep in a range that doesn't clip adjusted // Now add in the desired throttle, but keep in a range that doesn't clip adjusted
// roll/pitch/yaw. This could move throttle down, but also up for those low throttle flips. // roll/pitch/yaw. This could move throttle down, but also up for those low throttle flips.
for (uint32_t i = 0; i < motorCount; i++) { for (uint32_t i = 0; i < motorCount; i++) {
float motorOutput = motorOutputMin + lrintf(motorOutputRange * (motorMix[i] + (throttle * currentMixer[i].throttle))); float motorOutput = motorOutputMin + motorOutputRange * (motorMix[i] + (throttle * currentMixer[i].throttle));
// Dshot works exactly opposite in lower 3D section. // Dshot works exactly opposite in lower 3D section.
if (mixerInversion) { if (mixerInversion) {
@ -642,7 +643,7 @@ void mixTable(pidProfile_t *pidProfile)
} }
} }
uint16_t convertExternalToMotor(uint16_t externalValue) float convertExternalToMotor(uint16_t externalValue)
{ {
uint16_t motorValue = externalValue; uint16_t motorValue = externalValue;
#ifdef USE_DSHOT #ifdef USE_DSHOT
@ -661,12 +662,12 @@ uint16_t convertExternalToMotor(uint16_t externalValue)
} }
#endif #endif
return motorValue; return (float)motorValue;
} }
uint16_t convertMotorToExternal(uint16_t motorValue) uint16_t convertMotorToExternal(float motorValue)
{ {
uint16_t externalValue = motorValue; uint16_t externalValue = lrintf(motorValue);
#ifdef USE_DSHOT #ifdef USE_DSHOT
if (isMotorProtocolDshot()) { if (isMotorProtocolDshot()) {
if (feature(FEATURE_3D) && motorValue >= DSHOT_MIN_THROTTLE && motorValue <= DSHOT_3D_DEADBAND_LOW) { if (feature(FEATURE_3D) && motorValue >= DSHOT_MIN_THROTTLE && motorValue <= DSHOT_3D_DEADBAND_LOW) {

8
src/main/flight/mixer.h
View File

@ -118,8 +118,8 @@ PG_DECLARE(motorConfig_t, motorConfig);
#define CHANNEL_FORWARDING_DISABLED (uint8_t)0xFF #define CHANNEL_FORWARDING_DISABLED (uint8_t)0xFF
extern const mixer_t mixers[]; extern const mixer_t mixers[];
extern int16_t motor[MAX_SUPPORTED_MOTORS]; extern float motor[MAX_SUPPORTED_MOTORS];
extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS]; extern float motor_disarmed[MAX_SUPPORTED_MOTORS];
struct rxConfig_s; struct rxConfig_s;
uint8_t getMotorCount(); uint8_t getMotorCount();
@ -141,5 +141,5 @@ void stopMotors(void);
void stopPwmAllMotors(void); void stopPwmAllMotors(void);
bool isMotorProtocolDshot(void); bool isMotorProtocolDshot(void);
uint16_t convertExternalToMotor(uint16_t externalValue); float convertExternalToMotor(uint16_t externalValue);
uint16_t convertMotorToExternal(uint16_t motorValue); uint16_t convertMotorToExternal(float motorValue);

2
src/main/target/COLIBRI_RACE/i2c_bst.c
View File

@ -139,7 +139,7 @@ static uint8_t activeBoxIds[CHECKBOX_ITEM_COUNT];
// this is the number of filled indexes in above array // this is the number of filled indexes in above array
static uint8_t activeBoxIdCount = 0; static uint8_t activeBoxIdCount = 0;
// from mixer.c // from mixer.c
extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS]; extern float motor_disarmed[MAX_SUPPORTED_MOTORS];
// cause reboot after BST processing complete // cause reboot after BST processing complete
static bool isRebootScheduled = false; static bool isRebootScheduled = false;

4
src/main/target/SITL/target.c
View File

@ -392,7 +392,7 @@ pwmOutputPort_t *pwmGetMotors(void) {
bool pwmAreMotorsEnabled(void) { bool pwmAreMotorsEnabled(void) {
return pwmMotorsEnabled; return pwmMotorsEnabled;
} }
void pwmWriteMotor(uint8_t index, uint16_t value) { void pwmWriteMotor(uint8_t index, float value) {
motorsPwm[index] = value - idlePulse; motorsPwm[index] = value - idlePulse;
} }
void pwmShutdownPulsesForAllMotors(uint8_t motorCount) { void pwmShutdownPulsesForAllMotors(uint8_t motorCount) {
@ -419,7 +419,7 @@ void pwmCompleteMotorUpdate(uint8_t motorCount) {
udpSend(&pwmLink, &pwmPkt, sizeof(servo_packet)); udpSend(&pwmLink, &pwmPkt, sizeof(servo_packet));
// printf("[pwm]%u:%u,%u,%u,%u\n", idlePulse, motorsPwm[0], motorsPwm[1], motorsPwm[2], motorsPwm[3]); // printf("[pwm]%u:%u,%u,%u,%u\n", idlePulse, motorsPwm[0], motorsPwm[1], motorsPwm[2], motorsPwm[3]);
} }
void pwmWriteServo(uint8_t index, uint16_t value) { void pwmWriteServo(uint8_t index, float value) {
servosPwm[index] = value; servosPwm[index] = value;
} }