diff --git a/src/boards/RIGv2/Actuators/Actuators.cpp b/src/boards/RIGv2/Actuators/Actuators.cpp
index 33c4c2acb69f904a6f68d937d10d9f05d8bdcfb9..eb744975e378d7ed46b92d443ef7e1b0ef9df178 100644
--- a/src/boards/RIGv2/Actuators/Actuators.cpp
+++ b/src/boards/RIGv2/Actuators/Actuators.cpp
@@ -25,9 +25,8 @@
#include <RIGv2/Actuators/ActuatorsData.h>
#include <RIGv2/Configs/ActuatorsConfig.h>
#include <common/Events.h>
-#include <events/EventBroker.h>
-// TODO(davide.mor): Remove TimestampTimer
#include <drivers/timer/TimestampTimer.h>
+#include <events/EventBroker.h>
using namespace Boardcore;
using namespace miosix;
diff --git a/src/boards/RIGv2/Buses.h b/src/boards/RIGv2/Buses.h
index ffbda4b787b9cdfdab6db03fc30799c0ef402d18..6ba76033d03bf9c547853360d2f0f05f1f1254db 100644
--- a/src/boards/RIGv2/Buses.h
+++ b/src/boards/RIGv2/Buses.h
@@ -30,13 +30,6 @@ namespace RIGv2
class Buses : public Boardcore::Injectable
{
-private:
- Boardcore::SPIBus spi1;
- Boardcore::SPIBus spi2;
- Boardcore::SPIBus spi3;
- Boardcore::SPIBus spi4;
- Boardcore::SPIBus spi6;
-
public:
Buses() : spi1(SPI1), spi2(SPI2), spi3(SPI3), spi4(SPI4), spi6(SPI6) {}
@@ -49,6 +42,13 @@ public:
Boardcore::SPIBus &getMAX31856_1() { return spi3; }
Boardcore::SPIBus &getMAX31856_2() { return spi1; }
Boardcore::SPIBus &getRadio() { return spi6; }
+
+private:
+ Boardcore::SPIBus spi1;
+ Boardcore::SPIBus spi2;
+ Boardcore::SPIBus spi3;
+ Boardcore::SPIBus spi4;
+ Boardcore::SPIBus spi6;
};
} // namespace RIGv2
\ No newline at end of file
diff --git a/src/boards/RIGv2/CanHandler/CanHandler.cpp b/src/boards/RIGv2/CanHandler/CanHandler.cpp
index 3b5d2a2aa185b54f820c8f280275a94c0d782f98..28a32203f04b996639f171590a3cb578f45c7922 100644
--- a/src/boards/RIGv2/CanHandler/CanHandler.cpp
+++ b/src/boards/RIGv2/CanHandler/CanHandler.cpp
@@ -31,7 +31,7 @@
using namespace miosix;
using namespace RIGv2;
using namespace Boardcore;
-using namespace Boardcore::Canbus;
+using namespace Canbus;
using namespace Common;
CanHandler::CanHandler()
@@ -74,7 +74,7 @@ bool CanHandler::start()
TimestampTimer::getTimestamp(),
static_cast<int16_t>(stats.logNumber),
static_cast<uint8_t>(state),
- state == GroundModeManagerState::GMM_STATE_ARMED,
+ state == GroundModeManagerState::ARMED,
false,
stats.lastWriteError == 0,
});
@@ -118,19 +118,19 @@ void CanHandler::sendServoOpenCommand(ServosList servo, uint32_t openingTime)
ServoCommand{TimestampTimer::getTimestamp(), openingTime});
}
-CanHandler::CanStatus CanHandler::getCanStatus()
-{
- Lock<FastMutex> lock{statusMutex};
- return status;
-}
-
void CanHandler::sendServoCloseCommand(ServosList servo)
{
// Closing a servo means opening it for 0s
sendServoOpenCommand(servo, 0);
}
-void CanHandler::handleMessage(const Boardcore::Canbus::CanMessage &msg)
+CanHandler::CanStatus CanHandler::getCanStatus()
+{
+ Lock<FastMutex> lock{statusMutex};
+ return status;
+}
+
+void CanHandler::handleMessage(const Canbus::CanMessage &msg)
{
CanConfig::PrimaryType type =
static_cast<CanConfig::PrimaryType>(msg.getPrimaryType());
@@ -169,14 +169,12 @@ void CanHandler::handleMessage(const Boardcore::Canbus::CanMessage &msg)
}
}
-void CanHandler::handleEvent(const Boardcore::Canbus::CanMessage &msg)
+void CanHandler::handleEvent(const Canbus::CanMessage &msg)
{
- CanConfig::EventId event =
- static_cast<CanConfig::EventId>(msg.getSecondaryType());
- LOG_WARN(logger, "Received unrecognized event: {}", event);
+ // The RIG doesn't actually listen to any of the rockets internal events
}
-void CanHandler::handleSensor(const Boardcore::Canbus::CanMessage &msg)
+void CanHandler::handleSensor(const Canbus::CanMessage &msg)
{
CanConfig::SensorId sensor =
static_cast<CanConfig::SensorId>(msg.getSecondaryType());
@@ -192,19 +190,19 @@ void CanHandler::handleSensor(const Boardcore::Canbus::CanMessage &msg)
break;
}
- case CanConfig::SensorId::BOTTOM_TANK_PRESSURE:
+ case CanConfig::SensorId::TOP_TANK_PRESSURE:
{
CanPressureData data = pressureDataFromCanMessage(msg);
sdLogger.log(data);
- sensors->setCanBottomTankPress(data);
+ sensors->setCanTopTankPress(data);
break;
}
- case CanConfig::SensorId::TOP_TANK_PRESSURE:
+ case CanConfig::SensorId::BOTTOM_TANK_PRESSURE:
{
CanPressureData data = pressureDataFromCanMessage(msg);
sdLogger.log(data);
- sensors->setCanTopTankPress(data);
+ sensors->setCanBottomTankPress(data);
break;
}
@@ -231,7 +229,7 @@ void CanHandler::handleSensor(const Boardcore::Canbus::CanMessage &msg)
}
}
-void CanHandler::handleActuator(const Boardcore::Canbus::CanMessage &msg)
+void CanHandler::handleActuator(const Canbus::CanMessage &msg)
{
ServosList servo = static_cast<ServosList>(msg.getSecondaryType());
CanServoFeedback data = servoFeedbackFromCanMessage(msg);
@@ -240,7 +238,7 @@ void CanHandler::handleActuator(const Boardcore::Canbus::CanMessage &msg)
getModule<Actuators>()->setCanServoOpen(servo, data.open);
}
-void CanHandler::handleStatus(const Boardcore::Canbus::CanMessage &msg)
+void CanHandler::handleStatus(const Canbus::CanMessage &msg)
{
CanConfig::Board source = static_cast<CanConfig::Board>(msg.getSource());
CanDeviceStatus deviceStatus = deviceStatusFromCanMessage(msg);
diff --git a/src/boards/RIGv2/Configs/GmmConfig.h b/src/boards/RIGv2/Configs/GMMConfig.h
similarity index 100%
rename from src/boards/RIGv2/Configs/GmmConfig.h
rename to src/boards/RIGv2/Configs/GMMConfig.h
diff --git a/src/boards/RIGv2/Configs/RadioConfig.h b/src/boards/RIGv2/Configs/RadioConfig.h
index b5e6b87c5a58f4114b47994aee90b7e1b66fc52f..1e73efd97e4f652a3e75ec4c4ebaad7046f9e917 100644
--- a/src/boards/RIGv2/Configs/RadioConfig.h
+++ b/src/boards/RIGv2/Configs/RadioConfig.h
@@ -39,7 +39,7 @@ constexpr unsigned int MAV_MAX_LENGTH = MAVLINK_MAX_DIALECT_PAYLOAD_SIZE;
constexpr uint16_t MAV_SLEEP_AFTER_SEND = 0;
constexpr size_t MAV_OUT_BUFFER_MAX_AGE = 10;
-constexpr unsigned int CIRCULAR_BUFFER_SIZE = 20;
+constexpr unsigned int CIRCULAR_BUFFER_SIZE = 30;
constexpr unsigned int MAX_PACKETS_PER_FLUSH = 4;
constexpr uint8_t MAV_SYSTEM_ID = 171;
diff --git a/src/boards/RIGv2/Configs/SensorsConfig.h b/src/boards/RIGv2/Configs/SensorsConfig.h
index 8faba975cd3e3cdd1f686d2558d003c030a0205c..30204b2cea8164c8f57b257cece8c9dd5fac444e 100644
--- a/src/boards/RIGv2/Configs/SensorsConfig.h
+++ b/src/boards/RIGv2/Configs/SensorsConfig.h
@@ -38,11 +38,11 @@ namespace Config
namespace Sensors
{
+/* linter off */ using namespace Boardcore::Units::Frequency;
+
namespace ADS131M08
{
-/* linter off */ using namespace Boardcore::Units::Frequency;
-
constexpr Boardcore::ADS131M08Defs::OversamplingRatio OSR =
Boardcore::ADS131M08Defs::OversamplingRatio::OSR_8192;
constexpr bool GLOBAL_CHOP_MODE_EN = true;
@@ -85,9 +85,6 @@ constexpr bool ENABLED = true;
namespace MAX31856
{
-
-/* linter off */ using namespace Boardcore::Units::Frequency;
-
constexpr Hertz PERIOD = 10_hz;
constexpr bool ENABLED = true;
} // namespace MAX31856
@@ -134,9 +131,6 @@ constexpr float VESSEL_P1_MASS = 6.916;
namespace InternalADC
{
-
-/* linter off */ using namespace Boardcore::Units::Frequency;
-
constexpr Boardcore::InternalADC::Channel BATTERY_VOLTAGE_CHANNEL =
Boardcore::InternalADC::CH14;
diff --git a/src/boards/RIGv2/Radio/Radio.cpp b/src/boards/RIGv2/Radio/Radio.cpp
index 1b194ec169d35e6d06ff358f1e9c7b6148f73a42..acce62a5e8f9736d4411c75ffe0011aea5e89f0c 100644
--- a/src/boards/RIGv2/Radio/Radio.cpp
+++ b/src/boards/RIGv2/Radio/Radio.cpp
@@ -24,10 +24,9 @@
#include <common/Events.h>
#include <common/Radio.h>
+#include <drivers/timer/TimestampTimer.h>
#include <events/EventBroker.h>
#include <radio/SX1278/SX1278Frontends.h>
-// TODO(davide.mor): Remove TimestampTimer
-#include <drivers/timer/TimestampTimer.h>
#include <atomic>
#include <unordered_map>
@@ -143,7 +142,7 @@ void Radio::enqueueNack(const mavlink_message_t& msg)
enqueuePacket(nackMsg);
}
-Boardcore::MavlinkStatus Radio::getMavStatus()
+MavlinkStatus Radio::getMavStatus()
{
if (mavDriver)
{
@@ -194,7 +193,8 @@ void Radio::handleMessage(const mavlink_message_t& msg)
case MAVLINK_MSG_ID_SENSOR_TM_REQUEST_TC:
{
- uint8_t tmId = mavlink_msg_system_tm_request_tc_get_tm_id(&msg);
+ uint8_t tmId =
+ mavlink_msg_sensor_tm_request_tc_get_sensor_name(&msg);
if (enqueueSensorTm(tmId))
{
enqueueAck(msg);
@@ -213,7 +213,7 @@ void Radio::handleMessage(const mavlink_message_t& msg)
mavlink_msg_wiggle_servo_tc_get_servo_id(&msg));
if (getModule<GroundModeManager>()->getState() ==
- GMM_STATE_DISARMED)
+ GroundModeManagerState::DISARMED)
{
if (getModule<Actuators>()->wiggleServo(servo))
{
@@ -288,26 +288,19 @@ void Radio::handleMessage(const mavlink_message_t& msg)
void Radio::handleCommand(const mavlink_message_t& msg)
{
- static const std::unordered_map<uint8_t, Events> cmdToEvent{
- {MAV_CMD_CALIBRATE, TMTC_CALIBRATE},
- {MAV_CMD_FORCE_INIT, TMTC_FORCE_INIT},
- {MAV_CMD_FORCE_REBOOT, TMTC_RESET_BOARD},
- {MAV_CMD_OPEN_NITROGEN, TMTC_OPEN_NITROGEN},
- };
-
- uint8_t cmd = mavlink_msg_command_tc_get_command_id(&msg);
- switch (cmd)
+ uint8_t cmdId = mavlink_msg_command_tc_get_command_id(&msg);
+ switch (cmdId)
{
case MAV_CMD_START_LOGGING:
{
- if (!Logger::getInstance().start())
+ if (Logger::getInstance().start())
{
- enqueueNack(msg);
+ Logger::getInstance().resetStats();
+ enqueueAck(msg);
}
else
{
- Logger::getInstance().resetStats();
- enqueueAck(msg);
+ enqueueNack(msg);
}
break;
}
@@ -354,19 +347,17 @@ void Radio::handleCommand(const mavlink_message_t& msg)
default:
{
- auto it = cmdToEvent.find(cmd);
- if (it != cmdToEvent.end())
+ // Try to map the command to an event
+ auto ev = mavCmdToEvent(cmdId);
+ if (ev != LAST_EVENT)
{
- EventBroker::getInstance().post(it->second, TOPIC_MOTOR);
+ EventBroker::getInstance().post(ev, TOPIC_TMTC);
enqueueAck(msg);
}
else
{
- // Unrecognized command
enqueueNack(msg);
}
-
- break;
}
}
}
@@ -550,14 +541,15 @@ bool Radio::enqueueSystemTm(uint8_t tmId)
tm.timestamp = TimestampTimer::getTimestamp();
// Sensors
- 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.battery_voltage = sensors->getBatteryVoltage().voltage;
- tm.current_consumption = sensors->getServoCurrent().current;
+ tm.loadcell_rocket = sensors->getTankWeightLastSample().load;
+ tm.loadcell_vessel = sensors->getVesselWeightLastSample().load;
+ tm.filling_pressure = sensors->getFillingPressLastSample().pressure;
+ tm.vessel_pressure = sensors->getVesselPressLastSample().pressure;
+ tm.battery_voltage = sensors->getBatteryVoltageLastSample().voltage;
+ tm.current_consumption =
+ sensors->getServoCurrentLastSample().current;
tm.umbilical_current_consumption =
- sensors->getUmbilicalCurrent().current;
+ sensors->getUmbilicalCurrentLastSample().current;
// Log data
LoggerStats stats = sdLogger.getStats();
@@ -576,12 +568,12 @@ bool Radio::enqueueSystemTm(uint8_t tmId)
tm.nitrogen_valve_state = actuators->isNitrogenOpen() ? 1 : 0;
// Internal states
- tm.gmm_state = getModule<GroundModeManager>()->getState();
- tm.tars_state = getModule<TARS1>()->isRefueling() ? 1 : 0;
- tm.arming_state =
- getModule<GroundModeManager>()->getState() == GMM_STATE_ARMED
- ? 1
- : 0;
+ tm.gmm_state = getModule<GroundModeManager>()->getState();
+ tm.tars_state = getModule<TARS1>()->isRefueling() ? 1 : 0;
+ tm.arming_state = getModule<GroundModeManager>()->getState() ==
+ GroundModeManagerState::ARMED
+ ? 1
+ : 0;
// Can data
CanHandler::CanStatus canStatus =
@@ -611,11 +603,15 @@ bool Radio::enqueueSystemTm(uint8_t tmId)
tm.timestamp = TimestampTimer::getTimestamp();
// Sensors (either CAN or local)
- tm.top_tank_pressure = sensors->getTopTankPress().pressure;
- tm.bottom_tank_pressure = sensors->getBottomTankPress().pressure;
- tm.combustion_chamber_pressure = sensors->getCCPress().pressure;
- tm.tank_temperature = sensors->getTc1LastSample().temperature;
- tm.battery_voltage = sensors->getMotorBatteryVoltage().voltage;
+ tm.top_tank_pressure =
+ sensors->getTopTankPressLastSample().pressure;
+ tm.bottom_tank_pressure =
+ sensors->getBottomTankPressLastSample().pressure;
+ tm.combustion_chamber_pressure =
+ sensors->getCCPressLastSample().pressure;
+ tm.tank_temperature = sensors->getTankTempLastSample().temperature;
+ tm.battery_voltage =
+ sensors->getMotorBatteryVoltageLastSample().voltage;
// Valve states
tm.main_valve_state =
@@ -687,11 +683,12 @@ bool Radio::enqueueSensorTm(uint8_t tmId)
mavlink_message_t msg;
mavlink_pressure_tm_t tm;
- PressureData data = getModule<Sensors>()->getVesselPress();
+ PressureData data =
+ getModule<Sensors>()->getVesselPressLastSample();
tm.timestamp = data.pressureTimestamp;
tm.pressure = data.pressure;
- strcpy(tm.sensor_name, "VESSEL_PRESS");
+ strcpy(tm.sensor_name, "VesselPressure");
mavlink_msg_pressure_tm_encode(Config::Radio::MAV_SYSTEM_ID,
Config::Radio::MAV_COMPONENT_ID,
@@ -705,11 +702,12 @@ bool Radio::enqueueSensorTm(uint8_t tmId)
mavlink_message_t msg;
mavlink_pressure_tm_t tm;
- PressureData data = getModule<Sensors>()->getFillingPress();
+ PressureData data =
+ getModule<Sensors>()->getFillingPressLastSample();
tm.timestamp = data.pressureTimestamp;
tm.pressure = data.pressure;
- strcpy(tm.sensor_name, "FILLING_PRESS");
+ strcpy(tm.sensor_name, "FillingPressure");
mavlink_msg_pressure_tm_encode(Config::Radio::MAV_SYSTEM_ID,
Config::Radio::MAV_COMPONENT_ID,
@@ -723,11 +721,12 @@ bool Radio::enqueueSensorTm(uint8_t tmId)
mavlink_message_t msg;
mavlink_pressure_tm_t tm;
- PressureData data = getModule<Sensors>()->getBottomTankPress();
+ PressureData data =
+ getModule<Sensors>()->getBottomTankPressLastSample();
tm.timestamp = data.pressureTimestamp;
tm.pressure = data.pressure;
- strcpy(tm.sensor_name, "BOTTOM_TANK_PRESS");
+ strcpy(tm.sensor_name, "BottomTankPressure");
mavlink_msg_pressure_tm_encode(Config::Radio::MAV_SYSTEM_ID,
Config::Radio::MAV_COMPONENT_ID,
@@ -741,11 +740,12 @@ bool Radio::enqueueSensorTm(uint8_t tmId)
mavlink_message_t msg;
mavlink_pressure_tm_t tm;
- PressureData data = getModule<Sensors>()->getTopTankPress();
+ PressureData data =
+ getModule<Sensors>()->getTopTankPressLastSample();
tm.timestamp = data.pressureTimestamp;
tm.pressure = data.pressure;
- strcpy(tm.sensor_name, "TOP_TANK_PRESS");
+ strcpy(tm.sensor_name, "TopTankPressure");
mavlink_msg_pressure_tm_encode(Config::Radio::MAV_SYSTEM_ID,
Config::Radio::MAV_COMPONENT_ID,
@@ -777,11 +777,12 @@ bool Radio::enqueueSensorTm(uint8_t tmId)
mavlink_message_t msg;
mavlink_load_tm_t tm;
- LoadCellData data = getModule<Sensors>()->getVesselWeight();
+ LoadCellData data =
+ getModule<Sensors>()->getVesselWeightLastSample();
tm.timestamp = data.loadTimestamp;
tm.load = data.load;
- strcpy(tm.sensor_name, "VESSEL_WEIGHT");
+ strcpy(tm.sensor_name, "VesselWeight");
mavlink_msg_load_tm_encode(Config::Radio::MAV_SYSTEM_ID,
Config::Radio::MAV_COMPONENT_ID, &msg,
@@ -795,11 +796,11 @@ bool Radio::enqueueSensorTm(uint8_t tmId)
mavlink_message_t msg;
mavlink_load_tm_t tm;
- LoadCellData data = getModule<Sensors>()->getTankWeight();
+ LoadCellData data = getModule<Sensors>()->getTankWeightLastSample();
tm.timestamp = data.loadTimestamp;
tm.load = data.load;
- strcpy(tm.sensor_name, "TANK_WEIGHT");
+ strcpy(tm.sensor_name, "TankWeight");
mavlink_msg_load_tm_encode(Config::Radio::MAV_SYSTEM_ID,
Config::Radio::MAV_COMPONENT_ID, &msg,
@@ -813,11 +814,12 @@ bool Radio::enqueueSensorTm(uint8_t tmId)
mavlink_message_t msg;
mavlink_voltage_tm_t tm;
- VoltageData data = getModule<Sensors>()->getBatteryVoltage();
+ VoltageData data =
+ getModule<Sensors>()->getBatteryVoltageLastSample();
tm.timestamp = data.voltageTimestamp;
tm.voltage = data.voltage;
- strcpy(tm.sensor_name, "TANK_WEIGHT");
+ strcpy(tm.sensor_name, "BatteryVoltage");
mavlink_msg_voltage_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 a3ba2e20f13734d0a7adb5870c3cef753537309d..57051fe105faa529147b78acd498b91b0bb8028b 100644
--- a/src/boards/RIGv2/Sensors/Sensors.cpp
+++ b/src/boards/RIGv2/Sensors/Sensors.cpp
@@ -23,9 +23,8 @@
#include "Sensors.h"
#include <RIGv2/Configs/SensorsConfig.h>
-#include <interfaces-impl/hwmapping.h>
-// TODO(davide.mor): Remove TimestampTimer
#include <drivers/timer/TimestampTimer.h>
+#include <interfaces-impl/hwmapping.h>
using namespace Boardcore;
using namespace miosix;
@@ -81,22 +80,21 @@ MAX31856Data Sensors::getTc1LastSample()
return tc1 ? tc1->getLastSample() : MAX31856Data{};
}
-PressureData Sensors::getVesselPress()
+PressureData Sensors::getVesselPressLastSample()
{
return vesselPressure ? vesselPressure->getLastSample() : PressureData{};
}
-PressureData Sensors::getFillingPress()
+PressureData Sensors::getFillingPressLastSample()
{
return fillingPressure ? fillingPressure->getLastSample() : PressureData{};
}
-PressureData Sensors::getTopTankPress()
+PressureData Sensors::getTopTankPressLastSample()
{
if (useCanData)
{
- Lock<FastMutex> lock{canMutex};
- return canTopTankPressure;
+ return getCanTopTankPressLastSample();
}
else
{
@@ -105,12 +103,11 @@ PressureData Sensors::getTopTankPress()
}
}
-PressureData Sensors::getBottomTankPress()
+PressureData Sensors::getBottomTankPressLastSample()
{
if (useCanData)
{
- Lock<FastMutex> lock{canMutex};
- return canBottomTankPressure;
+ return getCanBottomTankPressLastSample();
}
else
{
@@ -119,12 +116,11 @@ PressureData Sensors::getBottomTankPress()
}
}
-PressureData Sensors::getCCPress()
+PressureData Sensors::getCCPressLastSample()
{
if (useCanData)
{
- Lock<FastMutex> lock{canMutex};
- return canCCPressure;
+ return getCanCCPressLastSample();
}
else
{
@@ -132,12 +128,11 @@ PressureData Sensors::getCCPress()
}
}
-TemperatureData Sensors::getTankTemp()
+TemperatureData Sensors::getTankTempLastSample()
{
if (useCanData)
{
- Lock<FastMutex> lock{canMutex};
- return canTankTemperature;
+ return getCanTankTempLastSample();
}
else
{
@@ -145,7 +140,7 @@ TemperatureData Sensors::getTankTemp()
}
}
-LoadCellData Sensors::getVesselWeight()
+LoadCellData Sensors::getVesselWeightLastSample()
{
if (vesselWeight)
{
@@ -157,7 +152,7 @@ LoadCellData Sensors::getVesselWeight()
}
}
-LoadCellData Sensors::getTankWeight()
+LoadCellData Sensors::getTankWeightLastSample()
{
if (tankWeight)
{
@@ -169,12 +164,13 @@ LoadCellData Sensors::getTankWeight()
}
}
-CurrentData Sensors::getUmbilicalCurrent()
+CurrentData Sensors::getUmbilicalCurrentLastSample()
{
- return {TimestampTimer::getTimestamp(), -1.0};
+ // TODO: Implement this
+ return {};
}
-CurrentData Sensors::getServoCurrent()
+CurrentData Sensors::getServoCurrentLastSample()
{
auto sample = getADC1LastSample();
@@ -187,7 +183,7 @@ CurrentData Sensors::getServoCurrent()
return {sample.timestamp, -current / 5.0f * 50.0f};
}
-VoltageData Sensors::getBatteryVoltage()
+VoltageData Sensors::getBatteryVoltageLastSample()
{
auto sample = getInternalADCLastSample();
@@ -198,12 +194,11 @@ VoltageData Sensors::getBatteryVoltage()
return {sample.timestamp, voltage};
}
-VoltageData Sensors::getMotorBatteryVoltage()
+VoltageData Sensors::getMotorBatteryVoltageLastSample()
{
if (useCanData)
{
- Lock<FastMutex> lock{canMutex};
- return canMotorBatteryVoltage;
+ return getCanMotorBatteryVoltageLastSample();
}
else
{
@@ -211,35 +206,65 @@ VoltageData Sensors::getMotorBatteryVoltage()
}
}
-void Sensors::setCanTopTankPress(Boardcore::PressureData data)
+PressureData Sensors::getCanTopTankPressLastSample()
+{
+ Lock<FastMutex> lock{canMutex};
+ return canTopTankPressure;
+}
+
+PressureData Sensors::getCanBottomTankPressLastSample()
+{
+ Lock<FastMutex> lock{canMutex};
+ return canBottomTankPressure;
+}
+
+PressureData Sensors::getCanCCPressLastSample()
+{
+ Lock<FastMutex> lock{canMutex};
+ return canCCPressure;
+}
+
+TemperatureData Sensors::getCanTankTempLastSample()
+{
+ Lock<FastMutex> lock{canMutex};
+ return canTankTemperature;
+}
+
+VoltageData Sensors::getCanMotorBatteryVoltageLastSample()
+{
+ Lock<FastMutex> lock{canMutex};
+ return canMotorBatteryVoltage;
+}
+
+void Sensors::setCanTopTankPress(PressureData data)
{
Lock<FastMutex> lock{canMutex};
canTopTankPressure = data;
useCanData = true;
}
-void Sensors::setCanBottomTankPress(Boardcore::PressureData data)
+void Sensors::setCanBottomTankPress(PressureData data)
{
Lock<FastMutex> lock{canMutex};
canBottomTankPressure = data;
useCanData = true;
}
-void Sensors::setCanCCPress(Boardcore::PressureData data)
+void Sensors::setCanCCPress(PressureData data)
{
Lock<FastMutex> lock{canMutex};
canCCPressure = data;
useCanData = true;
}
-void Sensors::setCanTankTemp(Boardcore::TemperatureData data)
+void Sensors::setCanTankTemp(TemperatureData data)
{
Lock<FastMutex> lock{canMutex};
canTankTemperature = data;
useCanData = true;
}
-void Sensors::setCanMotorBatteryVoltage(Boardcore::VoltageData data)
+void Sensors::setCanMotorBatteryVoltage(VoltageData data)
{
Lock<FastMutex> lock{canMutex};
canMotorBatteryVoltage = data;
@@ -300,8 +325,7 @@ void Sensors::internalAdcInit()
void Sensors::internalAdcCallback()
{
- InternalADCData sample = internalAdc->getLastSample();
- sdLogger.log(sample);
+ sdLogger.log(getInternalADCLastSample());
}
void Sensors::adc1Init()
@@ -375,18 +399,7 @@ void Sensors::adc1Init()
spiConfig, config);
}
-void Sensors::adc1Callback()
-{
- ADS131M08Data sample = adc1->getLastSample();
-
- ADCsData data{sample.timestamp, 1,
- sample.voltage[0], sample.voltage[1],
- sample.voltage[2], sample.voltage[3],
- sample.voltage[4], sample.voltage[5],
- sample.voltage[6], sample.voltage[7]};
-
- sdLogger.log(data);
-}
+void Sensors::adc1Callback() { sdLogger.log(ADC1Data{getADC1LastSample()}); }
void Sensors::tc1Init()
{
@@ -398,14 +411,7 @@ void Sensors::tc1Init()
spiConfig, MAX31856::ThermocoupleType::K_TYPE);
}
-void Sensors::tc1Callback()
-{
- MAX31856Data sample = tc1->getLastSample();
-
- TCsData data{sample.temperatureTimestamp, 1, sample.temperature,
- sample.coldJunctionTemperature};
- sdLogger.log(data);
-}
+void Sensors::tc1Callback() { sdLogger.log(TC1Data{getTc1LastSample()}); }
void Sensors::vesselPressureInit()
{
@@ -424,9 +430,7 @@ void Sensors::vesselPressureInit()
void Sensors::vesselPressureCallback()
{
- PressureData sample = vesselPressure->getLastSample();
- PTsData data{sample.pressureTimestamp, 1, sample.pressure};
- sdLogger.log(data);
+ sdLogger.log(VesselPressureData{getVesselPressLastSample()});
}
void Sensors::fillingPressureInit()
@@ -446,9 +450,7 @@ void Sensors::fillingPressureInit()
void Sensors::fillingPressureCallback()
{
- PressureData sample = fillingPressure->getLastSample();
- PTsData data{sample.pressureTimestamp, 2, sample.pressure};
- sdLogger.log(data);
+ sdLogger.log(FillingPressureData{getFillingPressLastSample()});
}
void Sensors::topTankPressureInit()
@@ -468,9 +470,7 @@ void Sensors::topTankPressureInit()
void Sensors::topTankPressureCallback()
{
- PressureData sample = topTankPressure->getLastSample();
- PTsData data{sample.pressureTimestamp, 3, sample.pressure};
- sdLogger.log(data);
+ sdLogger.log(TopTankPressureData{topTankPressure->getLastSample()});
}
void Sensors::bottomTankPressureInit()
@@ -490,9 +490,7 @@ void Sensors::bottomTankPressureInit()
void Sensors::bottomTankPressureCallback()
{
- PressureData sample = bottomTankPressure->getLastSample();
- PTsData data{sample.pressureTimestamp, 4, sample.pressure};
- sdLogger.log(data);
+ sdLogger.log(BottomTankPressureData{bottomTankPressure->getLastSample()});
}
void Sensors::vesselWeightInit()
@@ -512,9 +510,7 @@ void Sensors::vesselWeightInit()
void Sensors::vesselWeightCallback()
{
- LoadCellData sample = vesselWeight->getLastSample();
- LCsData data{sample.loadTimestamp, 1, sample.load};
- sdLogger.log(data);
+ sdLogger.log(VesselWeightData{getVesselWeightLastSample()});
}
void Sensors::tankWeightInit()
@@ -534,9 +530,7 @@ void Sensors::tankWeightInit()
void Sensors::tankWeightCallback()
{
- LoadCellData sample = tankWeight->getLastSample();
- LCsData data{sample.loadTimestamp, 2, sample.load};
- sdLogger.log(data);
+ sdLogger.log(TankWeightData{getTankWeightLastSample()});
}
bool Sensors::sensorManagerInit()
diff --git a/src/boards/RIGv2/Sensors/Sensors.h b/src/boards/RIGv2/Sensors/Sensors.h
index 48b583c7a41af6d88b275cdc15f7ad53479825a3..69c3f75211da48551436923a50c4ff95b9746858 100644
--- a/src/boards/RIGv2/Sensors/Sensors.h
+++ b/src/boards/RIGv2/Sensors/Sensors.h
@@ -48,6 +48,8 @@ public:
[[nodiscard]] bool start();
+ void calibrate();
+
bool isStarted();
// Getters for raw data coming from sensors
@@ -56,18 +58,26 @@ public:
Boardcore::MAX31856Data getTc1LastSample();
// Getters for processed data
- Boardcore::PressureData getVesselPress();
- Boardcore::PressureData getFillingPress();
- Boardcore::PressureData getTopTankPress();
- Boardcore::PressureData getBottomTankPress();
- Boardcore::PressureData getCCPress();
- Boardcore::TemperatureData getTankTemp();
- Boardcore::LoadCellData getVesselWeight();
- Boardcore::LoadCellData getTankWeight();
- Boardcore::CurrentData getUmbilicalCurrent();
- Boardcore::CurrentData getServoCurrent();
- Boardcore::VoltageData getBatteryVoltage();
- Boardcore::VoltageData getMotorBatteryVoltage();
+ Boardcore::PressureData getVesselPressLastSample();
+ Boardcore::PressureData getFillingPressLastSample();
+ Boardcore::PressureData getTopTankPressLastSample();
+ Boardcore::PressureData getBottomTankPressLastSample();
+ Boardcore::PressureData getCCPressLastSample();
+ Boardcore::TemperatureData getTankTempLastSample();
+ Boardcore::LoadCellData getVesselWeightLastSample();
+ Boardcore::LoadCellData getTankWeightLastSample();
+ Boardcore::CurrentData getUmbilicalCurrentLastSample();
+ Boardcore::CurrentData getServoCurrentLastSample();
+ Boardcore::VoltageData getBatteryVoltageLastSample();
+ Boardcore::VoltageData getMotorBatteryVoltageLastSample();
+
+ Boardcore::PressureData getCanTopTankPressLastSample();
+ Boardcore::PressureData getCanBottomTankPressLastSample();
+ Boardcore::PressureData getCanCCPressLastSample();
+ Boardcore::TemperatureData getCanTankTempLastSample();
+ Boardcore::VoltageData getCanMotorBatteryVoltageLastSample();
+
+ std::vector<Boardcore::SensorInfo> getSensorInfos();
void setCanTopTankPress(Boardcore::PressureData data);
void setCanBottomTankPress(Boardcore::PressureData data);
@@ -75,10 +85,6 @@ public:
void setCanTankTemp(Boardcore::TemperatureData data);
void setCanMotorBatteryVoltage(Boardcore::VoltageData data);
- void calibrate();
-
- std::vector<Boardcore::SensorInfo> getSensorInfos();
-
private:
void vesselPressureInit();
void vesselPressureCallback();
diff --git a/src/boards/RIGv2/Sensors/SensorsData.h b/src/boards/RIGv2/Sensors/SensorsData.h
index f57cb516cb4041e0f79a0e03c62814b038154268..25b6e2399ff9c1b52ae89dfc42b7d09f0123a0d8 100644
--- a/src/boards/RIGv2/Sensors/SensorsData.h
+++ b/src/boards/RIGv2/Sensors/SensorsData.h
@@ -24,100 +24,89 @@
#include <sensors/ADS131M08/ADS131M08Data.h>
#include <sensors/MAX31856/MAX31856Data.h>
+#include <sensors/SensorData.h>
namespace RIGv2
{
-struct ADCsData : Boardcore::ADS131M08Data
-{
- uint8_t adcNumber = 0;
-
- ADCsData() : ADS131M08Data{}, 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)
- : ADS131M08Data{time, ch1, ch2, ch3, ch4, ch5, ch6, ch7, ch8},
- adcNumber{num}
+struct ADC1Data : Boardcore::ADS131M08Data
+{
+ explicit ADC1Data(const Boardcore::ADS131M08Data &data)
+ : Boardcore::ADS131M08Data(data)
{
}
- static std::string header()
- {
- return "timestamp,adcNumber,voltage_channel_1,voltage_channel_2,"
- "voltage_channel_3,voltage_channel_4,voltage_channel_5,voltage_"
- "channel_6,voltage_channel_7,voltage_channel_8\n";
- }
+ ADC1Data() {}
+};
- void print(std::ostream& os) const
+struct TC1Data : Boardcore::MAX31856Data
+{
+ explicit TC1Data(const Boardcore::MAX31856Data &data)
+ : Boardcore::MAX31856Data(data)
{
- os << timestamp << "," << (int)adcNumber << "," << voltage[0] << ","
- << voltage[1] << "," << voltage[2] << "," << voltage[3] << ","
- << voltage[4] << "," << voltage[5] << "," << voltage[6] << ","
- << voltage[7] << "\n";
}
+
+ TC1Data() {}
};
-struct TCsData : Boardcore::MAX31856Data
+struct VesselWeightData : Boardcore::LoadCellData
{
- uint8_t tcNumber = 0;
-
- TCsData() : MAX31856Data{}, tcNumber{0} {}
-
- TCsData(uint64_t time, uint8_t num, float temperature,
- float coldJunctionTemperature)
- : MAX31856Data{time, temperature, coldJunctionTemperature}, tcNumber{
- num}
+ explicit VesselWeightData(const Boardcore::LoadCellData &data)
+ : Boardcore::LoadCellData(data)
{
}
- static std::string header()
- {
- return "timestamp,tcNumber,temperature,"
- "coldJunctionTemperature\n";
- }
+ VesselWeightData() {}
+};
- void print(std::ostream& os) const
+struct TankWeightData : Boardcore::LoadCellData
+{
+ explicit TankWeightData(const Boardcore::LoadCellData &data)
+ : Boardcore::LoadCellData(data)
{
- os << temperatureTimestamp << "," << (int)tcNumber << "," << temperature
- << "," << coldJunctionTemperature << "\n";
}
+
+ TankWeightData() {}
};
-struct LCsData : Boardcore::LoadCellData
+struct VesselPressureData : Boardcore::PressureData
{
- uint8_t lcNumber = 0;
-
- LCsData() : LoadCellData{}, lcNumber{0} {}
-
- LCsData(uint64_t time, uint8_t num, float load)
- : LoadCellData{time, load}, lcNumber{num}
+ explicit VesselPressureData(const Boardcore::PressureData &data)
+ : Boardcore::PressureData(data)
{
}
- static std::string header() { return "timestamp,lcNumber,load\n"; }
+ VesselPressureData() {}
+};
- void print(std::ostream& os) const
+struct FillingPressureData : Boardcore::PressureData
+{
+ explicit FillingPressureData(const Boardcore::PressureData &data)
+ : Boardcore::PressureData(data)
{
- os << loadTimestamp << "," << (int)lcNumber << "," << load << "\n";
}
+
+ FillingPressureData() {}
};
-struct PTsData : Boardcore::PressureData
+struct TopTankPressureData : Boardcore::PressureData
{
- uint8_t ptNumber = 0;
-
- PTsData() : PressureData{}, ptNumber{0} {}
-
- PTsData(uint64_t time, uint8_t num, float pressure)
- : PressureData{time, pressure}, ptNumber{num}
+ explicit TopTankPressureData(const Boardcore::PressureData &data)
+ : Boardcore::PressureData(data)
{
}
- static std::string header() { return "timestamp,ptNumber,pressure\n"; }
+ TopTankPressureData() {}
+};
- void print(std::ostream& os) const
+struct BottomTankPressureData : Boardcore::PressureData
+{
+ explicit BottomTankPressureData(const Boardcore::PressureData &data)
+ : Boardcore::PressureData(data)
{
- os << pressureTimestamp << "," << (int)ptNumber << "," << pressure
- << "\n";
}
+
+ BottomTankPressureData() {}
};
+
} // namespace RIGv2
\ 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 d73867b330ead6abc0a813f84e711b8a506e2c25..b4024646faeb72a017f1c1c0bc206f96a899d148 100644
--- a/src/boards/RIGv2/StateMachines/GroundModeManager/GroundModeManager.cpp
+++ b/src/boards/RIGv2/StateMachines/GroundModeManager/GroundModeManager.cpp
@@ -22,7 +22,6 @@
#include "GroundModeManager.h"
-#include <RIGv2/Configs/GmmConfig.h>
#include <RIGv2/Configs/SchedulerConfig.h>
#include <common/Events.h>
#include <drivers/timer/TimestampTimer.h>
@@ -48,13 +47,13 @@ void GroundModeManager::setIgnitionTime(uint32_t time)
getModule<Registry>()->setUnsafe(CONFIG_ID_IGNITION_TIME, time);
}
-Boardcore::State GroundModeManager::state_idle(const Boardcore::Event &event)
+State GroundModeManager::state_idle(const Event &event)
{
switch (event)
{
case EV_ENTRY:
{
- updateAndLogStatus(GMM_STATE_IDLE);
+ updateAndLogStatus(GroundModeManagerState::IDLE);
return HANDLED;
}
@@ -86,13 +85,13 @@ Boardcore::State GroundModeManager::state_idle(const Boardcore::Event &event)
}
}
-Boardcore::State GroundModeManager::state_init(const Boardcore::Event &event)
+State GroundModeManager::state_init(const Event &event)
{
switch (event)
{
case EV_ENTRY:
{
- updateAndLogStatus(GMM_STATE_INIT);
+ updateAndLogStatus(GroundModeManagerState::INIT);
return HANDLED;
}
@@ -128,14 +127,13 @@ Boardcore::State GroundModeManager::state_init(const Boardcore::Event &event)
}
}
-Boardcore::State GroundModeManager::state_init_error(
- const Boardcore::Event &event)
+State GroundModeManager::state_init_error(const Event &event)
{
switch (event)
{
case EV_ENTRY:
{
- updateAndLogStatus(GMM_STATE_INIT_ERR);
+ updateAndLogStatus(GroundModeManagerState::INIT_ERR);
return HANDLED;
}
@@ -166,14 +164,13 @@ Boardcore::State GroundModeManager::state_init_error(
}
}
-Boardcore::State GroundModeManager::state_disarmed(
- const Boardcore::Event &event)
+State GroundModeManager::state_disarmed(const Event &event)
{
switch (event)
{
case EV_ENTRY:
{
- updateAndLogStatus(GMM_STATE_DISARMED);
+ updateAndLogStatus(GroundModeManagerState::DISARMED);
getModule<Actuators>()->armLightOff();
getModule<Registry>()->disarm();
getModule<CanHandler>()->sendEvent(CanConfig::EventId::DISARM);
@@ -209,8 +206,6 @@ Boardcore::State GroundModeManager::state_disarmed(
case TMTC_CALIBRATE:
{
getModule<Sensors>()->calibrate();
-
- // TODO(davide.mor): Also send CAN command
return HANDLED;
}
@@ -221,13 +216,13 @@ Boardcore::State GroundModeManager::state_disarmed(
}
}
-Boardcore::State GroundModeManager::state_armed(const Boardcore::Event &event)
+State GroundModeManager::state_armed(const Event &event)
{
switch (event)
{
case EV_ENTRY:
{
- updateAndLogStatus(GMM_STATE_ARMED);
+ updateAndLogStatus(GroundModeManagerState::ARMED);
getModule<Registry>()->arm();
getModule<CanHandler>()->sendEvent(CanConfig::EventId::ARM);
getModule<Actuators>()->armLightOn();
@@ -276,13 +271,13 @@ Boardcore::State GroundModeManager::state_armed(const Boardcore::Event &event)
}
}
-Boardcore::State GroundModeManager::state_firing(const Boardcore::Event &event)
+State GroundModeManager::state_firing(const Event &event)
{
switch (event)
{
case EV_ENTRY:
{
- updateAndLogStatus(GMM_STATE_FIRING);
+ updateAndLogStatus(GroundModeManagerState::FIRING);
return HANDLED;
}
@@ -320,7 +315,6 @@ Boardcore::State GroundModeManager::state_firing(const Boardcore::Event &event)
case MOTOR_COOLING_TIMEOUT: // Normal firing end
case TMTC_DISARM: // Abort signal
{
- // TODO(davide.mor): Set this to a sensible time
getModule<Actuators>()->openNitrogenWithTime(
Config::GroundModeManager::NITROGEN_TIME);
@@ -334,14 +328,13 @@ Boardcore::State GroundModeManager::state_firing(const Boardcore::Event &event)
}
}
-Boardcore::State GroundModeManager::state_igniting(
- const Boardcore::Event &event)
+State GroundModeManager::state_igniting(const Event &event)
{
switch (event)
{
case EV_ENTRY:
{
- updateAndLogStatus(GMM_STATE_IGNITING);
+ updateAndLogStatus(GroundModeManagerState::IGNITING);
// Start ignition
getModule<Actuators>()->igniterOn();
@@ -386,14 +379,13 @@ Boardcore::State GroundModeManager::state_igniting(
}
}
-Boardcore::State GroundModeManager::state_oxidizer(
- const Boardcore::Event &event)
+State GroundModeManager::state_oxidizer(const Event &event)
{
switch (event)
{
case EV_ENTRY:
{
- updateAndLogStatus(GMM_STATE_OXIDIZER);
+ updateAndLogStatus(GroundModeManagerState::OXIDIZER);
getModule<Actuators>()->openServo(ServosList::MAIN_VALVE);
@@ -427,13 +419,13 @@ Boardcore::State GroundModeManager::state_oxidizer(
}
}
-Boardcore::State GroundModeManager::state_cooling(const Boardcore::Event &event)
+State GroundModeManager::state_cooling(const Event &event)
{
switch (event)
{
case EV_ENTRY:
{
- updateAndLogStatus(GMM_STATE_COOLING);
+ updateAndLogStatus(GroundModeManagerState::COOLING);
coolingDelayEventId = EventBroker::getInstance().postDelayed(
MOTOR_COOLING_TIMEOUT, TOPIC_MOTOR,
diff --git a/src/boards/RIGv2/StateMachines/GroundModeManager/GroundModeManager.h b/src/boards/RIGv2/StateMachines/GroundModeManager/GroundModeManager.h
index 6b493ee7403465319dd3c8c0f726ac76c49f158d..1825140098b63349c350d09d42a3ef27bd5dbb1e 100644
--- a/src/boards/RIGv2/StateMachines/GroundModeManager/GroundModeManager.h
+++ b/src/boards/RIGv2/StateMachines/GroundModeManager/GroundModeManager.h
@@ -22,10 +22,9 @@
#pragma once
-#include <RIGv2/Configs/GmmConfig.h>
-// #include <RIGv2/StateMachines/GroundModeManager/GroundModeManagerData.h>
#include <RIGv2/Actuators/Actuators.h>
#include <RIGv2/CanHandler/CanHandler.h>
+#include <RIGv2/Configs/GMMConfig.h>
#include <RIGv2/Registry/Registry.h>
#include <RIGv2/Sensors/Sensors.h>
#include <diagnostic/PrintLogger.h>
@@ -68,7 +67,7 @@ private:
Boardcore::Logger &sdLogger = Boardcore::Logger::getInstance();
Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("gmm");
- std::atomic<GroundModeManagerState> state{GMM_STATE_IDLE};
+ std::atomic<GroundModeManagerState> state{GroundModeManagerState::IDLE};
uint16_t openOxidantDelayEventId = -1;
uint16_t coolingDelayEventId = -1;
diff --git a/src/boards/RIGv2/StateMachines/GroundModeManager/GroundModeManagerData.h b/src/boards/RIGv2/StateMachines/GroundModeManager/GroundModeManagerData.h
index 9589d9af8091f00be1b160161f8d9a1191e00776..864f5c5832e137bd5fbfa55f9f5b6b489952afd5 100644
--- a/src/boards/RIGv2/StateMachines/GroundModeManager/GroundModeManagerData.h
+++ b/src/boards/RIGv2/StateMachines/GroundModeManager/GroundModeManagerData.h
@@ -31,16 +31,16 @@ namespace RIGv2
enum GroundModeManagerState : uint8_t
{
- GMM_STATE_IDLE = 0,
- GMM_STATE_INIT,
- GMM_STATE_INIT_ERR,
- GMM_STATE_DISARMED,
- GMM_STATE_ARMED,
- GMM_STATE_FIRING,
- GMM_STATE_IGNITING,
- GMM_STATE_OXIDIZER,
- GMM_STATE_COOLING,
- GMM_STATE_INVALID,
+ IDLE = 0,
+ INIT,
+ INIT_ERR,
+ DISARMED,
+ ARMED,
+ FIRING,
+ IGNITING,
+ OXIDIZER,
+ COOLING,
+ INVALID,
};
struct GroundModeManagerData
@@ -48,7 +48,9 @@ struct GroundModeManagerData
uint64_t timestamp;
GroundModeManagerState state;
- GroundModeManagerData() : timestamp{0}, state{GMM_STATE_IDLE} {}
+ GroundModeManagerData() : timestamp{0}, state{GroundModeManagerState::IDLE}
+ {
+ }
GroundModeManagerData(uint64_t timestamp, GroundModeManagerState state)
: timestamp{timestamp}, state{state}
diff --git a/src/boards/RIGv2/StateMachines/TARS1/TARS1.cpp b/src/boards/RIGv2/StateMachines/TARS1/TARS1.cpp
index fb467932e91413c7259cf0f9578655539a149373..b48d54609fa77c55eee6ba4db38dc9ea518dc0c3 100644
--- a/src/boards/RIGv2/StateMachines/TARS1/TARS1.cpp
+++ b/src/boards/RIGv2/StateMachines/TARS1/TARS1.cpp
@@ -24,9 +24,8 @@
#include <RIGv2/Configs/SchedulerConfig.h>
#include <common/Events.h>
-#include <events/EventBroker.h>
-// TODO(davide.mor): Remove TimestampTimer
#include <drivers/timer/TimestampTimer.h>
+#include <events/EventBroker.h>
using namespace Boardcore;
using namespace RIGv2;
@@ -71,7 +70,7 @@ void TARS1::state_ready(const Event& event)
{
case EV_ENTRY:
{
- logAction(TARS_ACTION_READY);
+ logAction(TarsActionType::READY);
break;
}
@@ -97,13 +96,12 @@ void TARS1::state_refueling(const Event& event)
currentMass = 0.0f;
previousPressure = 0.0f;
currentPressure = 0.0f;
- // TODO(davide.mor): Add the rest
// First close all valves
actuators->closeAllServos();
LOG_INFO(logger, "TARS start washing");
- logAction(TARS_ACTION_WASHING);
+ logAction(TarsActionType::WASHING);
// Start washing
actuators->openServoWithTime(ServosList::VENTING_VALVE,
@@ -126,7 +124,7 @@ void TARS1::state_refueling(const Event& event)
case TARS_WASHING_DONE:
{
LOG_INFO(logger, "TARS washing done");
- logAction(TARS_ACTION_OPEN_FILLING);
+ logAction(TarsActionType::OPEN_FILLING);
// Open the filling for a long time
actuators->openServoWithTime(ServosList::FILLING_VALVE,
@@ -142,7 +140,7 @@ void TARS1::state_refueling(const Event& event)
case TARS_PRESSURE_STABILIZED:
{
LOG_INFO(logger, "TARS check mass");
- logAction(TARS_ACTION_CHECK_MASS);
+ logAction(TarsActionType::CHECK_MASS);
// Lock in a new mass value
{
@@ -175,7 +173,7 @@ void TARS1::state_refueling(const Event& event)
}
LOG_INFO(logger, "TARS open venting");
- logAction(TARS_ACTION_OPEN_VENTING);
+ logAction(TarsActionType::OPEN_VENTING);
// Open the venting and check for pressure stabilization
actuators->openServo(ServosList::VENTING_VALVE);
@@ -193,7 +191,7 @@ void TARS1::state_refueling(const Event& event)
case TARS_CHECK_PRESSURE_STABILIZE:
{
LOG_INFO(logger, "TARS check pressure");
- logAction(TARS_ACTION_CHECK_PRESSURE);
+ logAction(TarsActionType::CHECK_PRESSURE);
{
Lock<FastMutex> lock(sampleMutex);
@@ -223,7 +221,7 @@ void TARS1::state_refueling(const Event& event)
case TARS_FILLING_DONE:
{
LOG_INFO(logger, "TARS filling done");
- logAction(TARS_ACTION_AUTOMATIC_STOP);
+ logAction(TarsActionType::AUTOMATIC_STOP);
actuators->closeAllServos();
transition(&TARS1::state_ready);
@@ -233,7 +231,7 @@ void TARS1::state_refueling(const Event& event)
case MOTOR_MANUAL_ACTION:
{
LOG_INFO(logger, "TARS manual stop");
- logAction(TARS_ACTION_MANUAL_STOP);
+ logAction(TarsActionType::MANUAL_STOP);
// Disable next event
EventBroker::getInstance().removeDelayed(nextDelayedEventId);
@@ -244,7 +242,7 @@ void TARS1::state_refueling(const Event& event)
case MOTOR_START_TARS:
{
LOG_INFO(logger, "TARS manual stop");
- logAction(TARS_ACTION_MANUAL_STOP);
+ logAction(TarsActionType::MANUAL_STOP);
// The user requested that we stop
getModule<Actuators>()->closeAllServos();
@@ -259,8 +257,8 @@ void TARS1::sample()
{
Sensors* sensors = getModule<Sensors>();
- pressureFilter.add(sensors->getBottomTankPress().pressure);
- massFilter.add(sensors->getTankWeight().load);
+ pressureFilter.add(sensors->getBottomTankPressLastSample().pressure);
+ massFilter.add(sensors->getTankWeightLastSample().load);
medianSamples++;
if (medianSamples == Config::TARS1::MEDIAN_SAMPLE_NUMBER)
diff --git a/src/boards/RIGv2/StateMachines/TARS1/TARS1Data.h b/src/boards/RIGv2/StateMachines/TARS1/TARS1Data.h
index 2842bea144478276e17f665598cc2b29285fd5f3..f3d9a2005a54591142c6f25fd8c2e8a86c600179 100644
--- a/src/boards/RIGv2/StateMachines/TARS1/TARS1Data.h
+++ b/src/boards/RIGv2/StateMachines/TARS1/TARS1Data.h
@@ -29,16 +29,16 @@
namespace RIGv2
{
-enum TarsActionType : uint8_t
+enum class TarsActionType : uint8_t
{
- TARS_ACTION_READY = 0,
- TARS_ACTION_WASHING,
- TARS_ACTION_OPEN_FILLING,
- TARS_ACTION_OPEN_VENTING,
- TARS_ACTION_CHECK_PRESSURE,
- TARS_ACTION_CHECK_MASS,
- TARS_ACTION_AUTOMATIC_STOP,
- TARS_ACTION_MANUAL_STOP,
+ READY = 0,
+ WASHING,
+ OPEN_FILLING,
+ OPEN_VENTING,
+ CHECK_PRESSURE,
+ CHECK_MASS,
+ AUTOMATIC_STOP,
+ MANUAL_STOP,
};
struct TarsActionData
@@ -46,7 +46,7 @@ struct TarsActionData
uint64_t timestamp;
TarsActionType action;
- TarsActionData() : timestamp{0}, action{TARS_ACTION_READY} {}
+ TarsActionData() : timestamp{0}, action{TarsActionType::READY} {}
TarsActionData(uint64_t timestamp, TarsActionType action)
: timestamp{timestamp}, action{action}