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()
//From mixer.c:
extern uint16_t motorOutputHigh, motorOutputLow;
extern float motorOutputHigh, motorOutputLow;
//From rc_controls.c
extern boxBitmask_t rcModeActivationMask;
@ -1188,6 +1188,9 @@ static bool sendFieldDefinition(char mainFrameChar, char deltaFrameChar, const v
*/
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)
if (blackboxDeviceReserveBufferSpace(64) != BLACKBOX_RESERVE_SUCCESS) {
return false;
@ -1203,7 +1206,7 @@ static bool blackboxWriteSysinfo(void)
BLACKBOX_PRINT_HEADER_LINE("minthrottle", "%d", motorConfig()->minthrottle);
BLACKBOX_PRINT_HEADER_LINE("maxthrottle", "%d", motorConfig()->maxthrottle);
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_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;
}
static void pwmWriteUnused(uint8_t index, uint16_t value)
static void pwmWriteUnused(uint8_t index, float value)
{
UNUSED(index);
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) {
pwmWritePtr(index, value);
@ -374,10 +374,10 @@ void pwmWriteDshotCommand(uint8_t index, uint8_t command)
#endif
#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) {
*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;
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 struct {
@ -167,8 +167,8 @@ void pwmServoConfig(const struct timerHardware_s *timerHardware, uint8_t servoIn
#ifdef USE_DSHOT
uint32_t getDshotHz(motorPwmProtocolTypes_e pwmProtocolType);
void pwmWriteDshotCommand(uint8_t index, uint8_t command);
void pwmWriteProShot(uint8_t index, uint16_t value);
void pwmWriteDshot(uint8_t index, uint16_t value);
void pwmWriteProShot(uint8_t index, float value);
void pwmWriteDshot(uint8_t index, float value);
void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output);
void pwmCompleteDigitalMotorUpdate(uint8_t motorCount);
#endif
@ -179,11 +179,11 @@ void pwmToggleBeeper(void);
void beeperPwmInit(IO_t io, uint16_t frequency);
#endif
void pwmWriteMotor(uint8_t index, uint16_t value);
void pwmWriteMotor(uint8_t index, float value);
void pwmShutdownPulsesForAllMotors(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);
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;
}
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];
if (!motor->timerHardware || !motor->timerHardware->dmaRef) {
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
// compute checksum
@ -85,15 +87,17 @@ void pwmWriteDshot(uint8_t index, uint16_t value)
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];
if (!motor->timerHardware || !motor->timerHardware->dmaRef) {
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
// 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;
}
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];
if (!motor->timerHardware || !motor->timerHardware->dmaRef) {
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
// 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];
if (!motor->timerHardware || !motor->timerHardware->dmaRef) {
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
// 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 float motorMixRange;
int16_t motor[MAX_SUPPORTED_MOTORS];
int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
float motor[MAX_SUPPORTED_MOTORS];
float motor_disarmed[MAX_SUPPORTED_MOTORS];
mixerMode_e currentMixerMode;
static motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS];
@ -312,8 +312,8 @@ const mixer_t mixers[] = {
};
#endif // !USE_QUAD_MIXER_ONLY
static uint16_t disarmMotorOutput, deadbandMotor3dHigh, deadbandMotor3dLow;
uint16_t motorOutputHigh, motorOutputLow;
static float disarmMotorOutput, deadbandMotor3dHigh, deadbandMotor3dLow;
float motorOutputHigh, motorOutputLow;
static float rcCommandThrottleRange, rcCommandThrottleRange3dLow, rcCommandThrottleRange3dHigh;
uint8_t getMotorCount()
@ -352,17 +352,18 @@ bool isMotorProtocolDshot(void) {
#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) {
#ifdef USE_DSHOT
if (isMotorProtocolDshot()) {
disarmMotorOutput = DSHOT_DISARM_COMMAND;
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
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;
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;
} else
#endif
@ -509,7 +510,7 @@ void mixTable(pidProfile_t *pidProfile)
// Scale roll/pitch/yaw uniformly to fit within throttle range
// Initial mixer concept by bdoiron74 reused and optimized for Air Mode
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
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
// roll/pitch/yaw. This could move throttle down, but also up for those low throttle flips.
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.
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;
#ifdef USE_DSHOT
@ -661,12 +662,12 @@ uint16_t convertExternalToMotor(uint16_t externalValue)
}
#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
if (isMotorProtocolDshot()) {
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
extern const mixer_t mixers[];
extern int16_t motor[MAX_SUPPORTED_MOTORS];
extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
extern float motor[MAX_SUPPORTED_MOTORS];
extern float motor_disarmed[MAX_SUPPORTED_MOTORS];
struct rxConfig_s;
uint8_t getMotorCount();
@ -141,5 +141,5 @@ void stopMotors(void);
void stopPwmAllMotors(void);
bool isMotorProtocolDshot(void);
uint16_t convertExternalToMotor(uint16_t externalValue);
uint16_t convertMotorToExternal(uint16_t motorValue);
float convertExternalToMotor(uint16_t externalValue);
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
static uint8_t activeBoxIdCount = 0;
// from mixer.c
extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
extern float motor_disarmed[MAX_SUPPORTED_MOTORS];
// cause reboot after BST processing complete
static bool isRebootScheduled = false;

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

@ -392,7 +392,7 @@ pwmOutputPort_t *pwmGetMotors(void) {
bool pwmAreMotorsEnabled(void) {
return pwmMotorsEnabled;
}
void pwmWriteMotor(uint8_t index, uint16_t value) {
void pwmWriteMotor(uint8_t index, float value) {
motorsPwm[index] = value - idlePulse;
}
void pwmShutdownPulsesForAllMotors(uint8_t motorCount) {
@ -419,7 +419,7 @@ void pwmCompleteMotorUpdate(uint8_t motorCount) {
udpSend(&pwmLink, &pwmPkt, sizeof(servo_packet));
// 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;
}