Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-23 16:25:26 +03:00
Code Issues Wiki Activity

Unify protocol scaling handling

Browse source
This commit is contained in:
Pawel Spychalski (DzikuVx) 2020-02-23 20:32:22 +01:00
parent 0ffcac2974
commit af5b36b0fe
2 changed files with 81 additions and 30 deletions
Download patch file Download diff file Expand all files Collapse all files

2
src/main/fc/rc_controls.c
View file

@ -99,7 +99,7 @@ bool areSticksDeflectedMoreThanPosHoldDeadband(void)
return (ABS(rcCommand[ROLL]) > rcControlsConfig()->pos_hold_deadband) || (ABS(rcCommand[PITCH]) > rcControlsConfig()->pos_hold_deadband); return (ABS(rcCommand[ROLL]) > rcControlsConfig()->pos_hold_deadband) || (ABS(rcCommand[PITCH]) > rcControlsConfig()->pos_hold_deadband);
} }
throttleStatus_e calculateThrottleStatus(throttleStatusType_e type) throttleStatus_e FAST_CODE NOINLINE calculateThrottleStatus(throttleStatusType_e type)
{ {
int value; int value;
if (type == THROTTLE_STATUS_TYPE_RC) { if (type == THROTTLE_STATUS_TYPE_RC) {

109
src/main/flight/mixer.c
View file

@ -283,6 +283,34 @@ void mixerResetDisarmedMotors(void)
} }
} }
static FAST_CODE NOINLINE uint16_t handleOutputScaling(
int16_t input, // Input value from the mixer
int16_t stopThreshold, // Threshold value to check if motor should be rotating or not
int16_t onStopValue, // Value sent to the ESC when min rotation is required - on motor_stop it is STOP command, without motor_stop it's a value that keeps rotation
int16_t inputScaleMin, // Input range - min value
int16_t inputScaleMax, // Input range - max value
int16_t outputScaleMin, // Output range - min value
int16_t outputScaleMax, // Output range - max value
bool moveForward // If motor should be rotating FORWARD or BACKWARD
)
{
int value;
if (moveForward && input < stopThreshold) {
//Send motor stop command
value = onStopValue;
}
else if (!moveForward && input > stopThreshold) {
//Send motor stop command
value = onStopValue;
}
else {
//Scale input to protocol output values
value = scaleRangef(input, inputScaleMin, inputScaleMax, outputScaleMin, outputScaleMax);
value = constrain(value, outputScaleMin, outputScaleMax);
}
return value;
}
void FAST_CODE NOINLINE writeMotors(void) void FAST_CODE NOINLINE writeMotors(void)
{ {
for (int i = 0; i < motorCount; i++) { for (int i = 0; i < motorCount; i++) {
@ -293,44 +321,67 @@ void FAST_CODE NOINLINE writeMotors(void)
if (isMotorProtocolDigital()) { if (isMotorProtocolDigital()) {
if (feature(FEATURE_REVERSIBLE_MOTORS)) { if (feature(FEATURE_REVERSIBLE_MOTORS)) {
if (motor[i] >= throttleIdleValue && motor[i] <= reversibleMotorsConfig()->deadband_low) { if (reversibleMotorsThrottleState == MOTOR_DIRECTION_FORWARD) {
motorValue = scaleRangef(motor[i], motorConfig()->mincommand, reversibleMotorsConfig()->deadband_low, DSHOT_3D_DEADBAND_LOW, DSHOT_MIN_THROTTLE); motorValue = handleOutputScaling(
motorValue = constrain(motorValue, DSHOT_MIN_THROTTLE, DSHOT_3D_DEADBAND_LOW); motor[i],
} throttleRangeMin,
else if (motor[i] >= reversibleMotorsConfig()->deadband_high && motor[i] <= motorConfig()->maxthrottle) { DSHOT_DISARM_COMMAND,
motorValue = scaleRangef(motor[i], reversibleMotorsConfig()->deadband_high, motorConfig()->maxthrottle, DSHOT_3D_DEADBAND_HIGH, DSHOT_MAX_THROTTLE); throttleRangeMin,
motorValue = constrain(motorValue, DSHOT_3D_DEADBAND_HIGH, DSHOT_MAX_THROTTLE); throttleRangeMax,
} DSHOT_3D_DEADBAND_HIGH,
else { DSHOT_MAX_THROTTLE,
motorValue = DSHOT_DISARM_COMMAND; true
);
} else {
motorValue = handleOutputScaling(
motor[i],
throttleRangeMax,
DSHOT_DISARM_COMMAND,
throttleRangeMin,
throttleRangeMax,
DSHOT_MIN_THROTTLE,
DSHOT_3D_DEADBAND_LOW,
false
);
} }
} }
else { else {
if (motor[i] < throttleIdleValue) { // motor disarmed motorValue = handleOutputScaling(
motorValue = DSHOT_DISARM_COMMAND; motor[i],
} throttleIdleValue,
else { DSHOT_DISARM_COMMAND,
motorValue = scaleRangef(motor[i], motorConfig()->mincommand, motorConfig()->maxthrottle, DSHOT_MIN_THROTTLE, DSHOT_MAX_THROTTLE); motorConfig()->mincommand,
motorValue = constrain(motorValue, DSHOT_MIN_THROTTLE, DSHOT_MAX_THROTTLE); motorConfig()->maxthrottle,
} DSHOT_MIN_THROTTLE,
DSHOT_MAX_THROTTLE,
true
);
} }
} }
else { else {
if (feature(FEATURE_REVERSIBLE_MOTORS)) { if (feature(FEATURE_REVERSIBLE_MOTORS)) {
if (reversibleMotorsThrottleState == MOTOR_DIRECTION_FORWARD) { if (reversibleMotorsThrottleState == MOTOR_DIRECTION_FORWARD) {
if (motor[i] < throttleRangeMin) { motorValue = handleOutputScaling(
motorValue = motor[i]; motor[i],
} else { throttleRangeMin,
motorValue = scaleRangef(motor[i], throttleRangeMin, throttleRangeMax, reversibleMotorsConfig()->deadband_high, motorConfig()->maxthrottle); motor[i],
motorValue = constrain(motorValue, reversibleMotorsConfig()->deadband_high, motorConfig()->maxthrottle); throttleRangeMin,
} throttleRangeMax,
reversibleMotorsConfig()->deadband_high,
motorConfig()->maxthrottle,
true
);
} else { } else {
if (motor[i] > throttleRangeMax) { motorValue = handleOutputScaling(
motorValue = motor[i]; motor[i],
} else { throttleRangeMax,
motorValue = scaleRangef(motor[i], throttleRangeMin, throttleRangeMax, motorConfig()->mincommand, reversibleMotorsConfig()->deadband_low); motor[i],
motorValue = constrain(motorValue, motorConfig()->mincommand, reversibleMotorsConfig()->deadband_low); throttleRangeMin,
} throttleRangeMax,
motorConfig()->mincommand,
reversibleMotorsConfig()->deadband_low,
false
);
} }
} else { } else {
motorValue = motor[i]; motorValue = motor[i];