Cons-n-Stuff/chu_pico
1
0
Fork 0
mirror of https://github.com/whowechina/chu_pico.git synced 2025-01-19 01:34:03 +01:00
Code Releases Activity

Better tof distance calc

Browse Source
This commit is contained in:
whowechina 2023-09-23 17:34:34 +08:00
parent ea77b1164b
commit 07ecf9e7bd
6 changed files with 49 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

BIN
Production/Firmware/chu_pico.uf2
View File

Binary file not shown.

14
firmware/src/air.c
View File

@ -48,9 +48,7 @@ void air_init()
vl53l0x_init_tof(true);
vl53l0x_start_continuous(0);
} else if (tof_model[i] == 2) {
gp2y0e_write(I2C_PORT, 0xa8, 0); // Accumulation 0:1, 1:5, 2:30, 3:10
gp2y0e_write(I2C_PORT, 0x3f, 0x30); // Filter 0x00:7, 0x10:5, 0x20:9, 0x30:1
gp2y0e_write(I2C_PORT, 0x13, 5); // Pulse [3..7]:[40, 80, 160, 240, 320] us
gp2y03_init(I2C_PORT);
}
}
}
@ -113,8 +111,16 @@ void air_update()
if (tof_model[i] == 1) {
distances[i] = readRangeContinuousMillimeters() * 10;
} else if (tof_model[i] == 2) {
distances[i] = gp2y0e_dist16(I2C_PORT);
/* compensation based on observation, don't know why*/
distances[i] = gp2y0e_dist16(I2C_PORT) * 16 / 10;
}
}
}
uint16_t air_raw(uint8_t index)
{
if (index >= sizeof(TOF_LIST)) {
return 0;
}
return distances[index];
}

1
firmware/src/air.h
View File

@ -12,6 +12,7 @@
void air_init();
size_t air_num();
unsigned air_value(uint8_t index);
uint16_t air_raw(uint8_t index);
uint8_t air_bitmap();
void air_update();

17
firmware/src/cmd.c
View File

@ -8,6 +8,7 @@
#include "pico/stdlib.h"
#include "config.h"
#include "air.h"
#include "slider.h"
#include "save.h"
@ -216,11 +217,20 @@ static void handle_tof(int argc, char *argv[])
const char *usage = "Usage: tof <offset> [pitch]\n"
" offset: 40..255\n"
" pitch: 4..50\n";
if ((argc < 1) || (argc > 2)) {
if (argc > 2) {
printf(usage);
return;
}
if (argc == 0) {
printf("TOF: ");
for (int i = air_num(); i > 0; i--) {
printf(" %4d", air_raw(i - 1) / 10);
}
printf("\n");
return;
}
int offset = chu_cfg->tof.offset;
int pitch = chu_cfg->tof.pitch;
if (argc >= 1) {
@ -410,6 +420,11 @@ static void process_cmd()
int argc;
char *cmd = strtok(cmd_buf, " \n");
if (strlen(cmd) == 0) {
return;
}
argc = 0;
while ((argc < MAX_PARAMETERS) &&
(argv[argc] = strtok(NULL, " \n")) != NULL) {

2
firmware/src/config.c
View File

@ -27,7 +27,7 @@ static chu_cfg_t default_cfg = {
},
.tof = {
.offset = 64,
.pitch = 18,
.pitch = 28,
},
.sense = {
.filter = 0x11,

36
firmware/src/gp2y0e.h
View File

@ -11,36 +11,42 @@
#define GP2Y0E_DEF_ADDR 0x40
static inline bool gp2y0e_is_present(i2c_inst_t *i2c_port)
static inline uint16_t gp2y0e_write(i2c_inst_t *port, uint8_t addr, uint8_t val)
{
uint8_t cmd[] = {addr, val};
i2c_write_blocking_until(port, GP2Y0E_DEF_ADDR, cmd, 2, false, time_us_64() + 1000);
}
static inline void gp2y03_init(i2c_inst_t *port)
{
gp2y0e_write(port, 0xa8, 0); // Accumulation 0:1, 1:5, 2:30, 3:10
gp2y0e_write(port, 0x3f, 0x30); // Filter 0x00:7, 0x10:5, 0x20:9, 0x30:1
gp2y0e_write(port, 0x13, 5); // Pulse [3..7]:[40, 80, 160, 240, 320] us
}
static inline bool gp2y0e_is_present(i2c_inst_t *port)
{
uint8_t cmd[] = {0x5e};
return i2c_write_blocking_until(i2c_port, GP2Y0E_DEF_ADDR, cmd, 1, true,
return i2c_write_blocking_until(port, GP2Y0E_DEF_ADDR, cmd, 1, true,
time_us_64() + 1000) == 1;
}
static inline uint16_t gp2y0e_write(i2c_inst_t *i2c_port, uint8_t addr, uint8_t val)
{
uint8_t cmd[] = {addr, val};
i2c_write_blocking_until(i2c_port, GP2Y0E_DEF_ADDR, cmd, 2, false, time_us_64() + 1000);
}
static inline uint8_t gp2y0e_dist(i2c_inst_t *i2c_port)
static inline uint8_t gp2y0e_dist(i2c_inst_t *port)
{
uint8_t cmd[] = {0x5e};
i2c_write_blocking_until(i2c_port, GP2Y0E_DEF_ADDR, cmd, 1, true, time_us_64() + 1000);
i2c_write_blocking_until(port, GP2Y0E_DEF_ADDR, cmd, 1, true, time_us_64() + 1000);
uint8_t data;
i2c_read_blocking_until(i2c_port, GP2Y0E_DEF_ADDR, &data, 1, false, time_us_64() + 1000);
i2c_read_blocking_until(port, GP2Y0E_DEF_ADDR, &data, 1, false, time_us_64() + 1000);
return data;
}
static inline uint16_t gp2y0e_dist16(i2c_inst_t *i2c_port)
static inline uint16_t gp2y0e_dist16(i2c_inst_t *port)
{
uint8_t cmd[] = {0x5e};
i2c_write_blocking_until(i2c_port, GP2Y0E_DEF_ADDR, cmd, 1, true, time_us_64() + 1000);
i2c_write_blocking_until(port, GP2Y0E_DEF_ADDR, cmd, 1, true, time_us_64() + 1000);
uint8_t data[2];
i2c_read_blocking_until(i2c_port, GP2Y0E_DEF_ADDR, data, 2, false, time_us_64() + 1000);
i2c_read_blocking_until(port, GP2Y0E_DEF_ADDR, data, 2, false, time_us_64() + 1000);
return (data[0] << 4) | data[1];
}