diff --git a/skyward-boardcore b/skyward-boardcore
index 48c4fa34e8f7e92da9cc5cd1abcfba4c3d134d74..b6387642f610fcb42c73780448130c319103f87f 160000
--- a/skyward-boardcore
+++ b/skyward-boardcore
@@ -1 +1 @@
-Subproject commit 48c4fa34e8f7e92da9cc5cd1abcfba4c3d134d74
+Subproject commit b6387642f610fcb42c73780448130c319103f87f
diff --git a/src/boards/Main/StateMachines/ADAController/ADAController.cpp b/src/boards/Main/StateMachines/ADAController/ADAController.cpp
index d85bbdfe8f35ed973b01cd867e00d257ac5782c5..0a3509d85862fb46773a9a7f52addd78faa773c9 100644
--- a/src/boards/Main/StateMachines/ADAController/ADAController.cpp
+++ b/src/boards/Main/StateMachines/ADAController/ADAController.cpp
@@ -238,18 +238,6 @@ void ADAController::calibrate()
EventBroker::getInstance().post(ADA_READY, TOPIC_ADA);
}
-ADAControllerStatus ADAController::getStatus()
-{
- PauseKernelLock lock;
- return status;
-}
-
-ADAState ADAController::getAdaState()
-{
- PauseKernelLock lock;
- return ada.getState();
-}
-
void ADAController::setDeploymentAltitude(float altitude)
{
deploymentAltitude = altitude;
@@ -282,11 +270,25 @@ void ADAController::setReferenceValues(const ReferenceValues reference)
ada.setReferenceValues(reference);
}
+ADAControllerStatus ADAController::getStatus()
+{
+ PauseKernelLock lock;
+ return status;
+}
+
+ADAState ADAController::getAdaState()
+{
+ PauseKernelLock lock;
+ return ada.getState();
+}
+
ReferenceValues ADAController::getReferenceValues()
{
return ada.getReferenceValues();
}
+float ADAController::getDeploymentAltitude() { return deploymentAltitude; }
+
void ADAController::state_idle(const Event& event)
{
switch (event)
@@ -509,15 +511,16 @@ void ADAController::state_landed(const Event& event)
}
}
+#ifdef HILSimulation
void ADAController::setUpdateDataFunction(
std::function<void(Boardcore::ADAState)> updateData)
{
this->updateData = updateData;
}
+#endif
ADAController::ADAController()
- : FSM(&ADAController::state_idle), ada(getADAKalmanConfig()),
- updateData([](Boardcore::ADAState) {})
+ : FSM(&ADAController::state_idle), ada(getADAKalmanConfig())
{
EventBroker::getInstance().subscribe(this, TOPIC_ADA);
EventBroker::getInstance().subscribe(this, TOPIC_FLIGHT);
diff --git a/src/boards/Main/StateMachines/ADAController/ADAController.h b/src/boards/Main/StateMachines/ADAController/ADAController.h
index a6740a992e7d6840dc7c8548d48c1b10bdd01d56..6cb0d4a346a80fb4ce53d03fe1c9232a0658d74b 100644
--- a/src/boards/Main/StateMachines/ADAController/ADAController.h
+++ b/src/boards/Main/StateMachines/ADAController/ADAController.h
@@ -52,6 +52,7 @@ public:
ADAControllerStatus getStatus();
Boardcore::ADAState getAdaState();
Boardcore::ReferenceValues getReferenceValues();
+ float getDeploymentAltitude();
void state_idle(const Boardcore::Event& event);
void state_calibrating(const Boardcore::Event& event);
@@ -64,8 +65,10 @@ public:
void state_terminal_descent(const Boardcore::Event& event);
void state_landed(const Boardcore::Event& event);
+#ifdef HILSimulation
void setUpdateDataFunction(
std::function<void(Boardcore::ADAState)> updateData);
+#endif
private:
ADAController();
@@ -87,7 +90,7 @@ private:
float deploymentAltitude = ADAConfig::DEFAULT_DEPLOYMENT_ALTITUDE;
Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("main.ada");
- std::function<void(Boardcore::ADAState)> updateData;
+ std::function<void(Boardcore::ADAState)> updateData = nullptr;
};
} // namespace Main
diff --git a/src/boards/Main/TMRepository/TMRepository.cpp b/src/boards/Main/TMRepository/TMRepository.cpp
index 0fe3a9b7ae21cc058bb36f2e1525af5487ec088a..4d4e03a0510f4602cd5a60be13dcd36005ebdee6 100644
--- a/src/boards/Main/TMRepository/TMRepository.cpp
+++ b/src/boards/Main/TMRepository/TMRepository.cpp
@@ -89,7 +89,6 @@ mavlink_message_t TMRepository::packSystemTm(SystemTMList tmId, uint8_t msgId,
Deployment::getInstance().getStatus().state);
tm.fmm_state = static_cast<uint8_t>(
FlightModeManager::getInstance().getStatus().state);
- tm.fsr_state = 0;
tm.nas_state = static_cast<uint8_t>(
NASController::getInstance().getStatus().state);
@@ -150,9 +149,10 @@ mavlink_message_t TMRepository::packSystemTm(SystemTMList tmId, uint8_t msgId,
{
mavlink_ada_tm_t tm;
- auto status = ADAController::getInstance().getStatus();
- auto state = ADAController::getInstance().getAdaState();
- auto ref = ADAController::getInstance().getReferenceValues();
+ ADAController &ada = ADAController::getInstance();
+ auto status = ada.getStatus();
+ auto state = ada.getAdaState();
+ auto ref = ada.getReferenceValues();
tm.timestamp = state.timestamp;
tm.state = static_cast<uint8_t>(status.state);
@@ -166,6 +166,7 @@ mavlink_message_t TMRepository::packSystemTm(SystemTMList tmId, uint8_t msgId,
tm.msl_altitude = state.mslAltitude;
tm.msl_pressure = ref.mslPressure;
tm.msl_temperature = ref.mslTemperature;
+ tm.dpl_altitude = ada.getDeploymentAltitude();
mavlink_msg_ada_tm_encode(RadioConfig::MAV_SYSTEM_ID,
RadioConfig::MAV_COMPONENT_ID, &msg, &tm);
@@ -205,23 +206,6 @@ mavlink_message_t TMRepository::packSystemTm(SystemTMList tmId, uint8_t msgId,
break;
}
- case SystemTMList::MAV_CAN_ID: // TODO
- {
- mavlink_can_tm_t tm;
-
- tm.timestamp = TimestampTimer::getTimestamp();
- tm.n_sent = 0;
- tm.n_rcv = 0;
- tm.last_sent = 0;
- tm.last_sent_ts = 0;
- tm.last_rcv = 0;
- tm.last_rcv_ts = 0;
-
- mavlink_msg_can_tm_encode(RadioConfig::MAV_SYSTEM_ID,
- RadioConfig::MAV_COMPONENT_ID, &msg, &tm);
-
- break;
- }
case SystemTMList::MAV_FLIGHT_ID:
{
mavlink_rocket_flight_tm_t tm;
@@ -282,6 +266,9 @@ mavlink_message_t TMRepository::packSystemTm(SystemTMList tmId, uint8_t msgId,
Actuators::getInstance().getServoAngle(AIR_BRAKES_SERVO);
tm.abk_estimated_cd = 0;
+ // Load cell
+ tm.parachute_load = sensors.getLoadCellLastSample().load;
+
// NAS
tm.nas_n = nasState.n;
tm.nas_e = nasState.e;
diff --git a/src/boards/Payload/TMRepository/TMRepository.cpp b/src/boards/Payload/TMRepository/TMRepository.cpp
index 3d15abdaf5e0475823ab836153dbfcf9d9b4ad03..6121cf8a6a7c4de5619255d3bc5d872b20f5ef46 100644
--- a/src/boards/Payload/TMRepository/TMRepository.cpp
+++ b/src/boards/Payload/TMRepository/TMRepository.cpp
@@ -250,16 +250,6 @@ mavlink_message_t TMRepository::packSystemTm(SystemTMList tmId, uint8_t msgId,
&msg, &tm);
break;
}
- case SystemTMList::MAV_CAN_ID:
- {
- mavlink_can_tm_t tm;
-
- // TODO
-
- mavlink_msg_can_tm_encode(RadioConfig::MAV_SYSTEM_ID,
- RadioConfig::MAV_COMPONENT_ID, &msg, &tm);
- break;
- }
case SystemTMList::MAV_FSM_ID:
{
mavlink_fsm_tm_t tm;
@@ -269,7 +259,6 @@ mavlink_message_t TMRepository::packSystemTm(SystemTMList tmId, uint8_t msgId,
tm.ada_state = 0;
tm.dpl_state = static_cast<uint8_t>(
Deployment::getInstance().getStatus().state);
- tm.fsr_state = 0;
tm.fmm_state = static_cast<uint8_t>(
FlightModeManager::getInstance().getStatus().state);
tm.nas_state = static_cast<uint8_t>(