From 5777f968b7770edf668c1f0894a143f00c8ff265 Mon Sep 17 00:00:00 2001
From: Davide Mor <davide.mor@skywarder.eu>
Date: Tue, 23 Jul 2024 19:29:47 +0200
Subject: [PATCH] [Main] Added ADAController
---
cmake/dependencies.cmake | 2 +-
src/boards/Main/Actuators/Actuators.cpp | 22 +-
src/boards/Main/Actuators/Actuators.h | 9 +-
src/boards/Main/CanHandler/CanHandler.cpp | 40 +-
src/boards/Main/CanHandler/CanHandler.h | 5 +
src/boards/Main/Configs/ADAConfig.h | 58 +++
src/boards/Main/Configs/ActuatorsConfig.h | 12 +-
src/boards/Main/Configs/CanHandlerConfig.h | 2 +-
src/boards/Main/Configs/RadioConfig.h | 4 +-
src/boards/Main/Configs/SensorsConfig.h | 16 +-
src/boards/Main/Radio/Radio.cpp | 16 +-
src/boards/Main/Sensors/Sensors.cpp | 44 +-
src/boards/Main/Sensors/Sensors.h | 25 +-
.../ADAController/ADAController.cpp | 416 ++++++++++++++++++
.../ADAController/ADAController.h | 78 ++++
.../ADAController/ADAControllerData.h | 77 ++++
.../FlightModeManager/FlightModeManager.cpp | 146 +++---
.../FlightModeManager/FlightModeManager.h | 8 +-
.../FlightModeManager/FlightModeManagerData.h | 38 +-
src/entrypoints/Main/main-entry.cpp | 25 +-
20 files changed, 840 insertions(+), 203 deletions(-)
create mode 100644 src/boards/Main/Configs/ADAConfig.h
create mode 100644 src/boards/Main/StateMachines/ADAController/ADAController.cpp
create mode 100644 src/boards/Main/StateMachines/ADAController/ADAController.h
create mode 100644 src/boards/Main/StateMachines/ADAController/ADAControllerData.h
diff --git a/cmake/dependencies.cmake b/cmake/dependencies.cmake
index 643a21e53..79c336a49 100644
--- a/cmake/dependencies.cmake
+++ b/cmake/dependencies.cmake
@@ -32,7 +32,7 @@ set(MAIN_COMPUTER
src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.cpp
src/boards/Main/Actuators/Actuators.cpp
# src/boards/Main/Sensors/RotatedIMU/RotatedIMU.cpp
- # src/boards/Main/StateMachines/ADAController/ADAController.cpp
+ src/boards/Main/StateMachines/ADAController/ADAController.cpp
src/boards/Main/PinHandler/PinHandler.cpp
# src/boards/Main/AltitudeTrigger/AltitudeTrigger.cpp
# src/boards/Main/StateMachines/ABKController/ABKController.cpp
diff --git a/src/boards/Main/Actuators/Actuators.cpp b/src/boards/Main/Actuators/Actuators.cpp
index 8da49bd11..edf8182e4 100644
--- a/src/boards/Main/Actuators/Actuators.cpp
+++ b/src/boards/Main/Actuators/Actuators.cpp
@@ -22,6 +22,7 @@
#include "Actuators.h"
+#include <Main/CanHandler/CanHandler.h>
#include <Main/Configs/ActuatorsConfig.h>
#include <interfaces-impl/hwmapping.h>
@@ -64,7 +65,7 @@ bool Actuators::start()
buzzerOff();
uint8_t result = scheduler.addTask([this]() { updateBuzzer(); },
- Config::Actuators::BUZZER_UPDATE_PERIOD,
+ Config::Actuators::BUZZER_UPDATE_RATE,
TaskScheduler::Policy::RECOVER);
if (result == 0)
@@ -74,7 +75,7 @@ bool Actuators::start()
}
result = scheduler.addTask([this]() { updateStatus(); },
- Config::Actuators::STATUS_UPDATE_PERIOD,
+ Config::Actuators::STATUS_UPDATE_RATE,
TaskScheduler::Policy::RECOVER);
if (result == 0)
@@ -119,6 +120,11 @@ bool Actuators::wiggleServo(ServosList servo)
}
}
+void Actuators::wiggleCanServo(ServosList servo)
+{
+ getModule<CanHandler>()->sendServoOpenCommand(servo, 1.0, 1000);
+}
+
float Actuators::getServoPosition(ServosList servo)
{
Lock<FastMutex> lock{servosMutex};
@@ -147,24 +153,24 @@ void Actuators::setStatusOff() { statusOverflow = 0; }
void Actuators::setStatusOk()
{
- statusOverflow = Config::Actuators::STATUS_OK_PERIOD;
+ statusOverflow = Config::Actuators::STATUS_OK_RATE;
}
void Actuators::setStatusErr()
{
- statusOverflow = Config::Actuators::STATUS_ERR_PERIOD;
+ statusOverflow = Config::Actuators::STATUS_ERR_RATE;
}
void Actuators::setBuzzerOff() { buzzerOverflow = 0; }
void Actuators::setBuzzerArmed()
{
- buzzerOverflow = Config::Actuators::BUZZER_ARM_PERIOD;
+ buzzerOverflow = Config::Actuators::BUZZER_ARM_RATE;
}
void Actuators::setBuzzerLand()
{
- buzzerOverflow = Config::Actuators::BUZZER_LAND_PERIOD;
+ buzzerOverflow = Config::Actuators::BUZZER_LAND_RATE;
}
void Actuators::setCanServoOpen(ServosList servo, bool open)
@@ -184,10 +190,14 @@ void Actuators::camOn() { gpios::camEnable::high(); }
void Actuators::camOff() { gpios::camEnable::low(); }
+bool Actuators::getCamState() { return gpios::camEnable::value() != 0; }
+
void Actuators::cutterOn() { gpios::mainDeploy::high(); }
void Actuators::cutterOff() { gpios::mainDeploy::low(); }
+bool Actuators::getCutterState() { return gpios::mainDeploy::value() != 0; }
+
void Actuators::statusOn() { gpios::statusLed::high(); }
void Actuators::statusOff() { gpios::statusLed::low(); }
diff --git a/src/boards/Main/Actuators/Actuators.h b/src/boards/Main/Actuators/Actuators.h
index 041f443d2..7b49d8b72 100644
--- a/src/boards/Main/Actuators/Actuators.h
+++ b/src/boards/Main/Actuators/Actuators.h
@@ -33,7 +33,10 @@
namespace Main
{
-class Actuators : public Boardcore::InjectableWithDeps<BoardScheduler>
+class CanHandler;
+
+class Actuators
+ : public Boardcore::InjectableWithDeps<BoardScheduler, CanHandler>
{
public:
Actuators();
@@ -48,13 +51,16 @@ public:
bool wiggleServo(ServosList servo);
float getServoPosition(ServosList servo);
+ void wiggleCanServo(ServosList servo);
bool isCanServoOpen(ServosList servo);
void camOn();
void camOff();
+ bool getCamState();
void cutterOn();
void cutterOff();
+ bool getCutterState();
void setBuzzerOff();
void setBuzzerArmed();
@@ -64,6 +70,7 @@ public:
void setStatusErr();
void setStatusOk();
+ // Methods for CanHandler
void setCanServoOpen(ServosList servo, bool open);
private:
diff --git a/src/boards/Main/CanHandler/CanHandler.cpp b/src/boards/Main/CanHandler/CanHandler.cpp
index 6b42dc47b..1dd201047 100644
--- a/src/boards/Main/CanHandler/CanHandler.cpp
+++ b/src/boards/Main/CanHandler/CanHandler.cpp
@@ -73,12 +73,12 @@ bool CanHandler::start()
TimestampTimer::getTimestamp(),
static_cast<int16_t>(stats.logNumber),
static_cast<uint8_t>(state),
- state == FlightModeManagerState::FMM_STATE_ARMED,
+ state == FlightModeManagerState::ARMED,
false, // TODO: HIL
stats.lastWriteError == 0,
});
},
- Config::CanHandler::STATUS_PERIOD);
+ Config::CanHandler::STATUS_RATE);
if (result == 0)
{
@@ -105,6 +105,25 @@ void CanHandler::sendEvent(Common::CanConfig::EventId event)
static_cast<uint8_t>(event));
}
+void CanHandler::sendServoOpenCommand(ServosList servo, float maxAperture,
+ uint16_t openingTime)
+{
+
+ protocol.enqueueData(
+ static_cast<uint8_t>(CanConfig::Priority::CRITICAL),
+ static_cast<uint8_t>(CanConfig::PrimaryType::COMMAND),
+ static_cast<uint8_t>(CanConfig::Board::MAIN),
+ static_cast<uint8_t>(CanConfig::Board::BROADCAST),
+ static_cast<uint8_t>(servo),
+ ServoCommand{TimestampTimer::getTimestamp(), maxAperture, openingTime});
+}
+
+void CanHandler::sendServoCloseCommand(ServosList servo)
+{
+ // Closing a servo means opening it for 0s
+ sendServoOpenCommand(servo, 0.0f, 0);
+}
+
CanHandler::CanStatus CanHandler::getCanStatus()
{
Lock<FastMutex> lock{statusMutex};
@@ -173,19 +192,20 @@ void CanHandler::handleSensor(const Boardcore::Canbus::CanMessage &msg)
break;
}
- case CanConfig::SensorId::BOTTOM_TANK_PRESSURE:
- {
- CanPressureData data = pressureDataFromCanMessage(msg);
- sdLogger.log(data);
- sensors->setCanBottomTankPress(data);
- break;
- }
+ /* No longer exists
+ case CanConfig::SensorId::BOTTOM_TANK_PRESSURE:
+ {
+ CanPressureData data = pressureDataFromCanMessage(msg);
+ sdLogger.log(data);
+ sensors->setCanBottomTankPress(data);
+ break;
+ }*/
case CanConfig::SensorId::TOP_TANK_PRESSURE:
{
CanPressureData data = pressureDataFromCanMessage(msg);
sdLogger.log(data);
- sensors->setCanTopTankPress(data);
+ sensors->setCanTopTankPress1(data);
break;
}
diff --git a/src/boards/Main/CanHandler/CanHandler.h b/src/boards/Main/CanHandler/CanHandler.h
index 23a40c84a..aa0f1eaf8 100644
--- a/src/boards/Main/CanHandler/CanHandler.h
+++ b/src/boards/Main/CanHandler/CanHandler.h
@@ -26,6 +26,7 @@
#include <Main/Configs/CanHandlerConfig.h>
#include <Main/Sensors/Sensors.h>
#include <common/CanConfig.h>
+#include <common/Mavlink.h>
#include <drivers/canbus/CanProtocol/CanProtocol.h>
#include <utils/DependencyManager/DependencyManager.h>
@@ -95,6 +96,10 @@ public:
void sendEvent(Common::CanConfig::EventId event);
+ void sendServoOpenCommand(ServosList servo, float maxAperture,
+ uint16_t openingTime);
+ void sendServoCloseCommand(ServosList servo);
+
CanStatus getCanStatus();
private:
diff --git a/src/boards/Main/Configs/ADAConfig.h b/src/boards/Main/Configs/ADAConfig.h
new file mode 100644
index 000000000..a9437167c
--- /dev/null
+++ b/src/boards/Main/Configs/ADAConfig.h
@@ -0,0 +1,58 @@
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Authors: Davide Mor
+ *
+ * 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 <units/Frequency.h>
+
+#include <cstdint>
+
+namespace Main
+{
+
+namespace Config
+{
+
+namespace ADA
+{
+
+/* linter off */ using namespace Boardcore::Units::Frequency;
+
+constexpr Hertz SAMPLE_RATE = 50_hz;
+constexpr float SAMPLE_RATE_SECONDS = 0.02; // [s]
+
+constexpr int CALIBRATION_SAMPLES_COUNT = 20;
+constexpr unsigned int CALIBRATION_SLEEP_TIME = 100; // [ms]
+
+constexpr unsigned int SHADOW_MODE_TIMEOUT = 18000; // [ms]
+
+constexpr float APOGEE_VERTICAL_SPEED_TARGET = 2.5; // [m/s]
+constexpr unsigned int APOGEE_N_SAMPLES = 5;
+
+constexpr float DEPLOYMENT_ALTITUDE_TARGET = 350; // [m]
+constexpr unsigned int DEPLOYMENT_N_SAMPLES = 5;
+
+} // namespace ADA
+
+} // namespace Config
+
+} // namespace Main
\ No newline at end of file
diff --git a/src/boards/Main/Configs/ActuatorsConfig.h b/src/boards/Main/Configs/ActuatorsConfig.h
index 662e6cab3..9ee790b39 100644
--- a/src/boards/Main/Configs/ActuatorsConfig.h
+++ b/src/boards/Main/Configs/ActuatorsConfig.h
@@ -44,14 +44,14 @@ constexpr unsigned int EXP_MAX_PULSE = 2000;
constexpr uint16_t BUZZER_FREQUENCY = 1000;
constexpr float BUZZER_DUTY_CYCLE = 0.5;
-constexpr Hertz BUZZER_UPDATE_PERIOD = 20_hz;
-constexpr uint32_t BUZZER_ARM_PERIOD = 10; // 10 * 50ms = 500ms
-constexpr uint32_t BUZZER_LAND_PERIOD = 20; // 20 * 50ms = 1000ms
+constexpr Hertz BUZZER_UPDATE_RATE = 20_hz;
+constexpr uint32_t BUZZER_ARM_RATE = 10; // 10 * 50ms = 500ms
+constexpr uint32_t BUZZER_LAND_RATE = 20; // 20 * 50ms = 1000ms
// Status configs
-constexpr Hertz STATUS_UPDATE_PERIOD = 20_hz;
-constexpr uint32_t STATUS_OK_PERIOD = 20; // 20 * 50ms = 1000ms
-constexpr uint32_t STATUS_ERR_PERIOD = 2; // 2 * 50ms = 10ms
+constexpr Hertz STATUS_UPDATE_RATE = 20_hz;
+constexpr uint32_t STATUS_OK_RATE = 20; // 20 * 50ms = 1000ms
+constexpr uint32_t STATUS_ERR_RATE = 2; // 2 * 50ms = 10ms
} // namespace Actuators
} // namespace Config
diff --git a/src/boards/Main/Configs/CanHandlerConfig.h b/src/boards/Main/Configs/CanHandlerConfig.h
index 8ff3b0c1f..a8a5ba6b7 100644
--- a/src/boards/Main/Configs/CanHandlerConfig.h
+++ b/src/boards/Main/Configs/CanHandlerConfig.h
@@ -35,7 +35,7 @@ namespace CanHandler
/* linter off */ using namespace std::chrono_literals;
-static constexpr std::chrono::nanoseconds STATUS_PERIOD = 1000ms;
+static constexpr std::chrono::nanoseconds STATUS_RATE = 1000ms;
static constexpr std::chrono::nanoseconds STATUS_TIMEOUT = 1500ms;
} // namespace CanHandler
diff --git a/src/boards/Main/Configs/RadioConfig.h b/src/boards/Main/Configs/RadioConfig.h
index 976347546..8d847d440 100644
--- a/src/boards/Main/Configs/RadioConfig.h
+++ b/src/boards/Main/Configs/RadioConfig.h
@@ -46,8 +46,8 @@ constexpr uint8_t MAV_COMPONENT_ID = 96;
constexpr unsigned int CIRCULAR_BUFFER_SIZE = 8;
-constexpr Hertz LOW_RATE_TELEMETRY_PERIOD = 2_hz;
-constexpr Hertz HIGH_RATE_TELEMETRY_PERIOD = 4_hz;
+constexpr Hertz LOW_RATE_TELEMETRY = 2_hz;
+constexpr Hertz HIGH_RATE_TELEMETRY = 4_hz;
} // namespace Radio
diff --git a/src/boards/Main/Configs/SensorsConfig.h b/src/boards/Main/Configs/SensorsConfig.h
index 9116bf41a..15e8fa985 100644
--- a/src/boards/Main/Configs/SensorsConfig.h
+++ b/src/boards/Main/Configs/SensorsConfig.h
@@ -46,7 +46,7 @@ namespace LPS22DF
constexpr Boardcore::LPS22DF::AVG AVG = Boardcore::LPS22DF::AVG_4;
constexpr Boardcore::LPS22DF::ODR ODR = Boardcore::LPS22DF::ODR_100;
-constexpr Hertz PERIOD = 50_hz;
+constexpr Hertz RATE = 50_hz;
constexpr bool ENABLED = true;
} // namespace LPS22DF
@@ -58,7 +58,7 @@ constexpr Boardcore::LPS28DFW::FullScaleRange FS = Boardcore::LPS28DFW::FS_1260;
constexpr Boardcore::LPS28DFW::AVG AVG = Boardcore::LPS28DFW::AVG_4;
constexpr Boardcore::LPS28DFW::ODR ODR = Boardcore::LPS28DFW::ODR_100;
-constexpr Hertz PERIOD = 50_hz;
+constexpr Hertz RATE = 50_hz;
constexpr bool ENABLED = true;
} // namespace LPS28DFW
@@ -71,7 +71,7 @@ constexpr Boardcore::H3LIS331DLDefs::OutputDataRate ODR =
constexpr Boardcore::H3LIS331DLDefs::FullScaleRange FS =
Boardcore::H3LIS331DLDefs::FullScaleRange::FS_100;
-constexpr Hertz PERIOD = 100_hz;
+constexpr Hertz RATE = 100_hz;
constexpr bool ENABLED = true;
} // namespace H3LIS331DL
@@ -82,7 +82,7 @@ namespace LIS2MDL
constexpr Boardcore::LIS2MDL::ODR ODR = Boardcore::LIS2MDL::ODR_100_HZ;
constexpr unsigned int TEMP_DIVIDER = 5;
-constexpr Hertz PERIOD = 100_hz;
+constexpr Hertz RATE = 100_hz;
constexpr bool ENABLED = true;
} // namespace LIS2MDL
@@ -90,7 +90,7 @@ namespace UBXGPS
{
/* linter off */ using namespace Boardcore::Units::Frequency;
-constexpr Hertz PERIOD = 5_hz;
+constexpr Hertz RATE = 5_hz;
constexpr bool ENABLED = true;
} // namespace UBXGPS
@@ -112,7 +112,7 @@ constexpr Boardcore::LSM6DSRXConfig::GYR_ODR GYR_ODR =
constexpr Boardcore::LSM6DSRXConfig::OPERATING_MODE GYR_OP_MODE =
Boardcore::LSM6DSRXConfig::OPERATING_MODE::HIGH_PERFORMANCE;
-constexpr Hertz PERIOD = 50_hz;
+constexpr Hertz RATE = 50_hz;
constexpr bool ENABLED = true;
} // namespace LSM6DSRX
@@ -140,7 +140,7 @@ constexpr float STATIC_PRESSURE_1_SCALE = CHANNEL_0_SCALE;
constexpr float STATIC_PRESSURE_2_SCALE = CHANNEL_1_SCALE;
constexpr float DPL_BAY_PRESSURE_SCALE = CHANNEL_2_SCALE;
-constexpr Hertz PERIOD = 100_hz;
+constexpr Hertz RATE = 100_hz;
constexpr bool ENABLED = true;
} // namespace ADS131M08
@@ -158,7 +158,7 @@ constexpr Boardcore::InternalADC::Channel CUTTER_SENSE_CH =
constexpr float VBAT_SCALE = 7500.0f / 2400.0f;
constexpr float CAM_VBAT_SCALE = 7500.0f / 2400.0f;
-constexpr Hertz PERIOD = 10_hz;
+constexpr Hertz RATE = 10_hz;
constexpr bool ENABLED = true;
} // namespace InternalADC
diff --git a/src/boards/Main/Radio/Radio.cpp b/src/boards/Main/Radio/Radio.cpp
index 80fc39c2d..fc70db5b2 100644
--- a/src/boards/Main/Radio/Radio.cpp
+++ b/src/boards/Main/Radio/Radio.cpp
@@ -105,7 +105,7 @@ bool Radio::start()
enqueueSystemTm(SystemTMList::MAV_FLIGHT_ID);
flushPackets();
},
- Config::Radio::HIGH_RATE_TELEMETRY_PERIOD);
+ Config::Radio::HIGH_RATE_TELEMETRY);
if (result == 0)
{
@@ -116,7 +116,7 @@ bool Radio::start()
// Low rate periodic telemetry
result = scheduler.addTask([this]()
{ enqueueSystemTm(SystemTMList::MAV_STATS_ID); },
- Config::Radio::LOW_RATE_TELEMETRY_PERIOD);
+ Config::Radio::LOW_RATE_TELEMETRY);
if (result == 0)
{
@@ -227,7 +227,7 @@ void Radio::handleMessage(const mavlink_message_t& msg)
mavlink_msg_wiggle_servo_tc_get_servo_id(&msg));
if (getModule<FlightModeManager>()->getState() ==
- FlightModeManagerState::FMM_STATE_TEST_MODE)
+ FlightModeManagerState::TEST_MODE)
{
// If the state is test mode, the wiggle is done
getModule<Actuators>()->wiggleServo(servoId);
@@ -556,11 +556,11 @@ bool Radio::enqueueSystemTm(uint8_t tmId)
tm.timestamp = TimestampTimer::getTimestamp();
// Sensors (either CAN or local)
- tm.top_tank_pressure = sensors->getTopTankPress().pressure;
- tm.bottom_tank_pressure = -1.0f; // TODO
- tm.combustion_chamber_pressure = sensors->getCCPress().pressure;
- tm.tank_temperature = -1.0f; // TODO
- tm.battery_voltage = sensors->getMotorBatteryVoltage().voltage;
+ tm.top_tank_pressure = sensors->getCanTopTankPress1().pressure;
+ tm.bottom_tank_pressure = -1.0f; // Doesn't exist
+ tm.combustion_chamber_pressure = sensors->getCanCCPress().pressure;
+ tm.tank_temperature = sensors->getCanTankTemp().temperature;
+ tm.battery_voltage = sensors->getCanMotorBatteryVoltage().voltage;
// Valve states
tm.main_valve_state =
diff --git a/src/boards/Main/Sensors/Sensors.cpp b/src/boards/Main/Sensors/Sensors.cpp
index c247058e1..3c02fff77 100644
--- a/src/boards/Main/Sensors/Sensors.cpp
+++ b/src/boards/Main/Sensors/Sensors.cpp
@@ -166,46 +166,46 @@ PressureData Sensors::getDplBayPressure()
return dplBayPressure ? dplBayPressure->getLastSample() : PressureData{};
}
-PressureData Sensors::getTopTankPress()
+PressureData Sensors::getCanTopTankPress1()
{
Lock<FastMutex> lock{canMutex};
- return canTopTankPressure;
+ return canTopTankPressure1;
}
-PressureData Sensors::getBottomTankPress()
+PressureData Sensors::getCanTopTankPress2()
{
Lock<FastMutex> lock{canMutex};
- return canBottomTankPressure;
+ return canTopTankPressure2;
}
-PressureData Sensors::getCCPress()
+PressureData Sensors::getCanCCPress()
{
Lock<FastMutex> lock{canMutex};
return canCCPressure;
}
-TemperatureData Sensors::getTankTemp()
+TemperatureData Sensors::getCanTankTemp()
{
Lock<FastMutex> lock{canMutex};
return canTankTemperature;
}
-VoltageData Sensors::getMotorBatteryVoltage()
+VoltageData Sensors::getCanMotorBatteryVoltage()
{
Lock<FastMutex> lock{canMutex};
return canMotorBatteryVoltage;
}
-void Sensors::setCanTopTankPress(Boardcore::PressureData data)
+void Sensors::setCanTopTankPress1(Boardcore::PressureData data)
{
Lock<FastMutex> lock{canMutex};
- canTopTankPressure = data;
+ canTopTankPressure1 = data;
}
-void Sensors::setCanBottomTankPress(Boardcore::PressureData data)
+void Sensors::setCanTopTankPress2(Boardcore::PressureData data)
{
Lock<FastMutex> lock{canMutex};
- canBottomTankPressure = data;
+ canTopTankPressure2 = data;
}
void Sensors::setCanCCPress(Boardcore::PressureData data)
@@ -488,77 +488,77 @@ bool Sensors::sensorManagerInit()
if (lps22df)
{
- SensorInfo info{"LPS22DF", Config::Sensors::LPS22DF::PERIOD,
+ SensorInfo info{"LPS22DF", Config::Sensors::LPS22DF::RATE,
[this]() { lps22dfCallback(); }};
map.emplace(lps22df.get(), info);
}
if (lps28dfw)
{
- SensorInfo info{"LPS28DFW", Config::Sensors::LPS28DFW::PERIOD,
+ SensorInfo info{"LPS28DFW", Config::Sensors::LPS28DFW::RATE,
[this]() { lps28dfwCallback(); }};
map.emplace(lps28dfw.get(), info);
}
if (h3lis331dl)
{
- SensorInfo info{"H3LIS331DL", Config::Sensors::H3LIS331DL::PERIOD,
+ SensorInfo info{"H3LIS331DL", Config::Sensors::H3LIS331DL::RATE,
[this]() { h3lis331dlCallback(); }};
map.emplace(h3lis331dl.get(), info);
}
if (lis2mdl)
{
- SensorInfo info{"LIS2MDL", Config::Sensors::LIS2MDL::PERIOD,
+ SensorInfo info{"LIS2MDL", Config::Sensors::LIS2MDL::RATE,
[this]() { lis2mdlCallback(); }};
map.emplace(lis2mdl.get(), info);
}
if (ubxgps)
{
- SensorInfo info{"UBXGPS", Config::Sensors::UBXGPS::PERIOD,
+ SensorInfo info{"UBXGPS", Config::Sensors::UBXGPS::RATE,
[this]() { ubxgpsCallback(); }};
map.emplace(ubxgps.get(), info);
}
if (lsm6dsrx)
{
- SensorInfo info{"LSM6DSRX", Config::Sensors::LSM6DSRX::PERIOD,
+ SensorInfo info{"LSM6DSRX", Config::Sensors::LSM6DSRX::RATE,
[this]() { lsm6dsrxCallback(); }};
map.emplace(lsm6dsrx.get(), info);
}
if (ads131m08)
{
- SensorInfo info{"ADS131M08", Config::Sensors::ADS131M08::PERIOD,
+ SensorInfo info{"ADS131M08", Config::Sensors::ADS131M08::RATE,
[this]() { ads131m08Callback(); }};
map.emplace(ads131m08.get(), info);
}
if (staticPressure1)
{
- SensorInfo info{"StaticPressure1", Config::Sensors::ADS131M08::PERIOD,
+ SensorInfo info{"StaticPressure1", Config::Sensors::ADS131M08::RATE,
[this]() { staticPressure1Callback(); }};
map.emplace(staticPressure1.get(), info);
}
if (staticPressure2)
{
- SensorInfo info{"StaticPressure2", Config::Sensors::ADS131M08::PERIOD,
+ SensorInfo info{"StaticPressure2", Config::Sensors::ADS131M08::RATE,
[this]() { staticPressure2Callback(); }};
map.emplace(staticPressure2.get(), info);
}
if (dplBayPressure)
{
- SensorInfo info{"DplBayPressure", Config::Sensors::ADS131M08::PERIOD,
+ SensorInfo info{"DplBayPressure", Config::Sensors::ADS131M08::RATE,
[this]() { dplBayPressureCallback(); }};
map.emplace(dplBayPressure.get(), info);
}
if (internalAdc)
{
- SensorInfo info{"InternalADC", Config::Sensors::InternalADC::PERIOD,
+ SensorInfo info{"InternalADC", Config::Sensors::InternalADC::RATE,
[this]() { internalAdcCallback(); }};
map.emplace(internalAdc.get(), info);
}
diff --git a/src/boards/Main/Sensors/Sensors.h b/src/boards/Main/Sensors/Sensors.h
index 5082dea0b..055af7eda 100644
--- a/src/boards/Main/Sensors/Sensors.h
+++ b/src/boards/Main/Sensors/Sensors.h
@@ -69,27 +69,28 @@ public:
Boardcore::PressureData getStaticPressure2();
Boardcore::PressureData getDplBayPressure();
- Boardcore::PressureData getTopTankPress();
- Boardcore::PressureData getBottomTankPress();
- Boardcore::PressureData getCCPress();
- Boardcore::TemperatureData getTankTemp();
- Boardcore::VoltageData getMotorBatteryVoltage();
-
- void setCanTopTankPress(Boardcore::PressureData data);
- void setCanBottomTankPress(Boardcore::PressureData data);
+ Boardcore::PressureData getCanTopTankPress1();
+ Boardcore::PressureData getCanTopTankPress2();
+ Boardcore::PressureData getCanCCPress();
+ Boardcore::TemperatureData getCanTankTemp();
+ Boardcore::VoltageData getCanMotorBatteryVoltage();
+
+ std::vector<Boardcore::SensorInfo> getSensorInfos();
+
+ // Methods for CanHandler
+ void setCanTopTankPress1(Boardcore::PressureData data);
+ void setCanTopTankPress2(Boardcore::PressureData data);
void setCanCCPress(Boardcore::PressureData data);
void setCanTankTemp(Boardcore::TemperatureData data);
void setCanMotorBatteryVoltage(Boardcore::VoltageData data);
- std::vector<Boardcore::SensorInfo> getSensorInfos();
-
protected:
virtual bool postSensorCreationHook() { return true; }
miosix::FastMutex canMutex;
Boardcore::PressureData canCCPressure;
- Boardcore::PressureData canBottomTankPressure;
- Boardcore::PressureData canTopTankPressure;
+ Boardcore::PressureData canTopTankPressure1;
+ Boardcore::PressureData canTopTankPressure2;
Boardcore::TemperatureData canTankTemperature;
Boardcore::VoltageData canMotorBatteryVoltage;
diff --git a/src/boards/Main/StateMachines/ADAController/ADAController.cpp b/src/boards/Main/StateMachines/ADAController/ADAController.cpp
new file mode 100644
index 000000000..8e88d84de
--- /dev/null
+++ b/src/boards/Main/StateMachines/ADAController/ADAController.cpp
@@ -0,0 +1,416 @@
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Author: Davide Mor
+ *
+ * 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 "ADAController.h"
+
+#include <Main/Configs/ADAConfig.h>
+#include <common/Events.h>
+#include <common/ReferenceConfig.h>
+#include <common/Topics.h>
+#include <events/EventBroker.h>
+#include <utils/AeroUtils/AeroUtils.h>
+
+using namespace Main;
+using namespace Boardcore;
+using namespace Common;
+using namespace miosix;
+
+// The default Kalman is empty, as calibrate will update it correctly
+const ADA::KalmanFilter::KalmanConfig DEFAULT_KALMAN_CONFIG{};
+
+ADA::KalmanFilter::KalmanConfig computeADAKalmanConfig(float refPressure)
+{
+ ADA::KalmanFilter::MatrixNN F;
+ ADA::KalmanFilter::MatrixPN H;
+ ADA::KalmanFilter::MatrixNN P;
+ ADA::KalmanFilter::MatrixNN Q;
+ ADA::KalmanFilter::MatrixPP R;
+ ADA::KalmanFilter::MatrixNM G;
+ ADA::KalmanFilter::CVectorN x;
+
+ // clang-format off
+ F = ADA::KalmanFilter::MatrixNN({
+ {1.0, Config::ADA::SAMPLE_RATE_SECONDS, 0.5f * Config::ADA::SAMPLE_RATE_SECONDS * Config::ADA::SAMPLE_RATE_SECONDS},
+ {0.0, 1.0, Config::ADA::SAMPLE_RATE_SECONDS },
+ {0.0, 0.0, 1.0 }});
+
+ H = {1.0, 0.0, 0.0};
+
+ P = ADA::KalmanFilter::MatrixNN({
+ {1.0, 0.0, 0.0},
+ {0.0, 1.0, 0.0},
+ {0.0, 0.0, 1.0}});
+
+ Q << ADA::KalmanFilter::MatrixNN({
+ {30.0, 0.0, 0.0},
+ {0.0, 10.0, 0.0},
+ {0.0, 0.0, 2.5}});
+
+ R[0] = 4000.0;
+
+ G = ADA::KalmanFilter::MatrixNM::Zero();
+
+ x = ADA::KalmanFilter::CVectorN(refPressure, 0, 0);
+ // clang-format on
+
+ return {F, H, Q, R, P, G, x};
+}
+
+ADAController::ADAController()
+ : FSM{&ADAController::state_init, miosix::STACK_DEFAULT_FOR_PTHREAD,
+ Config::Scheduler::ADA_PRIORITY},
+ ada{DEFAULT_KALMAN_CONFIG}
+{
+ EventBroker::getInstance().subscribe(this, TOPIC_ADA);
+ EventBroker::getInstance().subscribe(this, TOPIC_FLIGHT);
+
+ ada.setReferenceValues(ReferenceConfig::defaultReferenceValues);
+}
+
+bool ADAController::start()
+{
+ TaskScheduler& scheduler = getModule<BoardScheduler>()->getAdaScheduler();
+
+ uint8_t result =
+ scheduler.addTask([this]() { update(); }, Config::ADA::SAMPLE_RATE);
+
+ if (result == 0)
+ {
+ LOG_ERR(logger, "Failed to add ADA update task");
+ return false;
+ }
+
+ if (!FSM::start())
+ {
+ LOG_ERR(logger, "Failed to start ADA FSM");
+ return false;
+ }
+
+ return true;
+}
+
+ADAState ADAController::getADAState()
+{
+ Lock<FastMutex> lock{adaMutex};
+ return ada.getState();
+}
+
+void ADAController::update()
+{
+ PressureData baro = getModule<Sensors>()->getStaticPressure1();
+
+ // TODO(davide.mor): What about testing?
+ ADAControllerState curState = state;
+
+ // Lock ADA for the whole duration of the update
+ Lock<FastMutex> lock{adaMutex};
+
+ // First update the kalman
+ if (curState == ADAControllerState::ARMED ||
+ curState == ADAControllerState::SHADOW_MODE ||
+ curState == ADAControllerState::ACTIVE_ASCENT ||
+ curState == ADAControllerState::ACTIVE_DROGUE_DESCENT ||
+ curState == ADAControllerState::ACTIVE_TERMINAL_DESCENT)
+ {
+ // Perform baro correction
+ if (lastBaroTimestamp != baro.pressureTimestamp)
+ {
+ ada.update(baro.pressure);
+ }
+ }
+
+ // Then run detections
+ if (curState == ADAControllerState::SHADOW_MODE ||
+ curState == ADAControllerState::ACTIVE_ASCENT)
+ {
+ if (ada.getState().verticalSpeed <
+ Config::ADA::APOGEE_VERTICAL_SPEED_TARGET)
+ {
+ detectedApogees++;
+ }
+ else
+ {
+ // Apogees must be consecutive in order to be valid
+ detectedApogees = 0;
+ }
+
+ if (curState != ADAControllerState::SHADOW_MODE)
+ {
+ // DO NOT THROW EVENTS IN SHADOW_MODE!
+ if (detectedApogees > Config::ADA::APOGEE_N_SAMPLES)
+ {
+ EventBroker::getInstance().post(ADA_APOGEE_DETECTED, TOPIC_ADA);
+ }
+ }
+ }
+
+ if (curState == ADAControllerState::ACTIVE_DROGUE_DESCENT)
+ {
+ if (ada.getState().aglAltitude <
+ Config::ADA::DEPLOYMENT_ALTITUDE_TARGET)
+ {
+ detectedDeployments++;
+ }
+ else
+ {
+ detectedDeployments = 0;
+ }
+
+ if (detectedDeployments > Config::ADA::DEPLOYMENT_N_SAMPLES)
+ {
+ // TODO(davide.mor): Rename this event
+ EventBroker::getInstance().post(ALTITUDE_TRIGGER_ALTITUDE_REACHED,
+ TOPIC_ADA);
+ }
+ }
+
+ // Log this sample step
+ ADAControllerSampleData data = {TimestampTimer::getTimestamp(),
+ detectedApogees, detectedDeployments,
+ curState};
+ sdLogger.log(data);
+
+ lastBaroTimestamp = baro.pressureTimestamp;
+}
+
+void ADAController::calibrate()
+{
+ Stats baroStats;
+
+ for (int i = 0; i < Config::ADA::CALIBRATION_SAMPLES_COUNT; i++)
+ {
+ PressureData baro = getModule<Sensors>()->getStaticPressure1();
+ baroStats.add(baro.pressure);
+
+ Thread::sleep(Config::ADA::CALIBRATION_SLEEP_TIME);
+ }
+
+ Lock<FastMutex> lock{adaMutex};
+
+ ReferenceValues reference = ada.getReferenceValues();
+ reference.refPressure = baroStats.getStats().mean;
+ reference.refAltitude = Aeroutils::relAltitude(
+ reference.refPressure, reference.mslPressure, reference.mslTemperature);
+
+ ada.setReferenceValues(reference);
+ ada.setKalmanConfig(computeADAKalmanConfig(reference.refPressure));
+ ada.update(reference.refPressure);
+
+ EventBroker::getInstance().post(ADA_READY, TOPIC_ADA);
+}
+
+void ADAController::state_init(const Event& event)
+{
+ switch (event)
+ {
+ case EV_ENTRY:
+ {
+ updateAndLogStatus(ADAControllerState::INIT);
+ break;
+ }
+ case ADA_CALIBRATE:
+ {
+ transition(&ADAController::state_calibrating);
+ break;
+ }
+ }
+}
+
+void ADAController::state_calibrating(const Event& event)
+{
+ switch (event)
+ {
+ case EV_ENTRY:
+ {
+ updateAndLogStatus(ADAControllerState::CALIBRATING);
+
+ calibrate();
+ break;
+ }
+ case ADA_READY:
+ {
+ transition(&ADAController::state_ready);
+ break;
+ }
+ }
+}
+
+void ADAController::state_ready(const Event& event)
+{
+ switch (event)
+ {
+ case EV_ENTRY:
+ {
+ updateAndLogStatus(ADAControllerState::READY);
+ break;
+ }
+ case ADA_CALIBRATE:
+ {
+ transition(&ADAController::state_calibrating);
+ break;
+ }
+ case FLIGHT_ARMED:
+ {
+ transition(&ADAController::state_armed);
+ break;
+ }
+ }
+}
+
+void ADAController::state_armed(const Event& event)
+{
+ switch (event)
+ {
+ case EV_ENTRY:
+ {
+ updateAndLogStatus(ADAControllerState::ARMED);
+ break;
+ }
+ case FLIGHT_DISARMED:
+ {
+ transition(&ADAController::state_ready);
+ break;
+ }
+ case FLIGHT_LIFTOFF:
+ {
+ transition(&ADAController::state_shadow_mode);
+ break;
+ }
+ }
+}
+
+void ADAController::state_shadow_mode(const Event& event)
+{
+ switch (event)
+ {
+ case EV_ENTRY:
+ {
+ updateAndLogStatus(ADAControllerState::SHADOW_MODE);
+
+ shadowModeTimeoutEvent = EventBroker::getInstance().postDelayed(
+ ADA_SHADOW_MODE_TIMEOUT, TOPIC_ADA,
+ Config::ADA::SHADOW_MODE_TIMEOUT);
+ break;
+ }
+
+ case EV_EXIT:
+ {
+ EventBroker::getInstance().removeDelayed(shadowModeTimeoutEvent);
+ }
+
+ case ADA_SHADOW_MODE_TIMEOUT:
+ {
+ transition(&ADAController::state_active_ascent);
+ break;
+ }
+
+ case FLIGHT_LANDING_DETECTED:
+ {
+ transition(&ADAController::state_end);
+ break;
+ }
+ }
+}
+
+void ADAController::state_active_ascent(const Event& event)
+{
+ switch (event)
+ {
+ case EV_ENTRY:
+ {
+ updateAndLogStatus(ADAControllerState::ACTIVE_ASCENT);
+ break;
+ }
+
+ case FLIGHT_APOGEE_DETECTED:
+ {
+ transition(&ADAController::state_active_drogue_descent);
+ break;
+ }
+
+ case FLIGHT_LANDING_DETECTED:
+ {
+ transition(&ADAController::state_end);
+ break;
+ }
+ }
+}
+
+void ADAController::state_active_drogue_descent(const Event& event)
+{
+ switch (event)
+ {
+ case EV_ENTRY:
+ {
+ updateAndLogStatus(ADAControllerState::ACTIVE_DROGUE_DESCENT);
+ break;
+ }
+
+ case FLIGHT_DPL_ALT_DETECTED:
+ {
+ transition(&ADAController::state_active_terminal_descent);
+ break;
+ }
+
+ case FLIGHT_LANDING_DETECTED:
+ {
+ transition(&ADAController::state_end);
+ break;
+ }
+ }
+}
+
+void ADAController::state_active_terminal_descent(const Event& event)
+{
+ switch (event)
+ {
+ case EV_ENTRY:
+ {
+ updateAndLogStatus(ADAControllerState::ACTIVE_TERMINAL_DESCENT);
+ break;
+ }
+ case FLIGHT_LANDING_DETECTED:
+ {
+ transition(&ADAController::state_end);
+ break;
+ }
+ }
+}
+
+void ADAController::state_end(const Event& event)
+{
+ switch (event)
+ {
+ case EV_ENTRY:
+ {
+ updateAndLogStatus(ADAControllerState::END);
+ break;
+ }
+ }
+}
+
+void ADAController::updateAndLogStatus(ADAControllerState state)
+{
+ this->state = state;
+ ADAControllerStatus data = {TimestampTimer::getTimestamp(), state};
+ sdLogger.log(data);
+}
\ No newline at end of file
diff --git a/src/boards/Main/StateMachines/ADAController/ADAController.h b/src/boards/Main/StateMachines/ADAController/ADAController.h
new file mode 100644
index 000000000..5d559a858
--- /dev/null
+++ b/src/boards/Main/StateMachines/ADAController/ADAController.h
@@ -0,0 +1,78 @@
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Author: Davide Mor
+ *
+ * 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 <Main/BoardScheduler.h>
+#include <Main/Sensors/Sensors.h>
+#include <Main/StateMachines/ADAController/ADAControllerData.h>
+#include <algorithms/ADA/ADA.h>
+#include <events/FSM.h>
+#include <utils/DependencyManager/DependencyManager.h>
+
+namespace Main
+{
+
+class ADAController
+ : public Boardcore::InjectableWithDeps<BoardScheduler, Sensors>,
+ public Boardcore::FSM<ADAController>
+{
+public:
+ ADAController();
+
+ [[nodiscard]] bool start() override;
+
+ Boardcore::ADAState getADAState();
+
+private:
+ void update();
+ void calibrate();
+
+ // FSM states
+ void state_init(const Boardcore::Event& event);
+ void state_calibrating(const Boardcore::Event& event);
+ void state_ready(const Boardcore::Event& event);
+ void state_armed(const Boardcore::Event& event);
+ void state_shadow_mode(const Boardcore::Event& event);
+ void state_active_ascent(const Boardcore::Event& event);
+ void state_active_drogue_descent(const Boardcore::Event& event);
+ void state_active_terminal_descent(const Boardcore::Event& event);
+ void state_end(const Boardcore::Event& event);
+
+ void updateAndLogStatus(ADAControllerState state);
+
+ std::atomic<ADAControllerState> state{ADAControllerState::INIT};
+
+ Boardcore::Logger& sdLogger = Boardcore::Logger::getInstance();
+ Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("ada");
+
+ uint16_t shadowModeTimeoutEvent = 0;
+
+ miosix::FastMutex adaMutex;
+ Boardcore::ADA ada;
+
+ uint64_t lastBaroTimestamp = 0;
+ unsigned int detectedApogees = 0;
+ unsigned int detectedDeployments = 0;
+};
+
+} // namespace Main
\ No newline at end of file
diff --git a/src/boards/Main/StateMachines/ADAController/ADAControllerData.h b/src/boards/Main/StateMachines/ADAController/ADAControllerData.h
new file mode 100644
index 000000000..94f1a6474
--- /dev/null
+++ b/src/boards/Main/StateMachines/ADAController/ADAControllerData.h
@@ -0,0 +1,77 @@
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Author: Davide Mor
+ *
+ * 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 <cstdint>
+#include <ostream>
+#include <string>
+
+namespace Main
+{
+
+enum class ADAControllerState : uint8_t
+{
+ INIT = 0,
+ CALIBRATING,
+ READY,
+ ARMED,
+ SHADOW_MODE,
+ ACTIVE_ASCENT,
+ ACTIVE_DROGUE_DESCENT,
+ ACTIVE_TERMINAL_DESCENT,
+ END
+};
+
+struct ADAControllerStatus
+{
+ uint64_t timestamp = 0;
+ ADAControllerState state = ADAControllerState::INIT;
+
+ static std::string header() { return "timestamp,state\n"; }
+
+ void print(std::ostream& os) const
+ {
+ os << timestamp << "," << static_cast<int>(state) << "\n";
+ }
+};
+
+struct ADAControllerSampleData
+{
+ uint64_t timestamp = 0;
+ unsigned int detectedApogees = 0;
+ unsigned int detectedDeployments = 0;
+ ADAControllerState state = ADAControllerState::INIT;
+
+ static std::string header()
+ {
+ return "timestamp,detectedApogees,detectedDeployments,state\n";
+ }
+
+ void print(std::ostream& os) const
+ {
+ os << timestamp << "," << detectedApogees << "," << detectedDeployments
+ << "," << static_cast<int>(state) << "\n";
+ }
+};
+
+} // namespace Main
\ No newline at end of file
diff --git a/src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.cpp b/src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.cpp
index e105d054f..00a979e92 100644
--- a/src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.cpp
+++ b/src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.cpp
@@ -43,73 +43,7 @@ FlightModeManager::FlightModeManager()
EventBroker::getInstance().subscribe(this, TOPIC_NAS);
}
-FlightModeManagerState FlightModeManager::getState()
-{
- if (testState(&FlightModeManager::state_on_ground))
- {
- return FlightModeManagerState::FMM_STATE_ON_GROUND;
- }
- else if (testState(&FlightModeManager::state_init))
- {
- return FlightModeManagerState::FMM_STATE_INIT;
- }
- else if (testState(&FlightModeManager::state_init_error))
- {
- return FlightModeManagerState::FMM_STATE_INIT_ERROR;
- }
- else if (testState(&FlightModeManager::state_init_done))
- {
- return FlightModeManagerState::FMM_STATE_INIT_DONE;
- }
- else if (testState(&FlightModeManager::state_calibrate_sensors))
- {
- return FlightModeManagerState::FMM_STATE_CALIBRATE_SENSORS;
- }
- else if (testState(&FlightModeManager::state_calibrate_algorithms))
- {
- return FlightModeManagerState::FMM_STATE_CALIBRATE_ALGORITHMS;
- }
- else if (testState(&FlightModeManager::state_disarmed))
- {
- return FlightModeManagerState::FMM_STATE_DISARMED;
- }
- else if (testState(&FlightModeManager::state_test_mode))
- {
- return FlightModeManagerState::FMM_STATE_TEST_MODE;
- }
- else if (testState(&FlightModeManager::state_armed))
- {
- return FlightModeManagerState::FMM_STATE_ARMED;
- }
- else if (testState(&FlightModeManager::state_flying))
- {
- return FlightModeManagerState::FMM_STATE_FLYING;
- }
- else if (testState(&FlightModeManager::state_powered_ascent))
- {
- return FlightModeManagerState::FMM_STATE_POWERED_ASCENT;
- }
- else if (testState(&FlightModeManager::state_unpowered_ascent))
- {
- return FlightModeManagerState::FMM_STATE_UNPOWERED_ASCENT;
- }
- else if (testState(&FlightModeManager::state_drogue_descent))
- {
- return FlightModeManagerState::FMM_STATE_DROGUE_DESCENT;
- }
- else if (testState(&FlightModeManager::state_terminal_descent))
- {
- return FlightModeManagerState::FMM_STATE_TERMINAL_DESCENT;
- }
- else if (testState(&FlightModeManager::state_landed))
- {
- return FlightModeManagerState::FMM_STATE_LANDED;
- }
- else
- {
- return FlightModeManagerState::FMM_STATE_INVALID;
- }
-}
+FlightModeManagerState FlightModeManager::getState() { return state; }
State FlightModeManager::state_on_ground(const Event& event)
{
@@ -117,7 +51,7 @@ State FlightModeManager::state_on_ground(const Event& event)
{
case EV_ENTRY:
{
- updateAndLogStatus(FMM_STATE_ON_GROUND);
+ updateAndLogStatus(FlightModeManagerState::ON_GROUND);
return HANDLED;
}
case EV_EXIT:
@@ -150,7 +84,7 @@ State FlightModeManager::state_init(const Event& event)
{
case EV_ENTRY:
{
- updateAndLogStatus(FMM_STATE_INIT);
+ updateAndLogStatus(FlightModeManagerState::INIT);
return HANDLED;
}
case EV_EXIT:
@@ -186,7 +120,7 @@ State FlightModeManager::state_init_error(const Event& event)
{
case EV_ENTRY:
{
- updateAndLogStatus(FMM_STATE_INIT_ERROR);
+ updateAndLogStatus(FlightModeManagerState::INIT_ERROR);
return HANDLED;
}
case EV_EXIT:
@@ -207,7 +141,7 @@ State FlightModeManager::state_init_error(const Event& event)
}
case TMTC_FORCE_INIT:
{
- // TODO(davide.mor): Also send this via CanBus
+ getModule<CanHandler>()->sendEvent(CanConfig::EventId::FORCE_INIT);
return transition(&FlightModeManager::state_init_done);
}
default:
@@ -223,7 +157,7 @@ State FlightModeManager::state_init_done(const Event& event)
{
case EV_ENTRY:
{
- updateAndLogStatus(FMM_STATE_INIT_DONE);
+ updateAndLogStatus(FlightModeManagerState::INIT_DONE);
EventBroker::getInstance().post(FMM_CALIBRATE, TOPIC_FMM);
return HANDLED;
}
@@ -256,7 +190,7 @@ State FlightModeManager::state_calibrate_sensors(const Event& event)
{
case EV_ENTRY:
{
- updateAndLogStatus(FMM_STATE_CALIBRATE_SENSORS);
+ updateAndLogStatus(FlightModeManagerState::CALIBRATE_SENSORS);
// TODO(davide.mor): Calibrate sensors
Thread::sleep(2000);
EventBroker::getInstance().post(FMM_SENSORS_CAL_DONE, TOPIC_FMM);
@@ -291,7 +225,7 @@ State FlightModeManager::state_calibrate_algorithms(const Event& event)
{
case EV_ENTRY:
{
- updateAndLogStatus(FMM_STATE_CALIBRATE_ALGORITHMS);
+ updateAndLogStatus(FlightModeManagerState::CALIBRATE_ALGORITHMS);
// Reset readiness status
nasReady = false;
@@ -303,7 +237,6 @@ State FlightModeManager::state_calibrate_algorithms(const Event& event)
// Quick hack to make the state machine go forward
Thread::sleep(2000);
- EventBroker::getInstance().post(NAS_READY, TOPIC_NAS);
EventBroker::getInstance().post(ADA_READY, TOPIC_ADA);
return HANDLED;
@@ -357,11 +290,13 @@ State FlightModeManager::state_disarmed(const Event& event)
{
case EV_ENTRY:
{
- updateAndLogStatus(FMM_STATE_DISARMED);
+ updateAndLogStatus(FlightModeManagerState::DISARMED);
getModule<Actuators>()->camOff();
getModule<Actuators>()->setBuzzerOff();
+ EventBroker::getInstance().post(FLIGHT_DISARMED, TOPIC_FLIGHT);
+
return HANDLED;
}
case EV_EXIT:
@@ -390,17 +325,18 @@ State FlightModeManager::state_disarmed(const Event& event)
}
case TMTC_CALIBRATE:
{
- // TODO(davide.mor): Also send this via CanBus
+ getModule<CanHandler>()->sendEvent(CanConfig::EventId::CALIBRATE);
return transition(&FlightModeManager::state_calibrate_sensors);
}
case TMTC_ENTER_TEST_MODE:
{
- // TODO(davide.mor): Also send this via CanBus
+ getModule<CanHandler>()->sendEvent(
+ CanConfig::EventId::ENTER_TEST_MODE);
return transition(&FlightModeManager::state_test_mode);
}
case TMTC_ARM:
{
- // TODO(davide.mor): Also send this via CanBus
+ getModule<CanHandler>()->sendEvent(CanConfig::EventId::ARM);
return transition(&FlightModeManager::state_armed);
}
default:
@@ -416,7 +352,7 @@ State FlightModeManager::state_test_mode(const Event& event)
{
case EV_ENTRY:
{
- updateAndLogStatus(FMM_STATE_TEST_MODE);
+ updateAndLogStatus(FlightModeManagerState::TEST_MODE);
// TODO(davide.mor): Start algorithms
return HANDLED;
}
@@ -449,7 +385,8 @@ State FlightModeManager::state_test_mode(const Event& event)
}
case TMTC_EXIT_TEST_MODE:
{
- // TODO(davide.mor): Also send this via CanBus
+ getModule<CanHandler>()->sendEvent(
+ CanConfig::EventId::EXIT_TEST_MODE);
return transition(&FlightModeManager::state_disarmed);
}
default:
@@ -465,14 +402,17 @@ State FlightModeManager::state_armed(const Event& event)
{
case EV_ENTRY:
{
- updateAndLogStatus(FMM_STATE_ARMED);
+ updateAndLogStatus(FlightModeManagerState::ARMED);
Logger::getInstance().stop();
Logger::getInstance().start();
+ Logger::getInstance().resetStats();
getModule<Actuators>()->camOn();
getModule<Actuators>()->setBuzzerArmed();
+ EventBroker::getInstance().post(FLIGHT_ARMED, TOPIC_FLIGHT);
+
return HANDLED;
}
case EV_EMPTY:
@@ -493,13 +433,13 @@ State FlightModeManager::state_armed(const Event& event)
}
case TMTC_DISARM:
{
- // TODO(davide.mor): Also send this via CanBus
+ getModule<CanHandler>()->sendEvent(CanConfig::EventId::DISARM);
return transition(&FlightModeManager::state_disarmed);
}
case TMTC_FORCE_LAUNCH:
case FLIGHT_LAUNCH_PIN_DETACHED:
{
- // TODO(davide.mor): Also send this via CanBus
+ getModule<CanHandler>()->sendEvent(CanConfig::EventId::LIFTOFF);
return transition(&FlightModeManager::state_flying);
}
default:
@@ -515,7 +455,7 @@ State FlightModeManager::state_flying(const Event& event)
{
case EV_ENTRY:
{
- updateAndLogStatus(FMM_STATE_FLYING);
+ updateAndLogStatus(FlightModeManagerState::FLYING);
getModule<Actuators>()->setBuzzerOff();
@@ -556,7 +496,9 @@ State FlightModeManager::state_powered_ascent(const Event& event)
{
case EV_ENTRY:
{
- updateAndLogStatus(FMM_STATE_POWERED_ASCENT);
+ updateAndLogStatus(FlightModeManagerState::POWERED_ASCENT);
+
+ EventBroker::getInstance().post(FLIGHT_LIFTOFF, TOPIC_FLIGHT);
// Safety engine shutdown
engineShutdownEvent = EventBroker::getInstance().postDelayed(
@@ -581,8 +523,8 @@ State FlightModeManager::state_powered_ascent(const Event& event)
case MEA_SHUTDOWN_DETECTED:
case MOTOR_CLOSE_FEED_VALVE:
{
- // TODO(davide.mor): Actually shutdown the motor via CanBus
-
+ getModule<CanHandler>()->sendServoCloseCommand(
+ ServosList::MAIN_VALVE);
return transition(&FlightModeManager::state_unpowered_ascent);
}
default:
@@ -598,7 +540,7 @@ State FlightModeManager::state_unpowered_ascent(const Event& event)
{
case EV_ENTRY:
{
- updateAndLogStatus(FMM_STATE_UNPOWERED_ASCENT);
+ updateAndLogStatus(FlightModeManagerState::UNPOWERED_ASCENT);
apogeeTimeoutEvent = EventBroker::getInstance().postDelayed(
TMTC_FORCE_APOGEE, TOPIC_TMTC,
@@ -621,9 +563,6 @@ State FlightModeManager::state_unpowered_ascent(const Event& event)
case TMTC_FORCE_APOGEE:
case ADA_APOGEE_DETECTED:
{
- // TODO(davide.mor): Also send this via CanBus
- getModule<Actuators>()->openExpulsion();
-
return transition(&FlightModeManager::state_drogue_descent);
}
default:
@@ -639,9 +578,19 @@ State FlightModeManager::state_drogue_descent(const Event& event)
{
case EV_ENTRY:
{
- updateAndLogStatus(FMM_STATE_DROGUE_DESCENT);
+ updateAndLogStatus(FlightModeManagerState::DROGUE_DESCENT);
+
+ getModule<Actuators>()->openExpulsion();
+ getModule<CanHandler>()->sendEvent(
+ CanConfig::EventId::APOGEE_DETECTED);
- // TODO(davide.mor): Perform venting?
+ EventBroker::getInstance().post(FLIGHT_APOGEE_DETECTED,
+ TOPIC_FLIGHT);
+
+ // TODO: We need a way to signal a full vent, the current setup
+ // doesn't allow to vent for more than 60s
+ getModule<CanHandler>()->sendServoOpenCommand(
+ ServosList::VENTING_VALVE, 1.0f, 60000);
return HANDLED;
}
@@ -675,7 +624,10 @@ State FlightModeManager::state_terminal_descent(const Event& event)
{
case EV_ENTRY:
{
- updateAndLogStatus(FMM_STATE_TERMINAL_DESCENT);
+ updateAndLogStatus(FlightModeManagerState::TERMINAL_DESCENT);
+
+ EventBroker::getInstance().post(FLIGHT_DPL_ALT_DETECTED,
+ TOPIC_FLIGHT);
// TODO(davide.mor): Actuate cutters?
@@ -710,8 +662,10 @@ State FlightModeManager::state_landed(const Event& event)
{
case EV_ENTRY:
{
- updateAndLogStatus(FMM_STATE_LANDED);
+ updateAndLogStatus(FlightModeManagerState::LANDED);
+ EventBroker::getInstance().post(FLIGHT_LANDING_DETECTED,
+ TOPIC_FLIGHT);
Logger::getInstance().stop();
getModule<Actuators>()->setBuzzerLand();
diff --git a/src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.h b/src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.h
index b25dda68a..6c4fd88d9 100644
--- a/src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.h
+++ b/src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.h
@@ -23,6 +23,7 @@
#pragma once
#include <Main/Actuators/Actuators.h>
+#include <Main/CanHandler/CanHandler.h>
#include <events/EventBroker.h>
#include <events/HSM.h>
#include <utils/DependencyManager/DependencyManager.h>
@@ -32,8 +33,9 @@
namespace Main
{
-class FlightModeManager : public Boardcore::InjectableWithDeps<Actuators>,
- public Boardcore::HSM<FlightModeManager>
+class FlightModeManager
+ : public Boardcore::InjectableWithDeps<Actuators, CanHandler>,
+ public Boardcore::HSM<FlightModeManager>
{
public:
FlightModeManager();
@@ -83,7 +85,7 @@ private:
uint16_t engineShutdownEvent = 0;
uint16_t apogeeTimeoutEvent = 0;
- std::atomic<FlightModeManagerState> state{FMM_STATE_INVALID};
+ std::atomic<FlightModeManagerState> state{FlightModeManagerState::INVALID};
};
} // namespace Main
\ No newline at end of file
diff --git a/src/boards/Main/StateMachines/FlightModeManager/FlightModeManagerData.h b/src/boards/Main/StateMachines/FlightModeManager/FlightModeManagerData.h
index d275f7dc1..2f462a36c 100644
--- a/src/boards/Main/StateMachines/FlightModeManager/FlightModeManagerData.h
+++ b/src/boards/Main/StateMachines/FlightModeManager/FlightModeManagerData.h
@@ -25,31 +25,31 @@
namespace Main
{
-enum FlightModeManagerState : uint8_t
+enum class FlightModeManagerState : uint8_t
{
- FMM_STATE_ON_GROUND = 0,
- FMM_STATE_INIT,
- FMM_STATE_INIT_ERROR,
- FMM_STATE_INIT_DONE,
- FMM_STATE_CALIBRATE_SENSORS,
- FMM_STATE_CALIBRATE_ALGORITHMS,
- FMM_STATE_DISARMED,
- FMM_STATE_TEST_MODE,
- FMM_STATE_ARMED,
- FMM_STATE_IGNITION, // < Unused, kept for backward compatibility
- FMM_STATE_FLYING,
- FMM_STATE_POWERED_ASCENT,
- FMM_STATE_UNPOWERED_ASCENT,
- FMM_STATE_DROGUE_DESCENT,
- FMM_STATE_TERMINAL_DESCENT,
- FMM_STATE_LANDED,
- FMM_STATE_INVALID
+ ON_GROUND = 0,
+ INIT,
+ INIT_ERROR,
+ INIT_DONE,
+ CALIBRATE_SENSORS,
+ CALIBRATE_ALGORITHMS,
+ DISARMED,
+ TEST_MODE,
+ ARMED,
+ IGNITION, // < Unused, kept for backward compatibility
+ FLYING,
+ POWERED_ASCENT,
+ UNPOWERED_ASCENT,
+ DROGUE_DESCENT,
+ TERMINAL_DESCENT,
+ LANDED,
+ INVALID
};
struct FlightModeManagerStatus
{
uint64_t timestamp = 0;
- FlightModeManagerState state = FlightModeManagerState::FMM_STATE_INVALID;
+ FlightModeManagerState state = FlightModeManagerState::INVALID;
static std::string header() { return "timestamp,state\n"; }
diff --git a/src/entrypoints/Main/main-entry.cpp b/src/entrypoints/Main/main-entry.cpp
index 334bcae20..c56cad48f 100644
--- a/src/entrypoints/Main/main-entry.cpp
+++ b/src/entrypoints/Main/main-entry.cpp
@@ -27,6 +27,7 @@
#include <Main/PinHandler/PinHandler.h>
#include <Main/Radio/Radio.h>
#include <Main/Sensors/Sensors.h>
+#include <Main/StateMachines/ADAController/ADAController.h>
#include <Main/StateMachines/FlightModeManager/FlightModeManager.h>
#include <actuators/Servo/Servo.h>
#include <drivers/timer/PWM.h>
@@ -56,16 +57,18 @@ int main()
CanHandler *canHandler = new CanHandler();
PinHandler *pinHandler = new PinHandler();
FlightModeManager *fmm = new FlightModeManager();
+ ADAController *ada = new ADAController();
// Insert modules
- bool initResult =
- manager.insert<Buses>(buses) &&
- manager.insert<BoardScheduler>(scheduler) &&
- manager.insert<Sensors>(sensors) && manager.insert<Radio>(radio) &&
- manager.insert<Actuators>(actuators) &&
- manager.insert<CanHandler>(canHandler) &&
- manager.insert<PinHandler>(pinHandler) &&
- manager.insert<FlightModeManager>(fmm) && manager.inject();
+ bool initResult = manager.insert<Buses>(buses) &&
+ manager.insert<BoardScheduler>(scheduler) &&
+ manager.insert<Sensors>(sensors) &&
+ manager.insert<Radio>(radio) &&
+ manager.insert<Actuators>(actuators) &&
+ manager.insert<CanHandler>(canHandler) &&
+ manager.insert<PinHandler>(pinHandler) &&
+ manager.insert<FlightModeManager>(fmm) &&
+ manager.insert<ADAController>(ada) && manager.inject();
manager.graphviz(std::cout);
@@ -135,6 +138,12 @@ int main()
std::cout << "Error failed to start scheduler" << std::endl;
}
+ if (!ada->start())
+ {
+ initResult = false;
+ std::cout << "Error failed to start ADAController" << std::endl;
+ }
+
if (!fmm->start())
{
initResult = false;
--
GitLab