From 13e21c969e278bd7272b0bc312fb9b47faee26b0 Mon Sep 17 00:00:00 2001 From: Bryce Johnson Date: Fri, 9 Jun 2017 01:55:04 -0500 Subject: [PATCH] Added Dshot commands for reversing the motors and beeper for blheli_s --- src/main/drivers/pwm_output.c | 2 +- src/main/drivers/pwm_output.h | 20 ++++++++++++++++++++ src/main/fc/fc_core.c | 21 ++++++++++++++++++++- src/main/fc/fc_core.h | 1 + src/main/fc/fc_msp.c | 4 +--- src/main/fc/fc_rc.c | 1 + src/main/flight/mixer.c | 11 +++++++---- src/main/io/beeper.c | 12 +++++++++++- 8 files changed, 62 insertions(+), 10 deletions(-) diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index 1624598a56a..34467696c17 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -338,7 +338,7 @@ void pwmWriteDshotCommand(uint8_t index, uint8_t command) motorDmaOutput_t *const motor = getMotorDmaOutput(index); unsigned repeats; - if ((command >= 7 && command <= 10) || command == 12) { + if ((DSHOT_CMD_SPIN_ONE_WAY >= 7 && DSHOT_CMD_3D_MODE_ON <= 10) || DSHOT_CMD_SAVE_SETTINGS == 12 || (DSHOT_CMD_ROTATE_NORMAL >= 20 && DSHOT_CMD_ROTATE_REVERSE <= 21) ) { repeats = 10; } else { repeats = 1; diff --git a/src/main/drivers/pwm_output.h b/src/main/drivers/pwm_output.h index f4249f4adf2..1a23e493ff4 100644 --- a/src/main/drivers/pwm_output.h +++ b/src/main/drivers/pwm_output.h @@ -28,6 +28,26 @@ #define MAX_SUPPORTED_SERVOS 8 #endif +typedef enum { + DSHOT_CMD_MOTOR_STOP = 0, + DSHOT_CMD_BEEP1, + DSHOT_CMD_BEEP2, + DSHOT_CMD_BEEP3, + DSHOT_CMD_BEEP4, + DSHOT_CMD_BEEP5, + DSHOT_CMD_ESC_INFO, + DSHOT_CMD_SPIN_ONE_WAY, + DSHOT_CMD_SPIN_OTHER_WAY, + DSHOT_CMD_3D_MODE_OFF, + DSHOT_CMD_3D_MODE_ON, + DSHOT_CMD_SETTINGS_REQUEST, + DSHOT_CMD_SAVE_SETTINGS, + DSHOT_CMD_ROTATE_NORMAL = 20, //Blheli_S only command + DSHOT_CMD_ROTATE_REVERSE = 21, //Blheli_S only command + DSHOT_CMD_MAX = 47 +} dshotCommands_e; + + typedef enum { PWM_TYPE_STANDARD = 0, PWM_TYPE_ONESHOT125, diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index b70b08719f0..bba6594a3af 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -106,7 +106,7 @@ int16_t magHold; int16_t headFreeModeHold; uint8_t motorControlEnable = false; - +static bool reverseMotors; static uint32_t disarmAt; // Time of automatic disarm when "Don't spin the motors when armed" is enabled and auto_disarm_delay is nonzero bool isRXDataNew; @@ -205,6 +205,20 @@ void mwArm(void) return; } if (!ARMING_FLAG(PREVENT_ARMING)) { + #ifdef USE_DSHOT + if (!feature(FEATURE_3D) && !IS_RC_MODE_ACTIVE(BOX3DDISABLESWITCH)) { + reverseMotors = false; + for (unsigned index = 0; index < getMotorCount(); index++) { + pwmWriteDshotCommand(index, DSHOT_CMD_ROTATE_NORMAL); + } + } + if (!feature(FEATURE_3D) && IS_RC_MODE_ACTIVE(BOX3DDISABLESWITCH)) { + reverseMotors = true; + for (unsigned index = 0; index < getMotorCount(); index++) { + pwmWriteDshotCommand(index, DSHOT_CMD_ROTATE_REVERSE); + } + } + #endif ENABLE_ARMING_FLAG(ARMED); ENABLE_ARMING_FLAG(WAS_EVER_ARMED); headFreeModeHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw); @@ -644,3 +658,8 @@ void taskMainPidLoop(timeUs_t currentTimeUs) runTaskMainSubprocesses = true; } } + +bool isMotorsReversed() +{ + return reverseMotors; +} diff --git a/src/main/fc/fc_core.h b/src/main/fc/fc_core.h index e242886a1ad..baba693d5c5 100644 --- a/src/main/fc/fc_core.h +++ b/src/main/fc/fc_core.h @@ -48,3 +48,4 @@ void updateLEDs(void); void updateRcCommands(void); void taskMainPidLoop(timeUs_t currentTimeUs); +bool isMotorsReversed(void); \ No newline at end of file diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 690e2222c0a..abaa5c27275 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -370,9 +370,7 @@ void initActiveBoxIds(void) BME(BOXFPVANGLEMIX); - if (feature(FEATURE_3D)) { - BME(BOX3DDISABLESWITCH); - } + BME(BOX3DDISABLESWITCH); if (feature(FEATURE_SERVO_TILT)) { BME(BOXCAMSTAB); diff --git a/src/main/fc/fc_rc.c b/src/main/fc/fc_rc.c index 2511fcfd5c0..63f305b537d 100755 --- a/src/main/fc/fc_rc.c +++ b/src/main/fc/fc_rc.c @@ -46,6 +46,7 @@ #include "flight/failsafe.h" #include "flight/imu.h" #include "flight/pid.h" +#include "flight/mixer.h" static float setpointRate[3], rcDeflection[3], rcDeflectionAbs[3]; static float throttlePIDAttenuation; diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 094e16ae5b9..de7c331f705 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -41,6 +41,7 @@ #include "fc/config.h" #include "fc/rc_controls.h" #include "fc/runtime_config.h" +#include "fc/fc_core.h" #include "flight/failsafe.h" #include "flight/imu.h" @@ -119,7 +120,6 @@ PG_REGISTER_ARRAY(motorMixer_t, MAX_SUPPORTED_MOTORS, customMotorMixer, PG_MOTOR #define TRICOPTER_ERROR_RATE_YAW_SATURATED 75 // rate at which tricopter yaw axis becomes saturated, determined experimentally by TriFlight - static uint8_t motorCount; static float motorMixRange; @@ -576,11 +576,14 @@ void mixTable(pidProfile_t *pidProfile) float motorMix[MAX_SUPPORTED_MOTORS]; float motorMixMax = 0, motorMixMin = 0; const int yawDirection = GET_DIRECTION(mixerConfig()->yaw_motors_reversed); + int motorDirection = GET_DIRECTION(isMotorsReversed()); + + for (int i = 0; i < motorCount; i++) { float mix = - scaledAxisPidRoll * currentMixer[i].roll + - scaledAxisPidPitch * currentMixer[i].pitch + - scaledAxisPidYaw * currentMixer[i].yaw * (-yawDirection); + scaledAxisPidRoll * currentMixer[i].roll * (motorDirection) + + scaledAxisPidPitch * currentMixer[i].pitch * (motorDirection) + + scaledAxisPidYaw * currentMixer[i].yaw * (-yawDirection) * (motorDirection); if (vbatCompensationFactor > 1.0f) { mix *= vbatCompensationFactor; // Add voltage compensation diff --git a/src/main/io/beeper.c b/src/main/io/beeper.c index 9b610eadf5b..3b35e4be8f0 100644 --- a/src/main/io/beeper.c +++ b/src/main/io/beeper.c @@ -28,6 +28,9 @@ #include "drivers/sound_beeper.h" #include "drivers/time.h" +#include "drivers/pwm_output.h" + +#include "flight/mixer.h" #include "fc/config.h" #include "fc/runtime_config.h" @@ -153,7 +156,6 @@ static uint8_t beep_multiBeeps[MAX_MULTI_BEEPS + 2]; #define BEEPER_CONFIRMATION_BEEP_DURATION 2 #define BEEPER_CONFIRMATION_BEEP_GAP_DURATION 20 - // Beeper off = 0 Beeper on = 1 static uint8_t beeperIsOn = 0; @@ -338,6 +340,14 @@ void beeperUpdate(timeUs_t currentTimeUs) return; } + #ifdef USE_DSHOT + if (!ARMING_FLAG(ARMED) && currentBeeperEntry->mode == BEEPER_RX_SET) { + for (unsigned index = 0; index < getMotorCount(); index++) { + pwmWriteDshotCommand(index, DSHOT_CMD_BEEP3); + } + } + #endif + if (!beeperIsOn) { beeperIsOn = 1; if (currentBeeperEntry->sequence[beeperPos] != 0) {