From 6b9b09349219af568b8e6e477af1e4cb33c0f660 Mon Sep 17 00:00:00 2001
From: Alberto Nidasio <alberto.nidasio@skywarder.eu>
Date: Wed, 5 Oct 2022 00:53:07 +0200
Subject: [PATCH] [HIL] Added logger to sensors and other improvements
---
CMakeLists.txt | 4 +--
skyward-boardcore | 2 +-
.../Configs/AirBrakesControllerConfigInterp.h | 2 +-
.../Configs/AirBrakesControllerConfigPI.h | 2 +-
src/boards/Main/Radio/Radio.cpp | 2 +-
.../AirBrakesController.cpp | 25 +++++--------------
.../HIL_sensors/HILAccelerometer.h | 6 ++++-
.../HIL_sensors/HILBarometer.h | 6 ++++-
src/hardware_in_the_loop/HIL_sensors/HILGps.h | 4 +++
.../HIL_sensors/HILGyroscope.h | 6 ++++-
src/hardware_in_the_loop/HIL_sensors/HILImu.h | 4 +++
.../HIL_sensors/HILMagnetometer.h | 4 +++
.../HIL_sensors/HILPitot.h | 5 +++-
.../HIL_sensors/HILSensorsData.h | 1 +
.../HIL_sensors/HILTemperature.h | 6 ++++-
src/scripts/logdecoder/Main/Makefile | 5 ++--
src/scripts/logdecoder/Main/logdecoder.cpp | 11 ++++++++
17 files changed, 63 insertions(+), 32 deletions(-)
diff --git a/CMakeLists.txt b/CMakeLists.txt
index a013cb2a0..859b26d6f 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -39,7 +39,7 @@ sbs_target(auxiliary-entry stm32f429zi_skyward_pyxis_auxiliary)
add_executable(main-entry-euroc src/entrypoints/Main/main-entry.cpp ${MAIN_COMPUTER})
target_include_directories(main-entry-euroc PRIVATE ${OBSW_INCLUDE_DIRS})
-target_compile_definitions(main-entry-euroc PRIVATE EUROC BUZZER_LOW INTERP)
+target_compile_definitions(main-entry-euroc PRIVATE EUROC BUZZER_LOW INTERP USE_SERIAL_TRANSCEIVER)
sbs_target(main-entry-euroc stm32f429zi_skyward_death_stack_v3)
add_executable(main-entry-roccaraso src/entrypoints/Main/main-entry.cpp ${MAIN_COMPUTER})
@@ -54,7 +54,7 @@ sbs_target(main-entry-milano stm32f429zi_skyward_death_stack_v3)
add_executable(main-entry-hil-euroc src/entrypoints/Main/main-entry.cpp ${MAIN_COMPUTER} ${HIL})
target_include_directories(main-entry-hil-euroc PRIVATE ${OBSW_INCLUDE_DIRS})
-target_compile_definitions(main-entry-hil-euroc PRIVATE HILSimulation EUROC BUZZER_LOW INTERP)
+target_compile_definitions(main-entry-hil-euroc PRIVATE HILSimulation EUROC BUZZER_LOW INTERP USE_SERIAL_TRANSCEIVER)
sbs_target(main-entry-hil-euroc stm32f429zi_skyward_death_stack_v3)
add_executable(main-entry-hil-roccaraso src/entrypoints/Main/main-entry.cpp ${MAIN_COMPUTER} ${HIL})
diff --git a/skyward-boardcore b/skyward-boardcore
index cb195bf7c..e60ca34f7 160000
--- a/skyward-boardcore
+++ b/skyward-boardcore
@@ -1 +1 @@
-Subproject commit cb195bf7c6263d81228e9647892806cdeddd5cb3
+Subproject commit e60ca34f7557877779f3da84d33a76ca0e1d84d5
diff --git a/src/boards/Main/Configs/AirBrakesControllerConfigInterp.h b/src/boards/Main/Configs/AirBrakesControllerConfigInterp.h
index 22e27612f..4bbc8d307 100644
--- a/src/boards/Main/Configs/AirBrakesControllerConfigInterp.h
+++ b/src/boards/Main/Configs/AirBrakesControllerConfigInterp.h
@@ -31,7 +31,7 @@ namespace Main
namespace AirBrakesControllerConfig
{
-static const Boardcore::AirBrakesInterpConfig ABK_CONFIG_INTERP{
+static const Boardcore::AirBrakesInterpConfig ABK_CONFIG_ALG{
.INITIAL_FILTER_COEFF = 0.3,
.INITIAL_T_FILTER = 12,
.DELTA_T_FILTER = 2.5,
diff --git a/src/boards/Main/Configs/AirBrakesControllerConfigPI.h b/src/boards/Main/Configs/AirBrakesControllerConfigPI.h
index e2696756f..f0d67a9f0 100644
--- a/src/boards/Main/Configs/AirBrakesControllerConfigPI.h
+++ b/src/boards/Main/Configs/AirBrakesControllerConfigPI.h
@@ -31,7 +31,7 @@ namespace Main
namespace AirBrakesControllerConfig
{
-static const Boardcore::AirBrakesPIConfig ABK_CONFIG_PI{
+static const Boardcore::AirBrakesPIConfig ABK_CONFIG_ALG{
.KP = 20, .KI = 5, .TS = UPDATE_PERIOD / 1000.0};
} // namespace AirBrakesControllerConfig
diff --git a/src/boards/Main/Radio/Radio.cpp b/src/boards/Main/Radio/Radio.cpp
index a8b4a27d2..7bdc19aad 100644
--- a/src/boards/Main/Radio/Radio.cpp
+++ b/src/boards/Main/Radio/Radio.cpp
@@ -96,7 +96,7 @@ Radio::Radio()
{
#if defined(USE_SERIAL_TRANSCEIVER)
Boardcore::SerialTransceiver* transceiver;
- transceiver = new SerialTransceiver(Buses::getInstance().usart2);
+ transceiver = new SerialTransceiver(Buses::getInstance().usart1);
#elif defined(USE_XBEE_TRANSCEIVER)
SPIBusConfig config;
config.clockDivider = SPI::ClockDivider::DIV_16;
diff --git a/src/boards/Main/StateMachines/AirBrakesController/AirBrakesController.cpp b/src/boards/Main/StateMachines/AirBrakesController/AirBrakesController.cpp
index 8996ae207..f4fad2904 100644
--- a/src/boards/Main/StateMachines/AirBrakesController/AirBrakesController.cpp
+++ b/src/boards/Main/StateMachines/AirBrakesController/AirBrakesController.cpp
@@ -197,7 +197,6 @@ void AirBrakesController::state_end(const Event& event)
AirBrakesController::AirBrakesController()
: FSM(&AirBrakesController::state_init),
abk(
-#ifndef INTERP
#ifndef HILMockNAS
[]() {
return TimedTrajectoryPoint{
@@ -207,28 +206,16 @@ AirBrakesController::AirBrakesController()
[]() { return Sensors::getInstance().state.kalman->getLastSample(); },
#endif // HILMockNAS
TRAJECTORY_SET, AirBrakesControllerConfig::ABK_CONFIG,
- AirBrakesControllerConfig::ABK_CONFIG_PI,
+ AirBrakesControllerConfig::ABK_CONFIG_ALG,
[](float position) {
Actuators::getInstance().setServo(ServosList::AIR_BRAKES_SERVO,
position);
- })
-#else // INTERP
-#ifndef HILMockNAS
- []() {
- return TimedTrajectoryPoint{
- NASController::getInstance().getNasState()};
- },
-#else // HILMockNAS
- []() { return Sensors::getInstance().state.kalman->getLastSample(); },
-#endif // HILMockNAS
- TRAJECTORY_SET, AirBrakesControllerConfig::ABK_CONFIG,
- AirBrakesControllerConfig::ABK_CONFIG_INTERP,
- [](float position) {
- Actuators::getInstance().setServo(ServosList::AIR_BRAKES_SERVO,
- position);
- },
- Main::dz)
+ }
+#ifdef INTERP
+ ,
+ Main::dz
#endif // INTERP
+ )
{
EventBroker::getInstance().subscribe(this, TOPIC_ABK);
EventBroker::getInstance().subscribe(this, TOPIC_FLIGHT);
diff --git a/src/hardware_in_the_loop/HIL_sensors/HILAccelerometer.h b/src/hardware_in_the_loop/HIL_sensors/HILAccelerometer.h
index 1d630feac..806b7e04f 100644
--- a/src/hardware_in_the_loop/HIL_sensors/HILAccelerometer.h
+++ b/src/hardware_in_the_loop/HIL_sensors/HILAccelerometer.h
@@ -22,6 +22,8 @@
#pragma once
+#include <logger/Logger.h>
+
#include "HILSensor.h"
/**
@@ -51,6 +53,8 @@ protected:
tempData.accelerationZ = matlabData.getZ();
tempData.accelerationTimestamp = updateTimestamp();
+ Boardcore::Logger::getInstance().log(tempData);
+
return tempData;
}
-};
\ No newline at end of file
+};
diff --git a/src/hardware_in_the_loop/HIL_sensors/HILBarometer.h b/src/hardware_in_the_loop/HIL_sensors/HILBarometer.h
index 425a92aa0..99f1ccfe6 100644
--- a/src/hardware_in_the_loop/HIL_sensors/HILBarometer.h
+++ b/src/hardware_in_the_loop/HIL_sensors/HILBarometer.h
@@ -22,6 +22,8 @@
#pragma once
+#include <logger/Logger.h>
+
#include "HILSensor.h"
/**
@@ -44,6 +46,8 @@ protected:
tempData.pressure = sensorData->barometer.measures[sampleCounter];
tempData.pressureTimestamp = updateTimestamp();
+ Boardcore::Logger::getInstance().log(tempData);
+
return tempData;
}
-};
\ No newline at end of file
+};
diff --git a/src/hardware_in_the_loop/HIL_sensors/HILGps.h b/src/hardware_in_the_loop/HIL_sensors/HILGps.h
index cc3c77559..8601d8699 100644
--- a/src/hardware_in_the_loop/HIL_sensors/HILGps.h
+++ b/src/hardware_in_the_loop/HIL_sensors/HILGps.h
@@ -22,6 +22,8 @@
#pragma once
+#include <logger/Logger.h>
+
#include <cmath>
#include "HILSensor.h"
@@ -64,6 +66,8 @@ protected:
tempData.gpsTimestamp = updateTimestamp();
+ Boardcore::Logger::getInstance().log(tempData);
+
return tempData;
}
};
diff --git a/src/hardware_in_the_loop/HIL_sensors/HILGyroscope.h b/src/hardware_in_the_loop/HIL_sensors/HILGyroscope.h
index 1a2dc3f5d..840ace99a 100644
--- a/src/hardware_in_the_loop/HIL_sensors/HILGyroscope.h
+++ b/src/hardware_in_the_loop/HIL_sensors/HILGyroscope.h
@@ -22,6 +22,8 @@
#pragma once
+#include <logger/Logger.h>
+
#include "HILSensor.h"
/**
@@ -50,6 +52,8 @@ protected:
tempData.angularVelocityZ = matlabData.getZ();
tempData.angularVelocityTimestamp = updateTimestamp();
+ Boardcore::Logger::getInstance().log(tempData);
+
return tempData;
}
-};
\ No newline at end of file
+};
diff --git a/src/hardware_in_the_loop/HIL_sensors/HILImu.h b/src/hardware_in_the_loop/HIL_sensors/HILImu.h
index 6c2ee30ac..22daa1a93 100644
--- a/src/hardware_in_the_loop/HIL_sensors/HILImu.h
+++ b/src/hardware_in_the_loop/HIL_sensors/HILImu.h
@@ -22,6 +22,8 @@
#pragma once
+#include <logger/Logger.h>
+
#include "HILAccelerometer.h"
#include "HILGyroscope.h"
#include "HILMagnetometer.h"
@@ -69,6 +71,8 @@ protected:
tempData.angularVelocityTimestamp = tempData.accelerationTimestamp;
tempData.magneticFieldTimestamp = tempData.accelerationTimestamp;
+ Boardcore::Logger::getInstance().log(tempData);
+
return tempData;
}
};
diff --git a/src/hardware_in_the_loop/HIL_sensors/HILMagnetometer.h b/src/hardware_in_the_loop/HIL_sensors/HILMagnetometer.h
index 9df731985..269b449a2 100644
--- a/src/hardware_in_the_loop/HIL_sensors/HILMagnetometer.h
+++ b/src/hardware_in_the_loop/HIL_sensors/HILMagnetometer.h
@@ -22,6 +22,8 @@
#pragma once
+#include <logger/Logger.h>
+
#include "HILSensor.h"
/**
@@ -52,6 +54,8 @@ protected:
matlabData.getZ() / 1000.0f; // from nanotesla to microtesla
tempData.magneticFieldTimestamp = updateTimestamp();
+ Boardcore::Logger::getInstance().log(tempData);
+
return tempData;
}
};
diff --git a/src/hardware_in_the_loop/HIL_sensors/HILPitot.h b/src/hardware_in_the_loop/HIL_sensors/HILPitot.h
index cf87bec71..9fabdc85b 100644
--- a/src/hardware_in_the_loop/HIL_sensors/HILPitot.h
+++ b/src/hardware_in_the_loop/HIL_sensors/HILPitot.h
@@ -22,6 +22,7 @@
#pragma once
+#include <logger/Logger.h>
#include <utils/AeroUtils/AeroUtils.h>
#include "HILSensor.h"
@@ -50,6 +51,8 @@ protected:
Boardcore::Aeroutils::relDensity(sensorData->barometer.measures[0]);
tempData.airspeed = sqrtf(2 * fabs(tempData.deltaP) / airDensity);
+ Boardcore::Logger::getInstance().log(tempData);
+
return tempData;
}
-};
\ No newline at end of file
+};
diff --git a/src/hardware_in_the_loop/HIL_sensors/HILSensorsData.h b/src/hardware_in_the_loop/HIL_sensors/HILSensorsData.h
index 62f276016..34b9ea3e5 100644
--- a/src/hardware_in_the_loop/HIL_sensors/HILSensorsData.h
+++ b/src/hardware_in_the_loop/HIL_sensors/HILSensorsData.h
@@ -21,6 +21,7 @@
*/
#include <sensors/SensorData.h>
+#include <sensors/analog/Pitot/PitotData.h>
struct HILAccelData : public Boardcore::AccelerometerData
{
diff --git a/src/hardware_in_the_loop/HIL_sensors/HILTemperature.h b/src/hardware_in_the_loop/HIL_sensors/HILTemperature.h
index 5940f3a54..3d9ee61a3 100644
--- a/src/hardware_in_the_loop/HIL_sensors/HILTemperature.h
+++ b/src/hardware_in_the_loop/HIL_sensors/HILTemperature.h
@@ -22,6 +22,8 @@
#pragma once
+#include <logger/Logger.h>
+
#include "HILSensor.h"
/**
@@ -44,6 +46,8 @@ protected:
tempData.temperature = sensorData->temperature.measure;
tempData.temperatureTimestamp = updateTimestamp();
+ Boardcore::Logger::getInstance().log(tempData);
+
return tempData;
}
-};
\ No newline at end of file
+};
diff --git a/src/scripts/logdecoder/Main/Makefile b/src/scripts/logdecoder/Main/Makefile
index 6bbaf34fc..991e56ed0 100644
--- a/src/scripts/logdecoder/Main/Makefile
+++ b/src/scripts/logdecoder/Main/Makefile
@@ -1,5 +1,5 @@
BOARDCORE := ../../../../skyward-boardcore/
-OBSW := ../../../../src/boards/
+OBSW := ../../../../src/
all:
g++ -std=c++17 -O2 -o logdecoder logdecoder.cpp \
@@ -11,6 +11,7 @@ all:
-I$(BOARDCORE)libs/eigen \
-I$(BOARDCORE)libs/tscpp \
-I$(BOARDCORE)src/shared \
- -I$(OBSW)
+ -I$(OBSW)hardware_in_the_loop \
+ -I$(OBSW)boards
clean:
rm logdecoder
diff --git a/src/scripts/logdecoder/Main/logdecoder.cpp b/src/scripts/logdecoder/Main/logdecoder.cpp
index 6e8176338..05a1d64ce 100644
--- a/src/scripts/logdecoder/Main/logdecoder.cpp
+++ b/src/scripts/logdecoder/Main/logdecoder.cpp
@@ -20,6 +20,7 @@
* THE SOFTWARE.
*/
+#include <HIL_sensors/HILSensorsData.h>
#include <Main/StateMachines/ADAController/ADAControllerData.h>
#include <Main/StateMachines/AirBrakesController/AirBrakesControllerData.h>
#include <Main/StateMachines/Deployment/DeploymentData.h>
@@ -127,6 +128,16 @@ void registerTypes(Deserializer& ds)
ds.registerType<DeploymentStatus>();
ds.registerType<FlightModeManagerStatus>();
ds.registerType<NASControllerStatus>();
+
+ // HIL
+ ds.registerType<HILAccelData>();
+ ds.registerType<HILGyroscopeData>();
+ ds.registerType<HILMagnetometerData>();
+ ds.registerType<HILImuData>();
+ ds.registerType<HILGpsData>();
+ ds.registerType<HILBaroData>();
+ ds.registerType<HILPitotData>();
+ ds.registerType<HILTempData>();
}
void showUsage(const string& cmdName)
--
GitLab