diff --git a/CMakeLists.txt b/CMakeLists.txt
index 8724140c8e6cc6bb6b272006ebf3e14cae583836..330cfb1625095bdf5a436ba5cdbd9f78fb82eb9b 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -33,10 +33,13 @@ project(OnBoardSoftware)
# Flight entrypoints #
#-----------------------------------------------------------------------------#
-add_executable(parafoil-entry-milano-hil src/entrypoints/Parafoil/parafoil-entry.cpp ${PARAFOIL_COMPUTER} ${HIL})
+# add_executable(parafoil-entry-milano-hil src/entrypoints/Parafoil/parafoil-entry.cpp ${PARAFOIL_COMPUTER} ${HIL_PARAFOIL})
+add_executable(parafoil-entry-milano-hil src/entrypoints/Parafoil/parafoil-entry.cpp ${PARAFOIL_COMPUTER})
target_include_directories(parafoil-entry-milano-hil PRIVATE ${OBSW_INCLUDE_DIRS})
-target_compile_definitions(parafoil-entry-milano-hil PRIVATE MILANO HILParafoil)
+target_compile_definitions(parafoil-entry-milano-hil PRIVATE MILANO USE_SERIAL_TRANSCEIVER)
+# target_compile_definitions(parafoil-entry-milano-hil PRIVATE MILANO HILParafoil USE_SERIAL_TRANSCEIVER)
sbs_target(parafoil-entry-milano-hil stm32f429zi_skyward_death_stack_x)
+# sbs_target(parafoil-entry-milano-hil stm32f429zi_stm32f4discovery)
add_executable(parafoil-entry-milano src/entrypoints/Parafoil/parafoil-entry.cpp ${PARAFOIL_COMPUTER})
target_include_directories(parafoil-entry-milano PRIVATE ${OBSW_INCLUDE_DIRS})
diff --git a/cmake/dependencies.cmake b/cmake/dependencies.cmake
index 3a26c56283f60dde9dbe7f4e134748281f234be8..164cdc5dd8e6319189a183815509225ada28ab1d 100644
--- a/cmake/dependencies.cmake
+++ b/cmake/dependencies.cmake
@@ -42,7 +42,12 @@ set(PARAFOIL_COMPUTER
set(HIL
src/hardware_in_the_loop/HIL/HILFlightPhasesManager.cpp
+ src/hardware_in_the_loop/HIL/HILTransceiver.cpp
+)
+
+set(HIL_PARAFOIL
+ ${HIL}
src/boards/Parafoil/Actuators/HILActuators.cpp
src/boards/Parafoil/Sensors/HILSensors.cpp
- src/hardware_in_the_loop/HIL/HILTransceiver.cpp
-)
\ No newline at end of file
+)
+
diff --git a/src/boards/Parafoil/Buses.h b/src/boards/Parafoil/Buses.h
index 426c2c90a6a8cad7072e122f81d0d0a452ecd900..0921d99c4e7beb07c6db59fc475def8fdd16d146 100644
--- a/src/boards/Parafoil/Buses.h
+++ b/src/boards/Parafoil/Buses.h
@@ -25,6 +25,7 @@
#include <Parafoil/ParafoilModule/ParafoilModule.h>
#include <drivers/spi/SPIBus.h>
#include <drivers/usart/USART.h>
+#include <interfaces-impl/hwmapping.h>
#include <miosix.h>
namespace Parafoil
@@ -32,9 +33,9 @@ namespace Parafoil
struct Buses : public ParafoilModule
{
- Boardcore::USART usart1;
- Boardcore::USART usart2;
- Boardcore::USART usart3;
+ Boardcore::USART hil;
+ Boardcore::USART serialRadio;
+ Boardcore::USART debug;
Boardcore::USART uart4;
Boardcore::SPIBus spi1;
@@ -43,27 +44,37 @@ struct Buses : public ParafoilModule
public:
#ifndef USE_MOCK_PERIPHERALS
Buses()
- : usart1(USART1, Boardcore::USARTInterface::Baudrate::B115200),
- usart2(USART2, Boardcore::USARTInterface::Baudrate::B115200),
- usart3(USART3, Boardcore::USARTInterface::Baudrate::B115200),
+ : hil(USART1, Boardcore::USARTInterface::Baudrate::B115200),
+ serialRadio(USART2, Boardcore::USARTInterface::Baudrate::B115200),
+ debug(USART3, Boardcore::USARTInterface::Baudrate::B115200),
uart4(UART4, Boardcore::USARTInterface::Baudrate::B115200),
spi1(SPI1), spi2(SPI2)
{
- usart1.init();
- usart2.init();
- usart3.init();
- uart4.init();
+ // miosix::interfaces::uart1::rx::getPin().mode(miosix::Mode::ALTERNATE);
+ // miosix::interfaces::uart1::tx::getPin().mode(miosix::Mode::ALTERNATE);
+ // miosix::interfaces::uart1::rx::getPin().alternateFunction(7);
+ // miosix::interfaces::uart1::tx::getPin().alternateFunction(7);
+
+ // miosix::interfaces::uart2::rx::getPin().mode(miosix::Mode::ALTERNATE);
+ // miosix::interfaces::uart2::tx::getPin().mode(miosix::Mode::ALTERNATE);
+ // miosix::interfaces::uart2::rx::getPin().alternateFunction(7);
+ // miosix::interfaces::uart2::tx::getPin().alternateFunction(7);
+
+ // hil.init();
+ serialRadio.init();
+ // debug.init();
+ // uart4.init();
}
#else
Buses()
- : usart1(USART1, Boardcore::USARTInterface::Baudrate::B115200),
- usart2(USART2, Boardcore::USARTInterface::Baudrate::B115200),
- usart3(USART3, Boardcore::USARTInterface::Baudrate::B115200),
+ : hil(USART1, Boardcore::USARTInterface::Baudrate::B115200),
+ serialRadio(USART2, Boardcore::USARTInterface::Baudrate::B115200),
+ debug(USART3, Boardcore::USARTInterface::Baudrate::B115200),
uart4(UART4, Boardcore::USARTInterface::Baudrate::B115200), spi1({}),
spi2({})
{
- usart2.init();
- usart3.init();
+ hil.init();
+ serialRadio.init();
}
#endif
diff --git a/src/boards/Parafoil/Configs/HILSimulationConfig.h b/src/boards/Parafoil/Configs/HILSimulationConfig.h
index b4d20ad9e67fb31427ab4c2ab3d72917549915d5..185205bc63a82536d58e5be698e087c3bb81ac0f 100644
--- a/src/boards/Parafoil/Configs/HILSimulationConfig.h
+++ b/src/boards/Parafoil/Configs/HILSimulationConfig.h
@@ -28,6 +28,8 @@
#include <list>
+#include "NASConfig.h"
+#include "WingConfig.h"
#include "algorithms/ADA/ADAData.h"
#include "algorithms/NAS/NAS.h"
#include "algorithms/NAS/NASState.h"
@@ -45,23 +47,22 @@ struct SensorConfig : public Boardcore::SensorInfo
const int SIM_BAUDRATE = 115200;
/** Period of simulation in milliseconds */
-const int SIMULATION_PERIOD = 100;
+const int SIMULATION_PERIOD = Parafoil::WingConfig::WING_UPDATE_PERIOD;
/** sample frequency of sensor (samples/second) */
-const int ACCEL_FREQ = 100;
-const int GYRO_FREQ = 100;
-const int MAGN_FREQ = 100;
-const int IMU_FREQ = 100;
-const int BARO_FREQ = 20;
-const int PITOT_FREQ = 20;
+const int ACCEL_FREQ = 100; // BMX
+const int GYRO_FREQ = 100; // BMX
+const int MAGN_FREQ = 100; // BMX
+const int IMU_FREQ = 100; // BMX
+const int BARO_FREQ = 100; // MS5801
const int TEMP_FREQ = 10;
-const int GPS_FREQ = 10;
+const int GPS_FREQ = 10; // GPS
/** update frequency of the Navigation System */
-const int KALM_FREQ = 10;
+// const int KALM_FREQ = Parafoil::NASConfig::UPDATE_PERIOD;
-/** update frequency of airbrakes control algorithm */
-const int CONTROL_FREQ = 10;
+/** update frequency of wing control algorithm */
+const int CONTROL_FREQ = Parafoil::WingConfig::WING_UPDATE_PERIOD;
/** min and max values in radiants of the actuator */
const float MinAlphaDegree = 0.0;
@@ -73,21 +74,19 @@ const SensorConfig gyroConfig("gyro", GYRO_FREQ);
const SensorConfig magnConfig("magn", MAGN_FREQ);
const SensorConfig imuConfig("imu", IMU_FREQ);
const SensorConfig baroConfig("baro", BARO_FREQ);
-const SensorConfig pitotConfig("pitot", PITOT_FREQ);
const SensorConfig gpsConfig("gps", GPS_FREQ);
const SensorConfig tempConfig("temp", TEMP_FREQ);
-const SensorConfig kalmConfig("kalm", KALM_FREQ);
+// const SensorConfig kalmConfig("kalm", KALM_FREQ);
/** Number of samples per sensor at each simulator iteration */
const int N_DATA_ACCEL = (ACCEL_FREQ * SIMULATION_PERIOD) / 1000; // 10
const int N_DATA_GYRO = (GYRO_FREQ * SIMULATION_PERIOD) / 1000; // 10
const int N_DATA_MAGN = (MAGN_FREQ * SIMULATION_PERIOD) / 1000; // 10
const int N_DATA_IMU = (IMU_FREQ * SIMULATION_PERIOD) / 1000; // 10
-const int N_DATA_BARO = (BARO_FREQ * SIMULATION_PERIOD) / 1000; // 2
-const int N_DATA_PITOT = (PITOT_FREQ * SIMULATION_PERIOD) / 1000; // 2
+const int N_DATA_BARO = (BARO_FREQ * SIMULATION_PERIOD) / 1000; // 10
const int N_DATA_GPS = (GPS_FREQ * SIMULATION_PERIOD) / 1000; // 1
const int N_DATA_TEMP = (TEMP_FREQ * SIMULATION_PERIOD) / 1000; // 1
-const int N_DATA_KALM = (KALM_FREQ * SIMULATION_PERIOD) / 1000; // 1
+// const int N_DATA_KALM = (KALM_FREQ * SIMULATION_PERIOD) / 1000; // 5
/**
* @brief Data structure used by the simulator in order to directly deserialize
@@ -98,6 +97,11 @@ const int N_DATA_KALM = (KALM_FREQ * SIMULATION_PERIOD) / 1000; // 1
*/
struct SimulatorData
{
+ struct Barometer
+ {
+ float measures[N_DATA_BARO];
+ } barometer;
+
struct Accelerometer
{
Boardcore::Vec3 measures[N_DATA_ACCEL];
@@ -121,27 +125,17 @@ struct SimulatorData
float num_satellites;
} gps;
- struct Barometer
- {
- float measures[N_DATA_BARO];
- } barometer;
-
- struct Pitot
- {
- float measures[N_DATA_PITOT];
- } pitot;
-
struct Temperature
{
- float measure;
+ float measure[N_DATA_TEMP];
} temperature;
- struct Kalman
- {
- float z;
- float vz;
- float vMod;
- } kalman;
+ // struct Kalman
+ // {
+ // float z;
+ // float vz;
+ // float vMod;
+ // } kalman;
struct Flags
{
@@ -203,13 +197,6 @@ struct SimulatorData
TRACE("%+.3f\n", barometer.measures[i]);
}
- void printPitot()
- {
- TRACE("pitot\n");
- for (int i = 0; i < N_DATA_PITOT; i++)
- TRACE("%+.3f\n", pitot.measures[i]);
- }
-
void printTemperature()
{
TRACE("temp\n");
@@ -217,12 +204,12 @@ struct SimulatorData
TRACE("%+.3f\n", temperature.measure);
}
- void printKalman()
- {
- TRACE("kalm\n");
- TRACE("z:%+.3f\tvz:%+.3f\tvMod:%+.3f\n", kalman.z, kalman.vz,
- kalman.vMod);
- }
+ // void printKalman()
+ // {
+ // TRACE("kalm\n");
+ // TRACE("z:%+.3f\tvz:%+.3f\tvMod:%+.3f\n", kalman.z, kalman.vz,
+ // kalman.vMod);
+ // }
void printFlags()
{
@@ -245,9 +232,8 @@ struct SimulatorData
printMagnetometer();
printGPS();
printBarometer();
- printPitot();
printTemperature();
- printKalman();
+ // printKalman();
printFlags();
}
};
diff --git a/src/boards/Parafoil/Radio/Radio.cpp b/src/boards/Parafoil/Radio/Radio.cpp
index 451fec7094c1c0dd4eba28473b1484874550e71a..5733600341d978a7916015681b25a186322392ae 100644
--- a/src/boards/Parafoil/Radio/Radio.cpp
+++ b/src/boards/Parafoil/Radio/Radio.cpp
@@ -96,7 +96,7 @@ bool Radio::startModule()
#if defined(USE_SERIAL_TRANSCEIVER)
Boardcore::SerialTransceiver* transceiver;
- transceiver = new SerialTransceiver(modules.get<Buses>()->usart2);
+ transceiver = new SerialTransceiver(modules.get<Buses>()->serialRadio);
#else
// Create the SPI bus configuration
SPIBusConfig config{};
diff --git a/src/boards/Parafoil/Sensors/HILSensors.cpp b/src/boards/Parafoil/Sensors/HILSensors.cpp
index 435aa94e532a521a01f5aeeba07a1fa95d6b6915..ae4e1d8d392d697fe3d70324982791cd91983608 100644
--- a/src/boards/Parafoil/Sensors/HILSensors.cpp
+++ b/src/boards/Parafoil/Sensors/HILSensors.cpp
@@ -45,12 +45,13 @@ namespace Parafoil
{
HILSensors::HILSensors()
- : state{
- new HILAccelerometer(N_DATA_ACCEL), new HILBarometer(N_DATA_BARO),
- new HILPitot(N_DATA_PITOT), new HILGps(N_DATA_GPS),
- new HILGyroscope(N_DATA_GYRO), new HILMagnetometer(N_DATA_MAGN),
- new HILTemperature(N_DATA_TEMP), new HILImu(N_DATA_IMU),
- new HILKalman(N_DATA_KALM)}
+ : state{new HILAccelerometer(N_DATA_ACCEL),
+ new HILBarometer(N_DATA_BARO),
+ new HILGps(N_DATA_GPS),
+ new HILGyroscope(N_DATA_GYRO),
+ new HILMagnetometer(N_DATA_MAGN),
+ new HILTemperature(N_DATA_TEMP),
+ new HILImu(N_DATA_IMU)}
{
}
@@ -67,13 +68,11 @@ HILSensors::~HILSensors()
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;
sensorManager->stop();
delete sensorManager;
@@ -83,13 +82,11 @@ bool HILSensors::startModule()
{
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.temperature, tempConfig}};
// Create the sensor manager
sensorManager = new SensorManager(sensorsMap);
@@ -194,7 +191,7 @@ SSCDANN030PAAData HILSensors::getDplPressureLastSample()
SSCDRRN015PDAData HILSensors::getPitotPressureLastSample()
{
miosix::PauseKernelLock lock;
- auto pitotData = state.pitot->getLastSample();
+ auto pitotData = pitot != nullptr ? pitot->getLastSample() : PitotData{};
SSCDRRN015PDAData data;
data.pressureTimestamp = pitotData.timestamp;
data.pressure = pitotData.deltaP;
@@ -204,7 +201,7 @@ SSCDRRN015PDAData HILSensors::getPitotPressureLastSample()
PitotData HILSensors::getPitotLastSample()
{
miosix::PauseKernelLock lock;
- return state.pitot->getLastSample();
+ return pitot != nullptr ? pitot->getLastSample() : PitotData{};
}
InternalADCData HILSensors::getInternalADCLastSample()
diff --git a/src/boards/Parafoil/Sensors/HILSensors.h b/src/boards/Parafoil/Sensors/HILSensors.h
index 1e95dfff6550d5f6a9cdb2e858f30028d4ec4d32..2e540570cfe78798105db13dbac1e8c2902bba0d 100644
--- a/src/boards/Parafoil/Sensors/HILSensors.h
+++ b/src/boards/Parafoil/Sensors/HILSensors.h
@@ -23,7 +23,6 @@
#pragma once
#include "HIL.h"
-#include "HIL_algorithms/HILMockKalman.h"
#include "HIL_sensors/HILSensors.h"
#include "Sensors.h"
@@ -73,13 +72,12 @@ private:
{
HILAccelerometer* accelerometer;
HILBarometer* barometer;
- HILPitot* pitot;
HILGps* gps;
HILGyroscope* gyro;
HILMagnetometer* magnetometer;
HILTemperature* temperature;
HILImu* imu;
- HILKalman* kalman;
+ // HILKalman* kalman;
} state;
};
diff --git a/src/entrypoints/Parafoil/parafoil-entry.cpp b/src/entrypoints/Parafoil/parafoil-entry.cpp
index fdaf3ec47bf21f638c0ffafd70bb991b56e7a634..0144dfb7291f51773bf28ca3a1acec704ec9b70c 100644
--- a/src/entrypoints/Parafoil/parafoil-entry.cpp
+++ b/src/entrypoints/Parafoil/parafoil-entry.cpp
@@ -34,6 +34,7 @@
#include <Parafoil/Configs/WingConfig.h>
#include <Parafoil/PinHandler/PinHandler.h>
#include <Parafoil/Radio/Radio.h>
+// #include <Parafoil/Radio/SerialRadio.h>
#include <Parafoil/Sensors/Sensors.h>
#include <Parafoil/StateMachines/FlightModeManager/FlightModeManager.h>
#include <Parafoil/StateMachines/NASController/NASController.h>
@@ -73,7 +74,7 @@ int main()
if (!EventBroker::getInstance().start())
{
initResult = false;
- LOG_ERR(logger, "Error initializing WingController module");
+ LOG_ERR(logger, "Error initializing the EventBroker");
}
if (!Logger::getInstance().start())
{
@@ -146,11 +147,20 @@ int main()
LOG_ERR(logger, "Error inserting the PinHandler module");
}
+// #ifndef USE_SERIAL_TRANSCEIVER
if (!modules.insert<Radio>(new Radio()))
{
initResult = false;
LOG_ERR(logger, "Error inserting the Radio module");
}
+// #else
+// if (!modules.insert<Radio>(new SerialRadio()))
+// {
+// initResult = false;
+// LOG_ERR(logger, "Error inserting the Radio module");
+// }
+// printf("SerialRadio successfully inserted\n");
+// #endif
if (!modules.insert<TMRepository>(new TMRepository()))
{
@@ -259,9 +269,15 @@ int main()
Logger::getInstance().log(f); // logs the config file
// If all is correctly set up i publish the init ok
if (initResult)
+ {
EventBroker::getInstance().post(FMM_INIT_OK, TOPIC_FMM);
+ printf("init ok\n");
+ }
else
+ {
EventBroker::getInstance().post(FMM_INIT_ERROR, TOPIC_FMM);
+ printf("init error\n");
+ }
// Log all events
EventSniffer sniffer(
diff --git a/src/hardware_in_the_loop/HIL_sensors/HILSensors.h b/src/hardware_in_the_loop/HIL_sensors/HILSensors.h
index 3a150a5b38832d66f534bf12e24122a30ccef25e..01c23a70dc94f1936ce5d31518449541b9e834c6 100644
--- a/src/hardware_in_the_loop/HIL_sensors/HILSensors.h
+++ b/src/hardware_in_the_loop/HIL_sensors/HILSensors.h
@@ -22,14 +22,13 @@
#pragma once
-#include "HIL_algorithms/HILMockKalman.h"
#include "HIL_sensors/HILAccelerometer.h"
#include "HIL_sensors/HILBarometer.h"
#include "HIL_sensors/HILGps.h"
#include "HIL_sensors/HILGyroscope.h"
#include "HIL_sensors/HILImu.h"
#include "HIL_sensors/HILMagnetometer.h"
-#include "HIL_sensors/HILPitot.h"
+// #include "HIL_sensors/HILPitot.h"
#include "HIL_sensors/HILSensor.h"
#include "HIL_sensors/HILTemperature.h"
#include "HIL_sensors/HILTimestampManagement.h"
diff --git a/src/hardware_in_the_loop/HIL_sensors/HILTemperature.h b/src/hardware_in_the_loop/HIL_sensors/HILTemperature.h
index 5940f3a545ad6cc213a461983ccb6525972ddb0b..770b42007d499664a07545d7def9a206782cdbc9 100644
--- a/src/hardware_in_the_loop/HIL_sensors/HILTemperature.h
+++ b/src/hardware_in_the_loop/HIL_sensors/HILTemperature.h
@@ -41,7 +41,7 @@ protected:
{
HILTempData tempData;
- tempData.temperature = sensorData->temperature.measure;
+ tempData.temperature = sensorData->temperature.measure[sampleCounter];
tempData.temperatureTimestamp = updateTimestamp();
return tempData;