diff --git a/src/boards/RIGv2/Actuators/Actuators.cpp b/src/boards/RIGv2/Actuators/Actuators.cpp
index bf8f2a84e725a0f878bdd1e677ea7623b4061473..012ceed80a22fac489a08632007a3f6f28ffb711 100644
--- a/src/boards/RIGv2/Actuators/Actuators.cpp
+++ b/src/boards/RIGv2/Actuators/Actuators.cpp
@@ -227,6 +227,8 @@ Actuators::Actuators()
info->unsafeSetServoPosition(0.0f);
}
+bool Actuators::isStarted() { return started; }
+
bool Actuators::start()
{
TaskScheduler &scheduler =
@@ -243,16 +245,17 @@ bool Actuators::start()
infos[8].servo->enable();
infos[9].servo->enable();
- updatePositionTaskId =
+ uint8_t result =
scheduler.addTask([this]() { updatePositionsTask(); },
Config::Servos::SERVO_TIMINGS_CHECK_PERIOD);
- if (updatePositionTaskId == 0)
+ if (result == 0)
{
LOG_ERR(logger, "Failed to add updatePositionsTask");
return false;
}
+ started = true;
return true;
}
diff --git a/src/boards/RIGv2/Actuators/Actuators.h b/src/boards/RIGv2/Actuators/Actuators.h
index cda786c3990a2a02a783c12e75adbc55b50ccbdb..630a06be52c9a545c399895ec00284cc1b0ff915 100644
--- a/src/boards/RIGv2/Actuators/Actuators.h
+++ b/src/boards/RIGv2/Actuators/Actuators.h
@@ -80,6 +80,8 @@ public:
[[nodiscard]] bool start();
+ bool isStarted();
+
bool wiggleServo(ServosList servo);
bool toggleServo(ServosList servo);
bool openServo(ServosList servo);
@@ -120,7 +122,7 @@ private:
Boardcore::Logger &sdLogger = Boardcore::Logger::getInstance();
Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("actuators");
- size_t updatePositionTaskId = 0;
+ std::atomic<bool> started{false};
miosix::FastMutex infosMutex;
ServoInfo infos[10] = {};
diff --git a/src/boards/RIGv2/BoardScheduler.h b/src/boards/RIGv2/BoardScheduler.h
index 39e1a5d1b4481c4a85038fea9334366ed40e7d46..bea27fb47284c64d222ea827363ef3b4f0637cbd 100644
--- a/src/boards/RIGv2/BoardScheduler.h
+++ b/src/boards/RIGv2/BoardScheduler.h
@@ -38,7 +38,25 @@ public:
{
}
- [[nodiscard]] bool start() { return tars1.start() && sensors.start(); }
+ [[nodiscard]] bool start()
+ {
+ if (!tars1.start())
+ {
+ LOG_ERR(logger, "Failed to start TARS1 scheduler");
+ return false;
+ }
+
+ if (!sensors.start())
+ {
+ LOG_ERR(logger, "Failed to start sensors scheduler");
+ return false;
+ }
+
+ started = true;
+ return true;
+ }
+
+ bool isStarted() { return started; }
Boardcore::TaskScheduler &getTars1Scheduler() { return tars1; }
@@ -49,6 +67,11 @@ public:
Boardcore::TaskScheduler &getCanBusScheduler() { return sensors; }
private:
+ Boardcore::PrintLogger logger =
+ Boardcore::Logging::getLogger("boardscheduler");
+
+ std::atomic<bool> started{false};
+
Boardcore::TaskScheduler tars1;
Boardcore::TaskScheduler sensors;
};
diff --git a/src/boards/RIGv2/CanHandler/CanHandler.cpp b/src/boards/RIGv2/CanHandler/CanHandler.cpp
index 1f991c4b86bab8d184e8c5c74d83927822c6fa3d..6b2a9d7dd8020c365616a99d64a3303fca758a25 100644
--- a/src/boards/RIGv2/CanHandler/CanHandler.cpp
+++ b/src/boards/RIGv2/CanHandler/CanHandler.cpp
@@ -23,7 +23,6 @@
#include "CanHandler.h"
#include <RIGv2/Actuators/Actuators.h>
-#include <RIGv2/Configs/CanHandlerConfig.h>
#include <RIGv2/Configs/SchedulerConfig.h>
#include <RIGv2/StateMachines/GroundModeManager/GroundModeManager.h>
@@ -51,6 +50,8 @@ CanHandler::CanHandler()
static_cast<uint8_t>(CanConfig::Board::BROADCAST));
}
+bool CanHandler::isStarted() { return started; }
+
bool CanHandler::start()
{
driver.init();
@@ -61,7 +62,20 @@ bool CanHandler::start()
uint8_t result = scheduler.addTask([this]() { periodicMessage(); },
Config::CanHandler::STATUS_PERIOD);
- return result != 0 && protocol.start();
+ if (result != 0)
+ {
+ LOG_ERR(logger, "Failed to add periodicMessageTask");
+ return false;
+ }
+
+ if (!protocol.start())
+ {
+ LOG_ERR(logger, "Failed to start CanProtocol");
+ return false;
+ }
+
+ started = true;
+ return true;
}
void CanHandler::sendEvent(CanConfig::EventId event)
@@ -158,32 +172,44 @@ void CanHandler::handleSensor(const Boardcore::Canbus::CanMessage &msg)
{
case CanConfig::SensorId::CC_PRESSURE:
{
- PressureData data = pressureDataFromCanMessage(msg);
+ CanPressureData data = pressureDataFromCanMessage(msg);
+ sdLogger.log(data);
sensors->setCanCCPress(data);
break;
}
case CanConfig::SensorId::BOTTOM_TANK_PRESSURE:
{
- PressureData data = pressureDataFromCanMessage(msg);
+ CanPressureData data = pressureDataFromCanMessage(msg);
+ sdLogger.log(data);
sensors->setCanBottomTankPress(data);
break;
}
case CanConfig::SensorId::TOP_TANK_PRESSURE:
{
- PressureData data = pressureDataFromCanMessage(msg);
+ CanPressureData data = pressureDataFromCanMessage(msg);
+ sdLogger.log(data);
sensors->setCanTopTankPress(data);
break;
}
case CanConfig::SensorId::TANK_TEMPERATURE:
{
- TemperatureData data = temperatureDataFromCanMessage(msg);
+ CanTemperatureData data = temperatureDataFromCanMessage(msg);
+ sdLogger.log(data);
sensors->setCanTankTemp(data);
break;
}
+ case CanConfig::SensorId::MOTOR_BOARD_VOLTAGE:
+ {
+ CanVoltageData data = voltageDataFromCanMessage(msg);
+ sdLogger.log(data);
+ sensors->setCanMotorBatteryVoltage(data);
+ break;
+ }
+
default:
{
LOG_WARN(logger, "Received unsupported sensor data: {}", sensor);
@@ -196,6 +222,8 @@ void CanHandler::handleActuator(const Boardcore::Canbus::CanMessage &msg)
ServosList servo = static_cast<ServosList>(msg.getSecondaryType());
ServoData data = servoDataFromCanMessage(msg);
+ // TODO: Update with new message
+
getModule<Actuators>()->setCanServoAperture(servo, data.position);
}
@@ -203,34 +231,41 @@ void CanHandler::handleStatus(const Boardcore::Canbus::CanMessage &msg)
{
CanConfig::Board source = static_cast<CanConfig::Board>(msg.getSource());
- bool armed = msg.payload[0] != 0;
+ bool armed = (msg.payload[0] >> 8) != 0;
+ uint8_t state = msg.payload[0] & 0xff;
Lock<FastMutex> lock{statusMutex};
+ // TODO: Update with new message
+
switch (source)
{
case CanConfig::Board::MAIN:
{
- status.mainCounter = 4;
- status.mainArmed = armed;
+ status.mainLastStatus = getTime();
+ status.mainArmed = armed;
+ status.mainState = state;
break;
}
case CanConfig::Board::PAYLOAD:
{
- status.payloadCounter = 4;
- status.payloadArmed = armed;
+ status.payloadLastStatus = getTime();
+ status.payloadArmed = armed;
+ status.payloadState = state;
break;
}
case CanConfig::Board::MOTOR:
{
- status.motorCounter = 4;
+ status.motorLastStatus = getTime();
+ status.motorState = state;
break;
}
default:
{
+ LOG_WARN(logger, "Received unsupported status: {}", source);
}
}
}
@@ -239,16 +274,13 @@ void CanHandler::periodicMessage()
{
GroundModeManagerState state = getModule<GroundModeManager>()->getState();
+ uint64_t payload =
+ static_cast<uint8_t>(state) |
+ ((state == GroundModeManagerState::GMM_STATE_ARMED) ? 1 : 0) << 8;
+
protocol.enqueueSimplePacket(
static_cast<uint8_t>(CanConfig::Priority::MEDIUM),
static_cast<uint8_t>(CanConfig::PrimaryType::STATUS),
static_cast<uint8_t>(CanConfig::Board::RIG),
- static_cast<uint8_t>(CanConfig::Board::BROADCAST), 0x00,
- (state == GroundModeManagerState::GMM_STATE_ARMED) ? 1 : 0);
-
- // Update status counters
- Lock<FastMutex> lock{statusMutex};
- status.mainCounter = std::max(0, status.mainCounter - 1);
- status.payloadCounter = std::max(0, status.payloadCounter - 1);
- status.motorCounter = std::max(0, status.motorCounter - 1);
+ static_cast<uint8_t>(CanConfig::Board::BROADCAST), 0x00, payload);
}
\ No newline at end of file
diff --git a/src/boards/RIGv2/CanHandler/CanHandler.h b/src/boards/RIGv2/CanHandler/CanHandler.h
index 0a0ef95da21c302e8199ceef2e6d3c9be23b3b50..68f989d224d67a15769dafa92722cd2f4049808e 100644
--- a/src/boards/RIGv2/CanHandler/CanHandler.h
+++ b/src/boards/RIGv2/CanHandler/CanHandler.h
@@ -23,6 +23,7 @@
#pragma once
#include <RIGv2/BoardScheduler.h>
+#include <RIGv2/Configs/CanHandlerConfig.h>
#include <RIGv2/Sensors/Sensors.h>
#include <common/CanConfig.h>
#include <common/Mavlink.h>
@@ -42,21 +43,41 @@ class CanHandler
public:
struct CanStatus
{
- int mainCounter = 0;
- int payloadCounter = 0;
- int motorCounter = 0;
+ long long mainLastStatus = 0;
+ long long payloadLastStatus = 0;
+ long long motorLastStatus = 0;
+
+ uint8_t mainState = 0;
+ uint8_t payloadState = 0;
+ uint8_t motorState = 0;
bool mainArmed = false;
bool payloadArmed = false;
- bool isMainConnected() { return mainCounter == 0; }
-
- bool isPayloadConnected() { return payloadCounter == 0; }
-
- bool isMotorConnected() { return motorCounter == 0; }
+ bool isMainConnected()
+ {
+ return miosix::getTime() <=
+ (mainLastStatus +
+ Config::CanHandler::STATUS_TIMEOUT.count());
+ }
+ bool isPayloadConnected()
+ {
+ return miosix::getTime() <=
+ (payloadLastStatus +
+ Config::CanHandler::STATUS_TIMEOUT.count());
+ }
+ bool isMotorConnected()
+ {
+ return miosix::getTime() <=
+ (motorLastStatus +
+ Config::CanHandler::STATUS_TIMEOUT.count());
+ }
+
+ uint8_t getMainState() { return mainState; }
+ uint8_t getPayloadState() { return payloadState; }
+ uint8_t getMotorState() { return motorState; }
bool isMainArmed() { return mainArmed; }
-
bool isPayloadArmed() { return payloadArmed; }
};
@@ -83,13 +104,16 @@ private:
void periodicMessage();
+ Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("canhandler");
+ Boardcore::Logger &sdLogger = Boardcore::Logger::getInstance();
+
+ std::atomic<bool> started{false};
+
Boardcore::Canbus::CanbusDriver driver;
Boardcore::Canbus::CanProtocol protocol;
miosix::FastMutex statusMutex;
CanStatus status;
-
- Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("canhandler");
};
} // namespace RIGv2
\ No newline at end of file
diff --git a/src/boards/RIGv2/Configs/ActuatorsConfig.h b/src/boards/RIGv2/Configs/ActuatorsConfig.h
index b8aff726b5dbb117ce69fd2a7f768f86e56b13c0..e6e286ee323d8abe90058f9d4db76c102fe33388 100644
--- a/src/boards/RIGv2/Configs/ActuatorsConfig.h
+++ b/src/boards/RIGv2/Configs/ActuatorsConfig.h
@@ -23,6 +23,7 @@
#pragma once
#include <interfaces-impl/hwmapping.h>
+#include <units/Frequency.h>
namespace RIGv2
{
@@ -32,44 +33,46 @@ namespace Config
namespace Servos
{
+using namespace Boardcore::Units::Frequency;
+
// Generic pulse width for all servos
-static constexpr unsigned int MIN_PULSE = 500;
-static constexpr unsigned int MAX_PULSE = 2460;
+constexpr unsigned int MIN_PULSE = 500;
+constexpr unsigned int MAX_PULSE = 2460;
// Pulse width specific to SERVO 2 (disconnect servo)
// TODO(davide.mor): This actually needs tweaking
-static constexpr unsigned int SERVO2_MIN_PULSE = 900;
-static constexpr unsigned int SERVO2_MAX_PULSE = 2100;
-
-static constexpr unsigned int FREQUENCY = 333;
-
-static constexpr uint32_t SERVO_TIMINGS_CHECK_PERIOD = 100;
-static constexpr long long SERVO_CONFIDENCE_TIME = 500; // 0.5s
-static constexpr float SERVO_CONFIDENCE = 0.02; // 2%
-
-static constexpr uint32_t DEFAULT_FILLING_OPENING_TIME = 15000; // 15s
-static constexpr uint32_t DEFAULT_VENTING_OPENING_TIME = 15000; // 15s
-static constexpr uint32_t DEFAULT_MAIN_OPENING_TIME = 6000; // 6s
-static constexpr uint32_t DEFAULT_RELEASE_OPENING_TIME = 10000; // 10s
-static constexpr uint32_t DEFAULT_DISCONNECT_OPENING_TIME = 2000; // 2s
-
-static constexpr float DEFAULT_FILLING_MAX_APERTURE = 1.00f;
-static constexpr float DEFAULT_VENTING_MAX_APERTURE = 1.00f;
-static constexpr float DEFAULT_MAIN_MAX_APERTURE = 1.00f;
-static constexpr float DEFAULT_RELEASE_MAX_APERTURE = 1.00f;
-static constexpr float DEFAULT_DISCONNECT_MAX_APERTURE = 1.00f;
-
-static constexpr float FILLING_LIMIT = 0.90f;
-static constexpr float VENTING_LIMIT = 0.90f;
-static constexpr float MAIN_LIMIT = 0.90f;
-static constexpr float RELEASE_LIMIT = 0.50f;
-static constexpr float DISCONNECT_LIMIT = 1.00f;
-
-static constexpr bool FILLING_FLIPPED = true;
-static constexpr bool VENTING_FLIPPED = true;
-static constexpr bool MAIN_FLIPPED = true;
-static constexpr bool RELEASE_FLIPPED = true;
-static constexpr bool DISCONNECT_FLIPPED = false;
+constexpr unsigned int SERVO2_MIN_PULSE = 900;
+constexpr unsigned int SERVO2_MAX_PULSE = 2100;
+
+constexpr unsigned int FREQUENCY = 333;
+
+constexpr Hertz SERVO_TIMINGS_CHECK_PERIOD = 10_hz;
+constexpr long long SERVO_CONFIDENCE_TIME = 500; // 0.5s
+constexpr float SERVO_CONFIDENCE = 0.02; // 2%
+
+constexpr uint32_t DEFAULT_FILLING_OPENING_TIME = 15000; // 15s
+constexpr uint32_t DEFAULT_VENTING_OPENING_TIME = 15000; // 15s
+constexpr uint32_t DEFAULT_MAIN_OPENING_TIME = 6000; // 6s
+constexpr uint32_t DEFAULT_RELEASE_OPENING_TIME = 10000; // 10s
+constexpr uint32_t DEFAULT_DISCONNECT_OPENING_TIME = 2000; // 2s
+
+constexpr float DEFAULT_FILLING_MAX_APERTURE = 1.00f;
+constexpr float DEFAULT_VENTING_MAX_APERTURE = 1.00f;
+constexpr float DEFAULT_MAIN_MAX_APERTURE = 1.00f;
+constexpr float DEFAULT_RELEASE_MAX_APERTURE = 1.00f;
+constexpr float DEFAULT_DISCONNECT_MAX_APERTURE = 1.00f;
+
+constexpr float FILLING_LIMIT = 0.90f;
+constexpr float VENTING_LIMIT = 0.90f;
+constexpr float MAIN_LIMIT = 0.90f;
+constexpr float RELEASE_LIMIT = 0.50f;
+constexpr float DISCONNECT_LIMIT = 1.00f;
+
+constexpr bool FILLING_FLIPPED = true;
+constexpr bool VENTING_FLIPPED = true;
+constexpr bool MAIN_FLIPPED = true;
+constexpr bool RELEASE_FLIPPED = true;
+constexpr bool DISCONNECT_FLIPPED = false;
} // namespace Servos
diff --git a/src/boards/RIGv2/Configs/CanHandlerConfig.h b/src/boards/RIGv2/Configs/CanHandlerConfig.h
index fd08c6ca4d35d833b7d3797e8eebdeb228aed0e6..3a700c4a723ed4ab5901dc99f70551664b636e2f 100644
--- a/src/boards/RIGv2/Configs/CanHandlerConfig.h
+++ b/src/boards/RIGv2/Configs/CanHandlerConfig.h
@@ -35,7 +35,8 @@ namespace CanHandler
using namespace std::chrono_literals;
-static constexpr std::chrono::nanoseconds STATUS_PERIOD = 2s;
+static constexpr std::chrono::nanoseconds STATUS_PERIOD = 2s;
+static constexpr std::chrono::nanoseconds STATUS_TIMEOUT = 3s;
} // namespace CanHandler
diff --git a/src/boards/RIGv2/Configs/GmmConfig.h b/src/boards/RIGv2/Configs/GmmConfig.h
index e57ada74cf47c246d0cd3b71e02669e3c5632565..7656ea25d57bd4dd689bc43cffd0bf6b54ac399a 100644
--- a/src/boards/RIGv2/Configs/GmmConfig.h
+++ b/src/boards/RIGv2/Configs/GmmConfig.h
@@ -31,10 +31,10 @@ namespace Config
namespace GroundModeManager
{
-static constexpr uint32_t DEFAULT_IGNITION_WAITING_TIME = 3700; // [ms]
+constexpr uint32_t DEFAULT_IGNITION_WAITING_TIME = 3700; // [ms]
-static constexpr uint32_t MOTOR_COOLING_TIME = 1500;
-static constexpr uint32_t NITROGEN_TIME = 20000; // 20s
+constexpr uint32_t MOTOR_COOLING_TIME = 1500;
+constexpr uint32_t NITROGEN_TIME = 20000; // 20s
} // namespace GroundModeManager
} // namespace Config
diff --git a/src/boards/RIGv2/Configs/RadioConfig.h b/src/boards/RIGv2/Configs/RadioConfig.h
index 94b63d63370573fa8c83f9c8062c96d36f2ab480..b5e6b87c5a58f4114b47994aee90b7e1b66fc52f 100644
--- a/src/boards/RIGv2/Configs/RadioConfig.h
+++ b/src/boards/RIGv2/Configs/RadioConfig.h
@@ -33,19 +33,19 @@ namespace Config
namespace Radio
{
-static constexpr unsigned int MAV_OUT_QUEUE_SIZE = 20;
-static constexpr unsigned int MAV_MAX_LENGTH = MAVLINK_MAX_DIALECT_PAYLOAD_SIZE;
+constexpr unsigned int MAV_OUT_QUEUE_SIZE = 20;
+constexpr unsigned int MAV_MAX_LENGTH = MAVLINK_MAX_DIALECT_PAYLOAD_SIZE;
-static constexpr uint16_t MAV_SLEEP_AFTER_SEND = 0;
-static constexpr size_t MAV_OUT_BUFFER_MAX_AGE = 10;
+constexpr uint16_t MAV_SLEEP_AFTER_SEND = 0;
+constexpr size_t MAV_OUT_BUFFER_MAX_AGE = 10;
-static constexpr unsigned int CIRCULAR_BUFFER_SIZE = 20;
-static constexpr unsigned int MAX_PACKETS_PER_FLUSH = 4;
+constexpr unsigned int CIRCULAR_BUFFER_SIZE = 20;
+constexpr unsigned int MAX_PACKETS_PER_FLUSH = 4;
-static constexpr uint8_t MAV_SYSTEM_ID = 171;
-static constexpr uint8_t MAV_COMPONENT_ID = 96;
+constexpr uint8_t MAV_SYSTEM_ID = 171;
+constexpr uint8_t MAV_COMPONENT_ID = 96;
-static constexpr long long LAST_COMMAND_THRESHOLD = 300;
+constexpr long long LAST_COMMAND_THRESHOLD = 300;
} // namespace Radio
diff --git a/src/boards/RIGv2/Configs/SensorsConfig.h b/src/boards/RIGv2/Configs/SensorsConfig.h
index c25b47445dc60d327e79e43e405f7023b5d551b8..b638ad116f3bc53d547faaeccf08b03c233fff46 100644
--- a/src/boards/RIGv2/Configs/SensorsConfig.h
+++ b/src/boards/RIGv2/Configs/SensorsConfig.h
@@ -22,6 +22,11 @@
#pragma once
+#include <drivers/adc/InternalADC.h>
+#include <sensors/ADS131M08/ADS131M08.h>
+#include <sensors/MAX31856/MAX31856.h>
+#include <units/Frequency.h>
+
#include <cstdint>
namespace RIGv2
@@ -36,25 +41,27 @@ namespace Sensors
namespace ADS131M08
{
-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;
+using namespace Boardcore::Units::Frequency;
+
+constexpr float CH1_SHUNT_RESISTANCE = 29.4048;
+constexpr float CH2_SHUNT_RESISTANCE = 29.5830;
+constexpr float CH3_SHUNT_RESISTANCE = 29.4973;
+constexpr float CH4_SHUNT_RESISTANCE = 29.8849;
// ADC channels definitions for various sensors
-static constexpr Boardcore::ADS131M08Defs::Channel VESSEL_PT_CHANNEL =
+constexpr Boardcore::ADS131M08Defs::Channel VESSEL_PT_CHANNEL =
Boardcore::ADS131M08Defs::Channel::CHANNEL_0;
-static constexpr Boardcore::ADS131M08Defs::Channel FILLING_PT_CHANNEL =
+constexpr Boardcore::ADS131M08Defs::Channel FILLING_PT_CHANNEL =
Boardcore::ADS131M08Defs::Channel::CHANNEL_1;
-static constexpr Boardcore::ADS131M08Defs::Channel BOTTOM_PT_CHANNEL =
+constexpr Boardcore::ADS131M08Defs::Channel BOTTOM_PT_CHANNEL =
Boardcore::ADS131M08Defs::Channel::CHANNEL_2;
-static constexpr Boardcore::ADS131M08Defs::Channel TOP_PT_CHANNEL =
+constexpr Boardcore::ADS131M08Defs::Channel TOP_PT_CHANNEL =
Boardcore::ADS131M08Defs::Channel::CHANNEL_3;
-static constexpr Boardcore::ADS131M08Defs::Channel SERVO_CURRENT_CHANNEL =
+constexpr Boardcore::ADS131M08Defs::Channel SERVO_CURRENT_CHANNEL =
Boardcore::ADS131M08Defs::Channel::CHANNEL_4;
-static constexpr Boardcore::ADS131M08Defs::Channel VESSEL_LC_CHANNEL =
+constexpr Boardcore::ADS131M08Defs::Channel VESSEL_LC_CHANNEL =
Boardcore::ADS131M08Defs::Channel::CHANNEL_5;
-static constexpr Boardcore::ADS131M08Defs::Channel TANK_LC_CHANNEL =
+constexpr Boardcore::ADS131M08Defs::Channel TANK_LC_CHANNEL =
Boardcore::ADS131M08Defs::Channel::CHANNEL_6;
// Servo current sensor calibration data
@@ -65,71 +72,79 @@ static constexpr Boardcore::ADS131M08Defs::Channel TANK_LC_CHANNEL =
// - 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;
+constexpr float SERVO_CURRENT_SCALE = 4.5466;
+constexpr float SERVO_CURRENT_ZERO = 2.520 / SERVO_CURRENT_SCALE;
-static constexpr uint32_t SAMPLE_PERIOD = 10;
-static constexpr bool ENABLED = true;
+constexpr Hertz SAMPLE_PERIOD = 100_hz;
+constexpr bool ENABLED = true;
} // namespace ADS131M08
namespace MAX31856
{
-static constexpr uint32_t SAMPLE_PERIOD = 100;
-static constexpr bool ENABLED = true;
+
+using namespace Boardcore::Units::Frequency;
+
+constexpr Hertz SAMPLE_PERIOD = 10_hz;
+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
+
+using namespace Boardcore::Units::Frequency;
+
+constexpr float FILLING_SHUNT_RESISTANCE = ADS131M08::CH2_SHUNT_RESISTANCE;
+constexpr float TANK_TOP_SHUNT_RESISTANCE = ADS131M08::CH4_SHUNT_RESISTANCE;
+constexpr float TANK_BOTTOM_SHUNT_RESISTANCE = ADS131M08::CH3_SHUNT_RESISTANCE;
+constexpr float VESSEL_SHUNT_RESISTANCE = ADS131M08::CH1_SHUNT_RESISTANCE;
+
+constexpr float MIN_CURRENT = 4;
+constexpr float MAX_CURRENT = 20;
+
+constexpr float FILLING_MAX_PRESSURE = 100; // bar
+constexpr float TANK_TOP_MAX_PRESSURE = 100; // bar
+constexpr float TANK_BOTTOM_MAX_PRESSURE = 100; // bar
+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;
+
+using namespace Boardcore::Units::Frequency;
+
+constexpr unsigned int CALIBRATE_SAMPLE_COUNT = 10;
+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 TANK_P0_VOLTAGE = 0.000941;
-static constexpr float TANK_P0_MASS = 1.866;
-static constexpr float TANK_P1_VOLTAGE = 0.003559;
-static constexpr float TANK_P1_MASS = 6.916;
+constexpr float TANK_P0_VOLTAGE = 0.000941;
+constexpr float TANK_P0_MASS = 1.866;
+constexpr float TANK_P1_VOLTAGE = 0.003559;
+constexpr float TANK_P1_MASS = 6.916;
// LC Vessel sensor calibration data
// - 1.866kg V: 0.00027
// - 5.050kg V: 0.00073
// - 6.916kg V: 0.00100
-static constexpr float VESSEL_P0_VOLTAGE = 0.00027;
-static constexpr float VESSEL_P0_MASS = 1.866;
-static constexpr float VESSEL_P1_VOLTAGE = 0.0010;
-static constexpr float VESSEL_P1_MASS = 6.916;
+constexpr float VESSEL_P0_VOLTAGE = 0.00027;
+constexpr float VESSEL_P0_MASS = 1.866;
+constexpr float VESSEL_P1_VOLTAGE = 0.0010;
+constexpr float VESSEL_P1_MASS = 6.916;
} // namespace LoadCell
namespace InternalADC
{
-static constexpr Boardcore::InternalADC::Channel BATTERY_VOLTAGE_CHANNEL =
+
+using namespace Boardcore::Units::Frequency;
+
+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;
+constexpr float BATTERY_VOLTAGE_SCALE = 4.7917;
+constexpr Hertz SAMPLE_PERIOD = 10_hz;
+constexpr bool ENABLED = true;
} // namespace InternalADC
} // namespace Sensors
diff --git a/src/boards/RIGv2/Configs/TARS1Config.h b/src/boards/RIGv2/Configs/TARS1Config.h
index eda7d6fce80c58a5c40a4d8c4fa442fe3514c28b..a86becc3c76c28fe04bf7f4e48136c81e3b02594 100644
--- a/src/boards/RIGv2/Configs/TARS1Config.h
+++ b/src/boards/RIGv2/Configs/TARS1Config.h
@@ -22,6 +22,8 @@
#pragma once
+#include <units/Frequency.h>
+
#include <cstddef>
#include <cstdint>
@@ -31,20 +33,23 @@ namespace Config
{
namespace TARS1
{
-static constexpr uint32_t SAMPLE_PERIOD = 10;
-static constexpr size_t MEDIAN_SAMPLE_NUMBER = 10;
-static constexpr uint32_t WASHING_OPENING_TIME = 5000;
-static constexpr uint32_t WASHING_TIME_DELAY = 1000;
-static constexpr uint32_t FILLING_OPENING_TIME = 900000;
-static constexpr uint32_t PRESSURE_STABILIZE_WAIT_TIME = 1000;
+using namespace Boardcore::Units::Frequency;
+
+constexpr Hertz SAMPLE_PERIOD = 100_hz;
+constexpr size_t MEDIAN_SAMPLE_NUMBER = 10;
+
+constexpr uint32_t WASHING_OPENING_TIME = 5000;
+constexpr uint32_t WASHING_TIME_DELAY = 1000;
+constexpr uint32_t FILLING_OPENING_TIME = 900000;
+constexpr uint32_t PRESSURE_STABILIZE_WAIT_TIME = 1000;
-static constexpr int NUM_MASS_STABLE_ITERATIONS = 2;
+constexpr int NUM_MASS_STABLE_ITERATIONS = 2;
-static constexpr float MASS_TOLERANCE = 0.2; // [kg]
-static constexpr float PRESSURE_TOLERANCE = 0.035; // [bar]
+constexpr float MASS_TOLERANCE = 0.2; // [kg]
+constexpr float PRESSURE_TOLERANCE = 0.035; // [bar]
-static constexpr bool STOP_ON_MASS_STABILIZATION = false;
+constexpr bool STOP_ON_MASS_STABILIZATION = false;
} // namespace TARS1
} // namespace Config
} // namespace RIGv2
\ No newline at end of file
diff --git a/src/boards/RIGv2/Radio/Radio.cpp b/src/boards/RIGv2/Radio/Radio.cpp
index a563be212b623e0726196209b759bdfbb8e4af0b..8dee36f3ccdc400bd635a6e5fb792725ef797c96 100644
--- a/src/boards/RIGv2/Radio/Radio.cpp
+++ b/src/boards/RIGv2/Radio/Radio.cpp
@@ -139,7 +139,7 @@ void Radio::enqueueNack(const mavlink_message_t& msg)
mavlink_message_t nackMsg;
mavlink_msg_nack_tm_pack(Config::Radio::MAV_SYSTEM_ID,
Config::Radio::MAV_COMPONENT_ID, &nackMsg,
- msg.msgid, msg.seq);
+ msg.msgid, msg.seq, 0);
enqueuePacket(nackMsg);
}
@@ -444,8 +444,8 @@ bool Radio::enqueueSystemTm(uint8_t tmId)
mavlink_sensor_state_tm_t tm;
strcpy(tm.sensor_name, sensor.id.c_str());
- tm.state =
- (sensor.isInitialized ? 1 : 0) | (sensor.isEnabled ? 2 : 0);
+ tm.initialized = sensor.isInitialized ? 1 : 0;
+ tm.enabled = sensor.isEnabled ? 1 : 0;
mavlink_msg_sensor_state_tm_encode(
Config::Radio::MAV_SYSTEM_ID,
@@ -470,10 +470,12 @@ bool Radio::enqueueSystemTm(uint8_t tmId)
tm.timestamp = TimestampTimer::getTimestamp();
tm.logger = Logger::getInstance().isStarted() ? 1 : 0;
tm.event_broker = EventBroker::getInstance().isRunning() ? 1 : 0;
- // What? Why is this here? Of course the radio is started!
- tm.radio = isStarted() ? 1 : 0;
- tm.sensors = getModule<Sensors>()->isStarted() ? 1 : 0;
- tm.board_scheduler = 0; // TODO(davide.mor): No BoardScheduler yet
+ tm.radio = isStarted() ? 1 : 0;
+ tm.sensors = getModule<Sensors>()->isStarted() ? 1 : 0;
+ tm.actuators = getModule<Actuators>()->isStarted() ? 1 : 0;
+ tm.pin_handler = 0; // No pin_handler
+ tm.can_handler = getModule<CanHandler>()->isStarted() ? 1 : 0;
+ tm.scheduler = getModule<BoardScheduler>()->isStarted() ? 1 : 0;
mavlink_msg_sys_tm_encode(Config::Radio::MAV_SYSTEM_ID,
Config::Radio::MAV_COMPONENT_ID, &msg,
@@ -508,7 +510,7 @@ bool Radio::enqueueSystemTm(uint8_t tmId)
return true;
}
- case MAV_MAVLINK_STATS:
+ case MAV_MAVLINK_STATS_ID:
{
mavlink_message_t msg;
mavlink_mavlink_stats_tm_t tm;
@@ -541,15 +543,26 @@ bool Radio::enqueueSystemTm(uint8_t tmId)
mavlink_message_t msg;
mavlink_gse_tm_t tm = {0};
- Sensors* sensors = getModule<Sensors>();
- Actuators* actuators = getModule<Actuators>();
- CanHandler* canHandler = getModule<CanHandler>();
+ Sensors* sensors = getModule<Sensors>();
+ Actuators* actuators = getModule<Actuators>();
+
+ 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.umbilical_current_consumption =
+ sensors->getUmbilicalCurrent().current;
+
+ // Log data
+ tm.log_good = 0; // TODO
+ tm.log_number = 0; // TODO
- 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;
+ // Valve states
tm.filling_valve_state =
actuators->isServoOpen(ServosList::FILLING_VALVE) ? 1 : 0;
tm.venting_valve_state =
@@ -559,25 +572,24 @@ bool Radio::enqueueSystemTm(uint8_t tmId)
tm.main_valve_state =
actuators->isServoOpen(ServosList::MAIN_VALVE) ? 1 : 0;
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;
-
- // TODO(davide.mor): This should be updated with the new telemetry
- tm.main_board_status =
- canHandler->getCanStatus().isMainConnected() ? 1 : 0;
- tm.motor_board_status =
- canHandler->getCanStatus().isMotorConnected() ? 1 : 0;
- tm.payload_board_status =
- canHandler->getCanStatus().isPayloadConnected() ? 1 : 0;
- // TODO(davide.mor): Add the rest of these
-
- tm.battery_voltage = sensors->getBatteryVoltage().voltage;
- tm.current_consumption = sensors->getServoCurrent().current;
+ // Can data
+ CanHandler::CanStatus canStatus =
+ getModule<CanHandler>()->getCanStatus();
+ tm.main_board_state = canStatus.getMainState();
+ tm.payload_board_state = canStatus.getPayloadState();
+ tm.motor_board_state = canStatus.getMotorState();
+ tm.main_can_status = canStatus.isMainConnected() ? 1 : 0;
+ tm.payload_can_status = canStatus.isPayloadConnected() ? 1 : 0;
+ tm.motor_can_status = canStatus.isMotorConnected() ? 1 : 0;
mavlink_msg_gse_tm_encode(Config::Radio::MAV_SYSTEM_ID,
Config::Radio::MAV_COMPONENT_ID, &msg,
@@ -594,18 +606,27 @@ bool Radio::enqueueSystemTm(uint8_t tmId)
Sensors* sensors = getModule<Sensors>();
Actuators* actuators = getModule<Actuators>();
- tm.timestamp = TimestampTimer::getTimestamp();
- tm.tank_temperature = sensors->getTc1LastSample().temperature;
+ 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;
+
+ // Log data
+ tm.log_good = 0; // TODO
+ tm.log_number = 0; // TODO
+
+ // Valve states
tm.main_valve_state =
actuators->isCanServoOpen(ServosList::MAIN_VALVE) ? 1 : 0;
tm.venting_valve_state =
actuators->isCanServoOpen(ServosList::VENTING_VALVE) ? 1 : 0;
- // TODO(davide.mor): Add the rest of these
- tm.battery_voltage = 0.0f;
- tm.current_consumption = sensors->getUmbilicalCurrent().current;
+ // HIL metadata
+ tm.hil_state = 0; // TODO
mavlink_msg_motor_tm_encode(Config::Radio::MAV_SYSTEM_ID,
Config::Radio::MAV_COMPONENT_ID, &msg,
@@ -625,7 +646,7 @@ bool Radio::enqueueSensorTm(uint8_t tmId)
{
switch (tmId)
{
- case MAV_ADS_ID:
+ case MAV_ADS131M08_ID:
{
mavlink_message_t msg;
mavlink_adc_tm_t tm;
diff --git a/src/boards/RIGv2/Radio/Radio.h b/src/boards/RIGv2/Radio/Radio.h
index 0e8738bc29a2f4f6f8c01084d7f3d02a159d2111..0f24505b15c0a34bd1cf2d5b8e2680ba1d6cf512 100644
--- a/src/boards/RIGv2/Radio/Radio.h
+++ b/src/boards/RIGv2/Radio/Radio.h
@@ -23,6 +23,7 @@
#pragma once
#include <RIGv2/Actuators/Actuators.h>
+#include <RIGv2/BoardScheduler.h>
#include <RIGv2/Buses.h>
#include <RIGv2/CanHandler/CanHandler.h>
#include <RIGv2/Configs/RadioConfig.h>
@@ -44,8 +45,9 @@ using MavDriver = Boardcore::MavlinkDriver<Boardcore::SX1278Lora::MTU,
Config::Radio::MAV_MAX_LENGTH>;
class Radio
- : public Boardcore::InjectableWithDeps<Buses, Registry, Actuators, Sensors,
- CanHandler, GroundModeManager, TARS1>
+ : public Boardcore::InjectableWithDeps<Buses, BoardScheduler, Registry,
+ Actuators, Sensors, CanHandler,
+ GroundModeManager, TARS1>
{
public:
Radio() {}
diff --git a/src/boards/RIGv2/Sensors/AnalogLoadCellSensor.h b/src/boards/RIGv2/Sensors/AnalogLoadCellSensor.h
deleted file mode 100644
index b1eb038a97600642fab74dd7e8beb84189c3427d..0000000000000000000000000000000000000000
--- a/src/boards/RIGv2/Sensors/AnalogLoadCellSensor.h
+++ /dev/null
@@ -1,72 +0,0 @@
-/* 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 <sensors/Sensor.h>
-#include <sensors/SensorData.h>
-
-#include <functional>
-
-namespace RIGv2
-{
-
-class AnalogLoadCellSensor : public Boardcore::Sensor<Boardcore::LoadCellData>
-{
-public:
- AnalogLoadCellSensor(std::function<Boardcore::ADCData()> getVoltage,
- float p0Voltage, float p0Mass, float p1Voltage,
- float p1Mass)
- : getVoltage{getVoltage}, p0Voltage{p0Voltage}, p0Mass{p0Mass},
- p1Voltage{p1Voltage}, p1Mass{p1Mass}
- {
- }
-
- bool init() override { return true; }
-
- bool selfTest() override { return true; }
-
-private:
- Boardcore::LoadCellData sampleImpl() override
- {
- auto voltage = getVoltage();
- return {voltage.voltageTimestamp, -voltageToMass(voltage.voltage)};
- }
-
- float voltageToMass(float voltage)
- {
- // Two point calibration
- // m = dmass/dvoltage
- float scale = (p1Mass - p0Mass) / (p1Voltage - p0Voltage);
- float offset = p0Mass - scale * p0Voltage; // Calculate offset
- return scale * voltage + offset;
- }
-
- std::function<Boardcore::ADCData()> getVoltage;
-
- float p0Voltage;
- float p0Mass;
- float p1Voltage;
- float p1Mass;
-};
-
-} // namespace RIGv2
\ No newline at end of file
diff --git a/src/boards/RIGv2/Sensors/Sensors.cpp b/src/boards/RIGv2/Sensors/Sensors.cpp
index 2460cbb96ee2ce51f48597ffb09c96a3993248f8..80643405241abb6c585fb5d736435675aa12b6dd 100644
--- a/src/boards/RIGv2/Sensors/Sensors.cpp
+++ b/src/boards/RIGv2/Sensors/Sensors.cpp
@@ -152,9 +152,7 @@ LoadCellData Sensors::getVesselWeight()
{
if (vesselWeight)
{
- auto sample = vesselWeight->getLastSample();
- sample.load -= vesselLcOffset;
- return sample;
+ return vesselWeight->getLastSample();
}
else
{
@@ -166,9 +164,7 @@ LoadCellData Sensors::getTankWeight()
{
if (tankWeight)
{
- auto sample = tankWeight->getLastSample();
- sample.load -= tankLcOffset;
- return sample;
+ return tankWeight->getLastSample();
}
else
{
@@ -178,7 +174,7 @@ LoadCellData Sensors::getTankWeight()
CurrentData Sensors::getUmbilicalCurrent()
{
- return {TimestampTimer::getTimestamp(), 0.0};
+ return {TimestampTimer::getTimestamp(), -1.0};
}
CurrentData Sensors::getServoCurrent()
@@ -205,6 +201,19 @@ VoltageData Sensors::getBatteryVoltage()
return {sample.timestamp, voltage};
}
+VoltageData Sensors::getMotorBatteryVoltage()
+{
+ if (useCanData)
+ {
+ Lock<FastMutex> lock{canMutex};
+ return canMotorBatteryVoltage;
+ }
+ else
+ {
+ return VoltageData{};
+ }
+}
+
void Sensors::setCanTopTankPress(Boardcore::PressureData data)
{
Lock<FastMutex> lock{canMutex};
@@ -233,6 +242,13 @@ void Sensors::setCanTankTemp(Boardcore::TemperatureData data)
useCanData = true;
}
+void Sensors::setCanMotorBatteryVoltage(Boardcore::VoltageData data)
+{
+ Lock<FastMutex> lock{canMutex};
+ canMotorBatteryVoltage = data;
+ useCanData = true;
+}
+
void Sensors::calibrate()
{
Stats vesselStats, tankStats;
@@ -247,8 +263,8 @@ void Sensors::calibrate()
Thread::sleep(Config::Sensors::LoadCell::CALIBRATE_SAMPLE_PERIOD);
}
- vesselLcOffset = vesselStats.getStats().mean;
- tankLcOffset = tankStats.getStats().mean;
+ vesselWeight->updateOffset(vesselStats.getStats().mean);
+ tankWeight->updateOffset(tankStats.getStats().mean);
}
std::vector<SensorInfo> Sensors::getSensorInfos()
@@ -522,7 +538,7 @@ void Sensors::bottomTankPressureCallback()
void Sensors::vesselWeightInit(Boardcore::SensorManager::SensorMap_t &map)
{
- vesselWeight = std::make_unique<AnalogLoadCellSensor>(
+ vesselWeight = std::make_unique<TwoPointAnalogLoadCell>(
[this]()
{
auto sample = getADC1LastSample();
@@ -541,15 +557,14 @@ void Sensors::vesselWeightInit(Boardcore::SensorManager::SensorMap_t &map)
void Sensors::vesselWeightCallback()
{
- // Log CALIBRATED value
- LoadCellData sample = getVesselWeight();
+ LoadCellData sample = vesselWeight->getLastSample();
LCsData data{sample.loadTimestamp, 1, sample.load};
sdLogger.log(data);
}
void Sensors::tankWeightInit(Boardcore::SensorManager::SensorMap_t &map)
{
- tankWeight = std::make_unique<AnalogLoadCellSensor>(
+ tankWeight = std::make_unique<TwoPointAnalogLoadCell>(
[this]()
{
auto sample = getADC1LastSample();
@@ -568,8 +583,7 @@ void Sensors::tankWeightInit(Boardcore::SensorManager::SensorMap_t &map)
void Sensors::tankWeightCallback()
{
- // Log CALIBRATED value
- LoadCellData sample = getTankWeight();
+ LoadCellData sample = tankWeight->getLastSample();
LCsData data{sample.loadTimestamp, 2, sample.load};
sdLogger.log(data);
}
\ No newline at end of file
diff --git a/src/boards/RIGv2/Sensors/Sensors.h b/src/boards/RIGv2/Sensors/Sensors.h
index 77a922a08abcb9e214aa2ee465e77bd5128dacc0..6cae9b512111ce176a9389fbae20bc668ad4487a 100644
--- a/src/boards/RIGv2/Sensors/Sensors.h
+++ b/src/boards/RIGv2/Sensors/Sensors.h
@@ -24,13 +24,13 @@
#include <RIGv2/BoardScheduler.h>
#include <RIGv2/Buses.h>
-#include <RIGv2/Sensors/AnalogLoadCellSensor.h>
#include <RIGv2/Sensors/SensorsData.h>
-#include <RIGv2/Sensors/TrafagPressureSensor.h>
#include <drivers/adc/InternalADC.h>
#include <sensors/ADS131M08/ADS131M08.h>
#include <sensors/MAX31856/MAX31856.h>
#include <sensors/SensorManager.h>
+#include <sensors/analog/TrafagPressureSensor.h>
+#include <sensors/analog/TwoPointAnalogLoadCell.h>
#include <atomic>
#include <functional>
@@ -67,11 +67,13 @@ public:
Boardcore::CurrentData getUmbilicalCurrent();
Boardcore::CurrentData getServoCurrent();
Boardcore::VoltageData getBatteryVoltage();
+ Boardcore::VoltageData getMotorBatteryVoltage();
void setCanTopTankPress(Boardcore::PressureData data);
void setCanBottomTankPress(Boardcore::PressureData data);
void setCanCCPress(Boardcore::PressureData data);
void setCanTankTemp(Boardcore::TemperatureData data);
+ void setCanMotorBatteryVoltage(Boardcore::VoltageData data);
void calibrate();
@@ -108,9 +110,6 @@ private:
Boardcore::Logger &sdLogger = Boardcore::Logger::getInstance();
Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("sensors");
- std::atomic<float> vesselLcOffset{0.0f};
- std::atomic<float> tankLcOffset{0.0f};
-
std::atomic<bool> started{false};
std::atomic<bool> useCanData{false};
@@ -119,14 +118,15 @@ private:
Boardcore::PressureData canBottomTankPressure;
Boardcore::PressureData canTopTankPressure;
Boardcore::TemperatureData canTankTemperature;
+ Boardcore::VoltageData canMotorBatteryVoltage;
// Analog sensors
- std::unique_ptr<TrafagPressureSensor> vesselPressure;
- std::unique_ptr<TrafagPressureSensor> fillingPressure;
- std::unique_ptr<TrafagPressureSensor> topTankPressure;
- std::unique_ptr<TrafagPressureSensor> bottomTankPressure;
- std::unique_ptr<AnalogLoadCellSensor> vesselWeight;
- std::unique_ptr<AnalogLoadCellSensor> tankWeight;
+ std::unique_ptr<Boardcore::TrafagPressureSensor> vesselPressure;
+ std::unique_ptr<Boardcore::TrafagPressureSensor> fillingPressure;
+ std::unique_ptr<Boardcore::TrafagPressureSensor> topTankPressure;
+ std::unique_ptr<Boardcore::TrafagPressureSensor> bottomTankPressure;
+ std::unique_ptr<Boardcore::TwoPointAnalogLoadCell> vesselWeight;
+ std::unique_ptr<Boardcore::TwoPointAnalogLoadCell> tankWeight;
// Digital sensors
std::unique_ptr<Boardcore::ADS131M08> adc1;
diff --git a/src/boards/RIGv2/Sensors/TrafagPressureSensor.h b/src/boards/RIGv2/Sensors/TrafagPressureSensor.h
deleted file mode 100644
index ede4573b8c04a5a1dc1cf146e58170171131e0ba..0000000000000000000000000000000000000000
--- a/src/boards/RIGv2/Sensors/TrafagPressureSensor.h
+++ /dev/null
@@ -1,78 +0,0 @@
-/* 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 <sensors/Sensor.h>
-#include <sensors/SensorData.h>
-
-#include <functional>
-
-namespace RIGv2
-{
-
-class TrafagPressureSensor : public Boardcore::Sensor<Boardcore::PressureData>
-{
-public:
- TrafagPressureSensor(std::function<Boardcore::ADCData()> getVoltage,
- float shuntResistance, float maxPressure,
- float minCurrent = 4, float maxCurrent = 20)
- : getVoltage{getVoltage}, shuntResistance{shuntResistance},
- maxPressure{maxPressure}, minCurrent{minCurrent}, maxCurrent{
- maxCurrent}
- {
- }
-
- bool init() override { return true; }
-
- bool selfTest() override { return true; }
-
-private:
- Boardcore::PressureData sampleImpl() override
- {
- auto voltage = getVoltage();
- float pressure = voltageToPressure(voltage.voltage);
-
- return {voltage.voltageTimestamp, pressure};
- }
-
- float voltageToPressure(float voltage)
- {
- // First convert voltage to current
- float current = (voltage / shuntResistance) * 1000.0f;
-
- // Convert to a value between 0 and 1
- float value = (current - minCurrent) / (maxCurrent - minCurrent);
-
- // Scale from 0 to maxPressure
- return value * maxPressure;
- }
-
- std::function<Boardcore::ADCData()> getVoltage;
-
- float shuntResistance;
- float maxPressure;
- float minCurrent;
- float maxCurrent;
-};
-
-} // namespace RIGv2
\ No newline at end of file