diff --git a/src/boards/Parafoil/Configs/SensorsConfig.h b/src/boards/Parafoil/Configs/SensorsConfig.h
index 49b5ed675231844096ef9a6100ebc662280e8930..d8f879027af60d1fa82443f34c13b8d25874f785 100644
--- a/src/boards/Parafoil/Configs/SensorsConfig.h
+++ b/src/boards/Parafoil/Configs/SensorsConfig.h
@@ -68,9 +68,9 @@ constexpr Boardcore::BMX160Config::AccelerometerRange IMU_BMX_ACC_FSR_ENUM =
Boardcore::BMX160Config::AccelerometerRange::G_16;
constexpr Boardcore::BMX160Config::GyroscopeRange IMU_BMX_GYRO_FSR_ENUM =
Boardcore::BMX160Config::GyroscopeRange::DEG_1000;
-constexpr unsigned int IMU_BMX_ACC_GYRO_ODR = 1600;
+constexpr unsigned int IMU_BMX_ACC_GYRO_ODR = 400;
constexpr Boardcore::BMX160Config::OutputDataRate IMU_BMX_ACC_GYRO_ODR_ENUM =
- Boardcore::BMX160Config::OutputDataRate::HZ_1600;
+ Boardcore::BMX160Config::OutputDataRate::HZ_400;
constexpr unsigned int IMU_BMX_MAG_ODR = 100;
constexpr Boardcore::BMX160Config::OutputDataRate IMU_BMX_MAG_ODR_ENUM =
Boardcore::BMX160Config::OutputDataRate::HZ_100;
@@ -134,8 +134,11 @@ constexpr unsigned int CALIBRATION_DURATION = 2000;
// Load cell calibration
constexpr unsigned int LOAD_CELL_SAMPLE_PERIOD = (1 / 80.f) * 1000;
-constexpr float LOAD_CELL1_OFFSET = -93539.8f;
-constexpr float LOAD_CELL1_SCALE = -2.251307e-5f;
+// constexpr float LOAD_CELL1_OFFSET = -20500.0f;
+// constexpr float LOAD_CELL1_SCALE = 1 / 2169.0;
+
+constexpr float LOAD_CELL1_OFFSET = -71459.4862804878;
+constexpr float LOAD_CELL1_SCALE = 0.0005147433452090161;
} // namespace SensorsConfig
diff --git a/src/boards/Parafoil/Sensors/Sensors.cpp b/src/boards/Parafoil/Sensors/Sensors.cpp
index 45da43398b275d1309bee2603b767658463f18d7..327beb326d30c30163510d9c666636167f10951f 100644
--- a/src/boards/Parafoil/Sensors/Sensors.cpp
+++ b/src/boards/Parafoil/Sensors/Sensors.cpp
@@ -57,7 +57,7 @@ bool Sensors::startModule()
// Initialize all the sensors
lis3mdlInit();
ms5803Init();
- // ubxGpsInit();
+ ubxGpsInit();
ads1118Init();
staticPressureInit();
dplPressureInit();
@@ -167,31 +167,31 @@ MS5803Data Sensors::getMS5803LastSample()
#endif
}
-// UBXGPSData Sensors::getUbxGpsLastSample()
-// {
-// miosix::PauseKernelLock lock;
-// #ifndef HILSimulation
-// return ubxGps != nullptr ? ubxGps->getLastSample() : UBXGPSData{};
-// #else
-// auto data = state.gps->getLastSample();
-// UBXGPSData ubxData;
-
-// ubxData.gpsTimestamp = data.gpsTimestamp;
-// ubxData.latitude = data.latitude;
-// ubxData.longitude = data.longitude;
-// ubxData.height = data.height;
-// ubxData.velocityNorth = data.velocityNorth;
-// ubxData.velocityEast = data.velocityEast;
-// ubxData.velocityDown = data.velocityDown;
-// ubxData.speed = data.speed;
-// ubxData.track = data.track;
-// ubxData.positionDOP = data.positionDOP;
-// ubxData.satellites = data.satellites;
-// ubxData.fix = data.fix;
-
-// return ubxData;
-// #endif
-// }
+UBXGPSData Sensors::getUbxGpsLastSample()
+{
+ miosix::PauseKernelLock lock;
+#ifndef HILSimulation
+ return ubxGps != nullptr ? ubxGps->getLastSample() : UBXGPSData{};
+#else
+ auto data = state.gps->getLastSample();
+ UBXGPSData ubxData;
+
+ ubxData.gpsTimestamp = data.gpsTimestamp;
+ ubxData.latitude = data.latitude;
+ ubxData.longitude = data.longitude;
+ ubxData.height = data.height;
+ ubxData.velocityNorth = data.velocityNorth;
+ ubxData.velocityEast = data.velocityEast;
+ ubxData.velocityDown = data.velocityDown;
+ ubxData.speed = data.speed;
+ ubxData.track = data.track;
+ ubxData.positionDOP = data.positionDOP;
+ ubxData.satellites = data.satellites;
+ ubxData.fix = data.fix;
+
+ return ubxData;
+#endif
+}
ADS1118Data Sensors::getADS1118LastSample()
{
@@ -318,7 +318,7 @@ Sensors::~Sensors()
delete bmx160;
delete lis3mdl;
delete ms5803;
- // delete ubxGps;
+ delete ubxGps;
delete ads1118;
delete staticPressure;
delete dplPressure;
@@ -347,12 +347,12 @@ void Sensors::h3lis33Init()
cs.high();
h3lis33 =
new H3LIS331DL(ModuleManager::getInstance().get<Buses>()->spi1, cs,
- H3LIS331DLDefs::OutputDataRate::ODR_50,
+ H3LIS331DLDefs::OutputDataRate::ODR_400,
H3LIS331DLDefs::BlockDataUpdate::BDU_CONTINUOS_UPDATE,
H3LIS331DLDefs::FullScaleRange::FS_100);
// Create the sensor info
- SensorInfo info("H3LIS331DL", 100,
+ SensorInfo info("H3LIS331DL", 2,
[&]()
{ Logger::getInstance().log(h3lis33->getLastSample()); });
sensorsMap.emplace(make_pair(h3lis33, info));
@@ -374,7 +374,7 @@ void Sensors::hx711Init()
[&]()
{
Logger::getInstance().log(hx711->getLastSample());
- printf("%f\n", hx711->getLastSample().load);
+ // printf("%f\n", hx711->getLastSample().load);
});
sensorsMap.emplace(make_pair(hx711, info));
LOG_INFO(logger, "Initialized loadCell1");
@@ -488,18 +488,17 @@ void Sensors::ms5803Init()
LOG_INFO(logger, "MS5803 setup done!");
}
-// void Sensors::ubxGpsInit()
-// {
-// ubxGps = new UBXGPSSerial(GPS_BAUD_RATE, GPS_SAMPLE_RATE, USART2,
-// USARTInterface::Baudrate::B9600);
+void Sensors::ubxGpsInit()
+{
+ ubxGps = new UBXGPSSerial(GPS_BAUD_RATE, GPS_SAMPLE_RATE, USART2, 9600);
-// SensorInfo info("UBXGPS", SAMPLE_PERIOD_GPS,
-// [&]()
-// { Logger::getInstance().log(ubxGps->getLastSample()); });
-// sensorsMap.emplace(make_pair(ubxGps, info));
+ SensorInfo info("UBXGPS", SAMPLE_PERIOD_GPS,
+ [&]()
+ { Logger::getInstance().log(ubxGps->getLastSample()); });
+ sensorsMap.emplace(make_pair(ubxGps, info));
-// LOG_INFO(logger, "UbloxGPS setup done!");
-// }
+ LOG_INFO(logger, "UbloxGPS setup done!");
+}
void Sensors::ads1118Init()
{
diff --git a/src/boards/Parafoil/Sensors/Sensors.h b/src/boards/Parafoil/Sensors/Sensors.h
index 640bff2e7cec2dddee5c9ce10b45bf22085fef81..157f4592447e598de64c51a7066f1f74375d8759 100644
--- a/src/boards/Parafoil/Sensors/Sensors.h
+++ b/src/boards/Parafoil/Sensors/Sensors.h
@@ -84,7 +84,7 @@ public:
Boardcore::BMX160WithCorrectionData getBMX160WithCorrectionLastSample();
Boardcore::LIS3MDLData getMagnetometerLIS3MDLLastSample();
Boardcore::MS5803Data getMS5803LastSample();
- // Boardcore::UBXGPSData getUbxGpsLastSample();
+ Boardcore::UBXGPSData getUbxGpsLastSample();
Boardcore::ADS1118Data getADS1118LastSample();
Boardcore::MPXHZ6130AData getStaticPressureLastSample();
@@ -118,7 +118,7 @@ private:
void ms5803Init();
- // void ubxGpsInit();
+ void ubxGpsInit();
void ads1118Init();
@@ -138,7 +138,7 @@ private:
Boardcore::BMX160WithCorrection* bmx160WithCorrection;
Boardcore::LIS3MDL* lis3mdl;
Boardcore::MS5803* ms5803; // barometro digitale
- // Boardcore::UBXGPSSerial* ubxGps;
+ Boardcore::UBXGPSSerial* ubxGps;
Boardcore::H3LIS331DL* h3lis33;
Boardcore::ADS1118* ads1118; // adc
diff --git a/src/entrypoints/Parafoil/parafoil-entry.cpp b/src/entrypoints/Parafoil/parafoil-entry.cpp
index a9b51a8fe2c3ded63dc00a8f93ed965ab6448c55..dc7ba5bfdc2ae69c6674ee21cb4868fcd16aa411 100644
--- a/src/entrypoints/Parafoil/parafoil-entry.cpp
+++ b/src/entrypoints/Parafoil/parafoil-entry.cpp
@@ -223,7 +223,6 @@ int main()
if (initResult)
{
EventBroker::getInstance().post(FMM_INIT_OK, TOPIC_FMM);
- miosix::ledOn();
}
else
EventBroker::getInstance().post(FMM_INIT_ERROR, TOPIC_FMM);
@@ -238,11 +237,34 @@ int main()
});
// Periodically statistics
+ bool flash = true;
while (true)
{
- Thread::sleep(1000);
+ if (initResult)
+ {
+ miosix::ledOn();
+ if (ModuleManager::getInstance()
+ .get<Sensors>()
+ ->getUbxGpsLastSample()
+ .fix == 0)
+ {
+ if (flash)
+ {
+
+ miosix::ledOff();
+ flash = false;
+ }
+ else
+ {
+ miosix::ledOn();
+ flash = true;
+ }
+ }
+ }
Logger::getInstance().log(CpuMeter::getCpuStats());
CpuMeter::resetCpuStats();
StackLogger::getInstance().log();
+
+ Thread::sleep(1000);
}
}