diff --git a/src/flight/gesture_detect.c b/src/flight/gesture_detect.c index 31c1a469b..a546d98fc 100644 --- a/src/flight/gesture_detect.c +++ b/src/flight/gesture_detect.c @@ -1,7 +1,6 @@ #include "flight/gestures.h" #include -#include #include "core/project.h" #include "driver/time.h" @@ -37,271 +36,133 @@ #define GSIZE 7 #define OSD_GSIZE 3 -// L L D -const uint8_t command1[GSIZE] = {GESTURE_CENTER_IDLE, GESTURE_LEFT, GESTURE_CENTER, GESTURE_LEFT, GESTURE_CENTER, GESTURE_DOWN, GESTURE_CENTER}; +static uint8_t gbuffer[GSIZE]; -// R R D -const uint8_t command2[GSIZE] = {GESTURE_CENTER_IDLE, GESTURE_RIGHT, GESTURE_CENTER, GESTURE_RIGHT, GESTURE_CENTER, GESTURE_DOWN, GESTURE_CENTER}; +static const uint8_t commands[GESTURE_MAX][GSIZE] = { + [GESTURE_NONE] = {}, -// D D D -const uint8_t command3[GSIZE] = {GESTURE_CENTER_IDLE, GESTURE_DOWN, GESTURE_CENTER, GESTURE_DOWN, GESTURE_CENTER, GESTURE_DOWN, GESTURE_CENTER}; + // flash gestures + [GESTURE_DDD] = {GESTURE_CENTER_IDLE, GESTURE_DOWN, GESTURE_CENTER, GESTURE_DOWN, GESTURE_CENTER, GESTURE_DOWN, GESTURE_CENTER}, + [GESTURE_UUU] = {GESTURE_CENTER_IDLE, GESTURE_UP, GESTURE_CENTER, GESTURE_UP, GESTURE_CENTER, GESTURE_UP, GESTURE_CENTER}, -// GESTURES FOR PID TUNING + // Gesture aux + [GESTURE_LLD] = {GESTURE_CENTER_IDLE, GESTURE_LEFT, GESTURE_CENTER, GESTURE_LEFT, GESTURE_CENTER, GESTURE_DOWN, GESTURE_CENTER}, + [GESTURE_RRD] = {GESTURE_CENTER_IDLE, GESTURE_RIGHT, GESTURE_CENTER, GESTURE_RIGHT, GESTURE_CENTER, GESTURE_DOWN, GESTURE_CENTER}, -#ifdef PID_GESTURE_TUNING -// U D U - Next PID term -const uint8_t command4[GSIZE] = {GESTURE_CENTER_IDLE, GESTURE_UP, GESTURE_CENTER, GESTURE_DOWN, GESTURE_CENTER, GESTURE_UP, GESTURE_CENTER}; - -// U D D - Next PID Axis -const uint8_t command5[GSIZE] = {GESTURE_CENTER_IDLE, GESTURE_UP, GESTURE_CENTER, GESTURE_DOWN, GESTURE_CENTER, GESTURE_DOWN, GESTURE_CENTER}; + // Enter OSD + [GESTURE_RRR] = {GESTURE_CENTER_IDLE, GESTURE_RIGHT, GESTURE_CENTER, GESTURE_RIGHT, GESTURE_CENTER, GESTURE_RIGHT, GESTURE_CENTER}, + // Refresh OSD + [GESTURE_LRL] = {GESTURE_CENTER_IDLE, GESTURE_LEFT, GESTURE_CENTER, GESTURE_RIGHT, GESTURE_CENTER, GESTURE_LEFT, GESTURE_CENTER}, -// U D R - Increase value -const uint8_t command6[GSIZE] = {GESTURE_CENTER_IDLE, GESTURE_UP, GESTURE_CENTER, GESTURE_DOWN, GESTURE_CENTER, GESTURE_RIGHT, GESTURE_CENTER}; - -// U D L - Decrease value -const uint8_t command7[GSIZE] = {GESTURE_CENTER_IDLE, GESTURE_UP, GESTURE_CENTER, GESTURE_DOWN, GESTURE_CENTER, GESTURE_LEFT, GESTURE_CENTER}; +#ifdef PID_GESTURE_TUNING + [GESTURE_UDU] = {GESTURE_CENTER_IDLE, GESTURE_UP, GESTURE_CENTER, GESTURE_DOWN, GESTURE_CENTER, GESTURE_UP, GESTURE_CENTER}, + [GESTURE_UDD] = {GESTURE_CENTER_IDLE, GESTURE_UP, GESTURE_CENTER, GESTURE_DOWN, GESTURE_CENTER, GESTURE_DOWN, GESTURE_CENTER}, + [GESTURE_UDR] = {GESTURE_CENTER_IDLE, GESTURE_UP, GESTURE_CENTER, GESTURE_DOWN, GESTURE_CENTER, GESTURE_RIGHT, GESTURE_CENTER}, + [GESTURE_UDL] = {GESTURE_CENTER_IDLE, GESTURE_UP, GESTURE_CENTER, GESTURE_DOWN, GESTURE_CENTER, GESTURE_LEFT, GESTURE_CENTER}, #endif +}; -// NFE ADDED GESTURES - -// U U U -const uint8_t command8[GSIZE] = {GESTURE_CENTER_IDLE, GESTURE_UP, GESTURE_CENTER, GESTURE_UP, GESTURE_CENTER, GESTURE_UP, GESTURE_CENTER}; - -// R R R -const uint8_t command9[GSIZE] = {GESTURE_CENTER_IDLE, GESTURE_RIGHT, GESTURE_CENTER, GESTURE_RIGHT, GESTURE_CENTER, GESTURE_RIGHT, GESTURE_CENTER}; - -// D U D -const uint8_t command11[GSIZE] = {GESTURE_CENTER_IDLE, GESTURE_DOWN, GESTURE_CENTER, GESTURE_UP, GESTURE_CENTER, GESTURE_DOWN, GESTURE_CENTER}; - -// OSD GESRURES - -// OSD-UP -const uint8_t command12[OSD_GSIZE] = {GESTURE_CENTER_IDLE, GESTURE_UP, GESTURE_CENTER}; - -// OSD-DOWN -const uint8_t command13[OSD_GSIZE] = {GESTURE_CENTER_IDLE, GESTURE_DOWN, GESTURE_CENTER}; - -// OSD-RIGHT -const uint8_t command14[OSD_GSIZE] = {GESTURE_CENTER_IDLE, GESTURE_RIGHT, GESTURE_CENTER}; - -// OSD-LEFT -const uint8_t command15[OSD_GSIZE] = {GESTURE_CENTER_IDLE, GESTURE_LEFT, GESTURE_CENTER}; - -const uint8_t command16[GSIZE] = {GESTURE_CENTER_IDLE, GESTURE_LEFT, GESTURE_CENTER, GESTURE_RIGHT, GESTURE_CENTER, GESTURE_LEFT, GESTURE_CENTER}; - -int gesture_start; -int lastgesture; -int setgesture; -static unsigned gesturetime; - -int gestures2() { - if (flags.on_ground) { - if (GMACRO_XCENTER && GMACRO_PITCHCENTER) { - gesture_start = GESTURE_CENTER; - } else if (GMACRO_LEFT && GMACRO_PITCHCENTER) { - gesture_start = GESTURE_LEFT; - } else if (GMACRO_RIGHT && GMACRO_PITCHCENTER) { - gesture_start = GESTURE_RIGHT; - } else if (GMACRO_DOWN && GMACRO_XCENTER) { - gesture_start = GESTURE_DOWN; - } else if (GMACRO_UP && GMACRO_XCENTER) { - gesture_start = GESTURE_UP; - } else { - // gesture_start = GESTURE_OTHER; - } - - uint32_t time = time_micros(); - - if (gesture_start != lastgesture) { - gesturetime = time; - } - - if (time - gesturetime > GESTURETIME_MIN) { - int gesturetime_idle; - - if (osd_state.screen != OSD_SCREEN_REGULAR) - gesturetime_idle = GESTURETIME_IDLE_OSD; - else - gesturetime_idle = GESTURETIME_IDLE; - - if ((gesture_start == GESTURE_CENTER) && (time - gesturetime > gesturetime_idle)) { - setgesture = GESTURE_CENTER_IDLE; - } else if (time - gesturetime > GESTURETIME_MAX) { - if ((gesture_start != GESTURE_OTHER)) - setgesture = GESTURE_LONG; - } else { - setgesture = gesture_start; - } - } +static const uint8_t osd_commands[OSD_INPUT_MAX][OSD_GSIZE] = { + [OSD_INPUT_NONE] = {}, + [OSD_INPUT_UP] = {GESTURE_CENTER_IDLE, GESTURE_UP, GESTURE_CENTER}, + [OSD_INPUT_DOWN] = {GESTURE_CENTER_IDLE, GESTURE_DOWN, GESTURE_CENTER}, + [OSD_INPUT_RIGHT] = {GESTURE_CENTER_IDLE, GESTURE_RIGHT, GESTURE_CENTER}, + [OSD_INPUT_LEFT] = {GESTURE_CENTER_IDLE, GESTURE_LEFT, GESTURE_CENTER}, +}; - lastgesture = gesture_start; - - return gesture_sequence(setgesture); - - } else { - setgesture = GESTURE_OTHER; - lastgesture = GESTURE_OTHER; - } - - return 0; -} - -// LEAVING OFF HERE - need a way to change gbuffer size @ runtime -uint8_t gbuffer[GSIZE]; - -uint8_t check_command(uint8_t buffer1[], const uint8_t command[]) { +static bool gesture_check_command(const uint8_t input[], const uint8_t command[]) { if (osd_state.screen != OSD_SCREEN_REGULAR) { for (int i = 0; i < OSD_GSIZE; i++) { - if (buffer1[i] != command[OSD_GSIZE - i - 1]) - return 0; + if (input[i] != command[OSD_GSIZE - i - 1]) + return false; } } else { for (int i = 0; i < GSIZE; i++) { - if (buffer1[i] != command[GSIZE - i - 1]) - return 0; + if (input[i] != command[GSIZE - i - 1]) + return false; } } - return 1; + return true; } -int gesture_sequence(int currentgesture) { +static int32_t gesture_sequence(int32_t current_gesture) { + if (current_gesture == gbuffer[0]) { + return GESTURE_NONE; + } - if (currentgesture != gbuffer[0]) { // add to queue - if (osd_state.screen != OSD_SCREEN_REGULAR) { - for (int i = OSD_GSIZE - 1; i >= 1; i--) { - gbuffer[i] = gbuffer[i - 1]; - } - } else { - for (int i = GSIZE - 1; i >= 1; i--) { - gbuffer[i] = gbuffer[i - 1]; - } + if (osd_state.screen != OSD_SCREEN_REGULAR) { + for (int i = OSD_GSIZE - 1; i >= 1; i--) { + gbuffer[i] = gbuffer[i - 1]; } - gbuffer[0] = currentgesture; - - // check commands - if (osd_state.screen != OSD_SCREEN_REGULAR) { - if (check_command(&gbuffer[0], &command12[0])) { - // command 12 - - // change buffer so it does not trigger again - gbuffer[1] = GESTURE_OTHER; - return GESTURE_OSD_UP; - } - - if (check_command(&gbuffer[0], &command13[0])) { - // command 13 - - // change buffer so it does not trigger again - gbuffer[1] = GESTURE_OTHER; - return GESTURE_OSD_DOWN; - } - - if (check_command(&gbuffer[0], &command14[0])) { - // command 14 - - // change buffer so it does not trigger again - gbuffer[1] = GESTURE_OTHER; - return GESTURE_OSD_RIGHT; - } - - if (check_command(&gbuffer[0], &command15[0])) { - // command 15 - - // change buffer so it does not trigger again - gbuffer[1] = GESTURE_OTHER; - return GESTURE_OSD_LEFT; - } - - } else { - if (check_command(&gbuffer[0], &command16[0])) { - // command 16 - - // change buffer so it does not trigger again - gbuffer[1] = GESTURE_OTHER; - return GESTURE_LRL; - } - - if (check_command(&gbuffer[0], &command1[0])) { - // command 1 - - // change buffer so it does not trigger again - gbuffer[1] = GESTURE_OTHER; - return GESTURE_LLD; - } - - if (check_command(&gbuffer[0], &command2[0])) { - // command 2 - - // change buffer so it does not trigger again - gbuffer[1] = GESTURE_OTHER; - return GESTURE_RRD; - } - - if (check_command(&gbuffer[0], &command3[0])) { - // command 3 - - // change buffer so it does not trigger again - gbuffer[1] = GESTURE_OTHER; - return GESTURE_DDD; - } - if (check_command(&gbuffer[0], &command8[0])) { - // command 8 - - // change buffer so it does not trigger again - gbuffer[1] = GESTURE_OTHER; - return GESTURE_UUU; - } - if (check_command(&gbuffer[0], &command9[0])) { - // command 9 + gbuffer[0] = current_gesture; - // change buffer so it does not trigger again + for (uint32_t i = 1; i < OSD_INPUT_MAX; i++) { + if (gesture_check_command(gbuffer, osd_commands[i])) { gbuffer[1] = GESTURE_OTHER; - return GESTURE_RRR; + return i; } + } + } else { + for (int i = GSIZE - 1; i >= 1; i--) { + gbuffer[i] = gbuffer[i - 1]; + } - if (check_command(&gbuffer[0], &command11[0])) { - // command 11 + gbuffer[0] = current_gesture; - // change buffer so it does not trigger again + for (uint32_t i = 1; i < GESTURE_MAX; i++) { + if (gesture_check_command(gbuffer, commands[i])) { gbuffer[1] = GESTURE_OTHER; - return GESTURE_DUD; + return i; } + } + } -#ifdef PID_GESTURE_TUNING - if (check_command(&gbuffer[0], &command4[0])) { - // command 4 - - // change buffer so it does not trigger again - gbuffer[1] = GESTURE_OTHER; - return GESTURE_UDU; - } + return GESTURE_NONE; +} - if (check_command(&gbuffer[0], &command5[0])) { - // command 5 +int32_t gestures_detect() { + static int32_t gesture_start; + static int32_t last_gesture; + static int32_t current_gesture; + static uint32_t gesture_time; - // change buffer so it does not trigger again - gbuffer[1] = GESTURE_OTHER; - return GESTURE_UDD; - } + if (!flags.on_ground) { + current_gesture = GESTURE_OTHER; + last_gesture = GESTURE_OTHER; + return 0; + } - if (check_command(&gbuffer[0], &command6[0])) { - // command 6 + if (GMACRO_XCENTER && GMACRO_PITCHCENTER) { + gesture_start = GESTURE_CENTER; + } else if (GMACRO_LEFT && GMACRO_PITCHCENTER) { + gesture_start = GESTURE_LEFT; + } else if (GMACRO_RIGHT && GMACRO_PITCHCENTER) { + gesture_start = GESTURE_RIGHT; + } else if (GMACRO_DOWN && GMACRO_XCENTER) { + gesture_start = GESTURE_DOWN; + } else if (GMACRO_UP && GMACRO_XCENTER) { + gesture_start = GESTURE_UP; + } - // change buffer so it does not trigger again - gbuffer[1] = GESTURE_OTHER; - return GESTURE_UDR; - } - if (check_command(&gbuffer[0], &command7[0])) { - // command 7 + const uint32_t time = time_micros(); + if (gesture_start != last_gesture) { + gesture_time = time; + } - // change buffer so it does not trigger again - gbuffer[1] = GESTURE_OTHER; - return GESTURE_UDL; - } -#endif + if (time - gesture_time > GESTURETIME_MIN) { + const uint32_t gesture_time_idle = osd_state.screen != OSD_SCREEN_REGULAR ? GESTURETIME_IDLE_OSD : GESTURETIME_IDLE; + if ((gesture_start == GESTURE_CENTER) && (time - gesture_time > gesture_time_idle)) { + current_gesture = GESTURE_CENTER_IDLE; + } else if (time - gesture_time > GESTURETIME_MAX) { + if ((gesture_start != GESTURE_OTHER)) + current_gesture = GESTURE_LONG; + } else { + current_gesture = gesture_start; } } - return GESTURE_NONE; + last_gesture = gesture_start; + + return gesture_sequence(current_gesture); } diff --git a/src/flight/gestures.c b/src/flight/gestures.c index 414c2e731..2d04e9970 100644 --- a/src/flight/gestures.c +++ b/src/flight/gestures.c @@ -6,7 +6,6 @@ #include "flight/control.h" #include "flight/pid.h" #include "flight/sixaxis.h" -#include "osd/render.h" #include "rx/rx.h" #include "util/util.h" @@ -14,111 +13,96 @@ extern int ledcommand; extern int ledblink; extern profile_t profile; -int pid_gestures_used = 0; +bool pid_gestures_used = false; void gestures() { - int command = gestures2(); - - if (command != GESTURE_NONE) { - if (command == GESTURE_DDD) { - - // skip accel calibration if pid gestures used - if (!pid_gestures_used) { - sixaxis_gyro_cal(); // for flashing lights - sixaxis_acc_cal(); - } else { - ledcommand = 1; - pid_gestures_used = 0; - } - - flash_save(); - flash_load(); - - // reset flash numbers - extern int number_of_increments[3][3]; - for (int i = 0; i < 3; i++) - for (int j = 0; j < 3; j++) - number_of_increments[i][j] = 0; - - // reset loop time - looptime_reset(); - } - - if (command == GESTURE_DUD) { - profile.motor.invert_yaw = !profile.motor.invert_yaw; - ledblink = 2 - profile.motor.invert_yaw; - pid_gestures_used = 1; - } - - if (command == GESTURE_UUU) { - bind_storage.bind_saved = !bind_storage.bind_saved; - ledblink = 2 - bind_storage.bind_saved; - pid_gestures_used = 1; - } + const int32_t command = gestures_detect(); + if (command == GESTURE_NONE) { + return; + } - if (command == GESTURE_RRR) { - ledblink = 2 - osd_push_screen(OSD_SCREEN_MAIN_MENU); - } + if (osd_state.screen != OSD_SCREEN_REGULAR) { + return osd_handle_input(command); + } - if (command == GESTURE_RRD) { - state.aux[AUX_CHANNEL_GESTURE] = 1; + switch (command) { + case GESTURE_DDD: { + // skip accel calibration if pid gestures used + if (!pid_gestures_used) { + sixaxis_gyro_cal(); // for flashing lights + sixaxis_acc_cal(); + } else { ledcommand = 1; - } - if (command == GESTURE_LLD) { - ledcommand = 1; - state.aux[AUX_CHANNEL_GESTURE] = 0; - } - - if (command == GESTURE_LRL) { - while (osd_pop_screen() != OSD_SCREEN_CLEAR) - ; + pid_gestures_used = false; } - if (command == GESTURE_OSD_UP) { - osd_handle_input(OSD_INPUT_UP); - } + flash_save(); + flash_load(); - if (command == GESTURE_OSD_DOWN) { - osd_handle_input(OSD_INPUT_DOWN); - } - - if (command == GESTURE_OSD_RIGHT) { - osd_handle_input(OSD_INPUT_RIGHT); - } - - if (command == GESTURE_OSD_LEFT) { - osd_handle_input(OSD_INPUT_LEFT); - } + // reset flash numbers + extern int number_of_increments[3][3]; + for (int i = 0; i < 3; i++) + for (int j = 0; j < 3; j++) + number_of_increments[i][j] = 0; + // reset loop time + looptime_reset(); + break; + } + case GESTURE_UUU: { + bind_storage.bind_saved = !bind_storage.bind_saved; + ledblink = 2 - bind_storage.bind_saved; + pid_gestures_used = true; + break; + } + case GESTURE_RRR: { + ledblink = 2 - osd_push_screen(OSD_SCREEN_MAIN_MENU); + break; + } + case GESTURE_RRD: { + state.aux[AUX_CHANNEL_GESTURE] = 1; + ledcommand = 1; + break; + } + case GESTURE_LLD: { + state.aux[AUX_CHANNEL_GESTURE] = 0; + ledcommand = 1; + break; + } + case GESTURE_LRL: { + while (osd_pop_screen() != OSD_SCREEN_CLEAR) + ; + break; + } #ifdef PID_GESTURE_TUNING - if (command >= GESTURE_UDR) - pid_gestures_used = 1; - - if (command == GESTURE_UDU) { - // Cycle to next pid term (P I D) - ledblink = next_pid_term(); - } - if (command == GESTURE_UDD) { - // Cycle to next axis (Roll Pitch Yaw) - ledblink = next_pid_axis(); - } - if (command == GESTURE_UDR) { - // Increase by 10% - ledblink = increase_pid(); - } - if (command == GESTURE_UDL) { - // Descrease by 10% - ledblink = decrease_pid(); - } - // flash long on zero - if (pid_gestures_used && ledblink == 0) - ledcommand = 1; - - // U D U - Next PID term - // U D D - Next PID Axis - // U D R - Increase value - // U D L - Descrease value - // ledblink = blink; //Will cause led logic to blink the number of times ledblink has stored in it. + case GESTURE_UDU: { + // Cycle to next pid term (P I D) + ledblink = next_pid_term(); + pid_gestures_used = true; + break; + } + case GESTURE_UDD: { + // Cycle to next axis (Roll Pitch Yaw) + ledblink = next_pid_axis(); + pid_gestures_used = true; + break; + } + case GESTURE_UDR: { + // Increase by 10% + ledblink = increase_pid(); + pid_gestures_used = true; + break; + } + case GESTURE_UDL: { + // Descrease by 10% + ledblink = decrease_pid(); + pid_gestures_used = true; + break; + } #endif } + + // flash long on zero + if (pid_gestures_used && ledblink == 0) + ledcommand = 1; } diff --git a/src/flight/gestures.h b/src/flight/gestures.h index 519dd8c9a..909b0c1e3 100644 --- a/src/flight/gestures.h +++ b/src/flight/gestures.h @@ -1,25 +1,26 @@ #pragma once -// warning: using if x>GESTURE_UDR to check if pid gestures used -enum gestures_enum { - GESTURE_NONE = 0, +#include + +#include "core/project.h" +#include "osd/render.h" + +typedef enum { + GESTURE_NONE, GESTURE_DDD, GESTURE_UUU, GESTURE_LLD, GESTURE_RRD, + GESTURE_RRR, + GESTURE_LRL, +#ifdef PID_GESTURE_TUNING GESTURE_UDU, GESTURE_UDD, GESTURE_UDR, GESTURE_UDL, - GESTURE_RRR, - GESTURE_LRL, - GESTURE_DUD, - GESTURE_OSD_UP, - GESTURE_OSD_DOWN, - GESTURE_OSD_RIGHT, - GESTURE_OSD_LEFT -}; +#endif + GESTURE_MAX, +} gestures_t; -int gestures2(); -int gesture_sequence(int gesture); +int32_t gestures_detect(); void gestures(); diff --git a/src/osd/render.c b/src/osd/render.c index 9f7a64148..d496ecc75 100644 --- a/src/osd/render.c +++ b/src/osd/render.c @@ -205,6 +205,9 @@ osd_screens_t osd_pop_screen() { void osd_handle_input(osd_input_t input) { switch (input) { + default: + break; + case OSD_INPUT_UP: if (osd_state.selection) { osd_state.selection_increase = 1; @@ -266,10 +269,10 @@ void osd_save_exit() { } // check for fc reboot request - extern int pid_gestures_used; + extern bool pid_gestures_used; extern int ledcommand; - pid_gestures_used = 0; + pid_gestures_used = false; ledcommand = 1; flash_save(); diff --git a/src/osd/render.h b/src/osd/render.h index d606269c9..d33399168 100644 --- a/src/osd/render.h +++ b/src/osd/render.h @@ -47,10 +47,12 @@ typedef enum { } osd_screens_t; typedef enum { + OSD_INPUT_NONE, OSD_INPUT_UP, OSD_INPUT_DOWN, OSD_INPUT_LEFT, OSD_INPUT_RIGHT, + OSD_INPUT_MAX, } osd_input_t; typedef enum {