254 lines
No EOL
8.1 KiB
C++
254 lines
No EOL
8.1 KiB
C++
/*
|
|
* @file lsense.cpp
|
|
* @brief Light sensor implementation
|
|
*
|
|
* Created: 21.09.2025
|
|
* Author: ThePetrovich
|
|
*
|
|
* Copyright YKSA - Sakha Aerospace Systems, LLC.
|
|
* See the LICENSE file for details.
|
|
*
|
|
* SPDX-License-Identifier: BSD-3-Clause
|
|
*/
|
|
|
|
#include <Arduino.h>
|
|
#include <SoftwareI2C.h>
|
|
#include <stdint.h>
|
|
|
|
#include "config.h"
|
|
#include "iodefs.h"
|
|
#include "lsense.h"
|
|
|
|
static SoftwareI2C g_i2c_buses[NUM_BUSES];
|
|
|
|
/**
|
|
* @brief Light sensor data structure
|
|
*/
|
|
typedef union __attribute__((packed)) {
|
|
struct __attribute__((packed)) {
|
|
uint16_t red;
|
|
uint16_t green;
|
|
uint16_t blue;
|
|
uint16_t reserved;
|
|
uint16_t ir;
|
|
uint16_t green2;
|
|
} data;
|
|
uint8_t bytes[LSENSE_DATA_SIZE];
|
|
} light_sensor_data_t;
|
|
|
|
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;
|
|
|
|
i2c_bus.beginTransmission(addr);
|
|
i2c_bus.write(0x41);
|
|
i2c_bus.endTransmission();
|
|
|
|
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.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)
|
|
return;
|
|
|
|
uint8_t addr = LSENSE_I2C_BASE_ADDR | sensor_addr;
|
|
|
|
i2c_bus.beginTransmission(addr);
|
|
i2c_bus.write(0x50); // Start from red register
|
|
i2c_bus.endTransmission();
|
|
|
|
i2c_bus.requestFrom(addr, (uint8_t)LSENSE_DATA_SIZE);
|
|
for (uint8_t i = 0; i < LSENSE_DATA_SIZE; i++) {
|
|
sensor_data->bytes[i] = i2c_bus.read();
|
|
}
|
|
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.endTransmission();
|
|
|
|
i2c_bus.requestFrom(addr, (uint8_t)1);
|
|
if (i2c_bus.available()) {
|
|
response = i2c_bus.read();
|
|
if (response == 0xFF) {
|
|
response = 0; // Invalid response
|
|
}
|
|
}
|
|
|
|
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;
|
|
|
|
switch (bus_number) {
|
|
case BUS_X:
|
|
sda_pin = pin_order_inverted ? SCL1 : SDA1;
|
|
scl_pin = pin_order_inverted ? SDA1 : SCL1;
|
|
break;
|
|
case BUS_Y:
|
|
sda_pin = pin_order_inverted ? SCL0 : SDA0;
|
|
scl_pin = pin_order_inverted ? SDA0 : SCL0;
|
|
break;
|
|
case BUS_Z:
|
|
sda_pin = pin_order_inverted ? SCL2 : SDA2;
|
|
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)
|
|
{
|
|
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) {
|
|
g_sensors_detected_total++;
|
|
g_sensors_detected_per_bus[bus_number]++;
|
|
}
|
|
g_sensor_addresses[bus_number][SENSOR_POS] = detect_light_sensor(g_i2c_buses[bus_number], 1);
|
|
if (g_sensor_addresses[bus_number][SENSOR_POS] == LSENSE_EXPECTED_ID) {
|
|
g_sensors_detected_total++;
|
|
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);
|
|
if (g_sensor_addresses[bus_number][SENSOR_NEG] == LSENSE_EXPECTED_ID) {
|
|
g_sensors_detected_total++;
|
|
g_sensors_detected_per_bus[bus_number]++;
|
|
}
|
|
g_sensor_addresses[bus_number][SENSOR_POS] = detect_light_sensor(g_i2c_buses[bus_number], 1);
|
|
if (g_sensor_addresses[bus_number][SENSOR_POS] == LSENSE_EXPECTED_ID) {
|
|
g_sensors_detected_total++;
|
|
g_sensors_detected_per_bus[bus_number]++;
|
|
}
|
|
}
|
|
}
|
|
|
|
/*******************************************************************************/
|
|
/*******************************************************************************/
|
|
/* Command handlers */
|
|
/*******************************************************************************/
|
|
/*******************************************************************************/
|
|
|
|
void lsense_cmd_presence(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();
|
|
}
|
|
|
|
void lsense_cmd_read(void)
|
|
{
|
|
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]);
|
|
|
|
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]);
|
|
|
|
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]);
|
|
|
|
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);
|
|
|
|
Serial.flush();
|
|
SERIAL_BUFFER_CLEAR();
|
|
} |