From 53c7da24644e448176aff852aa0105e24c90ec46 Mon Sep 17 00:00:00 2001
From: Alberto Nidasio <alberto.nidasio@skywarder.eu>
Date: Fri, 7 Oct 2022 16:25:27 +0200
Subject: [PATCH] [HIL] Updated sensors and magnetometer error
---
src/boards/Main/Sensors/Sensors.cpp | 41 ++++++-------------
src/hardware_in_the_loop/HIL_sensors/HILImu.h | 7 +---
.../HIL_sensors/HILMagnetometer.h | 9 ++--
3 files changed, 16 insertions(+), 41 deletions(-)
diff --git a/src/boards/Main/Sensors/Sensors.cpp b/src/boards/Main/Sensors/Sensors.cpp
index 773637208..a2fa26189 100644
--- a/src/boards/Main/Sensors/Sensors.cpp
+++ b/src/boards/Main/Sensors/Sensors.cpp
@@ -295,25 +295,16 @@ Sensors::Sensors()
#ifdef HILSimulation
// Definition of the fake sensors for the simulation
- state.accelerometer = new HILAccelerometer(N_DATA_ACCEL);
- state.barometer = new HILBarometer(N_DATA_BARO);
- state.pitot = new HILPitot(N_DATA_PITOT);
- state.gps = new HILGps(N_DATA_GPS);
- state.gyro = new HILGyroscope(N_DATA_GYRO);
- state.magnetometer = new HILMagnetometer(N_DATA_MAGN);
- state.imu = new HILImu(N_DATA_IMU);
- state.temperature = new HILTemperature(N_DATA_TEMP);
- state.kalman = new HILKalman(N_DATA_KALM);
-
- sensorsMap = {{state.accelerometer, accelConfig},
- {state.barometer, baroConfig},
- {state.pitot, pitotConfig},
- {state.magnetometer, magnConfig},
- {state.imu, imuConfig},
- {state.gps, gpsConfig},
- {state.gyro, gyroConfig},
- {state.temperature, tempConfig},
- {state.kalman, kalmConfig}};
+ state.barometer = new HILBarometer(N_DATA_BARO);
+ state.pitot = new HILPitot(N_DATA_PITOT);
+ state.gps = new HILGps(N_DATA_GPS);
+ state.imu = new HILImu(N_DATA_IMU);
+ state.temperature = new HILTemperature(N_DATA_TEMP);
+ state.kalman = new HILKalman(N_DATA_KALM);
+
+ sensorsMap = {{state.barometer, baroConfig}, {state.pitot, pitotConfig},
+ {state.imu, imuConfig}, {state.gps, gpsConfig},
+ {state.temperature, tempConfig}, {state.kalman, kalmConfig}};
#endif
// Create the sensor manager
@@ -334,12 +325,9 @@ Sensors::~Sensors()
delete batteryVoltage;
#ifdef HILSimulation
- delete state.accelerometer;
delete state.barometer;
delete state.pitot;
delete state.gps;
- delete state.gyro;
- delete state.magnetometer;
delete state.imu;
delete state.temperature;
delete state.kalman;
@@ -396,13 +384,8 @@ void Sensors::bmx160Callback()
void Sensors::bmx160WithCorrectionInit()
{
- // Read the correction parameters
- BMX160CorrectionParameters correctionParameters =
- BMX160WithCorrection::readCorrectionParametersFromFile(
- BMX160_CORRECTION_PARAMETERS_FILE);
-
- bmx160WithCorrection = new BMX160WithCorrection(
- bmx160, correctionParameters, BMX160_AXIS_ROTATION);
+ bmx160WithCorrection =
+ new BMX160WithCorrection(bmx160, BMX160_AXIS_ROTATION);
SensorInfo info(
"BMX160WithCorrection", SAMPLE_PERIOD_IMU_BMX,
diff --git a/src/hardware_in_the_loop/HIL_sensors/HILImu.h b/src/hardware_in_the_loop/HIL_sensors/HILImu.h
index 22daa1a93..81cc2550e 100644
--- a/src/hardware_in_the_loop/HIL_sensors/HILImu.h
+++ b/src/hardware_in_the_loop/HIL_sensors/HILImu.h
@@ -47,8 +47,6 @@ protected:
HILImuData tempData;
Boardcore::Vec3 matlabData;
- /* I make a copy of the vector i have to memorize in the sensor
- * struct */
matlabData = sensorData->accelerometer.measures[sampleCounter];
tempData.accelerationX = matlabData.getX();
tempData.accelerationY = matlabData.getY();
@@ -62,11 +60,8 @@ protected:
matlabData = sensorData->magnetometer.measures[sampleCounter];
tempData.magneticFieldX = matlabData.getX();
tempData.magneticFieldY = matlabData.getY();
- tempData.magneticFieldZ =
- matlabData.getZ() / 1000.0f; // from nanotesla to microtesla
+ tempData.magneticFieldZ = matlabData.getZ();
- // only update the timestamp once and use it for all the 3 sensors
- // (this sensor assumes the same frequency for accel, gyro and mag)
tempData.accelerationTimestamp = updateTimestamp();
tempData.angularVelocityTimestamp = tempData.accelerationTimestamp;
tempData.magneticFieldTimestamp = tempData.accelerationTimestamp;
diff --git a/src/hardware_in_the_loop/HIL_sensors/HILMagnetometer.h b/src/hardware_in_the_loop/HIL_sensors/HILMagnetometer.h
index 269b449a2..4c4c441e1 100644
--- a/src/hardware_in_the_loop/HIL_sensors/HILMagnetometer.h
+++ b/src/hardware_in_the_loop/HIL_sensors/HILMagnetometer.h
@@ -43,15 +43,12 @@ protected:
{
HILMagnetometerData tempData;
- /* I make a copy of the vector i have to memorize in the sensor
- * struct */
Boardcore::Vec3 matlabData =
sensorData->magnetometer.measures[sampleCounter];
- tempData.magneticFieldX = matlabData.getX();
- tempData.magneticFieldY = matlabData.getY();
- tempData.magneticFieldZ =
- matlabData.getZ() / 1000.0f; // from nanotesla to microtesla
+ tempData.magneticFieldX = matlabData.getX();
+ tempData.magneticFieldY = matlabData.getY();
+ tempData.magneticFieldZ = matlabData.getZ();
tempData.magneticFieldTimestamp = updateTimestamp();
Boardcore::Logger::getInstance().log(tempData);
--
GitLab