Cons-n-Stuff/geki_pico
1
0
Fork 0
mirror of https://github.com/whowechina/geki_pico.git synced 2024-11-13 18:30:56 +01:00
Code Issues Actions Packages Projects Releases Wiki Activity

Separate tof mix and key triggering

Browse Source
This commit is contained in:
whowechina 2024-11-04 22:52:52 +08:00
parent 75844362dd
commit 5dc2275130
1 changed files with 30 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

50
firmware/src/airkey.c
View File

@ -99,6 +99,8 @@ void airkey_init()
}
static uint16_t tof_dist[TOF_NUM];
static uint16_t tof_mix[2];
static bool readings[AIRKEY_NUM];
static void print_tof(const char *name, uint16_t mm)
@ -118,11 +120,14 @@ static void tof_read()
tof_dist[i] = vl53l1x_readContinuousMillimeters();
print_tof("L1x", tof_dist[i]);
}
if (tof_dist[i] > 1000) { // treat >= 1M as invalid
tof_dist[i] = 0;
}
}
//printf("\n");
}
static uint16_t mix_tof(airkey_side_t side, uint16_t primary, uint16_t secondary)
static uint16_t mix_dist(airkey_side_t side, uint16_t primary, uint16_t secondary)
{
tof_mix_algo_t algo = geki_cfg->tof.mix[side].algo;
@ -155,7 +160,7 @@ static uint16_t mix_tof(airkey_side_t side, uint16_t primary, uint16_t secondary
if (algo == MIX_MIN) {
return primary < secondary ? primary : secondary;
}
if (algo == MIX_AVG) {
int delta = primary - secondary;
int max = primary;
@ -164,35 +169,37 @@ static uint16_t mix_tof(airkey_side_t side, uint16_t primary, uint16_t secondary
max = secondary;
}
window *= 5; // *5 percentage
if ((window == 0) || (window >= delta * 100 / max)) {
return (primary + secondary) / 2;
}
}
return 0;
}
static void calc_mix()
{
for (int side = 0; side < 2; side++) {
int a = side * 2;
int b = side * 2 + 1;
if ((tofs[a].init_ok && tofs[b].init_ok) &&
(tofs[a].model == tofs[b].model)) {
tof_mix[side] = mix_dist(side, tof_dist[a], tof_dist[b]);
} else if (tofs[a].init_ok) {
tof_mix[side] = tof_dist[a];
} else if (tofs[b].init_ok) {
tof_mix[side] = tof_dist[b];
} else {
tof_mix[side] = 0;
}
}
}
#define BETWEEN(x, a, b) (((x) >= (a)) && ((x) <= (b)))
static bool airkey_read(unsigned index)
{
airkey_side_t side = key_defs[index].side;
int a_id = side * 2;
int b_id = side * 2 + 1;
uint16_t dist;
if ((tofs[a_id].init_ok && tofs[b_id].init_ok) &&
(tofs[a_id].model == tofs[b_id].model)) {
dist = mix_tof(side, tof_dist[a_id], tof_dist[b_id]);
} else if (tofs[a_id].init_ok) {
dist = tof_dist[a_id];
} else if (tofs[b_id].init_ok) {
dist = tof_dist[b_id];
} else {
return false;
}
uint16_t dist = tof_mix[side];
if (readings[index]) { // currently triggered
return BETWEEN(dist, key_defs[index].out_low, key_defs[index].out_high);
} else {
@ -207,6 +214,9 @@ void airkey_update()
uint64_t now = time_us_64();
tof_read();
calc_mix();
//printf("%5d,%4d->%d | %5d,%5d->%5d\n", tof_dist[0], tof_dist[1], tof_mix[0], tof_dist[2], tof_dist[3], tof_mix[1]);
for (int i = 0; i < AIRKEY_NUM; i++) {
bool triggered = airkey_read(i);