Skip to content

Commit

Permalink
gestures: refactor
Browse files Browse the repository at this point in the history
  • Loading branch information
bkleiner committed Jun 26, 2023
1 parent 1331e25 commit bd1a95f
Show file tree
Hide file tree
Showing 5 changed files with 194 additions and 343 deletions.
325 changes: 93 additions & 232 deletions src/flight/gesture_detect.c
View file
Original file line number Diff line number Diff line change
@@ -1,7 +1,6 @@
#include "flight/gestures.h"

#include <math.h>
#include <stdint.h>

#include "core/project.h"
#include "driver/time.h"
Expand Down Expand Up @@ -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);
}
Loading

0 comments on commit bd1a95f

Please sign in to comment.