diff --git a/cmake/dependencies.cmake b/cmake/dependencies.cmake index f15b5c01bf51bf4a26af6e9e52d0fba696673210..2874d30e76943630ecde23ae28e94aed5c3d895b 100644 --- a/cmake/dependencies.cmake +++ b/cmake/dependencies.cmake @@ -85,7 +85,7 @@ set(PAYLOAD_COMPUTER src/boards/Payload/Sensors/Sensors.cpp src/boards/Payload/PinHandler/PinHandler.cpp src/boards/Payload/Radio/Radio.cpp - src/boards/Payload/TMRepository/TMRepository.cpp + src/boards/Payload/Radio/MessageHandler.cpp src/boards/Payload/StateMachines/NASController/NASController.cpp src/boards/Payload/StateMachines/FlightModeManager/FlightModeManager.cpp src/boards/Payload/StateMachines/WingController/WingController.cpp diff --git a/src/boards/Payload/Configs/ActuatorsConfig.h b/src/boards/Payload/Configs/ActuatorsConfig.h index 106c0dba16e35fadfa6a59393a687c766c1687e7..92c9c06a5fb3a871b20f85e3cdaa04d1bb2a524e 100644 --- a/src/boards/Payload/Configs/ActuatorsConfig.h +++ b/src/boards/Payload/Configs/ActuatorsConfig.h @@ -31,10 +31,7 @@ namespace Config namespace Actuators { -// clang-format off -// Indent to avoid the linter complaining about using namespace - using namespace std::chrono; -// clang-format on +/* linter off */ using namespace std::chrono_literals; namespace LeftServo { diff --git a/src/boards/Payload/Configs/PinHandlerConfig.h b/src/boards/Payload/Configs/PinHandlerConfig.h index 75d6cbde25a61d76de250725863517eeb93a062f..92f984c1c417c3d0a0886a243ac4a9c0aa057409 100644 --- a/src/boards/Payload/Configs/PinHandlerConfig.h +++ b/src/boards/Payload/Configs/PinHandlerConfig.h @@ -24,6 +24,8 @@ #include <utils/PinObserver/PinObserver.h> +#include <chrono> + namespace Payload { namespace Config @@ -31,13 +33,25 @@ namespace Config namespace PinHandler { +/* linter-off */ using namespace std::chrono_literals; + +namespace PinObserver +{ +constexpr auto PERIOD = 20ms; +} + namespace NoseconeDetach { constexpr auto DETECTION_THRESHOLD = 20; -constexpr auto PERIOD = 20; // [ms] constexpr auto TRIGGERING_TRANSITION = Boardcore::PinTransition::RISING_EDGE; } // namespace NoseconeDetach +namespace ExpulsionSense +{ +constexpr auto DETECTION_THRESHOLD = 20; +constexpr auto TRIGGERING_TRANSITION = Boardcore::PinTransition::RISING_EDGE; +} // namespace ExpulsionSense + } // namespace PinHandler } // namespace Config } // namespace Payload diff --git a/src/boards/Payload/Configs/RadioConfig.h b/src/boards/Payload/Configs/RadioConfig.h index 8bbf64ff9d3684f475157b3f41209ae188be0f10..6579f707466c1500f4d839ba7ed68b3c2fb06f86 100644 --- a/src/boards/Payload/Configs/RadioConfig.h +++ b/src/boards/Payload/Configs/RadioConfig.h @@ -1,5 +1,5 @@ -/* Copyright (c) 2023 Skyward Experimental Rocketry - * Author: Matteo Pignataro +/* Copyright (c) 2024 Skyward Experimental Rocketry + * Author: Niccolò Betto * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -21,26 +21,41 @@ */ #pragma once -#include <common/MavlinkGemini.h> +#include <common/Mavlink.h> +#include <units/Frequency.h> + +#include <chrono> namespace Payload { -namespace RadioConfig +namespace Config +{ +namespace Radio { -// Mavlink driver template parameters -constexpr uint32_t RADIO_PKT_LENGTH = 255; -constexpr uint32_t RADIO_OUT_QUEUE_SIZE = 20; -constexpr uint32_t RADIO_MAV_MSG_LENGTH = MAVLINK_MAX_DIALECT_PAYLOAD_SIZE; -constexpr uint32_t RADIO_PERIODIC_TELEMETRY_PERIOD = 200; +/* linter off */ using namespace Boardcore::Units::Frequency; +/* linter off */ using namespace std::chrono_literals; -constexpr uint16_t RADIO_SLEEP_AFTER_SEND = 50; -constexpr size_t RADIO_OUT_BUFFER_MAX_AGE = 10; +constexpr auto LOW_RATE_TELEMETRY = 2_hz; +constexpr auto HIGH_RATE_TELEMETRY = 4_hz; +constexpr auto MESSAGE_QUEUE_SIZE = 10; +constexpr auto MESSAGE_QUEUE_FLUSH_PERIOD = 200ms; -constexpr uint8_t MAV_SYSTEM_ID = 171; -constexpr uint8_t MAV_COMP_ID = 96; +namespace Mavlink +{ +constexpr uint8_t SYSTEM_ID = 171; +constexpr uint8_t COMPONENT_ID = 96; +} // namespace Mavlink -constexpr uint32_t MAVLINK_QUEUE_SIZE = 10; +namespace MavlinkDriver +{ +constexpr auto PKT_LENGTH = 255; +constexpr auto PKT_QUEUE_SIZE = 20; +constexpr auto MSG_LENGTH = MAVLINK_MAX_DIALECT_PAYLOAD_SIZE; +constexpr auto SLEEP_AFTER_SEND = 0ms; +constexpr auto MAX_PKT_AGE = 10ms; +} // namespace MavlinkDriver -} // namespace RadioConfig +} // namespace Radio +} // namespace Config } // namespace Payload diff --git a/src/boards/Payload/Configs/SensorsConfig.h b/src/boards/Payload/Configs/SensorsConfig.h index 08fe426a075d665045217512651af78d8e52855e..a1b315208de6e15b6ab44ac9082f80da5df1c426 100644 --- a/src/boards/Payload/Configs/SensorsConfig.h +++ b/src/boards/Payload/Configs/SensorsConfig.h @@ -41,11 +41,8 @@ namespace Config namespace Sensors { -// clang-format off -// Indent to avoid the linter complaining about using namespace - using namespace Boardcore::Units::Frequency; - using namespace std::chrono_literals; -// clang-format on +/* linter off */ using namespace Boardcore::Units::Frequency; +/* linter off */ using namespace std::chrono_literals; namespace LPS22DF { @@ -109,11 +106,6 @@ constexpr auto SAMPLING_RATE = 100_hz; constexpr auto OVERSAMPLING_RATIO = Boardcore::ADS131M08Defs::OversamplingRatio::OSR_8192; constexpr bool GLOBAL_CHOP_MODE = true; -// TODO: populate with final channels once sensors are soldered -constexpr auto STATIC_PRESSURE_CH = - Boardcore::ADS131M08Defs::Channel::CHANNEL_0; -constexpr auto DYNAMIC_PRESSURE_CH = - Boardcore::ADS131M08Defs::Channel::CHANNEL_1; } // namespace ADS131M08 namespace InternalADC @@ -131,12 +123,16 @@ namespace StaticPressure { constexpr auto ENABLED = true; constexpr auto SAMPLING_RATE = ADS131M08::SAMPLING_RATE; +constexpr auto ADC_CH = Boardcore::ADS131M08Defs::Channel::CHANNEL_0; +constexpr auto SCALE = (38300.0f + 13000.0f) / 13000.0f; } // namespace StaticPressure namespace DynamicPressure { constexpr auto ENABLED = true; constexpr auto SAMPLING_RATE = ADS131M08::SAMPLING_RATE; +constexpr auto ADC_CH = Boardcore::ADS131M08Defs::Channel::CHANNEL_1; +constexpr auto SCALE = (38300.0f + 13000.0f) / 13000.0f; } // namespace DynamicPressure namespace Pitot diff --git a/src/boards/Payload/FlightStatsRecorder/FlightStatsRecorder.cpp b/src/boards/Payload/FlightStatsRecorder/FlightStatsRecorder.cpp index 3d5b103409bf03b25b3726cf621cb8e69a46afb3..ae0cba716635762a1e3660cdc0e48feda0e00b07 100644 --- a/src/boards/Payload/FlightStatsRecorder/FlightStatsRecorder.cpp +++ b/src/boards/Payload/FlightStatsRecorder/FlightStatsRecorder.cpp @@ -73,7 +73,7 @@ void FlightStatsRecorder::update() // Data gathering Sensors* sensor = getModule<Sensors>(); AccelerometerData accData = sensor->getIMULastSample().accData; - PressureData baroData = sensor->getStaticPressureLastSample(); + PressureData baroData = sensor->getStaticPressure(); PitotData pitotData = sensor->getPitotLastSample(); GPSData gpsData = sensor->getUBXGPSLastSample(); NASState nasData = getModule<NASController>()->getNasState(); diff --git a/src/boards/Payload/PinHandler/PinHandler.cpp b/src/boards/Payload/PinHandler/PinHandler.cpp index 16ea05c5d90fa53f1c24457c7942507ed855d200..6ca5156517dc570795886895d578bbba280ce298 100644 --- a/src/boards/Payload/PinHandler/PinHandler.cpp +++ b/src/boards/Payload/PinHandler/PinHandler.cpp @@ -40,43 +40,67 @@ namespace Payload bool PinHandler::start() { + using namespace std::chrono; + auto& scheduler = getModule<BoardScheduler>()->pinHandler(); - pinObserver = std::make_unique<PinObserver>(scheduler, - config::NoseconeDetach::PERIOD); + pinObserver = std::make_unique<PinObserver>( + scheduler, milliseconds{config::PinObserver::PERIOD}.count()); - bool pinResult = pinObserver->registerPinCallback( + bool detachPayloadResult = pinObserver->registerPinCallback( hwmap::detachPayload::getPin(), - [this](auto t) { onDetachPinTransition(t); }, + [this](auto t) { onDetachPayloadTransition(t); }, config::NoseconeDetach::DETECTION_THRESHOLD); - if (!pinResult) + if (!detachPayloadResult) { + LOG_ERR(logger, + "Failed to register pin callback for the detach payload pin"); return false; } - started = true; + bool expSenseResult = pinObserver->registerPinCallback( + hwmap::expulsionSense::getPin(), + [this](auto t) { onExpulsionSenseTransition(t); }, + config::ExpulsionSense::DETECTION_THRESHOLD); + + if (!expSenseResult) + { + LOG_ERR(logger, + "Failed to register pin callback for the expulsion sense pin"); + return false; + } + started = true; return true; } bool PinHandler::isStarted() { return started; } -std::map<PinsList, PinData> PinHandler::getPinData() +void PinHandler::onDetachPayloadTransition(PinTransition transition) { - std::map<PinsList, PinData> data; - - data[PinsList::NOSECONE_PIN] = - pinObserver->getPinData(hwmap::detachPayload::getPin()); + // TODO: send event + LOG_INFO(logger, "onDetachPayloadTransition {}", + static_cast<int>(transition)); +} - return data; +void PinHandler::onExpulsionSenseTransition(PinTransition transition) +{ + // TODO: send event + LOG_INFO(logger, "onExpulsionSenseTransition {}", + static_cast<int>(transition)); } -void PinHandler::onDetachPinTransition(PinTransition transition) +PinData PinHandler::getPinData(PinList pin) { - if (transition == config::NoseconeDetach::TRIGGERING_TRANSITION) + switch (pin) { - EventBroker::getInstance().post(FLIGHT_NC_DETACHED, TOPIC_FLIGHT); + case PinList::DETACH_PAYLOAD_PIN: + return pinObserver->getPinData(hwmap::detachPayload::getPin()); + case PinList::EXPULSION_SENSE: + return pinObserver->getPinData(hwmap::expulsionSense::getPin()); + default: + return PinData{}; } } diff --git a/src/boards/Payload/PinHandler/PinHandler.h b/src/boards/Payload/PinHandler/PinHandler.h index e274b17af529d659c4bbb2d8987535ccc0159f0a..8d957cc91b4c1ddc40ecce624fb269ce227d21f2 100644 --- a/src/boards/Payload/PinHandler/PinHandler.h +++ b/src/boards/Payload/PinHandler/PinHandler.h @@ -31,6 +31,12 @@ namespace Payload { class BoardScheduler; +enum class PinList : uint8_t +{ + DETACH_PAYLOAD_PIN = 2, + EXPULSION_SENSE = 3, +}; + /** * @brief This class contains the handlers for the detach pins on the rocket. * @@ -40,24 +46,22 @@ class BoardScheduler; class PinHandler : public Boardcore::InjectableWithDeps<BoardScheduler> { public: - bool start(); + [[nodiscard]] bool start(); bool isStarted(); /** - * @brief Returns information about all pins handled by this class + * @brief Returns information about the specified pin. */ - std::map<PinsList, Boardcore::PinData> getPinData(); + Boardcore::PinData getPinData(PinList pin); private: - /** - * @brief Detach pin transition handler. - */ - void onDetachPinTransition(Boardcore::PinTransition transition); + void onDetachPayloadTransition(Boardcore::PinTransition transition); + void onExpulsionSenseTransition(Boardcore::PinTransition transition); std::unique_ptr<Boardcore::PinObserver> pinObserver; - bool started = false; + std::atomic<bool> started{false}; Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("PinHandler"); }; diff --git a/src/boards/Payload/Radio/MessageHandler.cpp b/src/boards/Payload/Radio/MessageHandler.cpp new file mode 100644 index 0000000000000000000000000000000000000000..bed62957684b5a9ee4c2dc104250f6ef51d3f2a4 --- /dev/null +++ b/src/boards/Payload/Radio/MessageHandler.cpp @@ -0,0 +1,652 @@ +/* Copyright (c) 2024 Skyward Experimental Rocketry + * Author: Niccolò Betto + * + * 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 <Payload/Actuators/Actuators.h> +#include <Payload/AltitudeTrigger/AltitudeTrigger.h> +#include <Payload/BoardScheduler.h> +#include <Payload/CanHandler/CanHandler.h> +#include <Payload/PinHandler/PinHandler.h> +#include <Payload/Sensors/Sensors.h> +#include <Payload/StateMachines/FlightModeManager/FlightModeManager.h> +#include <Payload/StateMachines/NASController/NASController.h> +#include <Payload/StateMachines/WingController/WingController.h> +#include <common/Events.h> +#include <common/Topics.h> +#include <diagnostic/CpuMeter/CpuMeter.h> +#include <events/EventBroker.h> + +#include "Radio.h" + +using namespace Boardcore; +using namespace Common; +namespace config = Payload::Config::Radio; + +namespace Payload +{ + +void Radio::handleMessage(const mavlink_message_t& msg) +{ + switch (msg.msgid) + { + + case MAVLINK_MSG_ID_PING_TC: + { + return enqueueAck(msg); + } + + case MAVLINK_MSG_ID_COMMAND_TC: + { + return handleCommand(msg); + } + + case MAVLINK_MSG_ID_SYSTEM_TM_REQUEST_TC: + { + auto tmId = static_cast<SystemTMList>( + mavlink_msg_system_tm_request_tc_get_tm_id(&msg)); + + if (!enqueueSystemTm(tmId)) + { + return enqueueNack(msg); + } + + return enqueueAck(msg); + } + + case MAVLINK_MSG_ID_SENSOR_TM_REQUEST_TC: + { + auto sensorId = static_cast<SensorsTMList>( + mavlink_msg_sensor_tm_request_tc_get_sensor_name(&msg)); + + if (!enqueueSensorsTm(sensorId)) + { + return enqueueNack(msg); + } + + return enqueueAck(msg); + } + + case MAVLINK_MSG_ID_WIGGLE_SERVO_TC: + { + bool testMode = getModule<FlightModeManager>()->getStatus().state == + FlightModeManagerState::TEST_MODE; + + // Perform the wiggle in test mode only + if (!testMode) + { + return enqueueNack(msg); + } + + auto servo = static_cast<ServosList>( + mavlink_msg_wiggle_servo_tc_get_servo_id(&msg)); + + getModule<Actuators>()->wiggleServo(servo); + return enqueueAck(msg); + } + + case MAVLINK_MSG_ID_RAW_EVENT_TC: + { + uint8_t topicId = mavlink_msg_raw_event_tc_get_topic_id(&msg); + uint8_t eventId = mavlink_msg_raw_event_tc_get_event_id(&msg); + + bool testMode = getModule<FlightModeManager>()->getStatus().state == + FlightModeManagerState::TEST_MODE; + + // Raw events are allowed in test mode only + if (!testMode) + { + return enqueueNack(msg); + } + + EventBroker::getInstance().post(topicId, eventId); + return enqueueAck(msg); + } + + default: + { + return enqueueNack(msg); + } + } +} + +void Radio::handleCommand(const mavlink_message_t& msg) +{ + // Command-to-event look-up table, uses operator[] to retrieve an event from + // a command + static struct + { + Events operator[](MavCommandList command) + { +#define MAP_CMD(command, event) \ + case command: \ + return event + + switch (command) + { + MAP_CMD(MAV_CMD_ARM, TMTC_ARM); + MAP_CMD(MAV_CMD_DISARM, TMTC_DISARM); + MAP_CMD(MAV_CMD_CALIBRATE, TMTC_CALIBRATE); + MAP_CMD(MAV_CMD_FORCE_INIT, TMTC_FORCE_INIT); + MAP_CMD(MAV_CMD_FORCE_LAUNCH, TMTC_FORCE_LAUNCH); + MAP_CMD(MAV_CMD_FORCE_LANDING, TMTC_FORCE_LANDING); + MAP_CMD(MAV_CMD_FORCE_EXPULSION, TMTC_FORCE_EXPULSION); + MAP_CMD(MAV_CMD_FORCE_DEPLOYMENT, TMTC_FORCE_DEPLOYMENT); + MAP_CMD(MAV_CMD_FORCE_REBOOT, TMTC_RESET_BOARD); + MAP_CMD(MAV_CMD_ENTER_TEST_MODE, TMTC_ENTER_TEST_MODE); + MAP_CMD(MAV_CMD_EXIT_TEST_MODE, TMTC_EXIT_TEST_MODE); + MAP_CMD(MAV_CMD_START_RECORDING, TMTC_START_RECORDING); + MAP_CMD(MAV_CMD_STOP_RECORDING, TMTC_STOP_RECORDING); + default: + return LAST_EVENT; + } +#undef MAP_CMD + } + } commandToEvent; // Command-to-event look-up table + + auto command = static_cast<MavCommandList>( + mavlink_msg_command_tc_get_command_id(&msg)); + + switch (command) + { + case MAV_CMD_START_LOGGING: + { + bool started = Logger::getInstance().start(); + if (!started) + { + return enqueueNack(msg); + } + + Logger::getInstance().resetStats(); + return enqueueAck(msg); + } + + case MAV_CMD_STOP_LOGGING: + { + Logger::getInstance().stop(); + return enqueueAck(msg); + } + + case MAV_CMD_SAVE_CALIBRATION: + { + bool magResult = getModule<Sensors>()->writeMagCalibration(); + if (magResult) + { + return enqueueAck(msg); + } + else + { + return enqueueNack(msg); + } + } + + default: + { + // Map the command to an event and post it, if it exists + auto event = commandToEvent[command]; + + if (event == LAST_EVENT) + { + return enqueueNack(msg); + } + + EventBroker::getInstance().post(event, TOPIC_TMTC); + return enqueueAck(msg); + } + } +} + +bool Radio::enqueueSystemTm(SystemTMList tmId) +{ + switch (tmId) + { + case MAV_SENSORS_STATE_ID: + { + auto sensors = getModule<Sensors>()->getSensorInfo(); + for (auto sensor : sensors) + { + mavlink_message_t msg; + mavlink_sensor_state_tm_t tm; + + strcpy(tm.sensor_name, sensor.id.c_str()); + tm.initialized = sensor.isInitialized ? 1 : 0; + tm.enabled = sensor.isEnabled ? 1 : 0; + + mavlink_msg_sensor_state_tm_encode( + config::Mavlink::SYSTEM_ID, config::Mavlink::COMPONENT_ID, + &msg, &tm); + enqueueMessage(msg); + } + + return true; + } + + case MAV_SYS_ID: + { + mavlink_message_t msg; + mavlink_sys_tm_t tm; + + tm.timestamp = TimestampTimer::getTimestamp(); + tm.logger = Logger::getInstance().isStarted() ? 1 : 0; + tm.event_broker = EventBroker::getInstance().isRunning() ? 1 : 0; + tm.radio = isStarted() ? 1 : 0; + tm.sensors = getModule<Sensors>()->isStarted() ? 1 : 0; + tm.actuators = 0; // TODO: implement isStarted + tm.pin_handler = getModule<PinHandler>()->isStarted() ? 1 : 0; + tm.can_handler = getModule<CanHandler>()->isStarted() ? 1 : 0; + tm.scheduler = getModule<BoardScheduler>()->isStarted() ? 1 : 0; + + mavlink_msg_sys_tm_encode(config::Mavlink::SYSTEM_ID, + config::Mavlink::COMPONENT_ID, &msg, &tm); + enqueueMessage(msg); + return true; + } + + case MAV_LOGGER_ID: + { + mavlink_message_t msg; + mavlink_logger_tm_t tm; + + // Get the logger stats + LoggerStats stats = Logger::getInstance().getStats(); + + tm.timestamp = stats.timestamp; + tm.log_number = stats.logNumber; + tm.too_large_samples = stats.tooLargeSamples; + tm.dropped_samples = stats.droppedSamples; + tm.queued_samples = stats.queuedSamples; + tm.buffers_filled = stats.buffersFilled; + tm.buffers_written = stats.buffersWritten; + tm.writes_failed = stats.writesFailed; + tm.last_write_error = stats.lastWriteError; + tm.average_write_time = stats.averageWriteTime; + tm.max_write_time = stats.maxWriteTime; + + mavlink_msg_logger_tm_encode(config::Mavlink::SYSTEM_ID, + config::Mavlink::COMPONENT_ID, &msg, + &tm); + enqueueMessage(msg); + return true; + } + + case MAV_MAVLINK_STATS_ID: + { + mavlink_message_t msg; + mavlink_mavlink_stats_tm_t tm; + + // Get the mavlink stats + MavlinkStatus stats = mavDriver->getStatus(); + + tm.timestamp = stats.timestamp; + tm.n_send_queue = stats.nSendQueue; + tm.max_send_queue = stats.maxSendQueue; + tm.n_send_errors = stats.nSendErrors; + tm.msg_received = stats.mavStats.msg_received; + tm.buffer_overrun = stats.mavStats.buffer_overrun; + tm.parse_error = stats.mavStats.parse_error; + tm.parse_state = stats.mavStats.parse_state; + tm.packet_idx = stats.mavStats.packet_idx; + tm.current_rx_seq = stats.mavStats.current_rx_seq; + tm.current_tx_seq = stats.mavStats.current_tx_seq; + tm.packet_rx_success_count = stats.mavStats.packet_rx_success_count; + tm.packet_rx_drop_count = stats.mavStats.packet_rx_drop_count; + + mavlink_msg_mavlink_stats_tm_encode(config::Mavlink::SYSTEM_ID, + config::Mavlink::COMPONENT_ID, + &msg, &tm); + enqueueMessage(msg); + return true; + } + + case MAV_FLIGHT_ID: + { + mavlink_message_t msg; + mavlink_payload_flight_tm_t tm; + + Sensors* sensors = getModule<Sensors>(); + PinHandler* pinHandler = getModule<PinHandler>(); + + auto imu = sensors->getLSM6DSRXLastSample(); + auto mag = sensors->getLIS2MDLLastSample(); + auto gps = sensors->getUBXGPSLastSample(); + auto pressDigi = sensors->getLPS22DFLastSample(); + auto pressStatic = sensors->getStaticPressure(); + auto pressPitot = sensors->getPitotLastSample(); + + tm.timestamp = TimestampTimer::getTimestamp(); + tm.pressure_digi = pressDigi.pressure; + tm.pressure_static = pressStatic.pressure; + tm.airspeed_pitot = pressPitot.airspeed; + tm.altitude_agl = -1.0f; // TODO + + // Sensors + tm.acc_x = imu.accelerationX; + tm.acc_y = imu.accelerationY; + tm.acc_z = imu.accelerationZ; + tm.gyro_x = imu.angularSpeedX; + tm.gyro_y = imu.angularSpeedY; + tm.gyro_z = imu.angularSpeedZ; + tm.mag_x = mag.magneticFieldX; + tm.mag_y = mag.magneticFieldY; + tm.mag_z = mag.magneticFieldZ; + tm.gps_lat = gps.latitude; + tm.gps_lon = gps.longitude; + tm.gps_alt = gps.height; + tm.gps_fix = gps.fix; + + // Servos + tm.left_servo_angle = getModule<Actuators>()->getServoAngle( + ServosList::PARAFOIL_LEFT_SERVO); + tm.right_servo_angle = getModule<Actuators>()->getServoAngle( + ServosList::PARAFOIL_RIGHT_SERVO); + + // Algorithms + tm.nas_n = -1.0f; // TODO + tm.nas_e = -1.0f; // TODO + tm.nas_d = -1.0f; // TODO + tm.nas_vn = -1.0f; // TODO + tm.nas_ve = -1.0f; // TODO + tm.nas_vd = -1.0f; // TODO + tm.nas_qx = -1.0f; // TODO + tm.nas_qy = -1.0f; // TODO + tm.nas_qz = -1.0f; // TODO + tm.nas_qw = -1.0f; // TODO + tm.nas_bias_x = -1.0f; // TODO + tm.nas_bias_y = -1.0f; // TODO + tm.nas_bias_z = -1.0f; // TODO + tm.wes_n = -1.0; // TODO + tm.wes_e = -1.0; // TODO + + tm.battery_voltage = sensors->getBatteryVoltage().voltage; + tm.cam_battery_voltage = sensors->getCamBatteryVoltage().voltage; + tm.temperature = pressDigi.temperature; + + // State machines + tm.fmm_state = static_cast<uint8_t>( + getModule<FlightModeManager>()->getStatus().state); + tm.nas_state = 255; // TODO + tm.wes_state = 255; // TODO + + // Pins + tm.pin_launch = + pinHandler->getPinData(PinList::DETACH_PAYLOAD_PIN).lastState + ? 1 + : 0; + tm.pin_nosecone = + pinHandler->getPinData(PinList::EXPULSION_SENSE).lastState ? 1 + : 0; + tm.cutter_presence = 255; // TODO + + mavlink_msg_payload_flight_tm_encode(config::Mavlink::SYSTEM_ID, + config::Mavlink::COMPONENT_ID, + &msg, &tm); + enqueueMessage(msg); + return true; + } + + case MAV_STATS_ID: + { + mavlink_message_t msg; + mavlink_payload_stats_tm_t tm; + + tm.liftoff_max_acc = -1.0f; // TODO + tm.liftoff_max_acc_ts = 0; // TODO + tm.dpl_ts = 0; // TODO + tm.dpl_max_acc = -1.0f; // TODO + tm.max_z_speed = -1.0f; // TODO + tm.max_z_speed_ts = 0; // TODO + tm.max_airspeed_pitot = -1.0f; // TODO + tm.max_speed_altitude = -1.0f; // TODO + tm.apogee_ts = 0; // TODO + tm.apogee_lat = -1.0f; // TODO + tm.apogee_lon = -1.0f; // TODO + tm.apogee_alt = -1.0f; // TODO + tm.min_pressure = -1.0f; // TODO + + // Cpu stuff + CpuMeterData cpuStats = CpuMeter::getCpuStats(); + CpuMeter::resetCpuStats(); + tm.cpu_load = cpuStats.mean; + tm.free_heap = cpuStats.freeHeap; + + // Log stuff + LoggerStats loggerStats = Logger::getInstance().getStats(); + tm.log_good = (loggerStats.lastWriteError == 0) ? 1 : 0; + tm.log_number = loggerStats.logNumber; + + tm.main_board_state = 0; // TODO + tm.motor_board_state = 0; // TODO + tm.main_can_status = 0; // TODO + tm.motor_can_status = 0; // TODO + tm.rig_can_status = 0; // TODO + + tm.hil_state = 0; // TODO + + mavlink_msg_payload_stats_tm_encode(config::Mavlink::SYSTEM_ID, + config::Mavlink::COMPONENT_ID, + &msg, &tm); + enqueueMessage(msg); + return true; + } + + default: + return false; + } +} + +bool Radio::enqueueSensorsTm(SensorsTMList tmId) +{ + switch (tmId) + { + case MAV_GPS_ID: + { + mavlink_message_t msg; + mavlink_gps_tm_t tm; + + auto sample = getModule<Sensors>()->getUBXGPSLastSample(); + + tm.fix = sample.fix; + tm.height = sample.height; + tm.latitude = sample.latitude; + tm.longitude = sample.longitude; + tm.n_satellites = sample.satellites; + tm.speed = sample.speed; + tm.timestamp = sample.gpsTimestamp; + tm.track = sample.track; + tm.vel_down = sample.velocityDown; + tm.vel_east = sample.velocityEast; + tm.vel_north = sample.velocityNorth; + strcpy(tm.sensor_name, "UBXGPS"); + + mavlink_msg_gps_tm_encode(config::Mavlink::SYSTEM_ID, + config::Mavlink::COMPONENT_ID, &msg, &tm); + enqueueMessage(msg); + return true; + } + + case MAV_ADS131M08_ID: + { + mavlink_message_t msg; + mavlink_adc_tm_t tm; + + auto sample = getModule<Sensors>()->getADS131M08LastSample(); + + tm.channel_0 = + sample.getVoltage(ADS131M08Defs::Channel::CHANNEL_0).voltage; + tm.channel_1 = + sample.getVoltage(ADS131M08Defs::Channel::CHANNEL_1).voltage; + tm.channel_2 = + sample.getVoltage(ADS131M08Defs::Channel::CHANNEL_2).voltage; + tm.channel_3 = + sample.getVoltage(ADS131M08Defs::Channel::CHANNEL_3).voltage; + tm.channel_4 = + sample.getVoltage(ADS131M08Defs::Channel::CHANNEL_4).voltage; + tm.channel_5 = + sample.getVoltage(ADS131M08Defs::Channel::CHANNEL_5).voltage; + tm.channel_6 = + sample.getVoltage(ADS131M08Defs::Channel::CHANNEL_6).voltage; + tm.channel_7 = + sample.getVoltage(ADS131M08Defs::Channel::CHANNEL_7).voltage; + tm.timestamp = sample.timestamp; + strcpy(tm.sensor_name, "ADS131M08"); + + mavlink_msg_adc_tm_encode(config::Mavlink::SYSTEM_ID, + config::Mavlink::COMPONENT_ID, &msg, &tm); + enqueueMessage(msg); + return true; + } + + case MAV_BATTERY_VOLTAGE_ID: + { + mavlink_message_t msg; + mavlink_voltage_tm_t tm; + + auto data = getModule<Sensors>()->getBatteryVoltage(); + + tm.voltage = data.voltage; + tm.timestamp = data.voltageTimestamp; + strcpy(tm.sensor_name, "BATTERY_VOLTAGE"); + + mavlink_msg_voltage_tm_encode(config::Mavlink::SYSTEM_ID, + config::Mavlink::COMPONENT_ID, &msg, + &tm); + enqueueMessage(msg); + return true; + } + + case MAV_LPS28DFW_ID: + { + mavlink_message_t msg; + mavlink_pressure_tm_t tm; + + auto sample = getModule<Sensors>()->getLPS28DFWLastSample(); + + tm.pressure = sample.pressure; + tm.timestamp = sample.pressureTimestamp; + strcpy(tm.sensor_name, "LPS28DFW"); + + mavlink_msg_pressure_tm_encode(config::Mavlink::SYSTEM_ID, + config::Mavlink::COMPONENT_ID, &msg, + &tm); + + enqueueMessage(msg); + return true; + } + + case MAV_LPS22DF_ID: + { + mavlink_message_t msg; + mavlink_pressure_tm_t tm; + + auto sample = getModule<Sensors>()->getLPS22DFLastSample(); + + tm.pressure = sample.pressure; + tm.timestamp = sample.pressureTimestamp; + strcpy(tm.sensor_name, "LPS22DF"); + + mavlink_msg_pressure_tm_encode(config::Mavlink::SYSTEM_ID, + config::Mavlink::COMPONENT_ID, &msg, + &tm); + + enqueueMessage(msg); + return true; + } + + case MAV_LIS2MDL_ID: + { + mavlink_message_t msg; + mavlink_imu_tm_t tm; + + auto sample = getModule<Sensors>()->getLIS2MDLLastSample(); + + tm.acc_x = 0; + tm.acc_y = 0; + tm.acc_z = 0; + tm.gyro_x = 0; + tm.gyro_y = 0; + tm.gyro_z = 0; + tm.mag_x = sample.magneticFieldX; + tm.mag_y = sample.magneticFieldY; + tm.mag_z = sample.magneticFieldZ; + tm.timestamp = sample.magneticFieldTimestamp; + strcpy(tm.sensor_name, "LIS2MDL"); + + mavlink_msg_imu_tm_encode(config::Mavlink::SYSTEM_ID, + config::Mavlink::COMPONENT_ID, &msg, &tm); + enqueueMessage(msg); + return true; + } + + case MAV_LSM6DSRX_ID: + { + mavlink_message_t msg; + mavlink_imu_tm_t tm; + + auto sample = getModule<Sensors>()->getLSM6DSRXLastSample(); + + tm.mag_x = 0; + tm.mag_y = 0; + tm.mag_z = 0; + tm.acc_x = sample.accelerationX; + tm.acc_y = sample.accelerationY; + tm.acc_z = sample.accelerationZ; + tm.gyro_x = sample.angularSpeedX; + tm.gyro_y = sample.angularSpeedY; + tm.gyro_z = sample.angularSpeedZ; + tm.timestamp = sample.accelerationTimestamp; + strcpy(tm.sensor_name, "LSM6DSRX"); + + mavlink_msg_imu_tm_encode(config::Mavlink::SYSTEM_ID, + config::Mavlink::COMPONENT_ID, &msg, &tm); + enqueueMessage(msg); + return true; + } + + case MAV_H3LIS331DL_ID: + { + mavlink_message_t msg; + mavlink_imu_tm_t tm; + + auto sample = getModule<Sensors>()->getH3LIS331DLLastSample(); + + tm.mag_x = 0; + tm.mag_y = 0; + tm.mag_z = 0; + tm.gyro_x = 0; + tm.gyro_y = 0; + tm.gyro_z = 0; + tm.acc_x = sample.accelerationX; + tm.acc_y = sample.accelerationY; + tm.acc_z = sample.accelerationZ; + tm.timestamp = sample.accelerationTimestamp; + strcpy(tm.sensor_name, "H3LIS331DL"); + + mavlink_msg_imu_tm_encode(config::Mavlink::SYSTEM_ID, + config::Mavlink::COMPONENT_ID, &msg, &tm); + enqueueMessage(msg); + return true; + } + + default: + return false; + } +} + +} // namespace Payload diff --git a/src/boards/Payload/Radio/Radio.cpp b/src/boards/Payload/Radio/Radio.cpp index 23a40a77db9f3aa0fce8d7b5c61676ea2645ff16..c51d6d93669bc29ef99de17b0759d18ca8b4b352 100644 --- a/src/boards/Payload/Radio/Radio.cpp +++ b/src/boards/Payload/Radio/Radio.cpp @@ -1,5 +1,5 @@ -/* Copyright (c) 2023 Skyward Experimental Rocketry - * Author: Matteo Pignataro, Federico Mandelli +/* Copyright (c) 2024 Skyward Experimental Rocketry + * Author: Niccolò Betto * * 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,504 +19,170 @@ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * THE SOFTWARE. */ -#include <Payload/Actuators/Actuators.h> -#include <Payload/AltitudeTrigger/AltitudeTrigger.h> + #include <Payload/BoardScheduler.h> #include <Payload/Buses.h> #include <Payload/Radio/Radio.h> -#include <Payload/Radio/RadioData.h> -#include <Payload/Sensors/Sensors.h> -#include <Payload/StateMachines/FlightModeManager/FlightModeManager.h> -#include <Payload/StateMachines/NASController/NASController.h> -#include <Payload/StateMachines/WingController/WingController.h> -#include <Payload/TMRepository/TMRepository.h> -#include <common/Events.h> -#include <common/Topics.h> -#include <drivers/interrupt/external_interrupts.h> -#include <events/EventBroker.h> +#include <common/Radio.h> #include <radio/SX1278/SX1278Frontends.h> -#include <memory> - using namespace Boardcore; using namespace Common; +namespace config = Payload::Config::Radio; -#define SX1278_IRQ_DIO0 EXTI3_IRQHandlerImpl -#define SX1278_IRQ_DIO1 EXTI4_IRQHandlerImpl -#define SX1278_IRQ_DIO3 EXTI5_IRQHandlerImpl +namespace +{ -static Payload::Radio* staticRadio = nullptr; +// Static radio instance that will handle the radio interrupts +SX1278Fsk* staticTransceiver = nullptr; -void __attribute__((used)) SX1278_IRQ_DIO0() +inline void handleDioIRQ() { - if (staticRadio && staticRadio->transceiver) + if (staticTransceiver) { - staticRadio->transceiver->handleDioIRQ(); + staticTransceiver->handleDioIRQ(); } } -void __attribute__((used)) SX1278_IRQ_DIO1() +} // namespace + +void __attribute__((used)) MIOSIX_RADIO_DIO0_IRQ() { handleDioIRQ(); } +void __attribute__((used)) MIOSIX_RADIO_DIO1_IRQ() { handleDioIRQ(); } +void __attribute__((used)) MIOSIX_RADIO_DIO3_IRQ() { handleDioIRQ(); } + +namespace Payload { - if (staticRadio && staticRadio->transceiver) - { - staticRadio->transceiver->handleDioIRQ(); - } -} -void __attribute__((used)) SX1278_IRQ_DIO3() +Radio::~Radio() { - if (staticRadio && staticRadio->transceiver) + if (staticTransceiver == transceiver.get()) { - staticRadio->transceiver->handleDioIRQ(); + staticTransceiver = nullptr; } } -namespace Payload -{ - -Radio::Radio() { staticRadio = this; } - bool Radio::start() { + using namespace Units::Frequency; + using namespace std::chrono; + auto& scheduler = getModule<BoardScheduler>()->radio(); - // Config the transceiver - SX1278Fsk::Config config = { - .freq_rf = 869000000, - .freq_dev = 50000, - .bitrate = 48000, - .rx_bw = Boardcore::SX1278Fsk::Config::RxBw::HZ_125000, - .afc_bw = Boardcore::SX1278Fsk::Config::RxBw::HZ_125000, - .ocp = 120, - .power = 13, - .shaping = Boardcore::SX1278Fsk::Config::Shaping::GAUSSIAN_BT_1_0, - .dc_free = Boardcore::SX1278Fsk::Config::DcFree::WHITENING, - .enable_crc = false}; - - std::unique_ptr<SX1278::ISX1278Frontend> frontend = - std::make_unique<Skyward433Frontend>(); - - transceiver = new SX1278Fsk( + // Initialize the radio + auto frontend = std::make_unique<Skyward433Frontend>(); + + transceiver = std::make_unique<SX1278Fsk>( getModule<Buses>()->radio(), miosix::radio::cs::getPin(), miosix::radio::dio0::getPin(), miosix::radio::dio1::getPin(), miosix::radio::dio3::getPin(), SPI::ClockDivider::DIV_128, std::move(frontend)); - // Config the radio - SX1278Fsk::Error error = transceiver->init(config); + // Set the static instance for handling radio interrupts + staticTransceiver = transceiver.get(); + + // Configure the radio + if (transceiver->init(PAYLOAD_RADIO_CONFIG) != SX1278Fsk::Error::NONE) + { + LOG_ERR(logger, "Failed to initialize the radio"); + return false; + } + + // Initialize the Mavlink driver + mavDriver = std::make_unique<MavDriver>( + transceiver.get(), + [this](MavDriver*, const mavlink_message_t& msg) + { handleMessage(msg); }, + milliseconds{config::MavlinkDriver::SLEEP_AFTER_SEND}.count(), + milliseconds{config::MavlinkDriver::MAX_PKT_AGE}.count()); - // Add periodic telemetry send task - uint8_t result = - scheduler.addTask([&]() { this->sendPeriodicMessage(); }, - RadioConfig::RADIO_PERIODIC_TELEMETRY_PERIOD, - TaskScheduler::Policy::RECOVER); - result *= scheduler.addTask( - [&]() + if (!mavDriver->start()) + { + LOG_ERR(logger, "Failed to initialize the Mavlink driver"); + return false; + } + + // Add the high rate telemetry task + auto highRateTask = scheduler.addTask( + [this]() { - this->enqueueMsg( - getModule<TMRepository>()->packSystemTm(MAV_STATS_ID, 0, 0)); + enqueueHighRateTelemetry(); + flushMessageQueue(); }, - RadioConfig::RADIO_PERIODIC_TELEMETRY_PERIOD * 2, - TaskScheduler::Policy::RECOVER); - - // Config mavDriver - mavDriver = new MavDriver( - transceiver, - [=](MavDriver*, const mavlink_message_t& msg) - { this->handleMavlinkMessage(msg); }, - RadioConfig::RADIO_SLEEP_AFTER_SEND, - RadioConfig::RADIO_OUT_BUFFER_MAX_AGE); - - // Check radio failure - if (error != SX1278Fsk::Error::NONE) + Config::Radio::HIGH_RATE_TELEMETRY); + + if (highRateTask == 0) { + LOG_ERR(logger, "Failed to add the high rate telemetry task"); return false; } - // Start the mavlink driver thread - return mavDriver->start() && result != 0; + auto lowRateTask = scheduler.addTask( + [this]() + { + enqueueLowRateTelemetry(); + flushMessageQueue(); + }, + Config::Radio::LOW_RATE_TELEMETRY); + + if (lowRateTask == 0) + { + LOG_ERR(logger, "Failed to add the low rate telemetry task"); + return false; + } + + started = true; + return true; } -void Radio::sendAck(const mavlink_message_t& msg) +bool Radio::isStarted() { return started; } + +void Radio::enqueueAck(const mavlink_message_t& msg) { mavlink_message_t ackMsg; - mavlink_msg_ack_tm_pack(RadioConfig::MAV_SYSTEM_ID, - RadioConfig::MAV_COMP_ID, &ackMsg, msg.msgid, + mavlink_msg_ack_tm_pack(config::Mavlink::SYSTEM_ID, + config::Mavlink::COMPONENT_ID, &ackMsg, msg.msgid, msg.seq); - enqueueMsg(ackMsg); + enqueueMessage(ackMsg); } -void Radio::sendNack(const mavlink_message_t& msg) +void Radio::enqueueNack(const mavlink_message_t& msg) { mavlink_message_t nackMsg; - mavlink_msg_nack_tm_pack(RadioConfig::MAV_SYSTEM_ID, - RadioConfig::MAV_COMP_ID, &nackMsg, msg.msgid, - msg.seq); - enqueueMsg(nackMsg); + mavlink_msg_nack_tm_pack(config::Mavlink::SYSTEM_ID, + config::Mavlink::COMPONENT_ID, &nackMsg, msg.msgid, + msg.seq, 0); + enqueueMessage(nackMsg); } -void Radio::logStatus() { Logger::getInstance().log(mavDriver->getStatus()); } +void Radio::enqueueHighRateTelemetry() { enqueueSystemTm(MAV_FLIGHT_ID); } -bool Radio::isStarted() { return mavDriver->isStarted(); } +void Radio::enqueueLowRateTelemetry() { enqueueSystemTm(MAV_STATS_ID); } -void Radio::handleMavlinkMessage(const mavlink_message_t& msg) +void Radio::enqueueMessage(const mavlink_message_t& msg) { - switch (msg.msgid) - { - case MAVLINK_MSG_ID_PING_TC: - { - // Do nothing, just add the ack to the queue - break; - } - case MAVLINK_MSG_ID_COMMAND_TC: - { - // Let the handle command reply to the message - return handleCommand(msg); - } - case MAVLINK_MSG_ID_SYSTEM_TM_REQUEST_TC: - { - SystemTMList tmId = static_cast<SystemTMList>( - mavlink_msg_system_tm_request_tc_get_tm_id(&msg)); - - if (tmId == SystemTMList::MAV_SENSORS_STATE_ID) - { - mavlink_message_t msg; - mavlink_sensor_state_tm_t tm; - auto sensorsState = getModule<Sensors>()->getSensorInfo(); - for (SensorInfo i : sensorsState) - { - strcpy(tm.sensor_name, i.id.c_str()); - tm.state = 0; - if (i.isEnabled) - { - tm.state += 1; - } - if (i.isInitialized) - { - tm.state += 2; - } - mavlink_msg_sensor_state_tm_encode( - RadioConfig::MAV_SYSTEM_ID, RadioConfig::MAV_COMP_ID, - &msg, &tm); - enqueueMsg(msg); - } - } - else - { - - // Add to the queue the respose - mavlink_message_t response = - getModule<TMRepository>()->packSystemTm(tmId, msg.msgid, - msg.seq); - - // Add the response to the queue - 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_SENSOR_TM_REQUEST_TC: - { - SensorsTMList tmId = static_cast<SensorsTMList>( - mavlink_msg_sensor_tm_request_tc_get_sensor_name(&msg)); - - // Add to the queue the respose - mavlink_message_t response = - getModule<TMRepository>()->packSensorsTm(tmId, msg.msgid, - msg.seq); - - // Add the response to the queue - 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_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 = getModule<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 (!getModule<FlightModeManager>()->testState( - &FlightModeManager::state_test_mode) || - !getModule<Actuators>()->setServoPosition(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 (!getModule<FlightModeManager>()->testState( - &FlightModeManager::state_test_mode) || - !getModule<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 (!getModule<FlightModeManager>()->testState( - &FlightModeManager::state_test_mode) || - !getModule<Actuators>()->setServoPosition(servoId, 0)) - { - return sendNack(msg); - } - - break; - } - case MAVLINK_MSG_ID_SET_REFERENCE_ALTITUDE_TC: - { - float altitude = - mavlink_msg_set_reference_altitude_tc_get_ref_altitude(&msg); - - getModule<NASController>()->setReferenceAltitude(altitude); - - RadioSetterParameter log{}; - log.timestamp = TimestampTimer::getTimestamp(); - log.refAltitude = altitude; - Logger::getInstance().log(log); - break; - } - case MAVLINK_MSG_ID_SET_REFERENCE_TEMPERATURE_TC: - { - float temperature = - mavlink_msg_set_reference_temperature_tc_get_ref_temp(&msg); - - getModule<NASController>()->setReferenceTemperature(temperature); - - RadioSetterParameter log{}; - log.timestamp = TimestampTimer::getTimestamp(); - log.refTemperature = temperature; - Logger::getInstance().log(log); - break; - } - case MAVLINK_MSG_ID_SET_DEPLOYMENT_ALTITUDE_TC: - { - float altitude = - mavlink_msg_set_deployment_altitude_tc_get_dpl_altitude(&msg); - - getModule<AltitudeTrigger>()->setDeploymentAltitude(altitude); - - RadioSetterParameter log{}; - log.timestamp = TimestampTimer::getTimestamp(); - log.depAltitude = altitude; - Logger::getInstance().log(log); - break; - } - case MAVLINK_MSG_ID_SET_ORIENTATION_TC: - { - float yaw = mavlink_msg_set_orientation_tc_get_yaw(&msg); - float pitch = mavlink_msg_set_orientation_tc_get_pitch(&msg); - float roll = mavlink_msg_set_orientation_tc_get_roll(&msg); - - getModule<NASController>()->setOrientation(yaw, pitch, roll); - - RadioSetterParameter log{}; - log.timestamp = TimestampTimer::getTimestamp(); - log.yawOri = yaw; - log.pitchOri = pitch; - log.rollOri = roll; - Logger::getInstance().log(log); - break; - } - case MAVLINK_MSG_ID_SET_COORDINATES_TC: - { - float latitude = mavlink_msg_set_coordinates_tc_get_latitude(&msg); - float longitude = - mavlink_msg_set_coordinates_tc_get_longitude(&msg); - - getModule<NASController>()->setCoordinates( - Eigen::Vector2f(latitude, longitude)); - - RadioSetterParameter log{}; - log.timestamp = TimestampTimer::getTimestamp(); - log.latCord = latitude; - log.lonCord = longitude; - Logger::getInstance().log(log); - 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); - - getModule<WingController>()->setTargetPosition( - Eigen::Vector2f(latitude, longitude)); - - RadioSetterParameter log{}; - log.timestamp = TimestampTimer::getTimestamp(); - log.latTargetCord = latitude; - log.lonTargetCord = longitude; - Logger::getInstance().log(log); - break; - } - case MAVLINK_MSG_ID_SET_ALGORITHM_TC: - { - uint8_t algoID = - mavlink_msg_set_algorithm_tc_get_algorithm_number(&msg); - - if (!getModule<WingController>()->selectAlgorithm(algoID)) - { - return sendNack(msg); - } - RadioSetterParameter log{}; - log.timestamp = TimestampTimer::getTimestamp(); - log.algoId = algoID; - Logger::getInstance().log(log); - break; - } - case MAVLINK_MSG_ID_RAW_EVENT_TC: - { - uint8_t topicId = mavlink_msg_raw_event_tc_get_topic_id(&msg); - uint8_t eventId = mavlink_msg_raw_event_tc_get_event_id(&msg); - - // Send the event only if the flight mode manager is in test mode - if (!getModule<FlightModeManager>()->testState( - &FlightModeManager::state_test_mode)) - { - return sendNack(msg); - } - - EventBroker::getInstance().post(topicId, eventId); - break; - } - } - - // At the end send the ack message - sendAck(msg); -} + Lock<FastMutex> lock(queueMutex); -void Radio::handleCommand(const mavlink_message_t& msg) -{ - MavCommandList commandId = static_cast<MavCommandList>( - mavlink_msg_command_tc_get_command_id(&msg)); - - static const std::map<MavCommandList, Events> commandToEvent{ - {MAV_CMD_ARM, TMTC_ARM}, - {MAV_CMD_DISARM, TMTC_DISARM}, - {MAV_CMD_CALIBRATE, TMTC_CALIBRATE}, - {MAV_CMD_FORCE_INIT, TMTC_FORCE_INIT}, - {MAV_CMD_FORCE_LAUNCH, TMTC_FORCE_LAUNCH}, - {MAV_CMD_FORCE_LANDING, TMTC_FORCE_LANDING}, - {MAV_CMD_FORCE_APOGEE, TMTC_FORCE_APOGEE}, - {MAV_CMD_FORCE_EXPULSION, TMTC_FORCE_EXPULSION}, - {MAV_CMD_FORCE_DEPLOYMENT, TMTC_FORCE_DEPLOYMENT}, - {MAV_CMD_START_LOGGING, TMTC_START_LOGGING}, - {MAV_CMD_STOP_LOGGING, TMTC_STOP_LOGGING}, - {MAV_CMD_FORCE_REBOOT, TMTC_RESET_BOARD}, - {MAV_CMD_ENTER_TEST_MODE, TMTC_ENTER_TEST_MODE}, - {MAV_CMD_EXIT_TEST_MODE, TMTC_EXIT_TEST_MODE}, - {MAV_CMD_START_RECORDING, TMTC_START_RECORDING}, - {MAV_CMD_STOP_RECORDING, TMTC_STOP_RECORDING}, - }; - switch (commandId) + // Insert the message inside the queue only if there is enough space + if (messageQueueIndex < messageQueue.size()) { - case MAV_CMD_SAVE_CALIBRATION: - { - // Save the sensor calibration and adopt it - if (!getModule<Sensors>()->writeMagCalibration()) - { - return sendNack(msg); - } - break; - } - default: - { - // If the command is not a particular one, look for it inside the - // map - auto it = commandToEvent.find(commandId); - - if (it != commandToEvent.end()) - { - EventBroker::getInstance().post(it->second, TOPIC_TMTC); - } - else - { - return sendNack(msg); - } - } + messageQueue[messageQueueIndex] = msg; + messageQueueIndex++; } - // Acknowledge the message - sendAck(msg); } -void Radio::sendPeriodicMessage() +void Radio::flushMessageQueue() { - // Send all the queue messages - { - Lock<FastMutex> lock(queueMutex); - - for (uint8_t i = 0; i < messageQueueIndex; i++) - { - mavDriver->enqueueMsg(messageQueue[i]); - } + Lock<FastMutex> lock(queueMutex); - // Reset the index - messageQueueIndex = 0; + for (uint32_t i = 0; i < messageQueueIndex; i++) + { + mavDriver->enqueueMsg(messageQueue[i]); } - mavDriver->enqueueMsg( - getModule<TMRepository>()->packSystemTm(MAV_FLIGHT_ID, 0, 0)); + // Reset the index + messageQueueIndex = 0; } -void Radio::enqueueMsg(const mavlink_message_t& msg) -{ - { - Lock<FastMutex> lock(queueMutex); - - // Insert the message inside the queue only if there is enough space - if (messageQueueIndex < RadioConfig::MAVLINK_QUEUE_SIZE) - { - messageQueue[messageQueueIndex] = msg; - messageQueueIndex++; - } - } -} +void Radio::logStatus() { Logger::getInstance().log(mavDriver->getStatus()); } } // namespace Payload diff --git a/src/boards/Payload/Radio/Radio.h b/src/boards/Payload/Radio/Radio.h index b7be55dd5483994fa1e8fea71ffd1c5bec981c33..b7bff6b637e85b4bf806b64ba0b9206298fc51d3 100644 --- a/src/boards/Payload/Radio/Radio.h +++ b/src/boards/Payload/Radio/Radio.h @@ -1,5 +1,5 @@ -/* Copyright (c) 2023 Skyward Experimental Rocketry - * Author: Matteo Pignataro, Federico Mandelli +/* Copyright (c) 2024 Skyward Experimental Rocketry + * Author: Niccolò Betto * * 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,98 +19,84 @@ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * THE SOFTWARE. */ + #pragma once #include <Payload/Configs/RadioConfig.h> -#include <common/MavlinkGemini.h> +#include <common/Mavlink.h> #include <radio/MavlinkDriver/MavlinkDriver.h> #include <radio/SX1278/SX1278Fsk.h> #include <utils/DependencyManager/DependencyManager.h> namespace Payload { -using MavDriver = Boardcore::MavlinkDriver<Boardcore::SX1278Fsk::MTU, - RadioConfig::RADIO_OUT_QUEUE_SIZE, - RadioConfig::RADIO_MAV_MSG_LENGTH>; +using MavDriver = + Boardcore::MavlinkDriver<Boardcore::SX1278Fsk::MTU, + Config::Radio::MavlinkDriver::PKT_QUEUE_SIZE, + Config::Radio::MavlinkDriver::MSG_LENGTH>; class BoardScheduler; class Sensors; class Buses; -class TMRepository; class FlightModeManager; class Actuators; class NASController; class WingController; class AltitudeTrigger; +class PinHandler; +class CanHandler; -class Radio - : public Boardcore::InjectableWithDeps< - BoardScheduler, Sensors, Buses, TMRepository, FlightModeManager, - Actuators, NASController, WingController, AltitudeTrigger> +class Radio : public Boardcore::InjectableWithDeps< + BoardScheduler, Sensors, Buses, FlightModeManager, Actuators, + NASController, WingController, AltitudeTrigger, PinHandler, + CanHandler> { public: /** - * @note This class may only be instantiated once. The constructed instance - * sets itself as the static instance for handling radio interrupts. + * @brief Unsets the static instance for handling radio interrupts, if the + * current one was set by this radio instance. */ - Radio(); + ~Radio(); /** - * @brief Starts the MavlinkDriver + * @brief Initializes the radio and Mavlink driver, and sets the static + * instance for handling radio interrupts. */ [[nodiscard]] bool start(); - /** - * @brief Sends via radio an acknowledge message about the parameter passed - * message - */ - void sendAck(const mavlink_message_t& msg); + bool isStarted(); - /** - * @brief Sends via radio an non-acknowledge message about the parameter - * passed message - */ - void sendNack(const mavlink_message_t& msg); +private: + void handleMessage(const mavlink_message_t& msg); + void handleCommand(const mavlink_message_t& msg); - /** - * @brief Saves the MavlinkDriver and transceiver status - */ - void logStatus(); + void enqueueAck(const mavlink_message_t& msg); + void enqueueNack(const mavlink_message_t& msg); - /** - * @brief Returns if the radio module is correctly started - */ - bool isStarted(); - - Boardcore::SX1278Fsk* transceiver = nullptr; - MavDriver* mavDriver = nullptr; + bool enqueueSystemTm(SystemTMList tmId); + bool enqueueSensorsTm(SensorsTMList sensorId); -private: - /** - * @brief Called by the MavlinkDriver when a message is received - */ - void handleMavlinkMessage(const mavlink_message_t& msg); + void enqueueHighRateTelemetry(); + void enqueueLowRateTelemetry(); - /** - * @brief Called by the handleMavlinkMessage to handle a command message - */ - void handleCommand(const mavlink_message_t& msg); + void enqueueMessage(const mavlink_message_t& msg); + void flushMessageQueue(); /** - * @brief Sends the periodic telemetry + * @brief Logs the status of MavlinkDriver and the transceiver */ - void sendPeriodicMessage(); + void logStatus(); - /** - * @brief Inserts the mavlink message into the queue - */ - void enqueueMsg(const mavlink_message_t& msg); + std::unique_ptr<Boardcore::SX1278Fsk> transceiver; + std::unique_ptr<MavDriver> mavDriver; - // Messages queue - mavlink_message_t messageQueue[RadioConfig::MAVLINK_QUEUE_SIZE]; + std::array<mavlink_message_t, Config::Radio::MESSAGE_QUEUE_SIZE> + messageQueue; uint32_t messageQueueIndex = 0; miosix::FastMutex queueMutex; + std::atomic<bool> started{false}; + Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("Radio"); }; diff --git a/src/boards/Payload/Sensors/Sensors.cpp b/src/boards/Payload/Sensors/Sensors.cpp index 2f8293217cdf79c9916249af836afd08c8064c1a..d7eac21b8c8fdb83e7fbd8a23a8ef36e060e864e 100644 --- a/src/boards/Payload/Sensors/Sensors.cpp +++ b/src/boards/Payload/Sensors/Sensors.cpp @@ -132,8 +132,8 @@ void Sensors::calibrate() for (auto i = 0; i < config::Calibration::SAMPLE_COUNT; i++) { lps28dfwStats.add(getLPS28DFWLastSample().pressure); - staticPressureStats.add(getStaticPressureLastSample().pressure); - dynamicPressureStats.add(getDynamicPressureLastSample().pressure); + staticPressureStats.add(getStaticPressure().pressure); + dynamicPressureStats.add(getDynamicPressure().pressure); auto wakeup = start + config::Calibration::SAMPLE_PERIOD; miosix::Thread::nanoSleepUntil(wakeup.time_since_epoch().count()); @@ -218,16 +218,14 @@ InternalADCData Sensors::getInternalADCLastSample() return internalAdc ? internalAdc->getLastSample() : InternalADCData{}; } -HSCMRNN015PAData Sensors::getStaticPressureLastSample() +PressureData Sensors::getStaticPressure() { - return staticPressure ? staticPressure->getLastSample() - : HSCMRNN015PAData{}; + return staticPressure ? staticPressure->getLastSample() : PressureData{}; } -SSCMRNN030PAData Sensors::getDynamicPressureLastSample() +PressureData Sensors::getDynamicPressure() { - return dynamicPressure ? dynamicPressure->getLastSample() - : SSCMRNN030PAData{}; + return dynamicPressure ? dynamicPressure->getLastSample() : PressureData{}; } PitotData Sensors::getPitotLastSample() @@ -240,7 +238,7 @@ IMUData Sensors::getIMULastSample() return imu ? imu->getLastSample() : IMUData{}; } -BatteryVoltageSensorData Sensors::getBatteryVoltageLastSample() +BatteryVoltageSensorData Sensors::getBatteryVoltage() { auto sample = getInternalADCLastSample(); @@ -254,7 +252,7 @@ BatteryVoltageSensorData Sensors::getBatteryVoltageLastSample() return data; } -BatteryVoltageSensorData Sensors::getCamBatteryVoltageLastSample() +BatteryVoltageSensorData Sensors::getCamBatteryVoltage() { auto sample = getInternalADCLastSample(); @@ -307,14 +305,20 @@ std::vector<SensorInfo> Sensors::getSensorInfo() { if (manager) { - return {manager->getSensorInfo(lps22df.get()), - manager->getSensorInfo(lps28dfw.get()), - manager->getSensorInfo(h3lis331dl.get()), - manager->getSensorInfo(lis2mdl.get()), - manager->getSensorInfo(ubxgps.get()), - manager->getSensorInfo(lsm6dsrx.get()), - manager->getSensorInfo(ads131m08.get()), - manager->getSensorInfo(internalAdc.get())}; + return { + manager->getSensorInfo(lps22df.get()), + manager->getSensorInfo(lps28dfw.get()), + manager->getSensorInfo(h3lis331dl.get()), + manager->getSensorInfo(lis2mdl.get()), + manager->getSensorInfo(ubxgps.get()), + manager->getSensorInfo(lsm6dsrx.get()), + manager->getSensorInfo(ads131m08.get()), + manager->getSensorInfo(internalAdc.get()), + manager->getSensorInfo(staticPressure.get()), + manager->getSensorInfo(dynamicPressure.get()), + manager->getSensorInfo(pitot.get()), + manager->getSensorInfo(imu.get()), + }; } else { @@ -534,8 +538,8 @@ void Sensors::ads131m08Create() channel.enabled = false; } // Enable required channels - channels[(uint8_t)config::ADS131M08::STATIC_PRESSURE_CH].enabled = true; - channels[(uint8_t)config::ADS131M08::DYNAMIC_PRESSURE_CH].enabled = true; + channels[(uint8_t)config::StaticPressure::ADC_CH].enabled = true; + channels[(uint8_t)config::DynamicPressure::ADC_CH].enabled = true; ads131m08 = std::make_unique<ADS131M08>(getModule<Buses>()->ADS131M08(), hwmap::ADS131M08::cs::getPin(), @@ -587,12 +591,14 @@ void Sensors::staticPressureCreate() auto readVoltage = [this] { - auto sample = ads131m08->getLastSample(); - return sample.getVoltage(config::ADS131M08::STATIC_PRESSURE_CH); + auto sample = getADS131M08LastSample(); + auto voltage = sample.getVoltage(config::StaticPressure::ADC_CH); + voltage.voltage *= config::StaticPressure::SCALE; + + return voltage; }; - // TODO: use updated static pressure driver - staticPressure = std::make_unique<HSCMRNN015PA>(readVoltage, 1.2); + staticPressure = std::make_unique<MPXH6115A>(readVoltage); } void Sensors::staticPressureInsert(SensorManager::SensorMap_t& map) @@ -615,12 +621,14 @@ void Sensors::dynamicPressureCreate() auto readVoltage = [this] { - auto sample = ads131m08->getLastSample(); - return sample.getVoltage(config::ADS131M08::DYNAMIC_PRESSURE_CH); + auto sample = getADS131M08LastSample(); + auto voltage = sample.getVoltage(config::DynamicPressure::ADC_CH); + voltage.voltage *= config::DynamicPressure::SCALE; + + return voltage; }; - // TODO: use updated dynamic pressure driver - dynamicPressure = std::make_unique<SSCMRNN030PA>(readVoltage, 1.2); + dynamicPressure = std::make_unique<MPXH6115A>(readVoltage); } void Sensors::dynamicPressureInsert(SensorManager::SensorMap_t& map) diff --git a/src/boards/Payload/Sensors/Sensors.h b/src/boards/Payload/Sensors/Sensors.h index ae5d1cda5e2bfe24333f655ed56b4d4598659c19..bfb33e82cfb8b759e6cf1afbf0fd4563005af79c 100644 --- a/src/boards/Payload/Sensors/Sensors.h +++ b/src/boards/Payload/Sensors/Sensors.h @@ -35,8 +35,7 @@ #include <sensors/UBXGPS/UBXGPSSpi.h> #include <sensors/analog/BatteryVoltageSensorData.h> #include <sensors/analog/Pitot/Pitot.h> -#include <sensors/analog/pressure/honeywell/HSCMRNN015PA.h> -#include <sensors/analog/pressure/honeywell/SSCMRNN030PA.h> +#include <sensors/analog/pressure/nxp/MPXH6115A.h> #include <sensors/calibration/SoftAndHardIronCalibration/SoftAndHardIronCalibration.h> #include <utils/DependencyManager/DependencyManager.h> @@ -75,7 +74,6 @@ public: */ bool writeMagCalibration(); - // Hardware Sensors Boardcore::LPS22DFData getLPS22DFLastSample(); Boardcore::LPS28DFWData getLPS28DFWLastSample(); Boardcore::H3LIS331DLData getH3LIS331DLLastSample(); @@ -85,15 +83,14 @@ public: Boardcore::ADS131M08Data getADS131M08LastSample(); Boardcore::InternalADCData getInternalADCLastSample(); - // Software Sensors - Boardcore::HSCMRNN015PAData getStaticPressureLastSample(); - Boardcore::SSCMRNN030PAData getDynamicPressureLastSample(); + Boardcore::PressureData getStaticPressure(); + Boardcore::PressureData getDynamicPressure(); + Boardcore::PitotData getPitotLastSample(); IMUData getIMULastSample(); - // Processed values - Boardcore::BatteryVoltageSensorData getBatteryVoltageLastSample(); - Boardcore::BatteryVoltageSensorData getCamBatteryVoltageLastSample(); + Boardcore::BatteryVoltageSensorData getBatteryVoltage(); + Boardcore::BatteryVoltageSensorData getCamBatteryVoltage(); Boardcore::MagnetometerData getCalibratedMagnetometerLastSample(); void pitotSetReferenceAltitude(float altitude); @@ -163,7 +160,6 @@ protected: */ virtual bool postSensorCreationHook() { return true; } - // Hardware sensor instances std::unique_ptr<Boardcore::LPS22DF> lps22df; std::unique_ptr<Boardcore::LPS28DFW> lps28dfw; std::unique_ptr<Boardcore::H3LIS331DL> h3lis331dl; @@ -173,9 +169,8 @@ protected: std::unique_ptr<Boardcore::ADS131M08> ads131m08; std::unique_ptr<Boardcore::InternalADC> internalAdc; - // Software sensor instances - std::unique_ptr<Boardcore::HSCMRNN015PA> staticPressure; - std::unique_ptr<Boardcore::SSCMRNN030PA> dynamicPressure; + std::unique_ptr<Boardcore::MPXH6115A> staticPressure; + std::unique_ptr<Boardcore::MPXH6115A> dynamicPressure; std::unique_ptr<Boardcore::Pitot> pitot; std::unique_ptr<Payload::RotatedIMU> imu; diff --git a/src/boards/Payload/StateMachines/NASController/NASController.cpp b/src/boards/Payload/StateMachines/NASController/NASController.cpp index bbfa7c3684dece17890ac26be74807f672160840..c82842422059c9355ff679ff762d7049e1b0a2ef 100644 --- a/src/boards/Payload/StateMachines/NASController/NASController.cpp +++ b/src/boards/Payload/StateMachines/NASController/NASController.cpp @@ -145,8 +145,7 @@ void NASController::calibrate() magData.magneticFieldZ); // Static pressure barometer - HSCMRNN015PAData barometerData = - getModule<Sensors>()->getStaticPressureLastSample(); + PressureData barometerData = getModule<Sensors>()->getStaticPressure(); pressure.add(barometerData.pressure); miosix::Thread::sleep(NASConfig::CALIBRATION_SLEEP_TIME); diff --git a/src/boards/Payload/TMRepository/TMRepository.cpp b/src/boards/Payload/TMRepository/TMRepository.cpp deleted file mode 100644 index d891da603fa7ddbbb961e373a23de2fb10822c8d..0000000000000000000000000000000000000000 --- a/src/boards/Payload/TMRepository/TMRepository.cpp +++ /dev/null @@ -1,594 +0,0 @@ -/* Copyright (c) 2023 Skyward Experimental Rocketry - * Author: Matteo Pignataro - * - * 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 <Payload/Actuators/Actuators.h> -#include <Payload/BoardScheduler.h> -#include <Payload/Configs/RadioConfig.h> -#include <Payload/FlightStatsRecorder/FlightStatsRecorder.h> -#include <Payload/PinHandler/PinHandler.h> -#include <Payload/Radio/Radio.h> -#include <Payload/Sensors/Sensors.h> -#include <Payload/StateMachines/FlightModeManager/FlightModeManager.h> -#include <Payload/StateMachines/NASController/NASController.h> -#include <Payload/StateMachines/WingController/WingController.h> -#include <Payload/TMRepository/TMRepository.h> -#include <Payload/WindEstimationScheme/WindEstimation.h> -#include <diagnostic/CpuMeter/CpuMeter.h> -#include <drivers/timer/TimestampTimer.h> -#include <events/EventBroker.h> -#include <interfaces-impl/hwmapping.h> - -using namespace miosix; -using namespace Boardcore; - -namespace Payload -{ -mavlink_message_t TMRepository::packSystemTm(SystemTMList tmId, uint8_t msgId, - uint8_t seq) -{ - mavlink_message_t msg; - - switch (tmId) - { - case SystemTMList::MAV_SYS_ID: - { - mavlink_sys_tm_t tm; - - tm.timestamp = TimestampTimer::getTimestamp(); - tm.logger = Logger::getInstance().isStarted(); - tm.board_scheduler = getModule<BoardScheduler>()->isStarted(); - tm.event_broker = EventBroker::getInstance().isRunning(); - tm.radio = getModule<Radio>()->isStarted(); - tm.sensors = getModule<Sensors>()->isStarted(); - tm.pin_observer = getModule<PinHandler>()->isStarted(); - - mavlink_msg_sys_tm_encode(RadioConfig::MAV_SYSTEM_ID, - RadioConfig::MAV_COMP_ID, &msg, &tm); - - break; - } - case SystemTMList::MAV_LOGGER_ID: - { - mavlink_logger_tm_t tm; - - // Get the logger stats - LoggerStats stats = Logger::getInstance().getStats(); - - tm.timestamp = stats.timestamp; - tm.average_write_time = stats.averageWriteTime; - tm.buffers_filled = stats.buffersFilled; - tm.buffers_written = stats.buffersWritten; - tm.dropped_samples = stats.droppedSamples; - tm.last_write_error = stats.lastWriteError; - tm.log_number = stats.logNumber; - tm.max_write_time = stats.maxWriteTime; - tm.queued_samples = stats.queuedSamples; - tm.too_large_samples = stats.tooLargeSamples; - tm.writes_failed = stats.writesFailed; - - mavlink_msg_logger_tm_encode(RadioConfig::MAV_SYSTEM_ID, - RadioConfig::MAV_COMP_ID, &msg, &tm); - - break; - } - case SystemTMList::MAV_MAVLINK_STATS: - { - mavlink_mavlink_stats_tm_t tm; - - // Get the mavlink stats - MavlinkStatus stats = getModule<Radio>()->mavDriver->getStatus(); - - tm.timestamp = stats.timestamp; - tm.n_send_queue = stats.nSendQueue; - tm.max_send_queue = stats.maxSendQueue; - tm.n_send_errors = stats.nSendErrors; - tm.msg_received = stats.mavStats.msg_received; - tm.buffer_overrun = stats.mavStats.buffer_overrun; - tm.parse_error = stats.mavStats.parse_error; - tm.parse_state = stats.mavStats.parse_state; - tm.packet_idx = stats.mavStats.packet_idx; - tm.current_rx_seq = stats.mavStats.current_rx_seq; - tm.current_tx_seq = stats.mavStats.current_tx_seq; - tm.packet_rx_success_count = stats.mavStats.packet_rx_success_count; - tm.packet_rx_drop_count = stats.mavStats.packet_rx_drop_count; - - mavlink_msg_mavlink_stats_tm_encode(RadioConfig::MAV_SYSTEM_ID, - RadioConfig::MAV_COMP_ID, &msg, - &tm); - break; - } - case SystemTMList::MAV_NAS_ID: - { - mavlink_nas_tm_t tm; - - // Get the current NAS state - NASState state = getModule<NASController>()->getNasState(); - ReferenceValues ref = - getModule<NASController>()->getReferenceValues(); - - tm.timestamp = state.timestamp; - tm.state = static_cast<uint8_t>( - getModule<NASController>()->getStatus().state); - tm.nas_n = state.n; - tm.nas_e = state.e; - tm.nas_d = state.d; - tm.nas_vn = state.vn; - tm.nas_ve = state.ve; - tm.nas_vd = state.vd; - tm.nas_qx = state.qx; - tm.nas_qy = state.qy; - tm.nas_qz = state.qz; - tm.nas_qw = state.qw; - tm.nas_bias_x = state.bx; - tm.nas_bias_y = state.by; - tm.nas_bias_z = state.bz; - tm.ref_pressure = ref.refPressure; - tm.ref_temperature = ref.refTemperature; - tm.ref_latitude = ref.refLatitude; - tm.ref_longitude = ref.refLongitude; - - mavlink_msg_nas_tm_encode(RadioConfig::MAV_SYSTEM_ID, - RadioConfig::MAV_COMP_ID, &msg, &tm); - - break; - } - case SystemTMList::MAV_FLIGHT_ID: - { - mavlink_payload_flight_tm_t tm; - - tm.timestamp = TimestampTimer::getTimestamp(); - - // Last samples - LPS28DFWData lps28dfw = - getModule<Sensors>()->getLPS28DFWLastSample(); - IMUData imu = getModule<Sensors>()->getIMULastSample(); - UBXGPSData gps = getModule<Sensors>()->getUBXGPSLastSample(); - Eigen::Vector2f wind = - getModule<WindEstimation>()->getWindEstimationScheme(); - - // NAS state - NASState nasState = getModule<NASController>()->getNasState(); - - tm.nas_state = static_cast<uint8_t>( - getModule<NASController>()->getStatus().state); - tm.fmm_state = static_cast<uint8_t>( - getModule<FlightModeManager>()->getStatus().state); - tm.wes_state = static_cast<uint8_t>( - getModule<WingController>()->getStatus().state); - - tm.wes_n = wind[0]; - tm.wes_e = wind[1]; - - tm.pressure_digi = lps28dfw.pressure; - tm.pressure_static = - getModule<Sensors>()->getStaticPressureLastSample().pressure; - // Pitot - tm.airspeed_pitot = - getModule<Sensors>()->getPitotLastSample().airspeed; - - // Altitude agl - tm.altitude_agl = -nasState.d; - - // IMU - tm.acc_x = imu.accData.accelerationX; - tm.acc_y = imu.accData.accelerationY; - tm.acc_z = imu.accData.accelerationZ; - tm.gyro_x = imu.gyroData.angularSpeedX; - tm.gyro_y = imu.gyroData.angularSpeedY; - tm.gyro_z = imu.gyroData.angularSpeedZ; - - // Magnetometer - tm.mag_x = imu.magData.magneticFieldX; - tm.mag_y = imu.magData.magneticFieldY; - tm.mag_z = imu.magData.magneticFieldZ; - - // GPS - tm.gps_fix = gps.fix; - tm.gps_lat = gps.latitude; - tm.gps_lon = gps.longitude; - tm.gps_alt = gps.height; - - // NAS - tm.nas_n = nasState.n; - tm.nas_e = nasState.e; - tm.nas_d = nasState.d; - tm.nas_vn = nasState.vn; - tm.nas_ve = nasState.ve; - tm.nas_vd = nasState.vd; - tm.nas_qx = nasState.qx; - tm.nas_qy = nasState.qy; - tm.nas_qz = nasState.qz; - tm.nas_qw = nasState.qw; - tm.nas_bias_x = nasState.bx; - tm.nas_bias_y = nasState.by; - tm.nas_bias_z = nasState.bz; - // Servos - tm.left_servo_angle = getModule<Actuators>()->getServoAngle( - ServosList::PARAFOIL_LEFT_SERVO); - tm.right_servo_angle = getModule<Actuators>()->getServoAngle( - ServosList::PARAFOIL_RIGHT_SERVO); - - tm.pin_nosecone = getModule<PinHandler>() - ->getPinData()[PinsList::NOSECONE_PIN] - .lastState; - - tm.cutter_presence = - static_cast<uint8_t>(adcs::cutterSense::value()); - - tm.battery_voltage = - getModule<Sensors>()->getBatteryVoltageLastSample().batVoltage; - tm.current_consumption = 0; - tm.temperature = lps28dfw.temperature; - tm.logger_error = Logger::getInstance().getStats().lastWriteError; - - tm.cam_battery_voltage = getModule<Sensors>() - ->getCamBatteryVoltageLastSample() - .batVoltage; - - mavlink_msg_payload_flight_tm_encode(RadioConfig::MAV_SYSTEM_ID, - RadioConfig::MAV_COMP_ID, &msg, - &tm); - - break; - } - case SystemTMList::MAV_STATS_ID: - { - mavlink_payload_stats_tm_t tm = - getModule<FlightStatsRecorder>()->getStats(); - tm.cpu_load = CpuMeter::getCpuStats().mean; - tm.free_heap = CpuMeter::getCpuStats().freeHeap; - - mavlink_msg_payload_stats_tm_encode(RadioConfig::MAV_SYSTEM_ID, - RadioConfig::MAV_COMP_ID, &msg, - &tm); - break; - } - case SystemTMList::MAV_FSM_ID: - { - mavlink_fsm_tm_t tm; - - tm.timestamp = TimestampTimer::getTimestamp(); - tm.abk_state = 0; - tm.ada_state = 0; - tm.dpl_state = static_cast<uint8_t>( - getModule<WingController>()->getStatus().state); - tm.fmm_state = static_cast<uint8_t>( - getModule<FlightModeManager>()->getStatus().state); - tm.nas_state = static_cast<uint8_t>( - getModule<NASController>()->getStatus().state); - tm.wes_state = 0; - - mavlink_msg_fsm_tm_encode(RadioConfig::MAV_SYSTEM_ID, - RadioConfig::MAV_COMP_ID, &msg, &tm); - - break; - } - case SystemTMList::MAV_PIN_OBS_ID: - { - mavlink_pin_tm_t tm; - - tm.timestamp = TimestampTimer::getTimestamp(); - tm.last_change_timestamp = - getModule<PinHandler>() - ->getPinData()[PinsList::NOSECONE_PIN] - .lastStateTimestamp; /*< Last change timestamp of pin*/ - tm.pin_id = - PinsList::NOSECONE_PIN; /*< A member of the PinsList enum*/ - tm.changes_counter = - getModule<PinHandler>() - ->getPinData()[PinsList::NOSECONE_PIN] - .changesCount; /*< Number of changes of pin*/ - tm.current_state = getModule<PinHandler>() - ->getPinData()[PinsList::NOSECONE_PIN] - .lastState; /*< Current state of pin*/ - - mavlink_msg_pin_tm_encode(RadioConfig::MAV_SYSTEM_ID, - RadioConfig::MAV_COMP_ID, &msg, &tm); - - break; - } - default: - { - // Send by default the nack message - mavlink_nack_tm_t nack; - - nack.recv_msgid = msgId; - nack.seq_ack = seq; - - LOG_ERR(logger, "Unknown message id: {}", tmId); - mavlink_msg_nack_tm_encode(RadioConfig::MAV_SYSTEM_ID, - RadioConfig::MAV_COMP_ID, &msg, &nack); - - break; - } - } - return msg; -} -mavlink_message_t TMRepository::packSensorsTm(SensorsTMList sensorId, - uint8_t msgId, uint8_t seq) -{ - mavlink_message_t msg; - - switch (sensorId) - { - case SensorsTMList::MAV_GPS_ID: - { - mavlink_gps_tm_t tm; - - UBXGPSData gpsData = getModule<Sensors>()->getUBXGPSLastSample(); - - tm.timestamp = gpsData.gpsTimestamp; - strcpy(tm.sensor_name, "UBXGPS"); - tm.fix = gpsData.fix; - tm.height = gpsData.height; - tm.latitude = gpsData.latitude; - tm.longitude = gpsData.longitude; - tm.n_satellites = gpsData.satellites; - tm.speed = gpsData.speed; - tm.track = gpsData.track; - tm.vel_down = gpsData.velocityDown; - tm.vel_east = gpsData.velocityEast; - tm.vel_north = gpsData.velocityNorth; - - mavlink_msg_gps_tm_encode(RadioConfig::MAV_SYSTEM_ID, - RadioConfig::MAV_COMP_ID, &msg, &tm); - - break; - } - - case SensorsTMList::MAV_STATIC_PRESS_ID: - { - mavlink_pressure_tm_t tm; - - HSCMRNN015PAData pressureData = - getModule<Sensors>()->getStaticPressureLastSample(); - - tm.timestamp = pressureData.pressureTimestamp; - strcpy(tm.sensor_name, "STATIC_PRESSURE"); - tm.pressure = pressureData.pressure; - - mavlink_msg_pressure_tm_encode(RadioConfig::MAV_SYSTEM_ID, - RadioConfig::MAV_COMP_ID, &msg, &tm); - - break; - } - case SensorsTMList::MAV_DPL_PRESS_ID: - { - mavlink_pressure_tm_t tm; - - SSCMRNN030PAData pressureData = - getModule<Sensors>()->getDynamicPressureLastSample(); - - tm.timestamp = pressureData.pressureTimestamp; - strcpy(tm.sensor_name, "DYNAMIC_PRESSURE"); - tm.pressure = pressureData.pressure; - - mavlink_msg_pressure_tm_encode(RadioConfig::MAV_SYSTEM_ID, - RadioConfig::MAV_COMP_ID, &msg, &tm); - - break; - } - case SensorsTMList::MAV_PITOT_PRESS_ID: - { - mavlink_pressure_tm_t tm; - - SSCMRNN030PAData pitot = - getModule<Sensors>()->getDynamicPressureLastSample(); - - tm.timestamp = pitot.pressureTimestamp; - tm.pressure = pitot.pressure; - strcpy(tm.sensor_name, "PITOT_PRESSURE"); - - mavlink_msg_pressure_tm_encode(RadioConfig::MAV_SYSTEM_ID, - RadioConfig::MAV_COMP_ID, &msg, &tm); - break; - } - case SensorsTMList::MAV_LIS2MDL_ID: - { - mavlink_imu_tm_t tm; - - LIS2MDLData mag = getModule<Sensors>()->getLIS2MDLLastSample(); - - tm.acc_x = 0; - tm.acc_y = 0; - tm.acc_z = 0; - tm.gyro_x = 0; - tm.gyro_y = 0; - tm.gyro_z = 0; - tm.mag_x = mag.magneticFieldX; - tm.mag_y = mag.magneticFieldY; - tm.mag_z = mag.magneticFieldZ; - tm.timestamp = mag.magneticFieldTimestamp; - strcpy(tm.sensor_name, "LIS2MD"); - - mavlink_msg_imu_tm_encode(RadioConfig::MAV_SYSTEM_ID, - RadioConfig::MAV_COMP_ID, &msg, &tm); - break; - } - case SensorsTMList::MAV_LSM6DSRX_ID: - { - mavlink_imu_tm_t tm; - - LSM6DSRXData imu = getModule<Sensors>()->getLSM6DSRXLastSample(); - - tm.acc_x = imu.accelerationX; - tm.acc_y = imu.accelerationY; - tm.acc_z = imu.accelerationZ; - tm.gyro_x = imu.angularSpeedX; - tm.gyro_y = imu.angularSpeedY; - tm.gyro_z = imu.angularSpeedZ; - tm.mag_x = 0; - tm.mag_y = 0; - tm.mag_z = 0; - - tm.timestamp = imu.accelerationTimestamp; - strcpy(tm.sensor_name, "LSM6DSRX"); - - mavlink_msg_imu_tm_encode(RadioConfig::MAV_SYSTEM_ID, - RadioConfig::MAV_COMP_ID, &msg, &tm); - break; - } - case SensorsTMList::MAV_H3LIS331DL_ID: - { - mavlink_imu_tm_t tm; - - H3LIS331DLData imu = - getModule<Sensors>()->getH3LIS331DLLastSample(); - - tm.acc_x = imu.accelerationX; - tm.acc_y = imu.accelerationY; - tm.acc_z = imu.accelerationZ; - tm.gyro_x = 0; - tm.gyro_y = 0; - tm.gyro_z = 0; - tm.mag_x = 0; - tm.mag_y = 0; - tm.mag_z = 0; - - tm.timestamp = imu.accelerationTimestamp; - strcpy(tm.sensor_name, "H3LIS331DL"); - - mavlink_msg_imu_tm_encode(RadioConfig::MAV_SYSTEM_ID, - RadioConfig::MAV_COMP_ID, &msg, &tm); - break; - } - case SensorsTMList::MAV_LPS22DF_ID: - { - mavlink_pressure_tm_t tm; - - LPS22DFData pressure = getModule<Sensors>()->getLPS22DFLastSample(); - - tm.timestamp = pressure.pressureTimestamp; - tm.pressure = pressure.pressure; - strcpy(tm.sensor_name, "LPS22DF"); - - mavlink_msg_pressure_tm_encode(RadioConfig::MAV_SYSTEM_ID, - RadioConfig::MAV_COMP_ID, &msg, &tm); - break; - } - case SensorsTMList::MAV_LPS28DFW_ID: - { - mavlink_pressure_tm_t tm; - LPS28DFWData pressure = - getModule<Sensors>()->getLPS28DFWLastSample(); - - tm.timestamp = pressure.pressureTimestamp; - tm.pressure = pressure.pressure; - strcpy(tm.sensor_name, "LPS28DFW"); - - mavlink_msg_pressure_tm_encode(RadioConfig::MAV_SYSTEM_ID, - RadioConfig::MAV_COMP_ID, &msg, &tm); - break; - } - case SensorsTMList::MAV_BATTERY_VOLTAGE_ID: - { - mavlink_voltage_tm_t tm; - BatteryVoltageSensorData voltage = - getModule<Sensors>()->getBatteryVoltageLastSample(); - tm.voltage = voltage.batVoltage; - strcpy(tm.sensor_name, "BATTERY"); - tm.timestamp = voltage.voltageTimestamp; - mavlink_msg_voltage_tm_encode(RadioConfig::MAV_SYSTEM_ID, - RadioConfig::MAV_COMP_ID, &msg, &tm); - break; - } - case SensorsTMList::MAV_ADS_ID: - { - mavlink_adc_tm_t tm; - ADS131M08Data voltage = - getModule<Sensors>()->getADS131M08LastSample(); - tm.channel_0 = - voltage.getVoltage(ADS131M08Defs::Channel::CHANNEL_0).voltage; - - tm.channel_1 = - voltage.getVoltage(ADS131M08Defs::Channel::CHANNEL_1).voltage; - - tm.channel_2 = - voltage.getVoltage(ADS131M08Defs::Channel::CHANNEL_2).voltage; - - tm.channel_3 = - voltage.getVoltage(ADS131M08Defs::Channel::CHANNEL_3).voltage; - - tm.channel_4 = - voltage.getVoltage(ADS131M08Defs::Channel::CHANNEL_4).voltage; - - tm.channel_5 = - voltage.getVoltage(ADS131M08Defs::Channel::CHANNEL_5).voltage; - - tm.channel_6 = - voltage.getVoltage(ADS131M08Defs::Channel::CHANNEL_6).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; - } - default: - { - mavlink_nack_tm_t nack; - - nack.recv_msgid = msgId; - nack.seq_ack = seq; - - LOG_DEBUG(logger, "Unknown telemetry id: {}", sensorId); - mavlink_msg_nack_tm_encode(RadioConfig::MAV_SYSTEM_ID, - RadioConfig::MAV_COMP_ID, &msg, &nack); - break; - } - } - - 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 = getModule<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 Payload diff --git a/src/boards/Payload/TMRepository/TMRepository.h b/src/boards/Payload/TMRepository/TMRepository.h deleted file mode 100644 index fbffe14a2882741d80e95fa4cf6e3db2aa1e58cc..0000000000000000000000000000000000000000 --- a/src/boards/Payload/TMRepository/TMRepository.h +++ /dev/null @@ -1,65 +0,0 @@ -/* Copyright (c) 2023 Skyward Experimental Rocketry - * Author: Matteo Pignataro - * - * 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 <common/MavlinkGemini.h> -#include <diagnostic/PrintLogger.h> -#include <utils/DependencyManager/DependencyManager.h> - -namespace Payload -{ -class Sensors; -class Radio; -class BoardScheduler; -class PinHandler; -class NASController; -class FlightModeManager; -class WingController; -class WindEstimation; -class Actuators; -class FlightStatsRecorder; - -class TMRepository : public Boardcore::InjectableWithDeps< - Sensors, Radio, BoardScheduler, PinHandler, - NASController, FlightModeManager, WingController, - WindEstimation, Actuators, FlightStatsRecorder> -{ -public: - inline TMRepository() {} - - // Packs the telemetry at system level (E.g. logger telemetry..) - mavlink_message_t packSystemTm(SystemTMList tmId, uint8_t msgId, - uint8_t seq); - - // Packs the telemetry at sensors level (E.g. pressure sensors..) - 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 Payload \ No newline at end of file diff --git a/src/entrypoints/Payload/payload-entry.cpp b/src/entrypoints/Payload/payload-entry.cpp index 4636eb2ef93d020e8c472be8a4252fc61919d8be..ac50118173f9743ce900282622e758e3d1e847f4 100644 --- a/src/entrypoints/Payload/payload-entry.cpp +++ b/src/entrypoints/Payload/payload-entry.cpp @@ -33,7 +33,6 @@ #include <Payload/StateMachines/FlightModeManager/FlightModeManager.h> #include <Payload/StateMachines/NASController/NASController.h> #include <Payload/StateMachines/WingController/WingController.h> -#include <Payload/TMRepository/TMRepository.h> #include <Payload/VerticalVelocityTrigger/VerticalVelocityTrigger.h> #include <Payload/WindEstimationScheme/WindEstimation.h> #include <common/Events.h> @@ -119,18 +118,17 @@ int main() // Statistics auto statsRecorder = new FlightStatsRecorder(); - auto tmRepository = new TMRepository(); // Insert modules - bool initResult = - depman.insert(buses) && depman.insert(scheduler) && - depman.insert(flightModeManager) && depman.insert(nas) && - depman.insert(sensors) && depman.insert(pinHandler) && - depman.insert(radio) && depman.insert(canHandler) && - depman.insert(altitudeTrigger) && depman.insert(wingController) && - depman.insert(verticalVelocityTrigger) && - depman.insert(windEstimation) && depman.insert(actuators) && - depman.insert(statsRecorder) && depman.insert(tmRepository); + bool initResult = depman.insert(buses) && depman.insert(scheduler) && + depman.insert(flightModeManager) && depman.insert(nas) && + depman.insert(sensors) && depman.insert(pinHandler) && + depman.insert(radio) && depman.insert(canHandler) && + depman.insert(altitudeTrigger) && + depman.insert(wingController) && + depman.insert(verticalVelocityTrigger) && + depman.insert(windEstimation) && + depman.insert(actuators) && depman.insert(statsRecorder); // Populate module dependencies initResult &= depman.inject(); @@ -147,7 +145,7 @@ int main() // Start module instances START_MODULE(sensors) { miosix::led1On(); } START_MODULE(pinHandler); - // START_MODULE(radio) { miosix::led2On(); } + START_MODULE(radio) { miosix::led2On(); } // START_MODULE(canHandler) { miosix::led3On(); } START_MODULE(flightModeManager); START_MODULE(nas);