diff --git a/.vscode/settings.json b/.vscode/settings.json
index a51e6cc187fb81ae22cabe4c3fa1497faa877e3e..b150ae705286719fdbc2297eca07e486435041af 100644
--- a/.vscode/settings.json
+++ b/.vscode/settings.json
@@ -173,5 +173,6 @@
"cmake.configureSettings": {
"CMAKE_C_COMPILER_LAUNCHER": "ccache",
"CMAKE_CXX_COMPILER_LAUNCHER": "ccache"
- }
+ },
+ "cmake.configureOnOpen": false
}
\ No newline at end of file
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 53b515bae4f5ac5919d2c39f60ed58ed128308d9..f0f756925e56820cb60cb73d6c93fb46162c01f4 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -33,12 +33,8 @@ project(OnBoardSoftware)
# Flight entrypoints #
#-----------------------------------------------------------------------------#
-add_executable(parafoil-guided-milano src/entrypoints/Parafoil/parafoil-entry.cpp ${PARAFOIL_COMPUTER})
-target_include_directories(parafoil-guided-milano PRIVATE ${OBSW_INCLUDE_DIRS})
-target_compile_definitions(parafoil-guided-milano PRIVATE CLOSED_LOOP MILANO)
-sbs_target(parafoil-guided-milano stm32f429zi_skyward_death_stack_x)
+add_executable(main-entry src/entrypoints/Main/main-entry.cpp ${MAIN_COMPUTER})
+target_include_directories(main-entry PRIVATE ${OBSW_INCLUDE_DIRS})
+target_compile_definitions(main-entry PRIVATE)
+sbs_target(main-entry stm32f429zi_skyward_death_stack_x)
-add_executable(parafoil-guided-jesolo src/entrypoints/Parafoil/parafoil-entry.cpp ${PARAFOIL_COMPUTER})
-target_include_directories(parafoil-guided-jesolo PRIVATE ${OBSW_INCLUDE_DIRS})
-target_compile_definitions(parafoil-guided-jesolo PRIVATE CLOSED_LOOP JESOLO)
-sbs_target(parafoil-guided-jesolo stm32f429zi_skyward_death_stack_x)
diff --git a/cmake/dependencies.cmake b/cmake/dependencies.cmake
index 00048a404a8c0dbd879a95d16b2ae47b78bd5d68..6136b8f687f3af8584c43416d7a20c71cb2b6183 100644
--- a/cmake/dependencies.cmake
+++ b/cmake/dependencies.cmake
@@ -23,21 +23,13 @@ set(OBSW_INCLUDE_DIRS
src/boards
)
-set(PARAFOIL_COMPUTER
- src/boards/Parafoil/Actuators/Actuators.cpp
- src/boards/Parafoil/Sensors/Sensors.cpp
- src/boards/Parafoil/BoardScheduler.cpp
- src/boards/Parafoil/PinHandler/PinHandler.cpp
- src/boards/Parafoil/Radio/Radio.cpp
- src/boards/Parafoil/TMRepository/TMRepository.cpp
- src/boards/Parafoil/StateMachines/NASController/NASController.cpp
- src/boards/Parafoil/StateMachines/FlightModeManager/FlightModeManager.cpp
- src/boards/Parafoil/StateMachines/WingController/WingController.cpp
- src/boards/Parafoil/AltitudeTrigger/AltitudeTrigger.cpp
- src/boards/Parafoil/Wing/AutomaticWingAlgorithm.cpp
- src/boards/Parafoil/Wing/Guidance/EarlyManeuverGuidanceAlgorithm.cpp
- src/boards/Parafoil/Wing/Guidance/ClosedLoopGuidanceAlgorithm.cpp
- src/boards/Parafoil/Wing/FileWingAlgorithm.cpp
- src/boards/Parafoil/Wing/WingAlgorithm.cpp
- src/boards/Parafoil/WindEstimationScheme/WindEstimation.cpp
+set(MAIN_COMPUTER
+ src/boards/Main/Sensors/Sensors.cpp
+ src/boards/Main/BoardScheduler.cpp
+ src/boards/Main/PinHandler/PinHandler.cpp
+ src/boards/Main/Radio/Radio.cpp
+ src/boards/Main/TMRepository/TMRepository.cpp
+ src/boards/Main/StateMachines/NASController/NASController.cpp
+ src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.cpp
+ src/boards/Main/AltitudeTrigger/AltitudeTrigger.cpp
)
diff --git a/src/boards/Parafoil/AltitudeTrigger/AltitudeTrigger.cpp b/src/boards/Main/AltitudeTrigger/AltitudeTrigger.cpp
similarity index 94%
rename from src/boards/Parafoil/AltitudeTrigger/AltitudeTrigger.cpp
rename to src/boards/Main/AltitudeTrigger/AltitudeTrigger.cpp
index f121fd91741519c2d50a4c9df4ccb11fc04639c4..f2fce72bdd16b2c48267ba904ede18f763dd9786 100644
--- a/src/boards/Parafoil/AltitudeTrigger/AltitudeTrigger.cpp
+++ b/src/boards/Main/AltitudeTrigger/AltitudeTrigger.cpp
@@ -22,16 +22,16 @@
#include "AltitudeTrigger.h"
-#include <Parafoil/BoardScheduler.h>
-#include <Parafoil/Configs/WingConfig.h>
-#include <Parafoil/StateMachines/NASController/NASController.h>
+#include <Main/BoardScheduler.h>
+#include <Main/Configs/WingConfig.h>
+#include <Main/StateMachines/NASController/NASController.h>
#include <common/Events.h>
#include <events/EventBroker.h>
using namespace Boardcore;
using namespace Common;
-namespace Parafoil
+namespace Main
{
AltitudeTrigger::AltitudeTrigger(TaskScheduler *sched)
@@ -98,4 +98,4 @@ void AltitudeTrigger::update()
}
}
}
-} // namespace Parafoil
+} // namespace Main
diff --git a/src/boards/Parafoil/AltitudeTrigger/AltitudeTrigger.h b/src/boards/Main/AltitudeTrigger/AltitudeTrigger.h
similarity index 96%
rename from src/boards/Parafoil/AltitudeTrigger/AltitudeTrigger.h
rename to src/boards/Main/AltitudeTrigger/AltitudeTrigger.h
index 2d06e72aca449aa3a26c028204b5a3277a5a017e..0c4e22aba7944876101b9c2f7e1bf7bce1956010 100644
--- a/src/boards/Parafoil/AltitudeTrigger/AltitudeTrigger.h
+++ b/src/boards/Main/AltitudeTrigger/AltitudeTrigger.h
@@ -22,12 +22,12 @@
#pragma once
-#include <Parafoil/BoardScheduler.h>
+#include <Main/BoardScheduler.h>
#include <atomic>
#include <utils/ModuleManager/ModuleManager.hpp>
-namespace Parafoil
+namespace Main
{
class AltitudeTrigger : public Boardcore::Module
@@ -78,4 +78,4 @@ private:
Boardcore::TaskScheduler *scheduler = nullptr;
};
-} // namespace Parafoil
+} // namespace Main
diff --git a/src/boards/Parafoil/BoardScheduler.cpp b/src/boards/Main/BoardScheduler.cpp
similarity index 98%
rename from src/boards/Parafoil/BoardScheduler.cpp
rename to src/boards/Main/BoardScheduler.cpp
index 228547cd2c4246c41323c9907d3c5786a3439418..489f657705a217a48021617b8e76f570c531173c 100644
--- a/src/boards/Parafoil/BoardScheduler.cpp
+++ b/src/boards/Main/BoardScheduler.cpp
@@ -24,7 +24,7 @@
using namespace Boardcore;
-namespace Parafoil
+namespace Main
{
BoardScheduler::BoardScheduler()
@@ -63,4 +63,4 @@ bool BoardScheduler::isStarted()
return scheduler1->isRunning() && scheduler2->isRunning() &&
scheduler3->isRunning() && scheduler4->isRunning();
}
-} // namespace Parafoil
+} // namespace Main
diff --git a/src/boards/Parafoil/BoardScheduler.h b/src/boards/Main/BoardScheduler.h
similarity index 98%
rename from src/boards/Parafoil/BoardScheduler.h
rename to src/boards/Main/BoardScheduler.h
index 8ebf8db1e49efd482f656bd2b1165a470cfa7bdf..6eeaa13042fe285d355583f8a2239992292a30ea 100644
--- a/src/boards/Parafoil/BoardScheduler.h
+++ b/src/boards/Main/BoardScheduler.h
@@ -25,7 +25,7 @@
#include <utils/ModuleManager/ModuleManager.hpp>
-namespace Parafoil
+namespace Main
{
/**
* @brief Class that wraps the 4 main task schedulers of the entire OBSW.
@@ -59,4 +59,4 @@ private:
Boardcore::TaskScheduler* scheduler3;
Boardcore::TaskScheduler* scheduler4;
};
-} // namespace Parafoil
+} // namespace Main
diff --git a/src/boards/Parafoil/Buses.h b/src/boards/Main/Buses.h
similarity index 97%
rename from src/boards/Parafoil/Buses.h
rename to src/boards/Main/Buses.h
index f22be150bf52ff43a95dcdaf4c542bb589186cd8..ad63eb10eb82f78858fc97d874fec6fd032ea938 100644
--- a/src/boards/Parafoil/Buses.h
+++ b/src/boards/Main/Buses.h
@@ -26,7 +26,7 @@
#include <drivers/usart/USART.h>
#include <utils/ModuleManager/ModuleManager.hpp>
-namespace Parafoil
+namespace Main
{
class Buses : public Boardcore::Module
{
@@ -45,4 +45,4 @@ public:
{
}
};
-} // namespace Parafoil
+} // namespace Main
diff --git a/src/boards/Parafoil/Configs/FlightModeManagerConfig.h b/src/boards/Main/Configs/FlightModeManagerConfig.h
similarity index 96%
rename from src/boards/Parafoil/Configs/FlightModeManagerConfig.h
rename to src/boards/Main/Configs/FlightModeManagerConfig.h
index a9489f1ff6aaf5ea140903fd3c62c6d5db9c93a3..53adf1ee357c0088cd9b827c357f3e6f1c04cc89 100644
--- a/src/boards/Parafoil/Configs/FlightModeManagerConfig.h
+++ b/src/boards/Main/Configs/FlightModeManagerConfig.h
@@ -22,9 +22,9 @@
#pragma once
-namespace Parafoil
+namespace Main
{
constexpr unsigned int LOGGING_DELAY = 5 * 1000; // [ms]
constexpr unsigned int CONTROL_DELAY = 5 * 1000; // [ms]
-} // namespace Parafoil
+} // namespace Main
diff --git a/src/boards/Parafoil/Configs/NASConfig.h b/src/boards/Main/Configs/NASConfig.h
similarity index 98%
rename from src/boards/Parafoil/Configs/NASConfig.h
rename to src/boards/Main/Configs/NASConfig.h
index 70a3b1aff2a7551b9268b6d70f55c29ab59b5f1b..c4a5d318e4bcbc9c28c8d56be2ddbac2dc96912d 100644
--- a/src/boards/Parafoil/Configs/NASConfig.h
+++ b/src/boards/Main/Configs/NASConfig.h
@@ -26,7 +26,7 @@
#include <algorithms/ReferenceValues.h>
#include <common/ReferenceConfig.h>
-namespace Parafoil
+namespace Main
{
namespace NASConfig
@@ -64,4 +64,4 @@ static const Boardcore::NASConfig config = {
} // namespace NASConfig
-} // namespace Parafoil
+} // namespace Main
diff --git a/src/boards/Parafoil/Configs/PinHandlerConfig.h b/src/boards/Main/Configs/PinHandlerConfig.h
similarity index 96%
rename from src/boards/Parafoil/Configs/PinHandlerConfig.h
rename to src/boards/Main/Configs/PinHandlerConfig.h
index 764925ca4c4b0686b6ca3498d9f0982b52fad217..00bc240734a3a06ac461ce676a32e65b7cfdfaed 100644
--- a/src/boards/Parafoil/Configs/PinHandlerConfig.h
+++ b/src/boards/Main/Configs/PinHandlerConfig.h
@@ -24,11 +24,11 @@
#include <utils/PinObserver/PinObserver.h>
-namespace Parafoil
+namespace Main
{
constexpr unsigned int NC_DETACH_PIN_THRESHOLD = 20;
constexpr Boardcore::PinTransition NC_DETACH_PIN_TRIGGER =
Boardcore::PinTransition::FALLING_EDGE;
-} // namespace Parafoil
+} // namespace Main
diff --git a/src/boards/Parafoil/Configs/RadioConfig.h b/src/boards/Main/Configs/RadioConfig.h
similarity index 97%
rename from src/boards/Parafoil/Configs/RadioConfig.h
rename to src/boards/Main/Configs/RadioConfig.h
index 90038c9b6b8b345802d73379ca95f660e9c2a175..43abbd36654fca1e7f676d60a5de132b470b1c6b 100644
--- a/src/boards/Parafoil/Configs/RadioConfig.h
+++ b/src/boards/Main/Configs/RadioConfig.h
@@ -23,7 +23,7 @@
#include <common/Mavlink.h>
-namespace Parafoil
+namespace Main
{
namespace RadioConfig
{
@@ -48,4 +48,4 @@ constexpr bool XBEE_80KBPS_DATA_RATE = true;
constexpr int XBEE_TIMEOUT = 5000; // [ms]
} // namespace RadioConfig
-} // namespace Parafoil
+} // namespace Main
diff --git a/src/boards/Parafoil/Configs/SensorsConfig.h b/src/boards/Main/Configs/SensorsConfig.h
similarity index 91%
rename from src/boards/Parafoil/Configs/SensorsConfig.h
rename to src/boards/Main/Configs/SensorsConfig.h
index 3cd7c0277f0c5877d965d8f8751a7392728d62ec..1f23c61785f715c5b6dd7d8faddae42b5433e79e 100644
--- a/src/boards/Parafoil/Configs/SensorsConfig.h
+++ b/src/boards/Main/Configs/SensorsConfig.h
@@ -30,11 +30,11 @@
#include <sensors/LPS22DF/LPS22DF.h>
#include <sensors/calibration/AxisOrientation.h>
-namespace Parafoil
+namespace Main
{
namespace SensorsConfig
{
-constexpr unsigned int NUMBER_OF_SENSORS = 9;
+constexpr unsigned int NUMBER_OF_SENSORS = 10;
constexpr uint32_t MAG_CALIBRATION_PERIOD = 100; // [ms]
// BMX160
@@ -123,5 +123,15 @@ constexpr unsigned int LIS3MDL_SAMPLE_PERIOD = 15;
constexpr unsigned int INTERNAL_ADC_SAMPLE_PERIOD = 1000; // [ms]
constexpr unsigned int INTERNAL_TEMP_SAMPLE_PERIOD = 2000; // [ms]
+// LoadCell
+constexpr unsigned int LOAD_CELL_SAMPLE_PERIOD = 1; // [ms]
+constexpr Boardcore::ADS131M08Defs::Channel LOAD_CELL_ADC_CHANNEL =
+ Boardcore::ADS131M08Defs::Channel::CHANNEL_0;
+// TODO set calibration
+static constexpr float LOAD_CELL_P0_VOLTAGE = 1;
+static constexpr float LOAD_CELL_P0_MASS = 1;
+static constexpr float LOAD_CELL_P1_VOLTAGE = 1;
+static constexpr float LOAD_CELL_P1_MASS = 1;
+
} // namespace SensorsConfig
-} // namespace Parafoil
+} // namespace Main
diff --git a/src/boards/Parafoil/WindEstimationScheme/WindEstimationData.h b/src/boards/Main/Configs/WingConfig.h
similarity index 52%
rename from src/boards/Parafoil/WindEstimationScheme/WindEstimationData.h
rename to src/boards/Main/Configs/WingConfig.h
index 28fa7a2f2951c0c5c8a2b14a990f9605f0bee115..e29ea6d3e34ed6890aedc2c39d852f41850c1eff 100644
--- a/src/boards/Parafoil/WindEstimationScheme/WindEstimationData.h
+++ b/src/boards/Main/Configs/WingConfig.h
@@ -1,5 +1,5 @@
-/* Copyright (c) 2022 Skyward Experimental Rocketry
- * Author: Alberto Nidasio
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Authors: Angelo Prete
*
* 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,26 +22,41 @@
#pragma once
-#include <stdint.h>
+#include <utils/Constants.h>
-#include <iostream>
-#include <string>
+#include <ostream>
-namespace Parafoil
+namespace Main
{
-struct WindLogging
+namespace WingConfig
{
- long long timestamp = 0;
- float vn = 0, ve = 0;
- bool cal = 0;
+constexpr float ALTITUDE_TRIGGER_DEPLOYMENT_ALTITUDE = 300; // [meters]
+constexpr int ALTITUDE_TRIGGER_CONFIDENCE = 10; // [number of sample]
+constexpr int ALTITUDE_TRIGGER_PERIOD = 100; //[ms]
- static std::string header() { return "timestamp,vn,ve,cal\n"; }
+constexpr float MAX_SERVO_APERTURE = 0.8; //[%]
+
+struct WingConfigStruct
+{
+ int altitudeTriggerUpdatePeriod = ALTITUDE_TRIGGER_PERIOD;
+ int altitudeTriggerConfidence = ALTITUDE_TRIGGER_CONFIDENCE;
+ int altitudeTriggerDeploymentAltitude =
+ ALTITUDE_TRIGGER_DEPLOYMENT_ALTITUDE;
+
+ static std::string header()
+ {
+ return "ALTITUDE_TRIGGER_PERIOD,ALTITUDE_TRIGGER_"
+ "CONFIDENCE,ALTITUDE_TRIGGER_DEPLOYMENT_ALTITUDE\n";
+ }
void print(std::ostream& os) const
{
- os << timestamp << "," << vn << "," << ve << "," << cal << "\n ";
+ os << altitudeTriggerUpdatePeriod << "," << altitudeTriggerConfidence
+ << "," << altitudeTriggerDeploymentAltitude << "," << "\n";
}
};
-} // namespace Parafoil
+} // namespace WingConfig
+
+} // namespace Main
diff --git a/src/boards/Parafoil/PinHandler/PinHandler.cpp b/src/boards/Main/PinHandler/PinHandler.cpp
similarity index 96%
rename from src/boards/Parafoil/PinHandler/PinHandler.cpp
rename to src/boards/Main/PinHandler/PinHandler.cpp
index 6a95940ccd4ead79b9e56c4a5648522047deebef..37570dd06f6c21466e1818c5b022b15340e71cab 100644
--- a/src/boards/Parafoil/PinHandler/PinHandler.cpp
+++ b/src/boards/Main/PinHandler/PinHandler.cpp
@@ -22,7 +22,7 @@
#include "PinHandler.h"
-#include <Parafoil/Configs/PinHandlerConfig.h>
+#include <Main/Configs/PinHandlerConfig.h>
#include <common/Events.h>
#include <events/EventBroker.h>
#include <interfaces-impl/hwmapping.h>
@@ -35,7 +35,7 @@ using namespace miosix;
using namespace Boardcore;
using namespace Common;
-namespace Parafoil
+namespace Main
{
void PinHandler::onExpulsionPinTransition(PinTransition transition)
@@ -69,4 +69,4 @@ PinHandler::PinHandler() : running(false)
std::bind(&PinHandler::onExpulsionPinTransition, this, _1),
NC_DETACH_PIN_THRESHOLD);
}
-} // namespace Parafoil
+} // namespace Main
diff --git a/src/boards/Parafoil/PinHandler/PinHandler.h b/src/boards/Main/PinHandler/PinHandler.h
similarity index 97%
rename from src/boards/Parafoil/PinHandler/PinHandler.h
rename to src/boards/Main/PinHandler/PinHandler.h
index 7ba21688a3af773a3c3e1478f86c6cb06bdd0360..b54aa55f3486742d96fb5d705426244b828268ab 100644
--- a/src/boards/Parafoil/PinHandler/PinHandler.h
+++ b/src/boards/Main/PinHandler/PinHandler.h
@@ -28,7 +28,7 @@
#include <utils/ModuleManager/ModuleManager.hpp>
-namespace Parafoil
+namespace Main
{
/**
@@ -63,4 +63,4 @@ private:
std::atomic<bool> running;
};
-} // namespace Parafoil
+} // namespace Main
diff --git a/src/boards/Parafoil/Radio/Radio.cpp b/src/boards/Main/Radio/Radio.cpp
similarity index 78%
rename from src/boards/Parafoil/Radio/Radio.cpp
rename to src/boards/Main/Radio/Radio.cpp
index e4ec807c26f65ceea64af61b202e4705c61cc62b..83b29adc49b91bd381b215a6b536172c686dad5c 100644
--- a/src/boards/Parafoil/Radio/Radio.cpp
+++ b/src/boards/Main/Radio/Radio.cpp
@@ -19,16 +19,14 @@
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
* THE SOFTWARE.
*/
-#include <Parafoil/Actuators/Actuators.h>
-#include <Parafoil/AltitudeTrigger/AltitudeTrigger.h>
-#include <Parafoil/Buses.h>
-#include <Parafoil/PinHandler/PinHandler.h>
-#include <Parafoil/Radio/Radio.h>
-#include <Parafoil/Sensors/Sensors.h>
-#include <Parafoil/StateMachines/FlightModeManager/FlightModeManager.h>
-#include <Parafoil/StateMachines/NASController/NASController.h>
-#include <Parafoil/StateMachines/WingController/WingController.h>
-#include <Parafoil/TMRepository/TMRepository.h>
+#include <Main/AltitudeTrigger/AltitudeTrigger.h>
+#include <Main/Buses.h>
+#include <Main/PinHandler/PinHandler.h>
+#include <Main/Radio/Radio.h>
+#include <Main/Sensors/Sensors.h>
+#include <Main/StateMachines/FlightModeManager/FlightModeManager.h>
+#include <Main/StateMachines/NASController/NASController.h>
+#include <Main/TMRepository/TMRepository.h>
#include <common/Events.h>
#include <common/Topics.h>
#include <drivers/interrupt/external_interrupts.h>
@@ -45,11 +43,11 @@ void __attribute__((used)) EXTI10_IRQHandlerImpl()
{
ModuleManager& modules = ModuleManager::getInstance();
- if (modules.get<Parafoil::Radio>()->transceiver != nullptr)
- modules.get<Parafoil::Radio>()->transceiver->handleATTNInterrupt();
+ if (modules.get<Main::Radio>()->transceiver != nullptr)
+ modules.get<Main::Radio>()->transceiver->handleATTNInterrupt();
}
-namespace Parafoil
+namespace Main
{
Radio::Radio(TaskScheduler* sched) : scheduler(sched) {}
@@ -295,73 +293,6 @@ void Radio::handleMavlinkMessage(const mavlink_message_t& msg)
break;
}
- case MAVLINK_MSG_ID_SERVO_TM_REQUEST_TC:
- {
- ServosList servoId = static_cast<ServosList>(
- mavlink_msg_servo_tm_request_tc_get_servo_id(&msg));
-
- // Add to the queue the respose
- mavlink_message_t response =
- modules.get<TMRepository>()->packServoTm(servoId, msg.msgid,
- msg.seq);
-
- // Add the response to the queue
- mavDriver->enqueueMsg(response);
-
- // Check if the TM repo answered with a NACK. If so the function
- // must return to avoid sending a default ack
- if (response.msgid == MAVLINK_MSG_ID_NACK_TM)
- {
- return;
- }
-
- break;
- }
- case MAVLINK_MSG_ID_SET_SERVO_ANGLE_TC:
- {
- ServosList servoId = static_cast<ServosList>(
- mavlink_msg_set_servo_angle_tc_get_servo_id(&msg));
- float angle = mavlink_msg_set_servo_angle_tc_get_angle(&msg);
-
- // Move the servo, if it fails send a nack
- if (modules.get<FlightModeManager>()->getStatus().state !=
- FlightModeManagerState::TEST_MODE ||
- !modules.get<Actuators>()->setServoAngle(servoId, angle))
- {
- return sendNack(msg);
- }
-
- break;
- }
- case MAVLINK_MSG_ID_WIGGLE_SERVO_TC:
- {
- ServosList servoId = static_cast<ServosList>(
- mavlink_msg_wiggle_servo_tc_get_servo_id(&msg));
-
- // Send nack if the FMM is not in test mode
- if (modules.get<FlightModeManager>()->getStatus().state !=
- FlightModeManagerState::TEST_MODE ||
- !modules.get<Actuators>()->wiggleServo(servoId))
- {
- return sendNack(msg);
- }
-
- break;
- }
- case MAVLINK_MSG_ID_RESET_SERVO_TC:
- {
- ServosList servoId = static_cast<ServosList>(
- mavlink_msg_reset_servo_tc_get_servo_id(&msg));
-
- if (modules.get<FlightModeManager>()->getStatus().state !=
- FlightModeManagerState::TEST_MODE ||
- !modules.get<Actuators>()->setServo(servoId, 0))
- {
- return sendNack(msg);
- }
-
- break;
- }
case MAVLINK_MSG_ID_SET_REFERENCE_ALTITUDE_TC:
{
float altitude =
@@ -429,33 +360,6 @@ void Radio::handleMavlinkMessage(const mavlink_message_t& msg)
Eigen::Vector2f(latitude, longitude));
break;
}
- case MAVLINK_MSG_ID_SET_TARGET_COORDINATES_TC:
- {
- float latitude = mavlink_msg_set_coordinates_tc_get_latitude(&msg);
- float longitude =
- mavlink_msg_set_coordinates_tc_get_longitude(&msg);
- if (modules.get<FlightModeManager>()->getStatus().state !=
- FlightModeManagerState::TEST_MODE)
- {
- return sendNack(msg);
- }
- modules.get<WingController>()->setTargetPosition(
- Eigen::Vector2f(latitude, longitude));
- break;
- }
- case MAVLINK_MSG_ID_SET_ALGORITHM_TC:
- {
- u_int8_t algoID =
- mavlink_msg_set_algorithm_tc_get_algorithm_number(&msg);
- if (modules.get<FlightModeManager>()->getStatus().state !=
- FlightModeManagerState::TEST_MODE ||
- !modules.get<WingController>()->selectAlgorithm(algoID))
- {
- return sendNack(msg);
- }
-
- break;
- }
case MAVLINK_MSG_ID_RAW_EVENT_TC:
{
uint8_t topicId = mavlink_msg_raw_event_tc_get_topic_id(&msg);
@@ -567,4 +471,4 @@ void Radio::enqueueMsg(const mavlink_message_t& msg)
}
}
}
-} // namespace Parafoil
+} // namespace Main
diff --git a/src/boards/Parafoil/Radio/Radio.h b/src/boards/Main/Radio/Radio.h
similarity index 97%
rename from src/boards/Parafoil/Radio/Radio.h
rename to src/boards/Main/Radio/Radio.h
index 80049452c9de9a0ac630fccf4cd7afdc95e48139..073369ee29fcec7a1efb67e80bdbee3ba40d9933 100644
--- a/src/boards/Parafoil/Radio/Radio.h
+++ b/src/boards/Main/Radio/Radio.h
@@ -21,7 +21,7 @@
*/
#pragma once
-#include <Parafoil/Configs/RadioConfig.h>
+#include <Main/Configs/RadioConfig.h>
#include <common/Mavlink.h>
#include <radio/MavlinkDriver/MavlinkDriver.h>
#include <radio/Xbee/Xbee.h>
@@ -29,7 +29,7 @@
#include <utils/ModuleManager/ModuleManager.hpp>
-namespace Parafoil
+namespace Main
{
using MavDriver = Boardcore::MavlinkDriver<RadioConfig::RADIO_PKT_LENGTH,
RadioConfig::RADIO_OUT_QUEUE_SIZE,
@@ -101,4 +101,4 @@ private:
Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("Radio");
Boardcore::TaskScheduler* scheduler = nullptr;
};
-} // namespace Parafoil
+} // namespace Main
diff --git a/src/boards/Parafoil/Sensors/AnalogLoadCellSensor.h b/src/boards/Main/Sensors/AnalogLoadCellSensor.h
similarity index 98%
rename from src/boards/Parafoil/Sensors/AnalogLoadCellSensor.h
rename to src/boards/Main/Sensors/AnalogLoadCellSensor.h
index 1e54bccbff972e6462efefefcd67feb0140316e9..7ae23c826f4800a12f76962acfcf0b8c53b5d742 100644
--- a/src/boards/Parafoil/Sensors/AnalogLoadCellSensor.h
+++ b/src/boards/Main/Sensors/AnalogLoadCellSensor.h
@@ -29,7 +29,7 @@
// The class was copied from rig-dev, commit 5c9464e5
-namespace Parafoil
+namespace Boardcore
{
class AnalogLoadCellSensor : public Boardcore::Sensor<Boardcore::LoadCellData>
@@ -71,4 +71,4 @@ private:
float p1Mass;
};
-} // namespace Parafoil
+} // namespace Boardcore
diff --git a/src/boards/Parafoil/Sensors/Sensors.cpp b/src/boards/Main/Sensors/Sensors.cpp
similarity index 92%
rename from src/boards/Parafoil/Sensors/Sensors.cpp
rename to src/boards/Main/Sensors/Sensors.cpp
index a6a3b393bcb263ffb89925c40d220b9d56ac748b..50da9c2bc5db05d7f2f45c286d9d91ddeab2888c 100644
--- a/src/boards/Parafoil/Sensors/Sensors.cpp
+++ b/src/boards/Main/Sensors/Sensors.cpp
@@ -22,27 +22,27 @@
#include "Sensors.h"
-#include <Parafoil/Buses.h>
-#include <Parafoil/Configs/SensorsConfig.h>
+#include <Main/Buses.h>
+#include <Main/Configs/SensorsConfig.h>
#include <common/ReferenceConfig.h>
#include <drivers/interrupt/external_interrupts.h>
#include <interfaces-impl/hwmapping.h>
using namespace Boardcore;
using namespace std;
-using namespace Parafoil::SensorsConfig;
+using namespace Main::SensorsConfig;
// BMX160 Watermark interrupt
void __attribute__((used)) EXTI5_IRQHandlerImpl()
{
- Parafoil::Sensors* sensors_module =
- ModuleManager::getInstance().get<Parafoil::Sensors>();
+ Main::Sensors* sensors_module =
+ ModuleManager::getInstance().get<Main::Sensors>();
if (sensors_module->bmx160 != nullptr)
sensors_module->bmx160->IRQupdateTimestamp(
TimestampTimer::getTimestamp());
}
-namespace Parafoil
+namespace Main
{
// Getters
BMX160Data Sensors::getBMX160LastSample()
@@ -96,23 +96,11 @@ BatteryVoltageSensorData Sensors::getBatteryVoltageLastSample()
: BatteryVoltageSensorData{};
}
-// // Processed Getters
-// BatteryVoltageSensorData Sensors::getBatteryVoltageLastSample()
-// {
-// // Do not need to pause the kernel, the last sample getter is already
-// // protected
-// ADS131M08Data sample = getADS131M08LastSample();
-// BatteryVoltageSensorData data;
-
-// // Populate the data
-// data.voltageTimestamp = sample.timestamp;
-// data.channelId = static_cast<uint8_t>(BATTERY_VOLTAGE_CHANNEL);
-// data.voltage =
-// sample.getVoltage(BATTERY_VOLTAGE_CHANNEL).voltage; data.batVoltage =
-// sample.getVoltage(BATTERY_VOLTAGE_CHANNEL).voltage *
-// BATTERY_VOLTAGE_CONVERSION_FACTOR;
-// return data;
-// }
+LoadCellData Sensors::getLoadCellLastSample()
+{
+ miosix::Lock<FastMutex> l(loadCellMutex);
+ return loadCell != nullptr ? loadCell->getLastSample() : LoadCellData{};
+}
// check used task scheduler error due the magnetometer calibration task
Sensors::Sensors(TaskScheduler* sched) : scheduler(sched), sensorsCounter(0) {}
@@ -427,6 +415,27 @@ void Sensors::batteryVoltageInit()
LOG_INFO(logger, "Battery voltage sensor setup done!");
}
+void Sensors::loadCellInit()
+{
+ loadCell = new AnalogLoadCellSensor(
+ [this]()
+ {
+ auto sample = getADS131LastSample();
+ return sample.getVoltage(LOAD_CELL_ADC_CHANNEL);
+ },
+ LOAD_CELL_P0_VOLTAGE, LOAD_CELL_P0_MASS, LOAD_CELL_P1_VOLTAGE,
+ LOAD_CELL_P1_MASS);
+
+ SensorInfo info("LOAD_CELL", LOAD_CELL_SAMPLE_PERIOD,
+ bind(&Sensors::batteryVoltageCallback, this));
+ sensorMap.emplace(std::make_pair(batteryVoltage, info));
+
+ auto loadCellStatus =
+ ([&]() -> SensorInfo { return manager->getSensorInfo(loadCell); });
+ sensorsInit[sensorsCounter++] = loadCellStatus;
+
+ LOG_INFO(logger, "Load cell setup done!");
+}
std::array<SensorInfo, NUMBER_OF_SENSORS> Sensors::getSensorInfo()
{
@@ -494,5 +503,10 @@ void Sensors::batteryVoltageCallback()
BatteryVoltageSensorData lastSample = batteryVoltage->getLastSample();
Logger::getInstance().log(lastSample);
}
+void Sensors::loadCellCallback()
+{
+ LoadCellData lastSample = loadCell->getLastSample();
+ Logger::getInstance().log(lastSample);
+}
-} // namespace Parafoil
+} // namespace Main
diff --git a/src/boards/Parafoil/Sensors/Sensors.h b/src/boards/Main/Sensors/Sensors.h
similarity index 94%
rename from src/boards/Parafoil/Sensors/Sensors.h
rename to src/boards/Main/Sensors/Sensors.h
index 26316ac973458ef1051b40976efd14c54b3fbc10..919fd92b63ac718a94ddd89bdd141ea3f209716f 100644
--- a/src/boards/Parafoil/Sensors/Sensors.h
+++ b/src/boards/Main/Sensors/Sensors.h
@@ -21,7 +21,7 @@
*/
#pragma once
-#include <Parafoil/Configs/SensorsConfig.h>
+#include <Main/Configs/SensorsConfig.h>
#include <drivers/adc/InternalADC.h>
#include <sensors/ADS131M08/ADS131M08.h>
#include <sensors/BMX160/BMX160.h>
@@ -36,7 +36,9 @@
#include <utils/ModuleManager/ModuleManager.hpp>
-namespace Parafoil
+#include "AnalogLoadCellSensor.h"
+
+namespace Main
{
class Sensors : public Boardcore::Module
@@ -79,6 +81,7 @@ public:
Boardcore::ADS131M08Data getADS131LastSample();
Boardcore::InternalADCData getInternalADCLastSample();
Boardcore::BatteryVoltageSensorData getBatteryVoltageLastSample();
+ Boardcore::LoadCellData getLoadCellLastSample();
std::array<Boardcore::SensorInfo, SensorsConfig::NUMBER_OF_SENSORS>
getSensorInfo();
@@ -114,6 +117,9 @@ private:
void batteryVoltageInit();
void batteryVoltageCallback();
+ void loadCellInit();
+ void loadCellCallback();
+
// Sensors instances
Boardcore::LIS3MDL* lis3mdl = nullptr;
Boardcore::H3LIS331DL* h3lis331dl = nullptr;
@@ -125,6 +131,7 @@ private:
// Fake processed sensors
Boardcore::BMX160WithCorrection* bmx160WithCorrection = nullptr;
Boardcore::BatteryVoltageSensor* batteryVoltage = nullptr;
+ Boardcore::AnalogLoadCellSensor* loadCell = nullptr;
// Mutexes for sampling
miosix::FastMutex lis3mdlMutex;
@@ -136,6 +143,7 @@ private:
miosix::FastMutex ads131Mutex;
miosix::FastMutex internalADCMutex;
miosix::FastMutex batteryVoltageMutex;
+ miosix::FastMutex loadCellMutex;
// Magnetometer live calibration
Boardcore::SoftAndHardIronCalibration magCalibrator;
@@ -155,4 +163,4 @@ private:
SensorsConfig::NUMBER_OF_SENSORS>
sensorsInit;
};
-} // namespace Parafoil
+} // namespace Main
diff --git a/src/boards/Parafoil/StateMachines/FlightModeManager/FlightModeManager.cpp b/src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.cpp
similarity index 96%
rename from src/boards/Parafoil/StateMachines/FlightModeManager/FlightModeManager.cpp
rename to src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.cpp
index 14ba646b831eef0a6df872526de1ca2abe3734ab..2ff71c9b6be03252e91445a3b763eef7d7dbe9cd 100644
--- a/src/boards/Parafoil/StateMachines/FlightModeManager/FlightModeManager.cpp
+++ b/src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.cpp
@@ -22,8 +22,8 @@
#include "FlightModeManager.h"
-#include <Parafoil/Configs/FlightModeManagerConfig.h>
-#include <Parafoil/Sensors/Sensors.h>
+#include <Main/Configs/FlightModeManagerConfig.h>
+#include <Main/Sensors/Sensors.h>
#include <common/Events.h>
#include <drivers/timer/TimestampTimer.h>
#include <events/EventBroker.h>
@@ -32,7 +32,7 @@ using namespace miosix;
using namespace Boardcore;
using namespace Common;
-namespace Parafoil
+namespace Main
{
FlightModeManager::FlightModeManager()
: HSM(&FlightModeManager::state_on_ground)
@@ -346,20 +346,16 @@ State FlightModeManager::state_ready(const Event& event)
State FlightModeManager::state_wing_descent(const Event& event)
{
- static uint16_t controlDelayId;
-
switch (event)
{
case EV_ENTRY:
{
logStatus(FlightModeManagerState::WING_DESCENT);
- controlDelayId = EventBroker::getInstance().postDelayed(
- FLIGHT_WING_DESCENT, TOPIC_FLIGHT, CONTROL_DELAY);
+ EventBroker::getInstance().post(FLIGHT_WING_DESCENT, TOPIC_FLIGHT);
return HANDLED;
}
case EV_EXIT:
{
- EventBroker::getInstance().removeDelayed(controlDelayId);
return HANDLED;
}
case EV_EMPTY:
@@ -435,4 +431,4 @@ void FlightModeManager::logStatus(FlightModeManagerState state)
Logger::getInstance().log(status);
}
-} // namespace Parafoil
+} // namespace Main
diff --git a/src/boards/Parafoil/StateMachines/FlightModeManager/FlightModeManager.h b/src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.h
similarity index 86%
rename from src/boards/Parafoil/StateMachines/FlightModeManager/FlightModeManager.h
rename to src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.h
index 4c411a110498fc7e3b525834d8b17f61200ae85e..2d708a9106b83bf1fab1eedcfe5aeba7d27abd95 100644
--- a/src/boards/Parafoil/StateMachines/FlightModeManager/FlightModeManager.h
+++ b/src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.h
@@ -29,7 +29,7 @@
#include "FlightModeManagerData.h"
-namespace Parafoil
+namespace Main
{
class FlightModeManager : public Boardcore::HSM<FlightModeManager>,
public Boardcore::Module
@@ -42,12 +42,12 @@ public:
FlightModeManagerStatus getStatus();
/**
- * @brief Super state for when the parafoil is on ground.
+ * @brief Super state for when the main is on ground.
*/
Boardcore::State state_on_ground(const Boardcore::Event& event);
/**
- * @brief Super state for when the parafoil is on ground.
+ * @brief Super state for when the main is on ground.
*/
Boardcore::State state_init(const Boardcore::Event& event);
@@ -73,24 +73,24 @@ public:
Boardcore::State state_algos_calibration(const Boardcore::Event& event);
/**
- * @brief The parafoil will accept specific telecommands otherwise
+ * @brief The main will accept specific telecommands otherwise
* considered risky.
*/
Boardcore::State state_test_mode(const Boardcore::Event& event);
/**
- * @brief State in which the parafoil is waiting to be dropped.
+ * @brief State in which the main is waiting to be dropped.
*/
Boardcore::State state_ready(const Boardcore::Event& event);
/**
- * @brief State in which the parafoil wing is opened and starts guiding
+ * @brief State in which the main wing is opened and starts guiding
* itself
*/
Boardcore::State state_wing_descent(const Boardcore::Event& event);
/**
- * @brief The parafoil ended the flight and closes the log.
+ * @brief The main ended the flight and closes the log.
*/
Boardcore::State state_landed(const Boardcore::Event& event);
@@ -101,4 +101,4 @@ private:
Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("fmm");
};
-} // namespace Parafoil
+} // namespace Main
diff --git a/src/boards/Parafoil/StateMachines/FlightModeManager/FlightModeManagerData.h b/src/boards/Main/StateMachines/FlightModeManager/FlightModeManagerData.h
similarity index 97%
rename from src/boards/Parafoil/StateMachines/FlightModeManager/FlightModeManagerData.h
rename to src/boards/Main/StateMachines/FlightModeManager/FlightModeManagerData.h
index 53bc94bc17d39331e124f70f8a727165a9a2b427..439f9c6872d30bf69b7118d5930284dda2035c9a 100644
--- a/src/boards/Parafoil/StateMachines/FlightModeManager/FlightModeManagerData.h
+++ b/src/boards/Main/StateMachines/FlightModeManager/FlightModeManagerData.h
@@ -27,7 +27,7 @@
#include <iostream>
#include <string>
-namespace Parafoil
+namespace Main
{
enum class FlightModeManagerState : uint8_t
@@ -57,4 +57,4 @@ struct FlightModeManagerStatus
}
};
-} // namespace Parafoil
+} // namespace Main
diff --git a/src/boards/Parafoil/StateMachines/NASController/NASController.cpp b/src/boards/Main/StateMachines/NASController/NASController.cpp
similarity index 98%
rename from src/boards/Parafoil/StateMachines/NASController/NASController.cpp
rename to src/boards/Main/StateMachines/NASController/NASController.cpp
index 76e42ebf811efa81aa4dfbeb1e124dc7f0cf0a6b..ebfe602174690b2817eff080228fd2a1897bab75 100644
--- a/src/boards/Parafoil/StateMachines/NASController/NASController.cpp
+++ b/src/boards/Main/StateMachines/NASController/NASController.cpp
@@ -20,9 +20,9 @@
* THE SOFTWARE.
*/
-#include <Parafoil/Configs/NASConfig.h>
-#include <Parafoil/Sensors/Sensors.h>
-#include <Parafoil/StateMachines/NASController/NASController.h>
+#include <Main/Configs/NASConfig.h>
+#include <Main/Sensors/Sensors.h>
+#include <Main/StateMachines/NASController/NASController.h>
#include <algorithms/NAS/StateInitializer.h>
#include <common/Events.h>
#include <common/ReferenceConfig.h>
@@ -33,7 +33,7 @@ using namespace Boardcore;
using namespace Eigen;
using namespace std;
using namespace Common;
-namespace Parafoil
+namespace Main
{
NASController::NASController(TaskScheduler* sched)
: FSM(&NASController::state_idle), nas(NASConfig::config), scheduler(sched)
@@ -357,4 +357,4 @@ void NASController::logStatus(NASControllerState state)
// Log the status
Logger::getInstance().log(status);
}
-} // namespace Parafoil
+} // namespace Main
diff --git a/src/boards/Parafoil/StateMachines/NASController/NASController.h b/src/boards/Main/StateMachines/NASController/NASController.h
similarity index 98%
rename from src/boards/Parafoil/StateMachines/NASController/NASController.h
rename to src/boards/Main/StateMachines/NASController/NASController.h
index 9deae16b76fe15c0296cf8e580a357b8b3a67c57..74b7a5f86ec8a814f7de0a5008edda32a775985b 100644
--- a/src/boards/Parafoil/StateMachines/NASController/NASController.h
+++ b/src/boards/Main/StateMachines/NASController/NASController.h
@@ -31,7 +31,7 @@
#include "NASControllerData.h"
-namespace Parafoil
+namespace Main
{
class NASController : public Boardcore::FSM<NASController>,
public Boardcore::Module
@@ -96,4 +96,4 @@ private:
Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("NAS");
};
-} // namespace Parafoil
+} // namespace Main
diff --git a/src/boards/Parafoil/StateMachines/NASController/NASControllerData.h b/src/boards/Main/StateMachines/NASController/NASControllerData.h
similarity index 97%
rename from src/boards/Parafoil/StateMachines/NASController/NASControllerData.h
rename to src/boards/Main/StateMachines/NASController/NASControllerData.h
index 11059d7fdda160d190f76d2924fae87614fcd11a..559722a552f163863d269ca81edc517b9d07b5d8 100644
--- a/src/boards/Parafoil/StateMachines/NASController/NASControllerData.h
+++ b/src/boards/Main/StateMachines/NASController/NASControllerData.h
@@ -27,7 +27,7 @@
#include <iostream>
#include <string>
-namespace Parafoil
+namespace Main
{
enum class NASControllerState : uint8_t
@@ -53,4 +53,4 @@ struct NASControllerStatus
}
};
-} // namespace Parafoil
+} // namespace Main
diff --git a/src/boards/Parafoil/TMRepository/TMRepository.cpp b/src/boards/Main/TMRepository/TMRepository.cpp
similarity index 81%
rename from src/boards/Parafoil/TMRepository/TMRepository.cpp
rename to src/boards/Main/TMRepository/TMRepository.cpp
index 78787bcdbcdb5361c9fca1065eb9a8baad5c5d67..95a1bda565afcb00e0958077f2e816577b869909 100644
--- a/src/boards/Parafoil/TMRepository/TMRepository.cpp
+++ b/src/boards/Main/TMRepository/TMRepository.cpp
@@ -1,5 +1,5 @@
-/* Copyright (c) 2023 Skyward Experimental Rocketry
- * Author: Matteo Pignataro
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Author: Matteo Pignataro, Angelo Prete
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
@@ -19,17 +19,15 @@
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
* THE SOFTWARE.
*/
-#include <Parafoil/Actuators/Actuators.h>
-#include <Parafoil/BoardScheduler.h>
-#include <Parafoil/Configs/RadioConfig.h>
-#include <Parafoil/PinHandler/PinHandler.h>
-#include <Parafoil/Radio/Radio.h>
-#include <Parafoil/Sensors/Sensors.h>
-#include <Parafoil/StateMachines/FlightModeManager/FlightModeManager.h>
-#include <Parafoil/StateMachines/NASController/NASController.h>
-#include <Parafoil/StateMachines/WingController/WingController.h>
-#include <Parafoil/TMRepository/TMRepository.h>
-#include <Parafoil/WindEstimationScheme/WindEstimation.h>
+
+#include <Main/BoardScheduler.h>
+#include <Main/Configs/RadioConfig.h>
+#include <Main/PinHandler/PinHandler.h>
+#include <Main/Radio/Radio.h>
+#include <Main/Sensors/Sensors.h>
+#include <Main/StateMachines/FlightModeManager/FlightModeManager.h>
+#include <Main/StateMachines/NASController/NASController.h>
+#include <Main/TMRepository/TMRepository.h>
#include <diagnostic/CpuMeter/CpuMeter.h>
#include <drivers/timer/TimestampTimer.h>
#include <events/EventBroker.h>
@@ -40,7 +38,7 @@
using namespace miosix;
using namespace Boardcore;
-namespace Parafoil
+namespace Main
{
mavlink_message_t TMRepository::packSystemTm(SystemTMList tmId, uint8_t msgId,
uint8_t seq)
@@ -163,8 +161,8 @@ mavlink_message_t TMRepository::packSystemTm(SystemTMList tmId, uint8_t msgId,
BMX160Data imu =
modules.get<Sensors>()->getBMX160WithCorrectionLastSample();
UBXGPSData gps = modules.get<Sensors>()->getUbxGpsLastSample();
- Eigen::Vector2f wind =
- modules.get<WindEstimation>()->getWindEstimationScheme();
+ // Eigen::Vector2f wind =
+ // modules.get<WindEstimation>()->getWindEstimationScheme();
// NAS state
NASState nasState = modules.get<NASController>()->getNasState();
@@ -173,11 +171,11 @@ mavlink_message_t TMRepository::packSystemTm(SystemTMList tmId, uint8_t msgId,
modules.get<NASController>()->getStatus().state);
tm.fmm_state = static_cast<uint8_t>(
modules.get<FlightModeManager>()->getStatus().state);
- tm.wes_state = static_cast<uint8_t>(
- modules.get<WingController>()->getStatus().state);
+ // tm.wes_state = static_cast<uint8_t>(
+ // modules.get<WingController>()->getStatus().state);
- tm.wes_n = wind[0];
- tm.wes_e = wind[1];
+ // tm.wes_n = wind[0];
+ // tm.wes_e = wind[1];
tm.pressure_digi = lps22df.pressure;
@@ -219,10 +217,10 @@ mavlink_message_t TMRepository::packSystemTm(SystemTMList tmId, uint8_t msgId,
tm.nas_bias_z = nasState.bz;
// Servos
- tm.left_servo_angle = modules.get<Actuators>()->getServoAngle(
- ServosList::PARAFOIL_LEFT_SERVO);
- tm.right_servo_angle = modules.get<Actuators>()->getServoAngle(
- ServosList::PARAFOIL_RIGHT_SERVO);
+ // tm.left_servo_angle = modules.get<Actuators>()->getServoAngle(
+ // ServosList::PARAFOIL_LEFT_SERVO);
+ // tm.right_servo_angle = modules.get<Actuators>()->getServoAngle(
+ // ServosList::PARAFOIL_RIGHT_SERVO);
tm.pin_nosecone = modules.get<PinHandler>()
->getPinsData()[PinsList::NOSECONE_PIN]
@@ -265,8 +263,8 @@ mavlink_message_t TMRepository::packSystemTm(SystemTMList tmId, uint8_t msgId,
tm.timestamp = TimestampTimer::getTimestamp();
tm.abk_state = 0;
tm.ada_state = 0;
- tm.dpl_state = static_cast<uint8_t>(
- modules.get<WingController>()->getStatus().state);
+ // tm.dpl_state = static_cast<uint8_t>(
+ // modules.get<WingController>()->getStatus().state);
tm.fmm_state = static_cast<uint8_t>(
modules.get<FlightModeManager>()->getStatus().state);
tm.nas_state = static_cast<uint8_t>(
@@ -402,41 +400,41 @@ mavlink_message_t TMRepository::packSensorsTm(SensorsTMList sensorId,
RadioConfig::MAV_COMP_ID, &msg, &tm);
break;
}
- // case SensorsTMList::MAV_ADS_ID:
- // {
- // mavlink_adc_tm_t tm;
- // ADS131M08Data voltage =
- // modules.get<Sensors>()->getADS131M08LastSample();
- // tm.channel_0 =
- // voltage.getVoltage(ADS131M08Defs::Channel::CHANNEL_0).voltage;
+ case SensorsTMList::MAV_ADS_ID:
+ {
+ mavlink_adc_tm_t tm;
+ ADS131M08Data voltage =
+ modules.get<Sensors>()->getADS131LastSample();
+ tm.channel_0 =
+ voltage.getVoltage(ADS131M08Defs::Channel::CHANNEL_0).voltage;
- // tm.channel_1 =
- // voltage.getVoltage(ADS131M08Defs::Channel::CHANNEL_1).voltage;
+ tm.channel_1 =
+ voltage.getVoltage(ADS131M08Defs::Channel::CHANNEL_1).voltage;
- // tm.channel_2 =
- // voltage.getVoltage(ADS131M08Defs::Channel::CHANNEL_2).voltage;
+ tm.channel_2 =
+ voltage.getVoltage(ADS131M08Defs::Channel::CHANNEL_2).voltage;
- // tm.channel_3 =
- // voltage.getVoltage(ADS131M08Defs::Channel::CHANNEL_3).voltage;
+ tm.channel_3 =
+ voltage.getVoltage(ADS131M08Defs::Channel::CHANNEL_3).voltage;
- // tm.channel_4 =
- // voltage.getVoltage(ADS131M08Defs::Channel::CHANNEL_4).voltage;
+ tm.channel_4 =
+ voltage.getVoltage(ADS131M08Defs::Channel::CHANNEL_4).voltage;
- // tm.channel_5 =
- // voltage.getVoltage(ADS131M08Defs::Channel::CHANNEL_5).voltage;
+ tm.channel_5 =
+ voltage.getVoltage(ADS131M08Defs::Channel::CHANNEL_5).voltage;
- // tm.channel_6 =
- // voltage.getVoltage(ADS131M08Defs::Channel::CHANNEL_6).voltage;
+ tm.channel_6 =
+ voltage.getVoltage(ADS131M08Defs::Channel::CHANNEL_6).voltage;
- // tm.channel_7 =
- // voltage.getVoltage(ADS131M08Defs::Channel::CHANNEL_7).voltage;
+ tm.channel_7 =
+ voltage.getVoltage(ADS131M08Defs::Channel::CHANNEL_7).voltage;
- // strcpy(tm.sensor_name, "ADS131M08");
- // tm.timestamp = voltage.timestamp;
- // mavlink_msg_adc_tm_encode(RadioConfig::MAV_SYSTEM_ID,
- // RadioConfig::MAV_COMP_ID, &msg, &tm);
- // break;
- // }
+ strcpy(tm.sensor_name, "ADS131M08");
+ tm.timestamp = voltage.timestamp;
+ mavlink_msg_adc_tm_encode(RadioConfig::MAV_SYSTEM_ID,
+ RadioConfig::MAV_COMP_ID, &msg, &tm);
+ break;
+ }
default:
{
mavlink_nack_tm_t nack;
@@ -454,35 +452,4 @@ mavlink_message_t TMRepository::packSensorsTm(SensorsTMList sensorId,
return msg;
}
-mavlink_message_t TMRepository::packServoTm(ServosList servoId, uint8_t msgId,
- uint8_t seq)
-{
- mavlink_message_t msg;
-
- if (servoId == PARAFOIL_LEFT_SERVO || servoId == PARAFOIL_RIGHT_SERVO)
- {
- mavlink_servo_tm_t tm;
-
- tm.servo_id = servoId;
- tm.servo_position =
- ModuleManager::getInstance().get<Actuators>()->getServoAngle(
- servoId);
-
- mavlink_msg_servo_tm_encode(RadioConfig::MAV_SYSTEM_ID,
- RadioConfig::MAV_COMP_ID, &msg, &tm);
- }
- else
- {
- mavlink_nack_tm_t nack;
-
- nack.recv_msgid = msgId;
- nack.seq_ack = seq;
-
- mavlink_msg_nack_tm_encode(RadioConfig::MAV_SYSTEM_ID,
- RadioConfig::MAV_COMP_ID, &msg, &nack);
- }
-
- return msg;
-}
-
-} // namespace Parafoil
+} // namespace Main
diff --git a/src/boards/Parafoil/TMRepository/TMRepository.h b/src/boards/Main/TMRepository/TMRepository.h
similarity index 89%
rename from src/boards/Parafoil/TMRepository/TMRepository.h
rename to src/boards/Main/TMRepository/TMRepository.h
index 2657b623391e0cdef423f4bef8fc90f77315b6b8..fa3a62535a9fc82b92dd2333f742ef6b183996f8 100644
--- a/src/boards/Parafoil/TMRepository/TMRepository.h
+++ b/src/boards/Main/TMRepository/TMRepository.h
@@ -26,7 +26,7 @@
#include <utils/ModuleManager/ModuleManager.hpp>
-namespace Parafoil
+namespace Main
{
class TMRepository : public Boardcore::Module
{
@@ -41,12 +41,8 @@ public:
mavlink_message_t packSensorsTm(SensorsTMList sensorId, uint8_t msgId,
uint8_t seq);
- // Packs the telemetry about servo positions states
- mavlink_message_t packServoTm(ServosList servoId, uint8_t msgId,
- uint8_t seq);
-
private:
Boardcore::PrintLogger logger =
Boardcore::Logging::getLogger("TMRepository");
};
-} // namespace Parafoil
+} // namespace Main
diff --git a/src/boards/Parafoil/Configs/ActuatorsConfigs.h b/src/boards/Parafoil/Configs/ActuatorsConfigs.h
deleted file mode 100644
index 0968a7398a731a3266305c9240f4d5aaa1381a00..0000000000000000000000000000000000000000
--- a/src/boards/Parafoil/Configs/ActuatorsConfigs.h
+++ /dev/null
@@ -1,61 +0,0 @@
-/* Copyright (c) 2024 Skyward Experimental Rocketry
- * Author: Alberto Nidasio, Federico Lolli, Angelo Prete
- *
- * Permission is hereby granted, free of charge, to any person obtaining a copy
- * of this software and associated documentation files (the "Software"), to deal
- * in the Software without restriction, including without limitation the rights
- * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
- * copies of the Software, and to permit persons to whom the Software is
- * furnished to do so, subject to the following conditions:
- *
- * The above copyright notice and this permission notice shall be included in
- * all copies or substantial portions of the Software.
- *
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
- * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
- * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
- * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
- * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
- * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
- * THE SOFTWARE.
- */
-
-#pragma once
-
-#include <drivers/timer/PWM.h>
-#include <drivers/timer/TimerUtils.h>
-
-#include "WingConfig.h"
-
-namespace Parafoil
-{
-
-namespace ActuatorsConfigs
-{
-
-// Left servo
-static TIM_TypeDef* const SERVO_1_TIMER = TIM4;
-constexpr Boardcore::TimerUtils::Channel SERVO_1_PWM_CH =
- Boardcore::TimerUtils::Channel::CHANNEL_1;
-
-constexpr float SERVO_ROTATION = 180;
-
-constexpr float LEFT_SERVO_ROTATION = SERVO_ROTATION; // [deg]
-constexpr float LEFT_SERVO_MIN_PULSE = 500; // [us]
-constexpr float LEFT_SERVO_MAX_PULSE = 2460; // [us]
-
-// Right servo
-static TIM_TypeDef* const SERVO_2_TIMER = TIM8;
-constexpr Boardcore::TimerUtils::Channel SERVO_2_PWM_CH =
- Boardcore::TimerUtils::Channel::CHANNEL_2;
-
-constexpr float RIGHT_SERVO_ROTATION = SERVO_ROTATION; // [deg]
-constexpr float RIGHT_SERVO_MIN_PULSE = 2460; // [us]
-constexpr float RIGHT_SERVO_MAX_PULSE = 500; // [us]
-
-// Parafoil twirl
-constexpr float SERVO_TWIRL_RADIUS = WingConfig::MAX_SERVO_APERTURE; // [%]
-
-} // namespace ActuatorsConfigs
-
-} // namespace Parafoil
diff --git a/src/boards/Parafoil/Configs/WESConfig.h b/src/boards/Parafoil/Configs/WESConfig.h
deleted file mode 100644
index 5fe0b701224305661b0490fc3113b46c53c996b4..0000000000000000000000000000000000000000
--- a/src/boards/Parafoil/Configs/WESConfig.h
+++ /dev/null
@@ -1,47 +0,0 @@
-/* Copyright (c) 2023 Skyward Experimental Rocketry
- * Author: Federico Mandelli
- *
- * Permission is hereby granted, free of charge, to any person obtaining a copy
- * of this software and associated documentation files (the "Software"), to deal
- * in the Software without restriction, including without limitation the rights
- * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
- * copies of the Software, and to permit persons to whom the Software is
- * furnished to do so, subject to the following conditions:
- *
- * The above copyright notice and this permission notice shall be included in
- * all copies or substantial portions of the Software.
- *
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
- * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
- * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
- * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
- * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
- * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
- * THE SOFTWARE.
- */
-
-#pragma once
-
-#include <miosix.h>
-
-namespace Parafoil
-{
-
-namespace WESConfig
-{
-
-constexpr uint32_t WES_CALIBRATION_TIMEOUT =
- 5 * 1000; // time needed for the first loop [ms]
-
-constexpr uint32_t WES_ROTATION_PERIOD = 10 * 1000 * 1000; // [us]
-
-constexpr int WES_CALIBRATION_SAMPLE_NUMBER =
- 20; // number to sample to take in the first loop
-constexpr uint32_t WES_CALIBRATION_UPDATE_PERIOD =
- WES_CALIBRATION_TIMEOUT / WES_CALIBRATION_SAMPLE_NUMBER;
-constexpr uint32_t WES_PREDICTION_UPDATE_PERIOD =
- 100; // update period of WES[ms]
-
-} // namespace WESConfig
-
-} // namespace Parafoil
diff --git a/src/boards/Parafoil/WindEstimationScheme/WindEstimation.cpp b/src/boards/Parafoil/WindEstimationScheme/WindEstimation.cpp
deleted file mode 100644
index fc3672a91368bbb044e7a4d8573dd5643d47548f..0000000000000000000000000000000000000000
--- a/src/boards/Parafoil/WindEstimationScheme/WindEstimation.cpp
+++ /dev/null
@@ -1,217 +0,0 @@
-/* Copyright (c) 2022 Skyward Experimental Rocketry
- * Author: Federico Mandelli
- *
- * Permission is hereby granted, free of charge, to any person obtaining a copy
- * of this software and associated documentation files (the "Software"), to deal
- * in the Software without restriction, including without limitation the rights
- * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
- * copies of the Software, and to permit persons to whom the Software is
- * furnished to do so, subject to the following conditions:
- *
- * The above copyright notice and this permission notice shall be included in
- * all copies or substantial portions of the Software.
- *
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
- * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
- * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
- * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
- * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
- * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
- * THE SOFTWARE.
- */
-
-#include "WindEstimation.h"
-
-#include <Parafoil/BoardScheduler.h>
-#include <Parafoil/Configs/WESConfig.h>
-#include <Parafoil/Sensors/Sensors.h>
-#include <common/Events.h>
-#include <events/EventBroker.h>
-
-#include <utils/ModuleManager/ModuleManager.hpp>
-
-using namespace Parafoil::WESConfig;
-using namespace Boardcore;
-using namespace Common;
-using namespace miosix;
-
-namespace Parafoil
-{
-
-WindEstimation::WindEstimation(TaskScheduler* sched)
- : running(false), calRunning(false), scheduler(sched)
-{
- funv << 1.0f, 0.0f, 0.0f, 1.0f; // cppcheck-suppress constStatement
-}
-
-bool WindEstimation::start()
-{
- scheduler->addTask(
- std::bind(&WindEstimation::windEstimationSchemeCalibration, this),
- WES_CALIBRATION_UPDATE_PERIOD);
-
- // Register the WES task
- scheduler->addTask(std::bind(&WindEstimation::windEstimationScheme, this),
- WES_PREDICTION_UPDATE_PERIOD);
-
- return true;
-}
-
-WindEstimation::~WindEstimation()
-{
- stopWindEstimationSchemeCalibration();
- stopWindEstimationScheme();
-}
-
-bool WindEstimation::getStatus() { return running; }
-
-void WindEstimation::startWindEstimationSchemeCalibration()
-{
- if (!calRunning && !running)
- {
- calRunning = true;
- nSampleCal = 0;
- vn = 0;
- ve = 0;
- v2 = 0;
- LOG_INFO(logger, "WindEstimationCalibration started");
- }
-}
-
-void WindEstimation::stopWindEstimationSchemeCalibration()
-{
- calRunning = false;
- LOG_INFO(logger, "WindEstimationSchemeCalibration stopped");
- // assign value to the array only if running != false;
- if (!running)
- {
- miosix::Lock<FastMutex> l(mutex);
- wind = windCalibration;
- }
- windLogger.cal = true;
- windLogger.vn = windCalibration[0];
- windLogger.ve = windCalibration[1];
- startWindEstimationScheme();
- logStatus();
-}
-
-void WindEstimation::windEstimationSchemeCalibration()
-{
-
- if (calRunning)
- {
- auto gpsData =
- ModuleManager::getInstance().get<Sensors>()->getUbxGpsLastSample();
- if (gpsData.fix != 0)
- {
- if (nSampleCal < WES_CALIBRATION_SAMPLE_NUMBER)
- {
- float gpsN = 0, gpsE = 0;
-
- gpsN = gpsData.velocityNorth;
- gpsE = gpsData.velocityEast;
- calibrationMatrix(nSampleCal, 0) = gpsN;
- calibrationMatrix(nSampleCal, 1) = gpsE;
- calibrationV2(nSampleCal) = gpsN * gpsN + gpsE * gpsE;
-
- vn += gpsN;
- ve += gpsE;
- v2 += gpsN * gpsN + gpsE * gpsE;
- nSampleCal++;
- }
- else
- {
- vn = vn / nSampleCal;
- ve = ve / nSampleCal;
- v2 = v2 / nSampleCal;
- for (int i = 0; i < nSampleCal; i++)
- {
- calibrationMatrix(i, 0) = calibrationMatrix(i, 0) - vn;
- calibrationMatrix(i, 1) = calibrationMatrix(i, 1) - ve;
- calibrationV2(i) = 0.5f * (calibrationV2(i) - v2);
- }
- Eigen::MatrixXf calibrationMatrixT =
- calibrationMatrix.transpose();
- windCalibration =
- (calibrationMatrixT * calibrationMatrix)
- .ldlt()
- .solve(calibrationMatrixT * calibrationV2);
- stopWindEstimationSchemeCalibration();
- }
- }
- }
-}
-
-void WindEstimation::startWindEstimationScheme()
-{
- if (!running)
- {
- running = true;
- // nSample has to be reset after the calibration
- nSample = nSampleCal;
- LOG_INFO(logger, "WindEstimationScheme started");
- }
-}
-
-void WindEstimation::stopWindEstimationScheme()
-{
- if (running)
- {
- running = false;
- LOG_INFO(logger, "WindEstimationScheme ended");
- }
-}
-
-void WindEstimation::windEstimationScheme()
-{
- if (running)
- {
- auto gpsData =
- ModuleManager::getInstance().get<Sensors>()->getUbxGpsLastSample();
- if (gpsData.fix != 0)
- {
- Eigen::Vector2f phi;
- Eigen::Matrix<float, 1, 2> phiT;
- Eigen::Vector2f temp;
- float y, gpsN = 0, gpsE = 0;
- gpsN = gpsData.velocityNorth;
- gpsE = gpsData.velocityEast;
- // update avg
- nSample++;
- vn = (vn * nSample + gpsN) / (nSample + 1);
- ve = (ve * nSample + gpsE) / (nSample + 1);
- v2 = (v2 * nSample + (gpsN * gpsN + gpsE * gpsE)) / (nSample + 1);
- phi(0) = gpsN - vn;
- phi(1) = gpsE - ve;
- y = 0.5f * ((gpsN * gpsN + gpsE * gpsE) - v2);
-
- phiT = phi.transpose();
- funv =
- (funv - (funv * phi * phiT * funv) / (1 + (phiT * funv * phi)));
- temp = (0.5 * (funv + funv.transpose()) * phi) *
- (y - phiT * getWindEstimationScheme());
- {
- miosix::Lock<FastMutex> l(mutex);
- wind = wind + temp;
- windLogger.vn = wind[0];
- windLogger.ve = wind[1];
- windLogger.cal = false;
- }
- logStatus();
- }
- }
-}
-
-Eigen::Vector2f WindEstimation::getWindEstimationScheme()
-{
- miosix::Lock<FastMutex> l(mutex);
- return wind;
-}
-
-void WindEstimation::logStatus()
-{
- windLogger.timestamp = TimestampTimer::getTimestamp();
- Logger::getInstance().log(windLogger);
-}
-
-} // namespace Parafoil
diff --git a/src/boards/Parafoil/WindEstimationScheme/WindEstimation.h b/src/boards/Parafoil/WindEstimationScheme/WindEstimation.h
deleted file mode 100644
index 0b4d9f044ef01b86c93390854d46895995282421..0000000000000000000000000000000000000000
--- a/src/boards/Parafoil/WindEstimationScheme/WindEstimation.h
+++ /dev/null
@@ -1,130 +0,0 @@
-/* Copyright (c) 2022 Skyward Experimental Rocketry
- * Author: Federico Mandelli
- *
- * Permission is hereby granted, free of charge, to any person obtaining a copy
- * of this software and associated documentation files (the "Software"), to deal
- * in the Software without restriction, including without limitation the rights
- * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
- * copies of the Software, and to permit persons to whom the Software is
- * furnished to do so, subject to the following conditions:
- *
- * The above copyright notice and this permission notice shall be included in
- * all copies or substantial portions of the Software.
- *
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
- * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
- * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
- * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
- * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
- * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
- * THE SOFTWARE.
- */
-
-#pragma once
-
-#include <Parafoil/BoardScheduler.h>
-#include <Parafoil/Configs/WESConfig.h>
-#include <diagnostic/PrintLogger.h>
-#include <logger/Logger.h>
-
-#include <Eigen/Core>
-#include <atomic>
-#include <utils/ModuleManager/ModuleManager.hpp>
-
-#include "WindEstimationData.h"
-namespace Parafoil
-{
-
-/**
- * @brief This class implements the wind prediction algorithm, the first part is
- * the initial setup, and then the continuos algoritms runs;
- */
-class WindEstimation : public Boardcore::Module
-{
-public:
- /**
- * @brief Construct a new Wing Controller object
- */
- explicit WindEstimation(Boardcore::TaskScheduler* sched);
-
- /**
- * @brief Destroy the Wing Controller object.
- */
- ~WindEstimation();
-
- bool start();
-
- void startWindEstimationSchemeCalibration();
-
- void stopWindEstimationSchemeCalibration();
-
- void startWindEstimationScheme();
-
- void stopWindEstimationScheme();
-
- bool getStatus();
-
- Eigen::Vector2f getWindEstimationScheme();
-
-private:
- /**
- * @brief Creates the windCalibration matrix with the starting prediction
- * value
- */
- void windEstimationSchemeCalibration();
-
- /**
- * @brief Updates the wind matrix with the updated wind prediction values
- */
- void windEstimationScheme();
-
- /**
- * @brief Logs the prediction
- */
- void logStatus();
-
- /**
- * @brief Parameters needed for calibration
- */
- Eigen::Vector2f windCalibration{0.0f, 0.0f};
- uint8_t nSampleCal = 0;
- Eigen::Matrix<float, WESConfig::WES_CALIBRATION_SAMPLE_NUMBER, 2>
- calibrationMatrix;
- Eigen::Vector<float, WESConfig::WES_CALIBRATION_SAMPLE_NUMBER>
- calibrationV2;
- float vn = 0;
- float ve = 0;
- float v2 = 0;
-
- /**
- * @brief Parameters needed for recursive
- */
- Eigen::Vector2f wind{0.0f, 0.0f};
- uint8_t nSample = 0;
- Eigen::Matrix2f funv;
-
- /**
- * @brief Mutex
- */
- miosix::FastMutex mutex;
-
- /**
- * @brief PrintLogger
- */
- Boardcore::PrintLogger logger =
- Boardcore::Logging::getLogger("ParafoilTest");
-
- /**
- * @brief Logging struct
- */
- WindLogging windLogger{0, 0, 0};
-
- /**
- * @brief Internal running state
- */
- std::atomic<bool> running;
- std::atomic<bool> calRunning;
-
- Boardcore::TaskScheduler* scheduler = nullptr;
-};
-} // namespace Parafoil
diff --git a/src/entrypoints/Parafoil/parafoil-entry.cpp b/src/entrypoints/Main/main-entry.cpp
similarity index 86%
rename from src/entrypoints/Parafoil/parafoil-entry.cpp
rename to src/entrypoints/Main/main-entry.cpp
index 2d6f07de2d13326e4032da8d783b62e215e991f9..c1ece383ae5b02cd58f0327d4ef973918f22b593 100644
--- a/src/entrypoints/Parafoil/parafoil-entry.cpp
+++ b/src/entrypoints/Main/main-entry.cpp
@@ -20,18 +20,16 @@
* THE SOFTWARE.
*/
-#include <Parafoil/Actuators/Actuators.h>
-#include <Parafoil/AltitudeTrigger/AltitudeTrigger.h>
-#include <Parafoil/BoardScheduler.h>
-#include <Parafoil/Buses.h>
-#include <Parafoil/Configs/WingConfig.h>
-#include <Parafoil/PinHandler/PinHandler.h>
-#include <Parafoil/Radio/Radio.h>
-#include <Parafoil/Sensors/Sensors.h>
-#include <Parafoil/StateMachines/FlightModeManager/FlightModeManager.h>
-#include <Parafoil/StateMachines/NASController/NASController.h>
-#include <Parafoil/TMRepository/TMRepository.h>
-#include <Parafoil/WindEstimationScheme/WindEstimation.h>
+#include <Main/AltitudeTrigger/AltitudeTrigger.h>
+#include <Main/BoardScheduler.h>
+#include <Main/Buses.h>
+#include <Main/Configs/WingConfig.h>
+#include <Main/PinHandler/PinHandler.h>
+#include <Main/Radio/Radio.h>
+#include <Main/Sensors/Sensors.h>
+#include <Main/StateMachines/FlightModeManager/FlightModeManager.h>
+#include <Main/StateMachines/NASController/NASController.h>
+#include <Main/TMRepository/TMRepository.h>
#include <common/Events.h>
#include <common/Topics.h>
#include <diagnostic/CpuMeter/CpuMeter.h>
@@ -45,7 +43,7 @@
#include <utils/ModuleManager/ModuleManager.hpp>
using namespace Boardcore;
-using namespace Parafoil;
+using namespace Main;
using namespace Common;
int main()
@@ -75,8 +73,6 @@ int main()
Radio* radio = new Radio(scheduler->getScheduler(miosix::PRIORITY_MAX - 2));
AltitudeTrigger* altTrigger =
new AltitudeTrigger(scheduler->getScheduler(miosix::PRIORITY_MAX - 2));
- WindEstimation* windEstimation =
- new WindEstimation(scheduler->getScheduler(miosix::PRIORITY_MAX - 2));
// Components without a scheduler
TMRepository* tmRepo = new TMRepository();
@@ -132,12 +128,6 @@ int main()
LOG_ERR(logger, "Error inserting the Altitude Trigger module");
}
- if (!modules.insert<WindEstimation>(windEstimation))
- {
- initResult = false;
- LOG_ERR(logger, "Error inserting the WindEstimation module");
- }
-
if (!modules.insert<PinHandler>(pinHandler))
{
initResult = false;
@@ -191,12 +181,6 @@ int main()
LOG_ERR(logger, "Error starting the AltitudeTrigger module");
}
- if (!modules.get<WindEstimation>()->start())
- {
- initResult = false;
- LOG_ERR(logger, "Error starting the WindEstimation module");
- }
-
if (!modules.get<PinHandler>()->start())
{
initResult = false;
diff --git a/src/entrypoints/Main/test-stackv2-shield.cpp b/src/entrypoints/Main/test-stackv2-shield.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..16a35efc43a80a99a65d537dcb1054e7001ceaf5
--- /dev/null
+++ b/src/entrypoints/Main/test-stackv2-shield.cpp
@@ -0,0 +1,280 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Federico Mandelli
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include <drivers/spi/SPIDriver.h>
+#include <miosix.h>
+#include <sensors/ADS131M08/ADS131M08.h>
+#include <sensors/H3LIS331DL/H3LIS331DL.h>
+#include <sensors/LPS22DF/LPS22DF.h>
+#include <sensors/SensorManager.h>
+#include <sensors/UBXGPS/UBXGPSSpi.h>
+#include <utils/Debug.h>
+
+using namespace Boardcore;
+using namespace miosix;
+
+GpioPin clk(GPIOA_BASE, 5);
+GpioPin miso(GPIOA_BASE, 6);
+GpioPin mosi(GPIOA_BASE, 7);
+
+SPIBus spiBus(SPI1);
+
+void initSPI();
+
+int testGPS(GpioPin cs);
+int testLPS(GpioPin cs);
+int testH3LIS(GpioPin cs);
+int testADS(GpioPin cs);
+
+int main()
+{
+
+ GpioPin lps22CS(GPIOB_BASE, 1);
+ GpioPin lps22DevBoardCS(GPIOG_BASE, 7);
+ GpioPin h3lisCS(GPIOC_BASE, 6);
+ GpioPin ads131CS(GPIOC_BASE, 2);
+ GpioPin ubxgpsCS(GPIOD_BASE, 7);
+
+ // disable cs
+ lps22CS.mode(miosix::Mode::OUTPUT);
+ lps22CS.high();
+
+ lps22DevBoardCS.mode(miosix::Mode::OUTPUT);
+ lps22DevBoardCS.high();
+
+ lps22CS.mode(miosix::Mode::OUTPUT);
+ lps22CS.high();
+
+ h3lisCS.mode(miosix::Mode::OUTPUT);
+ h3lisCS.high();
+
+ ads131CS.mode(miosix::Mode::OUTPUT);
+ ads131CS.high();
+
+ ubxgpsCS.mode(miosix::Mode::OUTPUT);
+ ubxgpsCS.high();
+
+ initSPI();
+
+ // testLPS(lps22CS);
+
+ // testLPS(lps22DevBoardCS);
+
+ // testH3LIS(h3lisCS);
+
+ testADS(ads131CS);
+
+ // testGPS(ubxgpsCS);
+}
+void initSPI()
+{
+ // Setup gpio pins
+ clk.mode(Mode::ALTERNATE);
+ clk.alternateFunction(5);
+ miso.mode(Mode::ALTERNATE);
+ miso.alternateFunction(5);
+ mosi.mode(Mode::ALTERNATE);
+ mosi.alternateFunction(5);
+}
+
+int testGPS(GpioPin cs)
+{
+ TRACE("Initializing UBXGPSSpi...\n");
+ UBXGPSSpi gps{spiBus, cs};
+
+ while (!gps.init())
+ {
+ TRACE("Init failed! (code: %d)\n", gps.getLastError());
+
+ TRACE("Retrying in 10 seconds...\n");
+ miosix::Thread::sleep(10000);
+ }
+
+ while (true)
+ {
+ gps.sample();
+ GPSData sample __attribute__((unused)) = gps.getLastSample();
+
+ TRACE(
+ "timestamp: %4.3f, lat: %f, lon: %f, height: %4.1f, velN: %3.2f, "
+ "velE: %3.2f, velD: %3.2f, speed: %3.2f, track %3.1f, pDOP: %f, "
+ "nsat: %2d, fix: %2d\n",
+ (float)sample.gpsTimestamp / 1000000, sample.latitude,
+ sample.longitude, sample.height, sample.velocityNorth,
+ sample.velocityEast, sample.velocityDown, sample.speed,
+ sample.track, sample.positionDOP, sample.satellites, sample.fix);
+
+ Thread::sleep(1000);
+ }
+ return 0;
+}
+
+int testLPS(GpioPin cs)
+{
+ LPS22DF::Config config;
+ config.odr = Boardcore::LPS22DF::ODR_100;
+ config.avg = Boardcore::LPS22DF::AVG_4;
+
+ auto lpsConfig = LPS22DF::getDefaultSPIConfig();
+
+ lpsConfig.clockDivider = SPI::ClockDivider::DIV_64;
+
+ LPS22DF sensor(spiBus, cs, lpsConfig, config);
+
+ printf("Starting...\n");
+
+ if (!sensor.init())
+ {
+ printf("Init failed\n");
+ miosix::Thread::sleep(10000);
+ return -1;
+ }
+
+ if (!sensor.selfTest())
+ {
+ printf("Error: selfTest() returned false!\n");
+ miosix::Thread::sleep(10000);
+ return -1;
+ }
+
+ // printf("Trying one shot mode for 10 seconds\n");
+ // for (int i = 0; i < 10 * 10; i++)
+ // {
+ // sensor.sample();
+ // LPS22DFData data = sensor.getLastSample();
+
+ // printf("%.2f C | %.2f Pa\n", data.temperature, data.pressure);
+
+ // miosix::Thread::sleep(10000);
+ // }
+
+ printf("Now setting 10Hz continuous mode\n");
+ sensor.setOutputDataRate(LPS22DF::ODR_10);
+ int i = 10;
+ while (i--)
+ {
+ sensor.sample();
+ LPS22DFData data = sensor.getLastSample();
+
+ printf("%.2f C | %.2f Pa\n", data.temperature, data.pressure);
+
+ miosix::Thread::sleep(100);
+ }
+ return 0;
+}
+
+int testH3LIS(GpioPin cs)
+{
+
+ H3LIS331DL sensor(spiBus, cs, H3LIS331DLDefs::OutputDataRate::ODR_50,
+ H3LIS331DLDefs::BlockDataUpdate::BDU_CONTINUOS_UPDATE,
+ H3LIS331DLDefs::FullScaleRange::FS_100);
+
+ if (!sensor.init())
+ {
+ printf("Failed init!\n");
+ if (sensor.getLastError() == SensorErrors::INVALID_WHOAMI)
+ {
+ printf("Invalid WHOAMI\n");
+ }
+ Thread::sleep(5000);
+ return -1;
+ }
+
+ H3LIS331DLData data;
+
+ // Print out the CSV header
+ printf(H3LIS331DLData::header().c_str());
+ // sample some data from the sensor
+ for (int i = 0; i < 30; i++)
+ {
+ // sensor intitialized, should return error if no new data exist
+ sensor.sample();
+
+ // if (sensor.getLastError() == SensorErrors::NO_NEW_DATA)
+ // {
+ // printf("\nWarning: no new data to be read \n");
+ // }
+
+ data = sensor.getLastSample();
+
+ printf("%llu,%f,%f,%f\n", data.accelerationTimestamp,
+ data.accelerationX, data.accelerationY, data.accelerationZ);
+
+ Thread::sleep(100);
+ }
+
+ return 0;
+}
+
+int testADS(GpioPin cs)
+{
+ // ADC configuration
+ ADS131M08::Config config{
+ .oversamplingRatio = ADS131M08Defs::OversamplingRatio::OSR_8192,
+ .globalChopModeEnabled = true,
+ };
+
+ // Device initialization
+
+ ADS131M08 ads131(spiBus, cs, {}, config);
+
+ ads131.init();
+
+ printf("Now performing self test...\n");
+ if (ads131.selfTest())
+ {
+ printf("Self test succeeded\n");
+ }
+ else
+ {
+ printf("Self test failed!\n");
+ return -1;
+ }
+
+ ads131.calibrateOffset(ADS131M08Defs::Channel::CHANNEL_0);
+ // ads131.calibrateOffset(ADS131M08Defs::Channel::CHANNEL_1);
+ // ads131.calibrateOffset(ADS131M08Defs::Channel::CHANNEL_2);
+ // ads131.calibrateOffset(ADS131M08Defs::Channel::CHANNEL_3);
+ // ads131.calibrateOffset(ADS131M08Defs::Channel::CHANNEL_4);
+ // ads131.calibrateOffset(ADS131M08Defs::Channel::CHANNEL_5);
+ // ads131.calibrateOffset(ADS131M08Defs::Channel::CHANNEL_6);
+ // ads131.calibrateOffset(ADS131M08Defs::Channel::CHANNEL_7);
+
+ int i = 10;
+ while (true)
+ {
+ ads131.sample();
+
+ ADS131M08Data data = ads131.getLastSample();
+
+ printf(
+ "% 2.8f\n",
+ // \t% 2.8f\t% 2.8f\t% 2.8f\t% 2.8f\t% 2.8f\t% 2.8f\t% 2.8f\n",
+ data.voltage[0]/*, data.voltage[1], data.voltage[2], data.voltage[3],
+ data.voltage[4], data.voltage[5], data.voltage[6], data.voltage[7]);*/
+ );
+ delayMs(200);
+ }
+
+ return 0;
+}