From 4a7fb979a87c3756ce91594e8d2c9c96a5cfcf0b Mon Sep 17 00:00:00 2001 From: Zach Radlicz Date: Wed, 28 May 2025 11:58:53 -0500 Subject: [PATCH] split sensor initialization into talons and sensors only --- src/FlightControl.cpp | 28 +++++++++++++++++++++------- src/configuration/SensorManager.cpp | 12 +++++++++++- src/configuration/SensorManager.h | 6 ++++++ 3 files changed, 38 insertions(+), 8 deletions(-) diff --git a/src/FlightControl.cpp b/src/FlightControl.cpp index 54edff0..8fb46a2 100644 --- a/src/FlightControl.cpp +++ b/src/FlightControl.cpp @@ -1282,7 +1282,7 @@ int detectSensors(String dummyStr) // } // delay(10000); //DEBUG! // Serial.println(talons[t]->talonInterface); //DEBUG! - if(talons[t] && talons[t]->talonInterface != BusType::NONE && talons[t]->getTalonPort() != 0) { //Only proceed if Talon has a bus which can be iterated over, and the talon in question exists and has been detected + if((talons[t]) && (talons[t]->talonInterface != BusType::NONE) && (talons[t]->getTalonPort() != 0)) { //Only proceed if Talon has a bus which can be iterated over, and the talon in question exists and has been detected logger.enableData(talons[t]->getTalonPort(), true); //Turn on specific channel // logger.enableI2C_Global(true); // logger.enableI2C_OB(false); @@ -1297,13 +1297,16 @@ int detectSensors(String dummyStr) Serial.print(","); Serial.println(p); for(int s = 0; s < sensors.size(); s++) { //Iterate over all sensors objects - if(sensors[s]->getTalonPort() == 0 && talons[t]->talonInterface == sensors[s]->sensorInterface) { //If Talon not already specified AND sensor bus is compatible with Talon bus + if((sensors[s]->getTalonPort() == 0) && (talons[t]->talonInterface == sensors[s]->sensorInterface)) { //If Talon not already specified AND sensor bus is compatible with Talon bus Serial.print("Test Sensor: "); //DEBUG! Serial.println(s); if(sensors[s]->isPresent()) { //Test if that sensor is present, if it is, configure the port sensors[s]->setTalonPort(talons[t]->getTalonPort()); //Set the Talon port for the sensor sensors[s]->setSensorPort(p); - if(sensors[s]->keepPowered == true) talons[sensors[s]->getTalonPort() - 1]->keepPowered = true; //If any of the sensors on a Talon require power, set the flag for the Talon + if(sensors[s]->keepPowered == true) { + int currentTalonIndex = getIndexOfPort(sensors[s]->getTalonPort()); + talons[currentTalonIndex]->keepPowered = true; //If any of the sensors on a Talon require power, set the flag for the Talon + } Serial.print("Sensor Found:\n\t"); //DEBUG! Serial.println(sensors[s]->getTalonPort()); Serial.print('\t'); @@ -1315,6 +1318,9 @@ int detectSensors(String dummyStr) // talons[t]->enableData(p, false); break; //Exit the interation after the first sensor tests positive } + else { + Serial.println("Sensor not present"); + } delay(10); //Wait in between sensor calls // talons[t]->enableData(p, false); } @@ -1536,19 +1542,27 @@ bool loadConfiguration() { } void initializeSensorSystem() { - // Create SDI12 adapter if needed + // First initialize talons only (without sensors) + sensorManager.initializeTalons(); + + // Create SDI12 adapter from actual talons if needed auto& sdi12Talons = sensorManager.getSDI12Talons(); + Serial.println("Got sdi12 talons"); + Serial.println(sdi12Talons.size()); if (!sdi12Talons.empty() && realSdi12 == nullptr) { + Serial.println("Creating real SDI12 adapter"); realSdi12 = new SDI12TalonAdapter(*sdi12Talons[0]); } - // Initialize sensors + // Now initialize sensors with proper adapter if (realSdi12 != nullptr) { - sensorManager.initializeSensors(realTimeProvider, *realSdi12); + Serial.println("Using real SDI12 adapter"); + sensorManager.initializeSensorsOnly(realTimeProvider, *realSdi12); } else { + Serial.println("Creating dummy adapter"); SDI12Talon dummyTalon(0, 0x14); SDI12TalonAdapter dummyAdapter(dummyTalon); - sensorManager.initializeSensors(realTimeProvider, dummyAdapter); + sensorManager.initializeSensorsOnly(realTimeProvider, dummyAdapter); } updateSensorVectors(); diff --git a/src/configuration/SensorManager.cpp b/src/configuration/SensorManager.cpp index 6dfb2a9..bd999bb 100644 --- a/src/configuration/SensorManager.cpp +++ b/src/configuration/SensorManager.cpp @@ -10,6 +10,14 @@ void SensorManager::initializeSensors(ITimeProvider& timeProvider, ISDI12Talon& // Clear existing sensors clearAllSensors(); + // Initialize talons first + initializeTalons(); + + // Initialize sensors + initializeSensorsOnly(timeProvider, sdi12Interface); +} + +void SensorManager::initializeTalons() { // Create Talons for (int i = 0; i < configManager.getNumAuxTalons(); i++) { auxTalons.push_back(ConfigurationManager::createAuxTalon()); @@ -22,7 +30,9 @@ void SensorManager::initializeSensors(ITimeProvider& timeProvider, ISDI12Talon& for (int i = 0; i < configManager.getNumSDI12Talons(); i++) { sdi12Talons.push_back(ConfigurationManager::createSDI12Talon()); } - +} + +void SensorManager::initializeSensorsOnly(ITimeProvider& timeProvider, ISDI12Talon& sdi12Interface) { // Create Sensors for (int i = 0; i < configManager.getNumHaar(); i++) { haarSensors.push_back(ConfigurationManager::createHaarSensor()); diff --git a/src/configuration/SensorManager.h b/src/configuration/SensorManager.h index 1834e83..3aff736 100644 --- a/src/configuration/SensorManager.h +++ b/src/configuration/SensorManager.h @@ -23,6 +23,12 @@ class SensorManager { // Initialize all sensor vectors based on configuration void initializeSensors(ITimeProvider& timeProvider, ISDI12Talon& sdi12Interface); + // Initialize only talons + void initializeTalons(); + + // Initialize only sensors (after talons are already initialized) + void initializeSensorsOnly(ITimeProvider& timeProvider, ISDI12Talon& sdi12Interface); + // Clear all sensors void clearAllSensors();