diff --git a/src/boards/RIGv2/Actuators/Actuators.cpp b/src/boards/RIGv2/Actuators/Actuators.cpp
index 62a214341945a756e752e92f672dedf8970e6d56..10cf616a9774265eb2cc0e21a2629a807705f756 100644
--- a/src/boards/RIGv2/Actuators/Actuators.cpp
+++ b/src/boards/RIGv2/Actuators/Actuators.cpp
@@ -213,6 +213,18 @@ bool Actuators::setOpeningTime(ServosList servo, uint64_t time)
return true;
}
+bool Actuators::isServoOpen(ServosList servo)
+{
+ Lock<FastMutex> lock(infosMutex);
+ ServoInfo *info = getServo(servo);
+ if (info == nullptr)
+ {
+ return false;
+ }
+
+ return info->servo->getPosition() > Config::Servos::SERVO_OPEN_THRESHOLD;
+}
+
void Actuators::openServoInner(ServoInfo *info, uint64_t time)
{
long long currentTime = getTime();
diff --git a/src/boards/RIGv2/Actuators/Actuators.h b/src/boards/RIGv2/Actuators/Actuators.h
index f443cf348e053b91684fd165330276c13ed6ac26..ab3ecb80dc1c9b34af0e5a31827ce09cd96cbfa6 100644
--- a/src/boards/RIGv2/Actuators/Actuators.h
+++ b/src/boards/RIGv2/Actuators/Actuators.h
@@ -68,6 +68,7 @@ public:
bool openServoAtomic(ServosList servo, uint64_t time);
bool setMaxAperture(ServosList servo, float aperture);
bool setOpeningTime(ServosList servo, uint64_t time);
+ bool isServoOpen(ServosList servo);
private:
ServoInfo *getServo(ServosList servo);
diff --git a/src/boards/RIGv2/Configs/ActuatorsConfig.h b/src/boards/RIGv2/Configs/ActuatorsConfig.h
index b29b1655b38128bc8e1ae10affcf9a4940a7e7c6..979d69ed92b22ac0c580f89b4c9dd5b838bb237d 100644
--- a/src/boards/RIGv2/Configs/ActuatorsConfig.h
+++ b/src/boards/RIGv2/Configs/ActuatorsConfig.h
@@ -36,8 +36,10 @@ static constexpr unsigned int MIN_PULSE = 500;
static constexpr unsigned int MAX_PULSE = 2500;
static constexpr uint32_t SERVO_TIMINGS_CHECK_PERIOD = 100;
-static constexpr long long SERVO_CONFIDENCE_TIME = 500; // 0.5s
-static constexpr float SERVO_CONFIDENCE = 1 / 50.0; // 2%
+static constexpr long long SERVO_CONFIDENCE_TIME = 500; // 0.5s
+static constexpr float SERVO_CONFIDENCE = 0.02; // 2%
+// More than 10% is considered open
+static constexpr float SERVO_OPEN_THRESHOLD = 0.10; // 10%
static constexpr uint32_t DEFAULT_FILLING_OPENING_TIME = 15000; // 15s
static constexpr uint32_t DEFAULT_VENTING_OPENING_TIME = 15000; // 15s
diff --git a/src/boards/RIGv2/Configs/SensorsConfig.h b/src/boards/RIGv2/Configs/SensorsConfig.h
index 28712cd8f496f7b662f29ab88cdc5019eb6c7ddc..5ccb05156c07a9e9cdc11ec889dda77ddb5ed8bb 100644
--- a/src/boards/RIGv2/Configs/SensorsConfig.h
+++ b/src/boards/RIGv2/Configs/SensorsConfig.h
@@ -36,6 +36,22 @@ namespace Sensors
static constexpr uint32_t ADC_SAMPLE_PERIOD = 10;
static constexpr uint32_t TC_SAMPLE_PERIOD = 100;
+static constexpr float ADC1_CH1_SHUNT_RESISTANCE = 30;
+static constexpr float ADC1_CH2_SHUNT_RESISTANCE = 30;
+static constexpr float ADC1_CH3_SHUNT_RESISTANCE = 30;
+static constexpr float ADC1_CH4_SHUNT_RESISTANCE = 30;
+
+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;
+
}
} // namespace Config
diff --git a/src/boards/RIGv2/Radio/Radio.cpp b/src/boards/RIGv2/Radio/Radio.cpp
index a4f98607e3a86b1b425ef943e4ca0a25723a517e..9e98aa18cc48cb33e8b5845e40f777e1d3ebcfa1 100644
--- a/src/boards/RIGv2/Radio/Radio.cpp
+++ b/src/boards/RIGv2/Radio/Radio.cpp
@@ -262,7 +262,8 @@ void Radio::handleMessage(const mavlink_message_t& msg)
void Radio::handleCommand(const mavlink_message_t& msg)
{
- uint8_t cmd = mavlink_msg_command_tc_get_command_id(&msg);
+ ModuleManager& modules = ModuleManager::getInstance();
+ uint8_t cmd = mavlink_msg_command_tc_get_command_id(&msg);
switch (cmd)
{
case MAV_CMD_START_LOGGING:
@@ -287,8 +288,8 @@ void Radio::handleCommand(const mavlink_message_t& msg)
case MAV_CMD_CALIBRATE:
{
- // TODO: Implement calibrate
- sendNack(msg);
+ modules.get<Sensors>()->calibrate();
+ sendAck(msg);
break;
}
@@ -378,9 +379,22 @@ bool Radio::packSystemTm(uint8_t tmId, mavlink_message_t& msg)
{
mavlink_gse_tm_t tm = {0};
- tm.timestamp = TimestampTimer::getTimestamp();
- tm.loadcell_rocket = 69;
- tm.loadcell_vessel = 420;
+ Sensors* sensors = modules.get<Sensors>();
+ Actuators* actuators = modules.get<Actuators>();
+
+ tm.timestamp = TimestampTimer::getTimestamp();
+ tm.loadcell_rocket = sensors->getTankWeight().load;
+ tm.loadcell_vessel = sensors->getVesselWeight().load;
+ tm.filling_pressure = sensors->getFillingPress().pressure;
+ tm.vessel_pressure = sensors->getVesselPress().pressure;
+ tm.filling_valve_state =
+ actuators->isServoOpen(ServosList::FILLING_VALVE) ? 1 : 0;
+ tm.venting_valve_state =
+ actuators->isServoOpen(ServosList::VENTING_VALVE) ? 1 : 0;
+ tm.release_valve_state =
+ actuators->isServoOpen(ServosList::RELEASE_VALVE) ? 1 : 0;
+ tm.main_valve_state =
+ actuators->isServoOpen(ServosList::MAIN_VALVE) ? 1 : 0;
// TODO(davide.mor): Add the rest of these
mavlink_msg_gse_tm_encode(Config::Radio::MAV_SYSTEM_ID,
@@ -391,14 +405,16 @@ bool Radio::packSystemTm(uint8_t tmId, mavlink_message_t& msg)
case MAV_MOTOR_ID:
{
- mavlink_motor_tm_t tm;
+ mavlink_motor_tm_t tm = {0};
- auto tc1 = modules.get<Sensors>()->getTc1LastSample();
+ Sensors* sensors = modules.get<Sensors>();
tm.timestamp = TimestampTimer::getTimestamp();
- tm.tank_temperature = tc1.temperature;
- tm.top_tank_pressure = 69;
- tm.bottom_tank_pressure = 420;
+ tm.tank_temperature = sensors->getTc1LastSample().temperature;
+ tm.top_tank_pressure = sensors->getTankTopPress().pressure;
+ tm.bottom_tank_pressure = sensors->getTankBottomPress().pressure;
+ tm.floating_level = 69.0f; // Lol
+ // TODO(davide.mor): Add the rest of these
mavlink_msg_motor_tm_encode(Config::Radio::MAV_SYSTEM_ID,
Config::Radio::MAV_COMPONENT_ID, &msg,
@@ -439,21 +455,25 @@ void Radio::handleConrigState(const mavlink_message_t& msg)
(Config::Radio::LAST_COMMAND_THRESHOLD * Constants::NS_IN_MS))
{
// Ok we can accept new commands
- if(oldConrigState.arm_switch == 0 && state.arm_switch == 1) {
+ if (oldConrigState.arm_switch == 0 && state.arm_switch == 1)
+ {
// The ARM switch was pressed
// TODO(davide.mor): Arm the system
lastManualActuation = currentTime;
}
- if(oldConrigState.ignition_btn == 0 && state.ignition_btn == 1) {
+ if (oldConrigState.ignition_btn == 0 && state.ignition_btn == 1)
+ {
// The ignition switch was pressed
// TODO(davide.mor): Perform ignition
lastManualActuation = currentTime;
}
- if(oldConrigState.filling_valve_btn == 0 && state.filling_valve_btn == 1) {
+ if (oldConrigState.filling_valve_btn == 0 &&
+ state.filling_valve_btn == 1)
+ {
// The filling switch was pressed
// TODO(davide.mor): Notify everybody of a manual actuation
@@ -462,7 +482,9 @@ void Radio::handleConrigState(const mavlink_message_t& msg)
lastManualActuation = currentTime;
}
- if(oldConrigState.quick_connector_btn == 0 && state.quick_connector_btn == 1) {
+ if (oldConrigState.quick_connector_btn == 0 &&
+ state.quick_connector_btn == 1)
+ {
// The quick conector switch was pressed
// TODO(davide.mor): Notify everybody of a manual actuation
@@ -471,7 +493,9 @@ void Radio::handleConrigState(const mavlink_message_t& msg)
lastManualActuation = currentTime;
}
- if(oldConrigState.release_pressure_btn == 0 && state.release_pressure_btn == 1) {
+ if (oldConrigState.release_pressure_btn == 0 &&
+ state.release_pressure_btn == 1)
+ {
// The release switch was pressed
// TODO(davide.mor): Notify everybody of a manual actuation
@@ -480,7 +504,9 @@ void Radio::handleConrigState(const mavlink_message_t& msg)
lastManualActuation = currentTime;
}
- if(oldConrigState.venting_valve_btn == 0 && state.venting_valve_btn == 1) {
+ if (oldConrigState.venting_valve_btn == 0 &&
+ state.venting_valve_btn == 1)
+ {
// The venting switch was pressed
// TODO(davide.mor): Notify everybody of a manual actuation
@@ -491,7 +517,8 @@ void Radio::handleConrigState(const mavlink_message_t& msg)
}
// Special case for disarming, that can be done bypassing the timeout
- if(oldConrigState.arm_switch == 1 && state.arm_switch == 0) {
+ if (oldConrigState.arm_switch == 1 && state.arm_switch == 0)
+ {
// TODO(davide.mor): Disarm the system
lastManualActuation = currentTime;
diff --git a/src/boards/RIGv2/Sensors/Sensors.cpp b/src/boards/RIGv2/Sensors/Sensors.cpp
index 1add36cc4dc14597c28ae1fbfa37fc7a9c05ac71..fb401182ca91cf0c96da5c2994f5e8c24f356947 100644
--- a/src/boards/RIGv2/Sensors/Sensors.cpp
+++ b/src/boards/RIGv2/Sensors/Sensors.cpp
@@ -31,6 +31,19 @@ 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()
@@ -62,6 +75,85 @@ Boardcore::MAX31856Data Sensors::getTc1LastSample()
return tc1->getLastSample();
}
+Boardcore::PressureData Sensors::getVesselPress()
+{
+ auto sample = getADC1LastSample();
+
+ float pressure = pressureFromVoltage(
+ sample.voltage[0], 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};
+}
+
+Boardcore::PressureData Sensors::getFillingPress()
+{
+ auto sample = getADC1LastSample();
+
+ float pressure = pressureFromVoltage(
+ sample.voltage[1], 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};
+}
+
+Boardcore::PressureData Sensors::getTankTopPress()
+{
+ auto sample = getADC1LastSample();
+
+ float pressure = pressureFromVoltage(
+ sample.voltage[2], 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};
+}
+
+Boardcore::PressureData Sensors::getTankBottomPress()
+{
+ auto sample = getADC1LastSample();
+
+ float pressure = pressureFromVoltage(
+ sample.voltage[3], 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};
+}
+
+Boardcore::LoadCellData Sensors::getVesselWeight()
+{
+ auto sample = getADC1LastSample();
+ float calibratedVoltage = sample.voltage[6] - vesselLcOffset;
+
+ return {sample.timestamp, calibratedVoltage};
+}
+
+Boardcore::LoadCellData Sensors::getTankWeight()
+{
+ auto sample = getADC1LastSample();
+ float calibratedVoltage = sample.voltage[7] - tankLcOffset;
+
+ return {sample.timestamp, calibratedVoltage};
+}
+
+void Sensors::calibrate()
+{
+ Stats channel6, channel7;
+
+ for (unsigned int i = 0; i < Config::Sensors::LC_CALIBRATE_SAMPLE_COUNT;
+ i++)
+ {
+ auto sample = getADC1LastSample();
+
+ channel6.add(sample.voltage[6]);
+ channel7.add(sample.voltage[7]);
+
+ Thread::sleep(Config::Sensors::LC_CALIBRATE_SAMPLE_PERIOD);
+ }
+
+ vesselLcOffset = channel6.getStats().mean;
+ tankLcOffset = channel7.getStats().mean;
+}
+
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 794213747d6757961aca406eb4eb6fd1d7c951d7..4383c8788623c9da2f8b48c4b27bef9a28735ac9 100644
--- a/src/boards/RIGv2/Sensors/Sensors.h
+++ b/src/boards/RIGv2/Sensors/Sensors.h
@@ -26,8 +26,8 @@
#include <sensors/MAX31856/MAX31856.h>
#include <sensors/SensorManager.h>
-#include <memory>
#include <atomic>
+#include <memory>
#include <utils/ModuleManager/ModuleManager.hpp>
namespace RIGv2
@@ -36,26 +36,42 @@ namespace RIGv2
class Sensors : public Boardcore::Module
{
public:
- explicit Sensors(Boardcore::TaskScheduler &scheduler) : scheduler{scheduler} {}
+ explicit Sensors(Boardcore::TaskScheduler &scheduler) : scheduler{scheduler}
+ {
+ }
[[nodiscard]] bool start();
bool isStarted();
+ // Getters for raw data coming from sensors
Boardcore::ADS131M08Data getADC1LastSample();
Boardcore::MAX31856Data getTc1LastSample();
+ // Getters for processed data
+ Boardcore::PressureData getVesselPress();
+ Boardcore::PressureData getFillingPress();
+ Boardcore::PressureData getTankTopPress();
+ Boardcore::PressureData getTankBottomPress();
+ Boardcore::LoadCellData getVesselWeight();
+ Boardcore::LoadCellData getTankWeight();
+
+ void calibrate();
+
private:
void adc1Init(Boardcore::SensorManager::SensorMap_t &map);
void adc1Callback();
-
+
void tc1Init(Boardcore::SensorManager::SensorMap_t &map);
void tc1Callback();
- Boardcore::Logger &sdLogger = Boardcore::Logger::getInstance();
+ Boardcore::Logger &sdLogger = Boardcore::Logger::getInstance();
Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("sensors");
Boardcore::TaskScheduler &scheduler;
+ std::atomic<float> vesselLcOffset{0.0f};
+ std::atomic<float> tankLcOffset{0.0f};
+
std::atomic<bool> started{false};
std::unique_ptr<Boardcore::ADS131M08> adc1;
std::unique_ptr<Boardcore::MAX31856> tc1;