From dc69ee5754996148bbb2cef19fee40753c58037d Mon Sep 17 00:00:00 2001
From: Pietro Bortolus <Pietro.bortolus@skywarder.eu>
Date: Fri, 21 Mar 2025 11:20:08 +0100
Subject: [PATCH] [Main][Sensors] Update Main OBSW to Orion's sensor v2 board
---
.vscode/c_cpp_properties.json | 22 +
CMakeLists.txt | 6 +-
scripts/logdecoder/General/logdecoder.cpp | 8 +-
scripts/logdecoder/Main/logdecoder.cpp | 8 +-
src/Main/Buses.h | 7 +-
src/Main/Configs/HILSimulationConfig.h | 8 +-
src/Main/Configs/SensorsConfig.h | 74 ++-
src/Main/Radio/Radio.cpp | 188 +++-----
src/Main/Sensors/HILSensors.h | 55 +--
src/Main/Sensors/Sensors.cpp | 561 +++++++++++-----------
src/Main/Sensors/Sensors.h | 84 ++--
src/Main/Sensors/SensorsData.h | 91 ++--
src/Main/main-entry.cpp | 19 +-
13 files changed, 549 insertions(+), 582 deletions(-)
diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json
index 38b527054..f27148483 100755
--- a/.vscode/c_cpp_properties.json
+++ b/.vscode/c_cpp_properties.json
@@ -465,6 +465,28 @@
"${workspaceFolder}/skyward-boardcore/src/bsps/stm32f767zi_lyra_motor"
]
},
+ {
+ "name": "stm32f767zi_orion_biscotto",
+ "cStandard": "c11",
+ "cppStandard": "c++14",
+ "compilerPath": "/opt/arm-miosix-eabi/bin/arm-miosix-eabi-g++",
+ "defines": [
+ "${defaultDefines}",
+ "_MIOSIX_BOARDNAME=stm32f767zi_orion_biscotto",
+ "_BOARD_STM32F767ZI_ORION_BISCOTTO",
+ "_ARCH_CORTEXM7_STM32F7",
+ "HSE_VALUE=25000000",
+ "SYSCLK_FREQ_216MHz=216000000",
+ "__ENABLE_XRAM",
+ "V_DDA_VOLTAGE=3.3f"
+ ],
+ "includePath": [
+ "${defaultIncludePaths}",
+ "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM7_stm32f7/common",
+ "${workspaceFolder}/skyward-boardcore/src/bsps/stm32f767zi_orion_biscotto/config",
+ "${workspaceFolder}/skyward-boardcore/src/bsps/stm32f767zi_orion_biscotto"
+ ]
+ },
{
"name": "logdecoder",
"includePath": [
diff --git a/CMakeLists.txt b/CMakeLists.txt
index a1510b8c6..1902a11e2 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -38,17 +38,17 @@ include(cmake/dependencies.cmake)
add_executable(main-entry src/Main/main-entry.cpp ${MAIN_COMPUTER})
target_include_directories(main-entry PRIVATE ${OBSW_INCLUDE_DIRS})
target_compile_definitions(main-entry PRIVATE DEFAULT_STDOUT_LOG_LEVEL=20)
-sbs_target(main-entry stm32f767zi_lyra_biscotto)
+sbs_target(main-entry stm32f767zi_orion_biscotto)
add_executable(main-entry-roccaraso src/Main/main-entry.cpp ${MAIN_COMPUTER})
target_include_directories(main-entry-roccaraso PRIVATE ${OBSW_INCLUDE_DIRS})
target_compile_definitions(main-entry-roccaraso PRIVATE DEFAULT_STDOUT_LOG_LEVEL=20 ROCCARASO)
-sbs_target(main-entry-roccaraso stm32f767zi_lyra_biscotto)
+sbs_target(main-entry-roccaraso stm32f767zi_orion_biscotto)
add_executable(main-entry-euroc src/Main/main-entry.cpp ${MAIN_COMPUTER})
target_include_directories(main-entry-euroc PRIVATE ${OBSW_INCLUDE_DIRS})
target_compile_definitions(main-entry-euroc PRIVATE DEFAULT_STDOUT_LOG_LEVEL=20 EUROC)
-sbs_target(main-entry-euroc stm32f767zi_lyra_biscotto)
+sbs_target(main-entry-euroc stm32f767zi_orion_biscotto)
add_executable(payload-entry src/Payload/payload-entry.cpp ${PAYLOAD_COMPUTER})
target_include_directories(payload-entry PRIVATE ${OBSW_INCLUDE_DIRS})
diff --git a/scripts/logdecoder/General/logdecoder.cpp b/scripts/logdecoder/General/logdecoder.cpp
index d6226ce7d..a237a6efc 100644
--- a/scripts/logdecoder/General/logdecoder.cpp
+++ b/scripts/logdecoder/General/logdecoder.cpp
@@ -80,9 +80,13 @@ void registerTypes(Deserializer& ds)
ds.registerType<Main::ADAControllerStatus>();
ds.registerType<Main::ABKControllerStatus>();
ds.registerType<Main::PinChangeData>();
- ds.registerType<Main::StaticPressureData1>();
- ds.registerType<Main::StaticPressureData2>();
+ ds.registerType<Main::StaticPressure0Data>();
+ ds.registerType<Main::StaticPressure1Data>();
+ ds.registerType<Main::StaticPressure2Data>();
ds.registerType<Main::DplBayPressureData>();
+ ds.registerType<Main::LSM6DSRX0Data>();
+ ds.registerType<Main::LSM6DSRX1Data>();
+ ds.registerType<Main::LIS2MDLExternalData>();
ds.registerType<Main::CalibrationData>();
// Motor
diff --git a/scripts/logdecoder/Main/logdecoder.cpp b/scripts/logdecoder/Main/logdecoder.cpp
index f87f57760..a59403f70 100644
--- a/scripts/logdecoder/Main/logdecoder.cpp
+++ b/scripts/logdecoder/Main/logdecoder.cpp
@@ -64,9 +64,13 @@ void registerTypes(Deserializer& ds)
ds.registerType<ADAControllerStatus>();
ds.registerType<ABKControllerStatus>();
ds.registerType<PinChangeData>();
- ds.registerType<StaticPressureData1>();
- ds.registerType<StaticPressureData2>();
+ ds.registerType<StaticPressure0Data>();
+ ds.registerType<StaticPressure1Data>();
+ ds.registerType<StaticPressure2Data>();
ds.registerType<DplBayPressureData>();
+ ds.registerType<LSM6DSRX0Data>();
+ ds.registerType<LSM6DSRX1Data>();
+ ds.registerType<LIS2MDLExternalData>();
ds.registerType<CalibrationData>();
}
diff --git a/src/Main/Buses.h b/src/Main/Buses.h
index 76d1843b8..702cd0747 100644
--- a/src/Main/Buses.h
+++ b/src/Main/Buses.h
@@ -22,7 +22,6 @@
#pragma once
-#include <drivers/i2c/I2C.h>
#include <drivers/spi/SPIBus.h>
#include <drivers/usart/USART.h>
#include <interfaces-impl/hwmapping.h>
@@ -43,12 +42,11 @@ public:
Boardcore::SPIBus& getVN100() { return spi1; }
Boardcore::SPIBus& getUBXGps() { return spi3; }
Boardcore::SPIBus& getADS131M08() { return spi4; }
+ Boardcore::SPIBus& getND015A() { return spi4; }
Boardcore::SPIBus& getRadio() { return spi6; }
Boardcore::USART& getHILUart() { return usart4; }
- Boardcore::I2C& getLPS28DFW() { return i2c1; }
-
private:
Boardcore::SPIBus spi1{SPI1};
Boardcore::SPIBus spi3{SPI3};
@@ -56,9 +54,6 @@ private:
Boardcore::SPIBus spi6{SPI6};
Boardcore::USART usart4{UART4, 256000, 1024};
-
- Boardcore::I2C i2c1{I2C1, miosix::interfaces::i2c1::scl::getPin(),
- miosix::interfaces::i2c1::sda::getPin()};
};
} // namespace Main
diff --git a/src/Main/Configs/HILSimulationConfig.h b/src/Main/Configs/HILSimulationConfig.h
index 0077eb162..a4ae9ddbe 100644
--- a/src/Main/Configs/HILSimulationConfig.h
+++ b/src/Main/Configs/HILSimulationConfig.h
@@ -67,12 +67,10 @@ static_assert(N_DATA_MAGNETO * SIMULATION_RATE >= Sensors::LIS2MDL::RATE,
"N_DATA_MAGNETO not enough");
static_assert(N_DATA_GPS * SIMULATION_RATE >= Sensors::UBXGPS::RATE,
"N_DATA_GPS not enough");
-static_assert(N_DATA_BARO_STATIC * SIMULATION_RATE >= Sensors::ADS131M08::RATE,
- "N_DATA_BARO_STATIC not enough");
static_assert(N_DATA_BARO_STATIC * SIMULATION_RATE >= Sensors::LPS22DF::RATE,
- "N_DATA_BARO_STATIC not enough");
-static_assert(N_DATA_BARO_STATIC * SIMULATION_RATE >= Sensors::LPS28DFW::RATE,
- "N_DATA_BARO_STATIC not enough");
+ "N_DATA_BARO_STATIC not enough (LPS22DF)");
+static_assert(N_DATA_BARO_STATIC * SIMULATION_RATE >= Sensors::ND015A::RATE,
+ "N_DATA_BARO_STATIC not enough (ND015A)");
static_assert(N_DATA_BARO_CHAMBER * SIMULATION_RATE >= BARO_CHAMBER_RATE,
"N_DATA_BARO_CHAMBER not enough");
static_assert(N_DATA_PITOT * SIMULATION_RATE >= BARO_PITOT_RATE,
diff --git a/src/Main/Configs/SensorsConfig.h b/src/Main/Configs/SensorsConfig.h
index aa5d84f39..6a8bdfd2c 100644
--- a/src/Main/Configs/SensorsConfig.h
+++ b/src/Main/Configs/SensorsConfig.h
@@ -1,5 +1,5 @@
/* Copyright (c) 2024 Skyward Experimental Rocketry
- * Author: Davide Mor
+ * Authors: Davide Mor, Pietro Bortolus
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
@@ -23,12 +23,11 @@
#pragma once
#include <drivers/adc/InternalADC.h>
-#include <sensors/ADS131M08/ADS131M08.h>
#include <sensors/H3LIS331DL/H3LIS331DL.h>
#include <sensors/LIS2MDL/LIS2MDL.h>
#include <sensors/LPS22DF/LPS22DF.h>
-#include <sensors/LPS28DFW/LPS28DFW.h>
#include <sensors/LSM6DSRX/LSM6DSRX.h>
+#include <sensors/ND015X/ND015A.h>
#include <units/Frequency.h>
#include <string>
@@ -41,16 +40,18 @@ namespace Config
namespace Sensors
{
-
/* linter off */ using namespace Boardcore::Units::Frequency;
+// Switches between LPS22DF + LIS2MDL IN/EXT configuration and the dual
+// magnetometer configuration, LIS2MDL IN + LIS2MDL EXT.
+// The dual mag configuration is used during testing to compare magnetometer
+// positioning.
+// NOTE: Ensure the configuration pins on the board are soldered accordingly.
+constexpr bool USING_DUAL_MAGNETOMETER = false;
+
constexpr int CALIBRATION_SAMPLES_COUNT = 20;
constexpr unsigned int CALIBRATION_SLEEP_TIME = 100; // [ms]
-// Minimum lps28dfw in order for the measure to be considered valid and
-// compensate other sensors
-constexpr float ATMOS_CALIBRATION_THRESH = 50000.0; // [Pa]
-
constexpr Hertz MAG_CALIBRATION_RATE = 50_hz;
static const std::string MAG_CALIBRATION_FILENAME{"/sd/magCalibration.csv"};
@@ -63,16 +64,6 @@ constexpr Hertz RATE = 50_hz;
constexpr bool ENABLED = true;
} // namespace LPS22DF
-namespace LPS28DFW
-{
-constexpr Boardcore::LPS28DFW::FullScaleRange FS = Boardcore::LPS28DFW::FS_1260;
-constexpr Boardcore::LPS28DFW::AVG AVG = Boardcore::LPS28DFW::AVG_4;
-constexpr Boardcore::LPS28DFW::ODR ODR = Boardcore::LPS28DFW::ODR_100;
-
-constexpr Hertz RATE = 50_hz;
-constexpr bool ENABLED = true;
-} // namespace LPS28DFW
-
namespace H3LIS331DL
{
constexpr Boardcore::H3LIS331DLDefs::OutputDataRate ODR =
@@ -125,33 +116,20 @@ constexpr Hertz RATE = 100_hz;
constexpr bool ENABLED = true;
} // namespace VN100
-namespace ADS131M08
+namespace ND015A
{
-constexpr Boardcore::ADS131M08Defs::OversamplingRatio OSR =
- Boardcore::ADS131M08Defs::OversamplingRatio::OSR_8192;
-constexpr bool GLOBAL_CHOP_MODE_EN = true;
-
-// ADC channels definitions for various sensors
-constexpr Boardcore::ADS131M08Defs::Channel STATIC_PRESSURE_1_CHANNEL =
- Boardcore::ADS131M08Defs::Channel::CHANNEL_0;
-constexpr Boardcore::ADS131M08Defs::Channel STATIC_PRESSURE_2_CHANNEL =
- Boardcore::ADS131M08Defs::Channel::CHANNEL_1;
-constexpr Boardcore::ADS131M08Defs::Channel DPL_BAY_PRESSURE_CHANNEL =
- Boardcore::ADS131M08Defs::Channel::CHANNEL_2;
-
-// These values have been calibrated via vacuum chamber, by looking at LPS28DFW
-// output and analog sensor output.
-constexpr float CHANNEL_0_SCALE = 4.21662235677003f;
-constexpr float CHANNEL_1_SCALE = 4.21662235677003f;
-constexpr float CHANNEL_2_SCALE = 4.21662235677003f;
-
-constexpr float STATIC_PRESSURE_1_SCALE = CHANNEL_0_SCALE;
-constexpr float STATIC_PRESSURE_2_SCALE = CHANNEL_1_SCALE;
-constexpr float DPL_BAY_PRESSURE_SCALE = CHANNEL_2_SCALE;
+constexpr Boardcore::ND015A::IOWatchdogEnable IOW =
+ Boardcore::ND015A::IOWatchdogEnable::DISABLED;
+constexpr Boardcore::ND015A::BWLimitFilter BWL =
+ Boardcore::ND015A::BWLimitFilter::BWL_200;
+constexpr Boardcore::ND015A::NotchEnable NTC =
+ Boardcore::ND015A::NotchEnable::DISABLED;
+
+constexpr uint8_t ODR = 0x1C;
constexpr Hertz RATE = 100_hz;
constexpr bool ENABLED = true;
-} // namespace ADS131M08
+} // namespace ND015A
namespace InternalADC
{
@@ -178,15 +156,17 @@ constexpr bool ENABLED = true;
namespace Atmos
{
+enum class AtmosSensor
+{
+ SENSOR_0 = 0,
+ SENSOR_1 = 1,
+ SENSOR_2 = 2
+};
-// Setting this to true changes the atmospheric pressure to be the static
-// pressure port number 2
-constexpr bool USE_PORT_2 = false;
+// The sensor used for the atmospheric pressure, one of the 3 ND015A sensors
+constexpr AtmosSensor ATMOS_SENSOR = AtmosSensor::SENSOR_0;
} // namespace Atmos
-
} // namespace Sensors
-
} // namespace Config
-
} // namespace Main
diff --git a/src/Main/Radio/Radio.cpp b/src/Main/Radio/Radio.cpp
index fe7cbd2d8..6adb0c5a2 100644
--- a/src/Main/Radio/Radio.cpp
+++ b/src/Main/Radio/Radio.cpp
@@ -312,41 +312,6 @@ void Radio::handleMessage(const mavlink_message_t& msg)
break;
}
- case MAVLINK_MSG_ID_SET_CALIBRATION_PRESSURE_TC:
- {
- if (getModule<FlightModeManager>()->getState() ==
- FlightModeManagerState::DISARMED)
- {
- float press =
- mavlink_msg_set_calibration_pressure_tc_get_pressure(&msg);
-
- if (press == 0)
- {
- getModule<Sensors>()->resetBaroCalibrationReference();
- EventBroker::getInstance().post(
- TMTC_SET_CALIBRATION_PRESSURE, TOPIC_TMTC);
- enqueueAck(msg);
- }
- else
- {
- getModule<Sensors>()->setBaroCalibrationReference(press);
- EventBroker::getInstance().post(
- TMTC_SET_CALIBRATION_PRESSURE, TOPIC_TMTC);
-
- if (press < 50000)
- enqueueWack(msg, 0);
- else
- enqueueAck(msg);
- }
- }
- else
- {
- enqueueNack(msg, 0);
- }
-
- break;
- }
-
default:
{
enqueueNack(msg, 0);
@@ -591,12 +556,12 @@ bool Radio::enqueueSystemTm(uint8_t tmId)
tm.mag_scale_x = data.magScaleX;
tm.mag_scale_y = data.magScaleY;
tm.mag_scale_z = data.magScaleZ;
- tm.static_press_1_bias = data.staticPress1Bias;
- tm.static_press_1_scale = data.staticPress1Scale;
- tm.static_press_2_bias = data.staticPress2Bias;
- tm.static_press_2_scale = data.staticPress2Scale;
- tm.dpl_bay_press_bias = data.dplBayPressBias;
- tm.dpl_bay_press_scale = data.dplBayPressScale;
+ tm.static_press_1_bias = -1.0f; // TODO: remove in mavlink
+ tm.static_press_1_scale = -1.0f; // TODO: remove in mavlink
+ tm.static_press_2_bias = -1.0f; // TODO: remove in mavlink
+ tm.static_press_2_scale = -1.0f; // TODO: remove in mavlink
+ tm.dpl_bay_press_bias = -1.0f; // TODO: remove in mavlink
+ tm.dpl_bay_press_scale = -1.0f; // TODO: remove in mavlink
mavlink_msg_calibration_tm_encode(Config::Radio::MAV_SYSTEM_ID,
Config::Radio::MAV_COMPONENT_ID,
@@ -689,11 +654,11 @@ bool Radio::enqueueSystemTm(uint8_t tmId)
MEAController* mea = getModule<MEAController>();
FlightModeManager* fmm = getModule<FlightModeManager>();
- auto pressDigi = sensors->getLPS28DFWLastSample();
auto imu = sensors->getIMULastSample();
auto gps = sensors->getUBXGPSLastSample();
auto vn100 = sensors->getVN100LastSample();
- auto pressStatic = sensors->getStaticPressure1LastSample();
+ auto temperature = sensors->getTemperatureLastSample();
+ auto pressStatic = sensors->getAtmosPressureLastSample();
auto pressDpl = sensors->getDplBayPressureLastSample();
auto pitotStatic = sensors->getCanPitotStaticPressLastSample();
auto pitotDynamic = sensors->getCanPitotDynamicPressLastSample();
@@ -717,7 +682,7 @@ bool Radio::enqueueSystemTm(uint8_t tmId)
tm.mea_apogee = meaState.estimatedApogee;
// Sensors
- tm.pressure_digi = pressDigi.pressure;
+ tm.pressure_digi = -1.0f; // TODO: rmeove in mavlink
tm.pressure_static = pressStatic.pressure;
tm.pressure_dpl = pressDpl.pressure;
@@ -771,7 +736,7 @@ bool Radio::enqueueSystemTm(uint8_t tmId)
tm.battery_voltage = sensors->getBatteryVoltageLastSample().voltage;
tm.cam_battery_voltage =
sensors->getCamBatteryVoltageLastSample().voltage;
- tm.temperature = pressDigi.temperature;
+ tm.temperature = temperature.temperature;
mavlink_msg_rocket_flight_tm_encode(Config::Radio::MAV_SYSTEM_ID,
Config::Radio::MAV_COMPONENT_ID,
@@ -968,39 +933,6 @@ bool Radio::enqueueSensorsTm(uint8_t tmId)
return true;
}
- case MAV_ADS131M08_ID:
- {
- mavlink_message_t msg;
-
- auto sample = getModule<Sensors>()->getADS131M08LastSample();
-
- mavlink_adc_tm_t tm;
- tm.channel_0 =
- sample.getVoltage(ADS131M08Defs::Channel::CHANNEL_0).voltage;
- tm.channel_1 =
- sample.getVoltage(ADS131M08Defs::Channel::CHANNEL_1).voltage;
- tm.channel_2 =
- sample.getVoltage(ADS131M08Defs::Channel::CHANNEL_2).voltage;
- tm.channel_3 =
- sample.getVoltage(ADS131M08Defs::Channel::CHANNEL_3).voltage;
- tm.channel_4 =
- sample.getVoltage(ADS131M08Defs::Channel::CHANNEL_4).voltage;
- tm.channel_5 =
- sample.getVoltage(ADS131M08Defs::Channel::CHANNEL_5).voltage;
- tm.channel_6 =
- sample.getVoltage(ADS131M08Defs::Channel::CHANNEL_6).voltage;
- tm.channel_7 =
- sample.getVoltage(ADS131M08Defs::Channel::CHANNEL_7).voltage;
- tm.timestamp = sample.timestamp;
- strcpy(tm.sensor_name, "ADS131M08");
-
- mavlink_msg_adc_tm_encode(Config::Radio::MAV_SYSTEM_ID,
- Config::Radio::MAV_COMPONENT_ID, &msg,
- &tm);
- enqueuePacket(msg);
- return true;
- }
-
case MAV_BATTERY_VOLTAGE_ID:
{
mavlink_message_t msg;
@@ -1019,35 +951,6 @@ bool Radio::enqueueSensorsTm(uint8_t tmId)
return true;
}
- case MAV_LPS28DFW_ID:
- {
- mavlink_message_t msg;
-
- auto sample = getModule<Sensors>()->getLPS28DFWLastSample();
-
- mavlink_pressure_tm_t tm1;
- tm1.pressure = sample.pressure;
- tm1.timestamp = sample.pressureTimestamp;
- strcpy(tm1.sensor_name, "LPS28DFW");
-
- mavlink_msg_pressure_tm_encode(Config::Radio::MAV_SYSTEM_ID,
- Config::Radio::MAV_COMPONENT_ID,
- &msg, &tm1);
- enqueuePacket(msg);
-
- mavlink_temp_tm_t tm2;
- tm2.temperature = sample.temperature;
- tm2.timestamp = sample.temperatureTimestamp;
- strcpy(tm2.sensor_name, "LPS28DFW");
-
- mavlink_msg_temp_tm_encode(Config::Radio::MAV_SYSTEM_ID,
- Config::Radio::MAV_COMPONENT_ID, &msg,
- &tm2);
- enqueuePacket(msg);
-
- return true;
- }
-
case MAV_LPS22DF_ID:
{
mavlink_message_t msg;
@@ -1116,27 +1019,54 @@ bool Radio::enqueueSensorsTm(uint8_t tmId)
case MAV_LSM6DSRX_ID:
{
- mavlink_message_t msg;
+ {
+ mavlink_message_t msg;
- auto sample = getModule<Sensors>()->getLSM6DSRXLastSample();
+ auto sample = getModule<Sensors>()->getLSM6DSRX0LastSample();
+
+ mavlink_imu_tm_t tm;
+ tm.mag_x = -1.0f;
+ tm.mag_y = -1.0f;
+ tm.mag_z = -1.0f;
+ tm.acc_x = sample.accelerationX;
+ tm.acc_y = sample.accelerationY;
+ tm.acc_z = sample.accelerationZ;
+ tm.gyro_x = sample.angularSpeedX;
+ tm.gyro_y = sample.angularSpeedY;
+ tm.gyro_z = sample.angularSpeedZ;
+ tm.timestamp = sample.accelerationTimestamp;
+ strcpy(tm.sensor_name, "LSM6DSRX_0");
+
+ mavlink_msg_imu_tm_encode(Config::Radio::MAV_SYSTEM_ID,
+ Config::Radio::MAV_COMPONENT_ID, &msg,
+ &tm);
+ enqueuePacket(msg);
+ }
- mavlink_imu_tm_t tm;
- tm.mag_x = -1.0f;
- tm.mag_y = -1.0f;
- tm.mag_z = -1.0f;
- tm.acc_x = sample.accelerationX;
- tm.acc_y = sample.accelerationY;
- tm.acc_z = sample.accelerationZ;
- tm.gyro_x = sample.angularSpeedX;
- tm.gyro_y = sample.angularSpeedY;
- tm.gyro_z = sample.angularSpeedZ;
- tm.timestamp = sample.accelerationTimestamp;
- strcpy(tm.sensor_name, "LSM6DSRX");
+ {
+ mavlink_message_t msg;
+
+ auto sample = getModule<Sensors>()->getLSM6DSRX1LastSample();
+
+ mavlink_imu_tm_t tm;
+ tm.mag_x = -1.0f;
+ tm.mag_y = -1.0f;
+ tm.mag_z = -1.0f;
+ tm.acc_x = sample.accelerationX;
+ tm.acc_y = sample.accelerationY;
+ tm.acc_z = sample.accelerationZ;
+ tm.gyro_x = sample.angularSpeedX;
+ tm.gyro_y = sample.angularSpeedY;
+ tm.gyro_z = sample.angularSpeedZ;
+ tm.timestamp = sample.accelerationTimestamp;
+ strcpy(tm.sensor_name, "LSM6DSRX_1");
+
+ mavlink_msg_imu_tm_encode(Config::Radio::MAV_SYSTEM_ID,
+ Config::Radio::MAV_COMPONENT_ID, &msg,
+ &tm);
+ enqueuePacket(msg);
+ }
- mavlink_msg_imu_tm_encode(Config::Radio::MAV_SYSTEM_ID,
- Config::Radio::MAV_COMPONENT_ID, &msg,
- &tm);
- enqueuePacket(msg);
return true;
}
@@ -1213,12 +1143,12 @@ bool Radio::enqueueSensorsTm(uint8_t tmId)
{
mavlink_message_t msg;
- auto sample = getModule<Sensors>()->getStaticPressure1LastSample();
+ auto sample = getModule<Sensors>()->getND015A0LastSample();
mavlink_pressure_tm_t tm;
tm.pressure = sample.pressure;
tm.timestamp = sample.pressureTimestamp;
- strcpy(tm.sensor_name, "StaticPressure1");
+ strcpy(tm.sensor_name, "ND015A_0");
mavlink_msg_pressure_tm_encode(Config::Radio::MAV_SYSTEM_ID,
Config::Radio::MAV_COMPONENT_ID,
@@ -1232,12 +1162,12 @@ bool Radio::enqueueSensorsTm(uint8_t tmId)
{
mavlink_message_t msg;
- auto sample = getModule<Sensors>()->getStaticPressure2LastSample();
+ auto sample = getModule<Sensors>()->getND015A0LastSample();
mavlink_pressure_tm_t tm;
tm.pressure = sample.pressure;
tm.timestamp = sample.pressureTimestamp;
- strcpy(tm.sensor_name, "StaticPressure2");
+ strcpy(tm.sensor_name, "ND015A_1");
mavlink_msg_pressure_tm_encode(Config::Radio::MAV_SYSTEM_ID,
Config::Radio::MAV_COMPONENT_ID,
diff --git a/src/Main/Sensors/HILSensors.h b/src/Main/Sensors/HILSensors.h
index 87e74fec5..dfc34428f 100644
--- a/src/Main/Sensors/HILSensors.h
+++ b/src/Main/Sensors/HILSensors.h
@@ -1,5 +1,5 @@
/* Copyright (c) 2024 Skyward Experimental Rocketry
- * Author: Emilio Corigliano
+ * Authors: Emilio Corigliano, Pietro Bortolus
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
@@ -41,12 +41,22 @@ public:
explicit HILSensors(bool enableHw) : Super{}, enableHw{enableHw} {}
private:
- void lsm6dsrxCallback() override
+ void lsm6dsrx0Callback() override
{
- if (!lsm6dsrx)
+ if (!lsm6dsrx_0)
return;
- Boardcore::Logger::getInstance().log(lsm6dsrx->getLastSample());
+ Boardcore::Logger::getInstance().log(
+ LSM6DSRX0Data{lsm6dsrx_0->getLastSample()});
+ }
+
+ void lsm6dsrx1Callback() override
+ {
+ if (!lsm6dsrx_1)
+ return;
+
+ Boardcore::Logger::getInstance().log(
+ LSM6DSRX1Data{lsm6dsrx_1->getLastSample()});
}
bool postSensorCreationHook() override
@@ -75,8 +85,6 @@ private:
Config::HIL::BARO_PITOT_RATE);
}
- hillificator<>(lps28dfw, enableHw,
- [this]() { return updateLPS28DFWData(); });
hillificator<>(lps22df, enableHw,
[this]() { return updateLPS22DFData(); });
hillificator<>(h3lis331dl, enableHw,
@@ -85,11 +93,15 @@ private:
[this]() { return updateLIS2MDLData(); });
hillificator<>(ubxgps, enableHw,
[this]() { return updateUBXGPSData(); });
- hillificator<>(lsm6dsrx, enableHw,
+ hillificator<>(lsm6dsrx_0, enableHw,
+ [this]() { return updateLSM6DSRXData(); });
+ hillificator<>(lsm6dsrx_1, enableHw,
[this]() { return updateLSM6DSRXData(); });
- hillificator<>(staticPressure1, enableHw,
+ hillificator<>(nd015a_0, enableHw,
[this]() { return updateStaticPressureData(); });
- hillificator<>(staticPressure2, enableHw,
+ hillificator<>(nd015a_1, enableHw,
+ [this]() { return updateStaticPressureData(); });
+ hillificator<>(nd015a_2, enableHw,
[this]() { return updateStaticPressureData(); });
hillificator<>(rotatedImu, enableHw,
[this]() { return updateIMUData(*this); });
@@ -128,23 +140,6 @@ private:
return sampleCounter;
}
- Boardcore::LPS28DFWData updateLPS28DFWData()
- {
- Boardcore::LPS28DFWData data;
-
- auto* sensorData = getModule<MainHIL>()->getSensorData();
-
- int iBaro = getSampleCounter(sensorData->barometer1.NDATA);
- int iTemp = getSampleCounter(sensorData->temperature.NDATA);
-
- data.pressureTimestamp = data.temperatureTimestamp =
- Boardcore::TimestampTimer::getTimestamp();
- data.pressure = sensorData->barometer1.measures[iBaro];
- data.temperature = sensorData->temperature.measures[iTemp];
-
- return data;
- };
-
Boardcore::LPS22DFData updateLPS22DFData()
{
Boardcore::LPS22DFData data;
@@ -245,9 +240,9 @@ private:
return data;
};
- Boardcore::MPXH6115AData updateStaticPressureData()
+ Boardcore::ND015XData updateStaticPressureData()
{
- Boardcore::MPXH6115AData data;
+ Boardcore::ND015XData data;
auto* sensorData = getModule<MainHIL>()->getSensorData();
@@ -262,8 +257,8 @@ private:
Boardcore::IMUData updateIMUData(Main::Sensors& sensors)
{
auto imu6 = Config::Sensors::IMU::USE_CALIBRATED_LSM6DSRX
- ? getCalibratedLSM6DSRXLastSample()
- : getLSM6DSRXLastSample();
+ ? getCalibratedLSM6DSRX0LastSample()
+ : getLSM6DSRX0LastSample();
auto mag = getLIS2MDLLastSample();
return Boardcore::IMUData{imu6, imu6, mag};
diff --git a/src/Main/Sensors/Sensors.cpp b/src/Main/Sensors/Sensors.cpp
index 9a86955e5..edeaa29a2 100644
--- a/src/Main/Sensors/Sensors.cpp
+++ b/src/Main/Sensors/Sensors.cpp
@@ -1,5 +1,5 @@
/* Copyright (c) 2024 Skyward Experimental Rocketry
- * Author: Davide Mor
+ * Authors: Davide Mor, Pietro Bortolus
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
@@ -26,6 +26,9 @@
#include <interfaces-impl/hwmapping.h>
#include <sensors/calibration/BiasCalibration/BiasCalibration.h>
+#include <chrono>
+
+using namespace std::chrono;
using namespace Main;
using namespace Boardcore;
using namespace miosix;
@@ -38,15 +41,17 @@ bool Sensors::start()
// Read the magnetometer calibration from predefined file
magCalibration.fromFile(Config::Sensors::MAG_CALIBRATION_FILENAME);
- if (Config::Sensors::LPS22DF::ENABLED)
+ if (Config::Sensors::LPS22DF::ENABLED &&
+ !Config::Sensors::USING_DUAL_MAGNETOMETER)
lps22dfInit();
- if (Config::Sensors::LPS28DFW::ENABLED)
- lps28dfwInit();
-
if (Config::Sensors::H3LIS331DL::ENABLED)
h3lis331dlInit();
+ if (Config::Sensors::LIS2MDL::ENABLED &&
+ Config::Sensors::USING_DUAL_MAGNETOMETER)
+ lis2mdlExtInit();
+
if (Config::Sensors::LIS2MDL::ENABLED)
lis2mdlInit();
@@ -54,17 +59,20 @@ bool Sensors::start()
ubxgpsInit();
if (Config::Sensors::LSM6DSRX::ENABLED)
- lsm6dsrxInit();
+ {
+ lsm6dsrx0Init();
+ lsm6dsrx1Init();
+ }
if (Config::Sensors::VN100::ENABLED)
vn100Init();
- if (Config::Sensors::ADS131M08::ENABLED)
+ if (Config::Sensors::ND015A::ENABLED)
{
- ads131m08Init();
- staticPressure1Init();
- staticPressure2Init();
- dplBayPressureInit();
+ nd015a0Init();
+ nd015a1Init();
+ nd015a2Init();
+ nd015a3Init();
}
if (Config::Sensors::InternalADC::ENABLED)
@@ -88,7 +96,7 @@ bool Sensors::start()
magCalibrationTaskId = getSensorsScheduler().addTask(
[this]()
{
- auto mag = getLIS2MDLLastSample();
+ auto mag = getLIS2MDLExtLastSample();
Lock<FastMutex> lock{magCalibrationMutex};
magCalibrator.feed(mag);
@@ -111,51 +119,16 @@ bool Sensors::start()
void Sensors::calibrate()
{
BiasCalibration gyroCalibrator{};
- float staticPressure1Acc = 0.0f;
- float staticPressure2Acc = 0.0f;
- float dplBayPressureAcc = 0.0f;
- float lps28dfwAcc = 0.0f;
for (int i = 0; i < Config::Sensors::CALIBRATION_SAMPLES_COUNT; i++)
{
- auto lsm6dsrx = getLSM6DSRXLastSample();
- auto staticPressure1 = getStaticPressure1LastSample();
- auto staticPressure2 = getStaticPressure2LastSample();
- auto dplBayPressure = getDplBayPressureLastSample();
- auto lps28dfw = getLPS28DFWLastSample();
+ auto lsm6dsrx = getLSM6DSRX0LastSample();
gyroCalibrator.feed(static_cast<GyroscopeData>(lsm6dsrx));
- staticPressure1Acc += staticPressure1.pressure;
- staticPressure2Acc += staticPressure2.pressure;
- dplBayPressureAcc += dplBayPressure.pressure;
- lps28dfwAcc += lps28dfw.pressure;
Thread::sleep(Config::Sensors::CALIBRATION_SLEEP_TIME);
}
- staticPressure1Acc /= Config::Sensors::CALIBRATION_SAMPLES_COUNT;
- staticPressure2Acc /= Config::Sensors::CALIBRATION_SAMPLES_COUNT;
- dplBayPressureAcc /= Config::Sensors::CALIBRATION_SAMPLES_COUNT;
- lps28dfwAcc /= Config::Sensors::CALIBRATION_SAMPLES_COUNT;
-
- // Calibrate all analog pressure sensors against the LPS28DFW or the
- // telemetry reference
- float reference;
- {
- Lock<FastMutex> lock{baroCalibrationMutex};
- reference = useBaroCalibrationReference ? baroCalibrationReference
- : lps28dfwAcc;
- }
-
- if (reference > Config::Sensors::ATMOS_CALIBRATION_THRESH)
- {
- // Calibrate sensors only if reference is valid
- // LPS28DFW might be disabled or unresponsive
- staticPressure1->updateOffset(staticPressure1Acc - reference);
- staticPressure2->updateOffset(staticPressure2Acc - reference);
- dplBayPressure->updateOffset(dplBayPressureAcc - reference);
- }
-
{
Lock<FastMutex> lock{gyroCalibrationMutex};
gyroCalibration = gyroCalibrator.computeResult();
@@ -173,39 +146,19 @@ CalibrationData Sensors::getCalibration()
CalibrationData data;
data.timestamp = TimestampTimer::getTimestamp();
- data.gyroBiasX = gyroCalibration.getb().x();
- data.gyroBiasY = gyroCalibration.getb().y();
- data.gyroBiasZ = gyroCalibration.getb().z();
- data.magBiasX = magCalibration.getb().x();
- data.magBiasY = magCalibration.getb().y();
- data.magBiasZ = magCalibration.getb().z();
- data.magScaleX = magCalibration.getA().x();
- data.magScaleY = magCalibration.getA().y();
- data.magScaleZ = magCalibration.getA().z();
- data.staticPress1Bias = staticPressure1->getOffset();
- data.staticPress1Scale = 1.0f;
- data.staticPress2Bias = staticPressure2->getOffset();
- data.staticPress2Scale = 1.0f;
- data.dplBayPressBias = dplBayPressure->getOffset();
- data.dplBayPressScale = 1.0f;
+ data.gyroBiasX = gyroCalibration.getb().x();
+ data.gyroBiasY = gyroCalibration.getb().y();
+ data.gyroBiasZ = gyroCalibration.getb().z();
+ data.magBiasX = magCalibration.getb().x();
+ data.magBiasY = magCalibration.getb().y();
+ data.magBiasZ = magCalibration.getb().z();
+ data.magScaleX = magCalibration.getA().x();
+ data.magScaleY = magCalibration.getA().y();
+ data.magScaleZ = magCalibration.getA().z();
return data;
}
-void Sensors::setBaroCalibrationReference(float reference)
-{
- Lock<FastMutex> lock{baroCalibrationMutex};
- baroCalibrationReference = reference;
- useBaroCalibrationReference = true;
-}
-
-void Sensors::resetBaroCalibrationReference()
-{
- Lock<FastMutex> lock{baroCalibrationMutex};
- baroCalibrationReference = 0.0f;
- useBaroCalibrationReference = false;
-}
-
void Sensors::resetMagCalibrator()
{
Lock<FastMutex> lock{magCalibrationMutex};
@@ -251,14 +204,14 @@ LPS22DFData Sensors::getLPS22DFLastSample()
return lps22df ? lps22df->getLastSample() : LPS22DFData{};
}
-LPS28DFWData Sensors::getLPS28DFWLastSample()
+H3LIS331DLData Sensors::getH3LIS331DLLastSample()
{
- return lps28dfw ? lps28dfw->getLastSample() : LPS28DFWData{};
+ return h3lis331dl ? h3lis331dl->getLastSample() : H3LIS331DLData{};
}
-H3LIS331DLData Sensors::getH3LIS331DLLastSample()
+LIS2MDLData Sensors::getLIS2MDLExtLastSample()
{
- return h3lis331dl ? h3lis331dl->getLastSample() : H3LIS331DLData{};
+ return lis2mdl_ext ? lis2mdl_ext->getLastSample() : LIS2MDLData{};
}
LIS2MDLData Sensors::getLIS2MDLLastSample()
@@ -271,19 +224,19 @@ UBXGPSData Sensors::getUBXGPSLastSample()
return ubxgps ? ubxgps->getLastSample() : UBXGPSData{};
}
-LSM6DSRXData Sensors::getLSM6DSRXLastSample()
+LSM6DSRXData Sensors::getLSM6DSRX0LastSample()
{
- return lsm6dsrx ? lsm6dsrx->getLastSample() : LSM6DSRXData{};
+ return lsm6dsrx_0 ? lsm6dsrx_0->getLastSample() : LSM6DSRXData{};
}
-VN100SpiData Sensors::getVN100LastSample()
+LSM6DSRXData Sensors::getLSM6DSRX1LastSample()
{
- return vn100 ? vn100->getLastSample() : VN100SpiData{};
+ return lsm6dsrx_1 ? lsm6dsrx_1->getLastSample() : LSM6DSRXData{};
}
-ADS131M08Data Sensors::getADS131M08LastSample()
+VN100SpiData Sensors::getVN100LastSample()
{
- return ads131m08 ? ads131m08->getLastSample() : ADS131M08Data{};
+ return vn100 ? vn100->getLastSample() : VN100SpiData{};
}
InternalADCData Sensors::getInternalADCLastSample()
@@ -308,19 +261,40 @@ VoltageData Sensors::getCamBatteryVoltageLastSample()
return {sample.timestamp, voltage};
}
-PressureData Sensors::getStaticPressure1LastSample()
+ND015XData Sensors::getND015A0LastSample()
{
- return staticPressure1 ? staticPressure1->getLastSample() : PressureData{};
+ return nd015a_0 ? nd015a_0->getLastSample() : ND015XData{};
}
-PressureData Sensors::getStaticPressure2LastSample()
+ND015XData Sensors::getND015A1LastSample()
{
- return staticPressure2 ? staticPressure2->getLastSample() : PressureData{};
+ return nd015a_1 ? nd015a_1->getLastSample() : ND015XData{};
}
-PressureData Sensors::getDplBayPressureLastSample()
+ND015XData Sensors::getND015A2LastSample()
+{
+ return nd015a_3 ? nd015a_2->getLastSample() : ND015XData{};
+}
+
+ND015XData Sensors::getND015A3LastSample()
+{
+ return nd015a_3 ? nd015a_3->getLastSample() : ND015XData{};
+}
+
+LIS2MDLData Sensors::getCalibratedLIS2MDLExtLastSample()
{
- return dplBayPressure ? dplBayPressure->getLastSample() : PressureData{};
+ auto sample = getLIS2MDLExtLastSample();
+
+ {
+ Lock<FastMutex> lock{magCalibrationMutex};
+ auto corrected =
+ magCalibration.correct(static_cast<MagnetometerData>(sample));
+ sample.magneticFieldX = corrected.x();
+ sample.magneticFieldY = corrected.y();
+ sample.magneticFieldZ = corrected.z();
+ }
+
+ return sample;
}
LIS2MDLData Sensors::getCalibratedLIS2MDLLastSample()
@@ -339,9 +313,26 @@ LIS2MDLData Sensors::getCalibratedLIS2MDLLastSample()
return sample;
}
-LSM6DSRXData Sensors::getCalibratedLSM6DSRXLastSample()
+LSM6DSRXData Sensors::getCalibratedLSM6DSRX0LastSample()
{
- auto sample = getLSM6DSRXLastSample();
+ auto sample = getLSM6DSRX0LastSample();
+
+ {
+ // This is for my boy Giuseppe <3
+ Lock<FastMutex> lock{gyroCalibrationMutex};
+ auto corrected =
+ gyroCalibration.correct(static_cast<GyroscopeData>(sample));
+ sample.angularSpeedX = corrected.x();
+ sample.angularSpeedY = corrected.y();
+ sample.angularSpeedZ = corrected.z();
+ }
+
+ return sample;
+}
+
+LSM6DSRXData Sensors::getCalibratedLSM6DSRX1LastSample()
+{
+ auto sample = getLSM6DSRX1LastSample();
{
// This is for my boy Giuseppe <3
@@ -361,12 +352,34 @@ IMUData Sensors::getIMULastSample()
return rotatedImu ? rotatedImu->getLastSample() : IMUData{};
}
-PressureData Sensors::getAtmosPressureLastSample()
+PressureData Sensors::getAtmosPressureLastSample(
+ Config::Sensors::Atmos::AtmosSensor sensor)
{
- if (Config::Sensors::Atmos::USE_PORT_2)
- return getStaticPressure2LastSample();
- else
- return getStaticPressure1LastSample();
+ switch (sensor)
+ {
+ case (Config::Sensors::Atmos::AtmosSensor::SENSOR_0):
+ return getND015A0LastSample();
+ case (Config::Sensors::Atmos::AtmosSensor::SENSOR_1):
+ return getND015A1LastSample();
+ case (Config::Sensors::Atmos::AtmosSensor::SENSOR_2):
+ return getND015A2LastSample();
+ default:
+ {
+ LOG_ERR(logger,
+ "Invalid ATMOS_SENSOR value, using ND015A_0 sensor");
+ return getND015A0LastSample();
+ }
+ }
+}
+
+PressureData Sensors::getDplBayPressureLastSample()
+{
+ return getND015A3LastSample();
+}
+
+TemperatureData Sensors::getTemperatureLastSample()
+{
+ return getLSM6DSRX0LastSample();
}
PressureData Sensors::getCanPitotDynamicPressLastSample()
@@ -459,44 +472,25 @@ std::vector<SensorInfo> Sensors::getSensorInfos()
{
std::vector<SensorInfo> infos{};
- if (lps22df)
- infos.push_back(manager->getSensorInfo(lps22df.get()));
-
- if (lps28dfw)
- infos.push_back(manager->getSensorInfo(lps28dfw.get()));
-
- if (h3lis331dl)
- infos.push_back(manager->getSensorInfo(h3lis331dl.get()));
-
- if (lis2mdl)
- infos.push_back(manager->getSensorInfo(lis2mdl.get()));
-
- if (ubxgps)
- infos.push_back(manager->getSensorInfo(ubxgps.get()));
-
- if (lsm6dsrx)
- infos.push_back(manager->getSensorInfo(lsm6dsrx.get()));
-
- if (vn100)
- infos.push_back(manager->getSensorInfo(vn100.get()));
-
- if (ads131m08)
- infos.push_back(manager->getSensorInfo(ads131m08.get()));
-
- if (internalAdc)
- infos.push_back(manager->getSensorInfo(internalAdc.get()));
-
- if (staticPressure1)
- infos.push_back(manager->getSensorInfo(staticPressure1.get()));
-
- if (staticPressure2)
- infos.push_back(manager->getSensorInfo(staticPressure2.get()));
-
- if (dplBayPressure)
- infos.push_back(manager->getSensorInfo(dplBayPressure.get()));
-
- if (rotatedImu)
- infos.push_back(manager->getSensorInfo(rotatedImu.get()));
+#define PUSH_SENSOR_INFO(instance, name) \
+ if (instance) \
+ infos.push_back(manager->getSensorInfo(instance.get())); \
+ else \
+ infos.push_back(SensorInfo{name, 0, nullptr, false})
+
+ PUSH_SENSOR_INFO(lps22df, "LPS22DF");
+ PUSH_SENSOR_INFO(lis2mdl_ext, "LIS2MDL_EXT");
+ PUSH_SENSOR_INFO(lis2mdl, "LIS2MDL");
+ PUSH_SENSOR_INFO(ubxgps, "UBXGPS");
+ PUSH_SENSOR_INFO(lsm6dsrx_0, "LSM6DSRX_0");
+ PUSH_SENSOR_INFO(lsm6dsrx_1, "LSM6DSRX_1");
+ PUSH_SENSOR_INFO(vn100, "VN100");
+ PUSH_SENSOR_INFO(internalAdc, "InternalADC");
+ PUSH_SENSOR_INFO(nd015a_0, "ND015A_0");
+ PUSH_SENSOR_INFO(nd015a_1, "ND015A_1");
+ PUSH_SENSOR_INFO(nd015a_2, "ND015A_2");
+ PUSH_SENSOR_INFO(nd015a_3, "ND015A_3");
+ PUSH_SENSOR_INFO(rotatedImu, "RotatedIMU");
return infos;
}
@@ -527,21 +521,6 @@ void Sensors::lps22dfInit()
void Sensors::lps22dfCallback() { sdLogger.log(getLPS22DFLastSample()); }
-void Sensors::lps28dfwInit()
-{
- LPS28DFW::SensorConfig config;
- config.sa0 = true;
- config.fsr = Config::Sensors::LPS28DFW::FS;
- config.avg = Config::Sensors::LPS28DFW::AVG;
- config.odr = Config::Sensors::LPS28DFW::ODR;
- config.drdy = false;
-
- lps28dfw =
- std::make_unique<LPS28DFW>(getModule<Buses>()->getLPS28DFW(), config);
-}
-
-void Sensors::lps28dfwCallback() { sdLogger.log(getLPS28DFWLastSample()); }
-
void Sensors::h3lis331dlInit()
{
SPIBusConfig spiConfig = H3LIS331DL::getDefaultSPIConfig();
@@ -563,9 +542,29 @@ void Sensors::h3lis331dlCallback()
sdLogger.log(sample);
}
+void Sensors::lis2mdlExtInit()
+{
+ SPIBusConfig spiConfig = LIS2MDL::getDefaultSPIConfig();
+ spiConfig.clockDivider = SPI::ClockDivider::DIV_16;
+
+ LIS2MDL::Config config;
+ config.deviceMode = LIS2MDL::MD_CONTINUOUS;
+ config.odr = Config::Sensors::LIS2MDL::ODR;
+ config.temperatureDivider = Config::Sensors::LIS2MDL::TEMP_DIVIDER;
+
+ lis2mdl_ext = std::make_unique<LIS2MDL>(getModule<Buses>()->getLIS2MDL(),
+ sensors::LIS2MDL_EXT::cs::getPin(),
+ spiConfig, config);
+}
+
+void Sensors::lis2mdlExtCallback()
+{
+ sdLogger.log(LIS2MDLExternalData{getLIS2MDLExtLastSample()});
+}
+
void Sensors::lis2mdlInit()
{
- SPIBusConfig spiConfig = H3LIS331DL::getDefaultSPIConfig();
+ SPIBusConfig spiConfig = LIS2MDL::getDefaultSPIConfig();
spiConfig.clockDivider = SPI::ClockDivider::DIV_16;
LIS2MDL::Config config;
@@ -592,7 +591,7 @@ void Sensors::ubxgpsInit()
void Sensors::ubxgpsCallback() { sdLogger.log(getUBXGPSLastSample()); }
-void Sensors::lsm6dsrxInit()
+void Sensors::lsm6dsrx0Init()
{
SPIBusConfig spiConfig;
spiConfig.clockDivider = SPI::ClockDivider::DIV_32;
@@ -614,84 +613,74 @@ void Sensors::lsm6dsrxInit()
LSM6DSRXConfig::FIFO_TIMESTAMP_DECIMATION::DEC_1;
config.fifoTemperatureBdr = LSM6DSRXConfig::FIFO_TEMPERATURE_BDR::DISABLED;
- lsm6dsrx = std::make_unique<LSM6DSRX>(getModule<Buses>()->getLSM6DSRX(),
- sensors::LSM6DSRX::cs::getPin(),
- spiConfig, config);
+ lsm6dsrx_0 = std::make_unique<LSM6DSRX>(getModule<Buses>()->getLSM6DSRX(),
+ sensors::LSM6DSRX_0::cs::getPin(),
+ spiConfig, config);
}
-void Sensors::lsm6dsrxCallback()
+void Sensors::lsm6dsrx0Callback()
{
- if (!lsm6dsrx)
+ if (!lsm6dsrx_0)
return;
// For every instance inside the fifo log the sample
uint16_t lastFifoSize;
- const auto lastFifo = lsm6dsrx->getLastFifo(lastFifoSize);
+ const auto lastFifo = lsm6dsrx_0->getLastFifo(lastFifoSize);
for (uint16_t i = 0; i < lastFifoSize; i++)
- sdLogger.log(lastFifo.at(i));
+ sdLogger.log(LSM6DSRX0Data{lastFifo.at(i)});
}
-void Sensors::vn100Init()
+void Sensors::lsm6dsrx1Init()
{
SPIBusConfig spiConfig;
spiConfig.clockDivider = SPI::ClockDivider::DIV_32;
- spiConfig.mode = SPI::Mode::MODE_3;
+ spiConfig.mode = SPI::Mode::MODE_0;
- vn100 = std::make_unique<VN100Spi>(getModule<Buses>()->getVN100(),
- sensors::VN100::cs::getPin(), spiConfig,
- 200);
+ LSM6DSRXConfig config;
+ config.bdu = LSM6DSRXConfig::BDU::CONTINUOUS_UPDATE;
+
+ config.fsAcc = Config::Sensors::LSM6DSRX::ACC_FS;
+ config.odrAcc = Config::Sensors::LSM6DSRX::ACC_ODR;
+ config.opModeAcc = Config::Sensors::LSM6DSRX::ACC_OP_MODE;
+
+ config.fsGyr = Config::Sensors::LSM6DSRX::GYR_FS;
+ config.odrGyr = Config::Sensors::LSM6DSRX::GYR_ODR;
+ config.opModeGyr = Config::Sensors::LSM6DSRX::GYR_OP_MODE;
+
+ config.fifoMode = LSM6DSRXConfig::FIFO_MODE::CONTINUOUS;
+ config.fifoTimestampDecimation =
+ LSM6DSRXConfig::FIFO_TIMESTAMP_DECIMATION::DEC_1;
+ config.fifoTemperatureBdr = LSM6DSRXConfig::FIFO_TEMPERATURE_BDR::DISABLED;
+
+ lsm6dsrx_1 = std::make_unique<LSM6DSRX>(getModule<Buses>()->getLSM6DSRX(),
+ sensors::LSM6DSRX_1::cs::getPin(),
+ spiConfig, config);
}
-void Sensors::vn100Callback() { sdLogger.log(getVN100LastSample()); }
+void Sensors::lsm6dsrx1Callback()
+{
+ if (!lsm6dsrx_1)
+ return;
+
+ // For every instance inside the fifo log the sample
+ uint16_t lastFifoSize;
+ const auto lastFifo = lsm6dsrx_1->getLastFifo(lastFifoSize);
+ for (uint16_t i = 0; i < lastFifoSize; i++)
+ sdLogger.log(LSM6DSRX1Data{lastFifo.at(i)});
+}
-void Sensors::ads131m08Init()
+void Sensors::vn100Init()
{
SPIBusConfig spiConfig;
spiConfig.clockDivider = SPI::ClockDivider::DIV_32;
+ spiConfig.mode = SPI::Mode::MODE_3;
- ADS131M08::Config config;
- config.oversamplingRatio = Config::Sensors::ADS131M08::OSR;
- config.globalChopModeEnabled =
- Config::Sensors::ADS131M08::GLOBAL_CHOP_MODE_EN;
-
- // Disable all channels
- config.channelsConfig[0].enabled = false;
- config.channelsConfig[1].enabled = false;
- config.channelsConfig[2].enabled = false;
- config.channelsConfig[3].enabled = false;
- config.channelsConfig[4].enabled = false;
- config.channelsConfig[5].enabled = false;
- config.channelsConfig[6].enabled = false;
- config.channelsConfig[7].enabled = false;
-
- // Enable required channels
- config.channelsConfig[(
- int)Config::Sensors::ADS131M08::STATIC_PRESSURE_1_CHANNEL] = {
- .enabled = true,
- .pga = ADS131M08Defs::PGA::PGA_1,
- .offset = 0,
- .gain = 1.0};
-
- config.channelsConfig[(
- int)Config::Sensors::ADS131M08::STATIC_PRESSURE_2_CHANNEL] = {
- .enabled = true,
- .pga = ADS131M08Defs::PGA::PGA_1,
- .offset = 0,
- .gain = 1.0};
-
- config.channelsConfig[(
- int)Config::Sensors::ADS131M08::DPL_BAY_PRESSURE_CHANNEL] = {
- .enabled = true,
- .pga = ADS131M08Defs::PGA::PGA_1,
- .offset = 0,
- .gain = 1.0};
-
- ads131m08 = std::make_unique<ADS131M08>(getModule<Buses>()->getADS131M08(),
- sensors::ADS131M08::cs::getPin(),
- spiConfig, config);
+ vn100 = std::make_unique<VN100Spi>(getModule<Buses>()->getVN100(),
+ sensors::VN100::cs::getPin(), spiConfig,
+ 200);
}
-void Sensors::ads131m08Callback() { sdLogger.log(getADS131M08LastSample()); }
+void Sensors::vn100Callback() { sdLogger.log(getVN100LastSample()); }
void Sensors::internalAdcInit()
{
@@ -707,67 +696,64 @@ void Sensors::internalAdcCallback()
sdLogger.log(getInternalADCLastSample());
}
-void Sensors::staticPressure1Init()
+void Sensors::nd015a0Init()
{
- staticPressure1 = std::make_unique<MPXH6115A>(
- [this]()
- {
- auto sample = getADS131M08LastSample();
- auto voltage = sample.getVoltage(
- Config::Sensors::ADS131M08::STATIC_PRESSURE_1_CHANNEL);
- voltage.voltage *=
- Config::Sensors::ADS131M08::STATIC_PRESSURE_1_SCALE;
+ SPIBusConfig spiConfig = ND015A::getDefaultSPIConfig();
- return voltage;
- });
+ nd015a_0 = std::make_unique<ND015A>(
+ getModule<Buses>()->getND015A(), sensors::ND015A_0::cs::getPin(),
+ spiConfig, Config::Sensors::ND015A::IOW, Config::Sensors::ND015A::BWL,
+ Config::Sensors::ND015A::NTC, Config::Sensors::ND015A::ODR);
}
-void Sensors::staticPressure1Callback()
+void Sensors::nd015a0Callback()
{
- sdLogger.log(StaticPressureData1{getStaticPressure1LastSample()});
+ sdLogger.log(StaticPressure0Data{getND015A0LastSample()});
}
-void Sensors::staticPressure2Init()
+void Sensors::nd015a1Init()
{
- staticPressure2 = std::make_unique<MPXH6115A>(
- [this]()
- {
- auto sample = getADS131M08LastSample();
- auto voltage = sample.getVoltage(
- Config::Sensors::ADS131M08::STATIC_PRESSURE_2_CHANNEL);
- voltage.voltage *=
- Config::Sensors::ADS131M08::STATIC_PRESSURE_2_SCALE;
+ SPIBusConfig spiConfig = ND015A::getDefaultSPIConfig();
- return voltage;
- });
+ nd015a_1 = std::make_unique<ND015A>(
+ getModule<Buses>()->getND015A(), sensors::ND015A_1::cs::getPin(),
+ spiConfig, Config::Sensors::ND015A::IOW, Config::Sensors::ND015A::BWL,
+ Config::Sensors::ND015A::NTC, Config::Sensors::ND015A::ODR);
}
-void Sensors::staticPressure2Callback()
+void Sensors::nd015a1Callback()
{
- sdLogger.log(StaticPressureData2{getStaticPressure2LastSample()});
+ sdLogger.log(StaticPressure1Data{getND015A1LastSample()});
}
-void Sensors::dplBayPressureInit()
+void Sensors::nd015a2Init()
{
- dplBayPressure = std::make_unique<MPXH6115A>(
- [this]()
- {
- auto sample = getADS131M08LastSample();
- auto voltage = sample.getVoltage(
- Config::Sensors::ADS131M08::DPL_BAY_PRESSURE_CHANNEL);
- voltage.voltage *=
- Config::Sensors::ADS131M08::DPL_BAY_PRESSURE_SCALE;
+ SPIBusConfig spiConfig = ND015A::getDefaultSPIConfig();
- return voltage;
- });
+ nd015a_2 = std::make_unique<ND015A>(
+ getModule<Buses>()->getND015A(), sensors::ND015A_2::cs::getPin(),
+ spiConfig, Config::Sensors::ND015A::IOW, Config::Sensors::ND015A::BWL,
+ Config::Sensors::ND015A::NTC, Config::Sensors::ND015A::ODR);
}
-void Sensors::dplBayPressureCallback()
+void Sensors::nd015a2Callback()
{
- auto sample = getDplBayPressureLastSample();
+ sdLogger.log(StaticPressure2Data{getND015A2LastSample()});
+}
- getModule<StatsRecorder>()->updateDplPressure(sample);
- sdLogger.log(DplBayPressureData{sample});
+void Sensors::nd015a3Init()
+{
+ SPIBusConfig spiConfig = ND015A::getDefaultSPIConfig();
+
+ nd015a_3 = std::make_unique<ND015A>(
+ getModule<Buses>()->getND015A(), sensors::ND015A_3::cs::getPin(),
+ spiConfig, Config::Sensors::ND015A::IOW, Config::Sensors::ND015A::BWL,
+ Config::Sensors::ND015A::NTC, Config::Sensors::ND015A::ODR);
+}
+
+void Sensors::nd015a3Callback()
+{
+ sdLogger.log(DplBayPressureData{getND015A3LastSample()});
}
void Sensors::rotatedImuInit()
@@ -776,8 +762,8 @@ void Sensors::rotatedImuInit()
[this]()
{
auto imu6 = Config::Sensors::IMU::USE_CALIBRATED_LSM6DSRX
- ? getCalibratedLSM6DSRXLastSample()
- : getLSM6DSRXLastSample();
+ ? getCalibratedLSM6DSRX0LastSample()
+ : getLSM6DSRX0LastSample();
auto mag = Config::Sensors::IMU::USE_CALIBRATED_LIS2MDL
? getCalibratedLIS2MDLLastSample()
: getLIS2MDLLastSample();
@@ -812,13 +798,6 @@ bool Sensors::sensorManagerInit()
map.emplace(lps22df.get(), info);
}
- if (lps28dfw)
- {
- SensorInfo info{"LPS28DFW", Config::Sensors::LPS28DFW::RATE,
- [this]() { lps28dfwCallback(); }};
- map.emplace(lps28dfw.get(), info);
- }
-
if (h3lis331dl)
{
SensorInfo info{"H3LIS331DL", Config::Sensors::H3LIS331DL::RATE,
@@ -826,6 +805,13 @@ bool Sensors::sensorManagerInit()
map.emplace(h3lis331dl.get(), info);
}
+ if (lis2mdl_ext)
+ {
+ SensorInfo info{"LIS2MDL_EXT", Config::Sensors::LIS2MDL::RATE,
+ [this]() { lis2mdlExtCallback(); }};
+ map.emplace(lis2mdl_ext.get(), info);
+ }
+
if (lis2mdl)
{
SensorInfo info{"LIS2MDL", Config::Sensors::LIS2MDL::RATE,
@@ -840,11 +826,18 @@ bool Sensors::sensorManagerInit()
map.emplace(ubxgps.get(), info);
}
- if (lsm6dsrx)
+ if (lsm6dsrx_0)
{
- SensorInfo info{"LSM6DSRX", Config::Sensors::LSM6DSRX::RATE,
- [this]() { lsm6dsrxCallback(); }};
- map.emplace(lsm6dsrx.get(), info);
+ SensorInfo info{"LSM6DSRX_0", Config::Sensors::LSM6DSRX::RATE,
+ [this]() { lsm6dsrx0Callback(); }};
+ map.emplace(lsm6dsrx_0.get(), info);
+ }
+
+ if (lsm6dsrx_1)
+ {
+ SensorInfo info{"LSM6DSRX_1", Config::Sensors::LSM6DSRX::RATE,
+ [this]() { lsm6dsrx1Callback(); }};
+ map.emplace(lsm6dsrx_1.get(), info);
}
if (vn100)
@@ -854,39 +847,39 @@ bool Sensors::sensorManagerInit()
map.emplace(vn100.get(), info);
}
- if (ads131m08)
+ if (internalAdc)
{
- SensorInfo info{"ADS131M08", Config::Sensors::ADS131M08::RATE,
- [this]() { ads131m08Callback(); }};
- map.emplace(ads131m08.get(), info);
+ SensorInfo info{"InternalADC", Config::Sensors::InternalADC::RATE,
+ [this]() { internalAdcCallback(); }};
+ map.emplace(internalAdc.get(), info);
}
- if (staticPressure1)
+ if (nd015a_0)
{
- SensorInfo info{"StaticPressure1", Config::Sensors::ADS131M08::RATE,
- [this]() { staticPressure1Callback(); }};
- map.emplace(staticPressure1.get(), info);
+ SensorInfo info{"ND015A_0", Config::Sensors::ND015A::RATE,
+ [this]() { nd015a0Callback(); }};
+ map.emplace(nd015a_0.get(), info);
}
- if (staticPressure2)
+ if (nd015a_1)
{
- SensorInfo info{"StaticPressure2", Config::Sensors::ADS131M08::RATE,
- [this]() { staticPressure2Callback(); }};
- map.emplace(staticPressure2.get(), info);
+ SensorInfo info{"ND015A_1", Config::Sensors::ND015A::RATE,
+ [this]() { nd015a1Callback(); }};
+ map.emplace(nd015a_1.get(), info);
}
- if (dplBayPressure)
+ if (nd015a_2)
{
- SensorInfo info{"DplBayPressure", Config::Sensors::ADS131M08::RATE,
- [this]() { dplBayPressureCallback(); }};
- map.emplace(dplBayPressure.get(), info);
+ SensorInfo info{"ND015A_2", Config::Sensors::ND015A::RATE,
+ [this]() { nd015a2Callback(); }};
+ map.emplace(nd015a_2.get(), info);
}
- if (internalAdc)
+ if (nd015a_3)
{
- SensorInfo info{"InternalADC", Config::Sensors::InternalADC::RATE,
- [this]() { internalAdcCallback(); }};
- map.emplace(internalAdc.get(), info);
+ SensorInfo info{"ND015A_3", Config::Sensors::ND015A::RATE,
+ [this]() { nd015a3Callback(); }};
+ map.emplace(nd015a_3.get(), info);
}
if (rotatedImu)
diff --git a/src/Main/Sensors/Sensors.h b/src/Main/Sensors/Sensors.h
index 92850391f..bb4bda036 100644
--- a/src/Main/Sensors/Sensors.h
+++ b/src/Main/Sensors/Sensors.h
@@ -1,5 +1,5 @@
/* Copyright (c) 2024 Skyward Experimental Rocketry
- * Author: Davide Mor
+ * Authors: Davide Mor, Pietro Bortolus
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
@@ -24,17 +24,17 @@
#include <Main/BoardScheduler.h>
#include <Main/Buses.h>
+#include <Main/Configs/SensorsConfig.h>
#include <Main/Sensors/SensorsData.h>
#include <Main/StatsRecorder/StatsRecorder.h>
#include <diagnostic/PrintLogger.h>
#include <drivers/adc/InternalADC.h>
#include <scheduler/TaskScheduler.h>
-#include <sensors/ADS131M08/ADS131M08.h>
#include <sensors/H3LIS331DL/H3LIS331DL.h>
#include <sensors/LIS2MDL/LIS2MDL.h>
#include <sensors/LPS22DF/LPS22DF.h>
-#include <sensors/LPS28DFW/LPS28DFW.h>
#include <sensors/LSM6DSRX/LSM6DSRX.h>
+#include <sensors/ND015X/ND015A.h>
#include <sensors/RotatedIMU/RotatedIMU.h>
#include <sensors/SensorManager.h>
#include <sensors/UBXGPS/UBXGPSSpi.h>
@@ -63,36 +63,40 @@ public:
CalibrationData getCalibration();
- void setBaroCalibrationReference(float reference);
- void resetBaroCalibrationReference();
-
void resetMagCalibrator();
void enableMagCalibrator();
void disableMagCalibrator();
bool saveMagCalibration();
Boardcore::LPS22DFData getLPS22DFLastSample();
- Boardcore::LPS28DFWData getLPS28DFWLastSample();
Boardcore::H3LIS331DLData getH3LIS331DLLastSample();
+ Boardcore::LIS2MDLData getLIS2MDLExtLastSample();
Boardcore::LIS2MDLData getLIS2MDLLastSample();
Boardcore::UBXGPSData getUBXGPSLastSample();
- Boardcore::LSM6DSRXData getLSM6DSRXLastSample();
+ Boardcore::LSM6DSRXData getLSM6DSRX0LastSample();
+ Boardcore::LSM6DSRXData getLSM6DSRX1LastSample();
Boardcore::VN100SpiData getVN100LastSample();
- Boardcore::ADS131M08Data getADS131M08LastSample();
Boardcore::InternalADCData getInternalADCLastSample();
+ Boardcore::LIS2MDLData getCalibratedLIS2MDLExtLastSample();
Boardcore::LIS2MDLData getCalibratedLIS2MDLLastSample();
- Boardcore::LSM6DSRXData getCalibratedLSM6DSRXLastSample();
+ Boardcore::LSM6DSRXData getCalibratedLSM6DSRX0LastSample();
+ Boardcore::LSM6DSRXData getCalibratedLSM6DSRX1LastSample();
+ Boardcore::IMUData getIMULastSample();
+
+ Boardcore::ND015XData getND015A0LastSample();
+ Boardcore::ND015XData getND015A1LastSample();
+ Boardcore::ND015XData getND015A2LastSample();
+ Boardcore::ND015XData getND015A3LastSample();
Boardcore::VoltageData getBatteryVoltageLastSample();
Boardcore::VoltageData getCamBatteryVoltageLastSample();
- Boardcore::PressureData getStaticPressure1LastSample();
- Boardcore::PressureData getStaticPressure2LastSample();
+ Boardcore::PressureData getAtmosPressureLastSample(
+ Config::Sensors::Atmos::AtmosSensor =
+ Config::Sensors::Atmos::ATMOS_SENSOR);
Boardcore::PressureData getDplBayPressureLastSample();
-
- Boardcore::IMUData getIMULastSample();
- Boardcore::PressureData getAtmosPressureLastSample();
+ Boardcore::TemperatureData getTemperatureLastSample();
Boardcore::PressureData getCanPitotDynamicPressLastSample();
Boardcore::PressureData getCanPitotStaticPressLastSample();
@@ -116,7 +120,8 @@ public:
protected:
virtual bool postSensorCreationHook() { return true; }
- virtual void lsm6dsrxCallback();
+ virtual void lsm6dsrx0Callback();
+ virtual void lsm6dsrx1Callback();
Boardcore::TaskScheduler& getSensorsScheduler();
@@ -131,19 +136,18 @@ protected:
// Digital sensors
std::unique_ptr<Boardcore::LPS22DF> lps22df;
- std::unique_ptr<Boardcore::LPS28DFW> lps28dfw;
std::unique_ptr<Boardcore::H3LIS331DL> h3lis331dl;
+ std::unique_ptr<Boardcore::LIS2MDL> lis2mdl_ext;
std::unique_ptr<Boardcore::LIS2MDL> lis2mdl;
std::unique_ptr<Boardcore::UBXGPSSpi> ubxgps;
- std::unique_ptr<Boardcore::LSM6DSRX> lsm6dsrx;
+ std::unique_ptr<Boardcore::LSM6DSRX> lsm6dsrx_0;
+ std::unique_ptr<Boardcore::LSM6DSRX> lsm6dsrx_1;
std::unique_ptr<Boardcore::VN100Spi> vn100;
- std::unique_ptr<Boardcore::ADS131M08> ads131m08;
std::unique_ptr<Boardcore::InternalADC> internalAdc;
-
- // Analog sensors
- std::unique_ptr<Boardcore::MPXH6115A> staticPressure1;
- std::unique_ptr<Boardcore::MPXH6115A> staticPressure2;
- std::unique_ptr<Boardcore::MPXH6115A> dplBayPressure;
+ std::unique_ptr<Boardcore::ND015A> nd015a_0;
+ std::unique_ptr<Boardcore::ND015A> nd015a_1;
+ std::unique_ptr<Boardcore::ND015A> nd015a_2;
+ std::unique_ptr<Boardcore::ND015A> nd015a_3;
// Virtual sensors
std::unique_ptr<Boardcore::RotatedIMU> rotatedImu;
@@ -154,47 +158,45 @@ private:
void lps22dfInit();
void lps22dfCallback();
- void lps28dfwInit();
- void lps28dfwCallback();
-
void h3lis331dlInit();
void h3lis331dlCallback();
+ void lis2mdlExtInit();
+ void lis2mdlExtCallback();
+
void lis2mdlInit();
void lis2mdlCallback();
void ubxgpsInit();
void ubxgpsCallback();
- void lsm6dsrxInit();
+ void lsm6dsrx0Init();
+
+ void lsm6dsrx1Init();
void vn100Init();
void vn100Callback();
- void ads131m08Init();
- void ads131m08Callback();
-
void internalAdcInit();
void internalAdcCallback();
- void staticPressure1Init();
- void staticPressure1Callback();
+ void nd015a0Init();
+ void nd015a0Callback();
+
+ void nd015a1Init();
+ void nd015a1Callback();
- void staticPressure2Init();
- void staticPressure2Callback();
+ void nd015a2Init();
+ void nd015a2Callback();
- void dplBayPressureInit();
- void dplBayPressureCallback();
+ void nd015a3Init();
+ void nd015a3Callback();
void rotatedImuInit();
void rotatedImuCallback();
bool sensorManagerInit();
- miosix::FastMutex baroCalibrationMutex;
- float baroCalibrationReference = 0;
- bool useBaroCalibrationReference = false;
-
miosix::FastMutex magCalibrationMutex;
Boardcore::SoftAndHardIronCalibration magCalibrator;
Boardcore::SixParametersCorrector magCalibration;
diff --git a/src/Main/Sensors/SensorsData.h b/src/Main/Sensors/SensorsData.h
index 628e83912..7112ad700 100644
--- a/src/Main/Sensors/SensorsData.h
+++ b/src/Main/Sensors/SensorsData.h
@@ -1,5 +1,5 @@
/* Copyright (c) 2024 Skyward Experimental Rocketry
- * Author: Davide Mor
+ * Authors: Davide Mor, Pietro Bortolus
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
@@ -22,29 +22,41 @@
#pragma once
+#include <sensors/LIS2MDL/LIS2MDLData.h>
+#include <sensors/LSM6DSRX/LSM6DSRXData.h>
#include <sensors/SensorData.h>
namespace Main
{
-struct StaticPressureData1 : Boardcore::PressureData
+struct StaticPressure0Data : public Boardcore::PressureData
{
- explicit StaticPressureData1(const Boardcore::PressureData& data)
+ explicit StaticPressure0Data(const Boardcore::PressureData& data)
: Boardcore::PressureData(data)
{
}
- StaticPressureData1() {}
+ StaticPressure0Data() {}
};
-struct StaticPressureData2 : Boardcore::PressureData
+struct StaticPressure1Data : Boardcore::PressureData
{
- explicit StaticPressureData2(const Boardcore::PressureData& data)
+ explicit StaticPressure1Data(const Boardcore::PressureData& data)
: Boardcore::PressureData(data)
{
}
- StaticPressureData2() {}
+ StaticPressure1Data() {}
+};
+
+struct StaticPressure2Data : Boardcore::PressureData
+{
+ explicit StaticPressure2Data(const Boardcore::PressureData& data)
+ : Boardcore::PressureData(data)
+ {
+ }
+
+ StaticPressure2Data() {}
};
struct DplBayPressureData : Boardcore::PressureData
@@ -57,41 +69,60 @@ struct DplBayPressureData : Boardcore::PressureData
DplBayPressureData() {}
};
+struct LSM6DSRX0Data : Boardcore::LSM6DSRXData
+{
+ explicit LSM6DSRX0Data(const Boardcore::LSM6DSRXData& data)
+ : Boardcore::LSM6DSRXData(data)
+ {
+ }
+
+ LSM6DSRX0Data() {}
+};
+
+struct LSM6DSRX1Data : Boardcore::LSM6DSRXData
+{
+ explicit LSM6DSRX1Data(const Boardcore::LSM6DSRXData& data)
+ : Boardcore::LSM6DSRXData(data)
+ {
+ }
+
+ LSM6DSRX1Data() {}
+};
+
+struct LIS2MDLExternalData : Boardcore::LIS2MDLData
+{
+ explicit LIS2MDLExternalData(const Boardcore::LIS2MDLData& data)
+ : Boardcore::LIS2MDLData(data)
+ {
+ }
+
+ LIS2MDLExternalData() {}
+};
+
struct CalibrationData
{
- uint64_t timestamp = 0;
- float gyroBiasX = 0.0f;
- float gyroBiasY = 0.0f;
- float gyroBiasZ = 0.0f;
- float magBiasX = 0.0f;
- float magBiasY = 0.0f;
- float magBiasZ = 0.0f;
- float magScaleX = 0.0f;
- float magScaleY = 0.0f;
- float magScaleZ = 0.0f;
- float staticPress1Bias = 0.0f;
- float staticPress1Scale = 0.0f;
- float staticPress2Bias = 0.0f;
- float staticPress2Scale = 0.0f;
- float dplBayPressBias = 0.0f;
- float dplBayPressScale = 0.0f;
+ uint64_t timestamp = 0;
+ float gyroBiasX = 0.0f;
+ float gyroBiasY = 0.0f;
+ float gyroBiasZ = 0.0f;
+ float magBiasX = 0.0f;
+ float magBiasY = 0.0f;
+ float magBiasZ = 0.0f;
+ float magScaleX = 0.0f;
+ float magScaleY = 0.0f;
+ float magScaleZ = 0.0f;
static std::string header()
{
return "timestamp,gyroBiasX,gyroBiasY,gyroBiasZ,magBiasX,magBiasY,"
- "magBiasZ,magScaleX,magScaleY,magScaleZ,staticPress1Bias,"
- "staticPress1Scale,staticPress2Bias,staticPress2Scale,"
- "dplBayPressBias,dplBayPressScale\n";
+ "magBiasZ,magScaleX,magScaleY,magScaleZ\n";
}
void print(std::ostream& os) const
{
os << timestamp << "," << gyroBiasX << "," << gyroBiasY << ","
<< gyroBiasZ << "," << magBiasX << "," << magBiasY << "," << magBiasZ
- << "," << magScaleX << "," << magScaleY << "," << magScaleZ << ","
- << staticPress1Bias << "," << staticPress1Scale << ","
- << staticPress2Bias << "," << staticPress2Scale << ","
- << dplBayPressBias << "," << dplBayPressScale << "\n";
+ << "," << magScaleX << "," << magScaleY << "," << magScaleZ << "\n";
}
};
diff --git a/src/Main/main-entry.cpp b/src/Main/main-entry.cpp
index 29dbf5478..94979f4ab 100644
--- a/src/Main/main-entry.cpp
+++ b/src/Main/main-entry.cpp
@@ -47,9 +47,11 @@
#include <interfaces-impl/hwmapping.h>
#include <miosix.h>
+#include <chrono>
#include <iomanip>
#include <iostream>
+using namespace std::chrono;
using namespace miosix;
using namespace Boardcore;
using namespace Main;
@@ -292,11 +294,22 @@ int main()
std::cout << "*** Init failure ***" << std::endl;
}
- std::cout << "Sensor status:" << std::endl;
+ std::string sensorConfig;
+ if (Config::Sensors::USING_DUAL_MAGNETOMETER)
+ sensorConfig = "LIS2MDL IN + LIS2MDL EXT";
+ else
+ sensorConfig = "LPS22DF + LIS2MDL IN/EXT";
+
+ std::cout << "Sensor status (config: " << sensorConfig << "):" << std::endl;
for (auto info : sensors->getSensorInfos())
{
- std::cout << "\t" << std::setw(16) << std::left << info.id << " "
- << (info.isInitialized ? "Ok" : "Error") << std::endl;
+ // The period being 0 means the sensor is disabled
+ auto statusStr = info.period == 0ns ? "Disabled"
+ : info.isInitialized ? "Ok"
+ : "Error";
+
+ std::cout << "\t" << std::setw(20) << std::left << info.id << " "
+ << statusStr << std::endl;
}
while (true)
--
GitLab