diff --git a/src/boards/RIGv2/Actuators/Actuators.cpp b/src/boards/RIGv2/Actuators/Actuators.cpp
index 10cf616a9774265eb2cc0e21a2629a807705f756..01b66facabd8da5306204ad9a49d6621cb92d0bf 100644
--- a/src/boards/RIGv2/Actuators/Actuators.cpp
+++ b/src/boards/RIGv2/Actuators/Actuators.cpp
@@ -33,6 +33,59 @@ using namespace miosix;
using namespace Common;
using namespace RIGv2;
+void Actuators::ServoInfo::openServo(uint64_t time)
+{
+ long long currentTime = getTime();
+
+ openedTs = currentTime;
+ closeTs = currentTime + (time * Constants::NS_IN_MS);
+
+ // TODO(davide.mor): Dispatch the open event
+}
+
+void Actuators::ServoInfo::closeServo()
+{
+ closeTs = 0;
+
+ // TODO(davide.mor): Dispatch the close event
+}
+
+void Actuators::ServoInfo::unsafeSetServoPosition(float position)
+{
+ // Check that the servo is actually there, just to be safe
+ if (!servo)
+ {
+ return;
+ }
+
+ if (flipped)
+ {
+ servo->setPosition(1.0f - position);
+ }
+ else
+ {
+ servo->setPosition(position);
+ }
+}
+
+float Actuators::ServoInfo::getServoPosition()
+{
+ // Check that the servo is actually there, just to be safe
+ if (!servo)
+ {
+ return 0.0f;
+ }
+
+ if (flipped)
+ {
+ return 1.0f - servo->getPosition();
+ }
+ else
+ {
+ return servo->getPosition();
+ }
+}
+
Actuators::Actuators(Boardcore::TaskScheduler &scheduler) : scheduler{scheduler}
{
// Initialize servos
@@ -76,6 +129,7 @@ Actuators::Actuators(Boardcore::TaskScheduler &scheduler) : scheduler{scheduler}
info->flipped = Config::Servos::FILLING_FLIPPED;
info->openingEvent = Common::Events::MOTOR_OPEN_FILLING_VALVE;
info->closingEvent = Common::Events::MOTOR_CLOSE_FILLING_VALVE;
+ info->unsafeSetServoPosition(0.0f);
info = getServo(ServosList::RELEASE_VALVE);
info->maxAperture = Config::Servos::DEFAULT_RELEASE_MAXIMUM_APERTURE;
@@ -83,12 +137,14 @@ Actuators::Actuators(Boardcore::TaskScheduler &scheduler) : scheduler{scheduler}
info->flipped = Config::Servos::RELEASE_FLIPPED;
info->openingEvent = Common::Events::MOTOR_OPEN_RELEASE_VALVE;
info->closingEvent = Common::Events::MOTOR_CLOSE_RELEASE_VALVE;
+ info->unsafeSetServoPosition(0.0f);
info = getServo(ServosList::DISCONNECT_SERVO);
info->maxAperture = Config::Servos::DEFAULT_DISCONNECT_MAXIMUM_APERTURE;
info->openingTime = Config::Servos::DEFAULT_DISCONNECT_OPENING_TIME;
info->flipped = Config::Servos::DISCONNECT_FLIPPED;
info->openingEvent = Common::Events::MOTOR_DISCONNECT;
+ info->unsafeSetServoPosition(0.0f);
info = getServo(ServosList::MAIN_VALVE);
info->maxAperture = Config::Servos::DEFAULT_MAIN_MAXIMUM_APERTURE;
@@ -96,14 +152,16 @@ Actuators::Actuators(Boardcore::TaskScheduler &scheduler) : scheduler{scheduler}
info->flipped = Config::Servos::MAIN_FLIPPED;
info->openingEvent = Common::Events::MOTOR_OPEN_FEED_VALVE;
info->closingEvent = Common::Events::MOTOR_CLOSE_FEED_VALVE;
+ info->unsafeSetServoPosition(0.0f);
// This servo is not yet enabled
- // info = getServo(ServosList::VENTING_VALVE);
- // info->maxAperture = Config::Servos::DEFAULT_VENTING_MAXIMUM_APERTURE;
- // info->openingTime = Config::Servos::DEFAULT_VENTING_OPENING_TIME;
- // info->flipped = true;
- // info->openingEvent = Common::Events::MOTOR_OPEN_VENTING_VALVE;
- // info->closingEvent = Common::Events::MOTOR_CLOSE_VENTING_VALVE;
+ info = getServo(ServosList::VENTING_VALVE);
+ info->maxAperture = Config::Servos::DEFAULT_VENTING_MAXIMUM_APERTURE;
+ info->openingTime = Config::Servos::DEFAULT_VENTING_OPENING_TIME;
+ info->flipped = Config::Servos::VENTING_FLIPPED;
+ info->openingEvent = Common::Events::MOTOR_OPEN_VENTING_VALVE;
+ info->closingEvent = Common::Events::MOTOR_CLOSE_VENTING_VALVE;
+ info->unsafeSetServoPosition(0.0f);
}
bool Actuators::start()
@@ -150,12 +208,12 @@ bool Actuators::toggleServo(ServosList servo)
if (info->closeTs == 0)
{
// The servo is closed, open it
- openServoInner(info, info->openingTime);
+ info->openServo(info->openingTime);
}
else
{
// The servo is open, close it
- closeServoInner(info);
+ info->closeServo();
}
return true;
@@ -170,7 +228,7 @@ bool Actuators::closeServo(ServosList servo)
return false;
}
- closeServoInner(info);
+ info->closeServo();
return true;
}
@@ -183,7 +241,7 @@ bool Actuators::openServoAtomic(ServosList servo, uint64_t time)
return false;
}
- openServoInner(info, time);
+ info->openServo(time);
return true;
}
@@ -222,24 +280,7 @@ bool Actuators::isServoOpen(ServosList servo)
return false;
}
- return info->servo->getPosition() > Config::Servos::SERVO_OPEN_THRESHOLD;
-}
-
-void Actuators::openServoInner(ServoInfo *info, uint64_t time)
-{
- long long currentTime = getTime();
-
- info->openedTs = currentTime;
- info->closeTs = currentTime + (time * Constants::NS_IN_MS);
-
- // TODO(davide.mor): Dispatch the open event
-}
-
-void Actuators::closeServoInner(ServoInfo *info)
-{
- info->closeTs = 0;
-
- // TODO(davide.mor): Dispatch the close event
+ return info->getServoPosition() > Config::Servos::SERVO_OPEN_THRESHOLD;
}
Actuators::ServoInfo *Actuators::getServo(ServosList servo)
@@ -254,9 +295,8 @@ Actuators::ServoInfo *Actuators::getServo(ServosList servo)
return &infos[1];
case MAIN_VALVE:
return &infos[0];
- // TODO(davide.mor): Decide this servo
- // case VENTING_VALVE:
- // return &infos[8];
+ case VENTING_VALVE:
+ return &infos[6];
default:
// Oh FUCK
@@ -267,15 +307,7 @@ Actuators::ServoInfo *Actuators::getServo(ServosList servo)
void Actuators::unsafeSetServoPosition(uint8_t idx, float position)
{
- // Invert the position if the servo is flipped
- if (infos[idx].flipped)
- {
- infos[idx].servo->setPosition(1.0 - position);
- }
- else
- {
- infos[idx].servo->setPosition(position);
- }
+ infos[idx].unsafeSetServoPosition(position);
// Log the update
ActuatorsData data;
diff --git a/src/boards/RIGv2/Actuators/Actuators.h b/src/boards/RIGv2/Actuators/Actuators.h
index ab3ecb80dc1c9b34af0e5a31827ce09cd96cbfa6..b588f2be54c7b7e428fd0891c275cf3832fb8bea 100644
--- a/src/boards/RIGv2/Actuators/Actuators.h
+++ b/src/boards/RIGv2/Actuators/Actuators.h
@@ -55,6 +55,11 @@ private:
long long closeTs = 0;
// Timestamp of when the servo was opened
long long openedTs = 0;
+
+ void openServo(uint64_t time);
+ void closeServo();
+ void unsafeSetServoPosition(float position);
+ float getServoPosition();
};
public:
@@ -73,8 +78,6 @@ public:
private:
ServoInfo *getServo(ServosList servo);
- void openServoInner(ServoInfo *info, uint64_t time);
- void closeServoInner(ServoInfo *info);
void unsafeSetServoPosition(uint8_t idx, float position);
void updatePositionsTask();
diff --git a/src/boards/RIGv2/Configs/SensorsConfig.h b/src/boards/RIGv2/Configs/SensorsConfig.h
index 5ccb05156c07a9e9cdc11ec889dda77ddb5ed8bb..e573dc4ce50c5a5f0265360e609a99c8173b36f8 100644
--- a/src/boards/RIGv2/Configs/SensorsConfig.h
+++ b/src/boards/RIGv2/Configs/SensorsConfig.h
@@ -41,6 +41,14 @@ static constexpr float ADC1_CH2_SHUNT_RESISTANCE = 30;
static constexpr float ADC1_CH3_SHUNT_RESISTANCE = 30;
static constexpr float ADC1_CH4_SHUNT_RESISTANCE = 30;
+// ADC channels definitions for various sensors
+static constexpr int ADC1_VESSEL_PT_CHANNEL = 0;
+static constexpr int ADC1_FILLING_PT_CHANNEL = 1;
+static constexpr int ADC1_BOTTOM_PT_CHANNEL = 2;
+static constexpr int ADC1_TOP_PT_CHANNEL = 3;
+static constexpr int ADC1_VESSEL_LC_CHANNEL = 6;
+static constexpr int ADC1_TANK_LC_CHANNEL = 7;
+
static constexpr float PT_MIN_CURRENT = 4;
static constexpr float PT_MAX_CURRENT = 20;
diff --git a/src/boards/RIGv2/Radio/Radio.cpp b/src/boards/RIGv2/Radio/Radio.cpp
index ddf68abd5e4f956b3af3337987a4269484a0fdf0..b6d5863a71a76ef9e6fb3e123a144012a97ddc59 100644
--- a/src/boards/RIGv2/Radio/Radio.cpp
+++ b/src/boards/RIGv2/Radio/Radio.cpp
@@ -198,10 +198,13 @@ void Radio::handleMessage(const mavlink_message_t& msg)
ServosList servo = static_cast<ServosList>(
mavlink_msg_wiggle_servo_tc_get_servo_id(&msg));
- if (modules.get<GroundModeManager>()->isDisarmed() &&
- modules.get<Actuators>()->wiggleServo(servo))
+ if (modules.get<GroundModeManager>()->isDisarmed())
{
- sendAck(msg);
+ if(modules.get<Actuators>()->wiggleServo(servo)) {
+ sendAck(msg);
+ } else {
+ sendNack(msg);
+ }
}
else
{
@@ -405,7 +408,7 @@ bool Radio::packSystemTm(uint8_t tmId, mavlink_message_t& msg)
actuators->isServoOpen(ServosList::RELEASE_VALVE) ? 1 : 0;
tm.main_valve_state =
actuators->isServoOpen(ServosList::MAIN_VALVE) ? 1 : 0;
- tm.arming_state = modules.get<GroundModeManager>()->isArmed();
+ tm.arming_state = modules.get<GroundModeManager>()->isArmed();
tm.ignition_state = modules.get<GroundModeManager>()->isIgniting();
// TODO(davide.mor): Add the rest of these
diff --git a/src/boards/RIGv2/Sensors/Sensors.cpp b/src/boards/RIGv2/Sensors/Sensors.cpp
index fb401182ca91cf0c96da5c2994f5e8c24f356947..b96c250da6c475aca3358b7ea41633284e21eea6 100644
--- a/src/boards/RIGv2/Sensors/Sensors.cpp
+++ b/src/boards/RIGv2/Sensors/Sensors.cpp
@@ -44,6 +44,16 @@ float pressureFromVoltage(float voltage, float shuntResistance,
return value * maxPressure;
}
+float loadFromVoltage(float voltage, float excitationVoltage, float ratedOutput,
+ float ratedCapacity)
+{
+ // Convert to a value between [0, 1] based on 0 and maximum output voltage
+ float value = (voltage * 1000.0f) / (ratedOutput * excitationVoltage);
+
+ // Finally remap to the range [0, ratedCapacity]
+ return value * ratedCapacity;
+}
+
bool Sensors::isStarted() { return started; }
bool Sensors::start()
@@ -80,7 +90,8 @@ Boardcore::PressureData Sensors::getVesselPress()
auto sample = getADC1LastSample();
float pressure = pressureFromVoltage(
- sample.voltage[0], Config::Sensors::ADC1_CH1_SHUNT_RESISTANCE,
+ sample.voltage[Config::Sensors::ADC1_VESSEL_PT_CHANNEL],
+ 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};
@@ -91,7 +102,8 @@ Boardcore::PressureData Sensors::getFillingPress()
auto sample = getADC1LastSample();
float pressure = pressureFromVoltage(
- sample.voltage[1], Config::Sensors::ADC1_CH2_SHUNT_RESISTANCE,
+ sample.voltage[Config::Sensors::ADC1_FILLING_PT_CHANNEL],
+ 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};
@@ -102,7 +114,8 @@ Boardcore::PressureData Sensors::getTankTopPress()
auto sample = getADC1LastSample();
float pressure = pressureFromVoltage(
- sample.voltage[2], Config::Sensors::ADC1_CH3_SHUNT_RESISTANCE,
+ sample.voltage[Config::Sensors::ADC1_TOP_PT_CHANNEL],
+ 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};
@@ -113,7 +126,8 @@ Boardcore::PressureData Sensors::getTankBottomPress()
auto sample = getADC1LastSample();
float pressure = pressureFromVoltage(
- sample.voltage[3], Config::Sensors::ADC1_CH4_SHUNT_RESISTANCE,
+ sample.voltage[Config::Sensors::ADC1_BOTTOM_PT_CHANNEL],
+ 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};
@@ -121,37 +135,45 @@ Boardcore::PressureData Sensors::getTankBottomPress()
Boardcore::LoadCellData Sensors::getVesselWeight()
{
- auto sample = getADC1LastSample();
- float calibratedVoltage = sample.voltage[6] - vesselLcOffset;
+ auto sample = getADC1LastSample();
+ float calibratedVoltage =
+ sample.voltage[Config::Sensors::ADC1_VESSEL_LC_CHANNEL] -
+ vesselLcOffset;
+
+ float load = loadFromVoltage(calibratedVoltage, 8.0f, 2.0f, 500.0f);
- return {sample.timestamp, calibratedVoltage};
+ return {sample.timestamp, load};
}
Boardcore::LoadCellData Sensors::getTankWeight()
{
- auto sample = getADC1LastSample();
- float calibratedVoltage = sample.voltage[7] - tankLcOffset;
+ auto sample = getADC1LastSample();
+ float calibratedVoltage =
+ sample.voltage[Config::Sensors::ADC1_TANK_LC_CHANNEL] - tankLcOffset;
+
+ float load = loadFromVoltage(calibratedVoltage, 8.0f, 2.0f, 500.0f);
- return {sample.timestamp, calibratedVoltage};
+ return {sample.timestamp, load};
}
void Sensors::calibrate()
{
- Stats channel6, channel7;
+ Stats vesselStats, tankStats;
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]);
+ vesselStats.add(
+ sample.voltage[Config::Sensors::ADC1_VESSEL_LC_CHANNEL]);
+ tankStats.add(sample.voltage[Config::Sensors::ADC1_TANK_LC_CHANNEL]);
Thread::sleep(Config::Sensors::LC_CALIBRATE_SAMPLE_PERIOD);
}
- vesselLcOffset = channel6.getStats().mean;
- tankLcOffset = channel7.getStats().mean;
+ vesselLcOffset = vesselStats.getStats().mean;
+ tankLcOffset = tankStats.getStats().mean;
}
void Sensors::adc1Init(SensorManager::SensorMap_t &map)
@@ -162,7 +184,11 @@ void Sensors::adc1Init(SensorManager::SensorMap_t &map)
spiConfig.mode = SPI::Mode::MODE_0;
spiConfig.clockDivider = SPI::ClockDivider::DIV_32;
- ADS131M08::Config config = {};
+ ADS131M08::Config config = {};
+ config.channelsConfig[Config::Sensors::ADC1_VESSEL_LC_CHANNEL].pga =
+ ADS131M08Defs::PGA::PGA_32;
+ config.channelsConfig[Config::Sensors::ADC1_TANK_LC_CHANNEL].pga =
+ ADS131M08Defs::PGA::PGA_32;
config.oversamplingRatio = ADS131M08Defs::OversamplingRatio::OSR_8192;
config.globalChopModeEnabled = true;
diff --git a/src/entrypoints/RIGv2/rig-v2-entry.cpp b/src/entrypoints/RIGv2/rig-v2-entry.cpp
index 06dce4da33bd770ec1bb6c260c0cb7029e73db50..d8f94a7d798d192441c1f840ea328f91a3fedf3a 100644
--- a/src/entrypoints/RIGv2/rig-v2-entry.cpp
+++ b/src/entrypoints/RIGv2/rig-v2-entry.cpp
@@ -46,8 +46,8 @@ int main()
PrintLogger logger = Logging::getLogger("main");
// TODO: Move this to a dedicated board scheduler
- TaskScheduler *scheduler1 = new TaskScheduler(3);
- TaskScheduler *scheduler2 = new TaskScheduler(4);
+ TaskScheduler *scheduler1 = new TaskScheduler(2);
+ TaskScheduler *scheduler2 = new TaskScheduler(3);
Buses *buses = new Buses();
Sensors *sensors = new Sensors(*scheduler1);
@@ -101,13 +101,13 @@ int main()
}
// Start modules
- if (sdLogger.testSDCard())
+ if (!sdLogger.testSDCard())
{
initResult = false;
LOG_ERR(logger, "SD card test failed");
}
- if (broker.start())
+ if (!broker.start())
{
initResult = false;
LOG_ERR(logger, "Failed to start EventBroker");