diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json index f271484838c73b30512fd12e39515d079062ba97..8904aa9867061dead40b723fc7546c552407fbe6 100755 --- a/.vscode/c_cpp_properties.json +++ b/.vscode/c_cpp_properties.json @@ -487,6 +487,28 @@ "${workspaceFolder}/skyward-boardcore/src/bsps/stm32f767zi_orion_biscotto" ] }, + { + "name": "stm32f767zi_orion_engine", + "cStandard": "c11", + "cppStandard": "c++14", + "compilerPath": "/opt/arm-miosix-eabi/bin/arm-miosix-eabi-g++", + "defines": [ + "${defaultDefines}", + "_MIOSIX_BOARDNAME=stm32f767zi_orion_engine", + "_BOARD_STM32F767ZI_ORION_ENGINE", + "_ARCH_CORTEXM7_STM32F7", + "HSE_VALUE=25000000", + "SYSCLK_FREQ_216MHz=216000000", + "__ENABLE_XRAM", + "V_DDA_VOLTAGE=3.3f" + ], + "includePath": [ + "${defaultIncludePaths}", + "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM7_stm32f7/common", + "${workspaceFolder}/skyward-boardcore/src/bsps/stm32f767zi_orion_engine/config", + "${workspaceFolder}/skyward-boardcore/src/bsps/stm32f767zi_orion_engine" + ] + }, { "name": "logdecoder", "includePath": [ diff --git a/CMakeLists.txt b/CMakeLists.txt index 670a5267d89485f34359194a6a46640f52f24ff7..5a32b3505ae020b3e820d04a9c18dfa93237d08d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -71,7 +71,7 @@ sbs_target(payload-entry-euroc stm32f767zi_lyra_biscotto) add_executable(motor-entry src/Motor/motor-entry.cpp ${MOTOR_SOURCES}) target_include_directories(motor-entry PRIVATE ${OBSW_INCLUDE_DIRS}) -sbs_target(motor-entry stm32f767zi_lyra_motor) +sbs_target(motor-entry stm32f767zi_orion_engine) add_executable(rig-v2-entry src/RIGv2/rig-v2-entry.cpp ${RIG_V2_COMPUTER}) target_include_directories(rig-v2-entry PRIVATE ${OBSW_INCLUDE_DIRS}) diff --git a/scripts/logdecoder/logdecoder.cpp b/scripts/logdecoder/logdecoder.cpp index d6ca802b9ccb2f320ef4f80df17dc5ef3d1dc313..d59f9889d93f1b4501f80691bebff0421410c996 100644 --- a/scripts/logdecoder/logdecoder.cpp +++ b/scripts/logdecoder/logdecoder.cpp @@ -96,8 +96,9 @@ void registerTypes(Deserializer& ds) ds.registerType<Main::CalibrationData>(); // Motor - ds.registerType<Motor::TopTankPressureData>(); - ds.registerType<Motor::BottomTankPressureData>(); + ds.registerType<Motor::OxTopTankPressureData>(); + ds.registerType<Motor::OxBottomTankPressureData>(); + ds.registerType<Motor::N2TankPressureData>(); ds.registerType<Motor::CCPressureData>(); ds.registerType<Motor::ActuatorsData>(); diff --git a/src/Motor/Actuators/Actuators.cpp b/src/Motor/Actuators/Actuators.cpp index 8582d035f283338c4c430993bcfff76c04cfc1a1..70878c24976685e4c388bd4a751506363a3d7dd5 100644 --- a/src/Motor/Actuators/Actuators.cpp +++ b/src/Motor/Actuators/Actuators.cpp @@ -74,13 +74,21 @@ float Actuators::ServoInfo::getServoPosition() Actuators::Actuators() { infos[0].servo = std::make_unique<Servo>( - MIOSIX_SERVOS_1_TIM, TimerUtils::Channel::MIOSIX_SERVOS_1_CHANNEL, + MIOSIX_SERVOS_0_TIM, TimerUtils::Channel::MIOSIX_SERVOS_0_CHANNEL, Config::Servos::MIN_PULSE, Config::Servos::MAX_PULSE, Config::Servos::FREQUENCY); infos[1].servo = std::make_unique<Servo>( + MIOSIX_SERVOS_1_TIM, TimerUtils::Channel::MIOSIX_SERVOS_1_CHANNEL, + Config::Servos::MIN_PULSE, Config::Servos::MAX_PULSE, + Config::Servos::FREQUENCY); + infos[2].servo = std::make_unique<Servo>( MIOSIX_SERVOS_2_TIM, TimerUtils::Channel::MIOSIX_SERVOS_2_CHANNEL, Config::Servos::MIN_PULSE, Config::Servos::MAX_PULSE, Config::Servos::FREQUENCY); + infos[3].servo = std::make_unique<Servo>( + MIOSIX_SERVOS_3_TIM, TimerUtils::Channel::MIOSIX_SERVOS_3_CHANNEL, + Config::Servos::MIN_PULSE, Config::Servos::MAX_PULSE, + Config::Servos::FREQUENCY); ServoInfo* info; info = getServo(ServosList::MAIN_VALVE); @@ -88,9 +96,19 @@ Actuators::Actuators() info->flipped = Config::Servos::MAIN_FLIPPED; info->unsafeSetServoPosition(0.0f); - info = getServo(ServosList::VENTING_VALVE); - info->limit = Config::Servos::VENTING_LIMIT; - info->flipped = Config::Servos::VENTING_FLIPPED; + info = getServo(ServosList::OX_VENTING_VALVE); + info->limit = Config::Servos::OX_VENTING_LIMIT; + info->flipped = Config::Servos::OX_VENTING_FLIPPED; + info->unsafeSetServoPosition(0.0f); + + info = getServo(ServosList::NITROGEN_VALVE); + info->limit = Config::Servos::NITROGEN_LIMIT; + info->flipped = Config::Servos::NITROGEN_FLIPPED; + info->unsafeSetServoPosition(0.0f); + + info = getServo(ServosList::N2_QUENCHING_VALVE); + info->limit = Config::Servos::N2_QUENCHING_LIMIT; + info->flipped = Config::Servos::N2_QUENCHING_FLIPPED; info->unsafeSetServoPosition(0.0f); } @@ -99,8 +117,8 @@ bool Actuators::start() TaskScheduler& scheduler = getModule<BoardScheduler>()->getActuatorsScheduler(); - infos[0].servo->enable(); - infos[1].servo->enable(); + for (size_t i = 0; i < infos.size(); ++i) + infos[i].servo->enable(); // Reset all actions lastActionTs = getTime(); @@ -165,10 +183,14 @@ Actuators::ServoInfo* Actuators::getServo(ServosList servo) { switch (servo) { - case VENTING_VALVE: + case OX_VENTING_VALVE: return &infos[0]; case MAIN_VALVE: return &infos[1]; + case NITROGEN_VALVE: + return &infos[2]; + case N2_QUENCHING_VALVE: + return &infos[3]; default: // Oh FUCK @@ -251,7 +273,7 @@ void Actuators::updatePositionsTask() if (shouldVent) { // Open for at least timeout time - openServoWithTime(ServosList::VENTING_VALVE, + openServoWithTime(ServosList::OX_VENTING_VALVE, Config::Servos::SERVO_ACTION_TIMEOUT + 1000); } } diff --git a/src/Motor/Actuators/Actuators.h b/src/Motor/Actuators/Actuators.h index 15133ade166bccf168662904efbb00b6062f760a..2cfad5c921bf9463ecf89d180c80455113ba9ed5 100644 --- a/src/Motor/Actuators/Actuators.h +++ b/src/Motor/Actuators/Actuators.h @@ -25,7 +25,7 @@ #include <Motor/BoardScheduler.h> #include <Motor/CanHandler/CanHandler.h> #include <actuators/Servo/Servo.h> -#include <common/MavlinkLyra.h> +#include <common/MavlinkOrion.h> #include <utils/DependencyManager/DependencyManager.h> namespace Motor @@ -76,7 +76,7 @@ private: miosix::FastMutex infosMutex; // Timestamp of last servo action long long lastActionTs = 0; - ServoInfo infos[2] = {}; + std::array<ServoInfo, 4> infos; }; } // namespace Motor diff --git a/src/Motor/Buses.h b/src/Motor/Buses.h index 62a25fde5d8e258c69e59a8467c9da5ea6bc708d..2f0ab859049d4dbf2363085795263abb1037c103 100644 --- a/src/Motor/Buses.h +++ b/src/Motor/Buses.h @@ -39,6 +39,7 @@ public: Boardcore::SPIBus& getLIS2MDL() { return spi3; } Boardcore::SPIBus& getLSM6DSRX() { return spi3; } Boardcore::SPIBus& getADS131M08() { return spi4; } + Boardcore::SPIBus& getThermocouple() { return spi1; } Boardcore::USART& getHILUart() { return usart4; } diff --git a/src/Motor/CanHandler/CanHandler.cpp b/src/Motor/CanHandler/CanHandler.cpp index a110d856d9fba927476b02dc9d4f289f422efc22..8a26ffb3d79f5c75cc1ec7af67c3b6cbcee8b97d 100644 --- a/src/Motor/CanHandler/CanHandler.cpp +++ b/src/Motor/CanHandler/CanHandler.cpp @@ -100,18 +100,37 @@ bool CanHandler::start() static_cast<uint8_t>(CanConfig::PrimaryType::SENSORS), static_cast<uint8_t>(CanConfig::Board::MOTOR), static_cast<uint8_t>(CanConfig::Board::BROADCAST), - static_cast<uint8_t>(CanConfig::SensorId::TOP_TANK_PRESSURE), + static_cast<uint8_t>(CanConfig::SensorId::OX_TOP_TANK_PRESSURE), static_cast<PressureData>( - sensors->getTopTankPressLastSample())); + sensors->getOxTopTankPressLastSample())); protocol.enqueueData( static_cast<uint8_t>(CanConfig::Priority::HIGH), static_cast<uint8_t>(CanConfig::PrimaryType::SENSORS), static_cast<uint8_t>(CanConfig::Board::MOTOR), static_cast<uint8_t>(CanConfig::Board::BROADCAST), - static_cast<uint8_t>(CanConfig::SensorId::BOTTOM_TANK_PRESSURE), + static_cast<uint8_t>( + CanConfig::SensorId::OX_BOTTOM_TANK_PRESSURE_0), static_cast<PressureData>( - sensors->getBottomTankPressLastSample())); + sensors->getOxBottomTankPress0LastSample())); + + protocol.enqueueData( + static_cast<uint8_t>(CanConfig::Priority::HIGH), + static_cast<uint8_t>(CanConfig::PrimaryType::SENSORS), + static_cast<uint8_t>(CanConfig::Board::MOTOR), + static_cast<uint8_t>(CanConfig::Board::BROADCAST), + static_cast<uint8_t>( + CanConfig::SensorId::OX_BOTTOM_TANK_PRESSURE_1), + static_cast<PressureData>( + sensors->getOxBottomTankPress1LastSample())); + + protocol.enqueueData( + static_cast<uint8_t>(CanConfig::Priority::HIGH), + static_cast<uint8_t>(CanConfig::PrimaryType::SENSORS), + static_cast<uint8_t>(CanConfig::Board::MOTOR), + static_cast<uint8_t>(CanConfig::Board::BROADCAST), + static_cast<uint8_t>(CanConfig::SensorId::N2_TANK_PRESSURE), + static_cast<PressureData>(sensors->getN2TankPressLastSample())); }, Config::CanHandler::PRESSURE_PERIOD); @@ -172,11 +191,33 @@ bool CanHandler::start() static_cast<uint8_t>(CanConfig::PrimaryType::ACTUATORS), static_cast<uint8_t>(CanConfig::Board::MOTOR), static_cast<uint8_t>(CanConfig::Board::BROADCAST), - static_cast<uint8_t>(ServosList::VENTING_VALVE), + static_cast<uint8_t>(ServosList::OX_VENTING_VALVE), + ServoFeedback{ + TimestampTimer::getTimestamp(), + actuators->getServoPosition(ServosList::OX_VENTING_VALVE), + actuators->isServoOpen(ServosList::OX_VENTING_VALVE)}); + + protocol.enqueueData( + static_cast<uint8_t>(CanConfig::Priority::HIGH), + static_cast<uint8_t>(CanConfig::PrimaryType::ACTUATORS), + static_cast<uint8_t>(CanConfig::Board::MOTOR), + static_cast<uint8_t>(CanConfig::Board::BROADCAST), + static_cast<uint8_t>(ServosList::NITROGEN_VALVE), + ServoFeedback{ + TimestampTimer::getTimestamp(), + actuators->getServoPosition(ServosList::NITROGEN_VALVE), + actuators->isServoOpen(ServosList::NITROGEN_VALVE)}); + + protocol.enqueueData( + static_cast<uint8_t>(CanConfig::Priority::HIGH), + static_cast<uint8_t>(CanConfig::PrimaryType::ACTUATORS), + static_cast<uint8_t>(CanConfig::Board::MOTOR), + static_cast<uint8_t>(CanConfig::Board::BROADCAST), + static_cast<uint8_t>(ServosList::N2_QUENCHING_VALVE), ServoFeedback{ TimestampTimer::getTimestamp(), - actuators->getServoPosition(ServosList::VENTING_VALVE), - actuators->isServoOpen(ServosList::VENTING_VALVE)}); + actuators->getServoPosition(ServosList::N2_QUENCHING_VALVE), + actuators->isServoOpen(ServosList::N2_QUENCHING_VALVE)}); }, Config::CanHandler::ACTUATORS_PERIOD); diff --git a/src/Motor/Configs/ActuatorsConfig.h b/src/Motor/Configs/ActuatorsConfig.h index 36a876cd7fef7e0a3bfd45096f3f3342319a9b0e..820b74693299491826e295a21ee2adc1897f47c2 100644 --- a/src/Motor/Configs/ActuatorsConfig.h +++ b/src/Motor/Configs/ActuatorsConfig.h @@ -49,11 +49,17 @@ constexpr float SERVO_CONFIDENCE = 0.02; // 2% // Timeout is currently set at 2h constexpr long long SERVO_ACTION_TIMEOUT = 2 * 60 * 60 * 1000; -constexpr float VENTING_LIMIT = 0.90f; -constexpr float MAIN_LIMIT = 0.90f; +// TODO: check this values +constexpr float OX_VENTING_LIMIT = 0.90f; +constexpr float MAIN_LIMIT = 0.90f; +constexpr float NITROGEN_LIMIT = 0.90f; +constexpr float N2_QUENCHING_LIMIT = 0.90f; -constexpr bool VENTING_FLIPPED = true; -constexpr bool MAIN_FLIPPED = true; +// TODO: check this values +constexpr bool OX_VENTING_FLIPPED = true; +constexpr bool MAIN_FLIPPED = true; +constexpr bool NITROGEN_FLIPPED = true; +constexpr bool N2_QUENCHING_FLIPPED = true; } // namespace Servos diff --git a/src/Motor/Configs/SensorsConfig.h b/src/Motor/Configs/SensorsConfig.h index f3d039dcc40e288d1404ea345e621379e73ab35c..a2adb90bd2318cf24c88e50ab196dd952f600bbe 100644 --- a/src/Motor/Configs/SensorsConfig.h +++ b/src/Motor/Configs/SensorsConfig.h @@ -111,16 +111,26 @@ Channel 4 - 29.65ohm 0.3584V 12.06mA 29.72ohm 0.5911V 20mA 29.55ohm */ + +// TODO: calibrate ch0 and ch1 +constexpr float CH0_SHUNT_RESISTANCE = 29.0; +constexpr float CH1_SHUNT_RESISTANCE = 29.0; + constexpr float CH4_SHUNT_RESISTANCE = 29.79; constexpr float CH5_SHUNT_RESISTANCE = 29.7; constexpr float CH6_SHUNT_RESISTANCE = 29.65; +// TODO: check if the selected channels are fine constexpr Boardcore::ADS131M08Defs::Channel TANK_TC_CHANNEL = Boardcore::ADS131M08Defs::Channel::CHANNEL_3; -constexpr Boardcore::ADS131M08Defs::Channel TANK_TOP_PT_CHANNEL = +constexpr Boardcore::ADS131M08Defs::Channel OX_TANK_TOP_PT_CHANNEL = Boardcore::ADS131M08Defs::Channel::CHANNEL_5; -constexpr Boardcore::ADS131M08Defs::Channel TANK_BOTTOM_PT_CHANNEL = +constexpr Boardcore::ADS131M08Defs::Channel OX_TANK_BOTTOM_0_PT_CHANNEL = Boardcore::ADS131M08Defs::Channel::CHANNEL_6; +constexpr Boardcore::ADS131M08Defs::Channel OX_TANK_BOTTOM_1_PT_CHANNEL = + Boardcore::ADS131M08Defs::Channel::CHANNEL_0; +constexpr Boardcore::ADS131M08Defs::Channel N2_TANK_PT_CHANNEL = + Boardcore::ADS131M08Defs::Channel::CHANNEL_1; constexpr Boardcore::ADS131M08Defs::Channel ENGINE_PT_CHANNEL = Boardcore::ADS131M08Defs::Channel::CHANNEL_4; @@ -130,16 +140,23 @@ constexpr bool ENABLED = true; namespace Trafag { -constexpr float TANK_TOP_SHUNT_RESISTANCE = ADS131M08::CH5_SHUNT_RESISTANCE; -constexpr float TANK_BOTTOM_SHUNT_RESISTANCE = ADS131M08::CH6_SHUNT_RESISTANCE; -constexpr float ENGINE_SHUNT_RESISTANCE = ADS131M08::CH4_SHUNT_RESISTANCE; +constexpr float OX_TANK_TOP_SHUNT_RESISTANCE = ADS131M08::CH5_SHUNT_RESISTANCE; +constexpr float OX_TANK_BOTTOM_0_SHUNT_RESISTANCE = + ADS131M08::CH6_SHUNT_RESISTANCE; +constexpr float OX_TANK_BOTTOM_1_SHUNT_RESISTANCE = + ADS131M08::CH0_SHUNT_RESISTANCE; +constexpr float N2_TANK_SHUNT_RESISTANCE = ADS131M08::CH1_SHUNT_RESISTANCE; +constexpr float ENGINE_SHUNT_RESISTANCE = ADS131M08::CH4_SHUNT_RESISTANCE; constexpr float MIN_CURRENT = 4; constexpr float MAX_CURRENT = 20; -constexpr float TANK_TOP_MAX_PRESSURE = 100; // bar -constexpr float TANK_BOTTOM_MAX_PRESSURE = 100; // bar -constexpr float ENGINE_MAX_PRESSURE = 40; // bar +// TODO: check these values +constexpr float OX_TANK_TOP_MAX_PRESSURE = 100; // bar +constexpr float OX_TANK_BOTTOM_0_MAX_PRESSURE = 100; // bar +constexpr float OX_TANK_BOTTOM_1_MAX_PRESSURE = 100; // bar +constexpr float N2_TANK_MAX_PRESSURE = 100; // TODO: CHECK +constexpr float ENGINE_MAX_PRESSURE = 40; // bar } // namespace Trafag namespace Kulite @@ -156,6 +173,12 @@ constexpr float TANK_P1_TEMP = 34.0f; } // namespace Kulite +namespace MAX31856 +{ +constexpr Hertz PERIOD = 10_hz; +constexpr bool ENABLED = true; +} // namespace MAX31856 + namespace InternalADC { constexpr Boardcore::InternalADC::Channel VBAT_CH = diff --git a/src/Motor/HIL/HIL.cpp b/src/Motor/HIL/HIL.cpp index 4f5f6ded024e30901093f6d300c3f6bc811bac4e..2338864659c3d062acd596b78e9ff9adf8acc029 100644 --- a/src/Motor/HIL/HIL.cpp +++ b/src/Motor/HIL/HIL.cpp @@ -134,7 +134,10 @@ ActuatorData MotorHIL::updateActuatorData() ActuatorsStateHIL actuatorsStateHIL{ (actuators->getServoPosition(MAIN_VALVE)), - (actuators->getServoPosition(VENTING_VALVE))}; + (actuators->getServoPosition(OX_VENTING_VALVE)), + (actuators->getServoPosition(NITROGEN_VALVE)), + (actuators->getServoPosition(N2_QUENCHING_VALVE)), + }; // Returning the feedback for the simulator return ActuatorData{actuatorsStateHIL}; diff --git a/src/Motor/HIL/HILData.h b/src/Motor/HIL/HILData.h index 14051e4528417bad2beada7bcfcb63fd08ad21a9..2728875d16fc8f7ba74e6ed8929dbfe090761f3b 100644 --- a/src/Motor/HIL/HILData.h +++ b/src/Motor/HIL/HILData.h @@ -46,17 +46,24 @@ enum class MotorFlightPhases struct ActuatorsStateHIL { - float mainValvePercentage = 0; - float ventingValvePercentage = 0; + float mainValvePercentage = 0; + float oxVentingValvePercentage = 0; + float nitrogenValvePercentage = 0; + float n2QuenchingValvePercentage = 0; ActuatorsStateHIL() - : mainValvePercentage(0.0f), ventingValvePercentage(0.0f) + : mainValvePercentage(0.0f), oxVentingValvePercentage(0.0f), + nitrogenValvePercentage(0.0f), n2QuenchingValvePercentage(0.0f) { } - ActuatorsStateHIL(float mainValvePercentage, float ventingValvePercentage) + ActuatorsStateHIL(float mainValvePercentage, float oxVentingValvePercentage, + float nitrogenValvePercentage, + float n2QuenchingValvePercentage) : mainValvePercentage(mainValvePercentage), - ventingValvePercentage(ventingValvePercentage) + oxVentingValvePercentage(oxVentingValvePercentage), + nitrogenValvePercentage(nitrogenValvePercentage), + n2QuenchingValvePercentage(n2QuenchingValvePercentage) { } @@ -64,8 +71,11 @@ struct ActuatorsStateHIL { printf( "mainValve: %f perc\n" - "venting: %f perc\n", - mainValvePercentage * 100, ventingValvePercentage * 100); + "oxVenting: %f perc\n" + "nitrogen: %f perc\n" + "n2Quenching: %f perc\n", + mainValvePercentage * 100, oxVentingValvePercentage * 100, + nitrogenValvePercentage * 100, n2QuenchingValvePercentage * 100); } }; diff --git a/src/Motor/Sensors/Sensors.cpp b/src/Motor/Sensors/Sensors.cpp index c930178caefaa881ba8822aeb866d65913b0601e..a25fb3da8d19330f97ee52ba338c7c93c81daf29 100644 --- a/src/Motor/Sensors/Sensors.cpp +++ b/src/Motor/Sensors/Sensors.cpp @@ -48,12 +48,16 @@ bool Sensors::start() if (Config::Sensors::ADS131M08::ENABLED) { ads131m08Init(); - topTankPressureInit(); - bottomTankPressureInit(); + oxTopTankPressureInit(); + oxBottomTankPressureInit(); + n2TankPressureInit(); ccPressureInit(); tankTempInit(); } + if (Config::Sensors::MAX31856::ENABLED) + thermocoupleInit(); + if (Config::Sensors::InternalADC::ENABLED) internalAdcInit(); @@ -97,20 +101,37 @@ LIS2MDLData Sensors::getLIS2MDLLastSample() return lis2mdl ? lis2mdl->getLastSample() : LIS2MDLData{}; } -LSM6DSRXData Sensors::getLSM6DSRXLastSample() +LSM6DSRXData Sensors::getLSM6DSRX0LastSample() +{ + return lsm6dsrx0 ? lsm6dsrx0->getLastSample() : LSM6DSRXData{}; +} + +LSM6DSRXData Sensors::getLSM6DSRX1LastSample() { - return lsm6dsrx ? lsm6dsrx->getLastSample() : LSM6DSRXData{}; + return lsm6dsrx1 ? lsm6dsrx1->getLastSample() : LSM6DSRXData{}; } -PressureData Sensors::getTopTankPressLastSample() +PressureData Sensors::getOxTopTankPressLastSample() { - return topTankPressure ? topTankPressure->getLastSample() : PressureData{}; + return oxTopTankPressure ? oxTopTankPressure->getLastSample() + : PressureData{}; } -PressureData Sensors::getBottomTankPressLastSample() +PressureData Sensors::getOxBottomTankPress0LastSample() { - return bottomTankPressure ? bottomTankPressure->getLastSample() - : PressureData{}; + return oxBottomTankPressure0 ? oxBottomTankPressure0->getLastSample() + : PressureData{}; +} + +PressureData Sensors::getOxBottomTankPress1LastSample() +{ + return oxBottomTankPressure1 ? oxBottomTankPressure1->getLastSample() + : PressureData{}; +} + +PressureData Sensors::getN2TankPressLastSample() +{ + return n2TankPressure ? n2TankPressure->getLastSample() : PressureData{}; } PressureData Sensors::getCCPressLastSample() @@ -123,6 +144,11 @@ TemperatureData Sensors::getTankTempLastSample() return tankTemp ? tankTemp->getLastSample() : TemperatureData{}; } +TemperatureData Sensors::getThermocoupleLastSample() +{ + return thermocouple ? thermocouple->getLastSample() : TemperatureData{}; +} + VoltageData Sensors::getBatteryVoltageLastSample() { auto sample = getInternalADCLastSample(); @@ -146,8 +172,11 @@ std::vector<SensorInfo> Sensors::getSensorInfos() if (lis2mdl) infos.push_back(manager->getSensorInfo(lis2mdl.get())); - if (lsm6dsrx) - infos.push_back(manager->getSensorInfo(lsm6dsrx.get())); + if (lsm6dsrx0) + infos.push_back(manager->getSensorInfo(lsm6dsrx0.get())); + + if (lsm6dsrx1) + infos.push_back(manager->getSensorInfo(lsm6dsrx1.get())); if (ads131m08) infos.push_back(manager->getSensorInfo(ads131m08.get())); @@ -155,11 +184,19 @@ std::vector<SensorInfo> Sensors::getSensorInfos() if (internalAdc) infos.push_back(manager->getSensorInfo(internalAdc.get())); - if (topTankPressure) - infos.push_back(manager->getSensorInfo(topTankPressure.get())); + if (oxTopTankPressure) + infos.push_back(manager->getSensorInfo(oxTopTankPressure.get())); + + if (oxBottomTankPressure0) + infos.push_back( + manager->getSensorInfo(oxBottomTankPressure0.get())); + + if (oxBottomTankPressure1) + infos.push_back( + manager->getSensorInfo(oxBottomTankPressure1.get())); - if (bottomTankPressure) - infos.push_back(manager->getSensorInfo(bottomTankPressure.get())); + if (n2TankPressure) + infos.push_back(manager->getSensorInfo(n2TankPressure.get())); if (ccPressure) infos.push_back(manager->getSensorInfo(ccPressure.get())); @@ -167,6 +204,9 @@ std::vector<SensorInfo> Sensors::getSensorInfos() if (tankTemp) infos.push_back(manager->getSensorInfo(tankTemp.get())); + if (thermocouple) + infos.push_back(manager->getSensorInfo(thermocouple.get())); + return infos; } else @@ -249,18 +289,34 @@ void Sensors::lsm6dsrxInit() LSM6DSRXConfig::FIFO_TIMESTAMP_DECIMATION::DEC_1; config.fifoTemperatureBdr = LSM6DSRXConfig::FIFO_TEMPERATURE_BDR::DISABLED; - lsm6dsrx = std::make_unique<LSM6DSRX>(getModule<Buses>()->getLSM6DSRX(), - sensors::LSM6DSRX::cs::getPin(), - spiConfig, config); + lsm6dsrx0 = std::make_unique<LSM6DSRX>(getModule<Buses>()->getLSM6DSRX(), + sensors::LSM6DSRX0::cs::getPin(), + spiConfig, config); + lsm6dsrx1 = std::make_unique<LSM6DSRX>(getModule<Buses>()->getLSM6DSRX(), + sensors::LSM6DSRX1::cs::getPin(), + spiConfig, config); +} + +void Sensors::lsm6dsrx0Callback() +{ + if (!lsm6dsrx0) + return; + + uint16_t lastFifoSize; + const auto lastFifo = lsm6dsrx0->getLastFifo(lastFifoSize); + + // For every instance inside the fifo log the sample + for (uint16_t i = 0; i < lastFifoSize; i++) + sdLogger.log(lastFifo.at(i)); } -void Sensors::lsm6dsrxCallback() +void Sensors::lsm6dsrx1Callback() { - if (!lsm6dsrx) + if (!lsm6dsrx1) return; uint16_t lastFifoSize; - const auto lastFifo = lsm6dsrx->getLastFifo(lastFifoSize); + const auto lastFifo = lsm6dsrx1->getLastFifo(lastFifoSize); // For every instance inside the fifo log the sample for (uint16_t i = 0; i < lastFifoSize; i++) @@ -290,19 +346,32 @@ void Sensors::ads131m08Init() // Configure all required channels config.channelsConfig[( - int)Config::Sensors::ADS131M08::TANK_TOP_PT_CHANNEL] = { + int)Config::Sensors::ADS131M08::OX_TANK_TOP_PT_CHANNEL] = { .enabled = true, .pga = ADS131M08Defs::PGA::PGA_1, .offset = 0, .gain = 1.0}; config.channelsConfig[( - int)Config::Sensors::ADS131M08::TANK_BOTTOM_PT_CHANNEL] = { + int)Config::Sensors::ADS131M08::OX_TANK_BOTTOM_0_PT_CHANNEL] = { .enabled = true, .pga = ADS131M08Defs::PGA::PGA_1, .offset = 0, .gain = 1.0}; + config.channelsConfig[( + int)Config::Sensors::ADS131M08::OX_TANK_BOTTOM_1_PT_CHANNEL] = { + .enabled = true, + .pga = ADS131M08Defs::PGA::PGA_1, + .offset = 0, + .gain = 1.0}; + + config.channelsConfig[(int)Config::Sensors::ADS131M08::N2_TANK_PT_CHANNEL] = + {.enabled = true, + .pga = ADS131M08Defs::PGA::PGA_1, + .offset = 0, + .gain = 1.0}; + config.channelsConfig[(int)Config::Sensors::ADS131M08::ENGINE_PT_CHANNEL] = {.enabled = true, .pga = ADS131M08Defs::PGA::PGA_1, @@ -335,44 +404,81 @@ void Sensors::internalAdcCallback() sdLogger.log(getInternalADCLastSample()); } -void Sensors::topTankPressureInit() +void Sensors::oxTopTankPressureInit() +{ + oxTopTankPressure = std::make_unique<TrafagPressureSensor>( + [this]() + { + auto sample = getADS131M08LastSample(); + return sample.getVoltage( + Config::Sensors::ADS131M08::OX_TANK_TOP_PT_CHANNEL); + }, + Config::Sensors::Trafag::OX_TANK_TOP_SHUNT_RESISTANCE, + Config::Sensors::Trafag::OX_TANK_TOP_MAX_PRESSURE, + Config::Sensors::Trafag::MIN_CURRENT, + Config::Sensors::Trafag::MAX_CURRENT); +} + +void Sensors::oxTopTankPressureCallback() { - topTankPressure = std::make_unique<TrafagPressureSensor>( + sdLogger.log(OxTopTankPressureData{getOxTopTankPressLastSample()}); +} + +void Sensors::oxBottomTankPressureInit() +{ + oxBottomTankPressure0 = std::make_unique<TrafagPressureSensor>( [this]() { auto sample = getADS131M08LastSample(); return sample.getVoltage( - Config::Sensors::ADS131M08::TANK_TOP_PT_CHANNEL); + Config::Sensors::ADS131M08::OX_TANK_BOTTOM_0_PT_CHANNEL); }, - Config::Sensors::Trafag::TANK_TOP_SHUNT_RESISTANCE, - Config::Sensors::Trafag::TANK_TOP_MAX_PRESSURE, + Config::Sensors::Trafag::OX_TANK_BOTTOM_0_SHUNT_RESISTANCE, + Config::Sensors::Trafag::OX_TANK_BOTTOM_0_MAX_PRESSURE, + Config::Sensors::Trafag::MIN_CURRENT, + Config::Sensors::Trafag::MAX_CURRENT); + + oxBottomTankPressure1 = std::make_unique<TrafagPressureSensor>( + [this]() + { + auto sample = getADS131M08LastSample(); + return sample.getVoltage( + Config::Sensors::ADS131M08::OX_TANK_BOTTOM_1_PT_CHANNEL); + }, + Config::Sensors::Trafag::OX_TANK_BOTTOM_1_SHUNT_RESISTANCE, + Config::Sensors::Trafag::OX_TANK_BOTTOM_1_MAX_PRESSURE, Config::Sensors::Trafag::MIN_CURRENT, Config::Sensors::Trafag::MAX_CURRENT); } -void Sensors::topTankPressureCallback() +void Sensors::oxBottomTankPressure0Callback() { - sdLogger.log(TopTankPressureData{getTopTankPressLastSample()}); + sdLogger.log(OxBottomTankPressureData{getOxBottomTankPress0LastSample()}); } -void Sensors::bottomTankPressureInit() +void Sensors::oxBottomTankPressure1Callback() { - bottomTankPressure = std::make_unique<TrafagPressureSensor>( + sdLogger.log(OxBottomTankPressureData{getOxBottomTankPress1LastSample()}); +} + +void Sensors::n2TankPressureInit() +{ + n2TankPressure = std::make_unique<TrafagPressureSensor>( [this]() { auto sample = getADS131M08LastSample(); return sample.getVoltage( - Config::Sensors::ADS131M08::TANK_BOTTOM_PT_CHANNEL); + Config::Sensors::ADS131M08::N2_TANK_PT_CHANNEL); }, - Config::Sensors::Trafag::TANK_BOTTOM_SHUNT_RESISTANCE, - Config::Sensors::Trafag::TANK_BOTTOM_MAX_PRESSURE, + Config::Sensors::Trafag::N2_TANK_SHUNT_RESISTANCE, + Config::Sensors::Trafag::N2_TANK_MAX_PRESSURE, Config::Sensors::Trafag::MIN_CURRENT, Config::Sensors::Trafag::MAX_CURRENT); } -void Sensors::bottomTankPressureCallback() +void Sensors::n2TankPressureCallback() { - sdLogger.log(BottomTankPressureData{getBottomTankPressLastSample()}); + sdLogger.log(N2TankPressureData{getN2TankPressLastSample()}); } void Sensors::ccPressureInit() @@ -412,6 +518,25 @@ void Sensors::tankTempInit() void Sensors::tankTempCallback() { sdLogger.log(getTankTempLastSample()); } +void Sensors::thermocoupleInit() +{ + SPIBusConfig spiConfig = MAX31856::getDefaultSPIConfig(); + spiConfig.clockDivider = SPI::ClockDivider::DIV_32; + + thermocouple = std::make_unique<MAX31856>( + getModule<Buses>()->getThermocouple(), + sensors::thermocouple::cs::getPin(), spiConfig, + MAX31856::ThermocoupleType::K_TYPE); // TODO: verify the type +} + +void Sensors::thermocoupleCallback() +{ + if (!thermocouple) + return; + + sdLogger.log(getThermocoupleLastSample()); +} + bool Sensors::sensorManagerInit() { SensorManager::SensorMap_t map; @@ -438,11 +563,18 @@ bool Sensors::sensorManagerInit() map.emplace(lis2mdl.get(), lis2mdlInfo); } - if (lsm6dsrx) + if (lsm6dsrx0) { - SensorInfo lsm6dsrxInfo{"LSM6DSRX", Config::Sensors::LSM6DSRX::RATE, - [this]() { lsm6dsrxCallback(); }}; - map.emplace(lsm6dsrx.get(), lsm6dsrxInfo); + SensorInfo lsm6dsrxInfo{"LSM6DSRX0", Config::Sensors::LSM6DSRX::RATE, + [this]() { lsm6dsrx0Callback(); }}; + map.emplace(lsm6dsrx0.get(), lsm6dsrxInfo); + } + + if (lsm6dsrx1) + { + SensorInfo lsm6dsrxInfo{"LSM6DSRX1", Config::Sensors::LSM6DSRX::RATE, + [this]() { lsm6dsrx1Callback(); }}; + map.emplace(lsm6dsrx1.get(), lsm6dsrxInfo); } if (ads131m08) @@ -460,18 +592,34 @@ bool Sensors::sensorManagerInit() map.emplace(internalAdc.get(), internalAdcInfo); } - if (topTankPressure) + if (oxTopTankPressure) + { + SensorInfo info{"OxTopTankPressure", Config::Sensors::ADS131M08::RATE, + [this]() { oxTopTankPressureCallback(); }}; + map.emplace(std::make_pair(oxTopTankPressure.get(), info)); + } + + if (oxBottomTankPressure0) + { + SensorInfo info{"OxBottomTankPressure0", + Config::Sensors::ADS131M08::RATE, + [this]() { oxBottomTankPressure0Callback(); }}; + map.emplace(std::make_pair(oxBottomTankPressure0.get(), info)); + } + + if (oxBottomTankPressure1) { - SensorInfo info{"TopTankPressure", Config::Sensors::ADS131M08::RATE, - [this]() { topTankPressureCallback(); }}; - map.emplace(std::make_pair(topTankPressure.get(), info)); + SensorInfo info{"OxBottomTankPressure1", + Config::Sensors::ADS131M08::RATE, + [this]() { oxBottomTankPressure1Callback(); }}; + map.emplace(std::make_pair(oxBottomTankPressure1.get(), info)); } - if (bottomTankPressure) + if (n2TankPressure) { - SensorInfo info{"BottomTankPressure", Config::Sensors::ADS131M08::RATE, - [this]() { bottomTankPressureCallback(); }}; - map.emplace(std::make_pair(bottomTankPressure.get(), info)); + SensorInfo info{"N2TankPressure", Config::Sensors::ADS131M08::RATE, + [this]() { n2TankPressureCallback(); }}; + map.emplace(std::make_pair(n2TankPressure.get(), info)); } if (ccPressure) @@ -488,6 +636,13 @@ bool Sensors::sensorManagerInit() map.emplace(std::make_pair(tankTemp.get(), info)); } + if (thermocouple) + { + SensorInfo info("MAX31856", Config::Sensors::MAX31856::PERIOD, + [this]() { thermocoupleCallback(); }); + map.emplace(std::make_pair(thermocouple.get(), info)); + } + manager = std::make_unique<SensorManager>(map, &getSensorsScheduler()); return manager->start(); } diff --git a/src/Motor/Sensors/Sensors.h b/src/Motor/Sensors/Sensors.h index ce8b56fd4bd487e79ab297a08b26d6c06ee8a06c..5672fa969c7490b22d86373348428c6fc4ce0db2 100644 --- a/src/Motor/Sensors/Sensors.h +++ b/src/Motor/Sensors/Sensors.h @@ -32,6 +32,7 @@ #include <sensors/LIS2MDL/LIS2MDL.h> #include <sensors/LPS22DF/LPS22DF.h> #include <sensors/LSM6DSRX/LSM6DSRX.h> +#include <sensors/MAX31856/MAX31856.h> #include <sensors/SensorManager.h> #include <sensors/analog/TrafagPressureSensor.h> #include <utils/DependencyManager/DependencyManager.h> @@ -54,13 +55,17 @@ public: Boardcore::LPS22DFData getLPS22DFLastSample(); Boardcore::H3LIS331DLData getH3LIS331DLLastSample(); Boardcore::LIS2MDLData getLIS2MDLLastSample(); - Boardcore::LSM6DSRXData getLSM6DSRXLastSample(); + Boardcore::LSM6DSRXData getLSM6DSRX0LastSample(); + Boardcore::LSM6DSRXData getLSM6DSRX1LastSample(); - Boardcore::PressureData getTopTankPressLastSample(); - Boardcore::PressureData getBottomTankPressLastSample(); + Boardcore::PressureData getOxTopTankPressLastSample(); + Boardcore::PressureData getOxBottomTankPress0LastSample(); + Boardcore::PressureData getOxBottomTankPress1LastSample(); + Boardcore::PressureData getN2TankPressLastSample(); Boardcore::PressureData getCCPressLastSample(); Boardcore::TemperatureData getTankTempLastSample(); Boardcore::VoltageData getBatteryVoltageLastSample(); + Boardcore::TemperatureData getThermocoupleLastSample(); std::vector<Boardcore::SensorInfo> getSensorInfos(); @@ -73,13 +78,17 @@ protected: std::unique_ptr<Boardcore::LPS22DF> lps22df; std::unique_ptr<Boardcore::H3LIS331DL> h3lis331dl; std::unique_ptr<Boardcore::LIS2MDL> lis2mdl; - std::unique_ptr<Boardcore::LSM6DSRX> lsm6dsrx; + std::unique_ptr<Boardcore::LSM6DSRX> lsm6dsrx0; + std::unique_ptr<Boardcore::LSM6DSRX> lsm6dsrx1; std::unique_ptr<Boardcore::ADS131M08> ads131m08; std::unique_ptr<Boardcore::InternalADC> internalAdc; + std::unique_ptr<Boardcore::MAX31856> thermocouple; // Analog sensors - std::unique_ptr<Boardcore::TrafagPressureSensor> topTankPressure; - std::unique_ptr<Boardcore::TrafagPressureSensor> bottomTankPressure; + std::unique_ptr<Boardcore::TrafagPressureSensor> oxTopTankPressure; + std::unique_ptr<Boardcore::TrafagPressureSensor> oxBottomTankPressure0; + std::unique_ptr<Boardcore::TrafagPressureSensor> oxBottomTankPressure1; + std::unique_ptr<Boardcore::TrafagPressureSensor> n2TankPressure; std::unique_ptr<Boardcore::TrafagPressureSensor> ccPressure; std::unique_ptr<KuliteThermocouple> tankTemp; @@ -95,8 +104,10 @@ private: void lis2mdlInit(); void lis2mdlCallback(); + /// @brief Initialize both lsm6dsrx sensors. void lsm6dsrxInit(); - void lsm6dsrxCallback(); + void lsm6dsrx0Callback(); + void lsm6dsrx1Callback(); void ads131m08Init(); void ads131m08Callback(); @@ -104,11 +115,19 @@ private: void internalAdcInit(); void internalAdcCallback(); - void topTankPressureInit(); - void topTankPressureCallback(); + void thermocoupleInit(); + void thermocoupleCallback(); - void bottomTankPressureInit(); - void bottomTankPressureCallback(); + void oxTopTankPressureInit(); + void oxTopTankPressureCallback(); + + /// @brief Initialize both ox bottom pressure sensors. + void oxBottomTankPressureInit(); + void oxBottomTankPressure0Callback(); + void oxBottomTankPressure1Callback(); + + void n2TankPressureInit(); + void n2TankPressureCallback(); void ccPressureInit(); void ccPressureCallback(); diff --git a/src/Motor/Sensors/SensorsData.h b/src/Motor/Sensors/SensorsData.h index b205266fb42ecaabaff0f7a4ea0ccf4f78683822..168cf941c0ec11dd452529d0a6b928236e54e16b 100644 --- a/src/Motor/Sensors/SensorsData.h +++ b/src/Motor/Sensors/SensorsData.h @@ -26,24 +26,34 @@ namespace Motor { -struct TopTankPressureData : Boardcore::PressureData +struct OxTopTankPressureData : Boardcore::PressureData { - explicit TopTankPressureData(const Boardcore::PressureData& data) + explicit OxTopTankPressureData(const Boardcore::PressureData& data) : Boardcore::PressureData{data} { } - TopTankPressureData() {} + OxTopTankPressureData() {} }; -struct BottomTankPressureData : Boardcore::PressureData +struct OxBottomTankPressureData : Boardcore::PressureData { - explicit BottomTankPressureData(const Boardcore::PressureData& data) + explicit OxBottomTankPressureData(const Boardcore::PressureData& data) : Boardcore::PressureData{data} { } - BottomTankPressureData() {} + OxBottomTankPressureData() {} +}; + +struct N2TankPressureData : Boardcore::PressureData +{ + explicit N2TankPressureData(const Boardcore::PressureData& data) + : Boardcore::PressureData{data} + { + } + + N2TankPressureData() {} }; struct CCPressureData : Boardcore::PressureData diff --git a/src/common/CanConfig.h b/src/common/CanConfig.h index 579c5c920467014e2a7f97eeb8dc296bc620e170..60fff984b44f304b094ebc374b6cf9f9d03d4d8b 100644 --- a/src/common/CanConfig.h +++ b/src/common/CanConfig.h @@ -77,8 +77,10 @@ enum class SensorId : uint8_t PITOT_STATIC_PRESSURE, PITOT_TOTAL_PRESSURE, CC_PRESSURE, - BOTTOM_TANK_PRESSURE, - TOP_TANK_PRESSURE, + OX_BOTTOM_TANK_PRESSURE_0, + OX_BOTTOM_TANK_PRESSURE_1, + OX_TOP_TANK_PRESSURE, + N2_TANK_PRESSURE, TANK_TEMPERATURE, MOTOR_ACTUATORS_CURRENT, MAIN_BOARD_CURRENT,