Protocol rework
This commit is contained in:
ThePetrovich
parent
3e77d34ccc
commit
2b713a3e3a
15 changed files with 1347 additions and 1155 deletions
|
|
@ -1,6 +1,7 @@
|
|||
/*
|
||||
* @file lsense.cpp
|
||||
* @brief Light sensor implementation
|
||||
* @brief Light sensor implementation: software I2C, detection, and channel
|
||||
* readout for the six TCS-style light sensors on three buses.
|
||||
*
|
||||
* Created: 21.09.2025
|
||||
* Author: ThePetrovich
|
||||
|
|
@ -21,9 +22,6 @@
|
|||
|
||||
static SoftwareI2C g_i2c_buses[NUM_BUSES];
|
||||
|
||||
/**
|
||||
* @brief Light sensor data structure
|
||||
*/
|
||||
typedef union __attribute__((packed)) {
|
||||
struct __attribute__((packed)) {
|
||||
uint16_t red;
|
||||
|
|
@ -39,21 +37,9 @@ typedef union __attribute__((packed)) {
|
|||
static light_sensor_data_t g_sensor_data[NUM_BUSES][SENSORS_PER_BUS];
|
||||
static uint8_t g_sensor_addresses[NUM_BUSES][SENSORS_PER_BUS];
|
||||
|
||||
// Presence counters
|
||||
static uint8_t g_sensors_detected_total = 0;
|
||||
static uint8_t g_sensors_detected_per_bus[NUM_BUSES];
|
||||
|
||||
static void configure_light_sensor(SoftwareI2C &i2c_bus, uint8_t sensor_addr);
|
||||
static void read_light_sensor(SoftwareI2C &i2c_bus, uint8_t sensor_addr, light_sensor_data_t *sensor_data);
|
||||
static uint8_t detect_light_sensor(SoftwareI2C &i2c_bus, uint8_t sensor_addr);
|
||||
static void initialize_i2c_bus(int bus_number, bool pin_order_inverted);
|
||||
static void detect_sensors_on_bus_generic(int bus_number);
|
||||
|
||||
/**
|
||||
* @brief Configure a light sensor for measurement
|
||||
* @param i2c_bus Reference to I2C bus instance
|
||||
* @param sensor_addr LSB of sensor address (0 or 1)
|
||||
*/
|
||||
static void configure_light_sensor(SoftwareI2C &i2c_bus, uint8_t sensor_addr)
|
||||
{
|
||||
uint8_t addr = LSENSE_I2C_BASE_ADDR | sensor_addr;
|
||||
|
|
@ -64,17 +50,11 @@ static void configure_light_sensor(SoftwareI2C &i2c_bus, uint8_t sensor_addr)
|
|||
|
||||
i2c_bus.beginTransmission(addr);
|
||||
i2c_bus.write(0x41);
|
||||
i2c_bus.write(0b00101101); // IR gain x1, RGB gain x1, 35ms mode
|
||||
i2c_bus.write(0b00010000); // RGB_EN = 1, measurement active
|
||||
i2c_bus.write(0b00101101); /* IR gain x1, RGB gain x1, 35ms mode */
|
||||
i2c_bus.write(0b00010000); /* RGB_EN = 1, measurement active */
|
||||
i2c_bus.endTransmission();
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Read data from a light sensor
|
||||
* @param i2c_bus Reference to I2C bus instance
|
||||
* @param sensor_addr LSB of sensor address (0 or 1)
|
||||
* @param sensor_data Pointer to data structure to populate
|
||||
*/
|
||||
static void read_light_sensor(SoftwareI2C &i2c_bus, uint8_t sensor_addr, light_sensor_data_t *sensor_data)
|
||||
{
|
||||
if (sensor_data == nullptr)
|
||||
|
|
@ -83,7 +63,7 @@ static void read_light_sensor(SoftwareI2C &i2c_bus, uint8_t sensor_addr, light_s
|
|||
uint8_t addr = LSENSE_I2C_BASE_ADDR | sensor_addr;
|
||||
|
||||
i2c_bus.beginTransmission(addr);
|
||||
i2c_bus.write(0x50); // Start from red register
|
||||
i2c_bus.write(0x50);
|
||||
i2c_bus.endTransmission();
|
||||
|
||||
i2c_bus.requestFrom(addr, (uint8_t)LSENSE_DATA_SIZE);
|
||||
|
|
@ -93,37 +73,25 @@ static void read_light_sensor(SoftwareI2C &i2c_bus, uint8_t sensor_addr, light_s
|
|||
i2c_bus.endTransmission();
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Detect if a light sensor is present
|
||||
* @param i2c_bus Reference to I2C bus instance
|
||||
* @param sensor_addr LSB of sensor address (0 or 1)
|
||||
* @return Manufacturer ID (0xE0 if valid sensor detected, 0 otherwise)
|
||||
*/
|
||||
static uint8_t detect_light_sensor(SoftwareI2C &i2c_bus, uint8_t sensor_addr)
|
||||
{
|
||||
uint8_t response = 0;
|
||||
uint8_t addr = LSENSE_I2C_BASE_ADDR | sensor_addr;
|
||||
|
||||
i2c_bus.beginTransmission(addr);
|
||||
i2c_bus.write(0x92); // Manufacturer ID register
|
||||
i2c_bus.write(0x92);
|
||||
i2c_bus.endTransmission();
|
||||
|
||||
i2c_bus.requestFrom(addr, (uint8_t)1);
|
||||
if (i2c_bus.available()) {
|
||||
response = i2c_bus.read();
|
||||
if (response == 0xFF) {
|
||||
response = 0; // Invalid response
|
||||
response = 0;
|
||||
}
|
||||
}
|
||||
|
||||
return response;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Initialize an I2C bus and configure sensors
|
||||
* @param bus_number Bus number (0, 1, or 2)
|
||||
* @param pin_order_inverted Whether to invert SDA/SCL pin order
|
||||
*/
|
||||
static void initialize_i2c_bus(int bus_number, bool pin_order_inverted)
|
||||
{
|
||||
uint8_t sda_pin, scl_pin;
|
||||
|
|
@ -142,29 +110,22 @@ static void initialize_i2c_bus(int bus_number, bool pin_order_inverted)
|
|||
scl_pin = pin_order_inverted ? SDA2 : SCL2;
|
||||
break;
|
||||
default:
|
||||
// Invalid bus number
|
||||
return;
|
||||
}
|
||||
|
||||
SoftwareI2C *i2c_bus = &g_i2c_buses[bus_number];
|
||||
|
||||
i2c_bus->begin(sda_pin, scl_pin);
|
||||
configure_light_sensor(*i2c_bus, 0);
|
||||
configure_light_sensor(*i2c_bus, 1);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Generic function to detect sensors on any I2C bus
|
||||
* @param bus_number Bus number (0, 1, or 2)
|
||||
*/
|
||||
static void detect_sensors_on_bus_generic(int bus_number)
|
||||
static void detect_sensors_on_bus(int bus_number)
|
||||
{
|
||||
if (bus_number < 0 || bus_number >= NUM_BUSES)
|
||||
return;
|
||||
|
||||
g_sensors_detected_per_bus[bus_number] = 0;
|
||||
|
||||
// Try normal pin order first
|
||||
initialize_i2c_bus(bus_number, false);
|
||||
g_sensor_addresses[bus_number][SENSOR_NEG] = detect_light_sensor(g_i2c_buses[bus_number], 0);
|
||||
if (g_sensor_addresses[bus_number][SENSOR_NEG] == LSENSE_EXPECTED_ID) {
|
||||
|
|
@ -177,7 +138,6 @@ static void detect_sensors_on_bus_generic(int bus_number)
|
|||
g_sensors_detected_per_bus[bus_number]++;
|
||||
}
|
||||
|
||||
// If no sensors found, try inverted pin order
|
||||
if (g_sensors_detected_per_bus[bus_number] == 0) {
|
||||
initialize_i2c_bus(bus_number, true);
|
||||
g_sensor_addresses[bus_number][SENSOR_NEG] = detect_light_sensor(g_i2c_buses[bus_number], 0);
|
||||
|
|
@ -193,62 +153,67 @@ static void detect_sensors_on_bus_generic(int bus_number)
|
|||
}
|
||||
}
|
||||
|
||||
/*******************************************************************************/
|
||||
/*******************************************************************************/
|
||||
/* Command handlers */
|
||||
/*******************************************************************************/
|
||||
/*******************************************************************************/
|
||||
|
||||
void lsense_cmd_presence(void)
|
||||
void lsense_detect_all(void)
|
||||
{
|
||||
g_sensors_detected_total = 0;
|
||||
|
||||
detect_sensors_on_bus_generic(BUS_X);
|
||||
detect_sensors_on_bus_generic(BUS_Y);
|
||||
detect_sensors_on_bus_generic(BUS_Z);
|
||||
|
||||
Serial.write(g_sensors_detected_total);
|
||||
Serial.write(g_sensors_detected_per_bus[BUS_X]);
|
||||
Serial.write(g_sensor_addresses[BUS_X][SENSOR_NEG]);
|
||||
Serial.write(g_sensor_addresses[BUS_X][SENSOR_POS]);
|
||||
Serial.write(g_sensors_detected_per_bus[BUS_Y]);
|
||||
Serial.write(g_sensor_addresses[BUS_Y][SENSOR_NEG]);
|
||||
Serial.write(g_sensor_addresses[BUS_Y][SENSOR_POS]);
|
||||
Serial.write(g_sensors_detected_per_bus[BUS_Z]);
|
||||
Serial.write(g_sensor_addresses[BUS_Z][SENSOR_NEG]);
|
||||
Serial.write(g_sensor_addresses[BUS_Z][SENSOR_POS]);
|
||||
|
||||
Serial.flush();
|
||||
SERIAL_BUFFER_CLEAR();
|
||||
detect_sensors_on_bus(BUS_X);
|
||||
detect_sensors_on_bus(BUS_Y);
|
||||
detect_sensors_on_bus(BUS_Z);
|
||||
}
|
||||
|
||||
void lsense_cmd_read(void)
|
||||
void lsense_get_presence(lsense_presence_t *out)
|
||||
{
|
||||
detect_sensors_on_bus_generic(BUS_X);
|
||||
read_light_sensor(g_i2c_buses[BUS_X], 0, &g_sensor_data[BUS_X][SENSOR_NEG]);
|
||||
read_light_sensor(g_i2c_buses[BUS_X], 1, &g_sensor_data[BUS_X][SENSOR_POS]);
|
||||
if (out == nullptr)
|
||||
return;
|
||||
out->total_detected = g_sensors_detected_total;
|
||||
out->bus_x_count = g_sensors_detected_per_bus[BUS_X];
|
||||
out->bus_x_neg_addr = g_sensor_addresses[BUS_X][SENSOR_NEG];
|
||||
out->bus_x_pos_addr = g_sensor_addresses[BUS_X][SENSOR_POS];
|
||||
out->bus_y_count = g_sensors_detected_per_bus[BUS_Y];
|
||||
out->bus_y_neg_addr = g_sensor_addresses[BUS_Y][SENSOR_NEG];
|
||||
out->bus_y_pos_addr = g_sensor_addresses[BUS_Y][SENSOR_POS];
|
||||
out->bus_z_count = g_sensors_detected_per_bus[BUS_Z];
|
||||
out->bus_z_neg_addr = g_sensor_addresses[BUS_Z][SENSOR_NEG];
|
||||
out->bus_z_pos_addr = g_sensor_addresses[BUS_Z][SENSOR_POS];
|
||||
}
|
||||
|
||||
detect_sensors_on_bus_generic(BUS_Y);
|
||||
read_light_sensor(g_i2c_buses[BUS_Y], 0, &g_sensor_data[BUS_Y][SENSOR_NEG]);
|
||||
read_light_sensor(g_i2c_buses[BUS_Y], 1, &g_sensor_data[BUS_Y][SENSOR_POS]);
|
||||
static uint16_t extract_channel(const light_sensor_data_t *d, uint8_t channel)
|
||||
{
|
||||
switch (channel) {
|
||||
case 0:
|
||||
return d->data.red;
|
||||
case 1:
|
||||
return d->data.green;
|
||||
case 2:
|
||||
return d->data.blue;
|
||||
case 3:
|
||||
return d->data.ir;
|
||||
case 4:
|
||||
return d->data.green2;
|
||||
case 5: /* visible: R+G+B+G2 */
|
||||
return (uint16_t)((uint32_t)d->data.red + d->data.green + d->data.blue + d->data.green2);
|
||||
case 6: /* all: R+G+B+G2+IR */
|
||||
return (uint16_t)((uint32_t)d->data.red + d->data.green + d->data.blue + d->data.green2 +
|
||||
d->data.ir);
|
||||
default:
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
|
||||
detect_sensors_on_bus_generic(BUS_Z);
|
||||
read_light_sensor(g_i2c_buses[BUS_Z], 0, &g_sensor_data[BUS_Z][SENSOR_NEG]);
|
||||
read_light_sensor(g_i2c_buses[BUS_Z], 1, &g_sensor_data[BUS_Z][SENSOR_POS]);
|
||||
void lsense_read_channel(uint8_t channel, lsense_channel_values_t *out)
|
||||
{
|
||||
if (out == nullptr)
|
||||
return;
|
||||
|
||||
Serial.write(g_sensor_data[BUS_X][SENSOR_NEG].data.blue & 0xFF);
|
||||
Serial.write(g_sensor_data[BUS_X][SENSOR_NEG].data.blue >> 8);
|
||||
Serial.write(g_sensor_data[BUS_X][SENSOR_POS].data.blue & 0xFF);
|
||||
Serial.write(g_sensor_data[BUS_X][SENSOR_POS].data.blue >> 8);
|
||||
Serial.write(g_sensor_data[BUS_Y][SENSOR_NEG].data.blue & 0xFF);
|
||||
Serial.write(g_sensor_data[BUS_Y][SENSOR_NEG].data.blue >> 8);
|
||||
Serial.write(g_sensor_data[BUS_Y][SENSOR_POS].data.blue & 0xFF);
|
||||
Serial.write(g_sensor_data[BUS_Y][SENSOR_POS].data.blue >> 8);
|
||||
Serial.write(g_sensor_data[BUS_Z][SENSOR_NEG].data.blue & 0xFF);
|
||||
Serial.write(g_sensor_data[BUS_Z][SENSOR_NEG].data.blue >> 8);
|
||||
Serial.write(g_sensor_data[BUS_Z][SENSOR_POS].data.blue & 0xFF);
|
||||
Serial.write(g_sensor_data[BUS_Z][SENSOR_POS].data.blue >> 8);
|
||||
for (int bus = 0; bus < NUM_BUSES; bus++) {
|
||||
read_light_sensor(g_i2c_buses[bus], 0, &g_sensor_data[bus][SENSOR_NEG]);
|
||||
read_light_sensor(g_i2c_buses[bus], 1, &g_sensor_data[bus][SENSOR_POS]);
|
||||
}
|
||||
|
||||
Serial.flush();
|
||||
SERIAL_BUFFER_CLEAR();
|
||||
}
|
||||
out->values[0] = extract_channel(&g_sensor_data[BUS_X][SENSOR_NEG], channel);
|
||||
out->values[1] = extract_channel(&g_sensor_data[BUS_X][SENSOR_POS], channel);
|
||||
out->values[2] = extract_channel(&g_sensor_data[BUS_Y][SENSOR_NEG], channel);
|
||||
out->values[3] = extract_channel(&g_sensor_data[BUS_Y][SENSOR_POS], channel);
|
||||
out->values[4] = extract_channel(&g_sensor_data[BUS_Z][SENSOR_NEG], channel);
|
||||
out->values[5] = extract_channel(&g_sensor_data[BUS_Z][SENSOR_POS], channel);
|
||||
}
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue