dshot: send direction commands on every arm and on turtle disarm

src/drivers/drv_motor.h

@@ -20,7 +20,8 @@ void motor_init();
 void motor_wait_for_ready();
 void motor_beep();
 void motor_write(float *values);
-bool motor_set_direction(motor_direction_t dir);
+void motor_set_direction(motor_direction_t dir);
+bool motor_direction_change_done();
 
 // generic functions
 void motor_set(uint8_t number, float pwm);

src/drivers/drv_motor_dshot.c

@@ -16,7 +16,7 @@
 #if defined(USE_DSHOT_DMA_DRIVER)
 
 #define DSHOT_TIME profile.motor.dshot_time
-#define DSHOT_SYMBOL_TIME (PWM_CLOCK_FREQ_HZ / (3 * DSHOT_TIME * 1000 - 1))
+#define DSHOT_SYMBOL_TIME (PWM_CLOCK_FREQ_HZ / (3 * DSHOT_TIME * 1000) - 1)
 
 #define DSHOT_CMD_BEEP1 1
 #define DSHOT_CMD_BEEP2 2
@@ -59,8 +59,8 @@ typedef struct {
   dma_device_t dma_device;
 } dshot_gpio_port_t;
 
+static bool dir_change_done = true;
 static motor_direction_t motor_dir = MOTOR_FORWARD;
-static motor_direction_t last_motor_dir = MOTOR_FORWARD;
 
 static uint32_t pwm_failsafe_time = 1;
 static uint32_t dir_change_time = 0;
@@ -331,7 +331,7 @@ void motor_wait_for_ready() {
 }
 
 void motor_write(float *values) {
-  if (motor_dir == last_motor_dir) {
+  if (dir_change_done) {
     for (uint32_t i = 0; i < MOTOR_PIN_MAX; i++) {
       uint16_t value = 0;
 
@@ -404,25 +404,22 @@ void motor_write(float *values) {
     }
 
     case DIR_CHANGE_STOP:
-      last_motor_dir = motor_dir;
+      dir_change_done = true;
       state = DIR_CHANGE_START;
       break;
     }
   }
 }
 
-bool motor_set_direction(motor_direction_t dir) {
-  if (last_motor_dir != motor_dir) {
-    // the last direction change is not done yet
-    return false;
-  }
-  if (motor_dir != dir) {
-    // update the motor direction
+void motor_set_direction(motor_direction_t dir) {
+  if (dir_change_done) {
     motor_dir = dir;
-    return false;
+    dir_change_done = false;
   }
-  // success
-  return true;
+}
+
+bool motor_direction_change_done() {
+  return dir_change_done;
 }
 
 void motor_beep() {

src/drivers/drv_motor_pwm.c

@@ -134,7 +134,10 @@ void motor_write(float *values) {
   }
 }
 
-bool motor_set_direction(motor_direction_t dir) {
+void motor_set_direction(motor_direction_t dir) {
+}
+
+bool motor_direction_change_done() {
   return true;
 }
 

src/flight/control.c

@@ -319,6 +319,10 @@ void control() {
     if (!checked_prearm && rx_aux_on(AUX_PREARM)) {
       // CONDITION: AUX_PREARM is high AND we have not checked prearm this arm cycle
       flags.arm_switch = 1;
+
+      if (!flags.turtle_ready) {
+        motor_set_direction(MOTOR_FORWARD);
+      }
     } else if (!flags.arm_switch) {
       // throw up arming safety if we didnt manage to arm
       flags.arm_safety = 1;

src/flight/control.h

@@ -30,6 +30,7 @@ typedef struct {
   uint8_t controls_override : 1;  // will activate rx_override below & will write directly to the motors (motor_test)
   uint8_t motortest_override : 1; // tuns off digital idle in the dshot driver & will write either sticks or usb_motortest values directly to motors
   uint8_t turtle : 1;
+  uint8_t turtle_ready : 1;
   uint8_t gestures_disabled : 1;
 
   uint8_t usb_active : 1;

src/flight/turtle_mode.c

@@ -20,20 +20,19 @@ typedef enum {
 } turtle_mode_stage_t;
 
 static turtle_mode_stage_t turtle_state = TURTLE_STAGE_IDLE;
-static bool turtle_ready = false;
 static uint32_t turtle_time = 0;
 static uint8_t turtle_axis = 0;
 static uint8_t turtle_dir = 0;
 
 void turtle_mode_start() {
-  if (!turtle_ready && flags.on_ground && turtle_state == TURTLE_STAGE_IDLE) {
+  if (!flags.turtle_ready && flags.on_ground && turtle_state == TURTLE_STAGE_IDLE && (state.GEstG.yaw < 0)) {
     // only enable turtle if we are onground and not recovering from a interrupted turtle
-    turtle_ready = true;
+    flags.turtle_ready = 1;
   }
 }
 
 void turtle_mode_cancel() {
-  turtle_ready = false;
+  flags.turtle_ready = 0;
 }
 
 void turtle_mode_update() {
@@ -42,7 +41,7 @@ void turtle_mode_update() {
     flags.turtle = 1;
 
     if (
-        !turtle_ready ||          // turtle was canceled
+        !flags.turtle_ready ||    // turtle was canceled
         state.GEstG.yaw > 0.5f || // quad was flipped
         !flags.arm_state          // quad was disarmed
     ) {
@@ -51,9 +50,6 @@ void turtle_mode_update() {
     }
   } else {
     // turtle is in-active
-    if (flags.arm_state && !motor_set_direction(MOTOR_FORWARD)) {
-      return;
-    }
     flags.turtle = 0;
   }
 
@@ -65,7 +61,8 @@ void turtle_mode_update() {
       last_armed_state_turtle = flags.arm_switch;
 
       // quad was just armed and upside down, begin the turtle sequence
-      if (turtle_ready && flags.arm_switch && (state.GEstG.yaw < 0)) {
+      if (flags.turtle_ready && flags.arm_switch && (state.GEstG.yaw < 0)) {
+        motor_set_direction(MOTOR_REVERSE);
         turtle_state = TURTLE_STAGE_START;
       }
     }
@@ -79,7 +76,7 @@ void turtle_mode_update() {
     state.rx_override.yaw = 0;
     state.rx_override.axis[3] = 0;
 
-    if (!motor_set_direction(MOTOR_REVERSE)) {
+    if (!motor_direction_change_done()) {
       // wait for the motor to sucessfully change
       break;
     }
@@ -122,11 +119,12 @@ void turtle_mode_update() {
     break;
 
   case TURTLE_STAGE_EXIT:
+    motor_set_direction(MOTOR_FORWARD);
     flags.controls_override = 0;
     flags.motortest_override = 0;
     flags.arm_safety = 1;
     turtle_state = TURTLE_STAGE_IDLE;
-    turtle_ready = false;
+    flags.turtle_ready = 0;
     break;
   }
 }