diff --git a/.vscode/settings.json b/.vscode/settings.json
index 8ce097eb1f114745fbb65ea162008ba55ae3a1cb..c6fa1d36e4dabdf2f517992ebf0a72b3cdeed0a2 100644
--- a/.vscode/settings.json
+++ b/.vscode/settings.json
@@ -151,6 +151,7 @@
"Gpio",
"GPIOF",
"GPIOG",
+ "hwmap",
"KALM",
"kalman",
"Leds",
@@ -180,6 +181,7 @@
"Ublox",
"UBXGPS",
"usart",
+ "Vbat",
"VREF",
"Xbee"
],
diff --git a/cmake/dependencies.cmake b/cmake/dependencies.cmake
index f2dfc6220d9d5db5a19140f2843e00066b0a506a..6aa1c5f3cb9c85bb9f6ae98a04b336d73c76b218 100644
--- a/cmake/dependencies.cmake
+++ b/cmake/dependencies.cmake
@@ -120,5 +120,7 @@ set (LYRA_GS
)
set(PARAFOIL_COMPUTER
-
+ src/Parafoil/Sensors/Sensors.cpp
+ # src/Parafoil/StateMachines/NASController/NASController.cpp
+ src/Parafoil/StateMachines/FlightModeManager/FlightModeManager.cpp
)
\ No newline at end of file
diff --git a/src/Parafoil/Configs/NASConfig.h b/src/Parafoil/Configs/NASConfig.h
index d5b49789f62537b21d7b9f688977eb7849637e11..6b0ee310d9c593c692d9ee02d17e13cb6648a957 100644
--- a/src/Parafoil/Configs/NASConfig.h
+++ b/src/Parafoil/Configs/NASConfig.h
@@ -67,7 +67,7 @@ static const Boardcore::NASConfig CONFIG = {
constexpr int MAGNETOMETER_DECIMATE = 50;
// Maximum allowed acceleration to correct with GPS
-constexpr float DISABLE_GPS_ACCELERATION = 34.0f; // [m/s^2]
+constexpr float DISABLE_GPS_ACCELERATION = 34.0f; // [m/s^2].
// How much confidence (in m/s^2) to apply to the accelerometer to check if it
// is 1g
diff --git a/src/Parafoil/Configs/SensorsConfig.h b/src/Parafoil/Configs/SensorsConfig.h
index c7b8a5e536088dee32725c5e68f39eee7ba4e2cf..d5906dd378db620a635978c4aa7a38ff0b72a2a6 100644
--- a/src/Parafoil/Configs/SensorsConfig.h
+++ b/src/Parafoil/Configs/SensorsConfig.h
@@ -54,8 +54,8 @@ constexpr auto GYRO_FSR = Boardcore::BMX160Config::GyroscopeRange::DEG_1000;
constexpr auto ACC_GYRO_ODR = Boardcore::BMX160Config::OutputDataRate::HZ_200;
constexpr auto MAG_ODR = Boardcore::BMX160Config::OutputDataRate::HZ_100;
-constexpr auto AXIS_ROTATION = {Boardcore::Direction::NEGATIVE_Y,
- Boardcore::Direction::NEGATIVE_Z};
+static const Boardcore::AxisOrthoOrientation AXIS_ORIENTATION = {
+ Boardcore::Direction::NEGATIVE_Y, Boardcore::Direction::NEGATIVE_Z};
constexpr auto TEMP_DIVIDER = 1000;
constexpr auto FIFO_WATERMARK = 40;
diff --git a/src/Parafoil/Sensors/Sensors.cpp b/src/Parafoil/Sensors/Sensors.cpp
index a52727c36fd0a43d7cd5c6984e236e168ec7a984..e96aaf09c074d00bd03637e58b74a9e4b74a8515 100644
--- a/src/Parafoil/Sensors/Sensors.cpp
+++ b/src/Parafoil/Sensors/Sensors.cpp
@@ -245,7 +245,7 @@ std::vector<SensorInfo> Sensors::getSensorInfo()
infos.push_back(manager->getSensorInfo(instance.get())); \
else \
infos.push_back( \
- SensorInfo { #name, config::name::SAMPLING_RATE, nullptr, false })
+ SensorInfo{#name, config::name::SAMPLING_RATE, nullptr, false})
PUSH_SENSOR_INFO(bmx160, BMX160);
PUSH_SENSOR_INFO(bmx160WithCorrection, BMX160);
@@ -288,16 +288,17 @@ void Sensors::bmx160Init()
config.gyroscopeUnit = BMX160Config::GyroscopeMeasureUnit::RAD;
bmx160 = std::make_unique<BMX160>(getModule<Buses>()->spi1,
- hwmap::bmx160::cs::getPin(), spiConfig,
- config);
+ hwmap::bmx160::cs::getPin(), config,
+ spiConfig);
LOG_INFO(logger, "BMX160 initialized!");
}
void Sensors::bmx160WithCorrectionInit()
{
+
bmx160WithCorrection = std::make_unique<BMX160WithCorrection>(
- bmx160.get(), config::BMX160::AXIS_ROTATION);
+ bmx160.get(), config::BMX160::AXIS_ORIENTATION);
LOG_INFO(logger, "BMX160WithCorrection initialized!");
}
@@ -355,7 +356,7 @@ void Sensors::ubxGpsInit()
ubxgps = std::make_unique<UBXGPSSpi>(getModule<Buses>()->spi1,
hwmap::ubxgps::cs::getPin(), spiConfig,
- config::UBXGPS::SAMPLING_RATE);
+ config::UBXGPS::SAMPLING_RATE.value());
LOG_INFO(logger, "UBXGPS initialized!");
}
diff --git a/src/Parafoil/StateMachines/FlightModeManager/FlightModeManager.cpp b/src/Parafoil/StateMachines/FlightModeManager/FlightModeManager.cpp
index a3b781bddbb8ba0f0d775095e1374ae7d78799b0..6e6048716f00ad61f50e0aa9c859c64cf31b24a2 100644
--- a/src/Parafoil/StateMachines/FlightModeManager/FlightModeManager.cpp
+++ b/src/Parafoil/StateMachines/FlightModeManager/FlightModeManager.cpp
@@ -24,6 +24,7 @@
#include <Parafoil/BoardScheduler.h>
#include <Parafoil/Configs/FlightModeManagerConfig.h>
+#include <Parafoil/Sensors/Sensors.h>
#include <common/Events.h>
#include <drivers/timer/TimestampTimer.h>
#include <events/EventBroker.h>
diff --git a/src/Parafoil/StateMachines/FlightModeManager/FlightModeManager.h b/src/Parafoil/StateMachines/FlightModeManager/FlightModeManager.h
index 2abdf754460f2404dd92faef0d41da537ed74138..5eceae96893be7d29fe0f1f3fcdf8f6023719a52 100644
--- a/src/Parafoil/StateMachines/FlightModeManager/FlightModeManager.h
+++ b/src/Parafoil/StateMachines/FlightModeManager/FlightModeManager.h
@@ -56,6 +56,7 @@ class FlightModeManager
: public Boardcore::HSM<FlightModeManager>,
public Boardcore::InjectableWithDeps<Sensors, Actuators>
{
+public:
FlightModeManager();
~FlightModeManager();
diff --git a/src/Parafoil/StateMachines/NASController/NASController.cpp b/src/Parafoil/StateMachines/NASController/NASController.cpp
index 872f0c5b6b4f24ceb7cb8575c5b7c42b13ac9640..c4cf7f5c9b17a0c726a06207862abb91b537b705 100644
--- a/src/Parafoil/StateMachines/NASController/NASController.cpp
+++ b/src/Parafoil/StateMachines/NASController/NASController.cpp
@@ -149,6 +149,8 @@ void NASController::Calibrating(const Event& event)
}
}
+// State skipped on entry because we don't have Armed and Disarmed
+// states in the FMM
void NASController::Ready(const Event& event)
{
switch (event)
@@ -156,18 +158,6 @@ void NASController::Ready(const Event& event)
case EV_ENTRY:
{
updateState(NASControllerState::READY);
- break;
- }
-
- case NAS_CALIBRATE:
- {
- transition(&NASController::Calibrating);
- break;
- }
-
- case NAS_FORCE_START:
- case FLIGHT_ARMED:
- {
transition(&NASController::Active);
break;
}
@@ -184,16 +174,15 @@ void NASController::Active(const Event& event)
break;
}
- case FLIGHT_LANDING_DETECTED:
+ case NAS_CALIBRATE:
{
- transition(&NASController::End);
+ transition(&NASController::Calibrating);
break;
}
- case NAS_FORCE_STOP:
- case FLIGHT_DISARMED:
+ case FLIGHT_LANDING_DETECTED:
{
- transition(&NASController::Ready);
+ transition(&NASController::End);
break;
}
}
@@ -250,7 +239,7 @@ void NASController::calibrate()
ReferenceValues reference = nas.getReferenceValues();
reference.refPressure = meanBaro;
reference.refAltitude = Aeroutils::relAltitude(
- reference.refPressure, reference.mslPressure, reference.mslTemperature);
+ reference.refPressure, reference.mslPressure, reference.mslTemperature);
// Also update the reference with the GPS if we have fix
UBXGPSData gps = sensors->getUBXGPSLastSample();
diff --git a/src/Parafoil/parafoil-entry.cpp b/src/Parafoil/parafoil-entry.cpp
index acbf3e24b68309ceb124a051fc543c04be6404ff..d5c9d51b4c158da9a95be27a912f1d02c8eb9420 100644
--- a/src/Parafoil/parafoil-entry.cpp
+++ b/src/Parafoil/parafoil-entry.cpp
@@ -23,11 +23,48 @@
#include <Parafoil/BoardScheduler.h>
#include <Parafoil/Buses.h>
#include <Parafoil/Sensors/Sensors.h>
+#include <Parafoil/StateMachines/FlightModeManager/FlightModeManager.h>
+#include <common/Events.h>
+#include <common/Topics.h>
#include <diagnostic/PrintLogger.h>
+#include <diagnostic/StackLogger.h>
+#include <events/EventBroker.h>
#include <utils/DependencyManager/DependencyManager.h>
+#include <iomanip>
#include <iostream>
+/**
+ * @brief Starts a module and checks if it started correctly.
+ * Must be followed by a semicolon or a block of code.
+ * The block of code will be executed only if the module started correctly.
+ *
+ * @example START_MODULE(sensors) { miosix::ledOn(); }
+ */
+#define START_MODULE(module) \
+ std::cout << "Starting " #module << std::endl; \
+ if (!module->start()) \
+ { \
+ initResult = false; \
+ std::cerr << "*** Failed to start " #module " ***" << std::endl; \
+ } \
+ else
+
+/**
+ * @brief Starts a singleton and checks if it started correctly.
+ * Must be followed by a semicolon or a block of code.
+ * The block of code will be executed only if the singleton started correctly.
+ *
+ * @example `START_SINGLETON(Logger) { miosix::ledOn(); }`
+ */
+#define START_SINGLETON(singleton) \
+ std::cout << "Starting " #singleton << std::endl; \
+ if (!singleton::getInstance().start()) \
+ { \
+ initResult = false; \
+ std::cerr << "*** Failed to start " #singleton " ***" << std::endl; \
+ }
+
// Build type string for printing during startup
#if defined(DEBUG)
#define BUILD_TYPE "Debug"
@@ -37,13 +74,15 @@
using namespace Boardcore;
using namespace Parafoil;
+using namespace Common;
int main()
{
- std::cout << "Parafoil Entrypoint " << "(" << BUILD_TYPE << ")"
+ std::cout << "Parafoil Entrypoint "
+ << "(" << BUILD_TYPE << ")"
<< " by Skyward Experimental Rocketry" << std::endl;
- auto logger = Logging::getLogger("Mockup");
+ auto logger = Logging::getLogger("Parafoil");
DependencyManager depman{};
std::cout << "Instantiating modules" << std::endl;
@@ -55,9 +94,69 @@ int main()
auto scheduler = new BoardScheduler();
initResult &= depman.insert(scheduler);
+ // Global state machine
+ auto flightModeManager = new FlightModeManager();
+ initResult &= depman.insert(flightModeManager);
+
// Sensors
auto sensors = new Sensors();
initResult &= depman.insert(sensors);
+ // TODO: PinHandler
+
+ // TODO: Radio
+
+ // TODO: Flight algorithms
+
+ START_SINGLETON(Logger)
+ {
+ std::cout << "Logger Ok!\n"
+ << "\tLog number: "
+ << Logger::getInstance().getCurrentLogNumber() << std::endl;
+ }
+
+ START_MODULE(flightModeManager);
+ // START_MODULE(pinHandler);
+ // START_MODULE(radio) { miosix::led2On(); }
+ // START_MODULE(canHandler) { miosix::led3On(); }
+ // START_MODULE(nasController);
+ // START_MODULE(altitudeTrigger);
+ // START_MODULE(wingController);
+ // START_MODULE(actuators);
+
+ // START_MODULE(scheduler);
+
+ START_MODULE(sensors);
+
+ if (initResult)
+ {
+ EventBroker::getInstance().post(FMM_INIT_OK, TOPIC_FMM);
+ // Turn on the initialization led on the CU
+ miosix::ledOn();
+ // TODO: actuators->setStatusOk();
+ std::cout << "Payload initialization Ok!" << std::endl;
+ }
+ else
+ {
+ EventBroker::getInstance().post(FMM_INIT_ERROR, TOPIC_FMM);
+ // TODO: actuators->setStatusError();
+ std::cerr << "*** Payload initialization error ***" << std::endl;
+ }
+
+ std::cout << "Sensors status:" << std::endl;
+ auto sensorInfo = sensors->getSensorInfo();
+ for (const auto& info : sensorInfo)
+ {
+ std::cout << "\t" << std::setw(16) << std::left << info.id << " "
+ << (info.isInitialized ? "Ok" : "Error") << "\n";
+ }
+ std::cout.flush();
+
+ // Collect stack usage statistics
+ while (true)
+ {
+ StackLogger::getInstance().log();
+ Thread::sleep(1000);
+ }
return 0;
}
\ No newline at end of file