Merge pull request #3267 from brycedjohnson/dshotcommand

Added anti-turtle reversemotors and beeper using dshot commands on blheli_s

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;

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,

src/main/fc/fc_core.c

@@ -107,7 +107,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;
@@ -206,6 +206,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);
@@ -645,3 +659,8 @@ void taskMainPidLoop(timeUs_t currentTimeUs)
         runTaskMainSubprocesses = true;
     }
 }
+
+bool isMotorsReversed()
+{
+    return reverseMotors;
+}

src/main/fc/fc_core.h

@@ -48,3 +48,4 @@ void updateLEDs(void);
 void updateRcCommands(void);
 
 void taskMainPidLoop(timeUs_t currentTimeUs);
+bool isMotorsReversed(void);

src/main/fc/fc_msp.c

@@ -370,9 +370,7 @@ void initActiveBoxIds(void)
 
     BME(BOXFPVANGLEMIX);
 
-    if (feature(FEATURE_3D)) {
     BME(BOX3DDISABLESWITCH);
-    }
 
     if (feature(FEATURE_SERVO_TILT)) {
         BME(BOXCAMSTAB);

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;

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

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) {