diff --git a/src/boards/RIGv2/Actuators/Actuators.cpp b/src/boards/RIGv2/Actuators/Actuators.cpp index cacfb260fc161b03300ce0e91220a0159ca4a362..6ecc9f1afcde14f135d41e7d6d3aa5b55a22cbba 100644 --- a/src/boards/RIGv2/Actuators/Actuators.cpp +++ b/src/boards/RIGv2/Actuators/Actuators.cpp @@ -324,7 +324,7 @@ bool Actuators::isServoOpen(ServosList servo) return false; } - return info->closeTs == 0; + return info->closeTs != 0; } uint64_t Actuators::getServoOpeningTime(ServosList servo) diff --git a/src/boards/RIGv2/Configs/SensorsConfig.h b/src/boards/RIGv2/Configs/SensorsConfig.h index eb588426c2036fa1df5cf5a8d9c76fc836e598a8..4ea70e19b5023ef91be0dca36988b50285f99bf7 100644 --- a/src/boards/RIGv2/Configs/SensorsConfig.h +++ b/src/boards/RIGv2/Configs/SensorsConfig.h @@ -42,13 +42,20 @@ static constexpr float ADC1_CH3_SHUNT_RESISTANCE = 30.5; static constexpr float ADC1_CH4_SHUNT_RESISTANCE = 30.8; // ADC channels definitions for various sensors -static constexpr int ADC1_VESSEL_PT_CHANNEL = 0; -static constexpr int ADC1_FILLING_PT_CHANNEL = 1; -static constexpr int ADC1_BOTTOM_PT_CHANNEL = 2; -static constexpr int ADC1_TOP_PT_CHANNEL = 3; -static constexpr int ADC1_SERVO_CURRENT_CHANNEL = 4; -static constexpr int ADC1_VESSEL_LC_CHANNEL = 5; -static constexpr int ADC1_TANK_LC_CHANNEL = 6; +static constexpr Boardcore::ADS131M08Defs::Channel ADC1_VESSEL_PT_CHANNEL = + Boardcore::ADS131M08Defs::Channel::CHANNEL_0; +static constexpr Boardcore::ADS131M08Defs::Channel ADC1_FILLING_PT_CHANNEL = + Boardcore::ADS131M08Defs::Channel::CHANNEL_1; +static constexpr Boardcore::ADS131M08Defs::Channel ADC1_BOTTOM_PT_CHANNEL = + Boardcore::ADS131M08Defs::Channel::CHANNEL_2; +static constexpr Boardcore::ADS131M08Defs::Channel ADC1_TOP_PT_CHANNEL = + Boardcore::ADS131M08Defs::Channel::CHANNEL_3; +static constexpr Boardcore::ADS131M08Defs::Channel ADC1_SERVO_CURRENT_CHANNEL = + Boardcore::ADS131M08Defs::Channel::CHANNEL_4; +static constexpr Boardcore::ADS131M08Defs::Channel ADC1_VESSEL_LC_CHANNEL = + Boardcore::ADS131M08Defs::Channel::CHANNEL_5; +static constexpr Boardcore::ADS131M08Defs::Channel ADC1_TANK_LC_CHANNEL = + Boardcore::ADS131M08Defs::Channel::CHANNEL_6; static constexpr float PT_MIN_CURRENT = 4; static constexpr float PT_MAX_CURRENT = 20; diff --git a/src/boards/RIGv2/Radio/Radio.cpp b/src/boards/RIGv2/Radio/Radio.cpp index 54ae3ad9f5c4dbe05e30fffd49f5a60e21b56c28..ae216506ec4cf97ff8c3c85ca78965684b539f19 100644 --- a/src/boards/RIGv2/Radio/Radio.cpp +++ b/src/boards/RIGv2/Radio/Radio.cpp @@ -471,8 +471,8 @@ bool Radio::packSystemTm(uint8_t tmId, mavlink_message_t& msg) tm.timestamp = TimestampTimer::getTimestamp(); tm.tank_temperature = sensors->getTc1LastSample().temperature; - tm.top_tank_pressure = sensors->getTankTopPress().pressure; - tm.bottom_tank_pressure = sensors->getTankBottomPress().pressure; + tm.top_tank_pressure = sensors->getTopTankPress().pressure; + tm.bottom_tank_pressure = sensors->getBottomTankPress().pressure; tm.floating_level = 69.0f; // Lol // TODO(davide.mor): Add the rest of these diff --git a/src/boards/RIGv2/Sensors/AnalogLoadCellSensor.h b/src/boards/RIGv2/Sensors/AnalogLoadCellSensor.h new file mode 100644 index 0000000000000000000000000000000000000000..62654d48f8455851031d77b1fa8bc50fd8408bd2 --- /dev/null +++ b/src/boards/RIGv2/Sensors/AnalogLoadCellSensor.h @@ -0,0 +1,58 @@ +/* Copyright (c) 2024 Skyward Experimental Rocketry + * Authors: Davide Mor + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +#include <sensors/Sensor.h> +#include <sensors/SensorData.h> + +#include <functional> + +namespace RIGv2 +{ + +class AnalogLoadCellSensor : public Boardcore::Sensor<Boardcore::LoadCellData> +{ +public: + AnalogLoadCellSensor(std::function<Boardcore::ADCData()> getVoltage, + float scale) + : getVoltage{getVoltage}, scale{scale} + { + } + + bool init() override { return true; } + + bool selfTest() override { return true; } + +private: + Boardcore::LoadCellData sampleImpl() override + { + auto voltage = getVoltage(); + return {voltage.voltageTimestamp, -voltage.voltage * scale}; + } + + std::function<Boardcore::ADCData()> getVoltage; + + float scale; +}; + +} // namespace RIGv2 \ No newline at end of file diff --git a/src/boards/RIGv2/Sensors/Sensors.cpp b/src/boards/RIGv2/Sensors/Sensors.cpp index 174496907a4d4cfd102b1e770042a3592173891f..8f1871341ad933cc9ebc89375c87258e8b31f179 100644 --- a/src/boards/RIGv2/Sensors/Sensors.cpp +++ b/src/boards/RIGv2/Sensors/Sensors.cpp @@ -32,19 +32,6 @@ using namespace Boardcore; using namespace miosix; using namespace RIGv2; -float pressureFromVoltage(float voltage, float shuntResistance, - float minCurrent, float maxCurrent, float maxPressure) -{ - // First convert voltage to current [mA] - float current = (voltage / shuntResistance) * 1000.0f; - - // Convert to a value between [0, 1] based on the min and max current - float value = (current - minCurrent) / (maxCurrent - minCurrent); - - // Finally remap to the range [0, maxPressure] - return value * maxPressure; -} - bool Sensors::isStarted() { return started; } bool Sensors::start() @@ -53,6 +40,12 @@ bool Sensors::start() internalAdcInit(map); adc1Init(map); tc1Init(map); + vesselPressureInit(map); + fillingPressureInit(map); + topTankPressureInit(map); + bottomTankPressureInit(map); + vesselWeightInit(map); + tankWeightInit(map); manager = std::make_unique<SensorManager>(map, &scheduler); if (!manager->start()) @@ -68,89 +61,92 @@ bool Sensors::start() InternalADCData Sensors::getInternalADCLastSample() { PauseKernelLock l; + if (!internalAdc) + { + return {}; + } return internalAdc->getLastSample(); } ADS131M08Data Sensors::getADC1LastSample() { PauseKernelLock l; + if (!adc1) + { + return {}; + } return adc1->getLastSample(); } MAX31856Data Sensors::getTc1LastSample() { PauseKernelLock l; + if (!tc1) + { + return {}; + } + return tc1->getLastSample(); } PressureData Sensors::getVesselPress() { - auto sample = getADC1LastSample(); - - float pressure = pressureFromVoltage( - sample.voltage[Config::Sensors::ADC1_VESSEL_PT_CHANNEL], - Config::Sensors::ADC1_CH1_SHUNT_RESISTANCE, - Config::Sensors::PT_MIN_CURRENT, Config::Sensors::PT_MAX_CURRENT, - Config::Sensors::VESSEL_MAX_PRESSURE); - return {sample.timestamp, pressure}; + PauseKernelLock l; + if (!vesselPressure) + { + return {}; + } + return vesselPressure->getLastSample(); } PressureData Sensors::getFillingPress() { - auto sample = getADC1LastSample(); - - float pressure = pressureFromVoltage( - sample.voltage[Config::Sensors::ADC1_FILLING_PT_CHANNEL], - Config::Sensors::ADC1_CH2_SHUNT_RESISTANCE, - Config::Sensors::PT_MIN_CURRENT, Config::Sensors::PT_MAX_CURRENT, - Config::Sensors::FILLING_MAX_PRESSURE); - return {sample.timestamp, pressure}; + PauseKernelLock l; + if (!fillingPressure) + { + return {}; + } + return fillingPressure->getLastSample(); } -PressureData Sensors::getTankTopPress() +PressureData Sensors::getTopTankPress() { - auto sample = getADC1LastSample(); - - float pressure = pressureFromVoltage( - sample.voltage[Config::Sensors::ADC1_TOP_PT_CHANNEL], - Config::Sensors::ADC1_CH3_SHUNT_RESISTANCE, - Config::Sensors::PT_MIN_CURRENT, Config::Sensors::PT_MAX_CURRENT, - Config::Sensors::FILLING_MAX_PRESSURE); - return {sample.timestamp, pressure}; + PauseKernelLock l; + if (!topTankPressure) + { + return {}; + } + return topTankPressure->getLastSample(); } -PressureData Sensors::getTankBottomPress() +PressureData Sensors::getBottomTankPress() { - auto sample = getADC1LastSample(); - - float pressure = pressureFromVoltage( - sample.voltage[Config::Sensors::ADC1_BOTTOM_PT_CHANNEL], - Config::Sensors::ADC1_CH4_SHUNT_RESISTANCE, - Config::Sensors::PT_MIN_CURRENT, Config::Sensors::PT_MAX_CURRENT, - Config::Sensors::FILLING_MAX_PRESSURE); - return {sample.timestamp, pressure}; + PauseKernelLock l; + if (!bottomTankPressure) + { + return {}; + } + return bottomTankPressure->getLastSample(); } LoadCellData Sensors::getVesselWeight() { - auto sample = getADC1LastSample(); - float calibratedVoltage = - sample.voltage[Config::Sensors::ADC1_VESSEL_LC_CHANNEL] - - vesselLcOffset; - - return {sample.timestamp, - -calibratedVoltage * Config::Sensors::LC_VESSEL_SCALE}; + PauseKernelLock l; + if (!vesselWeight) + { + return {}; + } + return vesselWeight->getLastSample(); } LoadCellData Sensors::getTankWeight() { - auto sample = getADC1LastSample(); - float calibratedVoltage = - sample.voltage[Config::Sensors::ADC1_TANK_LC_CHANNEL] - tankLcOffset; - - // This ADC channel is flipped for some reason - return {sample.timestamp, - -calibratedVoltage * Config::Sensors::LC_TANK_SCALE}; + PauseKernelLock l; + if (!tankWeight) + { + return {}; + } + return tankWeight->getLastSample(); } CurrentData Sensors::getUmbilicalCurrent() @@ -163,7 +159,7 @@ CurrentData Sensors::getServoCurrent() auto sample = getADC1LastSample(); float current = - (sample.voltage[Config::Sensors::ADC1_SERVO_CURRENT_CHANNEL] - + (sample.voltage[(int)Config::Sensors::ADC1_SERVO_CURRENT_CHANNEL] - Config::Sensors::SERVO_CURRENT_ZERO) * Config::Sensors::SERVO_CURRENT_SCALE; // Current reading are flipped @@ -188,8 +184,9 @@ void Sensors::calibrate() auto sample = getADC1LastSample(); vesselStats.add( - sample.voltage[Config::Sensors::ADC1_VESSEL_LC_CHANNEL]); - tankStats.add(sample.voltage[Config::Sensors::ADC1_TANK_LC_CHANNEL]); + sample.voltage[(int)Config::Sensors::ADC1_VESSEL_LC_CHANNEL]); + tankStats.add( + sample.voltage[(int)Config::Sensors::ADC1_TANK_LC_CHANNEL]); Thread::sleep(Config::Sensors::LC_CALIBRATE_SAMPLE_PERIOD); } @@ -201,6 +198,12 @@ void Sensors::calibrate() std::vector<SensorInfo> Sensors::getSensorInfos() { return { + manager->getSensorInfo(vesselWeight.get()), + manager->getSensorInfo(tankWeight.get()), + manager->getSensorInfo(vesselPressure.get()), + manager->getSensorInfo(fillingPressure.get()), + manager->getSensorInfo(topTankPressure.get()), + manager->getSensorInfo(bottomTankPressure.get()), manager->getSensorInfo(internalAdc.get()), manager->getSensorInfo(adc1.get()), manager->getSensorInfo(tc1.get()), @@ -237,19 +240,62 @@ void Sensors::adc1Init(SensorManager::SensorMap_t &map) spiConfig.clockDivider = SPI::ClockDivider::DIV_32; ADS131M08::Config config = {}; - config.channelsConfig[Config::Sensors::ADC1_VESSEL_LC_CHANNEL].pga = - ADS131M08Defs::PGA::PGA_32; - config.channelsConfig[Config::Sensors::ADC1_TANK_LC_CHANNEL].pga = - ADS131M08Defs::PGA::PGA_32; + // Setup global configurations config.oversamplingRatio = ADS131M08Defs::OversamplingRatio::OSR_8192; config.globalChopModeEnabled = true; + // Disable all channels + config.channelsConfig[0].enabled = false; + config.channelsConfig[1].enabled = false; + config.channelsConfig[2].enabled = false; + config.channelsConfig[3].enabled = false; + config.channelsConfig[4].enabled = false; + config.channelsConfig[5].enabled = false; + config.channelsConfig[6].enabled = false; + config.channelsConfig[7].enabled = false; + + // Configure all required channels + config.channelsConfig[(int)Config::Sensors::ADC1_VESSEL_PT_CHANNEL] = { + .enabled = true, + .pga = ADS131M08Defs::PGA::PGA_1, + .offset = 0, + .gain = 1.0}; + + config.channelsConfig[(int)Config::Sensors::ADC1_FILLING_PT_CHANNEL] = { + .enabled = true, + .pga = ADS131M08Defs::PGA::PGA_1, + .offset = 0, + .gain = 1.0}; + + config.channelsConfig[(int)Config::Sensors::ADC1_BOTTOM_PT_CHANNEL] = { + .enabled = true, + .pga = ADS131M08Defs::PGA::PGA_1, + .offset = 0, + .gain = 1.0}; + + config.channelsConfig[(int)Config::Sensors::ADC1_TOP_PT_CHANNEL] = { + .enabled = true, + .pga = ADS131M08Defs::PGA::PGA_1, + .offset = 0, + .gain = 1.0}; + + config.channelsConfig[(int)Config::Sensors::ADC1_VESSEL_LC_CHANNEL] = { + .enabled = true, + .pga = ADS131M08Defs::PGA::PGA_32, + .offset = 0, + .gain = 1.0}; + + config.channelsConfig[(int)Config::Sensors::ADC1_TANK_LC_CHANNEL] = { + .enabled = true, + .pga = ADS131M08Defs::PGA::PGA_32, + .offset = 0, + .gain = 1.0}; + adc1 = std::make_unique<ADS131M08>(modules.get<Buses>()->getADS131M08_1(), sensors::ADS131_1::cs::getPin(), spiConfig, config); - SensorInfo info("ADS131M08_1", Config::Sensors::ADC_SAMPLE_PERIOD, - [this]() { adc1Callback(); }); + SensorInfo info("ADS131M08_1", 500, [this]() { adc1Callback(); }); map.emplace(std::make_pair(adc1.get(), info)); } @@ -263,6 +309,11 @@ void Sensors::adc1Callback() sample.voltage[4], sample.voltage[5], sample.voltage[6], sample.voltage[7]}; + /*LOG_INFO(logger, "{:.2f}\t{:.2f}\t{:.2f}\t{:.2f}", + (sample.voltage[0] / Config::Sensors::ADC1_CH1_SHUNT_RESISTANCE) * + 1000.0f, (sample.voltage[1] / 30.4f) * 1000.0f, (sample.voltage[2] + / 30.5f) * 1000.0f, (sample.voltage[3] / 30.8f) * 1000.0f);*/ + sdLogger.log(data); } @@ -289,6 +340,153 @@ void Sensors::tc1Callback() TCsData data{sample.temperatureTimestamp, 1, sample.temperature, sample.coldJunctionTemperature}; + sdLogger.log(data); +} + +void Sensors::vesselPressureInit(Boardcore::SensorManager::SensorMap_t &map) +{ + vesselPressure = std::make_unique<TrafagPressureSensor>( + [this]() + { + auto sample = adc1->getLastSample(); + return sample.getVoltage(Config::Sensors::ADC1_VESSEL_PT_CHANNEL); + }, + Config::Sensors::ADC1_CH1_SHUNT_RESISTANCE, + Config::Sensors::VESSEL_MAX_PRESSURE, Config::Sensors::PT_MIN_CURRENT, + Config::Sensors::PT_MAX_CURRENT); + + SensorInfo info("VesselPressure", Config::Sensors::ADC_SAMPLE_PERIOD, + [this]() { vesselPressureCallback(); }); + map.emplace(std::make_pair(vesselPressure.get(), info)); +} + +void Sensors::vesselPressureCallback() +{ + PressureData sample = vesselPressure->getLastSample(); + PTsData data{sample.pressureTimestamp, 1, sample.pressure}; + sdLogger.log(data); +} + +void Sensors::fillingPressureInit(Boardcore::SensorManager::SensorMap_t &map) +{ + fillingPressure = std::make_unique<TrafagPressureSensor>( + [this]() + { + auto sample = adc1->getLastSample(); + return sample.getVoltage(Config::Sensors::ADC1_FILLING_PT_CHANNEL); + }, + Config::Sensors::ADC1_CH2_SHUNT_RESISTANCE, + Config::Sensors::FILLING_MAX_PRESSURE, Config::Sensors::PT_MIN_CURRENT, + Config::Sensors::PT_MAX_CURRENT); + + SensorInfo info("FillingPressure", Config::Sensors::ADC_SAMPLE_PERIOD, + [this]() { fillingPressureCallback(); }); + map.emplace(std::make_pair(fillingPressure.get(), info)); +} + +void Sensors::fillingPressureCallback() +{ + PressureData sample = fillingPressure->getLastSample(); + PTsData data{sample.pressureTimestamp, 2, sample.pressure}; + sdLogger.log(data); +} + +void Sensors::topTankPressureInit(Boardcore::SensorManager::SensorMap_t &map) +{ + topTankPressure = std::make_unique<TrafagPressureSensor>( + [this]() + { + auto sample = adc1->getLastSample(); + return sample.getVoltage(Config::Sensors::ADC1_TOP_PT_CHANNEL); + }, + Config::Sensors::ADC1_CH3_SHUNT_RESISTANCE, + Config::Sensors::TANK_TOP_MAX_PRESSURE, Config::Sensors::PT_MIN_CURRENT, + Config::Sensors::PT_MAX_CURRENT); + + SensorInfo info("TopTankPressure", Config::Sensors::ADC_SAMPLE_PERIOD, + [this]() { topTankPressureCallback(); }); + map.emplace(std::make_pair(topTankPressure.get(), info)); +} +void Sensors::topTankPressureCallback() +{ + PressureData sample = topTankPressure->getLastSample(); + PTsData data{sample.pressureTimestamp, 3, sample.pressure}; + sdLogger.log(data); +} + +void Sensors::bottomTankPressureInit(Boardcore::SensorManager::SensorMap_t &map) +{ + bottomTankPressure = std::make_unique<TrafagPressureSensor>( + [this]() + { + auto sample = adc1->getLastSample(); + return sample.getVoltage(Config::Sensors::ADC1_BOTTOM_PT_CHANNEL); + }, + Config::Sensors::ADC1_CH4_SHUNT_RESISTANCE, + Config::Sensors::TANK_BOTTOM_MAX_PRESSURE, + Config::Sensors::PT_MIN_CURRENT, Config::Sensors::PT_MAX_CURRENT); + + SensorInfo info("BottomTankPressure", Config::Sensors::ADC_SAMPLE_PERIOD, + [this]() { bottomTankPressureCallback(); }); + map.emplace(std::make_pair(bottomTankPressure.get(), info)); +} + +void Sensors::bottomTankPressureCallback() +{ + PressureData sample = bottomTankPressure->getLastSample(); + PTsData data{sample.pressureTimestamp, 4, sample.pressure}; + sdLogger.log(data); +} + +void Sensors::vesselWeightInit(Boardcore::SensorManager::SensorMap_t &map) +{ + vesselWeight = std::make_unique<AnalogLoadCellSensor>( + [this]() + { + auto sample = adc1->getLastSample(); + auto voltage = + sample.getVoltage(Config::Sensors::ADC1_VESSEL_LC_CHANNEL); + voltage.voltage -= vesselLcOffset; + + return voltage; + }, + Config::Sensors::LC_VESSEL_SCALE); + + SensorInfo info("VesselWeight", Config::Sensors::ADC_SAMPLE_PERIOD, + [this]() { vesselWeightCallback(); }); + map.emplace(std::make_pair(vesselWeight.get(), info)); +} + +void Sensors::vesselWeightCallback() +{ + LoadCellData sample = vesselWeight->getLastSample(); + LCsData data{sample.loadTimestamp, 1, sample.load}; + sdLogger.log(data); +} + +void Sensors::tankWeightInit(Boardcore::SensorManager::SensorMap_t &map) +{ + tankWeight = std::make_unique<AnalogLoadCellSensor>( + [this]() + { + auto sample = adc1->getLastSample(); + auto voltage = + sample.getVoltage(Config::Sensors::ADC1_TANK_LC_CHANNEL); + voltage.voltage -= tankLcOffset; + + return voltage; + }, + Config::Sensors::LC_TANK_SCALE); + + SensorInfo info("TankWeight", Config::Sensors::ADC_SAMPLE_PERIOD, + [this]() { tankWeightCallback(); }); + map.emplace(std::make_pair(tankWeight.get(), info)); +} + +void Sensors::tankWeightCallback() +{ + LoadCellData sample = tankWeight->getLastSample(); + LCsData data{sample.loadTimestamp, 2, sample.load}; sdLogger.log(data); } \ No newline at end of file diff --git a/src/boards/RIGv2/Sensors/Sensors.h b/src/boards/RIGv2/Sensors/Sensors.h index 5dfa2c79eca9ce44662c6a4fdc68f5da466a00e0..70069e3c60e544123651083dbc2ee2a71575cc47 100644 --- a/src/boards/RIGv2/Sensors/Sensors.h +++ b/src/boards/RIGv2/Sensors/Sensors.h @@ -23,6 +23,8 @@ #pragma once #include <RIGv2/Sensors/SensorsData.h> +#include <RIGv2/Sensors/TrafagPressureSensor.h> +#include <RIGv2/Sensors/AnalogLoadCellSensor.h> #include <drivers/adc/InternalADC.h> #include <sensors/ADS131M08/ADS131M08.h> #include <sensors/MAX31856/MAX31856.h> @@ -31,8 +33,8 @@ #include <atomic> #include <functional> #include <memory> -#include <vector> #include <utils/ModuleManager/ModuleManager.hpp> +#include <vector> namespace RIGv2 { @@ -56,8 +58,8 @@ public: // Getters for processed data Boardcore::PressureData getVesselPress(); Boardcore::PressureData getFillingPress(); - Boardcore::PressureData getTankTopPress(); - Boardcore::PressureData getTankBottomPress(); + Boardcore::PressureData getTopTankPress(); + Boardcore::PressureData getBottomTankPress(); Boardcore::LoadCellData getVesselWeight(); Boardcore::LoadCellData getTankWeight(); Boardcore::CurrentData getUmbilicalCurrent(); @@ -69,6 +71,24 @@ public: std::vector<Boardcore::SensorInfo> getSensorInfos(); private: + void vesselPressureInit(Boardcore::SensorManager::SensorMap_t &map); + void vesselPressureCallback(); + + void fillingPressureInit(Boardcore::SensorManager::SensorMap_t &map); + void fillingPressureCallback(); + + void topTankPressureInit(Boardcore::SensorManager::SensorMap_t &map); + void topTankPressureCallback(); + + void bottomTankPressureInit(Boardcore::SensorManager::SensorMap_t &map); + void bottomTankPressureCallback(); + + void vesselWeightInit(Boardcore::SensorManager::SensorMap_t &map); + void vesselWeightCallback(); + + void tankWeightInit(Boardcore::SensorManager::SensorMap_t &map); + void tankWeightCallback(); + void internalAdcInit(Boardcore::SensorManager::SensorMap_t &map); void internalAdcCallback(); @@ -86,6 +106,16 @@ private: std::atomic<float> tankLcOffset{0.0f}; std::atomic<bool> started{false}; + + // Analog sensors + std::unique_ptr<TrafagPressureSensor> vesselPressure; + std::unique_ptr<TrafagPressureSensor> fillingPressure; + std::unique_ptr<TrafagPressureSensor> topTankPressure; + std::unique_ptr<TrafagPressureSensor> bottomTankPressure; + std::unique_ptr<AnalogLoadCellSensor> vesselWeight; + std::unique_ptr<AnalogLoadCellSensor> tankWeight; + + // Digital sensors std::unique_ptr<Boardcore::ADS131M08> adc1; std::unique_ptr<Boardcore::MAX31856> tc1; std::unique_ptr<Boardcore::InternalADC> internalAdc; diff --git a/src/boards/RIGv2/Sensors/SensorsData.h b/src/boards/RIGv2/Sensors/SensorsData.h index 801e399a93b7a5c583dd886177b8363e739cc1ab..d2d97acf293b5eae435f7bd26bf0d11256f05cc4 100644 --- a/src/boards/RIGv2/Sensors/SensorsData.h +++ b/src/boards/RIGv2/Sensors/SensorsData.h @@ -29,9 +29,7 @@ namespace RIGv2 { struct ADCsData : Boardcore::ADS131M08Data { - uint8_t adcNumber; - - ADCsData() : ADS131M08Data{0, 0, 0, 0, 0, 0, 0, 0, 0}, adcNumber{0} {} + uint8_t adcNumber = 0; ADCsData(uint64_t time, uint8_t num, float ch1, float ch2, float ch3, float ch4, float ch5, float ch6, float ch7, float ch8) @@ -58,9 +56,7 @@ struct ADCsData : Boardcore::ADS131M08Data struct TCsData : Boardcore::MAX31856Data { - uint8_t tcNumber; - - TCsData() : MAX31856Data{0, 0}, tcNumber{0} {} + uint8_t tcNumber = 0; TCsData(uint64_t time, uint8_t num, float temperature, float coldJunctionTemperature) @@ -71,7 +67,7 @@ struct TCsData : Boardcore::MAX31856Data static std::string header() { - return "temperatureTimestamp,tcNumber,temperature," + return "timestamp,tcNumber,temperature," "coldJunctionTemperature\n"; } @@ -82,6 +78,41 @@ struct TCsData : Boardcore::MAX31856Data } }; +struct LCsData : Boardcore::LoadCellData +{ + uint8_t lcNumber = 0; + + LCsData(uint64_t time, uint8_t num, float load) + : LoadCellData{time, load}, lcNumber{num} + { + } + + static std::string header() { return "timestamp,lcNumber,load\n"; } + + void print(std::ostream& os) const + { + os << loadTimestamp << "," << (int)lcNumber << "," << load << "\n"; + } +}; + +struct PTsData : Boardcore::PressureData +{ + uint8_t ptNumber = 0; + + PTsData(uint64_t time, uint8_t num, float pressure) + : PressureData{time, pressure}, ptNumber{num} + { + } + + static std::string header() { return "timestamp,ptNumber,pressure\n"; } + + void print(std::ostream& os) const + { + os << pressureTimestamp << "," << (int)ptNumber << "," << pressure + << "\n"; + } +}; + struct VoltageData { uint64_t voltageTimestamp; @@ -94,7 +125,7 @@ struct VoltageData { } - static std::string header() { return "voltageTimestamp,voltage\n"; } + static std::string header() { return "timestamp,voltage\n"; } void print(std::ostream& os) const { diff --git a/src/boards/RIGv2/Sensors/TrafagPressureSensor.h b/src/boards/RIGv2/Sensors/TrafagPressureSensor.h new file mode 100644 index 0000000000000000000000000000000000000000..3b2112705519f3a8fe947c4e7db887d4fad5c118 --- /dev/null +++ b/src/boards/RIGv2/Sensors/TrafagPressureSensor.h @@ -0,0 +1,77 @@ +/* Copyright (c) 2024 Skyward Experimental Rocketry + * Authors: Davide Mor + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +#include <sensors/Sensor.h> +#include <sensors/SensorData.h> + +#include <functional> + +namespace RIGv2 +{ + +class TrafagPressureSensor : public Boardcore::Sensor<Boardcore::PressureData> +{ +public: + TrafagPressureSensor(std::function<Boardcore::ADCData()> getVoltage, + float shuntResistance, float maxPressure, + float minCurrent = 4, float maxCurrent = 20) + : getVoltage{getVoltage}, shuntResistance{shuntResistance}, + maxPressure{maxPressure}, minCurrent{minCurrent}, + maxCurrent{maxCurrent} + { + } + + bool init() override { return true; } + + bool selfTest() override { return true; } + +private: + Boardcore::PressureData sampleImpl() override + { + auto voltage = getVoltage(); + float pressure = voltageToPressure(voltage.voltage); + + return {voltage.voltageTimestamp, pressure}; + } + + float voltageToPressure(float voltage) { + // First convert voltage to current + float current = (voltage / shuntResistance) * 1000.0f; + + // Convert to a value between 0 and 1 + float value = (current - minCurrent) / (maxCurrent - minCurrent); + + // Scale from 0 to maxPressure + return value * maxPressure; + } + + std::function<Boardcore::ADCData()> getVoltage; + + float shuntResistance; + float maxPressure; + float minCurrent; + float maxCurrent; +}; + +} // namespace RIGv2 \ No newline at end of file diff --git a/src/boards/RIGv2/StateMachines/TARS1/TARS1.cpp b/src/boards/RIGv2/StateMachines/TARS1/TARS1.cpp index c3e1e473f4bac9377637c522981838ace1ad07b9..870f91e81f8ed1181c7cca1636c74c19f7894794 100644 --- a/src/boards/RIGv2/StateMachines/TARS1/TARS1.cpp +++ b/src/boards/RIGv2/StateMachines/TARS1/TARS1.cpp @@ -256,7 +256,7 @@ void TARS1::sample() ModuleManager& modules = ModuleManager::getInstance(); Sensors* sensors = modules.get<Sensors>(); - pressureFilter.add(sensors->getTankBottomPress().pressure); + pressureFilter.add(sensors->getBottomTankPress().pressure); massFilter.add(sensors->getTankWeight().load); medianSamples++;