1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-21 07:15:18 +03:00

Inhibit Dshot motor beeping while motors are running (armed or motor test).

This commit is contained in:
mikeller 2017-08-22 22:54:55 +12:00
parent dfe868ef96
commit c8f528c0af
5 changed files with 27 additions and 24 deletions

View file

@ -925,25 +925,8 @@ void stopInTestMode(void)
*/
bool inMotorTestMode(void) {
static uint32_t resetTime = 0;
uint16_t inactiveMotorCommand;
if (feature(FEATURE_3D)) {
inactiveMotorCommand = flight3DConfig()->neutral3d;
#ifdef USE_DSHOT
} else if (isMotorProtocolDshot()) {
inactiveMotorCommand = DSHOT_DISARM_COMMAND;
#endif
} else {
inactiveMotorCommand = motorConfig()->mincommand;
}
int i;
bool atLeastOneMotorActivated = false;
// set disarmed motor values
for (i = 0; i < MAX_SUPPORTED_MOTORS; i++)
atLeastOneMotorActivated |= (motor_disarmed[i] != inactiveMotorCommand);
if (atLeastOneMotorActivated) {
if (!ARMING_FLAG(ARMED) && areMotorsRunning()) {
resetTime = millis() + 5000; // add 5 seconds
return true;
} else {

View file

@ -316,16 +316,34 @@ static float disarmMotorOutput, deadbandMotor3dHigh, deadbandMotor3dLow;
float motorOutputHigh, motorOutputLow;
static float rcCommandThrottleRange, rcCommandThrottleRange3dLow, rcCommandThrottleRange3dHigh;
uint8_t getMotorCount()
uint8_t getMotorCount(void)
{
return motorCount;
}
float getMotorMixRange()
float getMotorMixRange(void)
{
return motorMixRange;
}
bool areMotorsRunning(void)
{
bool motorsRunning = false;
if (ARMING_FLAG(ARMED)) {
motorsRunning = true;
} else {
for (int i = 0; i < motorCount; i++) {
if (motor_disarmed[i] != disarmMotorOutput) {
motorsRunning = true;
break;
}
}
}
return motorsRunning;
}
bool mixerIsOutputSaturated(int axis, float errorRate)
{
if (axis == FD_YAW && (currentMixerMode == MIXER_TRI || currentMixerMode == MIXER_CUSTOM_TRI)) {

View file

@ -107,8 +107,9 @@ extern float motor[MAX_SUPPORTED_MOTORS];
extern float motor_disarmed[MAX_SUPPORTED_MOTORS];
struct rxConfig_s;
uint8_t getMotorCount();
float getMotorMixRange();
uint8_t getMotorCount(void);
float getMotorMixRange(void);
bool areMotorsRunning(void);
bool mixerIsOutputSaturated(int axis, float errorRate);
void mixerLoadMix(int index, motorMixer_t *customMixers);

View file

@ -363,7 +363,7 @@ void beeperUpdate(timeUs_t currentTimeUs)
}
#ifdef USE_DSHOT
if (!ARMING_FLAG(ARMED) && beeperConfig()->dshotForward && currentBeeperEntry->mode == BEEPER_RX_SET) {
if (!areMotorsRunning() && beeperConfig()->dshotForward && currentBeeperEntry->mode == BEEPER_RX_SET) {
pwmWriteDshotCommand(ALL_MOTORS, getMotorCount(), DSHOT_CMD_BEEP3);
}
#endif

View file

@ -376,7 +376,8 @@ uint32_t rcModeActivationMask;
void mspSerialAllocatePorts(void) {}
uint32_t getArmingBeepTimeMicros(void) {return 0;}
uint16_t getBatteryVoltageLatest(void) {return 0;}
uint8_t getMotorCount() {return 4;}
uint8_t getMotorCount(void) {return 4;}
bool areMotorsRunning(void) { return false; }
bool IS_RC_MODE_ACTIVE(boxId_e) {return false;}
bool isModeActivationConditionPresent(boxId_e) {return false;}
uint32_t millis(void) {return 0;}