Move files to sbc_fw directory to make Arduino IDE shut up
This commit is contained in:
ThePetrovich
parent
db8f8408af
commit
63583ce984
11 changed files with 0 additions and 0 deletions
254
lsense.cpp
254
lsense.cpp
|
|
@ -1,254 +0,0 @@
|
|||
/*
|
||||
* @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();
|
||||
}
|
||||
Loading…
Add table
Add a link
Reference in a new issue