diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 257038ca58..c53364bc18 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -637,7 +637,7 @@ static void writeIntraframe(void) if (isFieldEnabled(FIELD_SELECT(MOTOR))) { //Motors can be below minimum output when disarmed, but that doesn't happen much - blackboxWriteUnsignedVB(blackboxCurrent->motor[0] - motorOutputLow); + blackboxWriteUnsignedVB(blackboxCurrent->motor[0] - getMotorOutputLow()); //Motors tend to be similar to each other so use the first motor's value as a predictor of the others const int motorCount = getMotorCount(); @@ -1262,8 +1262,8 @@ STATIC_UNIT_TESTED char *blackboxGetStartDateTime(char *buf) static bool blackboxWriteSysinfo(void) { #ifndef UNIT_TEST - const uint16_t motorOutputLowInt = lrintf(motorOutputLow); - const uint16_t motorOutputHighInt = lrintf(motorOutputHigh); + const uint16_t motorOutputLowInt = lrintf(getMotorOutputLow()); + const uint16_t motorOutputHighInt = lrintf(getMotorOutputHigh()); // 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) { @@ -1292,7 +1292,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", motorOutputLowInt,motorOutputHighInt); + BLACKBOX_PRINT_HEADER_LINE("motorOutput", "%d,%d", motorOutputLowInt, motorOutputHighInt); #if defined(USE_ACC) BLACKBOX_PRINT_HEADER_LINE("acc_1G", "%u", acc.dev.acc_1G); #endif diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index cd9ec53147..a0ad8b4f8a 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -284,7 +284,9 @@ const mixer_t mixers[] = { }; #endif // !USE_QUAD_MIXER_ONLY -FAST_DATA_ZERO_INIT float motorOutputHigh, motorOutputLow; +static FAST_DATA_ZERO_INIT bool feature3dEnabled; +static FAST_DATA_ZERO_INIT float motorOutputLow; +static FAST_DATA_ZERO_INIT float motorOutputHigh; static FAST_DATA_ZERO_INIT float disarmMotorOutput, deadbandMotor3dHigh, deadbandMotor3dLow; static FAST_DATA_ZERO_INIT float rcCommandThrottleRange; @@ -346,6 +348,9 @@ void initEscEndpoints(void) } motorInitEndpoints(motorConfig(), motorOutputLimit, &motorOutputLow, &motorOutputHigh, &disarmMotorOutput, &deadbandMotor3dHigh, &deadbandMotor3dLow); + if (!feature3dEnabled && currentPidProfile->idle_min_rpm) { + motorOutputLow = DSHOT_MIN_THROTTLE; + } rcCommandThrottleRange = PWM_RANGE_MAX - PWM_RANGE_MIN; } @@ -377,6 +382,8 @@ void mixerInit(mixerMode_e mixerMode) { currentMixerMode = mixerMode; + feature3dEnabled = featureIsEnabled(FEATURE_3D); + initEscEndpoints(); #ifdef USE_SERVOS if (mixerIsTricopter()) { @@ -512,9 +519,9 @@ static void calculateThrottleAndCurrentMotorEndpoints(timeUs_t currentTimeUs) static uint16_t rcThrottlePrevious = 0; // Store the last throttle direction for deadband transitions static timeUs_t reversalTimeUs = 0; // time when motors last reversed in 3D mode static float motorRangeMinIncrease = 0; - float currentThrottleInputRange = 0; - if (featureIsEnabled(FEATURE_3D)) { + float currentThrottleInputRange = 0; + if (feature3dEnabled) { uint16_t rcCommand3dDeadBandLow; uint16_t rcCommand3dDeadBandHigh; @@ -617,10 +624,8 @@ static void calculateThrottleAndCurrentMotorEndpoints(timeUs_t currentTimeUs) } } else { throttle = rcCommand[THROTTLE] - PWM_RANGE_MIN + throttleAngleCorrection; - float appliedMotorOutputLow = motorOutputLow; #ifdef USE_DYN_IDLE if (idleMinMotorRps > 0.0f) { - appliedMotorOutputLow = DSHOT_MIN_THROTTLE; const float maxIncrease = isAirmodeActivated() ? idleMaxIncrease : 0.04f; const float minRps = rpmMinMotorFrequency(); const float targetRpsChangeRate = (idleMinMotorRps - minRps) * currentPidProfile->idle_adjustment_speed; @@ -656,7 +661,7 @@ static void calculateThrottleAndCurrentMotorEndpoints(timeUs_t currentTimeUs) #endif currentThrottleInputRange = rcCommandThrottleRange; - motorRangeMin = appliedMotorOutputLow + motorRangeMinIncrease * (motorOutputHigh - appliedMotorOutputLow); + motorRangeMin = motorOutputLow + motorRangeMinIncrease * (motorOutputHigh - motorOutputLow); motorOutputMin = motorRangeMin; motorOutputRange = motorRangeMax - motorRangeMin; motorOutputMixSign = 1; @@ -963,7 +968,7 @@ FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs) if (featureIsEnabled(FEATURE_MOTOR_STOP) && ARMING_FLAG(ARMED) - && !featureIsEnabled(FEATURE_3D) + && !feature3dEnabled && !airmodeEnabled && !FLIGHT_MODE(GPS_RESCUE_MODE) // disable motor_stop while GPS Rescue is active && (rcData[THROTTLE] < rxConfig()->mincheck)) { @@ -1010,3 +1015,13 @@ bool isFixedWing(void) { return mixerModeIsFixedWing(currentMixerMode); } + +float getMotorOutputLow(void) +{ + return motorOutputLow; +} + +float getMotorOutputHigh(void) +{ + return motorOutputHigh; +} diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index 4b923a896a..ba89bb0fe9 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -91,7 +91,6 @@ PG_DECLARE(mixerConfig_t, mixerConfig); extern const mixer_t mixers[]; extern float motor[MAX_SUPPORTED_MOTORS]; extern float motor_disarmed[MAX_SUPPORTED_MOTORS]; -extern float motorOutputHigh, motorOutputLow; struct rxConfig_s; uint8_t getMotorCount(void); @@ -117,3 +116,6 @@ float mixerGetThrottle(void); mixerMode_e getMixerMode(void); bool mixerModeIsFixedWing(mixerMode_e mixerMode); bool isFixedWing(void); + +float getMotorOutputLow(void); +float getMotorOutputHigh(void); diff --git a/src/main/osd/osd_elements.c b/src/main/osd/osd_elements.c index 5e238fc789..31bb0b69ac 100644 --- a/src/main/osd/osd_elements.c +++ b/src/main/osd/osd_elements.c @@ -1050,7 +1050,7 @@ static void osdElementMotorDiagnostics(osdElementParms_t *element) const bool motorsRunning = areMotorsRunning(); for (; i < getMotorCount(); i++) { if (motorsRunning) { - element->buff[i] = 0x88 - scaleRange(motor[i], motorOutputLow, motorOutputHigh, 0, 8); + element->buff[i] = 0x88 - scaleRange(motor[i], getMotorOutputLow(), getMotorOutputHigh(), 0, 8); } else { element->buff[i] = 0x88; } diff --git a/src/test/unit/blackbox_unittest.cc b/src/test/unit/blackbox_unittest.cc index 3cc1df0bf5..149ea44b79 100644 --- a/src/test/unit/blackbox_unittest.cc +++ b/src/test/unit/blackbox_unittest.cc @@ -352,7 +352,6 @@ int32_t GPS_home[2]; gyro_t gyro; -float motorOutputHigh, motorOutputLow; float motor_disarmed[MAX_SUPPORTED_MOTORS]; struct pidProfile_s; struct pidProfile_s *currentPidProfile; @@ -383,5 +382,6 @@ failsafePhase_e failsafePhase(void) {return FAILSAFE_IDLE;} bool rxAreFlightChannelsValid(void) {return false;} bool rxIsReceivingSignal(void) {return false;} bool isRssiConfigured(void) {return false;} - +float getMotorOutputLow(void) {return 0.0;} +float getMotorOutputHigh(void) {return 0.0;} } diff --git a/src/test/unit/link_quality_unittest.cc b/src/test/unit/link_quality_unittest.cc index 84f73d1328..26d4619f1e 100644 --- a/src/test/unit/link_quality_unittest.cc +++ b/src/test/unit/link_quality_unittest.cc @@ -73,8 +73,6 @@ extern "C" { int32_t GPS_coord[2]; gpsSolutionData_t gpsSol; float motor[8]; - float motorOutputHigh = 2047; - float motorOutputLow = 1000; acc_t acc; float accAverage[XYZ_AXIS_COUNT]; @@ -551,4 +549,8 @@ extern "C" { } bool isUpright(void) { return true; } + + float getMotorOutputLow(void) { return 1000.0; } + + float getMotorOutputHigh(void) { return 2047.0; } } diff --git a/src/test/unit/osd_unittest.cc b/src/test/unit/osd_unittest.cc index 2b1e681fec..f15baf0064 100644 --- a/src/test/unit/osd_unittest.cc +++ b/src/test/unit/osd_unittest.cc @@ -78,8 +78,6 @@ extern "C" { int32_t GPS_coord[2]; gpsSolutionData_t gpsSol; float motor[8]; - float motorOutputHigh = 2047; - float motorOutputLow = 1000; linkQualitySource_e linkQualitySource; @@ -1244,4 +1242,6 @@ extern "C" { uint32_t persistentObjectRead(persistentObjectId_e) { return 0; } void persistentObjectWrite(persistentObjectId_e, uint32_t) {} bool isUpright(void) { return true; } + float getMotorOutputLow(void) { return 1000.0; } + float getMotorOutputHigh(void) { return 2047.0; } }