diff --git a/src/boards/RIGv2/Actuators/Actuators.cpp b/src/boards/RIGv2/Actuators/Actuators.cpp index 6ecc9f1afcde14f135d41e7d6d3aa5b55a22cbba..737cb4fb32a29636a40c7f3e0b7f07435d3a297b 100644 --- a/src/boards/RIGv2/Actuators/Actuators.cpp +++ b/src/boards/RIGv2/Actuators/Actuators.cpp @@ -212,6 +212,12 @@ bool Actuators::start() return true; } +void Actuators::armLightOn() { relays::armLight::low(); } +void Actuators::armLightOff() { relays::armLight::high(); } + +void Actuators::igniterOn() { relays::ignition::low(); } +void Actuators::igniterOff() { relays::ignition::high(); } + bool Actuators::wiggleServo(ServosList servo) { // Wiggle means open the servo for 1s diff --git a/src/boards/RIGv2/Actuators/Actuators.h b/src/boards/RIGv2/Actuators/Actuators.h index 7bc883ab1d555d3ef32472b4ad2f17967fe216fd..42282e2a20f36e7b08b92a4fb0e334b841229900 100644 --- a/src/boards/RIGv2/Actuators/Actuators.h +++ b/src/boards/RIGv2/Actuators/Actuators.h @@ -82,6 +82,12 @@ public: uint64_t getServoOpeningTime(ServosList servo); float getServoMaxAperture(ServosList servo); + void armLightOn(); + void armLightOff(); + + void igniterOn(); + void igniterOff(); + private: ServoInfo *getServo(ServosList servo); diff --git a/src/boards/RIGv2/BoardScheduler.h b/src/boards/RIGv2/BoardScheduler.h new file mode 100644 index 0000000000000000000000000000000000000000..c36bd00d664d641ce4540c6410223f8b664f5646 --- /dev/null +++ b/src/boards/RIGv2/BoardScheduler.h @@ -0,0 +1,51 @@ +/* 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 <RIGv2/Configs/SchedulerConfig.h> +#include <scheduler/TaskScheduler.h> + +#include <utils/ModuleManager/ModuleManager.hpp> + +namespace RIGv2 +{ + +class BoardScheduler : public Boardcore::Module +{ +public: + BoardScheduler() : tars1(TARS1_PRIORITY), sensors(SENSORS_PRIORITY) {} + + [[nodiscard]] bool start() { return tars1.start() && sensors.start(); } + + Boardcore::TaskScheduler &getTars1Scheduler() { return tars1; } + + Boardcore::TaskScheduler &getSensorsScheduler() { return sensors; } + + Boardcore::TaskScheduler &getActuatorsScheduler() { return sensors; } + +private: + Boardcore::TaskScheduler tars1; + Boardcore::TaskScheduler sensors; +}; + +} // namespace RIGv2 \ No newline at end of file diff --git a/src/boards/RIGv2/Configs/SchedulerConfig.h b/src/boards/RIGv2/Configs/SchedulerConfig.h new file mode 100644 index 0000000000000000000000000000000000000000..4d07ba5b4fe68388cb7499497ca15ea046f6b71d --- /dev/null +++ b/src/boards/RIGv2/Configs/SchedulerConfig.h @@ -0,0 +1,38 @@ +/* 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 <miosix.h> + +namespace RIGv2 +{ + +// Used for TARS1 task scheduler/FSM +static const miosix::Priority TARS1_PRIORITY = miosix::PRIORITY_MAX - 1; +// Used for Sensors TaskScheduler +static const miosix::Priority SENSORS_PRIORITY = miosix::PRIORITY_MAX - 2; + +// Used for GMM FSM +static const miosix::Priority GMM_PRIORITY = miosix::PRIORITY_MAX - 1; + +} \ No newline at end of file diff --git a/src/boards/RIGv2/Configs/SensorsConfig.h b/src/boards/RIGv2/Configs/SensorsConfig.h index 03c54eb8abbc26000984cfef221edb4b6c91901a..a9b2f1c65c380bdc9197ae2e12ff3f50ccd45e94 100644 --- a/src/boards/RIGv2/Configs/SensorsConfig.h +++ b/src/boards/RIGv2/Configs/SensorsConfig.h @@ -33,41 +33,30 @@ namespace Config namespace Sensors { -static constexpr uint32_t ADC_SAMPLE_PERIOD = 10; -static constexpr uint32_t TC_SAMPLE_PERIOD = 100; +namespace ADS131M08 +{ -static constexpr float ADC1_CH1_SHUNT_RESISTANCE = 29.4048; -static constexpr float ADC1_CH2_SHUNT_RESISTANCE = 29.5830; -static constexpr float ADC1_CH3_SHUNT_RESISTANCE = 29.4973; -static constexpr float ADC1_CH4_SHUNT_RESISTANCE = 29.8849; +static constexpr float CH1_SHUNT_RESISTANCE = 29.4048; +static constexpr float CH2_SHUNT_RESISTANCE = 29.5830; +static constexpr float CH3_SHUNT_RESISTANCE = 29.4973; +static constexpr float CH4_SHUNT_RESISTANCE = 29.8849; // ADC channels definitions for various sensors -static constexpr Boardcore::ADS131M08Defs::Channel ADC1_VESSEL_PT_CHANNEL = +static constexpr Boardcore::ADS131M08Defs::Channel VESSEL_PT_CHANNEL = Boardcore::ADS131M08Defs::Channel::CHANNEL_0; -static constexpr Boardcore::ADS131M08Defs::Channel ADC1_FILLING_PT_CHANNEL = +static constexpr Boardcore::ADS131M08Defs::Channel FILLING_PT_CHANNEL = Boardcore::ADS131M08Defs::Channel::CHANNEL_1; -static constexpr Boardcore::ADS131M08Defs::Channel ADC1_BOTTOM_PT_CHANNEL = +static constexpr Boardcore::ADS131M08Defs::Channel BOTTOM_PT_CHANNEL = Boardcore::ADS131M08Defs::Channel::CHANNEL_2; -static constexpr Boardcore::ADS131M08Defs::Channel ADC1_TOP_PT_CHANNEL = +static constexpr Boardcore::ADS131M08Defs::Channel TOP_PT_CHANNEL = Boardcore::ADS131M08Defs::Channel::CHANNEL_3; -static constexpr Boardcore::ADS131M08Defs::Channel ADC1_SERVO_CURRENT_CHANNEL = +static constexpr Boardcore::ADS131M08Defs::Channel SERVO_CURRENT_CHANNEL = Boardcore::ADS131M08Defs::Channel::CHANNEL_4; -static constexpr Boardcore::ADS131M08Defs::Channel ADC1_VESSEL_LC_CHANNEL = +static constexpr Boardcore::ADS131M08Defs::Channel VESSEL_LC_CHANNEL = Boardcore::ADS131M08Defs::Channel::CHANNEL_5; -static constexpr Boardcore::ADS131M08Defs::Channel ADC1_TANK_LC_CHANNEL = +static constexpr Boardcore::ADS131M08Defs::Channel TANK_LC_CHANNEL = Boardcore::ADS131M08Defs::Channel::CHANNEL_6; -static constexpr float PT_MIN_CURRENT = 4; -static constexpr float PT_MAX_CURRENT = 20; - -static constexpr float FILLING_MAX_PRESSURE = 100; // bar -static constexpr float TANK_TOP_MAX_PRESSURE = 100; // bar -static constexpr float TANK_BOTTOM_MAX_PRESSURE = 100; // bar -static constexpr float VESSEL_MAX_PRESSURE = 400; // bar - -static constexpr unsigned int LC_CALIBRATE_SAMPLE_COUNT = 10; -static constexpr unsigned int LC_CALIBRATE_SAMPLE_PERIOD = 40; - // Servo current sensor calibration data // - A: 0.0 V: 2.520 // - A: 0.5 V: 2.513 @@ -76,20 +65,64 @@ static constexpr unsigned int LC_CALIBRATE_SAMPLE_PERIOD = 40; // - A: 2.0 V: 2.490 // - A: 2.5 V: 2.484 // - A: 5.2 V: 2.441 -static constexpr float SERVO_CURRENT_SCALE = 4.5466; -static constexpr float SERVO_CURRENT_ZERO = 2.520 / SERVO_CURRENT_SCALE; -static constexpr float BATTERY_VOLTAGE_SCALE = 4.7917; +static constexpr float SERVO_CURRENT_SCALE = 4.5466; +static constexpr float SERVO_CURRENT_ZERO = 2.520 / SERVO_CURRENT_SCALE; + +static constexpr uint32_t SAMPLE_PERIOD = 10; +static constexpr bool ENABLED = true; +} // namespace ADS131M08 + +namespace MAX31856 +{ +static constexpr uint32_t SAMPLE_PERIOD = 100; +static constexpr bool ENABLED = true; +} // namespace MAX31856 + +namespace Trafag +{ +static constexpr float FILLING_SHUNT_RESISTANCE = + ADS131M08::CH2_SHUNT_RESISTANCE; +static constexpr float TANK_TOP_SHUNT_RESISTANCE = + ADS131M08::CH4_SHUNT_RESISTANCE; +static constexpr float TANK_BOTTOM_SHUNT_RESISTANCE = + ADS131M08::CH3_SHUNT_RESISTANCE; +static constexpr float VESSEL_SHUNT_RESISTANCE = + ADS131M08::CH1_SHUNT_RESISTANCE; + +static constexpr float MIN_CURRENT = 4; +static constexpr float MAX_CURRENT = 20; + +static constexpr float FILLING_MAX_PRESSURE = 100; // bar +static constexpr float TANK_TOP_MAX_PRESSURE = 100; // bar +static constexpr float TANK_BOTTOM_MAX_PRESSURE = 100; // bar +static constexpr float VESSEL_MAX_PRESSURE = 400; // bar +} // namespace Trafag + +namespace LoadCell +{ +static constexpr unsigned int CALIBRATE_SAMPLE_COUNT = 10; +static constexpr unsigned int CALIBRATE_SAMPLE_PERIOD = 40; // LC Tank sensor calibration data // - 1.866kg V: 0.000941 // - 5.050kg V: 0.002550 // - 6.916kg V: 0.003559 -static constexpr float LC_TANK_SCALE = 1968.8771f; +static constexpr float TANK_SCALE = 1968.8771f; // LC Vessel sensor calibration data // - 1.866kg V: 0.00027 // - 5.050kg V: 0.00073 // - 6.916kg V: 0.00100 -static constexpr float LC_VESSEL_SCALE = 6914.9731f; +static constexpr float VESSEL_SCALE = 6914.9731f; +} + +namespace InternalADC +{ +static constexpr Boardcore::InternalADC::Channel BATTERY_VOLTAGE_CHANNEL = Boardcore::InternalADC::CH14; + +static constexpr float BATTERY_VOLTAGE_SCALE = 4.7917; +static constexpr uint32_t SAMPLE_PERIOD = 100; +static constexpr bool ENABLED = true; +} // namespace InternalADC } // namespace Sensors diff --git a/src/boards/RIGv2/Sensors/Sensors.cpp b/src/boards/RIGv2/Sensors/Sensors.cpp index 2aa8581cf3db92320f3fb6c7eb3aa97ab83f9694..882ea23b7e8b21a0627552c583155d90d9e84664 100644 --- a/src/boards/RIGv2/Sensors/Sensors.cpp +++ b/src/boards/RIGv2/Sensors/Sensors.cpp @@ -60,93 +60,66 @@ bool Sensors::start() InternalADCData Sensors::getInternalADCLastSample() { - PauseKernelLock l; - if (!internalAdc) - { - return {}; - } - return internalAdc->getLastSample(); + return internalAdc ? internalAdc->getLastSample() : InternalADCData{}; } ADS131M08Data Sensors::getADC1LastSample() { - PauseKernelLock l; - if (!adc1) - { - return {}; - } - return adc1->getLastSample(); + return adc1 ? adc1->getLastSample() : ADS131M08Data{}; } MAX31856Data Sensors::getTc1LastSample() { - PauseKernelLock l; - if (!tc1) - { - return {}; - } - - return tc1->getLastSample(); + return tc1 ? tc1->getLastSample() : MAX31856Data{}; } PressureData Sensors::getVesselPress() { - PauseKernelLock l; - if (!vesselPressure) - { - return {}; - } - return vesselPressure->getLastSample(); + return vesselPressure ? vesselPressure->getLastSample() : PressureData{}; } PressureData Sensors::getFillingPress() { - PauseKernelLock l; - if (!fillingPressure) - { - return {}; - } - return fillingPressure->getLastSample(); + return fillingPressure ? fillingPressure->getLastSample() : PressureData{}; } PressureData Sensors::getTopTankPress() { - PauseKernelLock l; - if (!topTankPressure) - { - return {}; - } - return topTankPressure->getLastSample(); + return topTankPressure ? topTankPressure->getLastSample() : PressureData{}; } PressureData Sensors::getBottomTankPress() { - PauseKernelLock l; - if (!bottomTankPressure) - { - return {}; - } - return bottomTankPressure->getLastSample(); + return bottomTankPressure ? bottomTankPressure->getLastSample() + : PressureData{}; } LoadCellData Sensors::getVesselWeight() { - PauseKernelLock l; - if (!vesselWeight) + if (vesselWeight) + { + auto sample = vesselWeight->getLastSample(); + sample.load -= vesselLcOffset; + return sample; + } + else { return {}; } - return vesselWeight->getLastSample(); } LoadCellData Sensors::getTankWeight() { - PauseKernelLock l; - if (!tankWeight) + if (tankWeight) + { + auto sample = tankWeight->getLastSample(); + sample.load -= tankLcOffset; + return sample; + } + else { return {}; } - return tankWeight->getLastSample(); } CurrentData Sensors::getUmbilicalCurrent() @@ -159,9 +132,10 @@ CurrentData Sensors::getServoCurrent() auto sample = getADC1LastSample(); float current = - (sample.voltage[(int)Config::Sensors::ADC1_SERVO_CURRENT_CHANNEL] - - Config::Sensors::SERVO_CURRENT_ZERO) * - Config::Sensors::SERVO_CURRENT_SCALE; + (sample + .voltage[(int)Config::Sensors::ADS131M08::SERVO_CURRENT_CHANNEL] - + Config::Sensors::ADS131M08::SERVO_CURRENT_ZERO) * + Config::Sensors::ADS131M08::SERVO_CURRENT_SCALE; // Current reading are flipped return {sample.timestamp, -current / 5.0f * 50.0f}; } @@ -170,7 +144,10 @@ VoltageData Sensors::getBatteryVoltage() { auto sample = getInternalADCLastSample(); - float voltage = sample.voltage[14] * Config::Sensors::BATTERY_VOLTAGE_SCALE; + float voltage = + sample.voltage[( + int)Config::Sensors::InternalADC::BATTERY_VOLTAGE_CHANNEL] * + Config::Sensors::InternalADC::BATTERY_VOLTAGE_SCALE; return {sample.timestamp, voltage}; } @@ -178,17 +155,14 @@ void Sensors::calibrate() { Stats vesselStats, tankStats; - for (unsigned int i = 0; i < Config::Sensors::LC_CALIBRATE_SAMPLE_COUNT; - i++) + for (unsigned int i = 0; + i < Config::Sensors::LoadCell::CALIBRATE_SAMPLE_COUNT; i++) { - auto sample = getADC1LastSample(); - - vesselStats.add( - sample.voltage[(int)Config::Sensors::ADC1_VESSEL_LC_CHANNEL]); - tankStats.add( - sample.voltage[(int)Config::Sensors::ADC1_TANK_LC_CHANNEL]); + // Tank readings WITHOUT offsets + vesselStats.add(vesselWeight->getLastSample().load); + tankStats.add(tankWeight->getLastSample().load); - Thread::sleep(Config::Sensors::LC_CALIBRATE_SAMPLE_PERIOD); + Thread::sleep(Config::Sensors::LoadCell::CALIBRATE_SAMPLE_PERIOD); } vesselLcOffset = vesselStats.getStats().mean; @@ -197,17 +171,24 @@ 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()), - }; + if (manager) + { + 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()), + }; + } + else + { + return {}; + } } void Sensors::internalAdcInit(Boardcore::SensorManager::SensorMap_t &map) @@ -216,12 +197,15 @@ void Sensors::internalAdcInit(Boardcore::SensorManager::SensorMap_t &map) internalAdc->enableChannel(InternalADC::CH9); internalAdc->enableChannel(InternalADC::CH11); - internalAdc->enableChannel(InternalADC::CH14); + internalAdc->enableChannel( + Config::Sensors::InternalADC::BATTERY_VOLTAGE_CHANNEL); internalAdc->enableTemperature(); internalAdc->enableVbat(); - SensorInfo info("InternalAdc", Config::Sensors::ADC_SAMPLE_PERIOD, - [this]() { internalAdcCallback(); }); + SensorInfo info( + "InternalAdc", Config::Sensors::InternalADC::SAMPLE_PERIOD, + [this]() { internalAdcCallback(); }, + Config::Sensors::InternalADC::ENABLED); map.emplace(internalAdc.get(), info); } @@ -255,43 +239,44 @@ void Sensors::adc1Init(SensorManager::SensorMap_t &map) 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] = { + config.channelsConfig[(int)Config::Sensors::ADS131M08::VESSEL_PT_CHANNEL] = + {.enabled = true, + .pga = ADS131M08Defs::PGA::PGA_1, + .offset = 0, + .gain = 1.0}; + + config.channelsConfig[(int)Config::Sensors::ADS131M08::FILLING_PT_CHANNEL] = + {.enabled = true, + .pga = ADS131M08Defs::PGA::PGA_1, + .offset = 0, + .gain = 1.0}; + + config.channelsConfig[(int)Config::Sensors::ADS131M08::BOTTOM_PT_CHANNEL] = + {.enabled = true, + .pga = ADS131M08Defs::PGA::PGA_1, + .offset = 0, + .gain = 1.0}; + + config.channelsConfig[(int)Config::Sensors::ADS131M08::TOP_PT_CHANNEL] = { .enabled = true, .pga = ADS131M08Defs::PGA::PGA_1, .offset = 0, .gain = 1.0}; - config.channelsConfig[(int)Config::Sensors::ADC1_TOP_PT_CHANNEL] = { + config.channelsConfig[( + int)Config::Sensors::ADS131M08::SERVO_CURRENT_CHANNEL] = { .enabled = true, .pga = ADS131M08Defs::PGA::PGA_1, .offset = 0, .gain = 1.0}; - config.channelsConfig[(int)Config::Sensors::ADC1_SERVO_CURRENT_CHANNEL] = { - .enabled = true, - .pga = ADS131M08Defs::PGA::PGA_1, - .offset = 0, - .gain = 1.0}; + config.channelsConfig[(int)Config::Sensors::ADS131M08::VESSEL_LC_CHANNEL] = + {.enabled = true, + .pga = ADS131M08Defs::PGA::PGA_32, + .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] = { + config.channelsConfig[(int)Config::Sensors::ADS131M08::TANK_LC_CHANNEL] = { .enabled = true, .pga = ADS131M08Defs::PGA::PGA_32, .offset = 0, @@ -301,7 +286,9 @@ void Sensors::adc1Init(SensorManager::SensorMap_t &map) sensors::ADS131_1::cs::getPin(), spiConfig, config); - SensorInfo info("ADS131M08_1", 500, [this]() { adc1Callback(); }); + SensorInfo info( + "ADS131M08_1", Config::Sensors::ADS131M08::SAMPLE_PERIOD, + [this]() { adc1Callback(); }, Config::Sensors::ADS131M08::ENABLED); map.emplace(std::make_pair(adc1.get(), info)); } @@ -316,10 +303,12 @@ void Sensors::adc1Callback() sample.voltage[6], sample.voltage[7]}; // LOG_INFO(logger, "{:.4}\t{:.4}\t{:.4}\t{:.4}", - // (sample.voltage[0] / Config::Sensors::ADC1_CH1_SHUNT_RESISTANCE) * 1000.0f, - // (sample.voltage[1] / Config::Sensors::ADC1_CH2_SHUNT_RESISTANCE) * 1000.0f, - // (sample.voltage[2] / Config::Sensors::ADC1_CH3_SHUNT_RESISTANCE) * 1000.0f, - // (sample.voltage[3] / Config::Sensors::ADC1_CH4_SHUNT_RESISTANCE) * 1000.0f); + // (sample.voltage[0] / Config::Sensors::ADC1_CH1_SHUNT_RESISTANCE) + // * 1000.0f, (sample.voltage[1] / + // Config::Sensors::ADC1_CH2_SHUNT_RESISTANCE) * 1000.0f, + // (sample.voltage[2] / Config::Sensors::ADC1_CH3_SHUNT_RESISTANCE) + // * 1000.0f, (sample.voltage[3] / + // Config::Sensors::ADC1_CH4_SHUNT_RESISTANCE) * 1000.0f); sdLogger.log(data); } @@ -336,8 +325,9 @@ void Sensors::tc1Init(SensorManager::SensorMap_t &map) sensors::MAX31856_1::cs::getPin(), spiConfig, MAX31856::ThermocoupleType::K_TYPE); - SensorInfo info("MAX31856_1", Config::Sensors::TC_SAMPLE_PERIOD, - [this]() { tc1Callback(); }); + SensorInfo info( + "MAX31856_1", Config::Sensors::MAX31856::SAMPLE_PERIOD, + [this]() { tc1Callback(); }, Config::Sensors::MAX31856::ENABLED); map.emplace(std::make_pair(tc1.get(), info)); } @@ -356,13 +346,15 @@ void Sensors::vesselPressureInit(Boardcore::SensorManager::SensorMap_t &map) [this]() { auto sample = adc1->getLastSample(); - return sample.getVoltage(Config::Sensors::ADC1_VESSEL_PT_CHANNEL); + return sample.getVoltage( + Config::Sensors::ADS131M08::VESSEL_PT_CHANNEL); }, - Config::Sensors::ADC1_CH1_SHUNT_RESISTANCE, - Config::Sensors::VESSEL_MAX_PRESSURE, Config::Sensors::PT_MIN_CURRENT, - Config::Sensors::PT_MAX_CURRENT); + Config::Sensors::Trafag::VESSEL_SHUNT_RESISTANCE, + Config::Sensors::Trafag::VESSEL_MAX_PRESSURE, + Config::Sensors::Trafag::MIN_CURRENT, + Config::Sensors::Trafag::MAX_CURRENT); - SensorInfo info("VesselPressure", Config::Sensors::ADC_SAMPLE_PERIOD, + SensorInfo info("VesselPressure", Config::Sensors::ADS131M08::SAMPLE_PERIOD, [this]() { vesselPressureCallback(); }); map.emplace(std::make_pair(vesselPressure.get(), info)); } @@ -380,13 +372,16 @@ void Sensors::fillingPressureInit(Boardcore::SensorManager::SensorMap_t &map) [this]() { auto sample = adc1->getLastSample(); - return sample.getVoltage(Config::Sensors::ADC1_FILLING_PT_CHANNEL); + return sample.getVoltage( + Config::Sensors::ADS131M08::FILLING_PT_CHANNEL); }, - Config::Sensors::ADC1_CH2_SHUNT_RESISTANCE, - Config::Sensors::FILLING_MAX_PRESSURE, Config::Sensors::PT_MIN_CURRENT, - Config::Sensors::PT_MAX_CURRENT); + Config::Sensors::Trafag::FILLING_SHUNT_RESISTANCE, + Config::Sensors::Trafag::FILLING_MAX_PRESSURE, + Config::Sensors::Trafag::MIN_CURRENT, + Config::Sensors::Trafag::MAX_CURRENT); - SensorInfo info("FillingPressure", Config::Sensors::ADC_SAMPLE_PERIOD, + SensorInfo info("FillingPressure", + Config::Sensors::ADS131M08::SAMPLE_PERIOD, [this]() { fillingPressureCallback(); }); map.emplace(std::make_pair(fillingPressure.get(), info)); } @@ -404,13 +399,16 @@ void Sensors::topTankPressureInit(Boardcore::SensorManager::SensorMap_t &map) [this]() { auto sample = adc1->getLastSample(); - return sample.getVoltage(Config::Sensors::ADC1_TOP_PT_CHANNEL); + return sample.getVoltage( + Config::Sensors::ADS131M08::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); + Config::Sensors::Trafag::TANK_TOP_SHUNT_RESISTANCE, + Config::Sensors::Trafag::TANK_TOP_MAX_PRESSURE, + Config::Sensors::Trafag::MIN_CURRENT, + Config::Sensors::Trafag::MAX_CURRENT); - SensorInfo info("TopTankPressure", Config::Sensors::ADC_SAMPLE_PERIOD, + SensorInfo info("TopTankPressure", + Config::Sensors::ADS131M08::SAMPLE_PERIOD, [this]() { topTankPressureCallback(); }); map.emplace(std::make_pair(topTankPressure.get(), info)); } @@ -428,13 +426,16 @@ void Sensors::bottomTankPressureInit(Boardcore::SensorManager::SensorMap_t &map) [this]() { auto sample = adc1->getLastSample(); - return sample.getVoltage(Config::Sensors::ADC1_BOTTOM_PT_CHANNEL); + return sample.getVoltage( + Config::Sensors::ADS131M08::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); + Config::Sensors::Trafag::TANK_BOTTOM_SHUNT_RESISTANCE, + Config::Sensors::Trafag::TANK_BOTTOM_MAX_PRESSURE, + Config::Sensors::Trafag::MIN_CURRENT, + Config::Sensors::Trafag::MAX_CURRENT); - SensorInfo info("BottomTankPressure", Config::Sensors::ADC_SAMPLE_PERIOD, + SensorInfo info("BottomTankPressure", + Config::Sensors::ADS131M08::SAMPLE_PERIOD, [this]() { bottomTankPressureCallback(); }); map.emplace(std::make_pair(bottomTankPressure.get(), info)); } @@ -452,22 +453,20 @@ void Sensors::vesselWeightInit(Boardcore::SensorManager::SensorMap_t &map) [this]() { auto sample = adc1->getLastSample(); - auto voltage = - sample.getVoltage(Config::Sensors::ADC1_VESSEL_LC_CHANNEL); - voltage.voltage -= vesselLcOffset; - - return voltage; + return sample.getVoltage( + Config::Sensors::ADS131M08::VESSEL_LC_CHANNEL); }, - Config::Sensors::LC_VESSEL_SCALE); + Config::Sensors::LoadCell::VESSEL_SCALE); - SensorInfo info("VesselWeight", Config::Sensors::ADC_SAMPLE_PERIOD, + SensorInfo info("VesselWeight", Config::Sensors::ADS131M08::SAMPLE_PERIOD, [this]() { vesselWeightCallback(); }); map.emplace(std::make_pair(vesselWeight.get(), info)); } void Sensors::vesselWeightCallback() { - LoadCellData sample = vesselWeight->getLastSample(); + // Log CALIBRATED value + LoadCellData sample = getVesselWeight(); LCsData data{sample.loadTimestamp, 1, sample.load}; sdLogger.log(data); } @@ -478,22 +477,20 @@ void Sensors::tankWeightInit(Boardcore::SensorManager::SensorMap_t &map) [this]() { auto sample = adc1->getLastSample(); - auto voltage = - sample.getVoltage(Config::Sensors::ADC1_TANK_LC_CHANNEL); - voltage.voltage -= tankLcOffset; - - return voltage; + return sample.getVoltage( + Config::Sensors::ADS131M08::TANK_LC_CHANNEL); }, - Config::Sensors::LC_TANK_SCALE); + Config::Sensors::LoadCell::TANK_SCALE); - SensorInfo info("TankWeight", Config::Sensors::ADC_SAMPLE_PERIOD, + SensorInfo info("TankWeight", Config::Sensors::ADS131M08::SAMPLE_PERIOD, [this]() { tankWeightCallback(); }); map.emplace(std::make_pair(tankWeight.get(), info)); } void Sensors::tankWeightCallback() { - LoadCellData sample = tankWeight->getLastSample(); + // Log CALIBRATED value + LoadCellData sample = getTankWeight(); LCsData data{sample.loadTimestamp, 2, sample.load}; sdLogger.log(data); } \ No newline at end of file diff --git a/src/boards/RIGv2/StateMachines/GroundModeManager/GroundModeManager.cpp b/src/boards/RIGv2/StateMachines/GroundModeManager/GroundModeManager.cpp index c876bb0d56ce2540f94e5b43b8f6f20c5ef73b13..a7a5d751b47708b70c487fd66940bfbd04c49e93 100644 --- a/src/boards/RIGv2/StateMachines/GroundModeManager/GroundModeManager.cpp +++ b/src/boards/RIGv2/StateMachines/GroundModeManager/GroundModeManager.cpp @@ -23,6 +23,7 @@ #include "GroundModeManager.h" #include <RIGv2/Actuators/Actuators.h> +#include <RIGv2/Configs/SchedulerConfig.h> #include <RIGv2/Sensors/Sensors.h> #include <common/Events.h> #include <events/EventBroker.h> @@ -35,15 +36,9 @@ using namespace miosix; using namespace Common; using namespace RIGv2; -void armLightOn() { relays::armLight::low(); } - -void armLightOff() { relays::armLight::high(); } - -void igniterOn() { relays::ignition::low(); } - -void igniterOff() { relays::ignition::high(); } - -GroundModeManager::GroundModeManager() : FSM(&GroundModeManager::state_idle) +GroundModeManager::GroundModeManager() + : FSM(&GroundModeManager::state_idle, miosix::STACK_DEFAULT_FOR_PTHREAD, + GMM_PRIORITY) { EventBroker::getInstance().subscribe(this, TOPIC_MOTOR); } @@ -124,7 +119,7 @@ void GroundModeManager::state_disarmed(const Boardcore::Event &event) { case EV_ENTRY: { - armLightOff(); + modules.get<Actuators>()->armLightOff(); logStatus(GMM_STATE_DISARMED); break; } @@ -158,7 +153,7 @@ void GroundModeManager::state_armed(const Boardcore::Event &event) { case EV_ENTRY: { - armLightOn(); + modules.get<Actuators>()->armLightOn(); modules.get<Actuators>()->closeAllServos(); logStatus(GMM_STATE_ARMED); break; @@ -186,7 +181,7 @@ void GroundModeManager::state_igniting(const Boardcore::Event &event) case EV_ENTRY: { // Start ignition - igniterOn(); + modules.get<Actuators>()->igniterOn(); // Send event to open the oxidant openOxidantDelayEventId = EventBroker::getInstance().postDelayed( @@ -199,7 +194,7 @@ void GroundModeManager::state_igniting(const Boardcore::Event &event) case MOTOR_OPEN_OXIDANT: { // Stop ignition - igniterOff(); + modules.get<Actuators>()->igniterOff(); modules.get<Actuators>()->openServo(ServosList::MAIN_VALVE); break; } @@ -208,7 +203,7 @@ void GroundModeManager::state_igniting(const Boardcore::Event &event) case MOTOR_CLOSE_FEED_VALVE: { // Stop ignition - igniterOff(); + modules.get<Actuators>()->igniterOff(); // Close all of the valves modules.get<Actuators>()->closeAllServos(); diff --git a/src/boards/RIGv2/StateMachines/TARS1/TARS1.cpp b/src/boards/RIGv2/StateMachines/TARS1/TARS1.cpp index 5ef495e690e49e832849fd6c6fbcd3357f4ee5da..5a36bc527135392b19eb4541a38e61b4169dfdb6 100644 --- a/src/boards/RIGv2/StateMachines/TARS1/TARS1.cpp +++ b/src/boards/RIGv2/StateMachines/TARS1/TARS1.cpp @@ -23,6 +23,7 @@ #include "TARS1.h" #include <RIGv2/Actuators/Actuators.h> +#include <RIGv2/Configs/SchedulerConfig.h> #include <RIGv2/Sensors/Sensors.h> #include <common/Events.h> #include <events/EventBroker.h> @@ -35,7 +36,9 @@ using namespace Common; using namespace miosix; TARS1::TARS1(TaskScheduler& scheduler) - : FSM(&TARS1::state_ready), scheduler{scheduler} + : FSM(&TARS1::state_ready, miosix::STACK_DEFAULT_FOR_PTHREAD, + TARS1_PRIORITY), + scheduler{scheduler} { EventBroker::getInstance().subscribe(this, TOPIC_TARS); EventBroker::getInstance().subscribe(this, TOPIC_MOTOR); diff --git a/src/entrypoints/RIGv2/rig-v2-entry.cpp b/src/entrypoints/RIGv2/rig-v2-entry.cpp index 52d77721deefe641f288eb1dc4b4f844cdaa97d6..342d7853ab18c7711739f00f8fae3d70f3cc9c78 100644 --- a/src/entrypoints/RIGv2/rig-v2-entry.cpp +++ b/src/entrypoints/RIGv2/rig-v2-entry.cpp @@ -21,6 +21,7 @@ */ #include <RIGv2/Actuators/Actuators.h> +#include <RIGv2/BoardScheduler.h> #include <RIGv2/Buses.h> #include <RIGv2/Radio/Radio.h> #include <RIGv2/Sensors/Sensors.h> @@ -45,15 +46,13 @@ int main() PrintLogger logger = Logging::getLogger("main"); ModuleManager &modules = ModuleManager::getInstance(); - // TODO: Move this to a dedicated board scheduler - TaskScheduler *scheduler1 = new TaskScheduler(2); - TaskScheduler *scheduler2 = new TaskScheduler(3); + Buses *buses = new Buses(); + BoardScheduler *scheduler = new BoardScheduler(); - Buses *buses = new Buses(); - Sensors *sensors = new Sensors(*scheduler1); - Actuators *actuators = new Actuators(*scheduler2); + Sensors *sensors = new Sensors(scheduler->getSensorsScheduler()); + Actuators *actuators = new Actuators(scheduler->getActuatorsScheduler()); GroundModeManager *gmm = new GroundModeManager(); - TARS1 *tars1 = new TARS1(*scheduler2); + TARS1 *tars1 = new TARS1(scheduler->getTars1Scheduler()); Radio *radio = new Radio(); Logger &sdLogger = Logger::getInstance(); @@ -68,44 +67,13 @@ int main() sdLogger.log(data); }); - bool initResult = true; - // Insert modules - if (!modules.insert<Buses>(buses)) - { - initResult = false; - LOG_ERR(logger, "Error failed to insert Buses"); - } - - if (!modules.insert<Actuators>(actuators)) - { - initResult = false; - LOG_ERR(logger, "Error failed to insert Actuators"); - } - - if (!modules.insert<Sensors>(sensors)) - { - initResult = false; - LOG_ERR(logger, "Error failed to insert Sensors"); - } - - if (!modules.insert<Radio>(radio)) - { - initResult = false; - LOG_ERR(logger, "Error failed to insert Radio"); - } - - if (!modules.insert<GroundModeManager>(gmm)) - { - initResult = false; - LOG_ERR(logger, "Error failed to insert GroundModeManager"); - } - - if (!modules.insert<TARS1>(tars1)) - { - initResult = false; - LOG_ERR(logger, "Error failed to insert TARS1"); - } + bool initResult = + modules.insert<Buses>(buses) && + modules.insert<BoardScheduler>(scheduler) && + modules.insert<Actuators>(actuators) && + modules.insert<Sensors>(sensors) && modules.insert<Radio>(radio) && + modules.insert<GroundModeManager>(gmm) && modules.insert<TARS1>(tars1); // Start modules if (!sdLogger.testSDCard()) @@ -150,21 +118,21 @@ int main() LOG_ERR(logger, "Error failed to start TARS1 module"); } - if (!scheduler1->start() || !scheduler2->start()) + if (!scheduler->start()) { initResult = false; LOG_ERR(logger, "Error failed to start scheduler"); } - if (initResult) + if (!initResult) { - broker.post(FMM_INIT_OK, TOPIC_MOTOR); - LOG_INFO(logger, "All good!"); + broker.post(FMM_INIT_ERROR, TOPIC_MOTOR); + LOG_ERR(logger, "Init failure!"); } else { - broker.post(FMM_INIT_ERROR, TOPIC_MOTOR); - LOG_ERR(logger, "Init failure!"); + broker.post(FMM_INIT_OK, TOPIC_MOTOR); + LOG_INFO(logger, "All good!"); } // Periodic statistics