From f7a3736dff9af129612907486a7aab7ed3a042b1 Mon Sep 17 00:00:00 2001 From: bkleiner Date: Sat, 19 Nov 2022 18:25:53 +0100 Subject: [PATCH] dshot: send direction commands on every arm and on turtle disarm --- src/drivers/drv_motor.h | 3 ++- src/drivers/drv_motor_dshot.c | 25 +++++++++++-------------- src/drivers/drv_motor_pwm.c | 5 ++++- src/flight/control.c | 4 ++++ src/flight/control.h | 1 + src/flight/turtle_mode.c | 20 +++++++++----------- 6 files changed, 31 insertions(+), 27 deletions(-) diff --git a/src/drivers/drv_motor.h b/src/drivers/drv_motor.h index 8470dedd3..dda64d359 100644 --- a/src/drivers/drv_motor.h +++ b/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); diff --git a/src/drivers/drv_motor_dshot.c b/src/drivers/drv_motor_dshot.c index 4d245d927..01639bf8f 100644 --- a/src/drivers/drv_motor_dshot.c +++ b/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() { diff --git a/src/drivers/drv_motor_pwm.c b/src/drivers/drv_motor_pwm.c index b64f4cea3..75d2e5513 100644 --- a/src/drivers/drv_motor_pwm.c +++ b/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; } diff --git a/src/flight/control.c b/src/flight/control.c index b41ba415e..848148217 100644 --- a/src/flight/control.c +++ b/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; diff --git a/src/flight/control.h b/src/flight/control.h index 4d09c992f..3893971b8 100644 --- a/src/flight/control.h +++ b/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; diff --git a/src/flight/turtle_mode.c b/src/flight/turtle_mode.c index d231c29e7..35205c6b5 100644 --- a/src/flight/turtle_mode.c +++ b/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; } }