diff --git a/bin_delivery/13_05_2019/README.md b/bin_delivery/13_05_2019/README.md
deleted file mode 100644
index 73fbf28cfdeb8a6793a9380b2f6afd3bfaa428d3..0000000000000000000000000000000000000000
--- a/bin_delivery/13_05_2019/README.md
+++ /dev/null
@@ -1,19 +0,0 @@
-# Tests 13/05/2019
-
-**Working**
-* ramtest (changed Makefile.inc)
-* test-all-sensors (all a part from GPS)
-* xbee-send-rcv
-* test-actuators (thc1 and 2)
-* test-logger
-
-**Not working**
-* ADIS
-* IMU soufiane
-* Digital pressure sensor
-
-**Untested**
-* GPS
-* Nosecone motor
-* Rogallo servos
-
diff --git a/bin_delivery/final/ramtest/ramtest.bin b/bin_delivery/final/ramtest/ramtest.bin
deleted file mode 100755
index 711e6eca151221db539ae6f9139ac4b534628638..0000000000000000000000000000000000000000
Binary files a/bin_delivery/final/ramtest/ramtest.bin and /dev/null differ
diff --git a/bin_delivery/13_05_2019/test-all-sensors/test-all-sensors.bin b/bin_delivery/hermes/13_05_2019/test-all-sensors/test-all-sensors.bin
similarity index 100%
rename from bin_delivery/13_05_2019/test-all-sensors/test-all-sensors.bin
rename to bin_delivery/hermes/13_05_2019/test-all-sensors/test-all-sensors.bin
diff --git a/bin_delivery/13_05_2019/test-logger/test-logger.bin b/bin_delivery/hermes/13_05_2019/test-logger/test-logger.bin
similarity index 100%
rename from bin_delivery/13_05_2019/test-logger/test-logger.bin
rename to bin_delivery/hermes/13_05_2019/test-logger/test-logger.bin
diff --git a/bin_delivery/13_05_2019/xbee-send-rcv/xbee-send-rcv.bin b/bin_delivery/hermes/13_05_2019/xbee-send-rcv/xbee-send-rcv.bin
similarity index 100%
rename from bin_delivery/13_05_2019/xbee-send-rcv/xbee-send-rcv.bin
rename to bin_delivery/hermes/13_05_2019/xbee-send-rcv/xbee-send-rcv.bin
diff --git a/bin_delivery/14_05_2019/README.md b/bin_delivery/hermes/14_05_2019/README.md
similarity index 100%
rename from bin_delivery/14_05_2019/README.md
rename to bin_delivery/hermes/14_05_2019/README.md
diff --git a/bin_delivery/14_05_2019/test-mavchannel/test-mavchannel.bin b/bin_delivery/hermes/14_05_2019/test-mavchannel/test-mavchannel.bin
similarity index 100%
rename from bin_delivery/14_05_2019/test-mavchannel/test-mavchannel.bin
rename to bin_delivery/hermes/14_05_2019/test-mavchannel/test-mavchannel.bin
diff --git a/bin_delivery/14_05_2019/test-mavlink/test-mavlink.bin b/bin_delivery/hermes/14_05_2019/test-mavlink/test-mavlink.bin
similarity index 100%
rename from bin_delivery/14_05_2019/test-mavlink/test-mavlink.bin
rename to bin_delivery/hermes/14_05_2019/test-mavlink/test-mavlink.bin
diff --git a/bin_delivery/15_05_2019/README.md b/bin_delivery/hermes/15_05_2019/README.md
similarity index 100%
rename from bin_delivery/15_05_2019/README.md
rename to bin_delivery/hermes/15_05_2019/README.md
diff --git a/bin_delivery/15_05_2019/test-motor/test-motor.bin b/bin_delivery/hermes/15_05_2019/test-motor/test-motor.bin
similarity index 100%
rename from bin_delivery/15_05_2019/test-motor/test-motor.bin
rename to bin_delivery/hermes/15_05_2019/test-motor/test-motor.bin
diff --git a/bin_delivery/15_05_2019/test-tmtc/test-tmtc.bin b/bin_delivery/hermes/15_05_2019/test-tmtc/test-tmtc.bin
similarity index 100%
rename from bin_delivery/15_05_2019/test-tmtc/test-tmtc.bin
rename to bin_delivery/hermes/15_05_2019/test-tmtc/test-tmtc.bin
diff --git a/bin_delivery/20_05_19/death-stack-entry.bin b/bin_delivery/hermes/20_05_19/death-stack-entry.bin
similarity index 100%
rename from bin_delivery/20_05_19/death-stack-entry.bin
rename to bin_delivery/hermes/20_05_19/death-stack-entry.bin
diff --git a/bin_delivery/28_05_19/test-all-sensors/test-all-sensors.bin b/bin_delivery/hermes/28_05_19/test-all-sensors/test-all-sensors.bin
similarity index 100%
rename from bin_delivery/28_05_19/test-all-sensors/test-all-sensors.bin
rename to bin_delivery/hermes/28_05_19/test-all-sensors/test-all-sensors.bin
diff --git a/bin_delivery/final/death-stack-entry/death-stack-entry.bin b/bin_delivery/hermes/final/death-stack-entry/death-stack-entry.bin
similarity index 100%
rename from bin_delivery/final/death-stack-entry/death-stack-entry.bin
rename to bin_delivery/hermes/final/death-stack-entry/death-stack-entry.bin
diff --git a/bin_delivery/13_05_2019/ramtest/ramtest.bin b/bin_delivery/hermes/final/ramtest/ramtest.bin
similarity index 100%
rename from bin_delivery/13_05_2019/ramtest/ramtest.bin
rename to bin_delivery/hermes/final/ramtest/ramtest.bin
diff --git a/bin_delivery/lynx/01_09_2021/death-stack-x-entry.bin b/bin_delivery/lynx/01_09_2021/death-stack-x-entry.bin
new file mode 100644
index 0000000000000000000000000000000000000000..c2cabaefee395a0047aeb57f1251ff87c3ef0e81
Binary files /dev/null and b/bin_delivery/lynx/01_09_2021/death-stack-x-entry.bin differ
diff --git a/bin_delivery/lynx/01_09_2021/death-stack-x-hil-entry.bin b/bin_delivery/lynx/01_09_2021/death-stack-x-hil-entry.bin
new file mode 100644
index 0000000000000000000000000000000000000000..94f428f2556dc634a90d784d2a4040c43c595ff0
Binary files /dev/null and b/bin_delivery/lynx/01_09_2021/death-stack-x-hil-entry.bin differ
diff --git a/bin_delivery/lynx/01_09_2021/ramtest.bin b/bin_delivery/lynx/01_09_2021/ramtest.bin
new file mode 100644
index 0000000000000000000000000000000000000000..0e78678a46015a35e30eb1ab5f0cbdf3195a6944
Binary files /dev/null and b/bin_delivery/lynx/01_09_2021/ramtest.bin differ
diff --git a/sbs.conf b/sbs.conf
index 4af81d3674304fbf775e8820db82191a82b9bbc8..10f439fdb0560c476538059fad694fcaa38ca22c 100644
--- a/sbs.conf
+++ b/sbs.conf
@@ -196,6 +196,18 @@ Files: src/boards/DeathStack/events/EventStrings.cpp
Type: srcfiles
Files: src/boards/DeathStack/NavigationAttitudeSystem/NASCalibrator.cpp
src/boards/DeathStack/NavigationAttitudeSystem/ExtendedKalmanEigen.cpp
+
+[tests-obsw]
+Type: srcfiles
+Files: src/tests/catch/fsm/test-fmm.cpp
+ src/tests/catch/fsm/test-tmtc.cpp
+ src/tests/catch/fsm/test-ada.cpp
+ src/tests/catch/ada/ada_kalman_p/test-ada-simulation.cpp
+ src/tests/catch/fsm/test-deployment.cpp
+ src/tests/catch/fsm/test-flightstatsrecorder.cpp
+ src/tests/catch/fsm/test-airbrakes.cpp
+ #src/tests/catch/fsm/test-nas.cpp
+ #src/tests/catch/nas/test-nas-simulation.cpp
#--------------------------#
# Boards #
@@ -275,6 +287,15 @@ Main: death-stack-x-testsuite
# Defines: -DDEATH_STACK_1
# Main: catch/catch-tests-entry
+[tests-catch]
+Type: test
+BoardId: stm32f429zi_skyward_death_stack_x
+BinName: tests-catch
+Include: %shared %deathstack-new %tests-obsw %ada-test-sources %mock-sensors-data %math %ubloxgps %pwm %hbridge %spi %xbee %servo %sensormanager %ads1118 %bmx160 %bmx160withcorrection %internal-adc %calibration
+Defines:
+Libs: fmt eigen
+Main: catch/catch-tests-entry
+
## Driver tests
# [test-imus]
@@ -404,14 +425,14 @@ Main: test-pinhandler
# Defines: -DDEBUG -DDEATH_STACK_1
# Main: test-sm+tmtc
-[test-fmm]
-Type: test
-BoardId: stm32f429zi_skyward_death_stack_x
-BinName: test-fmm
-Include: %shared %deathstack-new %math %servo %xbee %pwm %spi %hbridge %ads1118 %internal-adc %sensormanager %ubloxgps %bmx160 %bmx160withcorrection %calibration
-Defines: -DSTANDALONE_CATCH1_TEST
-Libs: fmt
-Main: catch/fsm/test-fmm
+# [test-fmm]
+# Type: test
+# BoardId: stm32f429zi_skyward_death_stack_x
+# BinName: test-fmm
+# Include: %shared %deathstack-new %math %servo %xbee %pwm %spi %hbridge %ads1118 %internal-adc %sensormanager %ubloxgps %bmx160 %bmx160withcorrection %calibration
+# Defines: -DSTANDALONE_CATCH1_TEST
+# Libs: fmt eigen
+# Main: catch/fsm/test-fmm
[test-fmm-interactive]
Type: test
@@ -419,28 +440,28 @@ BoardId: stm32f429zi_skyward_death_stack_x
BinName: test-fmm-interactive
Include: %shared %deathstack-new %math %servo %xbee %pwm %spi %hbridge %ads1118 %internal-adc %sensormanager %ubloxgps %bmx160 %bmx160withcorrection %calibration
Defines: -DDEBUG
-Libs: fmt
+Libs: fmt eigen
Main: test-fmm-interactive
## FSM tests
-[test-ada]
-Type: test
-BoardId: stm32f429zi_skyward_death_stack_x
-BinName: test-ada
-Include: %shared %ada %test-utils %ada-test-sources %mock-sensors-data %logservice
-Defines: -DSTANDALONE_CATCH1_TEST -DDEBUG
-Libs: fmt
-Main: catch/fsm/test-ada
+# [test-ada]
+# Type: test
+# BoardId: stm32f429zi_skyward_death_stack_x
+# BinName: test-ada
+# Include: %shared %ada %test-utils %ada-test-sources %mock-sensors-data %logservice
+# Defines: -DSTANDALONE_CATCH1_TEST -DDEBUG
+# Libs: fmt
+# Main: catch/fsm/test-ada
-[test-ada-simulation]
-Type: test
-BoardId: stm32f429zi_skyward_death_stack_x
-BinName: test-ada-simulation
-Include: %shared %ada-test-sources %math %ubloxgps %deathstack-new %pwm %hbridge %spi %xbee %servo %sensormanager %ads1118 %bmx160 %bmx160withcorrection %internal-adc %calibration
-Defines: -DDEBUG -DSTANDALONE_CATCH1_TEST
-Libs: fmt eigen
-Main: catch/ada/ada_kalman_p/test-ada-simulation
+# [test-ada-simulation]
+# Type: test
+# BoardId: stm32f429zi_skyward_death_stack_x
+# BinName: test-ada-simulation
+# Include: %shared %ada-test-sources %math %ubloxgps %deathstack-new %pwm %hbridge %spi %xbee %servo %sensormanager %ads1118 %bmx160 %bmx160withcorrection %internal-adc %calibration
+# Defines: -DDEBUG -DSTANDALONE_CATCH1_TEST
+# Libs: fmt eigen
+# Main: catch/ada/ada_kalman_p/test-ada-simulation
[test-ada-dpl-simulation]
Type: test
@@ -469,23 +490,23 @@ Defines: -DSTANDALONE_CATCH1_TEST -DDEBUG
Libs: fmt
Main: catch/nas/test-nas-simulation
-[test-tmtc]
-Type: test
-BoardId: stm32f429zi_skyward_death_stack_x
-BinName: test-tmtc
-Include: %shared %test-utils %math %ubloxgps %deathstack-new %pwm %hbridge %spi %xbee %servo %sensormanager %ads1118 %bmx160 %bmx160withcorrection %internal-adc %calibration
-Defines: -DSTANDALONE_CATCH1_TEST -DDEBUG -DEIGEN_MAX_ALIGN_BYTES=0
-Libs: fmt
-Main: catch/fsm/test-tmtc
+# [test-tmtc]
+# Type: test
+# BoardId: stm32f429zi_skyward_death_stack_x
+# BinName: test-tmtc
+# Include: %shared %test-utils %math %ubloxgps %deathstack-new %pwm %hbridge %spi %xbee %servo %sensormanager %ads1118 %bmx160 %bmx160withcorrection %internal-adc %calibration
+# Defines: -DSTANDALONE_CATCH1_TEST -DDEBUG -DEIGEN_MAX_ALIGN_BYTES=0
+# Libs: fmt
+# Main: catch/fsm/test-tmtc
-[test-flightstatsrecorder]
-Type: test
-BoardId: stm32f429zi_skyward_death_stack_x
-BinName: test-flightstatsrecorder
-Include: %shared %test-utils %logservice
-Defines: -DSTANDALONE_CATCH1_TEST -DDEBUG
-Libs: fmt
-Main: catch/fsm/test-flightstatsrecorder
+# [test-flightstatsrecorder]
+# Type: test
+# BoardId: stm32f429zi_skyward_death_stack_x
+# BinName: test-flightstatsrecorder
+# Include: %shared %test-utils %logservice
+# Defines: -DSTANDALONE_CATCH1_TEST -DDEBUG
+# Libs: fmt
+# Main: catch/fsm/test-flightstatsrecorder
# [test-ignition]
# Type: testada_controller
@@ -497,14 +518,14 @@ Main: catch/fsm/test-flightstatsrecorder
# HIL testing algorithms
-[test-SerialInterface]
-Type: test
-BoardId: stm32f407vg_stm32f4discovery
-BinName: test-SerialInterface
-Include: %shared
-Defines: -DHIL_SERIALINTERFACE -DDEBUG
-Libs: fmt
-Main: hardware_in_the_loop/test-SerialInterface/test-SerialInterface
+# [test-SerialInterface]
+# Type: test
+# BoardId: stm32f407vg_stm32f4discovery
+# BinName: test-SerialInterface
+# Include: %shared
+# Defines: -DHIL_SERIALINTERFACE -DDEBUG
+# Libs: fmt
+# Main: hardware_in_the_loop/test-SerialInterface/test-SerialInterface
# [test-HIL]
# Type: test
@@ -571,32 +592,32 @@ Defines: -DDEBUG
Libs: fmt
Main: mock_sensors/test-mock-sensors
-[test-deployment]
-Type: test
-BoardId: stm32f429zi_skyward_death_stack_x
-BinName: test-deployment
-Include: %shared %test-utils %pwm %servo %deployment %hbridge %logservice
-Defines: -DSTANDALONE_CATCH1_TEST
-Libs: fmt
-Main: catch/fsm/test-deployment
+# [test-deployment]
+# Type: test
+# BoardId: stm32f429zi_skyward_death_stack_x
+# BinName: test-deployment
+# Include: %shared %test-utils %pwm %servo %deployment %hbridge %logservice
+# Defines: -DSTANDALONE_CATCH1_TEST
+# Libs: fmt
+# Main: catch/fsm/test-deployment
-[test-deployment-interactive]
-Type: test
-BoardId: stm32f429zi_skyward_death_stack_x
-BinName: test-deployment-interactive
-Include: %shared %test-utils %pwm %servo %deployment %hbridge %logservice
-Defines: -DSTANDALONE_CATCH1_TEST -DDEBUG
-Libs: fmt
-Main: deployment/test-deployment-interactive
+# [test-deployment-interactive]
+# Type: test
+# BoardId: stm32f429zi_skyward_death_stack_x
+# BinName: test-deployment-interactive
+# Include: %shared %test-utils %pwm %servo %deployment %hbridge %logservice
+# Defines: -DSTANDALONE_CATCH1_TEST -DDEBUG
+# Libs: fmt
+# Main: deployment/test-deployment-interactive
-[test-airbrakes]
-Type: test
-BoardId: stm32f429zi_skyward_death_stack_x
-BinName: test-airbrakes
-Include: %shared %test-utils %pwm %servo %logservice %airbrakes
-Defines: -DSTANDALONE_CATCH1_TEST
-Libs: fmt
-Main: catch/fsm/test-airbrakes
+# [test-airbrakes]
+# Type: test
+# BoardId: stm32f429zi_skyward_death_stack_x
+# BinName: test-airbrakes
+# Include: %shared %test-utils %pwm %servo %logservice %airbrakes
+# Defines: -DSTANDALONE_CATCH1_TEST
+# Libs: fmt
+# Main: catch/fsm/test-airbrakes
[test-airbrakes-interactive]
Type: test
@@ -617,39 +638,40 @@ Libs: fmt
Main: airbrakes/test-airbrakes-algorithm
# Comprehensive DeathStackX tests
+# (included in death-stack-x-testsuite)
-[test-power-board]
-Type: test
-BoardId: stm32f429zi_skyward_death_stack_x
-BinName: test-power-board
-Include: %shared %internal-adc %pwm %hbridge %test-utils %servo %logservice %airbrakes
-Defines: -DDEBUG
-Libs: fmt
-Main: deathstack-boards/test-power-board
+# [test-power-board]
+# Type: test
+# BoardId: stm32f429zi_skyward_death_stack_x
+# BinName: test-power-board
+# Include: %shared %internal-adc %pwm %hbridge %test-utils %servo %logservice %airbrakes
+# Defines: -DDEBUG
+# Libs: fmt
+# Main: deathstack-boards/test-power-board
-[test-stm-board]
-Type: test
-BoardId: stm32f429zi_skyward_death_stack_x
-BinName: test-stm-board
-Include: %shared
-Defines: -DDEBUG
-Libs: fmt
-Main: deathstack-boards/test-stm-board
+# [test-stm-board]
+# Type: test
+# BoardId: stm32f429zi_skyward_death_stack_x
+# BinName: test-stm-board
+# Include: %shared
+# Defines: -DDEBUG
+# Libs: fmt
+# Main: deathstack-boards/test-stm-board
-[test-rf-board]
-Type: test
-BoardId: stm32f429zi_skyward_death_stack_x
-BinName: test-rf-board
-Include: %shared %spi %ubloxgps %bmx160
-Defines: -DDEBUG
-Libs: fmt
-Main: deathstack-boards/test-rf-board
+# [test-rf-board]
+# Type: test
+# BoardId: stm32f429zi_skyward_death_stack_x
+# BinName: test-rf-board
+# Include: %shared %spi %ubloxgps %bmx160
+# Defines: -DDEBUG
+# Libs: fmt
+# Main: deathstack-boards/test-rf-board
-[test-analog-board]
-Type: test
-BoardId: stm32f429zi_skyward_death_stack_x
-BinName: test-analog-board
-Include: %shared %spi %ads1118 %pinhandler %logservice
-Defines: -DDEBUG
-Libs: fmt
-Main: deathstack-boards/test-analog-board
+# [test-analog-board]
+# Type: test
+# BoardId: stm32f429zi_skyward_death_stack_x
+# BinName: test-analog-board
+# Include: %shared %spi %ads1118 %pinhandler %logservice
+# Defines: -DDEBUG
+# Libs: fmt
+# Main: deathstack-boards/test-analog-board
diff --git a/scripts/flash-calibration-entry.sh b/scripts/flash-calibration-entry.sh
index fab345c681e66ad05ec480eb97cfaed645feeabb..f6f85e49db62848c9fd145e6890b3e2ebd2e682c 100755
--- a/scripts/flash-calibration-entry.sh
+++ b/scripts/flash-calibration-entry.sh
@@ -1,3 +1,3 @@
#!/bin/sh
-st-flash write bin_delivery/final/calibration-entry/calibration-entry.bin 0x8000000
+st-flash write bin_delivery/lynx/final/calibration-entry.bin 0x8000000
diff --git a/scripts/flash-entry.sh b/scripts/flash-entry.sh
index 0c4e66ac01f0b60d2e1a16a10b31b9d30aeb4a59..9c62109a6d5169bbe50df11ea2fc96827740cb78 100755
--- a/scripts/flash-entry.sh
+++ b/scripts/flash-entry.sh
@@ -1,3 +1,3 @@
#!/bin/sh
-st-flash write bin_delivery/final/death-stack-x-entry/death-stack-x-entry.bin 0x8000000
+st-flash write bin_delivery/lynx/final/death-stack-x-entry.bin 0x8000000
diff --git a/scripts/flash-ramtest.sh b/scripts/flash-ramtest.sh
index 8c1e4e111e0943de10e775f2963430063cbdc598..ed26dd8b4542171e7135a119052f07229b538af8 100755
--- a/scripts/flash-ramtest.sh
+++ b/scripts/flash-ramtest.sh
@@ -1,3 +1,3 @@
#!/bin/sh
-st-flash write bin_delivery/final/ramtest/ramtest.bin 0x8000000
+st-flash write bin_delivery/lynx/final/ramtest.bin 0x8000000
diff --git a/src/boards/DeathStack/ApogeeDetectionAlgorithm/ADAController.h b/src/boards/DeathStack/ApogeeDetectionAlgorithm/ADAController.h
index bd42cd03e6d72e91d6a4b13ac40485c5b6564f75..58fdb9a0afbc0fcc4cc63f0358478e5946373f1d 100644
--- a/src/boards/DeathStack/ApogeeDetectionAlgorithm/ADAController.h
+++ b/src/boards/DeathStack/ApogeeDetectionAlgorithm/ADAController.h
@@ -237,7 +237,7 @@ void ADAController<Press, GPS>::update()
{
// if new gps data available, update GPS, regardless of the current state
GPS gps_data = gps.getLastSample();
- if (gps_data.gps_timestamp > last_gps_timestamp)
+ if (gps_data.gps_timestamp != last_gps_timestamp)
{
last_gps_timestamp = gps_data.gps_timestamp;
@@ -247,7 +247,7 @@ void ADAController<Press, GPS>::update()
// if new pressure data available, update baro, according to current state
Press press_data = barometer.getLastSample();
- if (press_data.press_timestamp > last_press_timestamp)
+ if (press_data.press_timestamp != last_press_timestamp)
{
last_press_timestamp = press_data.press_timestamp;
diff --git a/src/boards/DeathStack/ApogeeDetectionAlgorithm/ADAData.h b/src/boards/DeathStack/ApogeeDetectionAlgorithm/ADAData.h
index 319b3f048fce860adba27323e02c47f22b04e6d5..dd1dc693708191b3dee252d6a6ffc23ceed2661a 100644
--- a/src/boards/DeathStack/ApogeeDetectionAlgorithm/ADAData.h
+++ b/src/boards/DeathStack/ApogeeDetectionAlgorithm/ADAData.h
@@ -24,7 +24,7 @@
#include <configs/ADAConfig.h>
#include <math/Stats.h>
-
+#include <Constants.h>
#include <ostream>
namespace DeathStackBoard
@@ -155,8 +155,8 @@ struct ADAReferenceValues
// Pressure at mean sea level for altitude calculation, to be updated with
// launch-day calibration
- float msl_pressure = DEFAULT_MSL_PRESSURE;
- float msl_temperature = DEFAULT_MSL_TEMPERATURE;
+ float msl_pressure = MSL_PRESSURE;
+ float msl_temperature = MSL_TEMPERATURE;
static std::string header()
{
diff --git a/src/boards/DeathStack/DeathStack.h b/src/boards/DeathStack/DeathStack.h
index 65040103fddb5693db136e2e7753b70646cfbe0d..1b9bf1ec36d4ebf5392a2e05539d65dc62061a98 100644
--- a/src/boards/DeathStack/DeathStack.h
+++ b/src/boards/DeathStack/DeathStack.h
@@ -119,7 +119,9 @@ public:
status.setError(&DeathStackStatus::pin_obs);
}
+#ifdef DEBUG
injector->start();
+#endif
logger->log(status);
@@ -202,7 +204,10 @@ private:
pin_handler = new PinHandler();
+#ifdef DEBUG
injector = new EventInjector();
+#endif
+
LOG_INFO(log, "Init finished");
}
diff --git a/src/boards/DeathStack/FlightStatsRecorder/FSRController.cpp b/src/boards/DeathStack/FlightStatsRecorder/FSRController.cpp
index 17d2293c1610d632cce498875087713552511d98..862574a93e4ec06d7f471d1467c8a406184e3814 100644
--- a/src/boards/DeathStack/FlightStatsRecorder/FSRController.cpp
+++ b/src/boards/DeathStack/FlightStatsRecorder/FSRController.cpp
@@ -125,15 +125,15 @@ void FlightStatsRecorder::update(const ADAData& t)
}
}
-void FlightStatsRecorder::update(const MS5803Data& t)
+void FlightStatsRecorder::update(const AirSpeedPitot& t)
{
switch (state)
{
- case FSRState::ASCENDING:
+ case FSRState::LIFTOFF:
{
- if (t.press < apogee_stats.digital_min_pressure)
+ if (fabs(t.airspeed) > liftoff_stats.airspeed_pitot_max)
{
- apogee_stats.digital_min_pressure = t.press;
+ liftoff_stats.airspeed_pitot_max = fabs(t.airspeed);
}
break;
}
@@ -142,15 +142,15 @@ void FlightStatsRecorder::update(const MS5803Data& t)
}
}
-void FlightStatsRecorder::update(const MPXHZ6130AData& t)
+void FlightStatsRecorder::update(const MS5803Data& t)
{
switch (state)
{
case FSRState::ASCENDING:
{
- if (t.press < apogee_stats.static_min_pressure)
+ if (t.press < apogee_stats.digital_min_pressure)
{
- apogee_stats.static_min_pressure = t.press;
+ apogee_stats.digital_min_pressure = t.press;
}
break;
}
@@ -159,28 +159,15 @@ void FlightStatsRecorder::update(const MPXHZ6130AData& t)
}
}
-// void FlightStatsRecorder::update(const SSCDRRN015PDAData& t)
-// {
-// switch (state)
-// {
-// case FSRState::ASCENDING:
-// {
-// break;
-// }
-// default:
-// break;
-// }
-// }
-
-void FlightStatsRecorder::update(const AirSpeedPitot& t)
+void FlightStatsRecorder::update(const MPXHZ6130AData& t)
{
switch (state)
{
case FSRState::ASCENDING:
{
- if (fabs(t.airspeed) > liftoff_stats.airspeed_pitot_max)
+ if (t.press < apogee_stats.static_min_pressure)
{
- liftoff_stats.airspeed_pitot_max = fabs(t.airspeed);
+ apogee_stats.static_min_pressure = t.press;
}
break;
}
@@ -313,6 +300,7 @@ void FlightStatsRecorder::state_idle(const Event& ev)
case EV_ENTRY:
{
LOG_DEBUG(log, "Entering IDLE state");
+
state = FSRState::IDLE;
StackLogger::getInstance()->updateStack(THID_STATS_FSM);
@@ -354,6 +342,7 @@ void FlightStatsRecorder::state_liftOff(const Event& ev)
case EV_ENTRY:
{
LOG_DEBUG(log, "Entering LIFTOFF state");
+
state = FSRState::LIFTOFF;
// Collect liftoff stats until this event is received
@@ -398,6 +387,7 @@ void FlightStatsRecorder::state_ascending(const Event& ev)
LOG_DEBUG(log, "Entering ASCENDING state");
state = FSRState::ASCENDING;
+
StackLogger::getInstance()->updateStack(THID_STATS_FSM);
break;
}
diff --git a/src/boards/DeathStack/FlightStatsRecorder/FSRController.h b/src/boards/DeathStack/FlightStatsRecorder/FSRController.h
index 03f6e7ac9a7f099c638a0c8d58421989a3d4f988..080c920925280100cc3cd37684b2689c10f9572a 100644
--- a/src/boards/DeathStack/FlightStatsRecorder/FSRController.h
+++ b/src/boards/DeathStack/FlightStatsRecorder/FSRController.h
@@ -63,7 +63,6 @@ public:
void update(const CurrentSensorData& t);
void update(const MS5803Data& t); // digitl baro
void update(const MPXHZ6130AData& t); // static ports baro
- // void update(const SSCDRRN015PDAData& t); // pitot baro
void update(const SSCDANN030PAAData& t); // DPL vane baro
void update(const AirSpeedPitot& t);
diff --git a/src/boards/DeathStack/Main/Radio.cpp b/src/boards/DeathStack/Main/Radio.cpp
index 6cd31b1728c20a3b88d430d27553144a8cf8fb1b..26953ff68a09787e95c773bc023fb3014d504759 100644
--- a/src/boards/DeathStack/Main/Radio.cpp
+++ b/src/boards/DeathStack/Main/Radio.cpp
@@ -62,10 +62,10 @@ Radio::Radio(SPIBusInterface& xbee_bus) : xbee_bus(xbee_bus)
xbee->setOnFrameReceivedListener(
bind(&Radio::onXbeeFrameReceived, this, _1));
- Xbee::setDataRate(*xbee, XBEE_80KBPS_DATA_RATE, 5000);
+ Xbee::setDataRate(*xbee, XBEE_80KBPS_DATA_RATE, XBEE_TIMEOUT);
- mav_driver = new MavDriver(xbee, handleMavlinkMessage, 0,
- 1000); // TODO: Use settings
+ mav_driver = new MavDriver(xbee, handleMavlinkMessage, SLEEP_AFTER_SEND,
+ MAV_OUT_BUFFER_MAX_AGE);
tmtc_manager = new TMTCController();
diff --git a/src/boards/DeathStack/Main/Sensors.cpp b/src/boards/DeathStack/Main/Sensors.cpp
index 674625c51d09e37ac0761e020d0a57636eaa612a..119a52cbe5f9b1d97a0c8acad15a8173a7259519 100644
--- a/src/boards/DeathStack/Main/Sensors.cpp
+++ b/src/boards/DeathStack/Main/Sensors.cpp
@@ -85,11 +85,10 @@ Sensors::~Sensors()
delete hil_imu;
delete hil_baro;
delete hil_gps;
-#else
+#endif
delete imu_bmx160;
delete press_digital;
delete gps_ublox;
-#endif
delete internal_adc;
delete cs_cutter_primary;
delete cs_cutter_backup;
@@ -134,7 +133,7 @@ void Sensors::calibrate()
imu_bmx160_with_correction->getGyroscopeBiases());
press_pitot->calibrate();
- // wait calibration end
+ // wait pitot calibration end
while (press_pitot->isCalibrating())
{
Thread::sleep(10);
@@ -604,31 +603,31 @@ void Sensors::updateSensorsStatus()
info = sensor_manager->getSensorInfo(imu_bmx160);
if (!info.is_initialized)
{
- status.bmx160 = 0;
+ status.bmx160 = SensorDriverStatus::DRIVER_ERROR;
}
info = sensor_manager->getSensorInfo(mag_lis3mdl);
if (!info.is_initialized)
{
- status.lis3mdl = 0;
+ status.lis3mdl = SensorDriverStatus::DRIVER_ERROR;
}
info = sensor_manager->getSensorInfo(gps_ublox);
if (!info.is_initialized)
{
- status.gps = 0;
+ status.gps = SensorDriverStatus::DRIVER_ERROR;
}
info = sensor_manager->getSensorInfo(internal_adc);
if (!info.is_initialized)
{
- status.internal_adc = 0;
+ status.internal_adc = SensorDriverStatus::DRIVER_ERROR;
}
info = sensor_manager->getSensorInfo(adc_ads1118);
if (!info.is_initialized)
{
- status.ads1118 = 0;
+ status.ads1118 = SensorDriverStatus::DRIVER_ERROR;
}
}
diff --git a/src/boards/DeathStack/Main/SensorsData.h b/src/boards/DeathStack/Main/SensorsData.h
index 7a5135bbd08fd7d89e4ab284a5711c01684cf48a..7ff9615e975deabea4899b6c8ccfb7b4ba7c285c 100644
--- a/src/boards/DeathStack/Main/SensorsData.h
+++ b/src/boards/DeathStack/Main/SensorsData.h
@@ -39,16 +39,20 @@ struct AirSpeedPitot
}
};
-struct SensorsStatus
+enum SensorDriverStatus
{
- uint8_t bmx160 = 1;
- uint8_t ms5803 = 1;
- uint8_t lis3mdl = 1;
- uint8_t gps = 1;
- uint8_t internal_adc = 1;
- uint8_t ads1118 = 1;
+ DRIVER_ERROR = 0,
+ DRIVER_OK = 1
+};
- SensorsStatus() {}
+struct SensorsStatus
+{
+ uint8_t bmx160 = DRIVER_OK;
+ uint8_t ms5803 = DRIVER_OK;
+ uint8_t lis3mdl = DRIVER_OK;
+ uint8_t gps = DRIVER_OK;
+ uint8_t internal_adc = DRIVER_OK;
+ uint8_t ads1118 = DRIVER_OK;
static std::string header()
{
@@ -57,8 +61,9 @@ struct SensorsStatus
void print(std::ostream& os) const
{
- os << bmx160 << "," << ms5803 << "," << lis3mdl << "," << gps << ","
- << internal_adc << "," << ads1118 << "\n";
+ os << (int)bmx160 << "," << (int)ms5803 << "," << (int)lis3mdl << ","
+ << (int)gps << "," << (int)internal_adc << "," << (int)ads1118
+ << "\n";
}
};
diff --git a/src/boards/DeathStack/NavigationAttitudeSystem/ExtendedKalmanEigen.cpp b/src/boards/DeathStack/NavigationAttitudeSystem/ExtendedKalmanEigen.cpp
index c4aab93f46cc15a8ec12963cc9e124e20b7aa48f..238a6bbe2089a86f70a9db78eeaea6dc463865c5 100644
--- a/src/boards/DeathStack/NavigationAttitudeSystem/ExtendedKalmanEigen.cpp
+++ b/src/boards/DeathStack/NavigationAttitudeSystem/ExtendedKalmanEigen.cpp
@@ -216,7 +216,7 @@ void ExtendedKalmanEigen::predictMEKF(const Vector3f& u)
Gatt * Q_mag * Gatttr; // Update only the attitude related part of P
}
-void ExtendedKalmanEigen::correct_MEKF(const Vector3f& y)
+void ExtendedKalmanEigen::correctMEKF(const Vector3f& y)
{
Matrix3f A;
Vector3f z;
diff --git a/src/boards/DeathStack/NavigationAttitudeSystem/ExtendedKalmanEigen.h b/src/boards/DeathStack/NavigationAttitudeSystem/ExtendedKalmanEigen.h
index 20b81c89233e6b57173f82ceff75d8e92e4d0288..ce89a4a91ff55e1b43169490874170670c847bf8 100644
--- a/src/boards/DeathStack/NavigationAttitudeSystem/ExtendedKalmanEigen.h
+++ b/src/boards/DeathStack/NavigationAttitudeSystem/ExtendedKalmanEigen.h
@@ -80,7 +80,7 @@ public:
*
* @param y 3x1 Vector of the magnetometer readings [mx my mz].
*/
- void correct_MEKF(const Vector3f& y);
+ void correctMEKF(const Vector3f& y);
/**
* @return 13x1 State vector [px py pz vx vy vz qx qy qz qw bx by bz].
diff --git a/src/boards/DeathStack/NavigationAttitudeSystem/InitStates.h b/src/boards/DeathStack/NavigationAttitudeSystem/InitStates.h
index 9b02b0a4f3644f24724c95168b2db70996ead8b4..91b16d10e3850f6c66d46c89a69d6729c246fbac 100644
--- a/src/boards/DeathStack/NavigationAttitudeSystem/InitStates.h
+++ b/src/boards/DeathStack/NavigationAttitudeSystem/InitStates.h
@@ -65,18 +65,13 @@ public:
* @param acc 3x1 accelerometer readings [ax ay az].
* @param mag 3x1 magnetometer readings [mx my mz].
*/
- void triad(const Vector3f acc, const Vector3f mag);
+ const Vector3f& triad(const Vector3f acc, const Vector3f mag);
/**
* @brief Initialization of the positions before the liftoff.
- *
- * @param gps_lat Latitude.
- * @param gps_lon Longitude.
* @param press Reference pressure [Pa].
- * @param press Reference temperature [Pa].
*/
- void positionInit(const float gps_lat, const float gps_lon,
- const float press); //, const float temp);
+ void positionInit(const float press); //, const float temp);
/**
* @brief Initialization of the velocities before the liftoff.
*
@@ -125,14 +120,14 @@ void InitStates::eCompass(const Vector3f acc, const Vector3f mag)
x_init(NL + 3) = x_quat(3);
}
-void InitStates::triad(const Vector3f acc, const Vector3f mag)
+const Vector3f& InitStates::triad(const Vector3f acc, const Vector3f mag)
{
LOG_DEBUG(log, "Executing TRIAD");
// The coulmns of the the triad matrices. b:body, i:inertial
Vector3f t1b, t2b, t3b, t1i, t2i, t3i;
- // vettore gravità "vero" in NED, normalizzato
+ // vettore gravità "vero" in NED, normalizzato :
// -1 su asse "down", perché da fermo l'accelerometro
// misura la reazione vincolare (rivolta verso l'alto)
Vector3f g_norm(0.0F, 0.0F, -1.0F);
@@ -163,20 +158,16 @@ void InitStates::triad(const Vector3f acc, const Vector3f mag)
x_init(NL + 1) = x_quat(1);
x_init(NL + 2) = x_quat(2);
x_init(NL + 3) = x_quat(3);
+
+ return eul;
}
-void InitStates::positionInit(const float gps_lat, const float gps_lon,
- const float press) //, const float temp)
+void InitStates::positionInit(const float press)
{
- x_init(0) = EARTH_RADIUS * gps_lon * CLAT;
- x_init(1) = EARTH_RADIUS * gps_lat;
+ x_init(0) = 0.0; // EARTH_RADIUS * gps_lon * CLAT;
+ x_init(1) = 0.0; // EARTH_RADIUS * gps_lat;
x_init(2) = aeroutils::relAltitude(press, MSL_PRESSURE,
MSL_TEMPERATURE); // msl altitude
-
- // starting point centered in the launchpad
- // x_init(0) = 0.0f;
- // x_init(1) = 0.0f;
- // x_init(2) = 0.0f; // agl altitude
}
void InitStates::velocityInit()
@@ -188,6 +179,8 @@ void InitStates::velocityInit()
void InitStates::biasInit()
{
+ // gyro biases set to zero since the sensor performs
+ // self-calibration and the measurements are already compensated
x_init(NL + 4) = 0.0F;
x_init(NL + 5) = 0.0F;
x_init(NL + 6) = 0.0F;
diff --git a/src/boards/DeathStack/NavigationAttitudeSystem/NAS.h b/src/boards/DeathStack/NavigationAttitudeSystem/NAS.h
index c01f8f795073c465b2219028becf6444516a8201..24afbe9a8dda6ed7b887e271167335fd076a23c2 100644
--- a/src/boards/DeathStack/NavigationAttitudeSystem/NAS.h
+++ b/src/boards/DeathStack/NavigationAttitudeSystem/NAS.h
@@ -113,6 +113,7 @@ private:
ExtendedKalmanEigen filter;
InitStates states_init;
NASReferenceValues ref_values;
+ Vector3f triad_result_eul;
uint64_t last_gps_timestamp = 0;
uint64_t last_accel_timestamp = 0;
@@ -141,8 +142,7 @@ NAS<IMU, Press, GPS>::NAS(Sensor<IMU>& imu, Sensor<Press>& baro,
template <typename IMU, typename Press, typename GPS>
bool NAS<IMU, Press, GPS>::init()
{
- states_init.positionInit(ref_values.ref_latitude, ref_values.ref_longitude,
- ref_values.ref_pressure);
+ states_init.positionInit(ref_values.ref_pressure);
states_init.velocityInit();
@@ -153,7 +153,8 @@ bool NAS<IMU, Press, GPS>::init()
acc_init.normalize();
mag_init.normalize();
- states_init.triad(acc_init, mag_init);
+ triad_result_eul = states_init.triad(acc_init, mag_init);
+
states_init.biasInit();
x = states_init.getInitX();
@@ -215,7 +216,7 @@ NASData NAS<IMU, Press, GPS>::sampleImpl()
}
// check if new pressure data is available
- if (pressure_data.press_timestamp > last_press_timestamp)
+ if (pressure_data.press_timestamp != last_press_timestamp)
{
last_press_timestamp = pressure_data.press_timestamp;
@@ -223,20 +224,20 @@ NASData NAS<IMU, Press, GPS>::sampleImpl()
}
// check if new gps data is available and the gps has fix
- if (gps_data.gps_timestamp > last_gps_timestamp && gps_data.fix == true)
+ if (gps_data.gps_timestamp != last_gps_timestamp && gps_data.fix == true)
{
last_gps_timestamp = gps_data.gps_timestamp;
- // float delta_lon = gps_data.longitude - ref_values.ref_longitude;
- // float delta_lat = gps_data.latitude - ref_values.ref_latitude;
+ float delta_lon = gps_data.longitude - ref_values.ref_longitude;
+ float delta_lat = gps_data.latitude - ref_values.ref_latitude;
- Vector4f gps_readings(gps_data.longitude, gps_data.latitude,
- gps_data.velocity_north, gps_data.velocity_east);
+ Vector4f gps_readings(delta_lon, delta_lat, gps_data.velocity_north,
+ gps_data.velocity_east);
filter.correctGPS(gps_readings, gps_data.num_satellites);
}
// check if new magnetometer data is available
- if (imu_data.mag_timestamp > last_mag_timestamp)
+ if (imu_data.mag_timestamp != last_mag_timestamp)
{
Vector3f mag_readings(imu_data.mag_x, imu_data.mag_y, imu_data.mag_z);
@@ -245,7 +246,7 @@ NASData NAS<IMU, Press, GPS>::sampleImpl()
last_mag_timestamp = imu_data.mag_timestamp;
mag_readings.normalize();
- filter.correct_MEKF(mag_readings);
+ filter.correctMEKF(mag_readings);
}
}
@@ -258,7 +259,7 @@ NASData NAS<IMU, Press, GPS>::sampleImpl()
nas_data.y = x(1);
nas_data.z =
-x(2) - z_init; // Negative sign because we're working in the NED
- // frame but we want a positive altitude as output.
+ // frame but we want a positive altitude as output.
nas_data.vx = x(3);
nas_data.vy = x(4);
nas_data.vz = -x(5);
@@ -298,15 +299,15 @@ template <typename IMU, typename Press, typename GPS>
NASTriadResult NAS<IMU, Press, GPS>::getTriadResult()
{
Matrix<float, N, 1> state = states_init.getInitX();
- Vector3f e = quat.quat2eul({state(6), state(7), state(8), state(9)});
+ //Vector3f e = quat.quat2eul({state(6), state(7), state(8), state(9)});
NASTriadResult result;
result.x = state(0);
result.y = state(1);
result.z = -state(2);
- result.roll = e(0);
- result.pitch = e(1);
- result.yaw = e(2);
+ result.roll = triad_result_eul(0); //e(0);
+ result.pitch = triad_result_eul(1); //e(1);
+ result.yaw = triad_result_eul(2); //e(2);
return result;
}
diff --git a/src/boards/DeathStack/NavigationAttitudeSystem/NASController.h b/src/boards/DeathStack/NavigationAttitudeSystem/NASController.h
index 64ec8b5993e81953bd8178355451c73443066306..9ac0863f4815a6e007b18614c6c848d4a808c5a9 100644
--- a/src/boards/DeathStack/NavigationAttitudeSystem/NASController.h
+++ b/src/boards/DeathStack/NavigationAttitudeSystem/NASController.h
@@ -154,21 +154,6 @@ void NASController<IMU, Press, GPS>::update()
{
Lock<FastMutex> l(mutex);
- // Add samples to the calibration
- if (press_data.press_timestamp > last_press_timestamp)
- {
- last_press_timestamp = press_data.press_timestamp;
- calibrator.addBaroSample(press_data.press);
- }
-
- if (gps_data.fix == true &&
- gps_data.gps_timestamp > last_gps_timestamp)
- {
- last_gps_timestamp = gps_data.gps_timestamp;
- calibrator.addGPSSample(gps_data.latitude,
- gps_data.longitude);
- }
-
// Accel and gyro sampled at higher rate than the NAS
// => always new sample available
{
@@ -177,13 +162,28 @@ void NASController<IMU, Press, GPS>::update()
imu_data.accel_x, imu_data.accel_y, imu_data.accel_z);
}
- //if (imu_data.mag_timestamp > last_mag_timestamp)
+ if (imu_data.mag_timestamp != last_mag_timestamp)
{
last_mag_timestamp = imu_data.mag_timestamp;
calibrator.addMagSample(imu_data.mag_x, imu_data.mag_y,
imu_data.mag_z);
}
+ // Add samples to the calibration
+ if (press_data.press_timestamp != last_press_timestamp)
+ {
+ last_press_timestamp = press_data.press_timestamp;
+ calibrator.addBaroSample(press_data.press);
+ }
+
+ if (gps_data.fix == true &&
+ gps_data.gps_timestamp != last_gps_timestamp)
+ {
+ last_gps_timestamp = gps_data.gps_timestamp;
+ calibrator.addGPSSample(gps_data.latitude,
+ gps_data.longitude);
+ }
+
// Save the state of calibration to release mutex
end_calib = calibrator.calibIsComplete();
}
diff --git a/src/boards/DeathStack/System/TaskID.h b/src/boards/DeathStack/System/TaskID.h
index 8eb3d717d3ab1624c21dfc09316ba8e82a5a56ae..04a664a7a1805533ee4e79c90368f801888ce5c3 100644
--- a/src/boards/DeathStack/System/TaskID.h
+++ b/src/boards/DeathStack/System/TaskID.h
@@ -31,17 +31,17 @@ namespace DeathStackBoard
*/
enum TaskIDs : uint8_t
{
- TASK_SENSORS_6_MS_ID,
- TASK_SENSORS_10_MS_ID,
- TASK_SENSORS_15_MS_ID,
- TASK_SENSORS_20_MS_ID,
- TASK_SENSORS_24_MS_ID,
- TASK_SENSORS_40_MS_ID,
- TASK_SENSORS_50_MS_ID,
- TASK_ADA_ID,
- TASK_ABK_ID,
- TASK_NAS_ID,
- TASK_SCHEDULER_STATS_ID
+ TASK_SCHEDULER_STATS_ID = 0,
+ TASK_SENSORS_6_MS_ID = 1,
+ TASK_SENSORS_10_MS_ID = 2,
+ TASK_SENSORS_15_MS_ID = 3,
+ TASK_SENSORS_20_MS_ID = 4,
+ TASK_SENSORS_24_MS_ID = 5,
+ TASK_SENSORS_40_MS_ID = 6,
+ TASK_SENSORS_50_MS_ID = 7,
+ TASK_ADA_ID = 8,
+ TASK_ABK_ID = 9,
+ TASK_NAS_ID = 10
};
-}
\ No newline at end of file
+} // namespace DeathStackBoard
\ No newline at end of file
diff --git a/src/boards/DeathStack/TelemetriesTelecommands/TCHandler.cpp b/src/boards/DeathStack/TelemetriesTelecommands/TCHandler.cpp
index 5abdfbc1152e7217005664328850fee887e0bac6..c3715918c3f0692b6932833d9b75b21e1efbb41b 100644
--- a/src/boards/DeathStack/TelemetriesTelecommands/TCHandler.cpp
+++ b/src/boards/DeathStack/TelemetriesTelecommands/TCHandler.cpp
@@ -71,7 +71,7 @@ const std::map<uint8_t, uint8_t> tcMap = {
void handleMavlinkMessage(MavDriver* mav_driver, const mavlink_message_t& msg)
{
// log status
- logger->log(mav_driver->getStatus());
+ logMavlinkStatus(mav_driver);
// acknowledge
sendAck(mav_driver, msg);
@@ -101,10 +101,12 @@ void handleMavlinkMessage(MavDriver* mav_driver, const mavlink_message_t& msg)
case MAV_CMD_STOP_LOGGING:
logger->stop();
sendTelemetry(mav_driver, MAV_LOGGER_TM_ID);
+ LOG_INFO(print_logger, "Received command CLOSE_LOG");
break;
case MAV_CMD_START_LOGGING:
DeathStack::getInstance()->startLogger();
sendTelemetry(mav_driver, MAV_LOGGER_TM_ID);
+ LOG_INFO(print_logger, "Received command START_LOG");
break;
default:
break;
@@ -237,9 +239,16 @@ bool sendTelemetry(MavDriver* mav_driver, const uint8_t tm_id)
mav_driver->enqueueMsg(TmRepository::getInstance()->packTM(tm_id));
// update status
- logger->log(mav_driver->getStatus());
+ logMavlinkStatus(mav_driver);
return ok;
}
+void logMavlinkStatus(MavDriver* mav_driver)
+{
+ MavlinkStatus status = mav_driver->getStatus();
+ status.timestamp = TimestampTimer::getTimestamp();
+ logger->log(status);
+}
+
} // namespace DeathStackBoard
\ No newline at end of file
diff --git a/src/boards/DeathStack/TelemetriesTelecommands/TCHandler.h b/src/boards/DeathStack/TelemetriesTelecommands/TCHandler.h
index 9028e5fc48b38b06fe8dfabc594cda75909dfc05..7ac29dc46b5468ce61cdc503d24cd4dac98a68b3 100644
--- a/src/boards/DeathStack/TelemetriesTelecommands/TCHandler.h
+++ b/src/boards/DeathStack/TelemetriesTelecommands/TCHandler.h
@@ -35,6 +35,8 @@ void sendAck(MavDriver* mav_driver, const mavlink_message_t& msg);
bool sendTelemetry(MavDriver* mav_driver, const uint8_t tm_id);
+void logMavlinkStatus(MavDriver* mav_driver);
+
static LoggerService* logger = LoggerService::getInstance();
static PrintLogger print_logger =
diff --git a/src/boards/DeathStack/TelemetriesTelecommands/TMTCController.cpp b/src/boards/DeathStack/TelemetriesTelecommands/TMTCController.cpp
index bf08f28114782c323230d9a6f1125684ee91d76f..cc53a4a4c9931cddca2b4ad0ccf038b3ffa7dbc0 100644
--- a/src/boards/DeathStack/TelemetriesTelecommands/TMTCController.cpp
+++ b/src/boards/DeathStack/TelemetriesTelecommands/TMTCController.cpp
@@ -50,7 +50,9 @@ bool TMTCController::send(const uint8_t tm_id)
// guarantee synchronicity)
bool ok = mav_driver->enqueueMsg(tm_repo->packTM(tm_id));
// update status
- logger.log(mav_driver->getStatus());
+ MavlinkStatus status = mav_driver->getStatus();
+ status.timestamp = TimestampTimer::getTimestamp();
+ logger.log(status);
return ok;
}
diff --git a/src/boards/DeathStack/TelemetriesTelecommands/TmRepository.cpp b/src/boards/DeathStack/TelemetriesTelecommands/TmRepository.cpp
index 9cd6717a6d3a4a2998607a5cf5f08c16dca5d236..f40e530f44d977141fbb6703d37fb43c5bb84971 100644
--- a/src/boards/DeathStack/TelemetriesTelecommands/TmRepository.cpp
+++ b/src/boards/DeathStack/TelemetriesTelecommands/TmRepository.cpp
@@ -33,81 +33,10 @@
namespace DeathStackBoard
{
-/* Periodic TM updaters */
-// bool TmRepository::updateHR()
-// {
-// // HighRateTMPacker packer(tm_repository.hr_tm.payload);
-
-// // packer.packTimestamp(miosix::getTick(), curHrIndex);
-
-// // packer.packPressureAda(hr_pkt.pressure_ada, curHrIndex);
-// // packer.packPressureDigi(hr_pkt.pressure_digi, curHrIndex);
-// // packer.packMslAltitude(hr_pkt.msl_altitude, curHrIndex);
-// // packer.packAglAltitude(hr_pkt.agl_altitude, curHrIndex);
-// // packer.packVertSpeed(hr_pkt.vert_speed, curHrIndex);
-// // packer.packVertSpeed2(hr_pkt.vert_speed_2, curHrIndex);
-// // packer.packAccX(hr_pkt.acc_x, curHrIndex);
-// // packer.packAccY(hr_pkt.acc_y, curHrIndex);
-// // packer.packAccZ(hr_pkt.acc_z, curHrIndex);
-// // packer.packGyroX(hr_pkt.gyro_x, curHrIndex);
-// // packer.packGyroY(hr_pkt.gyro_y, curHrIndex);
-// // packer.packGyroZ(hr_pkt.gyro_z, curHrIndex);
-// // packer.packGpsLat(hr_pkt.gps_lat, curHrIndex);
-// // packer.packGpsLon(hr_pkt.gps_lon, curHrIndex);
-// // packer.packGpsAlt(hr_pkt.gps_alt, curHrIndex);
-// // packer.packGpsFix(hr_pkt.gps_fix, curHrIndex);
-// // packer.packTemperature(hr_pkt.temperature, curHrIndex);
-// // packer.packFmmState(hr_pkt.fmm_state, curHrIndex);
-// // packer.packDplState(hr_pkt.dpl_state, curHrIndex);
-// // packer.packPinLaunch(hr_pkt.pin_launch, curHrIndex);
-// // packer.packPinNosecone(hr_pkt.pin_nosecone, curHrIndex);
-
-// // curHrIndex = (curHrIndex + 1) % N_PKT_HR;
-
-// // if (curHrIndex == 0)
-// // return true;
-// // else
-// return false;
-// }
+TmRepository::TmRepository() { stats_rec.start(); }
-// void TmRepository::updateLR()
-// {
-// // LowRateTMPacker packer(tm_repository.tm_repository.lr_tm.payload);
-
-// // packer.packLiftoffTs(tm_repository.lr_tm.liftoff_ts, 0);
-// // packer.packLiftoffMaxAccTs(tm_repository.lr_tm.liftoff_max_acc_ts,
-// 0);
-// // packer.packLiftoffMaxAcc(tm_repository.lr_tm.liftoff_max_acc_ts,
-// 0);
-// // packer.packMaxZspeedTs(tm_repository.lr_tm.max_zspeed_ts, 0);
-// // packer.packMaxZspeed(tm_repository.lr_tm.max_zspeed, 0);
-// // packer.packMaxSpeedAltitude(tm_repository.lr_tm.max_speed_altitude,
-// 0);
-// // packer.packApogeeTs(tm_repository.lr_tm.apogee_ts, 0);
-// // packer.packNxpMinPressure(tm_repository.lr_tm.nxp_min_pressure,
-// 0);
-// // packer.packHwMinPressure(tm_repository.lr_tm.hw_min_pressure, 0);
-// // packer.packKalmanMinPressure(tm_repository.lr_tm.kalman_min_pressure,
-// 0);
-// //
-// packer.packDigitalMinPressure(tm_repository.lr_tm.digital_min_pressure,
-// 0);
-// // packer.packBaroMaxAltitutde(tm_repository.lr_tm.baro_max_altitutde,
-// 0);
-// // packer.packGpsMaxAltitude(tm_repository.lr_tm.gps_max_altitude,
-// 0);
-// // packer.packApogeeLat(tm_repository.lr_tm.apogee_lat, 0);
-// // packer.packApogeeLon(tm_repository.lr_tm.apogee_lon, 0);
-// // packer.packDrogueDplTs(tm_repository.lr_tm.drogue_dpl_ts, 0);
-// // packer.packDrogueDplMaxAcc(tm_repository.lr_tm.drogue_dpl_max_acc,
-// 0);
-// // packer.packMainDplTs(tm_repository.lr_tm.main_dpl_ts, 0);
-// // packer.packMainDplAltitude(tm_repository.lr_tm.main_dpl_altitude,
-// 0);
-// // packer.packMainDplZspeed(tm_repository.lr_tm.main_dpl_zspeed, 0);
-// // packer.packMainDplAcc(tm_repository.lr_tm.main_dpl_acc, 0);
-// }
+TmRepository::~TmRepository() {}
/* TM getter */
@@ -363,8 +292,6 @@ void TmRepository::update<SSCDRRN015PDAData>(const SSCDRRN015PDAData& t)
tm_repository.wind_tm.pressure_differential = t.press;
tm_repository.sensors_tm.pitot_press = t.press;
tm_repository.adc_tm.pitot_pressure = t.press;
-
- // stats_rec.update(t);
}
template <>
@@ -550,9 +477,8 @@ void TmRepository::update<NASKalmanState>(const NASKalmanState& t)
tm_repository.nas_tm.pitch = orientation(1);
tm_repository.nas_tm.yaw = orientation(2);
- // Positions converted from meters to lat/lon for visualization
- tm_repository.hr_tm.nas_x = t.x0 / EARTH_RADIUS;
- tm_repository.hr_tm.nas_y = t.x1 / EARTH_RADIUS;
+ tm_repository.hr_tm.nas_x = t.x0;
+ tm_repository.hr_tm.nas_y = t.x1;
// Altitude has negative sign because we're working in the NED
// frame but we want a positive altitude as output (same for vz)
tm_repository.hr_tm.nas_z = -t.x2;
diff --git a/src/boards/DeathStack/TelemetriesTelecommands/TmRepository.h b/src/boards/DeathStack/TelemetriesTelecommands/TmRepository.h
index cbcefd3b46e71a6d4325bd92723a1b34d519f1d4..95c541f6c0ea6f36881e7242c693ecb50b47779f 100644
--- a/src/boards/DeathStack/TelemetriesTelecommands/TmRepository.h
+++ b/src/boards/DeathStack/TelemetriesTelecommands/TmRepository.h
@@ -99,8 +99,8 @@ public:
void sendTelemetry(MavDriver* mav_driver, const uint8_t tm_id);
private:
- TmRepository() {}
- ~TmRepository() {}
+ TmRepository();
+ ~TmRepository();
/**
* @brief Struct containing all TMs in the form of mavlink messages.
diff --git a/src/boards/DeathStack/configs/ADAConfig.h b/src/boards/DeathStack/configs/ADAConfig.h
index 3fdc662e6c57b3dd67f19cbfd953cb5350bcf825..0ac55832562bca3f170ab2d2c60d4a2425fc1dd3 100644
--- a/src/boards/DeathStack/configs/ADAConfig.h
+++ b/src/boards/DeathStack/configs/ADAConfig.h
@@ -64,9 +64,6 @@ static const float DEFAULT_REFERENCE_TEMPERATURE = 278.920f;
static const float DEFAULT_REFERENCE_ALTITUDE = 1420.0f;
static const float DEFAULT_REFERENCE_PRESSURE = 85389.4f;
-static const float DEFAULT_MSL_TEMPERATURE = 288.15f;
-static const float DEFAULT_MSL_PRESSURE = 101325.0f;
-
// Deployment altitude AGL
static const float DEFAULT_DEPLOYMENT_ALTITUDE = -100;
diff --git a/src/boards/DeathStack/configs/FlightStatsConfig.h b/src/boards/DeathStack/configs/FlightStatsConfig.h
index fded1fa69105a6fdf3aea77c525c1cb25ce61358..eb7fe88fc016a12ea6f3e4f1af232ff0e9972d46 100644
--- a/src/boards/DeathStack/configs/FlightStatsConfig.h
+++ b/src/boards/DeathStack/configs/FlightStatsConfig.h
@@ -29,10 +29,10 @@ namespace DeathStackBoard
namespace FlightStatsConfig
{
-static constexpr long long TIMEOUT_LIFTOFF_STATS = 7500;
-static constexpr long long TIMEOUT_APOGEE_STATS = 1500;
-static constexpr long long TIMEOUT_DROGUE_DPL_STATS = 10000;
-static constexpr long long TIMEOUT_MAIN_DPL_STATS = 10000;
+static constexpr long long TIMEOUT_LIFTOFF_STATS = 7500; // [ms]
+static constexpr long long TIMEOUT_APOGEE_STATS = 1500; // [ms]
+static constexpr long long TIMEOUT_DROGUE_DPL_STATS = 10000; // [ms]
+static constexpr long long TIMEOUT_MAIN_DPL_STATS = 10000; // [ms]
} // namespace FlightStatsConfig
} // namespace DeathStackBoard
\ No newline at end of file
diff --git a/src/boards/DeathStack/configs/TMTCConfig.h b/src/boards/DeathStack/configs/TMTCConfig.h
index 8713a5dffaa747516513974869ef661198474e19..b11b0065c85d015140c50bae33a22c97874e13be 100644
--- a/src/boards/DeathStack/configs/TMTCConfig.h
+++ b/src/boards/DeathStack/configs/TMTCConfig.h
@@ -30,7 +30,7 @@ namespace DeathStackBoard
{
/* Min guaranteed sleep time after each packet sent (milliseconds) */
-static const uint16_t TMTC_SLEEP_AFTER_SEND = 0;
+static const uint16_t SLEEP_AFTER_SEND = 0;
/* Xbee */
typedef miosix::xbee::cs XbeeCS;
@@ -38,6 +38,7 @@ typedef miosix::xbee::attn XbeeATTN;
typedef miosix::xbee::reset XbeeRST;
/* Xbee data rate (80 or 10 kbps) */
static const bool XBEE_80KBPS_DATA_RATE = true;
+static const bool XBEE_TIMEOUT = 5000; // milliseconds
/* Periodic telemetries periods */
static const unsigned int LR_TM_TIMEOUT = 1000;
diff --git a/src/entrypoints/death-stack-x-decoder/LogTypes.h b/src/entrypoints/death-stack-x-decoder/LogTypes.h
index 4af5f522a374a5979c87dfbb10be4b37f3561db6..1afc81d2f7492aa84a46e90262f8767b436e1672 100644
--- a/src/entrypoints/death-stack-x-decoder/LogTypes.h
+++ b/src/entrypoints/death-stack-x-decoder/LogTypes.h
@@ -56,6 +56,7 @@
#include "events/EventData.h"
#include "logger/Deserializer.h"
#include "scheduler/TaskSchedulerData.h"
+#include "../../hardware_in_the_loop/HIL_sensors/HILSensorsData.h"
// Serialized classes
using std::ofstream;
@@ -125,6 +126,7 @@ void registerTypes(Deserializer& ds)
registerType<ADAData>(ds);
registerType<ADAReferenceValues>(ds);
registerType<TargetDeploymentAltitude>(ds);
+ registerType<ApogeeDetected>(ds);
// NAS
registerType<NASStatus>(ds);
@@ -135,6 +137,13 @@ void registerTypes(Deserializer& ds)
// Airbrakes
registerType<AirBrakesData>(ds);
+ registerType<AirBrakesAlgorithmData>(ds);
+ registerType<AirBrakesChosenTrajectory>(ds);
+
+ // HIL
+ registerType<HILImuData>(ds);
+ registerType<HILGpsData>(ds);
+ registerType<HILBaroData>(ds);
// Others
registerType<EventData>(ds);
diff --git a/src/entrypoints/death-stack-x-entry.cpp b/src/entrypoints/death-stack-x-entry.cpp
index 47b2cd8683f5596a47b8cbaaa5c3c2dca39d0ff7..b21dd0366f8b985ab4484f12e29e9e1e0fc64fa7 100644
--- a/src/entrypoints/death-stack-x-entry.cpp
+++ b/src/entrypoints/death-stack-x-entry.cpp
@@ -44,9 +44,7 @@ int main()
StatsResult cpu_stat_res;
SystemData system_data;
- LOG_INFO(log, "Starting death stack...");
// Instantiate the stack
- Thread::sleep(1000);
DeathStack::getInstance()->start();
LOG_INFO(log, "Death stack started");
@@ -72,15 +70,7 @@ int main()
system_data.free_heap = MemoryProfiling::getCurrentFreeHeap();
logger_service->log(system_data);
-
- // LOG_INFO(log, "CPU : avg: {:.2f} max: {:.2f} min: {:.2f}",
- // cpu_stat.getStats().mean, cpu_stat.getStats().maxValue,
- // cpu_stat.getStats().minValue);
- /*TRACE("CPU : curr: {:.2f} avg: {:.2f} max: {:.2f} min: {:.2f}",
- cpu, cpu_stat.getStats().mean, cpu_stat.getStats().maxValue,
- cpu_stat.getStats().minValue);
- TRACE("Memory : absolute free heap : %u current free heap : %u",
- MemoryProfiling::getAbsoluteFreeHeap(),
- MemoryProfiling::getCurrentFreeHeap());*/
+
+ StackLogger::getInstance()->log();
}
}
\ No newline at end of file
diff --git a/src/hardware_in_the_loop/HIL_sensors/HILAccelerometer.h b/src/hardware_in_the_loop/HIL_sensors/HILAccelerometer.h
index da67703881db827164c1438a61575b4c83afd964..69f2ffabccbc093dcf61360dbde6b888857dd374 100644
--- a/src/hardware_in_the_loop/HIL_sensors/HILAccelerometer.h
+++ b/src/hardware_in_the_loop/HIL_sensors/HILAccelerometer.h
@@ -25,16 +25,6 @@
#include "HILSensor.h"
#include "math/Vec3.h"
-struct HILAccelData : public AccelerometerData
-{
- HILAccelData() : AccelerometerData{0, 0.0, 0.0, 0.0} {}
-
- HILAccelData(uint64_t t, float x, float y, float z)
- : AccelerometerData{t, x, y, z}
- {
- }
-};
-
/**
* @brief fake accelerometer sensor used for the simulation.
*
diff --git a/src/hardware_in_the_loop/HIL_sensors/HILBarometer.h b/src/hardware_in_the_loop/HIL_sensors/HILBarometer.h
index bb149d040f314e975897cfe17ef30f821fc3f4c7..3c23d72114bde57378b8eeffb74f151d55349bd3 100644
--- a/src/hardware_in_the_loop/HIL_sensors/HILBarometer.h
+++ b/src/hardware_in_the_loop/HIL_sensors/HILBarometer.h
@@ -25,13 +25,6 @@
#include "HILSensor.h"
#include "math/Vec3.h"
-struct HILBaroData : public PressureData
-{
- HILBaroData() : PressureData{0, 0.0} {}
-
- HILBaroData(uint64_t t, float p) : PressureData{t, p} {}
-};
-
/**
* @brief fake barometer sensor used for the simulation.
*
diff --git a/src/hardware_in_the_loop/HIL_sensors/HILGps.h b/src/hardware_in_the_loop/HIL_sensors/HILGps.h
index 15550c79500a1f92540724c298a6a7dc7de98b28..65bc495fc9b42086aae73852b87151525a917b2c 100644
--- a/src/hardware_in_the_loop/HIL_sensors/HILGps.h
+++ b/src/hardware_in_the_loop/HIL_sensors/HILGps.h
@@ -25,19 +25,6 @@
#include "HILSensor.h"
#include "math/Vec3.h"
-struct HILGpsData : public GPSData
-{
- HILGpsData() : GPSData{0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 3, true}
- {
- }
-
- HILGpsData(uint64_t t, float x, float y, float z, float v_x, float v_y,
- float v_z, float v)
- : GPSData{t, x, y, z, v_x, v_y, v_z, v, 0.0, 3, true}
- {
- }
-};
-
/**
* @brief fake gps sensor used for the HILulation.
*
diff --git a/src/hardware_in_the_loop/HIL_sensors/HILGyroscope.h b/src/hardware_in_the_loop/HIL_sensors/HILGyroscope.h
index 0929829e88f9d223dacb43a55a333f2c0e677975..2446badd8684cc8d65437800b05b85c72c4f0b74 100644
--- a/src/hardware_in_the_loop/HIL_sensors/HILGyroscope.h
+++ b/src/hardware_in_the_loop/HIL_sensors/HILGyroscope.h
@@ -25,16 +25,6 @@
#include "HILSensor.h"
#include "math/Vec3.h"
-struct HILGyroscopeData : public GyroscopeData
-{
- HILGyroscopeData() : GyroscopeData{0, 0.0, 0.0, 0.0} {}
-
- HILGyroscopeData(uint64_t t, float x, float y, float z)
- : GyroscopeData{t, x, y, z}
- {
- }
-};
-
/**
* @brief fake gyroscope sensor used for the simulation.
*
diff --git a/src/hardware_in_the_loop/HIL_sensors/HILImu.h b/src/hardware_in_the_loop/HIL_sensors/HILImu.h
index bc3a80289d898c921f3e1210567e3596912ee996..8a68c9c7ae39a79444f42bfdb5a277876a463dc0 100644
--- a/src/hardware_in_the_loop/HIL_sensors/HILImu.h
+++ b/src/hardware_in_the_loop/HIL_sensors/HILImu.h
@@ -28,12 +28,6 @@
#include "HILSensor.h"
#include "math/Vec3.h"
-struct HILImuData : public HILAccelData,
- public HILGyroscopeData,
- public HILMagnetometerData
-{
-};
-
/**
* @brief fake 9-axis IMU sensor used for the simulation.
*
@@ -70,7 +64,7 @@ protected:
matlabData = sensorData->magnetometer.measures[sampleCounter];
tempData.mag_x = matlabData.getX();
tempData.mag_y = matlabData.getY();
- tempData.mag_z = matlabData.getZ();
+ tempData.mag_z = matlabData.getZ() / 1000.0f; // from nanotesla to microtesla
// only update the timestamp once and use it for all the 3 sensors
// (this sensor assumes the same frequency for accel, gyro and mag)
diff --git a/src/hardware_in_the_loop/HIL_sensors/HILMagnetometer.h b/src/hardware_in_the_loop/HIL_sensors/HILMagnetometer.h
index 1b59de7ae3381a66860438be3c79608867d7d9f5..91d4beff5836f61c7a352c5639970ac7f8f9d945 100644
--- a/src/hardware_in_the_loop/HIL_sensors/HILMagnetometer.h
+++ b/src/hardware_in_the_loop/HIL_sensors/HILMagnetometer.h
@@ -25,16 +25,6 @@
#include "HILSensor.h"
#include "math/Vec3.h"
-struct HILMagnetometerData : public MagnetometerData
-{
- HILMagnetometerData() : MagnetometerData{0, 0.0, 0.0, 0.0} {}
-
- HILMagnetometerData(uint64_t t, float x, float y, float z)
- : MagnetometerData{t, x, y, z}
- {
- }
-};
-
/**
* @brief fake magnetometer sensor used for the simulation.
*
@@ -61,7 +51,7 @@ protected:
tempData.mag_x = matlabData.getX();
tempData.mag_y = matlabData.getY();
- tempData.mag_z = matlabData.getZ();
+ tempData.mag_z = matlabData.getZ() / 1000.0f; // from nanotesla to microtesla
tempData.mag_timestamp = updateTimestamp();
return tempData;
diff --git a/src/hardware_in_the_loop/HIL_sensors/HILSensor.h b/src/hardware_in_the_loop/HIL_sensors/HILSensor.h
index de8048d10dee63003973582e5057aa60a5be5783..57a1f0768e50e35f3250bbc2418283915a50a126 100644
--- a/src/hardware_in_the_loop/HIL_sensors/HILSensor.h
+++ b/src/hardware_in_the_loop/HIL_sensors/HILSensor.h
@@ -31,6 +31,7 @@
#include "math/Vec3.h"
#include "sensors/Sensor.h"
#include "sensors/SensorData.h"
+#include "HILSensorsData.h"
/**
* @brief Fake sensor base used for the simulation. Every sensor for the
diff --git a/src/hardware_in_the_loop/HIL_sensors/HILSensorsData.h b/src/hardware_in_the_loop/HIL_sensors/HILSensorsData.h
new file mode 100644
index 0000000000000000000000000000000000000000..9acedad4f9115691231fe6356f0e8286031c7953
--- /dev/null
+++ b/src/hardware_in_the_loop/HIL_sensors/HILSensorsData.h
@@ -0,0 +1,114 @@
+
+#include <sensors/SensorData.h>
+
+struct HILAccelData : public AccelerometerData
+{
+ HILAccelData() : AccelerometerData{0, 0.0, 0.0, 0.0} {}
+
+ HILAccelData(uint64_t t, float x, float y, float z)
+ : AccelerometerData{t, x, y, z}
+ {
+ }
+
+ static std::string header() { return "timestamp,x,y,z\n"; }
+
+ void print(std::ostream& os) const
+ {
+ os << accel_timestamp << "," << accel_x << "," << accel_y << ","
+ << accel_z << "\n";
+ }
+};
+
+struct HILGyroscopeData : public GyroscopeData
+{
+ HILGyroscopeData() : GyroscopeData{0, 0.0, 0.0, 0.0} {}
+
+ HILGyroscopeData(uint64_t t, float x, float y, float z)
+ : GyroscopeData{t, x, y, z}
+ {
+ }
+
+ static std::string header() { return "timestamp,x,y,z\n"; }
+
+ void print(std::ostream& os) const
+ {
+ os << gyro_timestamp << "," << gyro_x << "," << gyro_y << "," << gyro_z
+ << "\n";
+ }
+};
+
+struct HILMagnetometerData : public MagnetometerData
+{
+ HILMagnetometerData() : MagnetometerData{0, 0.0, 0.0, 0.0} {}
+
+ HILMagnetometerData(uint64_t t, float x, float y, float z)
+ : MagnetometerData{t, x, y, z}
+ {
+ }
+
+ static std::string header() { return "timestamp,x,y,z\n"; }
+
+ void print(std::ostream& os) const
+ {
+ os << mag_timestamp << "," << mag_x << "," << mag_y << "," << mag_z
+ << "\n";
+ }
+};
+
+struct HILImuData : public HILAccelData,
+ public HILGyroscopeData,
+ public HILMagnetometerData
+{
+ static std::string header()
+ {
+ return "accel_timestamp,accel_x,accel_y,accel_z,gyro_timestamp,gyro_x,"
+ "gyro_y,gyro_z,mag_timestamp,mag_x,mag_y,mag_z\n";
+ }
+
+ void print(std::ostream& os) const
+ {
+ os << accel_timestamp << "," << accel_x << "," << accel_y << "," << accel_z
+ << "," << gyro_timestamp << "," << gyro_x << "," << gyro_y << "," << gyro_z
+ << "," << mag_timestamp << "," << mag_x << "," << mag_y << "," << mag_z << "\n";
+ }
+};
+
+struct HILGpsData : public GPSData
+{
+ HILGpsData() : GPSData{0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 3, true}
+ {
+ }
+
+ HILGpsData(uint64_t t, float x, float y, float z, float v_x, float v_y,
+ float v_z, float v)
+ : GPSData{t, x, y, z, v_x, v_y, v_z, v, 0.0, 3, true}
+ {
+ }
+
+ static std::string header()
+ {
+ return "timestamp,x,y,z,v_x,v_y,v_z,v,track,n_sats,fix\n";
+ }
+
+ void print(std::ostream& os) const
+ {
+ os << gps_timestamp << "," << longitude << "," << latitude << ","
+ << height << "," << velocity_north << "," << velocity_east << ","
+ << velocity_down << "," << speed << "," << track << ","
+ << (int)num_satellites << "," << (int)fix << "\n";
+ }
+};
+
+struct HILBaroData : public PressureData
+{
+ HILBaroData() : PressureData{0, 0.0} {}
+
+ HILBaroData(uint64_t t, float p) : PressureData{t, p} {}
+
+ static std::string header() { return "timestamp,press\n"; }
+
+ void print(std::ostream& os) const
+ {
+ os << press_timestamp << "," << press << "\n";
+ }
+};
diff --git a/src/tests/catch/ada/ada_kalman_p/test-ada-simulation.cpp b/src/tests/catch/ada/ada_kalman_p/test-ada-simulation.cpp
index c0e837bdd7f79f8cf6f2ea89e0d142ad904f2e52..a2f7897ffd075988fb3b13fd064a829b3ba321f5 100644
--- a/src/tests/catch/ada/ada_kalman_p/test-ada-simulation.cpp
+++ b/src/tests/catch/ada/ada_kalman_p/test-ada-simulation.cpp
@@ -26,25 +26,25 @@
#define EIGEN_NO_MALLOC
-#include <Common.h>
-#include <events/EventBroker.h>
-#include <events/Events.h>
-#include <events/FSM.h>
-#include <events/utils/EventCounter.h>
+
+#include <utils/testutils/catch.hpp>
#define private public
#define protected public
-#include <ApogeeDetectionAlgorithm/ADAAlgorithm.h>
#include <ApogeeDetectionAlgorithm/ADAController.h>
+#include <ApogeeDetectionAlgorithm/ADAAlgorithm.h>
#include <DeathStack.h>
-
+#include <Common.h>
+#include <events/EventBroker.h>
+#include <events/Events.h>
+#include <events/FSM.h>
+#include <events/utils/EventCounter.h>
#include <algorithm>
#include <cmath>
#include <iostream>
#include <random>
#include <sstream>
-#include <utils/testutils/catch.hpp>
#include "test-ada-data.h"
diff --git a/src/tests/catch/fsm/test-nas.cpp b/src/tests/catch/fsm/test-nas.cpp
index 64e322d9ee272bef4cd7904caa05c1ee495862d9..ee2667fa03347e185ac4ad7e45db4bce735a452d 100644
--- a/src/tests/catch/fsm/test-nas.cpp
+++ b/src/tests/catch/fsm/test-nas.cpp
@@ -24,18 +24,17 @@
#include "catch/catch-tests-entry.cpp"
#endif
+#include <utils/testutils/catch.hpp>
+
// We need access to the handleEvent(...) function in state machines in order to
// test them synchronously
#define private public
#define protected public
-#include <NavigationAttitudeSystem/NASCalibrator.h>
#include <NavigationAttitudeSystem/NASController.h>
-#include <mocksensors/MockSensors.h>
-
-#include <utils/testutils/catch.hpp>
-
+#include <NavigationAttitudeSystem/NASCalibrator.h>
#include "events/Events.h"
+#include <mocksensors/MockSensors.h>
#include "utils/testutils/TestHelper.h"
using namespace DeathStackBoard;
diff --git a/src/tests/catch/nas/test-nas-simulation.cpp b/src/tests/catch/nas/test-nas-simulation.cpp
index 633983f3bf7694c84a444df392127e025973413f..38b39bcd83f7fdff9283ee33a2787d52a7485fdd 100644
--- a/src/tests/catch/nas/test-nas-simulation.cpp
+++ b/src/tests/catch/nas/test-nas-simulation.cpp
@@ -24,6 +24,8 @@
#include "catch/catch-tests-entry.cpp"
#endif
+#include <utils/testutils/catch.hpp>
+
#define private public
#define EIGEN_NO_MALLOC // enable eigen malloc usage assert
@@ -32,8 +34,6 @@
#include <miosix.h>
#include <mocksensors/MockSensors.h>
-#include <utils/testutils/catch.hpp>
-
#include "events/Events.h"
#include "utils/testutils/TestHelper.h"
@@ -132,7 +132,7 @@ TEST_CASE("Testing Navigation System Controller")
// (same as SIMULATED_PRESSURE[0])
gps_data = mock_gps.getLastSample(); // still before liftoff ...
imu_data = mock_imu.getLastSample(); // still before liftoff ...
- ref_values = {press_data.press, gps_data.latitude, gps_data.longitude,
+ ref_values = NASReferenceValues{press_data.press, gps_data.latitude, gps_data.longitude,
imu_data.accel_x, imu_data.accel_y, imu_data.accel_z,
imu_data.mag_x, imu_data.mag_y, imu_data.mag_z};
REQUIRE(ref_values == controller.calibrator.getReferenceValues());