diff --git a/src/boards/RIGv2/Configs/ActuatorsConfig.h b/src/boards/RIGv2/Configs/ActuatorsConfig.h index 979d69ed92b22ac0c580f89b4c9dd5b838bb237d..1db325bee962ed0c7a4b49f9855852ad4c5a6f25 100644 --- a/src/boards/RIGv2/Configs/ActuatorsConfig.h +++ b/src/boards/RIGv2/Configs/ActuatorsConfig.h @@ -48,10 +48,10 @@ static constexpr uint32_t DEFAULT_RELEASE_OPENING_TIME = 10000; // 10s static constexpr uint32_t DEFAULT_DISCONNECT_OPENING_TIME = 10000; // 10s static constexpr float DEFAULT_FILLING_MAXIMUM_APERTURE = 1.00f; -static constexpr float DEFAULT_VENTING_MAXIMUM_APERTURE = 0.80f; -static constexpr float DEFAULT_MAIN_MAXIMUM_APERTURE = 0.87f; -static constexpr float DEFAULT_RELEASE_MAXIMUM_APERTURE = 1.0f; -static constexpr float DEFAULT_DISCONNECT_MAXIMUM_APERTURE = 0.110f; +static constexpr float DEFAULT_VENTING_MAXIMUM_APERTURE = 1.00f; +static constexpr float DEFAULT_MAIN_MAXIMUM_APERTURE = 1.00f; +static constexpr float DEFAULT_RELEASE_MAXIMUM_APERTURE = 1.00f; +static constexpr float DEFAULT_DISCONNECT_MAXIMUM_APERTURE = 1.00f; static constexpr bool FILLING_FLIPPED = true; static constexpr bool VENTING_FLIPPED = true; diff --git a/src/boards/RIGv2/Radio/Radio.cpp b/src/boards/RIGv2/Radio/Radio.cpp index 6b6846dd1453951fd59ce7fd33182d1fc3881021..bd978c62364d8941098fc1e4b70aa4ad311291f9 100644 --- a/src/boards/RIGv2/Radio/Radio.cpp +++ b/src/boards/RIGv2/Radio/Radio.cpp @@ -417,8 +417,11 @@ bool Radio::packSystemTm(uint8_t tmId, mavlink_message_t& msg) tm.ignition_state = modules.get<GroundModeManager>()->isIgniting(); // TODO(davide.mor): Add the rest of these - tm.battery_voltage = sensors->getADC1LastSample().voltage[0]; - tm.current_consumption = sensors->getADC1LastSample().voltage[1]; + // Temporary hack to tell if the board initialized or not + tm.main_board_status = modules.get<GroundModeManager>()->isDisarmed(); + + tm.battery_voltage = sensors->getBatteryVoltage().voltage; + tm.current_consumption = sensors->getServoCurrent().current; mavlink_msg_gse_tm_encode(Config::Radio::MAV_SYSTEM_ID, Config::Radio::MAV_COMPONENT_ID, &msg, @@ -439,8 +442,8 @@ bool Radio::packSystemTm(uint8_t tmId, mavlink_message_t& msg) tm.floating_level = 69.0f; // Lol // TODO(davide.mor): Add the rest of these - tm.battery_voltage = sensors->getADC1LastSample().voltage[2]; - tm.current_consumption = sensors->getADC1LastSample().voltage[3]; + tm.battery_voltage = 0.0f; + tm.current_consumption = sensors->getUmbilicalCurrent().current; mavlink_msg_motor_tm_encode(Config::Radio::MAV_SYSTEM_ID, Config::Radio::MAV_COMPONENT_ID, &msg, diff --git a/src/boards/RIGv2/Sensors/Sensors.cpp b/src/boards/RIGv2/Sensors/Sensors.cpp index b96c250da6c475aca3358b7ea41633284e21eea6..9805efde877018f232cb803d5be20fbccdabb71b 100644 --- a/src/boards/RIGv2/Sensors/Sensors.cpp +++ b/src/boards/RIGv2/Sensors/Sensors.cpp @@ -24,7 +24,6 @@ #include <RIGv2/Buses.h> #include <RIGv2/Configs/SensorsConfig.h> -#include <RIGv2/Sensors/SensorsData.h> #include <interfaces-impl/hwmapping.h> using namespace Boardcore; @@ -59,6 +58,7 @@ bool Sensors::isStarted() { return started; } bool Sensors::start() { SensorManager::SensorMap_t map; + internalAdcInit(map); adc1Init(map); tc1Init(map); @@ -73,19 +73,25 @@ bool Sensors::start() return true; } +InternalADCData Sensors::getInternalADCLastSample() +{ + PauseKernelLock l; + return internalAdc->getLastSample(); +} + ADS131M08Data Sensors::getADC1LastSample() { PauseKernelLock l; return adc1->getLastSample(); } -Boardcore::MAX31856Data Sensors::getTc1LastSample() +MAX31856Data Sensors::getTc1LastSample() { PauseKernelLock l; return tc1->getLastSample(); } -Boardcore::PressureData Sensors::getVesselPress() +PressureData Sensors::getVesselPress() { auto sample = getADC1LastSample(); @@ -97,7 +103,7 @@ Boardcore::PressureData Sensors::getVesselPress() return {sample.timestamp, pressure}; } -Boardcore::PressureData Sensors::getFillingPress() +PressureData Sensors::getFillingPress() { auto sample = getADC1LastSample(); @@ -109,7 +115,7 @@ Boardcore::PressureData Sensors::getFillingPress() return {sample.timestamp, pressure}; } -Boardcore::PressureData Sensors::getTankTopPress() +PressureData Sensors::getTankTopPress() { auto sample = getADC1LastSample(); @@ -121,7 +127,7 @@ Boardcore::PressureData Sensors::getTankTopPress() return {sample.timestamp, pressure}; } -Boardcore::PressureData Sensors::getTankBottomPress() +PressureData Sensors::getTankBottomPress() { auto sample = getADC1LastSample(); @@ -133,7 +139,7 @@ Boardcore::PressureData Sensors::getTankBottomPress() return {sample.timestamp, pressure}; } -Boardcore::LoadCellData Sensors::getVesselWeight() +LoadCellData Sensors::getVesselWeight() { auto sample = getADC1LastSample(); float calibratedVoltage = @@ -145,7 +151,7 @@ Boardcore::LoadCellData Sensors::getVesselWeight() return {sample.timestamp, load}; } -Boardcore::LoadCellData Sensors::getTankWeight() +LoadCellData Sensors::getTankWeight() { auto sample = getADC1LastSample(); float calibratedVoltage = @@ -156,6 +162,24 @@ Boardcore::LoadCellData Sensors::getTankWeight() return {sample.timestamp, load}; } +CurrentData Sensors::getUmbilicalCurrent() { + auto sample = getInternalADCLastSample(); + + return {sample.timestamp, sample.voltage[11]}; +} + +CurrentData Sensors::getServoCurrent() { + auto sample = getInternalADCLastSample(); + + return {sample.timestamp, sample.voltage[9]}; +} + +VoltageData Sensors::getBatteryVoltage() { + auto sample = getInternalADCLastSample(); + + return {sample.timestamp, sample.voltage[14]}; +} + void Sensors::calibrate() { Stats vesselStats, tankStats; @@ -176,6 +200,27 @@ void Sensors::calibrate() tankLcOffset = tankStats.getStats().mean; } +void Sensors::internalAdcInit(Boardcore::SensorManager::SensorMap_t &map) +{ + internalAdc = std::make_unique<InternalADC>(ADC1); + + internalAdc->enableChannel(InternalADC::CH9); + internalAdc->enableChannel(InternalADC::CH11); + internalAdc->enableChannel(InternalADC::CH14); + internalAdc->enableTemperature(); + internalAdc->enableVbat(); + + SensorInfo info("InternalAdc", Config::Sensors::ADC_SAMPLE_PERIOD, + [this]() { internalAdcCallback(); }); + map.emplace(internalAdc.get(), info); +} + +void Sensors::internalAdcCallback() +{ + InternalADCData sample = internalAdc->getLastSample(); + sdLogger.log(sample); +} + void Sensors::adc1Init(SensorManager::SensorMap_t &map) { ModuleManager &modules = ModuleManager::getInstance(); diff --git a/src/boards/RIGv2/Sensors/Sensors.h b/src/boards/RIGv2/Sensors/Sensors.h index 4383c8788623c9da2f8b48c4b27bef9a28735ac9..43770888a3dd4648eb04c6f146f8d1eb1b3fdbaa 100644 --- a/src/boards/RIGv2/Sensors/Sensors.h +++ b/src/boards/RIGv2/Sensors/Sensors.h @@ -22,6 +22,8 @@ #pragma once +#include <RIGv2/Sensors/SensorsData.h> +#include <drivers/adc/InternalADC.h> #include <sensors/ADS131M08/ADS131M08.h> #include <sensors/MAX31856/MAX31856.h> #include <sensors/SensorManager.h> @@ -45,6 +47,7 @@ public: bool isStarted(); // Getters for raw data coming from sensors + Boardcore::InternalADCData getInternalADCLastSample(); Boardcore::ADS131M08Data getADC1LastSample(); Boardcore::MAX31856Data getTc1LastSample(); @@ -55,10 +58,16 @@ public: Boardcore::PressureData getTankBottomPress(); Boardcore::LoadCellData getVesselWeight(); Boardcore::LoadCellData getTankWeight(); + Boardcore::CurrentData getUmbilicalCurrent(); + Boardcore::CurrentData getServoCurrent(); + VoltageData getBatteryVoltage(); void calibrate(); private: + void internalAdcInit(Boardcore::SensorManager::SensorMap_t &map); + void internalAdcCallback(); + void adc1Init(Boardcore::SensorManager::SensorMap_t &map); void adc1Callback(); @@ -75,6 +84,7 @@ private: std::atomic<bool> started{false}; std::unique_ptr<Boardcore::ADS131M08> adc1; std::unique_ptr<Boardcore::MAX31856> tc1; + std::unique_ptr<Boardcore::InternalADC> internalAdc; std::unique_ptr<Boardcore::SensorManager> manager; }; diff --git a/src/boards/RIGv2/Sensors/SensorsData.h b/src/boards/RIGv2/Sensors/SensorsData.h index ac2a64432705f98d1d16ad30c6f101eb7546c256..801e399a93b7a5c583dd886177b8363e739cc1ab 100644 --- a/src/boards/RIGv2/Sensors/SensorsData.h +++ b/src/boards/RIGv2/Sensors/SensorsData.h @@ -81,4 +81,24 @@ struct TCsData : Boardcore::MAX31856Data << "," << coldJunctionTemperature << "\n"; } }; + +struct VoltageData +{ + uint64_t voltageTimestamp; + float voltage; + + VoltageData() : voltageTimestamp{0}, voltage{0} {} + + VoltageData(uint64_t time, float voltage) + : voltageTimestamp{time}, voltage{voltage} + { + } + + static std::string header() { return "voltageTimestamp,voltage\n"; } + + void print(std::ostream& os) const + { + os << voltageTimestamp << "," << voltage << "\n"; + } +}; } // namespace RIGv2 \ No newline at end of file diff --git a/src/entrypoints/RIGv2/rig-v2-entry.cpp b/src/entrypoints/RIGv2/rig-v2-entry.cpp index b390de8f69afec81369dedbce783969b8b47783e..4825a5a7cf65665a36135eed5faa25304474f2e1 100644 --- a/src/entrypoints/RIGv2/rig-v2-entry.cpp +++ b/src/entrypoints/RIGv2/rig-v2-entry.cpp @@ -41,7 +41,6 @@ using namespace miosix; int main() { - ModuleManager &modules = ModuleManager::getInstance(); PrintLogger logger = Logging::getLogger("main"); @@ -76,16 +75,16 @@ int main() LOG_ERR(logger, "Error failed to insert Buses"); } - if (!modules.insert<Sensors>(sensors)) + if (!modules.insert<Actuators>(actuators)) { initResult = false; - LOG_ERR(logger, "Error failed to insert Sensors"); + LOG_ERR(logger, "Error failed to insert Actuators"); } - if (!modules.insert<Actuators>(actuators)) + if (!modules.insert<Sensors>(sensors)) { initResult = false; - LOG_ERR(logger, "Error failed to insert Actuators"); + LOG_ERR(logger, "Error failed to insert Sensors"); } if (!modules.insert<Radio>(radio))