diff --git a/MIDAS/README.md b/MIDAS/README.md index 47786730..c7a12c7a 100644 --- a/MIDAS/README.md +++ b/MIDAS/README.md @@ -1,23 +1,268 @@ # MIDAS Flight Code -## Getting Started +Flight software for the **MIDAS** avionics board. This repository contains the firmware, simulation environments, hardware drivers, telemetry, logging, and testing infrastructure used throughout development. -To being, clone this whole repository onto your computer somewhere. +Documentation: +- Muhammad Ali (2026) +- Melody Parker (2026) +- Jennifer Luo (2026) +--- -// todo explain how to install/use platformio -// todo have both VSCode and CLion sections +# Table of Contents -### Command Line -If you want to run SILSIM, you'll need `gcc` to be available on your path. -On Linux/macOS, `gcc` should be preinstalled. -On Windows, you'll need to install the newest version of MinGW-w64 and add it's -`\bin` directory to your PATH environment variable. +* [Overview](#overview) +* [Project Structure](#project-structure) +* [Requirements](#requirements) +* [Getting Started](#getting-started) +* [Building the Project](#building-the-project) +* [Running SILSIM](#running-silsim) +* [Uploading to Hardware](#uploading-to-hardware) +* [Testing](#testing) +* [Development](#development) +* [Future Documentation](#future-documentation) -To flash flight code to the MIDAS board using the command line, run -`pio run -e mcu_main -t upload`. To just build flight code for MIDAS without -actually uploading it (to make sure it compiles), use `pio run -e mcu_main`. -Note that if you never directly installed platformio and instead are just using -the VSCode extension, these commands won't work until you install platformio -manually. +--- -To run SILSIM from the command line, use `pio run -e mcu_silsim`. \ No newline at end of file +# Overview + +The MIDAS flight code is built using: + +* **PlatformIO** +* **ESP32** +* **C++17** +* **FreeRTOS** + +The repository contains everything needed to build, simulate, test, and deploy the rocket flight software. + +The software is organized into independent modules such as the flight state machine, guidance/navigation, hardware drivers, telemetry, logging, and simulation. + +--- + +# Project Structure + +``` +MIDAS/ +│ +├── lib/ # External and custom driver libraries +│ +├── src/ +│ ├── flight-systems/ # Main system initialization +│ ├── finite-state-machines/ # Flight state machines +│ ├── gnc/ # Guidance, Navigation & Control +│ ├── hardware/ # Hardware interfaces +│ ├── hilsim/ # Hardware-In-The-Loop simulation +│ ├── silsim/ # Software-In-The-Loop simulation +│ ├── telemetry/ # Telemetry systems +│ ├── logging/ # Data logging +│ ├── util/ # Utility files that help +│ └── ... +│ +├── test/ # Test programs and previous flight data +├── tools/ # Tools to help unload midas flight data (log_enc.py) +│ +├── platformio.ini # PlatformIO configuration +│ +└── README.md +``` + +Additional modules will be documented individually as development progresses. + +--- + +# Requirements + +Before building the project, install: + +* PlatformIO +* Git +* C++ compiler + + +--- + +# Getting Started + +Clone the repository: + +```bash +git clone https://github.com/ISSUIUC/MIDAS-Software.git +cd MIDAS +``` + +More setup instructions will be added for: + +* VSCode +* PlatformIO installation + +--- + +# Building the Project + +## Build Flight Code + +Compile the firmware without uploading: + +```bash +pio run -e mcu_main +``` + +This verifies that the project builds successfully. + +--- + +# Uploading to Hardware + +Flash the firmware to the MIDAS board: + +```bash +pio run -e mcu_main -t upload +``` + +Make sure the board is connected before uploading. + +--- + + +--- + +# Testing + +The `test/` directory contains: + +* Previous flight datasets +* Test programs +* Validation utilities + +Future documentation will include: + +* Running unit tests +* Regression testing +* Flight replay testing +* Hardware validation + +--- + +# Development + +The project is divided into several major subsystems. + +## Flight State Machines + +Responsible for rocket state transitions throughout flight. + +Future documentation: + +* State diagrams +* Transition conditions +* Recovery logic + +--- + +## Guidance, Navigation & Control (GNC) + +Contains the vehicle estimation and control algorithms. + +Future documentation: + +* Sensor fusion +* Filtering +* Apogee detection +* Velocity estimation + +--- + +## Hardware + +Interfaces for onboard peripherals including sensors and communication devices. + +Future documentation: + +* Sensor drivers +* EEPROM +* SD card +* Radios +* Buzzer +* LEDs + +--- + +## Telemetry + +Handles communication between the flight computer and the ground station. + +Future documentation: + +* Packet formats +* Message types +* Protocol documentation + +--- + +## Logging + +Responsible for onboard data recording. + +Future documentation: + +* Log format +* Storage layout +* Replay tools + +--- + +## SILSIM + +Software-In-The-Loop simulator used during development. + +Future documentation: + +* Flight replay +* Sensor injection +* Configuration +* Debugging + +--- + +## HILSIM + +Hardware-In-The-Loop testing framework. + +Future documentation: + +* Hardware setup +* Test workflow +* Communication protocol + +--- + +# Coding Style + +Documentation for coding conventions, formatting, and contribution guidelines will be added in the future. + +--- + +# Future Documentation + +Planned additions include: + +* PlatformIO installation guide +* VSCode setup +* CLion setup +* Project architecture +* State machine documentation +* Sensor documentation +* Telemetry protocol +* Logging format +* Build environments +* Flashing multiple boards +* Debugging with PlatformIO +* Simulation guide +* Contribution guidelines +* Continuous Integration (CI) +* Flight data analysis + +--- + +# License + +*To be added.* diff --git a/MIDAS/lib/MS5611/MS5611.cpp b/MIDAS/lib/MS5611/MS5611.cpp index 2c2ff600..ef8b9744 100644 --- a/MIDAS/lib/MS5611/MS5611.cpp +++ b/MIDAS/lib/MS5611/MS5611.cpp @@ -193,7 +193,7 @@ void MS5611::reset() { digitalWrite(_cspin, HIGH); // pull CS line high SPI.endTransaction(); // end SPI transaction } -#include"../../src/hal.h" +#include"../../src/util/hal.h" void MS5611::convert(const uint8_t addr, uint8_t bits) { uint8_t del[5] = {1, 2, 3, 5, 10}; // array of MS5611 conversion time (in ms) diff --git a/MIDAS/src/data_logging.cpp b/MIDAS/src/data_logging.cpp deleted file mode 100644 index 13032eb9..00000000 --- a/MIDAS/src/data_logging.cpp +++ /dev/null @@ -1,143 +0,0 @@ -#include "data_logging.h" -#include "log_format.h" -#include "log_checksum.h" - -/** - * @brief Forward decleration of the ID recieving function -*/ -template -constexpr ReadingDiscriminant get_discriminant(); - - -/** - * @brief writes a reading, with its ID, timestamp, and data to a specific sink - * - * @param sink the LogSink to write to - * @param reading the data to read -*/ -template -void log_reading(LogSink& sink, Reading& reading) { - ReadingDiscriminant discriminant = get_discriminant(); - sink.write((uint8_t*) &discriminant, sizeof(ReadingDiscriminant)); - sink.write((uint8_t*) &reading.timestamp_ms, sizeof(uint32_t)); - sink.write((uint8_t*) &reading.data, sizeof(T)); -} - -/** - * @brief writes a SensorData's entire queue reading to a sink - * - * @param sink the LogSink to write to - * @param sensor_data the sensor data, with queue, to write from - * - * @return the number of packets written to the LogSink -*/ -template -uint32_t log_from_sensor_data(LogSink& sink, SensorData& sensor_data) { - Reading reading; - uint32_t read = 0; - while (read < 20 && sensor_data.getQueued(&reading)) { - log_reading(sink, reading); - read++; - } - return read; -} - -/** - * @brief Initializes a specific LogSink - * - * @param sink the LogSink to initialize -*/ -void log_begin(LogSink& sink) { - uint32_t checksum = LOG_CHECKSUM; - sink.write((uint8_t*) &checksum, 4); -} - -/** - * @brief logs all sensor data from the rocket - * - * @param sink the LogSink to write data to - * @param data the rocket which holds all the sensor data to write -*/ -void log_data(LogSink& sink, RocketData& data) { - log_from_sensor_data(sink, data.imu); - log_from_sensor_data(sink, data.sflp); - log_from_sensor_data(sink, data.barometer); - log_from_sensor_data(sink, data.voltage); - log_from_sensor_data(sink, data.gps); - log_from_sensor_data(sink, data.magnetometer); - log_from_sensor_data(sink, data.fsm_state); - log_from_sensor_data(sink, data.kalman); - log_from_sensor_data(sink, data.angular_kalman_data); - log_from_sensor_data(sink, data.pyro); - log_from_sensor_data(sink, data.cam_data); -} - - - -#ifndef SILSIM -#define MAX_FILES 999 - -/** - * @brief names a new file for a log sink depending on the files currently on said LogSink - * - * @param fileName buffer to write the file name to - * @param fileExtensionParam the file extension required for the file - * @param fs the FileSystem to check files off of - * - * @return buffer contianing string of file name -*/ -char* sdFileNamer(char* fileName, char* fileExtensionParam, FS& fs, uint16_t file_num, int* fileno_out) { - char fileExtension[strlen(fileExtensionParam) + 1]; - strcpy(fileExtension, fileExtensionParam); - - char inputName[256] = {0}; - strcpy(inputName, "/"); - strcat(inputName, fileName); - strcat(inputName, fileExtension); - - // checks to see if file already exists and adds 1 to filename if it does. - bool exists = fs.exists(inputName); - - if (exists) { - bool fileExists = false; - // We will start at file_num, which is default 0 if eeprom is erased - int i = file_num; - while (!fileExists) { - if (i > MAX_FILES) { - // max number of files reached. Don't want to overflow - // fileName[]. Will write new data to already existing - // data999.csv - strcpy(inputName, "/"); - strcat(inputName, fileName); - strcat(inputName, "999"); - strcat(inputName, fileExtension); - *fileno_out = 999; - break; - } - - // converts int i to char[] - char iStr[16] = {0}; - itoa(i, iStr, 10); - - // writes "(sensor)_data(number).csv to fileNameTemp" - strcpy(inputName, "/"); - strcat(inputName, fileName); - strcat(inputName, iStr); - strcat(inputName, fileExtension); - - if (!fs.exists(inputName)) { - fileExists = true; - *fileno_out = i + 1; - } - - i++; - } - } else { - *fileno_out = 0; - } - - strcpy(fileName, inputName); - - return fileName; -} -#endif \ No newline at end of file diff --git a/MIDAS/src/data_logging.h b/MIDAS/src/data_logging.h deleted file mode 100644 index ac43f42e..00000000 --- a/MIDAS/src/data_logging.h +++ /dev/null @@ -1,85 +0,0 @@ -#pragma once - -#include "rocket_state.h" -#include "errors.h" - -#if defined(SILSIM) -//#include "silsim/FileSink.h" -#elif defined(HILSIM) -#else -//#include "hardware/SDLog.h" -#endif - - - - -/** - * @class LogSink - * - * @brief Protocol for a sink, which is implemented as an SD card in hardware. - */ -class LogSink { -public: - LogSink() = default; - - bool failed_wr = false; // Failed a write - bool failed_mr = false; // Failed a meta write - - virtual ErrorCode init() = 0; - virtual void write(const uint8_t* data, size_t size) = 0; - - virtual void write_meta(const uint8_t* data, size_t size) = 0; - uint16_t current_file_no = 0; - char active_bin_name[20] = {0}; // basename of currently-open .bin (no leading '/') - char active_meta_name[20] = {0}; // basename of currently-open .meta -}; - -void log_begin(LogSink& sink); -void log_data(LogSink& sink, RocketData& data); - -template -class MultipleLogSink : public LogSink { -public: - MultipleLogSink() = default; - - ErrorCode init() override { - return ErrorCode::NoError; - }; - - void write(const uint8_t* data, size_t size) override {}; - - void write_meta(const uint8_t* data, size_t size) override {}; -}; - -template -class MultipleLogSink : public LogSink { -public: - MultipleLogSink() = default; - explicit MultipleLogSink(Sink sink_, Sinks... sinks_) : sink(sink_), sinks(sinks_...) { }; - - ErrorCode init() override { - ErrorCode result = sink.init(); - if (result != ErrorCode::NoError) { - return result; - } - return sinks.init(); - }; - void write(const uint8_t* data, size_t size) override { - sink.write(data, size); - sinks.write(data, size); - }; - - void write_meta(const uint8_t* data, size_t size) override { - sink.write(data, size); - sinks.write(data, size); - }; - -private: - Sink sink; - MultipleLogSink sinks; -}; - -#ifndef SILSIM -#include -char* sdFileNamer(char* fileName, char* fileExtensionParam, FS& fs, uint16_t file_num, int* fileno_out); -#endif \ No newline at end of file diff --git a/MIDAS/src/data_logging_meta.h b/MIDAS/src/data_logging_meta.h deleted file mode 100644 index 80ff0d2c..00000000 --- a/MIDAS/src/data_logging_meta.h +++ /dev/null @@ -1,138 +0,0 @@ -#include -#include -#include -#include -#include - -#define META_LOGGING_MAX_SIZE 64 - -enum MetaDataCode { - // Launch events - EVENT_TLAUNCH, - EVENT_TBURNOUT, - EVENT_TIGNITION, - EVENT_TAPOGEE, - EVENT_TMAIN, - EVENT_TMAX_ACCEL, - EVENT_TMAX_VEL, - EVENT_TMAX_DESCENT_RATE, - - // Non-events - DATA_LAUNCHSITE_BARO, - DATA_LAUNCHSITE_ALT, - DATA_LAUNCHSITE_GPS_ALT, - DATA_LAUNCHSITE_GPS_LAT, - DATA_LAUNCHSITE_GPS_LONG, - DATA_LAUNCH_INITIAL_TILT, - DATA_TILT_AT_BURNOUT, - DATA_TILT_AT_IGNITION, - DATA_BARO_AT_IGNITION, - DATA_MAX_ACCEL, - DATA_MAX_VEL, - DATA_ALT_AT_BURNOUT, - DATA_MAX_DESCENT_RATE -}; - -enum class MetalogSummaryEntryType{ - CURRENT, - MAXIMUM, - MINIMUM, -}; - -struct MetalogSummary; - -struct MetaLogging { - public: - struct MetaLogEntry { - MetaDataCode log_type; - size_t size; - char data[META_LOGGING_MAX_SIZE]; - }; - - MetalogSummary * summary; - - Queue _q; - - bool get_queued(MetaLogEntry* out) { return _q.receive(out); } - - template - void log_data(MetaDataCode data_type, const T& data) { - - // double check... - static_assert(sizeof(T) <= META_LOGGING_MAX_SIZE, "Datatype for log_data too large"); - - MetaLogEntry entry{data_type, 0, 0}; - entry.size = sizeof(T); - memcpy(entry.data, &data, entry.size); - _q.send(entry); - - // fprintf(stderr, "Data has been logged: %c", entry.data); - } -}; - -template -class MetalogSummaryEntry { - - public: - - MetalogSummaryEntry(const MetaDataCode &metacode, const MetalogSummaryEntryType &metatype = MetalogSummaryEntryType::CURRENT, const T &default_val = T()){ - code = metacode; - type = metatype; - data = default_val; - } - - void update(const T &newval){ - switch(type){ - case MetalogSummaryEntryType::CURRENT: - data = newval; - break; - case MetalogSummaryEntryType::MAXIMUM: - if (newval > data){ - data = newval; - } - break; - case MetalogSummaryEntryType::MINIMUM: - if (newval < data){ - data = newval; - } - break; - } - } - - void commit(MetaLogging &metalog); - - private: - T data; - MetaDataCode code; - MetalogSummaryEntryType type; -}; - -struct MetalogSummary{ - // Launch events - MetalogSummaryEntry event_tlaunch {MetaDataCode::EVENT_TLAUNCH}; - MetalogSummaryEntry event_tburnout {MetaDataCode::EVENT_TBURNOUT}; - MetalogSummaryEntry event_tignition {MetaDataCode::EVENT_TIGNITION}; - MetalogSummaryEntry event_tapogee {MetaDataCode::EVENT_TAPOGEE}; - MetalogSummaryEntry event_tmain {MetaDataCode::EVENT_TMAIN}; - MetalogSummaryEntry event_tmax_accel {MetaDataCode::EVENT_TMAX_ACCEL}; - MetalogSummaryEntry event_tmax_vel {MetaDataCode::EVENT_TMAX_VEL}; - MetalogSummaryEntry event_tmax_descent_rate {MetaDataCode::EVENT_TMAX_DESCENT_RATE}; - - // Non-events - MetalogSummaryEntry data_launchsite_baro {MetaDataCode::DATA_LAUNCHSITE_BARO}; - MetalogSummaryEntry data_launchsite_gps_alt {MetaDataCode::DATA_LAUNCHSITE_GPS_ALT}; - MetalogSummaryEntry data_launchsite_gps_lat {MetaDataCode::DATA_LAUNCHSITE_GPS_LAT}; - MetalogSummaryEntry data_launchsite_gps_long {MetaDataCode::DATA_LAUNCHSITE_GPS_LONG}; - MetalogSummaryEntry data_launch_initial_tilt {MetaDataCode::DATA_LAUNCH_INITIAL_TILT}; - MetalogSummaryEntry data_tilt_at_burnout {MetaDataCode::DATA_TILT_AT_BURNOUT}; - MetalogSummaryEntry data_tilt_at_ignition {MetaDataCode::DATA_TILT_AT_IGNITION}; - MetalogSummaryEntry data_baro_at_ignition {MetaDataCode::DATA_BARO_AT_IGNITION}; - MetalogSummaryEntry data_max_accel {MetaDataCode::DATA_MAX_ACCEL, MetalogSummaryEntryType::MAXIMUM, -std::numeric_limits::max()}; - MetalogSummaryEntry data_max_vel {MetaDataCode::DATA_MAX_VEL, MetalogSummaryEntryType::MAXIMUM, -std::numeric_limits::max()}; - MetalogSummaryEntry data_alt_at_burnout {MetaDataCode::DATA_ALT_AT_BURNOUT}; - MetalogSummaryEntry data_max_descent_rate {MetaDataCode::DATA_MAX_DESCENT_RATE, MetalogSummaryEntryType::MAXIMUM, -std::numeric_limits::max()}; -}; - -template -void MetalogSummaryEntry::commit(MetaLogging &metalog){metalog.log_data(code, data);} - diff --git a/MIDAS/src/esp_eeprom.cpp b/MIDAS/src/esp_eeprom.cpp deleted file mode 100644 index 9cfb519e..00000000 --- a/MIDAS/src/esp_eeprom.cpp +++ /dev/null @@ -1,56 +0,0 @@ -#include - - - -bool EEPROMController::read() { - MIDASEEPROM _read; - uint8_t buf[EEPROM_SIZE]; - for (int i = 0; i < EEPROM_SIZE; i++) { - buf[i] = EEPROM.read(i); - } - - memcpy(&_read, buf, EEPROM_SIZE); - - if(_read.checksum != EEPROM_CHECKSUM) { - // Wrong checksum, cannot read. - return false; - } - - // Otherwise, we set the data to the read EEPROM. - data = _read; - - return true; -} - -bool EEPROMController::commit() { - uint8_t buf[EEPROM_SIZE]; - data.checksum = EEPROM_CHECKSUM; - - memcpy(buf, &data, EEPROM_SIZE); - - for (int i = 0; i < EEPROM_SIZE; i++) { - EEPROM.write(i, buf[i]); - } - - EEPROM.commit(); - - return read(); -} - -ErrorCode EEPROMController::init() { - static_assert(EEPROM_SIZE <= EEPROM_MAX_SIZE); - EEPROM.begin((size_t)EEPROM_SIZE); - - if (!read()) { - // The current eeprom format is incompatible, so we're going to default initialize MIDASEEPROM - MIDASEEPROM empty_setting; - empty_setting.checksum = EEPROM_CHECKSUM; - data = empty_setting; - Serial.println("EEPROM CHECKSUM INCOMPATIBLE"); - // Clear EEPROM and write correct checksum - commit(); - } - - return ErrorCode::NoError; -} - diff --git a/MIDAS/src/esp_eeprom_format.h b/MIDAS/src/esp_eeprom_format.h deleted file mode 100644 index 6b464d4a..00000000 --- a/MIDAS/src/esp_eeprom_format.h +++ /dev/null @@ -1,20 +0,0 @@ -#pragma once -#include "sensor_data.h" -#include "finite-state-machines/fsm_config.h" -/* @brief This file stores the schema for MIDAS EEPROM data. It is used to generate the EEPROM checksum */ - -struct MIDASEEPROM { - uint32_t checksum; - - uint16_t sd_file_num_last = 0; - - uint8_t serial = 0; - - float frequency = 421.15; - - Acceleration lsm6dsv320x_hg_xl_bias = {0.0f, 0.0f, 0.0f}; - Magnetometer mmc5983ma_softiron_bias = {1.0f, 1.0f, 1.0f}; - Magnetometer mmc5983ma_hardiron_bias = {0.0f, 0.0f, 0.0f}; - - FSMConfiguration fsm_config; -}; \ No newline at end of file diff --git a/MIDAS/src/command_flags.h b/MIDAS/src/finite-state-machines/command_flags.h similarity index 100% rename from MIDAS/src/command_flags.h rename to MIDAS/src/finite-state-machines/command_flags.h diff --git a/MIDAS/src/finite-state-machines/fsm.cpp b/MIDAS/src/finite-state-machines/fsm.cpp index 256da2c6..be0f5d5b 100644 --- a/MIDAS/src/finite-state-machines/fsm.cpp +++ b/MIDAS/src/finite-state-machines/fsm.cpp @@ -5,15 +5,33 @@ #include "thresholds.h" #include "CRC.h" + bool FSMPyroAction::conditions_met(FSMState fsm_state, float cur_tilt, uint8_t cur_motor, float cur_time_since_launch, float cur_vx) const { + // Pyro must be enabled. if (!enable) { return false; } + + // Verify the required FSM state. if (fsm_state != fsm_trigger) { return false; } + + // Enforce maximum allowable tilt if configured. if (max_tilt != -1 && cur_tilt > max_tilt) { return false; } + + // Require the specified motor/stage to have completed. if (after_motor != 0 && cur_motor < after_motor) { return false; } + + // Check lower launch-time bound if enabled. if (launch_t_gt != -1 && cur_time_since_launch < launch_t_gt) { return false; } + + // Check upper launch-time bound if enabled. if (launch_t_lt != -1 && cur_time_since_launch > launch_t_lt) { return false; } + + // Check minimum horizontal velocity if enabled. if (vx_min != -1 && cur_vx < vx_min) { return false; } + + // Check maximum horizontal velocity if enabled. if (vx_max != -1 && cur_vx > vx_max) { return false; } + + // Every configured condition has passed. return true; } @@ -23,54 +41,87 @@ bool FSMPyroAction::soft_conditions_met(FSMState fsm_state) const { return true; } + uint32_t FSMConfiguration::calculate_crc(const FSMConfiguration& cfg) { constexpr size_t FSMConfigurationCRCSize = sizeof(FSMUserThresholds) + (sizeof(FSMPyroAction) * MIDAS_NUM_PYROS) + sizeof(uint8_t); uint8_t crc_buf[FSMConfigurationCRCSize]; + // Serialize the threshold configuration. size_t ptr = 0; memcpy(crc_buf + ptr, &cfg.thresholds, sizeof(FSMUserThresholds)); ptr += sizeof(FSMUserThresholds); + + // Serialize every configured pyro action. for (int i = 0; i < MIDAS_NUM_PYROS; i++) { memcpy(crc_buf + ptr, &cfg.pyro_actions[i], sizeof(FSMPyroAction)); ptr += sizeof(FSMPyroAction); } + + // Serialize the configuration version. memcpy(crc_buf + ptr, &cfg.version_num, sizeof(uint8_t)); + // Calculate the CRC32 over the serialized configuration. return CRC::Calculate(crc_buf, FSMConfigurationCRCSize, CRC::CRC_32()); } +/** + * @brief Ensure the configuration remains small enough for storage. + * + * This compile-time assertion prevents the configuration structure from + * exceeding the maximum supported size. + */ static_assert(sizeof(FSMConfiguration) <= 256); #ifdef FSM_SIMULATOR -#include "sensor_data.h" +#include "flight-systems/sensor_data.h" #include "command_flags.h" #else -#include "rocket_state.h" +#include "flight-systems/rocket_state.h" /** - * @brief Helper to calculate the average value of a buffered sensor data + * @brief Computes the arithmetic mean of the most recent sensor samples. * - * @param sensor Buffered sensor struct - * @param get_item Lambda get function + * Retrieves the most recent @p count entries from a buffered sensor and + * averages the value returned by the supplied accessor function. * - * @return Average value -*/ + * @tparam T Sensor data type. + * @tparam count Number of recent samples to average. + * + * @param sensor Buffered sensor data source. + * @param get_item Function that extracts the desired value from each sample. + * + * @return Average value over the selected samples. + */ template double sensor_average(BufferedSensorData& sensor, double (* get_item)(T&)) { auto arr = sensor.template getBufferRecent(); double sum = 0.0; + + // Accumulate the selected sensor quantity. for (T& item : arr) { sum += get_item(item); } + + // Return the arithmetic mean. return sum / count; } /** - * @brief Helper to calculate the derivative over a buffered sensor data + * @brief Estimates the first derivative of buffered sensor data. * - * @param sensor Buffered sensor struct - * @param get_item Lambda get function + * The derivative is computed by averaging the first half and second half + * of the buffered samples independently, then dividing the difference in + * their average values by the difference in their average timestamps. * - * @return Derivative -*/ + * This approach reduces the effect of measurement noise compared to using + * only two individual samples. + * + * @tparam T Sensor data type. + * @tparam count Number of samples used in the estimate. + * + * @param sensor Buffered sensor data source. + * @param get_item Function that extracts the desired value from each sample. + * + * @return Estimated derivative. + */ template double sensor_derivative(BufferedSensorData& sensor, double (* get_item)(T&)) { auto arr = sensor.template getBufferRecent(); @@ -79,37 +130,58 @@ double sensor_derivative(BufferedSensorData& sensor, double (* get_ite double first_average = 0.0; double first_average_time = 0.0; + + // Average the first half of the samples. for (; i < count / 2; i++) { first_average += get_item(arr[i]); first_average_time += pdTICKS_TO_MS(times[i]) / 1000.0; } + first_average /= (count / 2.0); first_average_time /= (count / 2.0); double second_average = 0.0; double second_average_time = 0.0; + + // Average the second half of the samples. for (; i < count; i++) { second_average += get_item(arr[i]); second_average_time += pdTICKS_TO_MS(times[i]) / 1000.0; } + second_average /= (count / 2.0); second_average_time /= (count / 2.0); + + // Compute the slope between the averaged sample groups. return (second_average - first_average) / (second_average_time - first_average_time); } /** - * @brief Populates StateEstimate struct with the correct values for accel, alt, jerk, and speed -*/ + * @brief Constructs a state estimate from buffered sensor data. + * + * The estimate uses moving averages and finite-difference derivatives to + * produce filtered values for acceleration, altitude, jerk, and vertical + * speed. These values are subsequently used by the flight state machine. + * + * @param state Current rocket sensor data. + */ StateEstimate::StateEstimate(RocketData& state) { + // Average longitudinal acceleration. acceleration = sensor_average(state.imu, [](IMU& data) { return (double) data.highg_acceleration.ax; //CHECK WITH DIVIJ/MICHAEL }); + + // Average altitude. altitude = sensor_average(state.barometer, [](Barometer& data) { return (double) data.altitude; }); + + // Estimate longitudinal jerk. jerk = sensor_derivative(state.imu, [](IMU& data) { return (double) data.highg_acceleration.ax; //CHECK WITH DIVIJ/MICHAEL }); + + // Estimate vertical velocity from altitude history. vertical_speed = sensor_derivative(state.barometer, [](Barometer& data) { return (double) data.altitude; }); @@ -118,24 +190,20 @@ StateEstimate::StateEstimate(RocketData& state) { bool FSM::set_cfg(const FSMConfiguration& new_cfg) { // Check if the new config has a valid CRC. - + uint32_t cfg_crc = new_cfg.crc32; uint32_t calculated_crc = FSMConfiguration::calculate_crc(new_cfg); + // Reject corrupted or invalid configurations. if(cfg_crc != calculated_crc) { return false; } + + // Store the validated configuration. config = new_cfg; return true; } -/** - * @brief FSM tick function, which will advance the current state if necessary - * - * @param state current FSM state - * @param fsm_data Struct containing all data for FSM transitions - * - * @return New FSM State -*/ FSMData FSM::tick_fsm(FSMTickData& fsm_data) { + // Cache commonly used values locally for readability and efficiency. double current_time = fsm_data.cur_time; FSMData& state_data = fsm_data.cur_state; FSMState& state = state_data.state; @@ -146,8 +214,18 @@ FSMData FSM::tick_fsm(FSMTickData& fsm_data) { const KalmanData& kf_data = fsm_data.kf_data; switch(state) { + + /** + * ============================================================ + * SAFE STATE + * ============================================================ + * + * The vehicle is fully disarmed. The only permitted transitions + * are into PYRO_TEST mode or ARMED mode through explicit commands. + */ case FSMState::STATE_SAFE: - // Deconflict if multiple commands are processed + + // Deconflict if multiple commands are processed simultaneously. if(commands.should_transition_safe) { commands.should_transition_pyro_test = false; commands.should_transition_armed = false; @@ -155,14 +233,14 @@ FSMData FSM::tick_fsm(FSMTickData& fsm_data) { break; } - // Only switch to STATE_PYRO_TEST if triggered wirelessly + // Enter pyro test mode only through an explicit command. if(commands.should_transition_pyro_test) { state = FSMState::STATE_PYRO_TEST; pyro_test_entry_time = current_time; commands.should_transition_pyro_test = false; } - // Only switch to STATE_ARMED if triggered wirelessly. + // Arm the flight computer and prepare for launch detection. if(commands.should_transition_armed) { state = FSMState::STATE_ARMED; current_motor = 0; @@ -170,9 +248,18 @@ FSMData FSM::tick_fsm(FSMTickData& fsm_data) { } break; + + /** + * ============================================================ + * PYRO TEST STATE + * ============================================================ + * + * Manual pyro testing mode. The system automatically exits after + * a timeout or immediately when commanded back to SAFE. + */ case FSMState::STATE_PYRO_TEST: - // Force transtion to safe if requested + clear all transition flags. + // Immediately return to SAFE if requested. if(commands.should_transition_safe) { state = FSMState::STATE_SAFE; commands.should_transition_pyro_test = false; @@ -181,16 +268,25 @@ FSMData FSM::tick_fsm(FSMTickData& fsm_data) { break; } - // Switch back to STATE_SAFE after a certain amount of time passes + // Automatically disarm after the configured timeout. if((current_time - pyro_test_entry_time) > fsms_pt_disarm_t) { commands.should_transition_pyro_test = false; state = FSMState::STATE_SAFE; } break; + + /** + * ============================================================ + * ARMED STATE + * ============================================================ + * + * Waiting on the launch pad for sustained acceleration that + * indicates motor ignition. + */ case FSMState::STATE_ARMED: - // Force transtion to safe if requested + clear all transition flags. + // Allow immediate disarm if commanded. if(commands.should_transition_safe) { state = FSMState::STATE_SAFE; commands.should_transition_pyro_test = false; @@ -199,142 +295,260 @@ FSMData FSM::tick_fsm(FSMTickData& fsm_data) { break; } - // once a significant amount of acceleration is detected change states + // Detect launch using longitudinal acceleration. if (state_estimate.acceleration > fsms_boost_xl) { - launch_time = current_time; // Record launch time - time_entered_cur_state = current_time; // Record time for false boost detect - commands.FSM_should_set_cam_feed_cam1 = true; // Set camera to side cam - cur_state_lockin = false; // Reset "state lock in" flag for boost detect + launch_time = current_time; // Record launch time. + time_entered_cur_state = current_time; // Record BOOST entry time. + commands.FSM_should_set_cam_feed_cam1 = true; // Switch camera feed. + cur_state_lockin = false; // Begin false-detection validation. state = FSMState::STATE_BOOST; } break; + + /** + * ============================================================ + * BOOST STATE + * ============================================================ + * + * The rocket is under motor thrust. This state validates that + * ignition is genuine before locking in the boost phase and + * later detects motor burnout. + */ case FSMState::STATE_BOOST: - // -- BEGIN FALSE MOTOR DETECTION -- + + // -------------------------------------------------------- + // FALSE MOTOR IGNITION DETECTION + // -------------------------------------------------------- + // + // During the lock-in period, acceleration must remain above + // the boost threshold. Otherwise, the launch detection is + // considered false and the FSM returns to the previous state. + // + // -------------------------------------------------------- if(!cur_state_lockin && (current_time - time_entered_cur_state) < fsms_boost_lockin_t) { - // The state has not been locked in yet, so we should check if we still meet the conditions + if(state_estimate.acceleration < fsms_boost_xl) { state = (current_motor == 0) ? FSMState::STATE_ARMED : FSMState::STATE_COAST; time_entered_cur_state = current_time; break; } + } else { + + // Boost has remained valid long enough to be accepted. if(!cur_state_lockin) { - current_motor++; // When "BOOST" gets locked in, we increment the current motor. + current_motor++; // Count the completed ignition. cur_state_lockin = true; } } - // -- END FALSE MOTOR DETECTION -- - - // Burnout detection + // -------------------------------------------------------- + // MOTOR BURNOUT DETECTION + // -------------------------------------------------------- + // + // Once acceleration falls below the burnout threshold, begin + // the coast phase while allowing false burnout validation. + // + // -------------------------------------------------------- if (state_estimate.acceleration < fsms_burnout_xl) { time_entered_cur_state = current_time; state = FSMState::STATE_COAST; - cur_state_lockin = false; // Reset "state lock in" flag for false burnout detection + cur_state_lockin = false; // Prepare burnout validation. } + break; + + /** + * ============================================================ + * COAST STATE + * ============================================================ + * + * The rocket has completed a motor burn and is coasting. This + * state validates burnout, detects subsequent stage ignitions for + * multistage flights, and determines when apogee has been reached. + */ case FSMState::STATE_COAST: { - // if low acceleration is too brief than go on to the previous state - // -- BEGIN FALSE BURNOUT DETECTION -- + // -------------------------------------------------------- + // FALSE BURNOUT DETECTION + // -------------------------------------------------------- + // + // Immediately after entering COAST, require acceleration to + // remain below the burnout threshold for the configured + // lock-in period. If thrust resumes before then, treat the + // burnout as a false detection and return to BOOST. + // if(!cur_state_lockin && (current_time - time_entered_cur_state) < fsms_burnout_lockin_t) { - // The state has not been locked in yet, so we should check if we still meet the conditions + + // The burnout has not yet been confirmed. if(state_estimate.acceleration >= fsms_burnout_xl) { state = FSMState::STATE_BOOST; - // Note: We do not reset the "time since entering current state" timer, as we're assuming we're still in the previous boost phase. - cur_state_lockin = true; // We are going back to a previously "locked-in" state. + + // Continue timing from the original boost phase. + cur_state_lockin = true; + break; } + } else { + + // Burnout has remained valid long enough to be accepted. cur_state_lockin = true; } - // -- END FALSE BURNOUT DETECTION -- - // -- BEGIN NEXT STAGE IGNITION DETECT -- - // After burnout is confirmed, check for another motor ignition (multistage) + // -------------------------------------------------------- + // NEXT STAGE IGNITION DETECTION + // -------------------------------------------------------- + // + // After burnout has been confirmed, monitor acceleration for + // another ignition event. This supports multistage vehicles. + // if(cur_state_lockin && state_estimate.acceleration > fsms_boost_xl) { time_entered_cur_state = current_time; cur_state_lockin = false; - apogee_detect_start = 0; // Reset apogee detection + + // Restart apogee detection for the next powered stage. + apogee_detect_start = 0; + state = FSMState::STATE_BOOST; break; } - // -- END NEXT STAGE IGNITION DETECT -- - // -- BEGIN APOGEE DETECT -- - // Condition 1: Vertical speed low + // -------------------------------------------------------- + // APOGEE DETECTION + // -------------------------------------------------------- + // + // Apogee is declared only after: + // 1. Vertical speed falls below the configured threshold. + // 2. Cruise lockout (if enabled) has been satisfied. + // 3. Both conditions remain true for the required lock-in + // duration. + // + + // Condition 1: Vertical speed indicates the vehicle is near apogee. bool apog_detect_low_speed = (state_estimate.vertical_speed <= fsms_apogee_detect_spd); - // Condition 2: Cruise lockout - bool apog_detect_cruise_lockout = (!config.thresholds.cruise_lockout_en || kf_data.velocity.vx <= fsms_cruise_lockout_spd); - // Note: This evaluates to TRUE (no lockout) if the lockout is disabled, OR if the condition is met + // Condition 2: Cruise lockout requirement. + bool apog_detect_cruise_lockout = + (!config.thresholds.cruise_lockout_en || + kf_data.velocity.vx <= fsms_cruise_lockout_spd); + // Evaluate both apogee conditions. if (apog_detect_low_speed && apog_detect_cruise_lockout) { - // Start the consecutive timer if not already running + + // Begin the consecutive detection timer. if (apogee_detect_start == 0) { apogee_detect_start = current_time; } - // Transition to DROGUE only after conditions are met for fsms_apogee_lockin_t consecutive ms + // Declare apogee once the conditions have remained valid + // for the entire lock-in interval. if (current_time - apogee_detect_start >= fsms_apogee_lockin_t) { time_entered_cur_state = current_time; apogee_time = current_time; state = FSMState::STATE_DROGUE; apogee_detect_start = 0; + // Switch to the alternate camera after apogee. commands.FSM_should_swap_camera_feed = true; } + } else { - // Conditions not met: reset the timer + + // Restart the lock-in timer whenever either condition fails. apogee_detect_start = 0; } - // -- END APOGEE DETECT -- + break; } + /** + * ============================================================ + * DROGUE STATE + * ============================================================ + * + * The vehicle is descending under the drogue parachute. Once the + * configured deployment altitude is reached and the lockout period + * has elapsed, transition to MAIN. + */ case FSMState::STATE_DROGUE: - // Simple: If we hit the main threshold, deploy main. - // Make sure to respect the MAIN_LOCKOUT_T rule. - if(state_estimate.altitude <= config.thresholds.main_alt && (current_time - time_entered_cur_state) > fsms_main_lockout_t) { + // Deploy the main parachute once the altitude threshold and + // minimum lockout time have both been satisfied. + if(state_estimate.altitude <= config.thresholds.main_alt && + (current_time - time_entered_cur_state) > fsms_main_lockout_t) { + time_entered_cur_state = current_time; state = FSMState::STATE_MAIN; } + break; + /** + * ============================================================ + * MAIN STATE + * ============================================================ + * + * The vehicle is descending under the main parachute. Landing is + * detected by requiring a sufficiently low vertical speed for a + * sustained period after the post-apogee lockout expires. + */ case FSMState::STATE_MAIN: { + + // Determine whether the vehicle appears stationary. bool landed_speed = (abs(state_estimate.vertical_speed) <= fsms_landed_detect_spd); + + // Prevent premature landing detection immediately after apogee. bool landed_lockout_passed = (current_time - apogee_time) > fsms_landed_t_lockout; if (landed_speed && landed_lockout_passed) { + + // Begin the landing confirmation timer. if (landed_detect_start == 0) { landed_detect_start = current_time; } + // Confirm landing after the vehicle has remained stationary + // for the configured duration. if (current_time - landed_detect_start >= fsms_landed_entry_t) { time_entered_cur_state = current_time; cur_state_lockin = false; state = FSMState::STATE_LANDED; landed_detect_start = 0; } + } else { + + // Movement detected; restart the landing timer. landed_detect_start = 0; } + break; } + /** + * ============================================================ + * LANDED STATE + * ============================================================ + * + * The flight has completed. After a lock-in period the system + * enters its post-flight behavior, including enabling power-saving + * mode. If significant motion is detected again, the FSM returns + * to MAIN. + */ case FSMState::STATE_LANDED: - // Landing lock-in + + // Wait for the landing state to become fully locked in. if((current_time - time_entered_cur_state) > fsms_landed_t) { - + + // Perform one-time landing actions. if(!cur_state_lockin) { cur_state_lockin = true; commands.FSM_should_power_save = true; } - // Check for any telem transitions - // Force transtion to safe if requested + clear all transition flags. + // Allow a telemetry command to safely disarm the system. if(commands.should_transition_safe) { state = FSMState::STATE_SAFE; commands.should_transition_pyro_test = false; @@ -345,7 +559,7 @@ FSMData FSM::tick_fsm(FSMTickData& fsm_data) { break; } - // If we still detect falling movement, go back to MAIN. + // Resume the MAIN state if descent is detected again. if (abs(state_estimate.vertical_speed) > fsms_landed_detect_spd) { state = FSMState::STATE_MAIN; time_entered_cur_state = current_time; @@ -354,5 +568,6 @@ FSMData FSM::tick_fsm(FSMTickData& fsm_data) { break; } + // Return the updated FSM state information. return state_data; } \ No newline at end of file diff --git a/MIDAS/src/finite-state-machines/fsm.h b/MIDAS/src/finite-state-machines/fsm.h index eddff0e1..b1d681f6 100644 --- a/MIDAS/src/finite-state-machines/fsm.h +++ b/MIDAS/src/finite-state-machines/fsm.h @@ -4,7 +4,7 @@ #include "fsm_config.h" #include "thresholds.h" -#include "Buffer.h" +#include "util/Buffer.h" struct RocketData; struct CommandFlags; @@ -48,28 +48,85 @@ class FSM { public: FSM() = default; + /** + * @brief Advances the finite state machine by one update cycle. + * + * This function evaluates the current flight conditions and command inputs, + * performs any required state transitions, updates internal timing variables, + * and returns the resulting FSM state. + * + * @param fsm_data Structure containing all data required for FSM evaluation. + * + * @return Updated FSM state information. + */ FSMData tick_fsm(FSMTickData& fsm_data); + /** + * @brief Returns the recorded launch time. + * + * @return Launch time in seconds. + */ double get_launch_time() { return launch_time; } - // Gets the FSM configuration in a non-writeable way. + /** + * @brief Returns the current FSM configuration. + * + * The returned reference is read-only. + * + * @return Constant reference to the active configuration. + */ const FSMConfiguration& get_cfg() const { return config; } - // Attempts to set the FSM configuration. Returns TRUE if successful & CRC checks pass. + /** + * @brief Loads a new FSM configuration after validating its CRC. + * + * The supplied configuration is accepted only if its stored CRC matches + * the CRC computed from its contents. Invalid configurations are rejected + * without modifying the currently active configuration. + * + * @param new_config Configuration to install. + * + * @return True if the configuration was successfully validated and stored. + */ bool set_cfg(const FSMConfiguration& new_config); - // Sets CRC to the fail state, preventing any pyro action. + /** + * @brief Forces the configuration into the CRC failure state. + * + * Setting the CRC to FSM_CRC_FAIL_STATE disables all configured + * pyro actions as a safety measure. + */ void set_crc_fail() { config.crc32 = FSM_CRC_FAIL_STATE; }; + /** + * @brief Returns whether the current state has been locked in. + * + * @return True if the current state's lock-in period has completed. + */ bool get_cur_state_lockin() { return cur_state_lockin; } private: + /// Active FSM configuration. FSMConfiguration config; + + /// Time at which PYRO_TEST mode was entered. double pyro_test_entry_time; + + /// Recorded launch time. double launch_time; + + /// Recorded apogee time. double apogee_time; + + /// Time the current FSM state was entered. double time_entered_cur_state; - double apogee_detect_start = 0; // (0 = not active) - double landed_detect_start = 0; // (0 = not active) + + /// Time at which apogee detection began (0 indicates inactive). + double apogee_detect_start = 0; + + /// Time at which landing detection began (0 indicates inactive). + double landed_detect_start = 0; + + /// Indicates whether the current state's lock-in period has completed. bool cur_state_lockin; }; diff --git a/MIDAS/src/finite-state-machines/fsm_config.h b/MIDAS/src/finite-state-machines/fsm_config.h index e78f374d..23694823 100644 --- a/MIDAS/src/finite-state-machines/fsm_config.h +++ b/MIDAS/src/finite-state-machines/fsm_config.h @@ -4,35 +4,120 @@ #define MIDAS_NUM_PYROS 4 +// A CRC value of 0 indicates an invalid or uninitialized configuration. +// When detected, all pyro actions are disabled as a safety precaution. #define FSM_CRC_FAIL_STATE ((uint32_t)0) +/** + * @struct FSMPyroAction + * + * @brief Defines the firing conditions for a single pyro channel. + * + * Each pyro output can be independently configured to activate only when + * all specified flight conditions are satisfied. These conditions include + * flight state, vehicle orientation, motor count, elapsed launch time, + * horizontal velocity, and an optional firing delay. + */ struct FSMPyroAction { + /// Enables or disables this pyro action. bool enable; + + /// FSM state required before this pyro may fire. FSMState fsm_trigger; + + /// Maximum allowable tilt angle (degrees) for activation. float max_tilt; + + /// Minimum number of completed motor burns required. uint8_t after_motor; + + /// Minimum allowable time since launch (seconds). float launch_t_gt; + + /// Maximum allowable time since launch (seconds). float launch_t_lt; + + /// Minimum allowable horizontal velocity. float vx_min; + + /// Maximum allowable horizontal velocity. float vx_max; + /// Delay (seconds) after all conditions become true before firing. double delay; + /** + * @brief Checks whether all configured firing conditions are satisfied. + * + * @param fsm_state Current finite state machine state. + * @param cur_tilt Current vehicle tilt. + * @param cur_motor Current motor count or stage. + * @param cur_time_since_launch Time since launch in seconds. + * @param cur_vx Current horizontal velocity. + * + * @return True if every configured firing condition is met. + */ bool conditions_met(FSMState fsm_state, float cur_tilt, uint8_t cur_motor, float cur_time_since_launch, float cur_vx) const; + + /** + * @brief Checks only the FSM state requirement. + * + * Useful for determining whether a pyro is eligible based solely on + * the current flight state without evaluating other constraints. + * + * @param fsm_state Current finite state machine state. + * @return True if the FSM state requirement is satisfied. + */ bool soft_conditions_met(FSMState fsm_state) const; }; +/** + * @struct FSMUserThresholds + * + * @brief User-configurable flight thresholds used by the FSM. + */ struct FSMUserThresholds { + /// Time (seconds) that pyro outputs remain active after firing. float pyro_fire_t; + + /// Main parachute deployment altitude. float main_alt; + + /// Enables cruise-phase deployment lockout. bool cruise_lockout_en; }; +/** + * @struct FSMConfiguration + * + * @brief Complete finite state machine configuration. + * + * Contains all user-configurable thresholds, pyro action definitions, + * configuration version information, and an integrity-check CRC. + */ struct FSMConfiguration { + /// User-configurable FSM thresholds. FSMUserThresholds thresholds; + + /// Configuration for each available pyro channel. FSMPyroAction pyro_actions[MIDAS_NUM_PYROS]; + + /// Configuration format version. uint8_t version_num; - uint32_t crc32; // The configuration CRC. If this value is 0, all pyro actions are disabled! + /** + * @brief CRC32 checksum of the configuration. + * + * If this value equals FSM_CRC_FAIL_STATE (0), the configuration is + * considered invalid and all pyro actions are disabled. + */ + uint32_t crc32; + + /** + * @brief Computes the CRC32 checksum for a configuration. + * + * @param cfg Configuration to compute the checksum for. + * @return CRC32 checksum. + */ static uint32_t calculate_crc(const FSMConfiguration& cfg); -}; +}; \ No newline at end of file diff --git a/MIDAS/src/finite-state-machines/fsm_states.h b/MIDAS/src/finite-state-machines/fsm_states.h index fcc714b4..ea99f64b 100644 --- a/MIDAS/src/finite-state-machines/fsm_states.h +++ b/MIDAS/src/finite-state-machines/fsm_states.h @@ -5,9 +5,12 @@ /** * @enum FSMState * - * @brief Enum for the different FSM states -*/ - + * @brief Enumerates all possible finite state machine (FSM) flight states. + * + * These states represent the major phases of flight, from pre-launch + * through landing. The FSM transitions between these states based on + * sensor data and configured flight logic. + */ enum FSMState { STATE_SAFE, // SAFE state STATE_PYRO_TEST, // Unsafe pyro testing state (accepts pyro firing commands) @@ -20,7 +23,20 @@ enum FSMState { FSM_STATE_COUNT // used to get the total number of fsm states so we can assert that the fsm will fit in 4 bits }; +/** + * @struct FSMData + * + * @brief Stores the current state of the finite state machine. + * + * This structure tracks the active flight state along with the current + * motor or stage index, allowing other subsystems to monitor the rocket's + * flight progress. + */ struct FSMData { + + /// Current finite state machine state. FSMState state; + + /// Index of the currently active or most recently completed motor. uint8_t current_motor; }; diff --git a/MIDAS/src/finite-state-machines/pyro_eval.h b/MIDAS/src/finite-state-machines/pyro_eval.h index a6ab076f..1bb451ea 100644 --- a/MIDAS/src/finite-state-machines/pyro_eval.h +++ b/MIDAS/src/finite-state-machines/pyro_eval.h @@ -2,33 +2,96 @@ /** * @author Michael Karpov (2026) - * @brief Evaluates pyro firing state for the MIDAS V2 FSM + * @brief Evaluates pyro firing state for the MIDAS V2 FSM. */ #include #include "fsm.h" -#include "sensor_data.h" +#include "flight-systems/sensor_data.h" +/** + * @struct PyroEvalState + * + * @brief Stores persistent runtime state for each pyro channel between + * consecutive calls to `pyro_eval()`. + * + * This structure tracks when each pyro event was first triggered, whether + * its post-delay condition check has been performed, and whether the event + * has already completed and should no longer be evaluated. + */ struct PyroEvalState { + /** + * @brief Timestamp when each pyro event was first triggered. + * + * A value of 0 indicates the event has not yet been triggered. + */ double trigger_times[MIDAS_NUM_PYROS] = {}; + + /** + * @brief Indicates whether the post-delay validation check has occurred. + */ bool event_check[MIDAS_NUM_PYROS] = {}; + + /** + * @brief Indicates whether each pyro event has finished and should no + * longer be evaluated. + */ bool event_consumed[MIDAS_NUM_PYROS] = {}; }; +/** + * @struct PyroEvalResult + * + * @brief Contains the evaluation results for each pyro channel after a + * single call to `pyro_eval()`. + */ struct PyroEvalResult { + /** + * @brief True if the corresponding pyro event has been triggered. + */ bool event_triggered[MIDAS_NUM_PYROS] = {}; + + /** + * @brief True while the corresponding pyro output should be actively firing. + */ bool channel_firing[MIDAS_NUM_PYROS] = {}; + + /** + * @brief True if the corresponding pyro event has completed and should no + * longer be evaluated. + */ bool event_consumed[MIDAS_NUM_PYROS] = {}; }; /** - * @brief Shared in-flight eval logic + * @brief Evaluates the state of every configured pyro channel. + * + * This function manages the complete lifecycle of each pyro event, + * including trigger detection, delay timing, optional condition + * revalidation, output firing, event completion, and soft reset handling. + * + * The supplied `PyroEvalState` is updated in-place and should be preserved + * between consecutive calls throughout flight. + * + * @param config FSM configuration containing pyro actions and timing thresholds. + * @param fsm Current finite state machine data. + * @param tilt_deg Current rocket tilt angle in degrees. + * @param time_since_launch Elapsed time since launch in seconds. + * @param vx Current vertical velocity. + * @param current_time Current system time used for timing delays. + * @param state Persistent pyro evaluation state. + * + * @return A `PyroEvalResult` describing which events are triggered, + * actively firing, and consumed after this evaluation. */ inline PyroEvalResult pyro_eval(const FSMConfiguration& config, const FSMData& fsm, double tilt_deg, float time_since_launch, float vx, double current_time, PyroEvalState& state) { PyroEvalResult result = {}; + // Evaluate each configured pyro channel independently. for (int i = 0; i < MIDAS_NUM_PYROS; i++) { result.event_consumed[i] = state.event_consumed[i]; + + // Skip pyros whose events have already completed. if (state.event_consumed[i]) { result.channel_firing[i] = false; result.event_triggered[i] = false; @@ -37,14 +100,23 @@ inline PyroEvalResult pyro_eval(const FSMConfiguration& config, const FSMData& f const FSMPyroAction& act = config.pyro_actions[i]; + // Wait for the pyro's trigger conditions to become true. if (state.trigger_times[i] == 0) { if (act.conditions_met(fsm.state, tilt_deg, fsm.current_motor, time_since_launch, vx)) { + // Record the trigger time to begin the delay countdown. state.trigger_times[i] = current_time; result.event_triggered[i] = true; } } else { + // The event has already been triggered. result.event_triggered[i] = true; + + // Wait until the configured firing delay has elapsed. if (current_time - state.trigger_times[i] >= act.delay) { + + // Before firing, optionally verify that the trigger + // conditions are still satisfied. If they are no longer + // valid, cancel the event permanently. if (!state.event_check[i] && !act.conditions_met(fsm.state, tilt_deg, fsm.current_motor, time_since_launch, vx)) { state.event_consumed[i] = true; result.event_consumed[i] = true; @@ -52,15 +124,20 @@ inline PyroEvalResult pyro_eval(const FSMConfiguration& config, const FSMData& f continue; } + // Mark that the verification step has completed and + // energize the pyro channel. state.event_check[i] = true; result.channel_firing[i] = true; + // Stop firing once the configured pulse duration expires. if (current_time - state.trigger_times[i] >= act.delay + config.thresholds.pyro_fire_t) { state.event_consumed[i] = true; result.event_consumed[i] = true; result.channel_firing[i] = false; } } else { + // Prior to the firing delay expiring, allow the event to + // reset if the FSM leaves the qualifying state. if(!act.soft_conditions_met(fsm.state)) { // Soft reset the pyro event. state.trigger_times[i] = 0; @@ -70,4 +147,4 @@ inline PyroEvalResult pyro_eval(const FSMConfiguration& config, const FSMData& f } return result; -} +} \ No newline at end of file diff --git a/MIDAS/src/rocket_state.h b/MIDAS/src/flight-systems/rocket_state.h similarity index 88% rename from MIDAS/src/rocket_state.h rename to MIDAS/src/flight-systems/rocket_state.h index bb3c98d9..c3413598 100644 --- a/MIDAS/src/rocket_state.h +++ b/MIDAS/src/flight-systems/rocket_state.h @@ -2,10 +2,10 @@ #include -#include "sensor_data.h" -#include "hal.h" -#include "Buffer.h" -#include "data_logging_meta.h" +#include "flight-systems/sensor_data.h" +#include "util/hal.h" +#include "util/Buffer.h" +#include "logging/data_logging_meta.h" #include "finite-state-machines/fsm.h" /** @@ -66,7 +66,7 @@ struct SensorData { }; /** - * @brief gets most recent data, will not acquire lock + * @brief gets most recent data without acquiring lock * * @return the most recent data */ @@ -112,14 +112,23 @@ struct BufferedSensorData : public SensorData { data_time.push(xTaskGetTickCount()); }; - // wrapper function to get easy access to buffer data template + /** + * @brief wrapper function to get easy access to buffer data + * + * @return recent buffer data + */ std::array getBufferRecent() { std::array arr = buffer. template read_recent(); return arr; }; template + /** + * @brief wrapper function to get easy access to data times + * + * @return recent buffer times + */ std::array getTimesRecent() { std::array arr = data_time. template read_recent(); return arr; @@ -146,16 +155,16 @@ class Latency { } /** - * @brief gets the msot recent latency + * @brief gets the most recent latency * - * @return most receent latency + * @return most recent latency */ [[nodiscard]] uint32_t getLatency() const { return latency; } }; -#include "command_flags.h" +#include "finite-state-machines/command_flags.h" /** * @struct RocketData diff --git a/MIDAS/src/sensor_data.h b/MIDAS/src/flight-systems/sensor_data.h similarity index 84% rename from MIDAS/src/sensor_data.h rename to MIDAS/src/flight-systems/sensor_data.h index 28da150c..1137b7c2 100644 --- a/MIDAS/src/sensor_data.h +++ b/MIDAS/src/flight-systems/sensor_data.h @@ -9,7 +9,7 @@ /** * @brief - * This header provides all the implementation for the data that comes from all of the sensors/ + * This header provides all the implementation for the data that comes from all of the sensors. * These structs will be individual packets of data passed between the sensor and the * rocket_state struct, and each will be tagged with a timestamp. */ @@ -22,29 +22,51 @@ struct Vec3 { float y = 0; float z = 0; }; - +/** + * @struct Position + * + * @brief stores a position vector with px, py, pz coordinates + */ struct Position { float px = 0; float py = 0; float pz = 0; }; +/** + * @struct Velocity + * + * @brief stores a velocity vector with vx, vy, vz coordinates + */ struct Velocity { float vx = 0; float vy = 0; float vz = 0; + /** + * @brief calculates and returns the absolute speed + * + * @return the absolute speed of the Velocity vector + */ float get_speed() { return sqrt(vx * vx + vy * vy + vz * vz); } }; +/** + * @struct Acceleration + * @brief stores an acceleration vector with ax, ay, az coordinates + */ struct Acceleration { float ax = 0; float ay = 0; float az = 0; - // Get G-Force applied on the rocket + /** + * @brief calculates and returns absolute acceleration (G-force) + * + * @return absolute acceleration (G-force) of the acceleration vector + */ float get_magnitude() { return sqrt(ax * ax + ay * ay + az * az); } @@ -106,7 +128,7 @@ struct GPS { // Unix timestamp since 1970 // This isn't included in the telem packet because this is // solely for the SD logger. We do not need to know what time it is - // when we are recieving telem packets. + // when we are receiving telem packets. uint32_t time; }; @@ -124,6 +146,12 @@ struct Magnetometer { struct Quaternion { //long term, remove or rename this struct, it will conflict with libraries where Quaternion is well defined. float w, x, y, z; + /** + * @brief calculates the quaternion dot product between the two parameters + * + * @param q1 first Quaternion + * @param q2 second Quaternion + */ static float dot(const Quaternion& q1, const Quaternion& q2) { return q1.w * q2.w + q1.x * q2.x + q1.y * q2.y + q1.z * q2.z; } @@ -131,7 +159,8 @@ struct Quaternion { //long term, remove or rename this struct, it will conflict }; /** - * @enum Orientation reading type + * @enum OrientationReadingType + * @brief represents the two possible types of orientation readings */ enum class OrientationReadingType { FULL_READING = 0, @@ -139,7 +168,7 @@ enum class OrientationReadingType { }; /** - * @struct SFLP + * @struct IMU_SFLP * * @brief Data from the LSM6DSV320X Sensor Fusion Low Power module * @@ -155,7 +184,7 @@ struct IMU_SFLP { * * @struct IMU * - * @brief IMU that stores High/Low G Acceleration, Angular Velocity, and IMU_SFLP + * @brief stores High/Low G Acceleration, Angular Velocity * */ struct IMU { @@ -257,7 +286,7 @@ struct KalmanData { }; /** - * @struct KalmanData + * @struct AngularKalmanData * * @brief data from the MQEKF thread */ @@ -272,6 +301,9 @@ struct AngularKalmanData { float pitch = 0; float roll = 0; // For yessir.cpp + /** + * @brief returns the euler representation of the current Kalman rotation vector + */ euler_t getEuler() const { euler_t euler; euler.yaw = this->yaw; @@ -301,7 +333,11 @@ struct PyroState { */ }; - +/** + * @struct CameraData + * + * @brief stores data about the state of the camera + */ struct CameraData { uint8_t camera_state = 255; float camera_voltage = 0; diff --git a/MIDAS/src/systems.cpp b/MIDAS/src/flight-systems/systems.cpp similarity index 91% rename from MIDAS/src/systems.cpp rename to MIDAS/src/flight-systems/systems.cpp index 36570cf7..9d475f3b 100644 --- a/MIDAS/src/systems.cpp +++ b/MIDAS/src/flight-systems/systems.cpp @@ -1,10 +1,10 @@ -#include "systems.h" -#include "sensor_data.h" -#include "hal.h" +#include "flight-systems/systems.h" +#include "flight-systems/sensor_data.h" +#include "util/hal.h" #include "gnc/ekf.h" #include "gnc/mqekf.h" -#include "log_format_AUTOGEN.h" -#include "midas_shell_commands.h" +#include "logging/log_format_AUTOGEN.h" +#include "shell/midas_shell_commands.h" static StaticSemaphore_t spi_mutex_buffer; static StaticSemaphore_t i2c_mutex_buffer; @@ -15,6 +15,14 @@ SemaphoreHandle_t i2c_mutex; #define METALOG_TEST +/** + * @brief writes data to log provided, with metalogging + * + * @param log the log to log to + * @param node_size the size (in bytes) of each node in the tree + * @param node_count the number of nodes in the tree + * @param data the actual data to log + */ void log_parse_tree(LogSink& log, size_t node_size, size_t node_count, uint8_t* data) { // To recreate log formats, we need data to reconstruct the parse tree for log format // This means knowing the size of each node and the pre-order traversal. @@ -55,25 +63,27 @@ DECLARE_THREAD(logger, RocketSystems *arg) log_parse_tree(arg->log_sink, sizeof(LogDiscMapEntry), LOG_DISCMAP_COUNT, (uint8_t*)LOG_DISCMAP_TABLE); log_parse_tree(arg->log_sink, sizeof(LogFormatMetaEntry), EEPROM_META_ENTRY_COUNT, (uint8_t*)EEPROM_META_ENTRIES); + // Thread loop while (true) { - log_data(arg->log_sink, arg->rocket_data); + log_data(arg->log_sink, arg->rocket_data); // Actually log the data arg->rocket_data.log_latency.tick(); meta_delay_ctr++; MetaLogging::MetaLogEntry entry; - if (meta_delay_ctr >= 100) { + if (meta_delay_ctr >= 100) { // rate limit the metalogging + // meta_delay_ctr = 0; // uncomment to actually rate limit the metalogging if(arg->meta_logging.get_queued(&entry)) { uint8_t buf[72]; size_t total_size = sizeof(MetaDataCode) + entry.size; - memcpy(buf, &entry.log_type, sizeof(MetaDataCode)); - memcpy(buf + sizeof(MetaDataCode), &entry.data, entry.size); - arg->log_sink.write_meta(buf, total_size); + memcpy(buf, &entry.log_type, sizeof(MetaDataCode)); // add the log_type metadata into the buffer + memcpy(buf + sizeof(MetaDataCode), &entry.data, entry.size); // copy the data into the buffer + arg->log_sink.write_meta(buf, total_size); // actually metalog } } - arg->rocket_data.err_flags.log_wr_err = arg->log_sink.failed_wr; + arg->rocket_data.err_flags.log_wr_err = arg->log_sink.failed_wr; // mark errors on rocket_data struct arg->rocket_data.err_flags.log_mr_err = arg->log_sink.failed_mr; THREAD_SLEEP(1); @@ -156,14 +166,14 @@ DECLARE_THREAD(imuthread, RocketSystems *arg) } } - if(!has_logged) { + if(!has_logged) { // once rocket has landed, log the max accel and when it happened, but only log this once if(arg->rocket_data.fsm_state.getRecentUnsync().state == FSMState::STATE_LANDED) { arg->meta_logging.log_data(MetaDataCode::DATA_MAX_ACCEL, max_accel); arg->meta_logging.log_data(MetaDataCode::EVENT_TMAX_ACCEL, max_accel_time); has_logged = true; } } - + // update rocket_data struct arg->rocket_data.imu.update(imudata); arg->rocket_data.sflp.update(sflp); @@ -188,7 +198,7 @@ DECLARE_THREAD(magnetometer, RocketSystems* arg) { // Sensor biases Magnetometer b = arg->sensors.magnetometer.calibration_bias_hardiron; // "Hard iron" / origin offset. Magnetometer s = arg->sensors.magnetometer.calibration_bias_softiron; // "Soft iron" / scale offset. - reading.mx = (reading.mx - b.mx) / s.mx; + reading.mx = (reading.mx - b.mx) / s.mx; // adjusts the sensor data according to the calibration reading.my = (reading.my - b.my) / s.my; reading.mz = (reading.mz - b.mz) / s.mz; @@ -205,6 +215,7 @@ DECLARE_THREAD(gps, RocketSystems *arg) // GPS's internal operations have a xSemaphoreTake. if (arg->sensors.gps.valid()) { + // read and update the GPS data GPS reading = arg->sensors.gps.read(); arg->rocket_data.gps.update(reading); } @@ -218,14 +229,16 @@ DECLARE_THREAD(pyro, RocketSystems *arg) while (true) { - FSMData current_fsm = arg->rocket_data.fsm_state.getRecentUnsync(); + FSMData current_fsm = arg->rocket_data.fsm_state.getRecentUnsync(); AngularKalmanData akf_data = arg->rocket_data.angular_kalman_data.getRecentUnsync(); KalmanData ekf_data = arg->rocket_data.kalman.getRecentUnsync(); CommandFlags &command_flags = arg->rocket_data.command_flags; + double current_time = pdTICKS_TO_MS(xTaskGetTickCount()); double launch_time = arg->fsm.get_launch_time(); - double time_since_launch = (current_time - launch_time); + + double time_since_launch = (current_time - launch_time); const FSMConfiguration& fsm_cfg = arg->fsm.get_cfg(); PyroTickData tick_data = { current_fsm, @@ -237,15 +250,15 @@ DECLARE_THREAD(pyro, RocketSystems *arg) time_since_launch }; - PyroState new_pyro_state = arg->sensors.pyro.tick(tick_data); + PyroState new_pyro_state = arg->sensors.pyro.tick(tick_data); // Actually update the pyro state! - xSemaphoreTake(i2c_mutex, portMAX_DELAY); - gpioDigitalWrite(PYRO_GLOBAL_ARM_PIN, new_pyro_state.is_global_armed ? HIGH : LOW); + xSemaphoreTake(i2c_mutex, portMAX_DELAY); + gpioDigitalWrite(PYRO_GLOBAL_ARM_PIN, new_pyro_state.is_global_armed ? HIGH : LOW); for(int i = 0; i < MIDAS_NUM_PYROS; i++) { - gpioDigitalWrite(PYRO_PINS[i], new_pyro_state.channel_firing[i] ? HIGH : LOW); + gpioDigitalWrite(PYRO_PINS[i], new_pyro_state.channel_firing[i] ? HIGH : LOW); } - xSemaphoreGive(i2c_mutex); + xSemaphoreGive(i2c_mutex); arg->rocket_data.pyro.update(new_pyro_state); arg->led.update(); @@ -256,21 +269,22 @@ DECLARE_THREAD(pyro, RocketSystems *arg) DECLARE_THREAD(voltage, RocketSystems* arg) { while (true) { - Voltage reading2 = arg->sensors.voltage.read(); - arg->rocket_data.voltage.update(reading2); + Voltage reading2 = arg->sensors.voltage.read(); + arg->rocket_data.voltage.update(reading2); THREAD_SLEEP(100); } } -//run threads - +/** + * @brief is called to do all the logging for the new FSM state + */ void fsm_transitioned_to(FSMData& new_state, FSMData& old_state, RocketSystems* sys, double current_time) { // Do something, NO delays allowed! switch (new_state.state) { case FSMState::STATE_BOOST: //first stage specific logging - if(new_state.current_motor == 0){ + if(new_state.current_motor == 0) { sys->meta_logging.summary->event_tlaunch.update(current_time); sys->meta_logging.summary->data_launchsite_baro.update(sys->rocket_data.barometer.getRecentUnsync().altitude); @@ -282,7 +296,7 @@ void fsm_transitioned_to(FSMData& new_state, FSMData& old_state, RocketSystems* sys->meta_logging.summary->data_launch_initial_tilt.update(sys->rocket_data.angular_kalman_data.getRecentUnsync().mq_tilt); } //all other stages logging - else{ + else { sys->meta_logging.summary->event_tignition.update(current_time); sys->meta_logging.summary->data_tilt_at_ignition.update(sys->rocket_data.angular_kalman_data.getRecentUnsync().mq_tilt); sys->meta_logging.summary->data_baro_at_ignition.update(sys->rocket_data.barometer.getRecentUnsync().altitude); @@ -309,7 +323,9 @@ void fsm_transitioned_to(FSMData& new_state, FSMData& old_state, RocketSystems* break; } } - +/** + * @brief actually commits all the metalogs for the new fsm state transition + */ void fsm_state_commit(FSMData& current_state, RocketSystems* sys) { // Do something, NO delays allowed! @@ -377,29 +393,29 @@ DECLARE_THREAD(fsm, RocketSystems *arg) arg->rocket_data.fsm_state.update(FSMData{FSMState::STATE_SAFE, 0}); while (true) { - FSMData current_state_data = arg->rocket_data.fsm_state.getRecentUnsync(); - StateEstimate state_estimate(arg->rocket_data); + FSMData current_state_data = arg->rocket_data.fsm_state.getRecentUnsync(); + StateEstimate state_estimate(arg->rocket_data); CommandFlags &telemetry_commands = arg->rocket_data.command_flags; - KalmanData kfd = arg->rocket_data.kalman.getRecentUnsync(); + KalmanData kfd = arg->rocket_data.kalman.getRecentUnsync(); double current_time = pdTICKS_TO_MS(xTaskGetTickCount()); const FSMConfiguration& fsm_cfg = arg->fsm.get_cfg(); FSMState current_state = current_state_data.state; - bool last_lockin_state = fsm.get_cur_state_lockin(); + bool last_lockin_state = fsm.get_cur_state_lockin(); // has the current state been locked in yet? FSMTickData tick_data = {current_state_data, telemetry_commands, state_estimate, kfd, fsm_cfg, current_time}; - FSMData next_state = fsm.tick_fsm(tick_data); + FSMData next_state = fsm.tick_fsm(tick_data); - arg->rocket_data.fsm_state.update(next_state); + arg->rocket_data.fsm_state.update(next_state); if(current_state != next_state.state) { - fsm_transitioned_to(next_state, current_state_data, arg, current_time); + fsm_transitioned_to(next_state, current_state_data, arg, current_time); // log the transition } else { if (last_lockin_state != fsm.get_cur_state_lockin() ) { - fsm_state_commit(current_state_data, arg); + fsm_state_commit(current_state_data, arg); // once the new state is locked in, commit it to the system } } @@ -493,33 +509,33 @@ DECLARE_THREAD(buzzer, RocketSystems *arg) cont[i] = v.continuity[i] > 3.0; } - arg->buzzer.report_beeps(cont, fsm_fail); + arg->buzzer.report_beeps(cont, fsm_fail); // play relevant beeps based on the current state last_beep_beep = cur_time; } - arg->buzzer.tick(); + arg->buzzer.tick(); THREAD_SLEEP(10); } } // angularkalmandata needs updates DECLARE_THREAD(angularkalman, RocketSystems *arg) -{ // +{ mqekf.initialize(arg); - // Serial.println("Initialized mqekf :("); TickType_t last = xTaskGetTickCount(); while (true) { FSMState FSM_state = arg->rocket_data.fsm_state.getRecent().state; - if (arg->rocket_data.command_flags.should_reset_kf) + if (arg->rocket_data.command_flags.should_reset_kf) { mqekf.initialize(arg); TickType_t last = xTaskGetTickCount(); arg->rocket_data.command_flags.should_reset_kf = false; } - IMU_SFLP current_imu_sflp = arg->rocket_data.sflp.getRecent(); + + IMU_SFLP current_imu_sflp = arg->rocket_data.sflp.getRecent(); IMU current_imu = arg->rocket_data.imu.getRecent(); Acceleration current_high_g = current_imu.highg_acceleration; Acceleration current_low_g = current_imu.lowg_acceleration; @@ -538,7 +554,7 @@ DECLARE_THREAD(angularkalman, RocketSystems *arg) // Check with Divij mqekf.tick(dt, current_mag, current_angular_velocity, current_accelerations, FSM_state, current_gyro_bias); - mqekf.calculate_tilt(current_imu_sflp); + mqekf.calculate_tilt(current_imu_sflp); AngularKalmanData current_state = mqekf.getState(); arg->rocket_data.angular_kalman_data.update(current_state); @@ -552,7 +568,6 @@ DECLARE_THREAD(angularkalman, RocketSystems *arg) DECLARE_THREAD(kalman, RocketSystems *arg) { ekf.initialize(arg); - // Serial.println("Initialized ekf :("); TickType_t last = xTaskGetTickCount(); @@ -563,7 +578,7 @@ DECLARE_THREAD(kalman, RocketSystems *arg) while (true) { - if (arg->rocket_data.command_flags.should_reset_kf) + if (arg->rocket_data.command_flags.should_reset_kf) // check for command { ekf.initialize(arg); TickType_t last = xTaskGetTickCount(); @@ -584,12 +599,13 @@ DECLARE_THREAD(kalman, RocketSystems *arg) Acceleration current_accelerations = { .ax = current_high_g.ax, .ay = current_high_g.ay, - .az = current_high_g.az}; // + .az = current_high_g.az}; float dt = pdTICKS_TO_MS(xTaskGetTickCount() - last) / 1000.0f; float timestamp = pdTICKS_TO_MS(xTaskGetTickCount()) / 1000.0f; // Check with Divij + // 13.0 is the "spectral density", for anyone wondering about that magic number ekf.tick(dt, 13.0, current_barom_buf, current_accelerations, current_angular_kalman, FSM_state, current_gps); KalmanData current_state = ekf.getState(); @@ -599,14 +615,15 @@ DECLARE_THREAD(kalman, RocketSystems *arg) last = xTaskGetTickCount(); //float prev_vel = current_state.velocity.vx; + // if we're boosting, coasting, or drogue-ing if(arg->rocket_data.fsm_state.getRecentUnsync().state >= FSMState::STATE_BOOST && arg->rocket_data.fsm_state.getRecentUnsync().state < FSMState::STATE_MAIN) { if(max_vel < current_state.velocity.vx) { max_vel = current_state.velocity.vx; - max_vel_time = pdTICKS_TO_MS(xTaskGetTickCount()); + max_vel_time = pdTICKS_TO_MS(xTaskGetTickCount()); // update max velocity } } - if(!has_logged) { + if(!has_logged) { // log landing metadata if we haven't and we should if(arg->rocket_data.fsm_state.getRecentUnsync().state == FSMState::STATE_LANDED) { arg->meta_logging.log_data(MetaDataCode::DATA_MAX_VEL, max_vel); arg->meta_logging.log_data(MetaDataCode::EVENT_TMAX_ACCEL, max_vel_time); @@ -617,10 +634,13 @@ DECLARE_THREAD(kalman, RocketSystems *arg) } } +/** + * @brief processes telemetry commands from ground station + */ void handle_tlm_command(TelemetryCommand &command, RocketSystems *arg, FSMState current_state) { // maybe we should move this somewhere else but it can stay here for now - switch (command.command) + switch (command.command) { case CommandType::RESET_KF: arg->rocket_data.command_flags.should_reset_kf = true; @@ -679,7 +699,7 @@ void handle_tlm_command(TelemetryCommand &command, RocketSystems *arg, FSMState arg->sensors.magnetometer.begin_calibration(arg->buzzer); break; default: - break; // how + break; // this shouldn't be possible } } diff --git a/MIDAS/src/systems.h b/MIDAS/src/flight-systems/systems.h similarity index 83% rename from MIDAS/src/systems.h rename to MIDAS/src/flight-systems/systems.h index ad5fab22..2dd881f6 100644 --- a/MIDAS/src/systems.h +++ b/MIDAS/src/flight-systems/systems.h @@ -2,18 +2,18 @@ #include -#include "sensor_data.h" -#include "hal.h" -#include "Buffer.h" -#include "data_logging.h" -#include "buzzer.h" -#include "led.h" -#include "telemetry.h" +#include "flight-systems/sensor_data.h" +#include "util/hal.h" +#include "util/Buffer.h" +#include "logging/data_logging.h" +#include "util/buzzer.h" +#include "util/led.h" +#include "telemetry/telemetry.h" #include "finite-state-machines/fsm.h" -#include "b2b_interface.h" -#include "esp_eeprom.h" +#include "util/b2b_interface.h" +#include "logging/esp_eeprom.h" #include "hardware/SDLog.h" -#include "midas_shell.h" +#include "shell/midas_shell.h" #if defined(SILSIM) #include "silsim/emulated_sensors.h" @@ -29,7 +29,6 @@ * @brief holds all interfaces for all sensors on MIDAS */ -//Remove the low_g sensor and high_g sensor, we will be using the imu sensor for the midas mini. //Barometer (Altitude from Barometer is bugged, need to fix) struct Sensors { diff --git a/MIDAS/src/gnc/ekf.h b/MIDAS/src/gnc/ekf.h index 886780cf..45084017 100644 --- a/MIDAS/src/gnc/ekf.h +++ b/MIDAS/src/gnc/ekf.h @@ -1,8 +1,8 @@ #pragma once #include "kalman_filter.h" -#include "sensor_data.h" -#include "Buffer.h" +#include "flight-systems/sensor_data.h" +#include "util/Buffer.h" #include "constants.h" #include "aero_coeff.h" #include "rotation.h" diff --git a/MIDAS/src/gnc/kalman_filter.h b/MIDAS/src/gnc/kalman_filter.h index 59799f20..42241d9e 100644 --- a/MIDAS/src/gnc/kalman_filter.h +++ b/MIDAS/src/gnc/kalman_filter.h @@ -6,8 +6,8 @@ #undef B1 #include -#include "sensor_data.h" -#include "systems.h" +#include "flight-systems/sensor_data.h" +#include "flight-systems/systems.h" struct KalmanState { diff --git a/MIDAS/src/gnc/mqekf.h b/MIDAS/src/gnc/mqekf.h index 1b5f3bbf..5a2eb35e 100644 --- a/MIDAS/src/gnc/mqekf.h +++ b/MIDAS/src/gnc/mqekf.h @@ -1,10 +1,10 @@ #pragma once -#include "sensor_data.h" // for sim -#include "Buffer.h" // for sim +#include "flight-systems/sensor_data.h" // for sim +#include "util/Buffer.h" // for sim #include "constants.h" #include -#include "systems.h" +#include "flight-systems/systems.h" class QuaternionMEKF { diff --git a/MIDAS/src/hardware/Barometer.cpp b/MIDAS/src/hardware/Barometer.cpp index 1aeee725..4c3bf59c 100644 --- a/MIDAS/src/hardware/Barometer.cpp +++ b/MIDAS/src/hardware/Barometer.cpp @@ -1,13 +1,22 @@ #include "sensors.h" #include +/** + * @brief Singleton instance of the MS5611 barometric pressure sensor. + * + * This object provides the interface for initializing the sensor and + * retrieving pressure, temperature, and altitude measurements. + */ MS5611 MS(MS5611_CS); //singleton object for the MS sensor /** - * @brief Initializes barometer, returns NoError - * - * @return Error code -*/ + * @brief Initializes the MS5611 barometer. + * + * Performs the sensor initialization routine required before any + * measurements can be taken. + * + * @return ErrorCode::NoError if initialization completes successfully. + */ ErrorCode BarometerSensor::init() { MS.init(); @@ -15,20 +24,39 @@ ErrorCode BarometerSensor::init() { } /** - * @brief Reads the pressure and temperature from the MS5611 - * - * @return Barometer data packet -*/ + * @brief Reads the latest barometer measurements. + * + * Performs a pressure conversion, retrieves the compensated pressure + * and temperature from the sensor, and computes the estimated altitude + * using the library's extended atmospheric model. + * + * @note Altitude is derived from pressure and is therefore relative to + * the atmospheric model and local weather conditions. + * + * @return A populated Barometer data packet containing: + * - Temperature (°C) + * - Pressure (Pa) + * - Estimated altitude (m) + */ Barometer BarometerSensor::read() { + // Perform a pressure conversion using the highest oversampling ratio. MS.read(12); /* - * TODO: Switch to latest version of library (0.3.9) when we get hardware to verify - * Equation derived from https://en.wikipedia.org/wiki/Atmospheric_pressure#Altitude_variation - */ - uint32_t pressure = MS.getPressure(); // Pascals - float temperature = MS.getTemperature(); // Celcius + * TODO: Switch to latest version of library (0.3.9) when we get hardware to verify. + * Equation derived from: + * https://en.wikipedia.org/wiki/Atmospheric_pressure#Altitude_variation + */ + + // Retrieve the compensated atmospheric pressure in Pascals. + uint32_t pressure = MS.getPressure(); + + // Retrieve the compensated sensor temperature in degrees Celsius. + float temperature = MS.getTemperature(); + + // Compute the altitude estimate from the measured pressure. float altitude = MS.getAltitudeExtendedModel(); - + + // Package the measurements into a Barometer data packet. return Barometer(temperature, pressure, altitude); -} +} \ No newline at end of file diff --git a/MIDAS/src/hardware/GPSSensor.cpp b/MIDAS/src/hardware/GPSSensor.cpp index b1dc5386..0b8fdcd0 100644 --- a/MIDAS/src/hardware/GPSSensor.cpp +++ b/MIDAS/src/hardware/GPSSensor.cpp @@ -6,43 +6,88 @@ #include "pins.h" #include "sensors.h" -#include "sensor_data.h" +#include "flight-systems/sensor_data.h" // see systems.cpp extern SemaphoreHandle_t i2c_mutex; -// override ublox for using the i2c mutex +/** + * @class MIDASUbloxGNSS + * @brief Wrapper around the SparkFun u-blox GNSS driver that provides + * thread-safe access to the shared I²C bus. + * + * The SparkFun library supports user-defined locking primitives. This + * implementation uses the system-wide FreeRTOS I²C mutex to ensure that + * multiple tasks cannot access the I²C bus simultaneously. + */ class MIDASUbloxGNSS : public SFE_UBLOX_GNSS { protected: + /** + * @brief Indicates that locking is handled externally. + * + * @return Always returns true. + */ bool createLock(void) override { return true; } + + /** + * @brief Acquires exclusive access to the shared I²C bus. + * + * Blocks until the global I²C mutex becomes available. + * + * @return Always returns true. + */ bool lock(void) override { xSemaphoreTake(i2c_mutex, portMAX_DELAY); return true; } + + /** + * @brief Releases exclusive access to the shared I²C bus. + */ void unlock(void) override { xSemaphoreGive(i2c_mutex); } + + /** + * @brief No-op since the mutex is owned by the system. + */ void deleteLock(void) override { } }; +/** + * @brief Singleton instance of the u-blox GNSS interface. + */ MIDASUbloxGNSS ublox; /** - * @brief Initializes GPS, returns NoError - * - * @return Error code + * @brief Initializes the GPS receiver. + * + * Configures the receiver for airborne operation, enables both UBX and + * NMEA output over I²C, sets a 10 Hz navigation update rate, and enables + * automatic PVT (Position, Velocity, Time) messages. + * + * @return ErrorCode::NoError if initialization succeeds. + * @return ErrorCode::GPSCouldNotBeInitialized if communication with the + * receiver fails. */ ErrorCode GPSSensor::init() { if (!ublox.begin()) { return ErrorCode::GPSCouldNotBeInitialized; } + // Configure the receiver for high-dynamics flight applications. ublox.setDynamicModel(DYN_MODEL_AIRBORNE4g); + + // Enable both UBX and NMEA output over I²C. ublox.setI2COutput(COM_TYPE_UBX | COM_TYPE_NMEA); - ublox.setMeasurementRate(100); // 10 Hz full nav + + // Set the navigation solution update rate to 10 Hz. + ublox.setMeasurementRate(100); + + // Automatically retrieve Position, Velocity, and Time data. ublox.setAutoPVT(true); return ErrorCode::NoError; @@ -50,14 +95,39 @@ ErrorCode GPSSensor::init() { /** - * @brief Reads the GPS data from the sensor (lat, long, altitude, sat count, etc) - * - * @return GPS data packet + * @brief Reads the most recent GPS navigation solution. + * + * Retrieves the latest Position, Velocity, and Time (PVT) data from the + * receiver. Altitude and ground speed are converted from millimeters to + * meters before being returned. + * + * @return GPS data packet containing: + * - Latitude + * - Longitude + * - Altitude (m) + * - Ground speed (m/s) + * - GNSS fix type + * - Satellites in view + * - Unix timestamp */ GPS GPSSensor::read() { - return GPS{ublox.getLatitude(), ublox.getLongitude(), (float) ublox.getAltitude() / 1000.f, (float) ublox.getGroundSpeed() / 1000.f, ublox.getFixType(), ublox.getSIV(), ublox.getUnixEpoch()}; + return GPS{ + ublox.getLatitude(), + ublox.getLongitude(), + (float) ublox.getAltitude() / 1000.f, + (float) ublox.getGroundSpeed() / 1000.f, + ublox.getFixType(), + ublox.getSIV(), + ublox.getUnixEpoch() + }; } +/** + * @brief Checks whether a valid PVT solution is available. + * + * @return True if fresh Position, Velocity, and Time data is available; + * false otherwise. + */ bool GPSSensor::valid() { return ublox.getPVT(); -} +} \ No newline at end of file diff --git a/MIDAS/src/hardware/IMU.cpp b/MIDAS/src/hardware/IMU.cpp index b40909c3..1e28aa8c 100644 --- a/MIDAS/src/hardware/IMU.cpp +++ b/MIDAS/src/hardware/IMU.cpp @@ -1,112 +1,106 @@ #include "lsm6dsv320x.h" -#include "errors.h" +#include "util/errors.h" #include "sensors.h" #define NUM_DIRECTIONS 3 #define NUM_READINGS_FOR_CALIB 50 +/** + * @brief Singleton interface to the LSM6DSV320X inertial measurement unit. + * + * Communicates with the IMU over SPI using the configured chip-select + * and interrupt pins. + */ LSM6DSV320XClass LSM6DSV(SPI, IMU_CS_PIN, IMU_IRQ_PIN); -int16_t raw_accel[NUM_DIRECTIONS]; -int16_t raw_accel_hg[NUM_DIRECTIONS]; -int16_t raw_av[NUM_DIRECTIONS]; - -unsigned long lastTime = 0; -float deltaTime = 0; - -IMU IMUSensor::read(){ +/** + * @brief Reads the latest measurements from the IMU. + * + * Retrieves any newly available low-G acceleration, high-G acceleration, + * and angular velocity measurements based on the sensor's status register. + * Measurements whose data-ready flags are not asserted retain their + * default-initialized values. + * + * @return IMU data packet containing the available sensor measurements. + */ +IMU IMUSensor::read() { + + // Determine which sensor outputs have new data available. lsm6dsv320x_status_reg_t status = LSM6DSV.get_status(); IMU reading{}; - //Low-G Acceleration + // Read the low-G accelerometer if fresh data is available. if(status.xlda){ LSM6DSV.get_lowg_acceleration_from_fs8_to_g(&reading.lowg_acceleration.ax, &reading.lowg_acceleration.ay, &reading.lowg_acceleration.az); } - //High-G Acceleration + // Read the high-G accelerometer if fresh data is available. if(status.xlhgda){ LSM6DSV.get_highg_acceleration_from_fs64_to_g(&reading.highg_acceleration.ax, - &reading.highg_acceleration.ay, - &reading.highg_acceleration.az); + &reading.highg_acceleration.ay, + &reading.highg_acceleration.az); } - //Angular rate + // Read the gyroscope if fresh data is available. if(status.gda){ - LSM6DSV.get_angular_velocity_from_fs2000_to_dps(&reading.angular_velocity.vx, - &reading.angular_velocity.vy, - &reading.angular_velocity.vz); + LSM6DSV.get_angular_velocity_from_fs2000_to_dps(&reading.angular_velocity.vx, + &reading.angular_velocity.vy, + &reading.angular_velocity.vz); } return reading; } +/** + * @brief Reads the Sensor Fusion Low Power (SFLP) outputs. + * + * Retrieves the orientation quaternion, estimated gyroscope bias, + * and gravity vector produced by the IMU's onboard sensor fusion + * engine. + * + * @return IMU_SFLP packet containing the latest sensor fusion outputs. + */ IMU_SFLP IMUSensor::read_sflp() { IMU_SFLP reading; uint16_t val[4]; - LSM6DSV.lsm6dsv320x_sflp_quaternion_raw_get(val);//4 elements + // Retrieve the estimated orientation quaternion. + LSM6DSV.lsm6dsv320x_sflp_quaternion_raw_get(val); - reading.quaternion.w = LSM6DSV.sflp_quaternion_raw_to_float(val[0]);//Will have to find a half to single precision conversion function somewhere + reading.quaternion.w = LSM6DSV.sflp_quaternion_raw_to_float(val[0]); reading.quaternion.x = LSM6DSV.sflp_quaternion_raw_to_float(val[1]); reading.quaternion.y = LSM6DSV.sflp_quaternion_raw_to_float(val[2]); reading.quaternion.z = LSM6DSV.sflp_quaternion_raw_to_float(val[3]); - //(Feature) UPDATE TO USE FIFO -> If the readings are currently okay, this wont be a priority. Circular Buffer FIFO will be a feature - LSM6DSV.sflp_gbias_raw_get((int16_t*)&val);//3 elements + // Retrieve the estimated gyroscope bias. + LSM6DSV.sflp_gbias_raw_get((int16_t*)&val); reading.gyro_bias.vx = LSM6DSV.sflp_gbias_raw_to_mdps(val[0]) / 1000.0; reading.gyro_bias.vy = LSM6DSV.sflp_gbias_raw_to_mdps(val[1]) / 1000.0; reading.gyro_bias.vz = LSM6DSV.sflp_gbias_raw_to_mdps(val[2]) / 1000.0; - //UPDATE TO USE FIFO -> If the readings are currently okay, Circular Buffer FIFO will be a feature - LSM6DSV.sflp_gravity_raw_get((int16_t*)&val);//3 elements - + // Retrieve the estimated gravity vector. + LSM6DSV.sflp_gravity_raw_get((int16_t*)&val); + reading.gravity.ax = LSM6DSV.sflp_gravity_raw_to_mg(val[0]) / 1000.0; reading.gravity.ay = LSM6DSV.sflp_gravity_raw_to_mg(val[1]) / 1000.0; reading.gravity.az = LSM6DSV.sflp_gravity_raw_to_mg(val[2]) / 1000.0; - + return reading; } -//dont need this anymore - -// AngularKalmanData IMUSensor::read_Kalman_Angular() { -// sh2_SensorValue_t event; -// Vec3 euler; -// Vec3 filtered_euler = {0, 0, 0}; -// const float alpha = 0.98; // Higher values dampen out current measurements --> reduce peaks -// unsigned long currentTime = millis(); -// deltaTime = (currentTime - lastTime) / 1000.0; -// lastTime = currentTime; -// if (imu.getSensorEvent(&event)) { -// AngularKalmanData sensor_reading; -// sensor_reading.has_data = true; -// if (event.sensorId == SH2_ARVR_STABILIZED_RV) { -// euler = quaternionToEulerRV(&event.un.arvrStabilizedRV, true); -// sensor_reading.reading_type = OrientationReadingType::FULL_READING; -// sensor_reading.quaternion.w = event.un.arvrStabilizedRV.real; -// sensor_reading.quaternion.x = event.un.arvrStabilizedRV.i; -// sensor_reading.quaternion.y = event.un.arvrStabilizedRV.j; -// sensor_reading.quaternion.z = event.un.arvrStabilizedRV.k; -// break; -// } -// sensor_reading.yaw = -euler.y; -// sensor_reading.pitch = euler.x; -// sensor_reading.roll = euler.z; -// if (initial_flag == 0) -// { -// initial_orientation = sensor_reading; -// initial_flag = 1; -// } -// } -// } - +/** + * @brief Audio cues used during the IMU calibration procedure. + * + * Different tone sequences indicate calibration ready, advancing to the + * next orientation, successful completion, or calibration abort. + */ #define XLC_TONE_PITCH Sound{3000, 65} #define XLC_TONE_PITCH_LONG Sound{3000, 250} #define XLC_TONE_WAIT Sound{0, 50} @@ -116,73 +110,146 @@ Sound xl_calib_next_axis[C_XL_LENGTH] = {XLC_TONE_PITCH, XLC_TONE_WAIT, XLC_TONE Sound xl_calib_done[C_XL_LENGTH] = {XLC_TONE_PITCH_LONG, XLC_TONE_PITCH, XLC_TONE_WAIT}; Sound xl_calib_abort[C_XL_LENGTH] = {XLC_TONE_PITCH_LONG, XLC_TONE_PITCH_LONG, XLC_TONE_PITCH_LONG}; +/** + * @brief Restores the previously saved IMU calibration from EEPROM. + * + * Exits calibration mode and reloads the stored high-G accelerometer + * bias values. + * + * @param eeprom EEPROM controller containing the saved calibration data. + */ void IMUSensor::restore_calibration(EEPROMController& eeprom) { + // Exit calibration mode. calibration_state = IMUSensor::IMUCalibrationState::NONE; - // Read back the calibration data from EEPROM: + // Restore the saved high-G accelerometer bias. calibration_sensor_bias = eeprom.data.lsm6dsv320x_hg_xl_bias; } +/** + * @brief Aborts the current IMU calibration procedure. + * + * Plays the calibration abort tone and restores the previously saved + * calibration values. + * + * @param buzzer Buzzer used to indicate calibration status. + * @param eeprom EEPROM controller containing the saved calibration. + */ void IMUSensor::abort_calibration(BuzzerController& buzzer, EEPROMController& eeprom) { buzzer.play_tune(xl_calib_abort, C_XL_LENGTH); restore_calibration(eeprom); } +/** + * @brief Begins the six-orientation IMU calibration procedure. + * + * Starts calibration in the +X orientation unless calibration is already + * in progress. + * + * @param buzzer Buzzer used to guide the user through calibration. + */ void IMUSensor::begin_calibration(BuzzerController& buzzer) { + // Ignore the request if calibration is already active. if(calibration_state != IMUSensor::IMUCalibrationState::NONE) { - return; // Already in calibration mode.. + return; } + // Signal that calibration is ready to begin. buzzer.play_tune(xl_calib_rdy, C_XL_LENGTH); + + // Record the calibration start time. _calib_begin_timestamp = millis(); + // Begin with the +X orientation. calibration_state = IMUSensor::IMUCalibrationState::CALIB_PX; } +/** + * @brief Determines whether the IMU is correctly oriented for calibration. + * + * Compares the measured low-G acceleration against the expected + * gravitational acceleration along a single axis. + * + * @param lowg_axis_reading Measured acceleration on the selected axis. + * @param nominal_axis_value Expected acceleration (±1 g). + * + * @return True if the reading is within the acceptable tolerance. + */ bool IMUSensor::accept_calib_reading(float lowg_axis_reading, float nominal_axis_value) { constexpr static float kLowgMaxDeviation = 0.012; return std::abs(lowg_axis_reading - nominal_axis_value) < kLowgMaxDeviation; } +/** + * @brief Advances calibration to the next orientation. + * + * Once all six orientations have been completed, the computed high-G + * accelerometer bias is saved to EEPROM. + * + * @param buzzer Buzzer used to indicate calibration progress. + * @param eeprom EEPROM controller used to store the calibration. + */ void IMUSensor::next_calib(BuzzerController& buzzer, EEPROMController& eeprom) { IMUCalibrationState next_state = static_cast(static_cast(calibration_state) + 1); + if (next_state == IMUCalibrationState::CALIB_DONE) { - // Calibration is finished, we will commit to EEPROM + // Calibration is complete. Save the computed bias values. calibration_state = IMUCalibrationState::NONE; eeprom.data.lsm6dsv320x_hg_xl_bias = calibration_sensor_bias; eeprom.commit(); buzzer.play_tune(xl_calib_done, C_XL_LENGTH); } else { + // Advance to the next calibration orientation. calibration_state = next_state; buzzer.play_tune(xl_calib_next_axis, C_XL_LENGTH); } + + // Reset the sample counter for the next orientation. _calib_valid_readings = 0; } +/** + * @brief Processes a calibration sample. + * + * During calibration, the rocket is placed in each of the six principal + * orientations (+X, -X, +Y, -Y, +Z, -Z). Once enough valid samples have + * been collected for an orientation, the corresponding high-G + * accelerometer bias is computed. + * + * @param lowg_reading Low-G accelerometer measurement. + * @param highg_reading High-G accelerometer measurement. + * @param buzzer_indicator Buzzer used for user feedback. + * @param eeprom EEPROM controller used to save calibration results. + */ void IMUSensor::calib_reading(Acceleration lowg_reading, Acceleration highg_reading, BuzzerController& buzzer_indicator, EEPROMController& eeprom) { switch (calibration_state) { + + // +X orientation. case IMUSensor::IMUCalibrationState::CALIB_PX: if(accept_calib_reading(lowg_reading.ax, 1.0)) { float cur_offset = 1.0 - highg_reading.ax; _calib_valid_readings++; _calib_average += cur_offset; - + + // Advance once enough valid samples have been collected. if (_calib_valid_readings >= NUM_READINGS_FOR_CALIB) { Serial.println("[+X] Good"); next_calib(buzzer_indicator, eeprom); } } break; + + // -X orientation. Compute the X-axis bias. case IMUSensor::IMUCalibrationState::CALIB_NX: if(accept_calib_reading(lowg_reading.ax, -1.0)) { float cur_offset = -1.0 - highg_reading.ax; _calib_valid_readings++; _calib_average += cur_offset; - + if (_calib_valid_readings >= NUM_READINGS_FOR_CALIB) { - float overall_offset = _calib_average / (NUM_READINGS_FOR_CALIB*2); + float overall_offset = _calib_average / (NUM_READINGS_FOR_CALIB * 2); Serial.println("[-X] Good."); calibration_sensor_bias.ax = overall_offset; _calib_average = 0.0; @@ -190,27 +257,31 @@ void IMUSensor::calib_reading(Acceleration lowg_reading, Acceleration highg_read } } break; + + // +Y orientation. case IMUSensor::IMUCalibrationState::CALIB_PY: if(accept_calib_reading(lowg_reading.ay, 1.0)) { float cur_offset = 1.0 - highg_reading.ay; _calib_valid_readings++; _calib_average += cur_offset; - + if (_calib_valid_readings >= NUM_READINGS_FOR_CALIB) { Serial.println("[+Y] Good"); next_calib(buzzer_indicator, eeprom); } } break; + + // -Y orientation. Compute the Y-axis bias. case IMUSensor::IMUCalibrationState::CALIB_NY: if(accept_calib_reading(lowg_reading.ay, -1.0)) { float cur_offset = -1.0 - highg_reading.ay; _calib_valid_readings++; _calib_average += cur_offset; - + if (_calib_valid_readings >= NUM_READINGS_FOR_CALIB) { - float overall_offset = _calib_average / (NUM_READINGS_FOR_CALIB*2); + float overall_offset = _calib_average / (NUM_READINGS_FOR_CALIB * 2); Serial.println("[-Y] Good."); calibration_sensor_bias.ay = overall_offset; _calib_average = 0.0; @@ -219,27 +290,30 @@ void IMUSensor::calib_reading(Acceleration lowg_reading, Acceleration highg_read } break; + // +Z orientation. case IMUSensor::IMUCalibrationState::CALIB_PZ: if(accept_calib_reading(lowg_reading.az, 1.0)) { float cur_offset = 1.0 - highg_reading.az; _calib_valid_readings++; _calib_average += cur_offset; - + if (_calib_valid_readings >= NUM_READINGS_FOR_CALIB) { Serial.println("[+Z] Good"); next_calib(buzzer_indicator, eeprom); } } break; + + // -Z orientation. Compute the Z-axis bias. case IMUSensor::IMUCalibrationState::CALIB_NZ: if(accept_calib_reading(lowg_reading.az, -1.0)) { float cur_offset = -1.0 - highg_reading.az; _calib_valid_readings++; _calib_average += cur_offset; - + if (_calib_valid_readings >= NUM_READINGS_FOR_CALIB) { - float overall_offset = _calib_average / (NUM_READINGS_FOR_CALIB*2); + float overall_offset = _calib_average / (NUM_READINGS_FOR_CALIB * 2); Serial.println("[-Z] Good."); calibration_sensor_bias.az = overall_offset; _calib_average = 0.0; @@ -247,38 +321,55 @@ void IMUSensor::calib_reading(Acceleration lowg_reading, Acceleration highg_read } } break; + + // Invalid calibration state. Reset the calibration state machine. default: calibration_state = IMUCalibrationState::NONE; break; } } -ErrorCode IMUSensor::init(){ +/** + * @brief Initializes and configures the LSM6DSV320X IMU. + * + * Verifies communication with the sensor, resets it to a known state, + * configures the accelerometers and gyroscope, enables the Sensor Fusion + * Low Power (SFLP) engine, and disables the default low-pass filters. + * + * @return ErrorCode::NoError if initialization succeeds. + * @return ErrorCode::IMUCouldNotBeInitialized if the IMU cannot be detected. + */ +ErrorCode IMUSensor::init() { uint8_t whoami; + // Verify that the connected device matches the expected IMU. LSM6DSV.device_id_get(&whoami); - if(whoami != LSM6DSV320X_ID) + if(whoami != LSM6DSV320X_ID) return IMUCouldNotBeInitialized; + // Reset the IMU to its power-on state. LSM6DSV.sw_por(); - // the second parameter used to be normal instead of high-performance + // Configure the low-G accelerometer, gyroscope, and high-G + // accelerometer to operate at 480 Hz in high-performance mode. LSM6DSV.xl_setup(LSM6DSV320X_ODR_AT_480Hz, LSM6DSV320X_XL_HIGH_PERFORMANCE_MD); LSM6DSV.gy_setup(LSM6DSV320X_ODR_AT_480Hz, LSM6DSV320X_GY_HIGH_PERFORMANCE_MD); - LSM6DSV.hg_xl_data_rate_set(LSM6DSV320X_HG_XL_ODR_AT_480Hz, 1);//xl_setup only handles lowg, this should also set the enable register - - LSM6DSV.hg_xl_full_scale_set(LSM6DSV320X_64g);//highg scale set - LSM6DSV.xl_full_scale_set(LSM6DSV320X_8g);//low scale set + LSM6DSV.hg_xl_data_rate_set(LSM6DSV320X_HG_XL_ODR_AT_480Hz, 1); + + // Configure the measurement ranges. + LSM6DSV.hg_xl_full_scale_set(LSM6DSV320X_64g); + LSM6DSV.xl_full_scale_set(LSM6DSV320X_8g); LSM6DSV.gy_full_scale_set(LSM6DSV320X_2000dps); - + + // Enable the onboard Sensor Fusion Low Power engine. LSM6DSV.sflp_enable_set(1); - //Filter initialization (I really have no idea) + // Configure the digital filter settling behavior. LSM6DSV.filt_settling_mask_set(false, false, false); - // Low-pass filters: + // Disable the default low-pass filters to preserve sensor bandwidth. LSM6DSV.filt_gy_lp1_set(PROPERTY_DISABLE); - //lsm6dsv320x_filt_gy_lp1_bandwidth_set(&dev_ctx, lsm6dsv320x_GY_ULTRA_LIGHT); + // lsm6dsv320x_filt_gy_lp1_bandwidth_set(&dev_ctx, lsm6dsv320x_GY_ULTRA_LIGHT); LSM6DSV.filt_xl_lp2_set(PROPERTY_DISABLE); return NoError; diff --git a/MIDAS/src/hardware/Magnetometer.cpp b/MIDAS/src/hardware/Magnetometer.cpp index f9a29d6a..ed1a7f43 100644 --- a/MIDAS/src/hardware/Magnetometer.cpp +++ b/MIDAS/src/hardware/Magnetometer.cpp @@ -1,10 +1,19 @@ #include #include "sensors.h" -#include "hal.h" - -SFE_MMC5983MA MMC5983; // global static instance of the sensor - +#include "util/hal.h" + +/** + * @brief Singleton interface to the MMC5983MA magnetometer. + */ +SFE_MMC5983MA MMC5983; + +/** + * @brief Audio cues used during magnetometer calibration. + * + * These tone sequences indicate calibration start, user prompts during + * calibration, successful completion, or calibration failure. + */ #define MGC_TONE_PITCH_H Sound{3000, 50} #define MGC_TONE_PITCH_L Sound{2200, 50} #define MGC_TONE_PITCH_LONG Sound{3000, 250} @@ -12,135 +21,223 @@ SFE_MMC5983MA MMC5983; // global static instance of the sensor #define MGC_TONE_WAIT Sound{0, 50} #define MGC_TONE_NOOP Sound{0, 1} -Sound mg_calib_rdy[C_MG_LENGTH] = {MGC_TONE_PITCH_H, MGC_TONE_WAIT, MGC_TONE_PITCH_H, MGC_TONE_WAIT, MGC_TONE_PITCH_H}; +Sound mg_calib_rdy[C_MG_LENGTH] = {MGC_TONE_PITCH_H, MGC_TONE_WAIT, MGC_TONE_PITCH_H, MGC_TONE_WAIT, MGC_TONE_PITCH_H}; Sound mg_calib_done[C_MG_LENGTH] = {MGC_TONE_PITCH_LONG, MGC_TONE_WAIT, MGC_TONE_PITCH_H, MGC_TONE_WAIT, MGC_TONE_PITCH_H}; -Sound mg_calib_inp[C_MG_LENGTH] = {MGC_TONE_PITCH_H, MGC_TONE_NOOP, MGC_TONE_NOOP, MGC_TONE_NOOP,MGC_TONE_NOOP}; -Sound mg_calib_bad[C_MG_LENGTH] = {MGC_TONE_PITCH_LONG_L, MGC_TONE_WAIT, MGC_TONE_PITCH_LONG_L, MGC_TONE_NOOP, MGC_TONE_NOOP}; - +Sound mg_calib_inp[C_MG_LENGTH] = {MGC_TONE_PITCH_H, MGC_TONE_NOOP, MGC_TONE_NOOP, MGC_TONE_NOOP, MGC_TONE_NOOP}; +Sound mg_calib_bad[C_MG_LENGTH] = {MGC_TONE_PITCH_LONG_L, MGC_TONE_WAIT, MGC_TONE_PITCH_LONG_L, MGC_TONE_NOOP, MGC_TONE_NOOP}; + +/** + * @brief Initializes the MMC5983MA magnetometer. + * + * Verifies communication with the sensor over SPI. + * + * @return ErrorCode::NoError if initialization succeeds. + * @return ErrorCode::MagnetometerCouldNotBeInitialized if the sensor + * cannot be detected. + */ ErrorCode MagnetometerSensor::init() { - // Checks if sensor is connected + // Verify communication with the magnetometer. if (!MMC5983.begin(MMC5983_CS)) { return ErrorCode::MagnetometerCouldNotBeInitialized; } + return ErrorCode::NoError; } +/** + * @brief Reads the latest magnetic field measurement. + * + * Converts the raw 18-bit sensor output into magnetic field values + * expressed in Gauss and applies the board coordinate transformation. + * + * @return Magnetometer measurement. + */ Magnetometer MagnetometerSensor::read() { - // read from aforementioned global instance of sensor + + // Raw 18-bit sensor measurements. uint32_t cx, cy, cz; double X, Y, Z; - + MMC5983.getMeasurementXYZ(&cx, &cy, &cz); - - // The magnetic field values are 18-bit unsigned. The _approximate_ zero (mid) point is 2^17 - // Here we scale each field to +/- 1.0 to make it easier to convert to Gauss - // https://github.com/sparkfun/SparkFun_MMC5983MA_Magnetometer_Arduino_Library/tree/main/examples + + // Convert the unsigned raw values into normalized values centered + // about zero. double sf = (double)(1 << 17); - X = ((double)cx - sf)/sf; - Y = ((double)cy - sf)/sf; - Z = ((double)cz - sf)/sf; - - // We multiply by 8, which is the full scale of the mag. - // https://github.com/sparkfun/SparkFun_MMC5983MA_Magnetometer_Arduino_Library/blob/main/examples/Example4-SPI_Simple_measurement/Example4-SPI_Simple_measurement.ino - Magnetometer reading{Y*8, -X*8, -Z*8}; + X = ((double)cx - sf) / sf; + Y = ((double)cy - sf) / sf; + Z = ((double)cz - sf) / sf; + + // Convert the normalized values to Gauss and rotate into the + // flight computer coordinate frame. + Magnetometer reading{Y * 8, -X * 8, -Z * 8}; + return reading; } +/** + * @brief Begins the magnetometer calibration procedure. + * + * Initializes the calibration state, resets accumulated statistics, + * and prompts the user to begin rotating the rocket. + * + * @param buzzer Buzzer used to provide user feedback. + */ void MagnetometerSensor::begin_calibration(BuzzerController& buzzer) { + Serial.println("[MAG] Calibration begin"); + buzzer.play_tune(mg_calib_rdy, C_MG_LENGTH); + in_calibration_mode = true; _calib_begin_timestamp = millis(); + + // Reset calibration state. _calib_beeping = 1; _calib_max_axis = {-INFINITY, -INFINITY, -INFINITY}; - _calib_min_axis = {INFINITY, INFINITY, INFINITY}; + _calib_min_axis = { INFINITY, INFINITY, INFINITY}; _calib_magnitude_sum = 0.0; _calib_num_datapoints = 0; } +/** + * @brief Processes a magnetometer calibration sample. + * + * Tracks the minimum and maximum measurement observed on each axis while + * accumulating the average magnetic field magnitude. Periodic audio + * prompts are played throughout the calibration until the configured + * calibration duration expires. + * + * @param reading Current magnetometer measurement. + * @param eeprom EEPROM controller used to store calibration data. + * @param buzzer Buzzer used for user feedback. + */ void MagnetometerSensor::calib_reading(Magnetometer& reading, EEPROMController& eeprom, BuzzerController& buzzer) { - // X reading - if(reading.mx > _calib_max_axis.mx) { _calib_max_axis.mx = reading.mx; }; - if(reading.mx < _calib_min_axis.mx) { _calib_min_axis.mx = reading.mx; }; - // Y - if(reading.my > _calib_max_axis.my) { _calib_max_axis.my = reading.my; }; - if(reading.my < _calib_min_axis.my) { _calib_min_axis.my = reading.my; }; - // Z - if(reading.mz > _calib_max_axis.mz) { _calib_max_axis.mz = reading.mz; }; - if(reading.mz < _calib_min_axis.mz) { _calib_min_axis.mz = reading.mz; }; - - // Magnitude - double mag = sqrtf(reading.mx*reading.mx + reading.my*reading.my + reading.mz*reading.mz); + + // Update the observed extrema for each axis. + if(reading.mx > _calib_max_axis.mx) _calib_max_axis.mx = reading.mx; + if(reading.mx < _calib_min_axis.mx) _calib_min_axis.mx = reading.mx; + + if(reading.my > _calib_max_axis.my) _calib_max_axis.my = reading.my; + if(reading.my < _calib_min_axis.my) _calib_min_axis.my = reading.my; + + if(reading.mz > _calib_max_axis.mz) _calib_max_axis.mz = reading.mz; + if(reading.mz < _calib_min_axis.mz) _calib_min_axis.mz = reading.mz; + + // Accumulate the average magnetic field magnitude. + double mag = sqrtf(reading.mx * reading.mx + + reading.my * reading.my + + reading.mz * reading.mz); + _calib_magnitude_sum += mag; _calib_num_datapoints++; - // Beeping + // Periodically remind the user that calibration is still active. if(get_time_since_calibration_start() > 15000 * _calib_beeping && _calib_beeping < 4) { _calib_beeping++; buzzer.play_tune(mg_calib_inp, C_MG_LENGTH); } + // Complete calibration once the allotted time expires. if(get_time_since_calibration_start() > _calib_time) { Serial.println("[MAG] Calibration done."); commit_calibration(eeprom, buzzer); } } +/** + * @brief Validates a computed magnetometer calibration. + * + * Performs basic sanity checks on the calculated soft-iron scale factors + * to reject obviously invalid calibration results. + * + * @param b Computed hard-iron bias. + * @param s Computed soft-iron scale factors. + * + * @return True if the calibration appears valid. + */ bool MagnetometerSensor::calibration_valid(const Magnetometer& b, const Magnetometer& s) { + constexpr float kSensorEpsilon = 1e-6f; - constexpr float kSensorMaxShear = 5.0f; // probably should tweak this + constexpr float kSensorMaxShear = 5.0f; + // Reject nearly-zero scale factors. if(s.mx < kSensorEpsilon || s.my < kSensorEpsilon || s.mz < kSensorEpsilon) { - // Scale data is too small, so we probably didn't get enough data return false; } - float s_min = fminf(s.mx, fminf(s.my, s.mz)); // Get minimum value of s - float s_max = fmaxf(s.mx, fmaxf(s.my, s.mz)); // Get minimum value of s + float s_min = fminf(s.mx, fminf(s.my, s.mz)); + float s_max = fmaxf(s.mx, fmaxf(s.my, s.mz)); + // Large differences between scale factors may indicate poor coverage + // during calibration. if(s_max > kSensorMaxShear * s_min) { - // this is iffy but could indicate strong sensor reference ellipsoid shear + // Reserved for future validation. // return false; } return true; } +/** + * @brief Restores the previously saved magnetometer calibration. + * + * Exits calibration mode and reloads the stored hard-iron and soft-iron + * calibration values from EEPROM. + * + * @param eeprom EEPROM controller containing the saved calibration. + */ void MagnetometerSensor::restore_calibration(EEPROMController& eeprom) { + in_calibration_mode = false; - // Read back the calibration data from EEPROM: + // Restore the saved calibration values. calibration_bias_softiron = eeprom.data.mmc5983ma_softiron_bias; calibration_bias_hardiron = eeprom.data.mmc5983ma_hardiron_bias; } +/** + * @brief Finalizes and stores the magnetometer calibration. + * + * Computes the hard-iron and soft-iron correction values from the + * collected calibration data, validates the result, and saves the + * calibration to EEPROM if successful. + * + * @param eeprom EEPROM controller used to store the calibration. + * @param buzzer Buzzer used to indicate success or failure. + */ void MagnetometerSensor::commit_calibration(EEPROMController& eeprom, BuzzerController& buzzer) { + in_calibration_mode = false; Magnetometer b, s; + + // Compute the average magnetic field magnitude. double magnitude_tgt = _calib_magnitude_sum / _calib_num_datapoints; - // b: Origin offset + // Compute the hard-iron bias (offset). b.mx = (_calib_min_axis.mx + _calib_max_axis.mx) / 2; b.my = (_calib_min_axis.my + _calib_max_axis.my) / 2; b.mz = (_calib_min_axis.mz + _calib_max_axis.mz) / 2; - // s: Scale offset - s.mx = (_calib_max_axis.mx - _calib_min_axis.mx) / (2*magnitude_tgt); - s.my = (_calib_max_axis.my - _calib_min_axis.my) / (2*magnitude_tgt); - s.mz = (_calib_max_axis.mz - _calib_min_axis.mz) / (2*magnitude_tgt); + // Compute the soft-iron scale factors. + s.mx = (_calib_max_axis.mx - _calib_min_axis.mx) / (2 * magnitude_tgt); + s.my = (_calib_max_axis.my - _calib_min_axis.my) / (2 * magnitude_tgt); + s.mz = (_calib_max_axis.mz - _calib_min_axis.mz) / (2 * magnitude_tgt); Serial.print("MAG: "); Serial.println(magnitude_tgt); + // Reject invalid calibration results. if(!calibration_valid(b, s)) { Serial.println("[MAG] Calibration is bad."); buzzer.play_tune(mg_calib_bad, C_MG_LENGTH); - return; // The calibration is garbage... this probably shouldn't fail silently but idc rn. + return; } + // Store the new calibration. calibration_bias_hardiron = b; calibration_bias_softiron = s; + buzzer.play_tune(mg_calib_done, C_MG_LENGTH); eeprom.data.mmc5983ma_softiron_bias = calibration_bias_softiron; diff --git a/MIDAS/src/hardware/Pyro.cpp b/MIDAS/src/hardware/Pyro.cpp index 4975fd1c..5a17d1d0 100644 --- a/MIDAS/src/hardware/Pyro.cpp +++ b/MIDAS/src/hardware/Pyro.cpp @@ -4,7 +4,7 @@ #include "pins.h" #include "TCAL9538.h" -#include +#include #include "finite-state-machines/pyro_eval.h" @@ -12,23 +12,32 @@ #define PYRO_TEST_FIRE_TIME 100 /** - * @brief Returns true if the error_code signals failure. + * @brief Returns whether a GPIO operation resulted in an error. + * + * @param error_code Result returned by a GPIO operation. + * + * @return True if the operation failed. */ bool error_is_failure(GpioError error_code) { return error_code != GpioError::NoError; } /** - * @brief Initializes the pyro thread. The main initialization will be done by the GPIO expander, so the pyro thread doesn't - * have to do anything special and will always return NoError. + * @brief Initializes the pyro subsystem. + * + * Configures the global arm pin and each pyro channel as outputs and + * resets all internal firing state. The GPIO driver currently reports + * erroneous failures, so this function always returns NoError. + * + * @return ErrorCode::NoError. */ ErrorCode Pyro::init() { bool has_failed_gpio_init = false; - // global arm + // Configure the global arm output. has_failed_gpio_init |= error_is_failure(gpioPinMode(PYRO_GLOBAL_ARM_PIN, OUTPUT)); - // fire pins + // Configure each pyro channel and clear its runtime state. for(int i = 0; i < MIDAS_NUM_PYROS; ++i) { has_failed_gpio_init |= error_is_failure(gpioPinMode(PYRO_PINS[i], OUTPUT)); pyro_event_consumed[i] = false; @@ -36,55 +45,92 @@ ErrorCode Pyro::init() { pyro_trigger_times[i] = 0; } - -// if (has_failed_gpio_init) { -// return ErrorCode::PyroGPIOCouldNotBeInitialized; -// } else { - return ErrorCode::NoError; // GPIO Driver always claimes it errored even when it doesn't. -// } + // The GPIO driver always reports an error even when initialization + // succeeds, so ignore the reported status for now. + return ErrorCode::NoError; } +/** + * @brief Disarms every pyro channel. + * + * Clears all channel firing outputs and disables the global arm signal. + * + * @param prev_state Pyro state to modify. + */ void Pyro::disarm_all_channels(PyroState& prev_state) { + + // Disable all individual pyro channels. for(int i = 0; i < MIDAS_NUM_PYROS; ++i) { prev_state.channel_firing[i] = false; } + + // Disable the global arm output. prev_state.is_global_armed = false; } +/** + * @brief Starts the pyro safety timer. + * + * Records the time at which a manual pyro firing began and prevents + * additional firing commands from being accepted during the same cycle. + */ void Pyro::set_pyro_safety() { safety_pyro_start_firing_time = pdTICKS_TO_MS(xTaskGetTickCount()); safety_has_fired_pyros_this_cycle = true; } +/** + * @brief Clears the pyro safety latch. + * + * Allows future manual pyro firing commands to be accepted. + */ void Pyro::reset_pyro_safety() { safety_has_fired_pyros_this_cycle = false; } /** - * @brief Fires channels by setting their desired fire state. - * - * @return A pyro struct indicating which pyro channels are armed and/or firing. + * @brief Computes the desired pyro firing state. + * + * Handles manual pyro testing, SAFE state behavior, and autonomous + * flight-event pyro deployment using the configured FSM logic. + * + * @param data Current flight state, sensor estimates, and command data. + * + * @return Desired pyro output state for this update. */ PyroState Pyro::tick(PyroTickData& data) { + PyroState new_pyro_state = PyroState(); double current_time = data.current_time; + // Never arm or fire pyros while in the SAFE state. if (data.fsm.state == FSMState::STATE_SAFE) { disarm_all_channels(new_pyro_state); return new_pyro_state; } - // If the state is not SAFE, we arm the global arm pin + // Arm the global pyro enable whenever we leave SAFE. new_pyro_state.is_global_armed = true; - if (data.fsm.state == FSMState::STATE_ARMED) { reset_pyro_safety(); return new_pyro_state; } + // In the ARMED state, only enable the global arm output. + if (data.fsm.state == FSMState::STATE_ARMED) { + reset_pyro_safety(); + return new_pyro_state; + } + + // Handle manual pyro testing. if (data.fsm.state == FSMState::STATE_PYRO_TEST) { + + // Ignore additional commands while a test firing is already active. if(safety_has_fired_pyros_this_cycle) { - // If a fire pyro command has already be acknowledged, do not acknowledge more commands, just fire pyro for the defined time - // then, transition to SAFE. + + // End the test once the configured firing duration expires. if((current_time - safety_pyro_start_firing_time) >= PYRO_TEST_FIRE_TIME) { + data.commands.should_transition_safe = true; disarm_all_channels(new_pyro_state); + + // Clear any remaining fire commands. data.commands.should_fire_pyro_a = false; data.commands.should_fire_pyro_b = false; data.commands.should_fire_pyro_c = false; @@ -92,40 +138,39 @@ PyroState Pyro::tick(PyroTickData& data) { reset_pyro_safety(); } + return new_pyro_state; } - // Respond to telem commands to fire igniters if(data.commands.should_fire_pyro_a) { new_pyro_state.channel_firing[0] = true; set_pyro_safety(); } if(data.commands.should_fire_pyro_b) { - // Fire pyro channel "B" new_pyro_state.channel_firing[1] = true; set_pyro_safety(); } if(data.commands.should_fire_pyro_c) { - // Fire pyro channel "C" new_pyro_state.channel_firing[2] = true; set_pyro_safety(); } if(data.commands.should_fire_pyro_d) { - // Fire pyro channel "D" new_pyro_state.channel_firing[3] = true; set_pyro_safety(); } + return new_pyro_state; } + // Ignore autonomous pyro events if the loaded FSM configuration is invalid. if(data.fsm_configuration.crc32 == FSM_CRC_FAIL_STATE) { return new_pyro_state; } - // Load state + // Populate the evaluator with the previous pyro state. PyroEvalState eval_state; for (int i = 0; i < MIDAS_NUM_PYROS; i++) { eval_state.trigger_times[i] = pyro_trigger_times[i]; @@ -133,22 +178,28 @@ PyroState Pyro::tick(PyroTickData& data) { eval_state.event_consumed[i] = pyro_event_consumed[i]; } - // Calculate new state + // Compute the updated pyro state using the shared evaluator. double tilt_deg = data.akf.mq_tilt * (180.0 / M_PI); + PyroEvalResult eval = pyro_eval( - data.fsm_configuration, data.fsm, tilt_deg, - data.time_since_launch, data.ekf.velocity.vx, - current_time, eval_state + data.fsm_configuration, + data.fsm, + tilt_deg, + data.time_since_launch, + data.ekf.velocity.vx, + current_time, + eval_state ); - // Write back state + // Save the evaluator state and output firing commands. for (int i = 0; i < MIDAS_NUM_PYROS; i++) { pyro_trigger_times[i] = eval_state.trigger_times[i]; pyro_event_check[i] = eval_state.event_check[i]; pyro_event_consumed[i] = eval_state.event_consumed[i]; + new_pyro_state.channel_firing[i] = eval.channel_firing[i]; new_pyro_state.pyro_event_consumed[i] = eval.event_consumed[i]; } return new_pyro_state; -} +} \ No newline at end of file diff --git a/MIDAS/src/hardware/SDLog.cpp b/MIDAS/src/hardware/SDLog.cpp index 005351bc..94b42269 100644 --- a/MIDAS/src/hardware/SDLog.cpp +++ b/MIDAS/src/hardware/SDLog.cpp @@ -3,92 +3,145 @@ #include #include "SDLog.h" -#include "hardware/pins.h" +#include "pins.h" /** - * @brief Initializes the SD card logger - * In MIDAS v2.x, the flash module has the same interface as - * the SD card, we just need to set the appropiate pins - * - * @return Error Code -*/ + * @brief Initializes the SD card logging system. + * + * Configures the SD/MMC interface, opens the flight log and metadata + * files, and determines the next available log file name. + * + * On MIDAS V2.x, the onboard eMMC flash module is accessed through the + * SD/MMC interface, so initialization is identical to an SD card after + * assigning the appropriate pins. + * + * @return ErrorCode::NoError if initialization succeeds. + * @return ErrorCode::SDBeginFailed if the SD/MMC interface or filesystem + * cannot be initialized. + * @return ErrorCode::SDCouldNotOpenFile if the log files cannot be opened. + */ ErrorCode SDSink::init() { + + // Configure the SD/MMC peripheral pins. Serial.println("[SD] Connecting to SD..."); if (!SD_MMC.setPins(FLASH_CLK, FLASH_CMD, FLASH_DAT0, FLASH_DAT1, FLASH_DAT2, FLASH_DAT3)) { return ErrorCode::SDBeginFailed; } + + // Mount the filesystem. if (!SD_MMC.begin("/sd", true, false, SDMMC_FREQ_52M, 5)) { failed = true; return ErrorCode::SDBeginFailed; } + Serial.println("[SD] Startup OK"); + // Determine the next available flight log filename. char file_name[16] = "data"; char ext[] = ".bin"; + Serial.println("[SD] Determining output file"); Serial.println(current_file_no); if(current_file_no != 0) { - Serial.print("[SD] EEPROM log file recovered: "); Serial.println(current_file_no); + Serial.print("[SD] EEPROM log file recovered: "); + Serial.println(current_file_no); } int filenumber = -1; sdFileNamer(file_name, ext, SD_MMC, current_file_no, &filenumber); - + if(filenumber != -1) { - Serial.print("[SD] Beginning log: "); Serial.println(current_file_no); + Serial.print("[SD] Beginning log: "); + Serial.println(current_file_no); } else { failed = true; return ErrorCode::SDBeginFailed; } + // Generate the metadata filename using the same base name. char meta_name[255] = {0}; strcpy(meta_name, file_name); + char* extpos = strrchr(meta_name, '.'); if (extpos) { strcpy(extpos, ".meta"); } + // Open the primary log and metadata files. file = SD_MMC.open(file_name, FILE_WRITE, true); meta = SD_MMC.open(meta_name, FILE_WRITE, true); + if (!file || !meta) { failed = true; return ErrorCode::SDCouldNotOpenFile; } - // Set the internal file number + // Store the allocated log number for future boots. current_file_no = static_cast(filenumber); Serial.println("[SD] Init done"); + return ErrorCode::NoError; } /** - * @brief Writes a byte buffer to the SD card - * - * @param data byte buffer of data - * @param size size of buffer -*/ + * @brief Writes binary flight data to the log file. + * + * Data is buffered internally and periodically flushed to reduce the + * number of expensive storage synchronization operations. + * + * @param data Pointer to the data buffer. + * @param size Number of bytes to write. + */ void SDSink::write(const uint8_t* data, size_t size) { + + // Write the data to the flight log. size_t bytes_written = file.write(data, size); + + // Track the amount of buffered data. unflushed_bytes += size; - if(unflushed_bytes > 32768){ + + // Flush periodically to reduce data loss while minimizing write + // overhead. + if(unflushed_bytes > 32768) { file.flush(); unflushed_bytes = 0; } + // Record write failures. if(bytes_written != size) { failed_wr = true; } } +/** + * @brief Writes metadata to the metadata log file. + * + * Metadata entries are written infrequently, so the file is flushed after + * every write to ensure the information is immediately committed. + * + * @param data Pointer to the metadata buffer. + * @param size Number of bytes to write. + */ void SDSink::write_meta(const uint8_t* data, size_t size) { - if (failed) { return; } + // Skip writes if the storage subsystem has already failed. + if (failed) { + return; + } + + // Write the metadata entry. size_t bytes_written = meta.write(data, size); + + // Separate metadata records with newlines. meta.write('\n'); - meta.flush(); // Meta writes are infrequent, so flushing is OK. + // Metadata writes are infrequent, so flushing immediately is + // acceptable. + meta.flush(); + + // Record write failures. if(bytes_written != size) { failed_mr = true; } diff --git a/MIDAS/src/hardware/SDLog.h b/MIDAS/src/hardware/SDLog.h index dc839656..eff276d4 100644 --- a/MIDAS/src/hardware/SDLog.h +++ b/MIDAS/src/hardware/SDLog.h @@ -3,25 +3,65 @@ #include #include -#include "data_logging.h" +#include "logging/data_logging.h" /** * @class SDSink - * - * @brief Class that wraps the SD card functions -*/ + * + * @brief Log sink that writes flight data to an SD card. + * + * Manages separate files for telemetry data and metadata while buffering + * writes to reduce the number of SD card flush operations. + */ class SDSink : public LogSink { public: + /** + * @brief Indicates whether SD card initialization or a write operation + * has failed. + */ bool failed = false; - + /** + * @brief Constructs an SD log sink. + */ SDSink() = default; + /** + * @brief Initializes the SD card and opens the log files. + * + * @return Error code indicating whether initialization succeeded. + */ ErrorCode init() override; + + /** + * @brief Writes flight log data to the SD card. + * + * @param data Pointer to the data buffer. + * @param size Number of bytes to write. + */ void write(const uint8_t* data, size_t size) override; + + /** + * @brief Writes metadata to the SD card. + * + * Metadata is stored separately from the primary flight log. + * + * @param data Pointer to the metadata buffer. + * @param size Number of bytes to write. + */ void write_meta(const uint8_t* data, size_t size) override; + private: + /// Flight data log file. File file; + + /// Metadata log file. File meta; + + /** + * @brief Number of bytes written since the last file flush. + * + * Used to reduce unnecessary SD card synchronization operations. + */ size_t unflushed_bytes = 0; -}; +}; \ No newline at end of file diff --git a/MIDAS/src/hardware/Voltage.cpp b/MIDAS/src/hardware/Voltage.cpp index d7a9a6f3..01ce7b32 100644 --- a/MIDAS/src/hardware/Voltage.cpp +++ b/MIDAS/src/hardware/Voltage.cpp @@ -5,32 +5,54 @@ #define ADC_I2C_ADDR 0x14 +/// Global instance of the ADS7138-Q1 analog-to-digital converter. ADS7138 ADC; /** - * @brief "Initializes" the voltage sensor. Since it reads directly from a pin without a library, there is no specific initialization. - * - * @return Error Code, will always be NoError -*/ + * @brief Initializes the ADS7138 voltage monitoring ADC. + * + * Configures communication with the ADS7138-Q1 over the shared I2C bus. + * This ADC is responsible for measuring the avionics battery voltage, + * pyro battery voltage, and the continuity sense lines for each pyro + * channel. + * + * @return ErrorCode::NoError if the ADC was initialized successfully. + * @return ErrorCode::ADCFailedToInit if communication with the ADC + * could not be established. + */ ErrorCode VoltageSensor::init() { - if(!ADC.init(&Wire, ADC_I2C_ADDR)) { + + // Attempt to initialize the ADC on the configured I2C address. + if (!ADC.init(&Wire, ADC_I2C_ADDR)) { return ErrorCode::ADCFailedToInit; } + return ErrorCode::NoError; } /** - * @brief Reads the value of the given analog pin and converts it to a battery voltage with the assumption that the voltage sensor is plugged into that pin - * - * @return The scaled voltage given by the voltage sensor -*/ + * @brief Reads all voltage monitoring channels from the ADS7138. + * + * Samples every analog channel used by the flight computer, including + * the avionics battery, pyro battery, and continuity sensing circuitry + * for each pyro output. The returned values are packaged into a + * Voltage data structure for downstream use by the flight software. + * + * @return Voltage structure containing the latest ADC measurements. + */ Voltage VoltageSensor::read() { Voltage voltage; + // Update the ADC's internal conversion state. ADC.tick(); + // Read the avionics battery voltage. voltage.v_Bat = ADC.read(VBAT_SENSE); + + // Read the dedicated pyro battery voltage. voltage.v_Pyro = ADC.read(PYRO_SENSE); + + // Read continuity sense voltages for each pyro channel. voltage.continuity[0] = ADC.read(SENSE_A); voltage.continuity[1] = ADC.read(SENSE_B); voltage.continuity[2] = ADC.read(SENSE_C); diff --git a/MIDAS/src/hardware/main.cpp b/MIDAS/src/hardware/main.cpp index e335447f..1637eb01 100644 --- a/MIDAS/src/hardware/main.cpp +++ b/MIDAS/src/hardware/main.cpp @@ -2,38 +2,56 @@ #include #include "TCAL9538.h" -#include "systems.h" -#include "hardware/pins.h" -#include "hardware/SDLog.h" -#include "sensor_data.h" +#include "flight-systems/systems.h" +#include "SDLog.h" +#include "flight-systems/sensor_data.h" #include "pins.h" /** - * Sets the config file and then starts all the threads using the config. + * @brief Global SD card log sink. + * + * Stores flight data to persistent storage throughout system operation. */ - SDSink sink; + // #else // MultipleLogSink<> sinks; // #endif -RocketSystems systems{.log_sink = sink}; + /** - * @brief Sets up pinmodes for all sensors and starts threads + * @brief Global rocket systems instance. + * + * Contains all subsystem controllers and shared resources used during + * flight. The configured log sink is supplied during construction. */ +RocketSystems systems{.log_sink = sink}; +/** + * @brief Initializes the flight computer hardware and starts all system + * tasks. + * + * This function performs the complete startup sequence: + * - Initializes serial communication. + * - Plays the startup tone. + * - Initializes the SPI and I²C buses. + * - Configures the I/O expander. + * - Configures all GPIO pins. + * - Initializes the flight systems. + * - Starts the system scheduler. + */ void setup() { - // begin serial port + // Initialize the serial console for debugging. Serial.begin(115200); delay(200); - // Immediate startup tone + // Configure the buzzer and play the startup tone. pinMode(BUZZER_PIN, OUTPUT); digitalWrite(BUZZER_PIN, LOW); ledcAttachPin(BUZZER_PIN, BUZZER_CHANNEL); - // Startup beeps + // Startup beeps. ledcWriteTone(BUZZER_CHANNEL, 3200); delay(250); ledcWriteTone(BUZZER_CHANNEL, 0); @@ -42,57 +60,71 @@ void setup() delay(250); ledcWriteTone(BUZZER_CHANNEL, 0); - - // begin sensor SPI bus + // Initialize the shared SPI bus used by onboard peripherals. Serial.println("Starting SPI..."); SPI.begin(SPI_SCK, SPI_MISO, SPI_MOSI); - //begin I2C bus + // Initialize the shared I²C bus. Serial.println("Starting I2C..."); Wire.begin(I2C_SDA, I2C_SCL, 100000); + // Initialize the GPIO expander. if (!TCAL9538Init(EXP_RESET)) { Serial.println(":("); } - //pinMode changes need to be made here for midas mini bc of new sensors. - - //set all chip selects high (deselected) - pinMode(E22_CS, OUTPUT); - pinMode(MS5611_CS, OUTPUT); + // Configure the SPI chip-select pins. + // (MIDAS Mini may require different sensor configurations.) + pinMode(E22_CS, OUTPUT); + pinMode(MS5611_CS, OUTPUT); pinMode(IMU_CS_PIN, OUTPUT); pinMode(MMC5983_CS, OUTPUT); - digitalWrite(MS5611_CS, HIGH); - digitalWrite(E22_CS, HIGH); + // Deselect all SPI devices before communication begins. + digitalWrite(MS5611_CS, HIGH); + digitalWrite(E22_CS, HIGH); digitalWrite(IMU_CS_PIN, HIGH); digitalWrite(MMC5983_CS, HIGH); - // b2b pins + // Configure the board-to-board interface. pinMode(B2B_EN, OUTPUT); pinMode(B2B_READY, INPUT); - digitalWrite(B2B_EN, HIGH); // Enables the B2B bus - //configure output leds + // Enable the board-to-board communication bus. + digitalWrite(B2B_EN, HIGH); + + // Configure the status indicator LEDs. pinMode(LED_BLUE, OUTPUT); pinMode(LED_GREEN, OUTPUT); pinMode(LED_ORANGE, OUTPUT); pinMode(LED_RED, OUTPUT); + // Configure all pyro output channels. for (int i = 0; i < MIDAS_NUM_PYROS; i++) { gpioPinMode(PYRO_PINS[i], OUTPUT); } + + // Configure the global pyro arm output. gpioPinMode(PYRO_GLOBAL_ARM_PIN, OUTPUT); + // Allow hardware to stabilize before initialization. delay(200); - // init and start threads + // Initialize all flight systems and start their tasks. begin_systems(&systems); + // Execution should not normally return from begin_systems(). loop(); } +/** + * @brief Default application loop. + * + * Once the RTOS has started, all application logic executes within + * dedicated tasks. This function should never be reached during normal + * operation. + */ void loop() { printf("\nHI!"); -} +} \ No newline at end of file diff --git a/MIDAS/src/hardware/sensors.h b/MIDAS/src/hardware/sensors.h index 8369a7f9..cd3acdac 100644 --- a/MIDAS/src/hardware/sensors.h +++ b/MIDAS/src/hardware/sensors.h @@ -1,18 +1,30 @@ #pragma once -#include "errors.h" -#include "sensor_data.h" +#include +#include "flight-systems/sensor_data.h" #include "hardware/pins.h" #include "TCAL9538.h" -#include "rocket_state.h" -#include "esp_eeprom.h" -#include "buzzer.h" +#include "flight-systems/rocket_state.h" +#include "logging/esp_eeprom.h" +#include "util/buzzer.h" /** * @struct IMUSensor + * + * @brief Interface for the onboard inertial measurement unit (IMU). + * + * Provides initialization, sensor data acquisition, and calibration + * utilities for the LSM6DSV320X low-G/high-G accelerometers, gyroscope, + * and Sensor Fusion Low Power (SFLP) engine. */ struct IMUSensor { + /** + * @brief States of the IMU calibration state machine. + * + * Calibration is performed by placing the rocket in each of the six + * principal orientations (+X, -X, +Y, -Y, +Z, -Z). + */ enum IMUCalibrationState { NONE = 0, CALIB_PX = 1, @@ -24,117 +36,344 @@ struct IMUSensor { CALIB_DONE = 7 }; + /** + * @brief Initializes the IMU. + * + * @return Error code indicating initialization status. + */ ErrorCode init(); + + /** + * @brief Reads the latest accelerometer and gyroscope data. + * + * @return IMU measurement packet. + */ IMU read(); + + /** + * @brief Reads the Sensor Fusion Low Power outputs. + * + * @return Sensor fusion measurement packet. + */ IMU_SFLP read_sflp(); + + /** + * @brief Begins the IMU calibration procedure. + * + * @param buzzer Buzzer used for user feedback. + */ void begin_calibration(BuzzerController& buzzer); - void calib_reading(Acceleration lowg_reading, Acceleration highg_reading, BuzzerController& buzzer_indicator, EEPROMController& eeprom); - unsigned long get_time_since_calibration_start() { return millis() - _calib_begin_timestamp; } + + /** + * @brief Processes a calibration sample. + * + * @param lowg_reading Low-G accelerometer measurement. + * @param highg_reading High-G accelerometer measurement. + * @param buzzer_indicator Buzzer used for user feedback. + * @param eeprom EEPROM controller used to store calibration data. + */ + void calib_reading(Acceleration lowg_reading, + Acceleration highg_reading, + BuzzerController& buzzer_indicator, + EEPROMController& eeprom); + + /** + * @brief Returns the elapsed calibration time. + * + * @return Time since calibration began in milliseconds. + */ + unsigned long get_time_since_calibration_start() { + return millis() - _calib_begin_timestamp; + } + + /** + * @brief Restores the saved calibration from EEPROM. + * + * @param eeprom EEPROM controller. + */ void restore_calibration(EEPROMController& eeprom); - void abort_calibration(BuzzerController& buzzer, EEPROMController& eeprom); - + + /** + * @brief Aborts the current calibration procedure. + * + * @param buzzer Buzzer used for user feedback. + * @param eeprom EEPROM controller. + */ + void abort_calibration(BuzzerController& buzzer, + EEPROMController& eeprom); + + /// Current calibration state. IMUCalibrationState calibration_state = IMUCalibrationState::NONE; + + /// Computed high-G accelerometer bias. Acceleration calibration_sensor_bias = {0.0, 0.0, 0.0}; - private: +private: + /// Number of accepted samples for the current orientation. int _calib_valid_readings = 0; + + /// Running sum of calibration offsets. float _calib_average = 0.0; - unsigned long _calib_begin_timestamp; - bool accept_calib_reading(float lowg_axis_reading, float nominal_axis_value); - void next_calib(BuzzerController& buzzer, EEPROMController& eeprom); + /// Timestamp when calibration began. + unsigned long _calib_begin_timestamp; + /** + * @brief Determines whether the current orientation is valid. + */ + bool accept_calib_reading(float lowg_axis_reading, + float nominal_axis_value); + /** + * @brief Advances to the next calibration orientation. + */ + void next_calib(BuzzerController& buzzer, + EEPROMController& eeprom); }; /** - * @struct Magnetometer interface + * @struct MagnetometerSensor + * + * @brief Interface for the onboard MMC5983MA magnetometer. + * + * Provides magnetic field measurements and calibration routines for + * hard-iron and soft-iron compensation. */ struct MagnetometerSensor { + + /** + * @brief Initializes the magnetometer. + * + * @return Error code indicating initialization status. + */ ErrorCode init(); + + /** + * @brief Reads the latest magnetic field measurement. + * + * @return Magnetometer data packet. + */ Magnetometer read(); - // Calibration functions + /// Indicates whether calibration is currently active. bool in_calibration_mode = false; + + /** + * @brief Begins magnetometer calibration. + * + * @param buzzer Buzzer used for user feedback. + */ void begin_calibration(BuzzerController& buzzer); - void calib_reading(Magnetometer& reading, EEPROMController& eeprom, BuzzerController& buzzer); + + /** + * @brief Processes a magnetometer calibration sample. + * + * @param reading Current magnetometer measurement. + * @param eeprom EEPROM controller. + * @param buzzer Buzzer used for user feedback. + */ + void calib_reading(Magnetometer& reading, + EEPROMController& eeprom, + BuzzerController& buzzer); + + /** + * @brief Restores calibration values from EEPROM. + * + * @param eeprom EEPROM controller. + */ void restore_calibration(EEPROMController& eeprom); - Magnetometer calibration_bias_hardiron = {0.0, 0.0, 0.0}; // hard iron offset -- "recenters" data on origin (0,0,0). - Magnetometer calibration_bias_softiron = {1.0, 1.0, 1.0}; // soft iron offset -- scales per-axis data (Should be 3x3, but we'll try 1x3 for now.) + /// Hard-iron offset correction. + Magnetometer calibration_bias_hardiron = {0.0, 0.0, 0.0}; + + /// Soft-iron scale correction. + Magnetometer calibration_bias_softiron = {1.0, 1.0, 1.0}; + + /** + * @brief Returns the elapsed calibration time. + * + * @return Time since calibration began in milliseconds. + */ + unsigned long get_time_since_calibration_start() { + return millis() - _calib_begin_timestamp; + } + +private: + /// Computes and stores the completed calibration. + void commit_calibration(EEPROMController& eeprom, + BuzzerController& buzzer); + + /// Performs sanity checks on the computed calibration. + bool calibration_valid(const Magnetometer& b, + const Magnetometer& s); - unsigned long get_time_since_calibration_start() { return millis() - _calib_begin_timestamp; } + /// Maximum observed magnetic field values. + Magnetometer _calib_max_axis; - private: - void commit_calibration(EEPROMController& eeprom, BuzzerController& buzzer); // Calculate and commit the calibration to memory - bool calibration_valid(const Magnetometer& b, const Magnetometer& s); // Calculate and sanity check calibration data - /* Maximum value per-axis during calibration */ - Magnetometer _calib_max_axis; - /* Minimum value per-axis during calibration */ + /// Minimum observed magnetic field values. Magnetometer _calib_min_axis; + + /// Calibration start timestamp. unsigned long _calib_begin_timestamp; + + /// Running sum of measured magnetic field magnitudes. double _calib_magnitude_sum = 0.0; + + /// Number of calibration samples collected. int _calib_num_datapoints = 0; + + /// Number of progress beeps already played. int _calib_beeping = 0; - /* Magnetometer calibration isn't based on a per-axis calibration, but on getting as many datapoints as possible. - For now let's try 60 sec */ - const unsigned long _calib_time = 60000; + /** + * @brief Duration of the magnetometer calibration procedure. + */ + const unsigned long _calib_time = 60000; }; /** - * @struct Barometer interface + * @struct BarometerSensor + * + * @brief Interface for the onboard barometer. */ struct BarometerSensor { + + /** + * @brief Initializes the barometer. + */ ErrorCode init(); + + /** + * @brief Reads the latest barometer measurement. + */ Barometer read(); }; /** - * @struct Voltage interface + * @struct VoltageSensor + * + * @brief Interface for the onboard voltage monitor. */ struct VoltageSensor { + + /** + * @brief Initializes the voltage monitor. + */ ErrorCode init(); + + /** + * @brief Reads the latest voltage measurement. + */ Voltage read(); }; /** - * @struct GPS interface + * @struct GPSSensor + * + * @brief Interface for the onboard GNSS receiver. */ struct GPSSensor { + + /** + * @brief Initializes the GPS receiver. + */ ErrorCode init(); + + /** + * @brief Returns whether a valid navigation solution is available. + */ bool valid(); + + /** + * @brief Reads the latest GPS navigation solution. + */ GPS read(); + + /// Indicates whether the current year is a leap year. bool is_leap = false; }; +/** + * @struct PyroTickData + * + * @brief Collection of inputs required for pyro state evaluation. + */ struct PyroTickData { + + /// Current finite state machine state. const FSMData& fsm; + + /// Angular Kalman filter state estimate. const AngularKalmanData& akf; + + /// Translational Kalman filter state estimate. const KalmanData& ekf; + + /// Active FSM configuration. const FSMConfiguration& fsm_configuration; + + /// Mutable command flags. CommandFlags& commands; + + /// Current system time. double current_time; + + /// Time elapsed since launch. double time_since_launch; }; /** - * @struct Pyro interface + * @struct Pyro + * + * @brief Controls all pyro firing logic. + * + * Handles manual pyro testing, autonomous deployment events, and + * maintains the persistent state required by the pyro evaluator. */ struct Pyro { + + /** + * @brief Initializes the pyro subsystem. + */ ErrorCode init(); + + /** + * @brief Computes the desired pyro output state. + * + * @param data Current flight state and sensor information. + * + * @return Desired pyro state. + */ PyroState tick(PyroTickData& data); - void set_pyro_safety(); // Sets pyro_start_firing_time and has_fired_pyros. - void reset_pyro_safety(); // Resets pyro_start_firing_time and has_fired_pyros. - - private: + /** + * @brief Starts the manual pyro safety timer. + */ + void set_pyro_safety(); + + /** + * @brief Clears the manual pyro safety timer. + */ + void reset_pyro_safety(); + +private: + /** + * @brief Disarms every pyro channel. + */ void disarm_all_channels(PyroState& prev_state); - - double safety_pyro_start_firing_time; // Time when pyros have fired "this cycle" (pyro test) -- Used to only fire pyros for a time then transition to SAFE - bool safety_has_fired_pyros_this_cycle; // If pyros have fired "this cycle" (pyro test) -- Allows only firing 1 pyro per cycle. - double pyro_trigger_times[MIDAS_NUM_PYROS]; // Storage for the time at which in-flight pyro event checks were triggered for each pyro. - bool pyro_event_check[MIDAS_NUM_PYROS]; // Storage to indicate whether the pyro condition was checked (for pyro delay rule) - bool pyro_event_consumed[MIDAS_NUM_PYROS]; // Storage for whether the pyro has attempted to have been fired. -}; + /// Time at which the current manual firing began. + double safety_pyro_start_firing_time; + + /// Indicates whether a manual firing has already occurred this cycle. + bool safety_has_fired_pyros_this_cycle; + + /// Trigger timestamps for autonomous pyro events. + double pyro_trigger_times[MIDAS_NUM_PYROS]; + + /// Indicates whether delayed-event conditions have been rechecked. + bool pyro_event_check[MIDAS_NUM_PYROS]; + + /// Indicates whether each autonomous pyro event has completed. + bool pyro_event_consumed[MIDAS_NUM_PYROS]; +}; \ No newline at end of file diff --git a/MIDAS/src/hardware/telemetry_backend.cpp b/MIDAS/src/hardware/telemetry_backend.cpp index 6715b502..fa2003fc 100644 --- a/MIDAS/src/hardware/telemetry_backend.cpp +++ b/MIDAS/src/hardware/telemetry_backend.cpp @@ -1,9 +1,8 @@ /** * @file telemetry.cpp * - * @brief This file defines the telemetry class used to facilitate - * telemetry commands and data transfer between the on-board flight - * computer and the ground station. + * @brief Implements the telemetry backend responsible for LoRa + * communication between the flight computer and ground station. * * Spaceshot Avionics 2023-24 * Illinois Space Society - Software Team @@ -22,56 +21,59 @@ // Change to 434.0 or other frequency, must match RX's freq! #define TX_FREQ 421.15 -#define TX_OUTPUT_POWER 22 // dBm -#define LORA_BANDWIDTH 0 // [0: 125 kHz, 1: 250 kHz, 2: 500 kHz, 3: Reserved] -#define LORA_SPREADING_FACTOR 8// [SF7..SF12] -#define LORA_CODINGRATE 4 // [1: 4/5, 2: 4/6, 3: 4/7, 4: 4/8] -#define LORA_PREAMBLE_LENGTH 10 // Same for Tx and Rx -#define LORA_SYMBOL_TIMEOUT 0 // Symbols +#define TX_OUTPUT_POWER 22 // dBm +#define LORA_BANDWIDTH 0 // [0: 125 kHz, 1: 250 kHz, 2: 500 kHz, 3: Reserved] +#define LORA_SPREADING_FACTOR 8 // [SF7..SF12] +#define LORA_CODINGRATE 4 // [1: 4/5, 2: 4/6, 3: 4/7, 4: 4/8] +#define LORA_PREAMBLE_LENGTH 10 // Same for Tx and Rx +#define LORA_SYMBOL_TIMEOUT 0 // Symbols #define LORA_FIX_LENGTH_PAYLOAD_ON false #define LORA_IQ_INVERSION_ON false #define RX_TIMEOUT_VALUE 1000 #define TX_TIMEOUT_VALUE 1000 -#define LORA_BUFFER_SIZE 64 // Define the payload size here +#define LORA_BUFFER_SIZE 64 // Define the payload size here -/** - * @brief Default constructor for the telemetry system -*/ -TelemetryBackend::TelemetryBackend() : lora(SPI, E22_CS, E22_BUSY, E22_DI01, E22_RXEN, E22_RESET) { +TelemetryBackend::TelemetryBackend() + : lora(SPI, E22_CS, E22_BUSY, E22_DI01, E22_RXEN, E22_RESET) { + // Construct the SX1268 driver with the board-specific SPI and GPIO + // connections. } -/** - * @brief Initializes the telemetry system - * - * @return Error Code -*/ + ErrorCode TelemetryBackend::init() { - if(lora.setup() != SX1268Error::NoError) return ErrorCode::LoraCouldNotBeInitialized; - if(lora.set_modulation_params(8, LORA_BW_250, LORA_CR_4_8, false) != SX1268Error::NoError) return ErrorCode::LoraCommunicationFailed; - if(lora.set_frequency((uint32_t) (TX_FREQ * 1e6)) != SX1268Error::NoError) return ErrorCode::LoraCommunicationFailed; - if(lora.set_tx_power(22) != SX1268Error::NoError) return ErrorCode::LoraCommunicationFailed; + + // Initialize communication with the LoRa transceiver. + if (lora.setup() != SX1268Error::NoError) + return ErrorCode::LoraCouldNotBeInitialized; + + // Configure modulation parameters (spreading factor, bandwidth, + // coding rate, and header mode). + if (lora.set_modulation_params(8, LORA_BW_250, LORA_CR_4_8, false) != SX1268Error::NoError) + return ErrorCode::LoraCommunicationFailed; + + // Configure the operating RF frequency. + if (lora.set_frequency((uint32_t)(TX_FREQ * 1e6)) != SX1268Error::NoError) + return ErrorCode::LoraCommunicationFailed; + + // Configure transmit output power. + if (lora.set_tx_power(22) != SX1268Error::NoError) + return ErrorCode::LoraCommunicationFailed; return ErrorCode::NoError; } -/** - * @brief Gets RSSI of recent packets - * - * @return RSSI of most recent packet -*/ int16_t TelemetryBackend::getRecentRssi() { + + // RSSI reporting is currently not implemented by the radio driver. return 0; } -/** - * @brief Sets new frequency for the LoRa module - * - * @param freq New frequency to set the LoRa module to (MHz) -*/ ErrorCode TelemetryBackend::setFrequency(float freq) { - if(lora.set_frequency((uint32_t) (freq * 1e6)) != SX1268Error::NoError) { + + // Update the radio operating frequency. + if (lora.set_frequency((uint32_t)(freq * 1e6)) != SX1268Error::NoError) { return ErrorCode::LoraCommunicationFailed; - } else { - return ErrorCode::NoError; } -} + + return ErrorCode::NoError; +} \ No newline at end of file diff --git a/MIDAS/src/hardware/telemetry_backend.h b/MIDAS/src/hardware/telemetry_backend.h index bb82df28..8a656251 100644 --- a/MIDAS/src/hardware/telemetry_backend.h +++ b/MIDAS/src/hardware/telemetry_backend.h @@ -1,77 +1,127 @@ #pragma once -#include "errors.h" -#include "hal.h" +#include +#include "util/hal.h" #include "pins.h" #include /** * @class TelemetryBackend - * - * @brief Class that wraps the Telemetry functions -*/ + * + * @brief Interface for the onboard LoRa telemetry radio. + * + * Wraps the SX1268/E22 radio driver and provides functions for + * initialization, packet transmission, reception, frequency control, + * and SPI synchronization. + */ class TelemetryBackend { public: + /** + * @brief Constructs the telemetry backend. + */ TelemetryBackend(); + + /** + * @brief Initializes the LoRa radio. + * + * Configures the radio hardware and prepares it for packet + * transmission and reception. + * + * @return Error code indicating initialization status. + */ [[nodiscard]] ErrorCode init(); + /** + * @brief Returns the RSSI of the most recently received packet. + * + * @return Received Signal Strength Indicator (RSSI) in dBm. + */ int16_t getRecentRssi(); + + /** + * @brief Changes the operating radio frequency. + * + * @param frequency Desired frequency in MHz. + * + * @return Error code indicating whether the operation succeeded. + */ ErrorCode setFrequency(float frequency); + + /** + * @brief Assigns the SPI mutex used by the radio driver. + * + * @param mtx FreeRTOS semaphore protecting the shared SPI bus. + */ void set_spi_mutex(SemaphoreHandle_t mtx) { lora.set_spi_mutex(mtx); } /** - * @brief This function transmits data from the struct provided as - * the parameter (data collected from sensor suite) to the - * ground station. The function also switches to a new commanded - * frequency based on a previously received command and waits for - * a response from the ground station. + * @brief Transmits a packet over the LoRa radio. * - * @param sensor_data: struct of data from the sensor suite to be - * transmitted to the ground station. + * The packet type must fit within the SX1268 maximum payload size + * of 255 bytes. If transmission fails, the radio is automatically + * reinitialized. * - * @return void + * @tparam T Packet type to transmit. + * + * @param data Packet to send. */ template void send(const T& data) { static_assert(sizeof(T) <= 0xFF, "The data type to send is too large"); // Max payload is 255 SX1268Error result = lora.send((uint8_t*) &data, sizeof(T)); + if(result != SX1268Error::NoError) { Serial.print("Lora TX error "); Serial.println((int)result); - // Re init the lora + + // Attempt to recover from communication failure. (void)init(); } } /** - * @brief Reads message from the LoRa - * - * @param write The buffer to write the data to - * - * @return bool indicating a successful read and write to buffer - */ + * @brief Attempts to receive a packet from the LoRa radio. + * + * Waits up to the specified timeout for a packet of type T. Radio + * errors automatically trigger a reinitialization attempt. + * + * @tparam T Packet type expected. + * + * @param write Buffer where the received packet will be stored. + * @param wait_milliseconds Maximum receive timeout in milliseconds. + * + * @return true if a packet was successfully received. + * @return false if the receive timed out or an error occurred. + */ template bool read(T* write, int wait_milliseconds) { static_assert(sizeof(T) <= 0xFF, "The data type to receive is too large"); + uint8_t len = sizeof(T); - // set receive mode + + // Receive a packet from the radio. SX1268Error result = lora.recv((uint8_t*) write, len, wait_milliseconds); + if(result == SX1268Error::NoError) { return true; - } else if(result == SX1268Error::RxTimeout) { + } + else if(result == SX1268Error::RxTimeout) { return false; - } else { + } + else { Serial.print("Lora error on rx "); Serial.println((int)result); - //Re init the lora + // Attempt to recover from communication failure. (void)init(); + return false; } } private: + /// SX1268 LoRa transceiver driver. SX1268 lora; -}; +}; \ No newline at end of file diff --git a/MIDAS/src/hilsim/kal.cpp b/MIDAS/src/hilsim/kal.cpp index 13749631..47f16d89 100644 --- a/MIDAS/src/hilsim/kal.cpp +++ b/MIDAS/src/hilsim/kal.cpp @@ -1,6 +1,6 @@ // KAL - Kamaji Abstraction Layer #include "kal.h" -#include "midas_shell.h" +#include "shell/midas_shell.h" // ---- GLOBALS ---- SDSink sink; diff --git a/MIDAS/src/hilsim/kal.h b/MIDAS/src/hilsim/kal.h index 8b4037c8..5d60492c 100644 --- a/MIDAS/src/hilsim/kal.h +++ b/MIDAS/src/hilsim/kal.h @@ -1,6 +1,6 @@ // KAL - Kamaji Abstraction Layer #pragma once -#include "systems.h" +#include "flight-systems/systems.h" #include "kamaji/kal_rocket.h" #include "kamaji/kal_error.h" #include "kamaji/kal_events.h" diff --git a/MIDAS/src/hilsim/kamaji/kal_events.h b/MIDAS/src/hilsim/kamaji/kal_events.h index 84f8cd4d..c2d70ca0 100644 --- a/MIDAS/src/hilsim/kamaji/kal_events.h +++ b/MIDAS/src/hilsim/kamaji/kal_events.h @@ -1,7 +1,7 @@ #pragma once #include #include "kal_error.h" -#include "log_checksum.h" +#include "logging/log_checksum.h" // ---- CONFIGURATION ---- #define EVENT_STACK_SIZE 8 diff --git a/MIDAS/src/hilsim/kamaji/kal_interface.h b/MIDAS/src/hilsim/kamaji/kal_interface.h index f858a12f..97c5e3fa 100644 --- a/MIDAS/src/hilsim/kamaji/kal_interface.h +++ b/MIDAS/src/hilsim/kamaji/kal_interface.h @@ -3,7 +3,7 @@ #include #include #include "TCAL9538.h" -#include +#include // Handles MIDAS system setup and interfacing inline void k_midas_setup() { diff --git a/MIDAS/src/hilsim/kamaji/kal_rocket.h b/MIDAS/src/hilsim/kamaji/kal_rocket.h index 22334c0e..4281fa28 100644 --- a/MIDAS/src/hilsim/kamaji/kal_rocket.h +++ b/MIDAS/src/hilsim/kamaji/kal_rocket.h @@ -1,7 +1,7 @@ #pragma once // A HILSIM version of the RocketData struct without any of the additional wrapping. -#include "sensor_data.h" -#include "rocket_state.h" +#include "flight-systems/sensor_data.h" +#include "flight-systems/rocket_state.h" struct KRocketData { diff --git a/MIDAS/src/hilsim/kamaji/kal_sensordata.h b/MIDAS/src/hilsim/kamaji/kal_sensordata.h index 2eaf7b0c..ad683947 100644 --- a/MIDAS/src/hilsim/kamaji/kal_sensordata.h +++ b/MIDAS/src/hilsim/kamaji/kal_sensordata.h @@ -1,6 +1,6 @@ #pragma once -#include "data_logging.h" -#include "log_format.h" +#include "logging/data_logging.h" +#include "logging/log_format.h" #include "kal_error.h" #include "kal_events.h" diff --git a/MIDAS/src/hilsim/sensors/sensors.h b/MIDAS/src/hilsim/sensors/sensors.h index a9d22689..8333e564 100644 --- a/MIDAS/src/hilsim/sensors/sensors.h +++ b/MIDAS/src/hilsim/sensors/sensors.h @@ -1,10 +1,10 @@ #pragma once -#include "errors.h" -#include "sensor_data.h" -#include "rocket_state.h" -#include "esp_eeprom.h" -#include "buzzer.h" +#include +#include "flight-systems/sensor_data.h" +#include "flight-systems/rocket_state.h" +#include "logging/esp_eeprom.h" +#include "util/buzzer.h" /** * @struct IMUSensor (HILSIM stub). diff --git a/MIDAS/src/hilsim/sensors/telemetry_backend.h b/MIDAS/src/hilsim/sensors/telemetry_backend.h index 9628fb71..7c930aa2 100644 --- a/MIDAS/src/hilsim/sensors/telemetry_backend.h +++ b/MIDAS/src/hilsim/sensors/telemetry_backend.h @@ -1,7 +1,7 @@ #pragma once -#include "errors.h" -#include "hal.h" +#include +#include "util/hal.h" #include "hardware/pins.h" #include "TCAL9538.h" diff --git a/MIDAS/src/hilsim/stream/main.cpp b/MIDAS/src/hilsim/stream/main.cpp index c3d5cb5f..46e905a8 100644 --- a/MIDAS/src/hilsim/stream/main.cpp +++ b/MIDAS/src/hilsim/stream/main.cpp @@ -12,8 +12,8 @@ #include #include -#include "sensor_data.h" -#include "log_format.h" +#include "flight-systems/sensor_data.h" +#include "logging/log_format.h" #include "crc.h" #define STR2(x) #x diff --git a/MIDAS/src/log_format.h b/MIDAS/src/log_format.h deleted file mode 100644 index 9891974e..00000000 --- a/MIDAS/src/log_format.h +++ /dev/null @@ -1,80 +0,0 @@ -#pragma once - -#include "sensor_data.h" -#define LOG_FMT_VERSION 1 - -/** - * @enum ReadingDiscriminant - * - * @brief ID for each sensor, 0 is not used to make it easier to spot bugs - */ -enum ReadingDiscriminant { - ID_IMU = 1, - ID_BAROMETER = 2, - ID_VOLTAGE = 4, - ID_GPS = 5, - ID_MAGNETOMETER = 6, - ID_KALMAN = 8, - ID_FSM = 9, - ID_PYRO = 10, - ID_CAMERADATA = 11, - ID_ANGULARKALMAN = 12, - ID_SFLP = 13, - COUNT = 14, // Last element must be COUNT for HIL -}; - -constexpr uint8_t READING_DISC_COUNT = static_cast(ReadingDiscriminant::COUNT); - -/** - * @struct LoggerReading - * - * @brief representation of data that will be logged - * - * @note - * This struct isn't actually logged as-is, because if we did we'd waste extra space since - * unions are the size of their largest member. This is just a reference struct. - * - * Instead, we use 4 bytes for the discriminant, 4 bytes for the timestamp, and then write the - * actual data. No padding between inside these items or between readings. - */ -struct LoggedReading { - ReadingDiscriminant discriminant; - uint32_t timestamp_ms; - union { - IMU imu; - IMU_SFLP sflp; - Barometer barometer; - Voltage voltage; - GPS gps; - Magnetometer magnetometer; - KalmanData kalman; - AngularKalmanData angular_kalman; - FSMData fsm; - PyroState pyro; - CameraData cameradata; - } data; -}; - -/** - * Associates a sensor type with its discriminant ID. - * Used by data_logging.cpp for compile-time lookup, and parsed by log_enc.py - * to build the discriminant-to-union-variant mapping for log metadata. - * - * Args: (type_name, discriminant_id, union_field_name) - */ -template -constexpr ReadingDiscriminant get_discriminant(); - -#define ASSOCIATE(ty, id, field) template<> constexpr ReadingDiscriminant get_discriminant() { return ReadingDiscriminant::id; } - -ASSOCIATE(IMU, ID_IMU, imu) -ASSOCIATE(IMU_SFLP, ID_SFLP, sflp) -ASSOCIATE(Barometer, ID_BAROMETER, barometer) -ASSOCIATE(Voltage, ID_VOLTAGE, voltage) -ASSOCIATE(GPS, ID_GPS, gps) -ASSOCIATE(Magnetometer, ID_MAGNETOMETER, magnetometer) -ASSOCIATE(KalmanData, ID_KALMAN, kalman) -ASSOCIATE(AngularKalmanData, ID_ANGULARKALMAN, angular_kalman) -ASSOCIATE(FSMData, ID_FSM, fsm) -ASSOCIATE(PyroState, ID_PYRO, pyro) -ASSOCIATE(CameraData, ID_CAMERADATA, cameradata) diff --git a/MIDAS/src/logging/data_logging.cpp b/MIDAS/src/logging/data_logging.cpp new file mode 100644 index 00000000..eff21c6f --- /dev/null +++ b/MIDAS/src/logging/data_logging.cpp @@ -0,0 +1,179 @@ +#include "data_logging.h" +#include "log_format.h" +#include "log_checksum.h" + +/** + * @brief Forward declaration for retrieving the log discriminant associated + * with a specific reading type. + * + * Each supported sensor or subsystem type has a unique ReadingDiscriminant + * value used to identify serialized log packets during parsing. + */ +template +constexpr ReadingDiscriminant get_discriminant(); + +/** + * @brief Writes a single sensor reading to the specified logging sink. + * + * Each logged reading is serialized in the following order: + * 1. Reading type identifier (ReadingDiscriminant) + * 2. Timestamp in milliseconds + * 3. Raw reading data + * + * This standardized format allows log parsers to identify packet types while + * replaying or analyzing flight logs. + * + * @tparam T Sensor data type contained in the reading. + * @param sink Destination logging backend. + * @param reading Timestamped sensor reading to serialize. + */ +template +void log_reading(LogSink& sink, Reading& reading) { + ReadingDiscriminant discriminant = get_discriminant(); + sink.write((uint8_t*) &discriminant, sizeof(ReadingDiscriminant)); + sink.write((uint8_t*) &reading.timestamp_ms, sizeof(uint32_t)); + sink.write((uint8_t*) &reading.data, sizeof(T)); +} + +/** + * @brief Flushes queued sensor readings to the logging sink. + * + * Reads queued samples from a SensorData object and writes each one using + * log_reading(). To prevent logging from monopolizing execution time, the + * function writes at most 20 readings per invocation. + * + * @tparam T Sensor data type stored by the SensorData queue. + * @param sink Destination logging backend. + * @param sensor_data Sensor queue containing pending readings. + * + * @return Number of readings successfully written. + */ +template +uint32_t log_from_sensor_data(LogSink& sink, SensorData& sensor_data) { + Reading reading; + uint32_t read = 0; + + while (read < 20 && sensor_data.getQueued(&reading)) { + log_reading(sink, reading); + read++; + } + + return read; +} + +/** + * @brief Begins a new flight log. + * + * Writes a fixed checksum header to the beginning of the log file so that + * parsing tools can verify the file format before processing logged data. + * + * @param sink Logging backend to initialize. + */ +void log_begin(LogSink& sink) { + uint32_t checksum = LOG_CHECKSUM; + sink.write((uint8_t*) &checksum, 4); +} + +/** + * @brief Logs all available queued flight data. + * + * Flushes pending readings from every major subsystem queue, including sensor + * measurements, state estimation, FSM state, pyro status, and camera data. + * + * @param sink Destination logging backend. + * @param data Rocket data structure containing all logging queues. + */ +void log_data(LogSink& sink, RocketData& data) { + log_from_sensor_data(sink, data.imu); + log_from_sensor_data(sink, data.sflp); + log_from_sensor_data(sink, data.barometer); + log_from_sensor_data(sink, data.voltage); + log_from_sensor_data(sink, data.gps); + log_from_sensor_data(sink, data.magnetometer); + log_from_sensor_data(sink, data.fsm_state); + log_from_sensor_data(sink, data.kalman); + log_from_sensor_data(sink, data.angular_kalman_data); + log_from_sensor_data(sink, data.pyro); + log_from_sensor_data(sink, data.cam_data); +} + +#ifndef SILSIM +#define MAX_FILES 999 + +/** + * @brief Generates a unique filename for a new flight log. + * + * Searches the filesystem for existing files using the supplied base name and + * extension. If a matching filename already exists, an incrementing numeric + * suffix is appended until an unused filename is found or the maximum file + * number is reached. + * + * The selected filename is written back into the supplied buffer. + * + * @param fileName Buffer containing the base filename. On return, contains the + * generated filename including path and numeric suffix. + * @param fileExtensionParam Desired file extension (e.g. ".bin"). + * @param fs Filesystem used to check for existing files. + * @param file_num Starting file number, typically recovered from EEPROM. + * @param fileno_out Pointer that receives the next file number. + * + * @return Pointer to the updated filename buffer. + */ +char* sdFileNamer(char* fileName, char* fileExtensionParam, FS& fs, uint16_t file_num, int* fileno_out) { + char fileExtension[strlen(fileExtensionParam) + 1]; + strcpy(fileExtension, fileExtensionParam); + + char inputName[256] = {0}; + strcpy(inputName, "/"); + strcat(inputName, fileName); + strcat(inputName, fileExtension); + + // Check whether the base filename already exists. + bool exists = fs.exists(inputName); + + if (exists) { + bool fileExists = false; + + // Start searching from the recovered file number. + int i = file_num; + + while (!fileExists) { + if (i > MAX_FILES) { + // Maximum file number reached. Reuse the final filename to avoid + // overflowing the filename buffer. + strcpy(inputName, "/"); + strcat(inputName, fileName); + strcat(inputName, "999"); + strcat(inputName, fileExtension); + *fileno_out = 999; + break; + } + + // Convert the current file number into a string. + char iStr[16] = {0}; + itoa(i, iStr, 10); + + // Construct "/data.ext". + strcpy(inputName, "/"); + strcat(inputName, fileName); + strcat(inputName, iStr); + strcat(inputName, fileExtension); + + // Stop once an unused filename is found. + if (!fs.exists(inputName)) { + fileExists = true; + *fileno_out = i + 1; + } + + i++; + } + } else { + // Base filename is unused. + *fileno_out = 0; + } + + strcpy(fileName, inputName); + + return fileName; +} +#endif \ No newline at end of file diff --git a/MIDAS/src/logging/data_logging.h b/MIDAS/src/logging/data_logging.h new file mode 100644 index 00000000..e69cd2f5 --- /dev/null +++ b/MIDAS/src/logging/data_logging.h @@ -0,0 +1,199 @@ +#pragma once + +#include "flight-systems/rocket_state.h" +#include + +#if defined(SILSIM) +//#include "silsim/FileSink.h" +#elif defined(HILSIM) +#else +//#include "hardware/SDLog.h" +#endif + +/** + * @class LogSink + * + * @brief Abstract interface for flight data logging backends. + * + * A LogSink provides a common interface for writing binary flight logs and + * associated metadata regardless of the underlying storage medium. Hardware + * implementations typically write to onboard flash or an SD card, while + * simulation builds may implement alternative sinks. + */ +class LogSink { +public: + LogSink() = default; + + bool failed_wr = false; ///< True if a binary log write has failed. + bool failed_mr = false; ///< True if a metadata write has failed. + + /** + * @brief Initializes the logging backend. + * + * Opens any required storage devices, creates log files, and prepares the + * sink for subsequent write operations. + * + * @return ErrorCode indicating whether initialization succeeded. + */ + virtual ErrorCode init() = 0; + + /** + * @brief Writes binary flight data to the log. + * + * @param data Pointer to the byte buffer to write. + * @param size Number of bytes to write. + */ + virtual void write(const uint8_t* data, size_t size) = 0; + + /** + * @brief Writes metadata to the metadata log. + * + * Metadata typically consists of important flight events or summary values + * that can be quickly parsed without processing the entire binary log. + * + * @param data Pointer to the metadata buffer. + * @param size Number of bytes in the metadata buffer. + */ + virtual void write_meta(const uint8_t* data, size_t size) = 0; + + uint16_t current_file_no = 0; ///< Current flight log file number. + char active_bin_name[20] = {0}; ///< Name of the active binary log file. + char active_meta_name[20] = {0};///< Name of the active metadata file. +}; + +/** + * @brief Initializes the flight logging system. + * + * Performs any required setup before flight data begins being recorded. + * + * @param sink Logging backend used for storage. + */ +void log_begin(LogSink& sink); + +/** + * @brief Logs a complete rocket telemetry packet. + * + * Serializes and writes a RocketData packet to the configured logging backend. + * + * @param sink Logging backend used for storage. + * @param data Rocket telemetry packet to log. + */ +void log_data(LogSink& sink, RocketData& data); + +template +class MultipleLogSink : public LogSink { +public: + MultipleLogSink() = default; + + /** + * @brief Initializes the logging sink. + * + * Base case for the recursive MultipleLogSink template. Since no sinks are + * present, initialization always succeeds. + * + * @return ErrorCode::NoError. + */ + ErrorCode init() override { + return ErrorCode::NoError; + }; + + /** + * @brief Writes binary log data. + * + * Base case implementation. No action is performed. + * + * @param data Pointer to the data buffer. + * @param size Number of bytes to write. + */ + void write(const uint8_t* data, size_t size) override {}; + + /** + * @brief Writes metadata. + * + * Base case implementation. No action is performed. + * + * @param data Pointer to the metadata buffer. + * @param size Number of bytes to write. + */ + void write_meta(const uint8_t* data, size_t size) override {}; +}; + +template +class MultipleLogSink : public LogSink { +public: + MultipleLogSink() = default; + + /** + * @brief Constructs a recursive collection of logging sinks. + * + * @param sink_ First logging sink. + * @param sinks_ Remaining logging sinks. + */ + explicit MultipleLogSink(Sink sink_, Sinks... sinks_) : sink(sink_), sinks(sinks_...) { }; + + /** + * @brief Initializes every logging sink. + * + * Initialization proceeds recursively. If any sink fails to initialize, + * initialization stops immediately and the corresponding error is returned. + * + * @return ErrorCode indicating success or the first initialization failure. + */ + ErrorCode init() override { + ErrorCode result = sink.init(); + if (result != ErrorCode::NoError) { + return result; + } + return sinks.init(); + }; + + /** + * @brief Writes binary data to every logging sink. + * + * The write operation is forwarded recursively so each configured backend + * receives an identical copy of the flight log. + * + * @param data Pointer to the data buffer. + * @param size Number of bytes to write. + */ + void write(const uint8_t* data, size_t size) override { + sink.write(data, size); + sinks.write(data, size); + }; + + /** + * @brief Writes metadata to every logging sink. + * + * The metadata packet is forwarded recursively to each configured backend. + * + * @param data Pointer to the metadata buffer. + * @param size Number of bytes to write. + */ + void write_meta(const uint8_t* data, size_t size) override { + sink.write(data, size); + sinks.write(data, size); + }; + +private: + Sink sink; ///< Current logging backend. + MultipleLogSink sinks; ///< Remaining logging backends. +}; + +#ifndef SILSIM +#include +/** + * @brief Determines the next available log filename. + * + * Generates a unique filename using the provided base name and extension, + * avoiding collisions with existing files on the filesystem. + * + * @param fileName Base filename buffer. + * @param fileExtensionParam Desired file extension. + * @param fs Filesystem to search. + * @param file_num Starting file number. + * @param fileno_out Pointer that receives the selected file number. + * + * @return Pointer to the generated filename. + */ +char* sdFileNamer(char* fileName, char* fileExtensionParam, FS& fs, uint16_t file_num, int* fileno_out); +#endif \ No newline at end of file diff --git a/MIDAS/src/logging/data_logging_meta.h b/MIDAS/src/logging/data_logging_meta.h new file mode 100644 index 00000000..92863e80 --- /dev/null +++ b/MIDAS/src/logging/data_logging_meta.h @@ -0,0 +1,265 @@ +#include +#include "util/Queue.h" +#include +#include +#include + +/// Maximum number of bytes that may be stored in a single metadata log entry. +#define META_LOGGING_MAX_SIZE 64 + +/** + * @enum MetaDataCode + * + * @brief Identifiers for every metadata value that can be recorded during + * flight. + * + * These identifiers are stored alongside metadata values so they can be + * interpreted correctly during post-flight analysis. + */ +enum MetaDataCode { + // Launch events + EVENT_TLAUNCH, + EVENT_TBURNOUT, + EVENT_TIGNITION, + EVENT_TAPOGEE, + EVENT_TMAIN, + EVENT_TMAX_ACCEL, + EVENT_TMAX_VEL, + EVENT_TMAX_DESCENT_RATE, + + // Non-events + DATA_LAUNCHSITE_BARO, + DATA_LAUNCHSITE_ALT, + DATA_LAUNCHSITE_GPS_ALT, + DATA_LAUNCHSITE_GPS_LAT, + DATA_LAUNCHSITE_GPS_LONG, + DATA_LAUNCH_INITIAL_TILT, + DATA_TILT_AT_BURNOUT, + DATA_TILT_AT_IGNITION, + DATA_BARO_AT_IGNITION, + DATA_MAX_ACCEL, + DATA_MAX_VEL, + DATA_ALT_AT_BURNOUT, + DATA_MAX_DESCENT_RATE +}; + +/** + * @enum MetalogSummaryEntryType + * + * @brief Specifies how a summary value should be updated throughout + * flight. + */ +enum class MetalogSummaryEntryType { + /// Always store the most recent value. + CURRENT, + + /// Store only the maximum value observed. + MAXIMUM, + + /// Store only the minimum value observed. + MINIMUM, +}; + +struct MetalogSummary; + +/** + * @struct MetaLogging + * + * @brief Queues metadata entries for persistent storage. + * + * Metadata consists of significant flight events and summary values that + * are written separately from the primary telemetry log. + */ +struct MetaLogging { +public: + + /** + * @struct MetaLogEntry + * + * @brief Represents a single queued metadata record. + */ + struct MetaLogEntry { + + /// Identifier describing the stored data. + MetaDataCode log_type; + + /// Size of the stored payload in bytes. + size_t size; + + /// Raw payload bytes. + char data[META_LOGGING_MAX_SIZE]; + }; + + /// Pointer to the metadata summary manager. + MetalogSummary* summary; + + /// Queue of pending metadata records. + Queue _q; + + /** + * @brief Retrieves the next queued metadata entry. + * + * @param out Destination for the dequeued entry. + * + * @return true if an entry was available. + * @return false if the queue was empty. + */ + bool get_queued(MetaLogEntry* out) { + return _q.receive(out); + } + + /** + * @brief Queues a metadata value for logging. + * + * The supplied object is copied directly into the metadata entry and + * later written to persistent storage. + * + * @tparam T Type of the value being logged. + * + * @param data_type Metadata identifier. + * @param data Value to record. + */ + template + void log_data(MetaDataCode data_type, const T& data) { + + // Ensure the value fits within the fixed metadata payload buffer. + static_assert(sizeof(T) <= META_LOGGING_MAX_SIZE, + "Datatype for log_data too large"); + + // Create a metadata entry. + MetaLogEntry entry{data_type, 0, 0}; + entry.size = sizeof(T); + + // Copy the raw bytes into the entry payload. + memcpy(entry.data, &data, entry.size); + + // Queue the entry for later storage. + _q.send(entry); + } +}; + +/** + * @class MetalogSummaryEntry + * + * @brief Tracks a single metadata summary statistic. + * + * A summary entry may store the current, maximum, or minimum value of a + * quantity throughout the flight. + * + * @tparam T Type of value being tracked. + */ +template +class MetalogSummaryEntry { + +public: + + /** + * @brief Constructs a metadata summary entry. + * + * @param metacode Metadata identifier. + * @param metatype Update policy. + * @param default_val Initial value. + */ + MetalogSummaryEntry(const MetaDataCode& metacode, + const MetalogSummaryEntryType& metatype = MetalogSummaryEntryType::CURRENT, + const T& default_val = T()) { + code = metacode; + type = metatype; + data = default_val; + } + + /** + * @brief Updates the tracked value. + * + * The update behavior depends on the configured summary type. + * + * @param newval New measurement. + */ + void update(const T& newval) { + switch(type) { + case MetalogSummaryEntryType::CURRENT: + data = newval; + break; + + case MetalogSummaryEntryType::MAXIMUM: + if (newval > data) { + data = newval; + } + break; + + case MetalogSummaryEntryType::MINIMUM: + if (newval < data) { + data = newval; + } + break; + } + } + + /** + * @brief Queues the tracked value for logging. + * + * @param metalog Metadata logger. + */ + void commit(MetaLogging& metalog); + +private: + /// Stored summary value. + T data; + + /// Metadata identifier. + MetaDataCode code; + + /// Update policy. + MetalogSummaryEntryType type; +}; + +/** + * @struct MetalogSummary + * + * @brief Collection of all flight metadata summary entries. + * + * Stores timestamps for significant flight events and summary statistics + * such as maximum acceleration, velocity, and descent rate. + */ +struct MetalogSummary { + + // Launch events + MetalogSummaryEntry event_tlaunch{MetaDataCode::EVENT_TLAUNCH}; + MetalogSummaryEntry event_tburnout{MetaDataCode::EVENT_TBURNOUT}; + MetalogSummaryEntry event_tignition{MetaDataCode::EVENT_TIGNITION}; + MetalogSummaryEntry event_tapogee{MetaDataCode::EVENT_TAPOGEE}; + MetalogSummaryEntry event_tmain{MetaDataCode::EVENT_TMAIN}; + MetalogSummaryEntry event_tmax_accel{MetaDataCode::EVENT_TMAX_ACCEL}; + MetalogSummaryEntry event_tmax_vel{MetaDataCode::EVENT_TMAX_VEL}; + MetalogSummaryEntry event_tmax_descent_rate{MetaDataCode::EVENT_TMAX_DESCENT_RATE}; + + // Flight summary values + MetalogSummaryEntry data_launchsite_baro{MetaDataCode::DATA_LAUNCHSITE_BARO}; + MetalogSummaryEntry data_launchsite_gps_alt{MetaDataCode::DATA_LAUNCHSITE_GPS_ALT}; + MetalogSummaryEntry data_launchsite_gps_lat{MetaDataCode::DATA_LAUNCHSITE_GPS_LAT}; + MetalogSummaryEntry data_launchsite_gps_long{MetaDataCode::DATA_LAUNCHSITE_GPS_LONG}; + MetalogSummaryEntry data_launch_initial_tilt{MetaDataCode::DATA_LAUNCH_INITIAL_TILT}; + MetalogSummaryEntry data_tilt_at_burnout{MetaDataCode::DATA_TILT_AT_BURNOUT}; + MetalogSummaryEntry data_tilt_at_ignition{MetaDataCode::DATA_TILT_AT_IGNITION}; + MetalogSummaryEntry data_baro_at_ignition{MetaDataCode::DATA_BARO_AT_IGNITION}; + MetalogSummaryEntry data_max_accel{MetaDataCode::DATA_MAX_ACCEL, + MetalogSummaryEntryType::MAXIMUM, + -std::numeric_limits::max()}; + MetalogSummaryEntry data_max_vel{MetaDataCode::DATA_MAX_VEL, + MetalogSummaryEntryType::MAXIMUM, + -std::numeric_limits::max()}; + MetalogSummaryEntry data_alt_at_burnout{MetaDataCode::DATA_ALT_AT_BURNOUT}; + MetalogSummaryEntry data_max_descent_rate{MetaDataCode::DATA_MAX_DESCENT_RATE, + MetalogSummaryEntryType::MAXIMUM, + -std::numeric_limits::max()}; +}; + +/** + * @brief Commits the stored summary value to the metadata logger. + * + * @param metalog Metadata logger. + */ +template +void MetalogSummaryEntry::commit(MetaLogging& metalog) { + metalog.log_data(code, data); +} \ No newline at end of file diff --git a/MIDAS/src/logging/esp_eeprom.cpp b/MIDAS/src/logging/esp_eeprom.cpp new file mode 100644 index 00000000..340bc24b --- /dev/null +++ b/MIDAS/src/logging/esp_eeprom.cpp @@ -0,0 +1,101 @@ +#include "esp_eeprom.h" + +/** + * @brief Reads and validates the contents of EEPROM. + * + * The raw EEPROM bytes are copied into a temporary MIDASEEPROM structure and + * verified using the stored checksum. If the checksum does not match the + * expected EEPROM schema checksum, the read is considered invalid and no data + * is loaded. + * + * @return true if the EEPROM contents are valid and successfully loaded into + * the controller. + * @return false if the checksum is invalid or the stored data is incompatible + * with the current EEPROM layout. + */ +bool EEPROMController::read() { + MIDASEEPROM _read; + uint8_t buf[EEPROM_SIZE]; + + // Read the raw EEPROM contents into a temporary buffer. + for (int i = 0; i < EEPROM_SIZE; i++) { + buf[i] = EEPROM.read(i); + } + + // Deserialize the buffer into the EEPROM schema. + memcpy(&_read, buf, EEPROM_SIZE); + + // Verify the stored checksum before accepting the data. + if(_read.checksum != EEPROM_CHECKSUM) { + // Wrong checksum, cannot read. + return false; + } + + // Store the validated EEPROM contents. + data = _read; + + return true; +} + +/** + * @brief Writes the current EEPROM data to non-volatile memory. + * + * The checksum is updated before serialization to ensure future reads can + * verify data integrity. After writing and committing the data, the EEPROM is + * immediately reread to confirm the stored contents are valid. + * + * @return true if the committed data can be successfully read back. + * @return false if verification fails after committing. + */ +bool EEPROMController::commit() { + uint8_t buf[EEPROM_SIZE]; + + // Update the checksum before writing. + data.checksum = EEPROM_CHECKSUM; + + // Serialize the EEPROM schema into a raw byte buffer. + memcpy(buf, &data, EEPROM_SIZE); + + // Write each byte to EEPROM. + for (int i = 0; i < EEPROM_SIZE; i++) { + EEPROM.write(i, buf[i]); + } + + // Commit pending writes to non-volatile memory. + EEPROM.commit(); + + // Verify the written data. + return read(); +} + +/** + * @brief Initializes the EEPROM controller. + * + * Allocates the EEPROM storage region and attempts to load the stored + * configuration. If the stored checksum is invalid or the EEPROM layout is + * incompatible with the current firmware, the EEPROM is reset to default + * values and rewritten using the current schema. + * + * @return ErrorCode::NoError after initialization completes. + */ +ErrorCode EEPROMController::init() { + // Ensure the configured EEPROM layout fits within the hardware limit. + static_assert(EEPROM_SIZE <= EEPROM_MAX_SIZE); + + EEPROM.begin((size_t)EEPROM_SIZE); + + if (!read()) { + // The stored EEPROM schema is incompatible with this firmware version. + // Create a default EEPROM image with the correct checksum. + MIDASEEPROM empty_setting; + empty_setting.checksum = EEPROM_CHECKSUM; + data = empty_setting; + + Serial.println("EEPROM CHECKSUM INCOMPATIBLE"); + + // Rewrite EEPROM using the default configuration. + commit(); + } + + return ErrorCode::NoError; +} \ No newline at end of file diff --git a/MIDAS/src/esp_eeprom.h b/MIDAS/src/logging/esp_eeprom.h similarity index 97% rename from MIDAS/src/esp_eeprom.h rename to MIDAS/src/logging/esp_eeprom.h index 47b1c7ed..350e136d 100644 --- a/MIDAS/src/esp_eeprom.h +++ b/MIDAS/src/logging/esp_eeprom.h @@ -3,7 +3,7 @@ // Handler for ESP32's eeprom read/writes for MIDAS FSW. #include "esp_eeprom_format.h" #include "esp_eeprom_checksum.h" -#include "errors.h" +#include #include #include diff --git a/MIDAS/src/logging/esp_eeprom_checksum.h b/MIDAS/src/logging/esp_eeprom_checksum.h new file mode 100644 index 00000000..b0639a23 --- /dev/null +++ b/MIDAS/src/logging/esp_eeprom_checksum.h @@ -0,0 +1,2 @@ +// autogenerated on build by applying crc32 on esp_eeprom_format.h +#define EEPROM_CHECKSUM (0xd5579c15) diff --git a/MIDAS/src/logging/esp_eeprom_format.h b/MIDAS/src/logging/esp_eeprom_format.h new file mode 100644 index 00000000..a581dba3 --- /dev/null +++ b/MIDAS/src/logging/esp_eeprom_format.h @@ -0,0 +1,70 @@ +#pragma once + +#include "flight-systems/sensor_data.h" +#include "finite-state-machines/fsm_config.h" + +/** + * @brief Defines the persistent EEPROM layout for the MIDAS flight computer. + * + * This structure represents all data stored in non-volatile memory. It is used + * to generate the EEPROM checksum, allowing firmware to verify that stored data + * is valid and compatible with the current EEPROM schema during startup. + * + * Any modification to this structure changes the EEPROM layout and should be + * accompanied by an updated checksum to prevent invalid data from being loaded. + */ +struct MIDASEEPROM { + /** + * @brief Checksum used to validate the EEPROM contents. + */ + uint32_t checksum; + + /** + * @brief Number of the most recently created flight log. + * + * Used to continue log numbering across power cycles and prevent + * overwriting previous flight data. + */ + uint16_t sd_file_num_last = 0; + + /** + * @brief Unique flight computer serial number. + */ + uint8_t serial = 0; + + /** + * @brief Default LoRa telemetry frequency in MHz. + */ + float frequency = 421.15; + + /** + * @brief High-G accelerometer calibration bias. + * + * Applied to compensate for sensor offset after calibration. + */ + Acceleration lsm6dsv320x_hg_xl_bias = {0.0f, 0.0f, 0.0f}; + + /** + * @brief Magnetometer soft-iron calibration scale factors. + * + * Used to compensate for magnetic distortion caused by nearby + * ferromagnetic materials. + */ + Magnetometer mmc5983ma_softiron_bias = {1.0f, 1.0f, 1.0f}; + + /** + * @brief Magnetometer hard-iron calibration offsets. + * + * Used to remove constant magnetic field offsets introduced by + * permanently magnetized components. + */ + Magnetometer mmc5983ma_hardiron_bias = {0.0f, 0.0f, 0.0f}; + + /** + * @brief Stored finite state machine configuration. + * + * Contains configurable flight state transitions, thresholds, + * timers, and pyro firing rules. + */ + FSMConfiguration fsm_config; +}; \ No newline at end of file diff --git a/MIDAS/src/logging/log_checksum.h b/MIDAS/src/logging/log_checksum.h new file mode 100644 index 00000000..89aa9382 --- /dev/null +++ b/MIDAS/src/logging/log_checksum.h @@ -0,0 +1,2 @@ +// autogenerated on build by applying crc32 on the concatenation of log_format.h and sensor_data.h +#define LOG_CHECKSUM (0x1e8c8dfd) diff --git a/MIDAS/src/logging/log_format.h b/MIDAS/src/logging/log_format.h new file mode 100644 index 00000000..626d8990 --- /dev/null +++ b/MIDAS/src/logging/log_format.h @@ -0,0 +1,134 @@ +#pragma once + +#include "flight-systems/sensor_data.h" + +#define LOG_FMT_VERSION 1 + +/** + * @enum ReadingDiscriminant + * + * @brief Unique identifier assigned to each logged sensor or subsystem. + * + * These discriminants are written before every logged reading so the log parser + * can determine which data structure follows in the binary log. The value `0` + * is intentionally left unused to make invalid or corrupted records easier to + * detect during parsing. + * + * @note + * `COUNT` is not a valid discriminant. It represents the number of defined + * discriminants and is primarily used for iteration and HIL simulation. + */ +enum ReadingDiscriminant { + ID_IMU = 1, + ID_BAROMETER = 2, + ID_VOLTAGE = 4, + ID_GPS = 5, + ID_MAGNETOMETER = 6, + ID_KALMAN = 8, + ID_FSM = 9, + ID_PYRO = 10, + ID_CAMERADATA = 11, + ID_ANGULARKALMAN = 12, + ID_SFLP = 13, + COUNT = 14, +}; + +/** + * @brief Total number of valid reading discriminants. + * + * This compile-time constant is primarily used by HIL simulation and other + * code that needs to iterate through every supported log record type. DO NOT BREAK THIS UP INTO 2 LINES + */ +constexpr uint8_t READING_DISC_COUNT = static_cast(ReadingDiscriminant::COUNT); + +/** + * @struct LoggedReading + * + * @brief Logical representation of a single binary log entry. + * + * A logged reading consists of: + * - A sensor/subsystem identifier (`ReadingDiscriminant`) + * - A timestamp in milliseconds + * - The associated sensor data + * + * This structure is provided primarily as documentation of the log format and + * for compile-time type information. + * + * @note + * The logger does **not** write this structure directly to storage. Instead it + * serializes each component individually: + * 1. Discriminant + * 2. Timestamp + * 3. Raw sensor data + * + * Writing fields separately avoids the padding that would otherwise be present + * due to the union, resulting in a compact binary log format. + */ +struct LoggedReading { + /// Identifies the type of sensor data stored in this entry. + ReadingDiscriminant discriminant; + + /// Timestamp of the reading in milliseconds. + uint32_t timestamp_ms; + + /** + * @brief Sensor data payload. + * + * Only one member is valid for any given log entry, as determined by + * the corresponding value in `discriminant`. + */ + union { + IMU imu; + IMU_SFLP sflp; + Barometer barometer; + Voltage voltage; + GPS gps; + Magnetometer magnetometer; + KalmanData kalman; + AngularKalmanData angular_kalman; + FSMData fsm; + PyroState pyro; + CameraData cameradata; + } data; +}; + +/** + * @brief Returns the log discriminant associated with a sensor data type. + * + * Template specializations provide a compile-time mapping between each sensor + * structure and its corresponding `ReadingDiscriminant`. This allows the logger + * to determine the correct record identifier without runtime lookups. + * + * The associations are also parsed by the metadata generation script + * (`log_enc.py`) to build the binary log schema used by log analysis tools. + * + * @tparam T Sensor or subsystem data type. + * + * @return Compile-time `ReadingDiscriminant` corresponding to `T`. + */ +template +constexpr ReadingDiscriminant get_discriminant(); + +/** + * @brief Defines a compile-time association between a data type and its + * corresponding log discriminant. + * + * Expands into a template specialization of `get_discriminant()`. + * + * @param ty Data type being associated. + * @param id ReadingDiscriminant value. + * @param field Corresponding union member name (used by metadata generation). + */ +#define ASSOCIATE(ty, id, field) template<> constexpr ReadingDiscriminant get_discriminant() { return ReadingDiscriminant::id;} + +ASSOCIATE(IMU, ID_IMU, imu) +ASSOCIATE(IMU_SFLP, ID_SFLP, sflp) +ASSOCIATE(Barometer, ID_BAROMETER, barometer) +ASSOCIATE(Voltage, ID_VOLTAGE, voltage) +ASSOCIATE(GPS, ID_GPS, gps) +ASSOCIATE(Magnetometer, ID_MAGNETOMETER, magnetometer) +ASSOCIATE(KalmanData, ID_KALMAN, kalman) +ASSOCIATE(AngularKalmanData, ID_ANGULARKALMAN, angular_kalman) +ASSOCIATE(FSMData, ID_FSM, fsm) +ASSOCIATE(PyroState, ID_PYRO, pyro) +ASSOCIATE(CameraData, ID_CAMERADATA, cameradata) \ No newline at end of file diff --git a/MIDAS/src/logging/log_format_AUTOGEN.h b/MIDAS/src/logging/log_format_AUTOGEN.h new file mode 100644 index 00000000..e69f4088 --- /dev/null +++ b/MIDAS/src/logging/log_format_AUTOGEN.h @@ -0,0 +1,175 @@ +// Auto-generated by log_enc.py -- do not edit +#pragma once + +#include + +enum LogFieldType : uint8_t { + LOG_TYPE_STRUCT = 0, + LOG_TYPE_FLOAT32 = 1, + LOG_TYPE_FLOAT64 = 2, + LOG_TYPE_INT8 = 3, + LOG_TYPE_UINT8 = 4, + LOG_TYPE_INT16 = 5, + LOG_TYPE_UINT16 = 6, + LOG_TYPE_INT32 = 7, + LOG_TYPE_UINT32 = 8, + LOG_TYPE_INT64 = 9, + LOG_TYPE_UINT64 = 10, + LOG_TYPE_BOOL = 11, + LOG_TYPE_ENUM = 12, + LOG_TYPE_ARRAY = 13, + LOG_TYPE_UNION = 14, +}; + +struct LogFormatMetaEntry { char identifier[32]; uint8_t type_id; uint16_t count; }; +struct LogDiscMapEntry { uint16_t discriminant; uint16_t entry_index; }; + +#define LOG_META_ENTRY_COUNT 91 +#define LOG_DISCMAP_COUNT 11 +#define EEPROM_META_ENTRY_COUNT 35 + +const LogFormatMetaEntry EEPROM_META_ENTRIES[EEPROM_META_ENTRY_COUNT] = { + {"MIDASEEPROM", LOG_TYPE_STRUCT, 8}, + {"checksum", LOG_TYPE_UINT32, 0}, + {"sd_file_num_last", LOG_TYPE_UINT16, 0}, + {"serial", LOG_TYPE_INT8, 0}, + {"frequency", LOG_TYPE_FLOAT32, 0}, + {"lsm6dsv320x_hg_xl_bias", LOG_TYPE_STRUCT, 3}, + {"ax", LOG_TYPE_FLOAT32, 0}, + {"ay", LOG_TYPE_FLOAT32, 0}, + {"az", LOG_TYPE_FLOAT32, 0}, + {"mmc5983ma_softiron_bias", LOG_TYPE_STRUCT, 3}, + {"mx", LOG_TYPE_FLOAT64, 0}, + {"my", LOG_TYPE_FLOAT64, 0}, + {"mz", LOG_TYPE_FLOAT64, 0}, + {"mmc5983ma_hardiron_bias", LOG_TYPE_STRUCT, 3}, + {"mx", LOG_TYPE_FLOAT64, 0}, + {"my", LOG_TYPE_FLOAT64, 0}, + {"mz", LOG_TYPE_FLOAT64, 0}, + {"fsm_config", LOG_TYPE_STRUCT, 4}, + {"thresholds", LOG_TYPE_STRUCT, 3}, + {"pyro_fire_t", LOG_TYPE_FLOAT32, 0}, + {"main_alt", LOG_TYPE_FLOAT32, 0}, + {"cruise_lockout_en", LOG_TYPE_BOOL, 0}, + {"pyro_actions", LOG_TYPE_ARRAY, 4}, + {"_", LOG_TYPE_STRUCT, 9}, + {"enable", LOG_TYPE_BOOL, 0}, + {"fsm_trigger", LOG_TYPE_ENUM, 0}, + {"max_tilt", LOG_TYPE_FLOAT32, 0}, + {"after_motor", LOG_TYPE_INT8, 0}, + {"launch_t_gt", LOG_TYPE_FLOAT32, 0}, + {"launch_t_lt", LOG_TYPE_FLOAT32, 0}, + {"vx_min", LOG_TYPE_FLOAT32, 0}, + {"vx_max", LOG_TYPE_FLOAT32, 0}, + {"delay", LOG_TYPE_FLOAT64, 0}, + {"version_num", LOG_TYPE_INT8, 0}, + {"crc32", LOG_TYPE_UINT32, 0}, +}; + +const LogFormatMetaEntry LOG_META_ENTRIES[LOG_META_ENTRY_COUNT] = { + {"LoggedReading", LOG_TYPE_STRUCT, 3}, + {"discriminant", LOG_TYPE_ENUM, 0}, + {"timestamp_ms", LOG_TYPE_UINT32, 0}, + {"data", LOG_TYPE_UNION, 11}, + {"imu", LOG_TYPE_STRUCT, 3}, + {"highg_acceleration", LOG_TYPE_STRUCT, 3}, + {"ax", LOG_TYPE_FLOAT32, 0}, + {"ay", LOG_TYPE_FLOAT32, 0}, + {"az", LOG_TYPE_FLOAT32, 0}, + {"lowg_acceleration", LOG_TYPE_STRUCT, 3}, + {"ax", LOG_TYPE_FLOAT32, 0}, + {"ay", LOG_TYPE_FLOAT32, 0}, + {"az", LOG_TYPE_FLOAT32, 0}, + {"angular_velocity", LOG_TYPE_STRUCT, 3}, + {"vx", LOG_TYPE_FLOAT32, 0}, + {"vy", LOG_TYPE_FLOAT32, 0}, + {"vz", LOG_TYPE_FLOAT32, 0}, + {"sflp", LOG_TYPE_STRUCT, 3}, + {"quaternion", LOG_TYPE_STRUCT, 4}, + {"w", LOG_TYPE_FLOAT32, 0}, + {"x", LOG_TYPE_FLOAT32, 0}, + {"y", LOG_TYPE_FLOAT32, 0}, + {"z", LOG_TYPE_FLOAT32, 0}, + {"gravity", LOG_TYPE_STRUCT, 3}, + {"ax", LOG_TYPE_FLOAT32, 0}, + {"ay", LOG_TYPE_FLOAT32, 0}, + {"az", LOG_TYPE_FLOAT32, 0}, + {"gyro_bias", LOG_TYPE_STRUCT, 3}, + {"vx", LOG_TYPE_FLOAT32, 0}, + {"vy", LOG_TYPE_FLOAT32, 0}, + {"vz", LOG_TYPE_FLOAT32, 0}, + {"barometer", LOG_TYPE_STRUCT, 3}, + {"temperature", LOG_TYPE_FLOAT32, 0}, + {"pressure", LOG_TYPE_UINT32, 0}, + {"altitude", LOG_TYPE_FLOAT32, 0}, + {"voltage", LOG_TYPE_STRUCT, 3}, + {"continuity", LOG_TYPE_ARRAY, 4}, + {"_", LOG_TYPE_FLOAT32, 0}, + {"v_Bat", LOG_TYPE_FLOAT32, 0}, + {"v_Pyro", LOG_TYPE_FLOAT32, 0}, + {"gps", LOG_TYPE_STRUCT, 7}, + {"latitude", LOG_TYPE_INT32, 0}, + {"longitude", LOG_TYPE_INT32, 0}, + {"altitude", LOG_TYPE_FLOAT32, 0}, + {"speed", LOG_TYPE_FLOAT32, 0}, + {"fix_type", LOG_TYPE_UINT16, 0}, + {"sats_in_view", LOG_TYPE_UINT16, 0}, + {"time", LOG_TYPE_UINT32, 0}, + {"magnetometer", LOG_TYPE_STRUCT, 3}, + {"mx", LOG_TYPE_FLOAT64, 0}, + {"my", LOG_TYPE_FLOAT64, 0}, + {"mz", LOG_TYPE_FLOAT64, 0}, + {"kalman", LOG_TYPE_STRUCT, 3}, + {"position", LOG_TYPE_STRUCT, 3}, + {"px", LOG_TYPE_FLOAT32, 0}, + {"py", LOG_TYPE_FLOAT32, 0}, + {"pz", LOG_TYPE_FLOAT32, 0}, + {"velocity", LOG_TYPE_STRUCT, 3}, + {"vx", LOG_TYPE_FLOAT32, 0}, + {"vy", LOG_TYPE_FLOAT32, 0}, + {"vz", LOG_TYPE_FLOAT32, 0}, + {"acceleration", LOG_TYPE_STRUCT, 3}, + {"ax", LOG_TYPE_FLOAT32, 0}, + {"ay", LOG_TYPE_FLOAT32, 0}, + {"az", LOG_TYPE_FLOAT32, 0}, + {"angular_kalman", LOG_TYPE_STRUCT, 8}, + {"quaternion", LOG_TYPE_STRUCT, 4}, + {"w", LOG_TYPE_FLOAT32, 0}, + {"x", LOG_TYPE_FLOAT32, 0}, + {"y", LOG_TYPE_FLOAT32, 0}, + {"z", LOG_TYPE_FLOAT32, 0}, + {"gyrobias", LOG_TYPE_ARRAY, 3}, + {"_", LOG_TYPE_FLOAT32, 0}, + {"sflp_tilt", LOG_TYPE_FLOAT64, 0}, + {"mq_tilt", LOG_TYPE_FLOAT64, 0}, + {"has_data", LOG_TYPE_BOOL, 0}, + {"yaw", LOG_TYPE_FLOAT32, 0}, + {"pitch", LOG_TYPE_FLOAT32, 0}, + {"roll", LOG_TYPE_FLOAT32, 0}, + {"fsm", LOG_TYPE_STRUCT, 2}, + {"state", LOG_TYPE_ENUM, 0}, + {"current_motor", LOG_TYPE_INT8, 0}, + {"pyro", LOG_TYPE_STRUCT, 3}, + {"is_global_armed", LOG_TYPE_BOOL, 0}, + {"channel_firing", LOG_TYPE_ARRAY, 4}, + {"_", LOG_TYPE_BOOL, 0}, + {"pyro_event_consumed", LOG_TYPE_ARRAY, 4}, + {"_", LOG_TYPE_BOOL, 0}, + {"cameradata", LOG_TYPE_STRUCT, 2}, + {"camera_state", LOG_TYPE_INT8, 0}, + {"camera_voltage", LOG_TYPE_FLOAT32, 0}, +}; + +const LogDiscMapEntry LOG_DISCMAP_TABLE[LOG_DISCMAP_COUNT] = { + {1, 4}, + {2, 31}, + {4, 35}, + {5, 40}, + {6, 48}, + {8, 52}, + {9, 79}, + {10, 82}, + {11, 88}, + {12, 65}, + {13, 17}, +}; diff --git a/MIDAS/src/midas_shell.cpp b/MIDAS/src/midas_shell.cpp deleted file mode 100644 index 46a20680..00000000 --- a/MIDAS/src/midas_shell.cpp +++ /dev/null @@ -1,25 +0,0 @@ -#include - -MShell m_shell_inst; - -MCommandExecutionResult _c_echo(const MShellContext& ctx) { - if(ctx.argc != 2) { return MCommandExecutionResult::ERR_INVAL_ARGC; } - char n = ctx.argv[1][0]; // First argument (argv 1) will be 0/1 for the echo status, discard all other chars. - if(n == '1') { m_shell_inst.settings.echo = true; return MCommandExecutionResult::OK; } - if(n == '0') { m_shell_inst.settings.echo = false; return MCommandExecutionResult::OK; } - return MCommandExecutionResult::ERR_INVAL_ARGUMENT; -} - -MCommandExecutionResult _c_help(const MShellContext& ctx) { - m_shell_inst.print_help(&Serial); - return MCommandExecutionResult::OK; -} - -void m_shell_setup() { - // Sets up internal shell commands for the global shell instance - m_shell_inst.register_command("echo", _c_echo, "\t\techo <0|1> - Enable/disable 'echo' during character input. (Default 1)"); - m_shell_inst.register_command("help", _c_help, "\t\thelp - Prints this menu"); -} - - - diff --git a/MIDAS/src/midas_shell.h b/MIDAS/src/midas_shell.h deleted file mode 100644 index ea87d54f..00000000 --- a/MIDAS/src/midas_shell.h +++ /dev/null @@ -1,119 +0,0 @@ -#pragma once -#include -#include -#include -#include - -// Midas serial shell - -enum class MCommandExecutionResult { - // Generic - OK = 0, // The command was successful - ERR_UNSPECIFIED = 1, // The execution returned an unspecified error - ERR_INVALID_CMD = 2, // The command given was not of a valid format - ERR_NO_CMD = 3, // The command given was not one that exists (but is of a valid format) - ERR_INVAL_ARGC = 4, // The amount of arguments is incorrect for the current function. - ERR_INVAL_ARGUMENT = 5, // One or more arguments is invalid (generic) - ERR_INVAL_ARG_RANGE = 6, // One or more arguments is out of range - ERR_INVAL_FSM = 7, // FSM commit failed - ERR_INVAL_EEPROM = 8, // EEPROM has garbage values - ERR_FS_FAIL_OPEN = 9, // A file or directory failed to open in the mFS - ERR_FORBIDDEN = 10 // Action is not allowed -}; - -struct MShellContext { - uint8_t argc; - const char** argv; - const void* sysarg = nullptr; // This will ALWAYS be RocketSystems*, but the type isn't available to us at the moment. -}; - -using MShellExecutor = MCommandExecutionResult(*)(const MShellContext&); - -struct MShellCommand { - const char* cmd; - MShellExecutor fn; - const char* help_text; -}; - -struct MShellSettings { - bool echo = true; // Determines if on character entry, the shell returns the character entered (for visibility for human operators) -}; - - -class MShell { - - public: - static constexpr uint8_t max_commands = 16; - static constexpr uint8_t max_args = 16; - static constexpr uint8_t max_arg_length = 32; - static constexpr uint8_t max_line_len = 255; - - MShellSettings settings; - - void register_command(const char* name, MShellExecutor fn, const char* help = "") { - if (cmd_count_ >= max_commands) { - return; - } - - commands_[cmd_count_++] = { name, fn, help }; - } - - MCommandExecutionResult execute_line(const char* line, const void* with_sysarg) { - char* argv_ptrs[max_args]; - uint8_t argc_ctx = 1; - char l_cpy[max_line_len]; - strncpy(l_cpy, line, max_line_len - 1); - l_cpy[max_line_len - 1] = '\0'; - - char* cmd = strtok(l_cpy, " "); - if(cmd == NULL) { return MCommandExecutionResult::ERR_INVALID_CMD; } - argv_ptrs[0] = cmd; - - while(true) { - char* arg_ptr = strtok(NULL, " "); - if(arg_ptr == NULL) { break; } - if(argc_ctx >= max_args) { break; } - - argv_ptrs[argc_ctx++] = arg_ptr; - } - - MShellContext ctx = {argc_ctx, const_cast(argv_ptrs), with_sysarg}; - int cmd_index = cmd_idx(cmd); - - if(cmd_index == -1) { - // Command does not exist.. - return MCommandExecutionResult::ERR_NO_CMD; - } - - MShellCommand& m_command = commands_.at(cmd_index); - return m_command.fn(ctx); - } - - void print_help(Stream* out) { - for(int i = 0; i < cmd_count_; i++) { - out->print(commands_[i].cmd); - out->print(": "); - out->println(commands_[i].help_text); - } - } - - private: - // Returns a command's index in commands_ - int cmd_idx(const char* cmd_name) { - for(int i = 0; i < cmd_count_; i++) { - MShellCommand c = commands_.at(i); - if(strcmp(c.cmd, cmd_name) == 0) { - return i; - } - } - return -1; - } - - uint8_t cmd_count_; - std::array commands_{}; -}; - -// Sets up the MIDAS shell -void m_shell_setup(); -extern MShell m_shell_inst; -void m_shell_init_commands(MShell* sh); \ No newline at end of file diff --git a/MIDAS/src/shell/midas_shell.cpp b/MIDAS/src/shell/midas_shell.cpp new file mode 100644 index 00000000..8d33a028 --- /dev/null +++ b/MIDAS/src/shell/midas_shell.cpp @@ -0,0 +1,52 @@ +#include "midas_shell.h" + +/** + * @brief Global instantiation of the MIDAS system command line engine. + * @details Handles the operational parsing arrays, settings configuration, + * and callback routing for all terminal interactions. + */ +MShell m_shell_inst; + +/** + * @brief Terminal callback handle modifying console input visibility. + * @details Toggles the character echo property on or off based on the parsed string arguments. + * @param ctx Shell context containing target setting parameters. + * argv[1][0]: character '1' to enable echo, '0' to disable it. + * @return MCommandExecutionResult Execution status code. + */ +MCommandExecutionResult _c_echo(const MShellContext& ctx) { + // Validate argument count: expects exactly 1 argument after the command keyword + if(ctx.argc != 2) { return MCommandExecutionResult::ERR_INVAL_ARGC; } + + // Isolate the first character of the secondary parameter array element + char n = ctx.argv[1][0]; // First argument (argv 1) will be 0/1 for the echo status, discard all other chars. + + // Evaluate operational switch criteria and assign the internal setting + if(n == '1') { m_shell_inst.settings.echo = true; return MCommandExecutionResult::OK; } + if(n == '0') { m_shell_inst.settings.echo = false; return MCommandExecutionResult::OK; } + + // Return error if input configuration state is unrecognized + return MCommandExecutionResult::ERR_INVAL_ARGUMENT; +} + +/** + * @brief Terminal callback to output the complete menu of registered shell options. + * @details Flushes the text descriptions and helper documentation strings out to the system Serial line. + * @param ctx Shell execution parameter context (unused). + * @return MCommandExecutionResult Execution status code. + */ +MCommandExecutionResult _c_help(const MShellContext& ctx) { + // Pipe all registered command mappings and help strings to the default Serial output hardware stream + m_shell_inst.print_help(&Serial); + return MCommandExecutionResult::OK; +} + +void m_shell_setup() { + // Sets up internal shell commands for the global shell instance + + // Map the built-in 'echo' terminal toggle option to its logic handler + m_shell_inst.register_command("echo", _c_echo, "\t\techo <0|1> - Enable/disable 'echo' during character input. (Default 1)"); + + // Map the standard 'help' index printing routing sequence + m_shell_inst.register_command("help", _c_help, "\t\thelp - Prints this menu"); +} \ No newline at end of file diff --git a/MIDAS/src/shell/midas_shell.h b/MIDAS/src/shell/midas_shell.h new file mode 100644 index 00000000..d93af462 --- /dev/null +++ b/MIDAS/src/shell/midas_shell.h @@ -0,0 +1,166 @@ +#pragma once +#include +#include +#include +#include + +/** + * @brief Return codes indicating the final execution status of an interactive shell command. + */ +enum class MCommandExecutionResult { + // Generic + OK = 0, /**< The command executed successfully without errors */ + ERR_UNSPECIFIED = 1, /**< Execution failed due to an unhandled or general system error */ + ERR_INVALID_CMD = 2, /**< The command string format or structure could not be parsed */ + ERR_NO_CMD = 3, /**< The parsed command keyword does not match any registered handler */ + ERR_INVAL_ARGC = 4, /**< The number of arguments provided does not match the command signature */ + ERR_INVAL_ARGUMENT = 5, /**< One or more provided arguments failed generic validation checks */ + ERR_INVAL_ARG_RANGE = 6, /**< One or more numeric parameters fall outside acceptable functional limits */ + ERR_INVAL_FSM = 7, /**< State transition profile commitment validation failed */ + ERR_INVAL_EEPROM = 8, /**< Non-volatile memory storage data is corrupted or contains garbage values */ + ERR_FS_FAIL_OPEN = 9, /**< File or directory target could not be loaded inside the local filesystem */ + ERR_FORBIDDEN = 10 /**< Requested operational sequence is locked or blocked due to current flight status */ +}; + +/** + * @brief Parameter envelope containing argument references and external system contexts passed into command functions. + */ +struct MShellContext { + uint8_t argc; /**< Total tokenized argument count including the command keyword identifier itself */ + const char** argv; /**< Array of null-terminated C-string parameter strings parsed from the command line */ + const void* sysarg = nullptr; /**< Runtime core system state reference object pointer (Opaque handle to RocketSystems*) */ +}; + +/** + * @brief Function pointer signature definitions corresponding to valid command executor callbacks. + */ +using MShellExecutor = MCommandExecutionResult(*)(const MShellContext&); + +/** + * @brief Storage structure linking an input command invocation keyword to its designated callback logic and usage menus. + */ +struct MShellCommand { + const char* cmd; /**< Unique string token keyword identifying the active command trigger */ + MShellExecutor fn; /**< Target function pointer executed upon a successful identifier keyword match */ + const char* help_text; /**< Human-readable informational text displaying syntax requirements and usage instructions */ +}; + +/** + * @brief Functional run configuration properties adjusting the local terminal console interface behavior. + */ +struct MShellSettings { + bool echo = true; /**< Controls if raw characters are sent back over standard output streams during input typing */ +}; + + +/** + * @brief Lightweight, non-allocating interactive shell terminal parsing engine for embedded rocket systems. + */ +class MShell { + + public: + static constexpr uint8_t max_commands = 16; /**< Limit defining the total amount of unique terminal commands allowed */ + static constexpr uint8_t max_args = 16; /**< Maximum separate sub-argument tokens supported within one command line string */ + static constexpr uint8_t max_arg_length = 32; /**< Arbitrary individual string dimension bounds constraint metric */ + static constexpr uint8_t max_line_len = 255; /**< Maximum text length boundaries processing continuous character buffers */ + + MShellSettings settings; /**< Active internal visual display configuration state values */ + + /** + * @brief Maps a terminal string keyword onto a distinct program execution routing path. + * @param name Unique trigger text string tracking incoming commands. + * @param fn Executable function callback mapping system parameters. + * @param help Informational text mapping string summaries detailing syntax targets. + */ + void register_command(const char* name, MShellExecutor fn, const char* help = "") { + if (cmd_count_ >= max_commands) { + return; + } + + commands_[cmd_count_++] = { name, fn, help }; + } + + /** + * @brief Tokenizes, extracts parameters, evaluates, and dispatches a complete raw input command sequence. + * @param line Raw input string array collected across communications pathways. + * @param with_sysarg Target runtime systems architecture configuration object references. + * @return MCommandExecutionResult Functional status detailing matching implementation responses. + */ + MCommandExecutionResult execute_line(const char* line, const void* with_sysarg) { + char* argv_ptrs[max_args]; + uint8_t argc_ctx = 1; + char l_cpy[max_line_len]; + strncpy(l_cpy, line, max_line_len - 1); + l_cpy[max_line_len - 1] = '\0'; + + char* cmd = strtok(l_cpy, " "); + if(cmd == NULL) { return MCommandExecutionResult::ERR_INVALID_CMD; } + argv_ptrs[0] = cmd; + + while(true) { + char* arg_ptr = strtok(NULL, " "); + if(arg_ptr == NULL) { break; } + if(argc_ctx >= max_args) { break; } + + argv_ptrs[argc_ctx++] = arg_ptr; + } + + MShellContext ctx = {argc_ctx, const_cast(argv_ptrs), with_sysarg}; + int cmd_index = cmd_idx(cmd); + + if(cmd_index == -1) { + // Command does not exist.. + return MCommandExecutionResult::ERR_NO_CMD; + } + + MShellCommand& m_command = commands_.at(cmd_index); + return m_command.fn(ctx); + } + + /** + * @brief Loops across registered structures to display a catalog of commands and instruction summaries. + * @param out Target output destination interface stream. + */ + void print_help(Stream* out) { + for(int i = 0; i < cmd_count_; i++) { + out->print(commands_[i].cmd); + out->print(": "); + out->println(commands_[i].help_text); + } + } + + private: + /** + * @brief Internal scan utility returning sequential positional database vectors for target terminal paths. + * @param cmd_name Query string keyword identifier under validation evaluation. + * @return int Array index of matching command definition entries, or -1 if no tracking register matches. + */ + int cmd_idx(const char* cmd_name) { + for(int i = 0; i < cmd_count_; i++) { + MShellCommand c = commands_.at(i); + if(strcmp(c.cmd, cmd_name) == 0) { + return i; + } + } + return -1; + } + + uint8_t cmd_count_; /**< Running tally tracking currently mapped functional definitions */ + std::array commands_{}; /**< Fixed-size buffer array managing active instruction handles */ +}; + +/** + * @brief Top-level system lifecycle setup bootstrapping underlying operational parameters. + */ +void m_shell_setup(); + +/** + * @brief Global instantiation reference managing command context handling globally. + */ +extern MShell m_shell_inst; + +/** + * @brief External functional callback parsing registration sequences to inject commands. + * @param sh Target engine tracking runtime configuration allocations. + */ +void m_shell_init_commands(MShell* sh); \ No newline at end of file diff --git a/MIDAS/src/midas_shell_commands.h b/MIDAS/src/shell/midas_shell_commands.h similarity index 79% rename from MIDAS/src/midas_shell_commands.h rename to MIDAS/src/shell/midas_shell_commands.h index bf9aca01..821ce2c2 100644 --- a/MIDAS/src/midas_shell_commands.h +++ b/MIDAS/src/shell/midas_shell_commands.h @@ -1,29 +1,49 @@ -#include +#include #include #include -#include -#include "log_checksum.h" -#include "esp_eeprom_checksum.h" - +#include +#include "logging/log_checksum.h" +#include "logging/esp_eeprom_checksum.h" + +/** + * @brief Global shell configuration structure holding the staged/queued + * Finite State Machine (FSM) configuration parameters. + */ FSMConfiguration shell_cfg; +/** + * @brief Supported data types for flight software parameters mapped in the command shell. + */ enum class DataType { - FLOAT, DOUBLE, BOOL, UINT8, FSMSTATE + FLOAT, /**< 32-bit floating point value */ + DOUBLE, /**< 64-bit floating point value */ + BOOL, /**< Boolean flag (true/false) */ + UINT8, /**< 8-bit unsigned integer */ + FSMSTATE /**< Enumerated Finite State Machine state */ }; +/** + * @brief Entry format for mapping string identifiers to internal structure fields via memory offsets. + */ struct MapEntry { - const char* name; - size_t offset; - DataType type; - const char* unit; + const char* name; /**< String command identifier passed via the shell */ + size_t offset; /**< Memory offset of the field within its parent structure */ + DataType type; /**< Underlying data type of the target field */ + const char* unit; /**< Human-readable unit symbol (e.g., "ms", "m", "degrees") */ }; +/** + * @brief Lookup map linking global FSM system thresholds to their memory offsets and data types. + */ static constexpr MapEntry threshold_map[] = { {"PYRO_FIRE_T", offsetof(FSMUserThresholds, pyro_fire_t), DataType::FLOAT, "ms"}, {"MAIN_ALT", offsetof(FSMUserThresholds, main_alt), DataType::FLOAT, "m"}, {"CRUISE_LOCKOUT_EN", offsetof(FSMUserThresholds, cruise_lockout_en), DataType::BOOL, ""}, }; +/** + * @brief Lookup map linking pyrotechnic channel configuration fields to their memory offsets and data types. + */ static constexpr MapEntry channel_map[] = { {"ENABLE", offsetof(FSMPyroAction, enable), DataType::BOOL, ""}, {"FSM_TRIGGER", offsetof(FSMPyroAction, fsm_trigger), DataType::FSMSTATE, ""}, @@ -36,6 +56,9 @@ static constexpr MapEntry channel_map[] = { {"VX_MAX", offsetof(FSMPyroAction, vx_max), DataType::FLOAT, "m/s"}, }; +/** + * @brief String representations of the flight FSM states for telemetry reporting and shell outputs. + */ static constexpr const char * state_names[] = { "SAFE", "PYRO_TEST", @@ -47,10 +70,17 @@ static constexpr const char * state_names[] = { "LANDED", }; +/** + * @brief Valid flight states authorized to safely trigger pyrotechnic deployment events. + */ static constexpr FSMState allowed_trigger_states[] = { FSMState::STATE_COAST, FSMState::STATE_DROGUE, FSMState::STATE_MAIN }; +/** + * @brief Helper utility to print an 8-bit board serial identifier with zero-padded 3-digit formatting over Serial. + * @param serial The raw 8-bit unsigned integer serial number to format and output. + */ void print_serial(uint8_t serial){ uint8_t str [3]; if (serial < 10){ @@ -66,6 +96,13 @@ void print_serial(uint8_t serial){ Serial.println(serial); } +/** + * @brief Manual override command to force alter the active flight state machine execution profile. + * @param ctx The current shell context parsed containing arguments. + * argv[1]: target state integer ID + * argv[2] (Optional): specific target motor index + * @return MCommandExecutionResult Execution status code. + */ MCommandExecutionResult set_fsm(const MShellContext& ctx) { // fix this command cause it's stupid RocketSystems* arg = (RocketSystems*) ctx.sysarg; @@ -83,6 +120,11 @@ MCommandExecutionResult set_fsm(const MShellContext& ctx) { return MCommandExecutionResult::OK; } +/** + * @brief Simple handshake shell command to echo an input string for debugging communication lines. + * @param ctx Shell context containing potential arguments to echo back. + * @return MCommandExecutionResult Execution status code. + */ MCommandExecutionResult hi_midas(const MShellContext& ctx) { Serial.print("hi "); // if no second argument, return @@ -91,6 +133,11 @@ MCommandExecutionResult hi_midas(const MShellContext& ctx) { return MCommandExecutionResult::OK; } +/** + * @brief Shell command interface to retrieve or rewrite the system serial identifier in non-volatile storage. + * @param ctx Shell context. Usage: `serial get` or `serial set ` + * @return MCommandExecutionResult Execution status code. + */ MCommandExecutionResult serial(const MShellContext& ctx){ RocketSystems* arg = (RocketSystems*) ctx.sysarg; // expecting 2 (get) or 3 (set) arguments @@ -114,6 +161,12 @@ MCommandExecutionResult serial(const MShellContext& ctx){ } } +/** + * @brief Shell command interface to fetch or modify the active 70cm radio telemetry band center frequency. + * @details Limits operational input to local safety brackets (420.0 MHz - 450.0 MHz) prior to updating hardware. + * @param ctx Shell context. Usage: `frequency get` or `frequency set ` + * @return MCommandExecutionResult Execution status code. + */ MCommandExecutionResult frequency(const MShellContext& ctx){ RocketSystems* arg = (RocketSystems*) ctx.sysarg; // expecting 2 (get) or 3 (set) arguments @@ -147,6 +200,11 @@ MCommandExecutionResult frequency(const MShellContext& ctx){ } } +/** + * @brief Shell command to examine active configuration variants or stage a new software version baseline. + * @param ctx Shell context. Usage: `fsm version` or `fsm version ` + * @return MCommandExecutionResult Execution status code. + */ MCommandExecutionResult fsm_version(const MShellContext& ctx){ RocketSystems* arg = (RocketSystems*) ctx.sysarg; // expecting 2 (get) or 3 (set) arguments @@ -164,6 +222,11 @@ MCommandExecutionResult fsm_version(const MShellContext& ctx){ return MCommandExecutionResult::ERR_INVAL_ARGUMENT; } +/** + * @brief Computes a validation checksum validation hash over the queued temporary shell configuration buffer. + * @param ctx Shell context. Usage: `fsm calculate` + * @return MCommandExecutionResult Execution status code. + */ MCommandExecutionResult fsm_calculate(const MShellContext& ctx){ RocketSystems* arg = (RocketSystems*) ctx.sysarg; // Expecting just "fsm calculate" @@ -173,6 +236,11 @@ MCommandExecutionResult fsm_calculate(const MShellContext& ctx){ return MCommandExecutionResult::OK; } +/** + * @brief Handles reading and writing individual parameters located inside the global flight safety limits block. + * @param ctx Shell context containing arguments for reading/writing global bounds structures. + * @return MCommandExecutionResult Execution status code. + */ MCommandExecutionResult fsm_threshold(const MShellContext& ctx){ if (ctx.argc>4 || ctx.argc<3) { return MCommandExecutionResult::ERR_INVAL_ARGC;} RocketSystems* arg = (RocketSystems*) ctx.sysarg; @@ -253,6 +321,12 @@ MCommandExecutionResult fsm_threshold(const MShellContext& ctx){ return MCommandExecutionResult::ERR_INVAL_ARGUMENT; } +/** + * @brief Modifies or queries operational profiles assigned to individual physical output pyro switches. + * @param ctx Shell context containing programmatic configuration elements. + * @param ch The targeted hardware pyro index identifier (0=A, 1=B, 2=C, 3=D). + * @return MCommandExecutionResult Execution status code. + */ MCommandExecutionResult fsm_channel(const MShellContext& ctx, uint8_t ch){ if (ctx.argc>4 || ctx.argc<3) { return MCommandExecutionResult::ERR_INVAL_ARGC;} RocketSystems* arg = (RocketSystems*) ctx.sysarg; @@ -381,6 +455,12 @@ MCommandExecutionResult fsm_channel(const MShellContext& ctx, uint8_t ch){ return MCommandExecutionResult::ERR_INVAL_ARGUMENT; } +/** + * @brief Validates, flushes, and commits the staged configuration data into non-volatile EEPROM storage. + * @details Resets internal FSM health condition structures and status indicator lights on successful verification. + * @param ctx Shell context containing target integrity CRC hashes for security validation. + * @return MCommandExecutionResult Execution status code. + */ MCommandExecutionResult fsm_commit(const MShellContext& ctx){ if (ctx.argc != 3) {return MCommandExecutionResult::ERR_INVAL_ARGC;} RocketSystems* arg = (RocketSystems*) ctx.sysarg; @@ -401,6 +481,10 @@ MCommandExecutionResult fsm_commit(const MShellContext& ctx){ return MCommandExecutionResult::ERR_INVAL_FSM; } +/** + * @brief Outputs structural descriptions and syntax examples for all FSM terminal commands over Serial. + * @return MCommandExecutionResult Execution status code. + */ MCommandExecutionResult fsm_help(){ Serial.println("FSM Configuration Help:\n"); @@ -429,6 +513,11 @@ MCommandExecutionResult fsm_help(){ return MCommandExecutionResult::OK; } +/** + * @brief Router terminal handle processing sub-demands targeted toward flight profiling rules. + * @param ctx Shell context containing arguments. + * @return MCommandExecutionResult Execution status code. + */ MCommandExecutionResult fsm(const MShellContext& ctx){ if (ctx.argc<2) {return MCommandExecutionResult::ERR_INVAL_ARGC;} RocketSystems* arg = (RocketSystems*) ctx.sysarg; @@ -466,10 +555,19 @@ MCommandExecutionResult fsm(const MShellContext& ctx){ return MCommandExecutionResult::ERR_INVAL_ARGUMENT; } +/** + * @brief Initializes the localized working shell buffer configuration image from runtime configurations. + * @param fsm_cfg Reference source configuration containing initialization state values. + */ void m_shell_load_fsm_config(const FSMConfiguration& fsm_cfg) { shell_cfg = fsm_cfg; } +/** + * @brief Shell command to query and structure file catalog details currently visible within storage card partitions. + * @param ctx Shell context. Usage: `ls` + * @return MCommandExecutionResult Execution status code. + */ MCommandExecutionResult cmd_ls(const MShellContext& ctx) { if(ctx.argc != 1) { return MCommandExecutionResult::ERR_INVAL_ARGC; } File root = SD_MMC.open("/"); @@ -504,6 +602,11 @@ MCommandExecutionResult cmd_ls(const MShellContext& ctx) { return MCommandExecutionResult::OK; } +/** + * @brief Streaming reader outputting full raw contents of an explicit file asset target directly to serial. + * @param ctx Shell context containing targeted local string file paths. Usage: `read ` + * @return MCommandExecutionResult Execution status code. + */ MCommandExecutionResult cmd_read(const MShellContext& ctx) { if (ctx.argc != 2) { return MCommandExecutionResult::ERR_INVAL_ARGC; } @@ -522,8 +625,12 @@ MCommandExecutionResult cmd_read(const MShellContext& ctx) { return MCommandExecutionResult::OK; } -// Returns true if the path refers to an active logfile. Compares basenames so -// any number of leading slashes is tolerated. +/** + * @brief Helper evaluation block verifying if a given directory file targets the live logging output metrics stream. + * @param arg Active flight vehicle operational class instances. + * @param path Extracted folder file lookup strings under comparative processing checks. + * @return true If the file matches active logs; false otherwise. + */ static bool is_active_log(RocketSystems* arg, const char* path) { const char* basename = strrchr(path, '/'); basename = basename ? basename + 1 : path; @@ -535,6 +642,12 @@ static bool is_active_log(RocketSystems* arg, const char* path) { return !strcmp(basename, bin) || !strcmp(basename, meta); } +/** + * @brief Safety sweep utility targeting deletion tasks spanning passive log file fragments across media partitions. + * @details Retains and isolates the currently running data structure files to guard active recording sessions. + * @param arg Hardware telemetry core coordination state machine structures. + * @return true On clean execution sweeps; false if hardware blockades disrupt operations. + */ bool delete_all_flash(RocketSystems* arg) { File root = SD_MMC.open("/"); while (true) { @@ -561,6 +674,12 @@ bool delete_all_flash(RocketSystems* arg) { return true; } +/** + * @brief Packages and streams unified high-fidelity launch session artifacts down telemetry pipelines. + * @details Interleaves structure elements, system metadata tracking logs, and direct checksum validation. + * @param ctx Shell context container parameters. Usage: `lfd ` (e.g., `lfd data17`) + * @return MCommandExecutionResult Execution status code. + */ MCommandExecutionResult cmd_lfd(const MShellContext& ctx) { // Launch File Dump: Outputs a .launch file as binary data over serial @@ -645,11 +764,21 @@ MCommandExecutionResult cmd_lfd(const MShellContext& ctx) { return MCommandExecutionResult::OK; } +/** + * @brief Responds with the flight computer platform identifier signature over active standard serial out. + * @param ctx Shell context. Usage: `ident` + * @return MCommandExecutionResult Execution status code. + */ MCommandExecutionResult ident_midas(const MShellContext& ctx) { Serial.println("IDENT_RESPONSE:MIDAS_MINI"); return MCommandExecutionResult::OK; } +/** + * @brief Shell command executing removal routines handling isolated singular files or widespread safety flash wipes. + * @param ctx Shell context. Usage: `rm ` or `rm *` + * @return MCommandExecutionResult Execution status code. + */ MCommandExecutionResult cmd_rm(const MShellContext& ctx) { if (ctx.argc != 2) { return MCommandExecutionResult::ERR_INVAL_ARGC; } RocketSystems* arg = (RocketSystems*) ctx.sysarg; @@ -676,6 +805,11 @@ MCommandExecutionResult cmd_rm(const MShellContext& ctx) { return MCommandExecutionResult::OK; } +/** + * @brief Enters an automated runtime state loop to update sensor internal zero points. + * @param ctx Shell context designating specific hardware channels. Usage: `calibrate [xl/mag]` + * @return MCommandExecutionResult Execution status code. + */ MCommandExecutionResult m_calibration(const MShellContext& ctx) { if (ctx.argc != 2) { return MCommandExecutionResult::ERR_INVAL_ARGC; } RocketSystems* arg = (RocketSystems*) ctx.sysarg; @@ -692,6 +826,10 @@ MCommandExecutionResult m_calibration(const MShellContext& ctx) { return MCommandExecutionResult::ERR_INVAL_ARGUMENT; } +/** + * @brief Prints documentation tracking direct programmatic hardware component correction offsets. + * @return MCommandExecutionResult Execution status code. + */ MCommandExecutionResult calibset_help(){ Serial.println("CalibSet Help:\n"); @@ -710,6 +848,12 @@ MCommandExecutionResult calibset_help(){ return MCommandExecutionResult::OK; } +/** + * @brief Command handler interface allowing manual software injection of calculated sensor bias matrices. + * @details Commits modified hard-iron, soft-iron, or high-G baseline bias vectors safely into EEPROM memory structures. + * @param ctx Shell context containing programmatic configuration elements. + * @return MCommandExecutionResult Execution status code. + */ MCommandExecutionResult m_calibset(const MShellContext& ctx) { if (ctx.argc < 2) { return MCommandExecutionResult::ERR_INVAL_ARGC; } @@ -754,6 +898,12 @@ MCommandExecutionResult m_calibset(const MShellContext& ctx) { return MCommandExecutionResult::ERR_INVAL_ARGUMENT; } +/** + * @brief Shell interface wrapper to fetch or adjust the rolling log prefix counter tracker. + * @details Prevents log name collisions during power cycle resets by tracking historical file numbers. + * @param ctx Shell context. Usage: `sdfn` (get) or `sdfn ` (set) + * @return MCommandExecutionResult Execution status code. + */ MCommandExecutionResult m_sdfn(const MShellContext& ctx) { RocketSystems* arg = (RocketSystems*) ctx.sysarg; if(ctx.argc == 1) { @@ -770,6 +920,10 @@ MCommandExecutionResult m_sdfn(const MShellContext& ctx) { return MCommandExecutionResult::ERR_INVAL_ARGC; } +/** + * @brief Main execution command registry mapping terminal text command tokens to software execution handles. + * @param sh Target shell processing structure passing operational registers. + */ void m_shell_init_commands(MShell* sh) { // sh->register_command("setfsm", set_fsm, "\tsetfsm - Sets the FSM state to state "); sh->register_command("hi", hi_midas, "\t\thi - Prints hi "); @@ -790,4 +944,4 @@ void m_shell_init_commands(MShell* sh) { //Identify sh->register_command("ident", ident_midas, "\tIdentify MIDAS"); -} +} \ No newline at end of file diff --git a/MIDAS/src/silsim/FileSink.h b/MIDAS/src/silsim/FileSink.h index 127b4acd..0da6c0bd 100644 --- a/MIDAS/src/silsim/FileSink.h +++ b/MIDAS/src/silsim/FileSink.h @@ -1,9 +1,9 @@ #pragma once -#include "errors.h" +#include #include #include -#include "data_logging.h" +#include "logging/data_logging.h" struct SDSink : public LogSink { explicit SDSink(const char* file_name); diff --git a/MIDAS/src/silsim/emulated_sensors.h b/MIDAS/src/silsim/emulated_sensors.h index c43d012b..f81a68aa 100644 --- a/MIDAS/src/silsim/emulated_sensors.h +++ b/MIDAS/src/silsim/emulated_sensors.h @@ -1,7 +1,7 @@ #pragma once -#include "errors.h" -#include "sensor_data.h" +#include +#include "flight-systems/sensor_data.h" #include "silsim/simulation/simulation.h" diff --git a/MIDAS/src/silsim/emulated_telemetry.h b/MIDAS/src/silsim/emulated_telemetry.h index 83452299..edb05bb6 100644 --- a/MIDAS/src/silsim/emulated_telemetry.h +++ b/MIDAS/src/silsim/emulated_telemetry.h @@ -2,8 +2,8 @@ #include -#include "errors.h" -#include "hal.h" +#include +#include "util/hal.h" class TelemetryBackend { diff --git a/MIDAS/src/silsim/main.cpp b/MIDAS/src/silsim/main.cpp index 36913512..edf5a135 100644 --- a/MIDAS/src/silsim/main.cpp +++ b/MIDAS/src/silsim/main.cpp @@ -1,5 +1,5 @@ //#include "silsim/emulation.h" -#include "systems.h" +#include "flight-systems/systems.h" #include "FileSink.h" void rocket_main_thread(RocketSystems* systems) { diff --git a/MIDAS/src/telemetry.h b/MIDAS/src/telemetry.h deleted file mode 100644 index 1a97f586..00000000 --- a/MIDAS/src/telemetry.h +++ /dev/null @@ -1,39 +0,0 @@ -#pragma once - -#include "telemetry_packet.h" -#include "rocket_state.h" -#include "errors.h" -#include "led.h" -#include "esp_eeprom.h" - -#if defined(SILSIM) -#include "silsim/emulated_telemetry.h" -#elif defined(HILSIM) -#include "hilsim/sensors/telemetry_backend.h" -#else -#include "hardware/telemetry_backend.h" -#endif - -/** - * @class Telemetry - * - * @brief wraps the telemetry system to create and send a packet -*/ -class Telemetry { -public: - Telemetry() = default; - explicit Telemetry(TelemetryBackend&& backend); - - ErrorCode __attribute__((warn_unused_result)) init(); - - void transmit(RocketData& rocket_data, const MIDASEEPROM& eeprom, LEDController& led); - bool receive(TelemetryCommand* command, int wait_milliseconds); - void acknowledgeReceived(); - ErrorCode setFrequency(float frequency); - void set_spi_mutex(SemaphoreHandle_t mtx) { backend.set_spi_mutex(mtx); } -private: - int received_count; - TelemetryPacket makePacket(RocketData& data, const MIDASEEPROM& eeprom); - - TelemetryBackend backend; -}; diff --git a/MIDAS/src/telemetry.cpp b/MIDAS/src/telemetry/telemetry.cpp similarity index 76% rename from MIDAS/src/telemetry.cpp rename to MIDAS/src/telemetry/telemetry.cpp index a0aed8c2..ccedae0b 100644 --- a/MIDAS/src/telemetry.cpp +++ b/MIDAS/src/telemetry/telemetry.cpp @@ -1,7 +1,14 @@ #include -#include "telemetry.h" +#include "telemetry/telemetry.h" +/** + * @brief Maps an integer value linearly from one range to another range. + * * @param x input value + * @param in_min, in_max lower and upper bounds of the input range + * @param out_min, out_max lower and upper bounds of the output range + * @return long The scaled value in the target output range. + */ inline long map(long x, long in_min, long in_max, long out_min, long out_max) { return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; } @@ -11,7 +18,7 @@ inline long map(long x, long in_min, long in_max, long out_min, long out_max) { * @brief This function maps an input value onto within a particular range into a fixed point value of a certin binary * size * - * @param val: number to map into target range, values outside of the range will be clamped + * @param val: number to map into target range, values outside the range will be clamped * * @param range: range to map number into. For unsigned output, [0, range). For signed output [-range/2, range) * @@ -19,18 +26,19 @@ inline long map(long x, long in_min, long in_max, long out_min, long out_max) { */ template T inv_convert_range(float val, float range) { + // Determine the full dynamic range of the target integral type size_t numeric_range = (int64_t)std::numeric_limits::max() - (int64_t)std::numeric_limits::min() + 1; + // Perform scaling operation relative to the expected maximum physical range float converted = val * (float)numeric_range / range; + // Clamp the final result to prevent out-of-bounds overflow before casting return std::max(std::min((float)std::numeric_limits::max(), converted), (float)std::numeric_limits::min()); } /** * @brief packs highg and tilt infomation into 3, 2 byte integers - * - * @param highg highg data to store - * @param tilt tilt information to store - * - * @return tuple with packed data + * * @param highg data to store + * @param tilt information to store + * * @return tuple with packed data */ // std::tuple pack_highg_tilt(HighGData const& highg, uint16_t tilt) { //update parameter and function @@ -53,31 +61,45 @@ Telemetry::Telemetry(TelemetryBackend&& backend) : backend(std::move(backend)) { /** * @brief transmit telemetry data through LoRa - * - * @param rocket_data rocket_data to transmit + * * @param rocket_data rocket_data to transmit * @param led led state to transmit */ void Telemetry::transmit(RocketData& rocket_data, const MIDASEEPROM& eeprom, LEDController& led) { // static_assert(sizeof(TelemetryPacket) == 20); + // Build the packed structured data frame TelemetryPacket packet = makePacket(rocket_data, eeprom); + // Provide visual indication of data frame transmission led.toggle(LED::BLUE); + // Hand over the payload to the specific RF transceiver driver backend.send(packet); } +/** + * @brief Checks for and receives incoming telemetry commands within a specified timeout. + * * @param command Pointer to the command structure where received data will be stored. + * @param wait_milliseconds The maximum time to block waiting for a packet, in milliseconds. + * @return true If a command was successfully received. + * @return false If the timeout expired or an error occurred. + */ bool Telemetry::receive(TelemetryCommand* command, int wait_milliseconds) { + // Query backend receiver and return structural status return backend.read(command, wait_milliseconds); } +/** + * @brief Sends an acknowledgment packet back to the ground station for a received command. + */ void Telemetry::acknowledgeReceived() { + // Increment tracking counter for total commands acknowledged received_count ++; } /** * @brief Sets new frequency for the LoRa module * - * @param freq New frequency to set the LoRa module to (MHz) + * @param freq New frequency in MHz */ ErrorCode Telemetry::setFrequency(float freq){ return backend.setFrequency(freq); @@ -85,12 +107,12 @@ ErrorCode Telemetry::setFrequency(float freq){ /** * @brief creates the packet to send through the telemetry system - * - * @param data the data to serialize into a packet -*/ + * * @param data the data to serialize into a packet + */ TelemetryPacket Telemetry::makePacket(RocketData& data, const MIDASEEPROM& eeprom) { TelemetryPacket packet { }; + // Retrieve un-synchronized snapshots of all sub-system data variables safely IMU imu = data.imu.getRecentUnsync(); IMU_SFLP sflp_data = data.sflp.getRecentUnsync(); GPS gps = data.gps.getRecentUnsync(); @@ -102,6 +124,7 @@ TelemetryPacket Telemetry::makePacket(RocketData& data, const MIDASEEPROM& eepro AngularKalmanData angular_kalman = data.angular_kalman_data.getRecentUnsync(); CameraData cam_data = data.cam_data.getRecentUnsync(); + // Map positional data coordinates directly to packet fields packet.lat = gps.latitude; packet.lon = gps.longitude; @@ -158,6 +181,7 @@ TelemetryPacket Telemetry::makePacket(RocketData& data, const MIDASEEPROM& eepro packet.gpsfix_satcount |= (gps.fix_type & 0x07); packet.gpsfix_satcount |= (gps.sats_in_view & 0x1F) << 3; + // Assign uniquely identifying hardware information from non-volatile storage packet.serial = eeprom.serial; @@ -169,8 +193,7 @@ TelemetryPacket Telemetry::makePacket(RocketData& data, const MIDASEEPROM& eepro /** * @brief initializes the Telemetry system - * - * @return Error Code + * * @return Error Code */ ErrorCode __attribute__((warn_unused_result)) Telemetry::init() { return backend.init(); diff --git a/MIDAS/src/telemetry/telemetry.h b/MIDAS/src/telemetry/telemetry.h new file mode 100644 index 00000000..e7cd10ed --- /dev/null +++ b/MIDAS/src/telemetry/telemetry.h @@ -0,0 +1,92 @@ +#pragma once + +#include "telemetry_packet.h" +#include "flight-systems/rocket_state.h" +#include +#include "util/led.h" +#include "logging/esp_eeprom.h" + +#if defined(SILSIM) +#include "silsim/emulated_telemetry.h" +#elif defined(HILSIM) +#include "hilsim/sensors/telemetry_backend.h" +#else +#include "hardware/telemetry_backend.h" +#endif + +/** + * @class Telemetry + * * @brief wraps the telemetry system to create and send a packet +*/ +class Telemetry { +public: + /** + * @brief Default constructor for Telemetry. + */ + Telemetry() = default; + + /** + * @brief Construct a new Telemetry object with a specific backend transceiver. + * * @param backend The telemetry hardware or simulation backend to move into this instance. + */ + explicit Telemetry(TelemetryBackend&& backend); + + /** + * @brief Initializes the telemetry hardware backend. + * * @return ErrorCode Returns ErrorCode::NoErrors on success, or a specific error code on failure. + */ + ErrorCode __attribute__((warn_unused_result)) init(); + + /** + * @brief Composes a telemetry packet and transmits it over the backend transceiver. + * * @param rocket_data Reference to the current state and sensor data of the rocket. + * @param eeprom Reference to the EEPROM configuration and logging system. + * @param led Reference to the LED controller to handle visual status signaling during transmission. + */ + void transmit(RocketData& rocket_data, const MIDASEEPROM& eeprom, LEDController& led); + + /** + * @brief Checks for and receives incoming telemetry commands within a specified timeout. + * * @param command Pointer to the command structure where received data will be stored. + * @param wait_milliseconds The maximum time to block waiting for a packet, in milliseconds. + * @return true If a command was successfully received. + * @return false If the timeout expired or an error occurred. + */ + bool receive(TelemetryCommand* command, int wait_milliseconds); + + /** + * @brief Sends an acknowledgment packet back to the ground station for a received command. + */ + void acknowledgeReceived(); + + /** + * @brief Sets the transmission/reception frequency of the telemetry transceiver. + * * @param frequency target frequency in MHz. + * @return ErrorCode Returns ErrorCode::NoErrors on success, or an error code if the frequency is invalid/unsupported. + */ + ErrorCode setFrequency(float frequency); + + /** + * @brief Sets the FreeRTOS mutex used to synchronize access to the shared SPI bus. + * * @param mtx The SemaphoreHandle_t representing the SPI mutex. + */ + void set_spi_mutex(SemaphoreHandle_t mtx) { backend.set_spi_mutex(mtx); } +private: + /** + * @brief Total number of valid telemetry commands received during this session. + */ + int received_count; + + /** + * @brief Gathers flight data and EEPROM states into a standardized TelemetryPacket structure. + * * @param data Reference to the source rocket flight and sensor telemetry data. + * @param eeprom Reference to the system EEPROM configuration. + * @return TelemetryPacket The formatted packet ready for transmission. + */ + TelemetryPacket makePacket(RocketData& data, const MIDASEEPROM& eeprom); + + /** + * @brief Underlying hardware, HIL, or SIL transceiver driver. + */ + TelemetryBackend backend; +}; \ No newline at end of file diff --git a/MIDAS/src/telemetry/telemetry_packet.h b/MIDAS/src/telemetry/telemetry_packet.h new file mode 100644 index 00000000..ee88d444 --- /dev/null +++ b/MIDAS/src/telemetry/telemetry_packet.h @@ -0,0 +1,93 @@ +#pragma once + +#include +#include +#include "flight-systems/rocket_state.h" + +#define MAX_TELEM_VOLTAGE_V 6.0f +#define MAX_TELEM_CONT_I 0.2f +#define MAX_KF_VPOSITION_M 100000.0f // Max vertical position of KF data (m) +#define MAX_KF_LPOSITION_M 50000.0f // Max lateral position of KF data (m) +#define MAX_ROLL_RATE_HZ 10.0f +#define MAX_ABS_ACCEL_RANGE_G 64 +#define MAX_KF_XVELOCITY_MS 2000.0f + +/** + * @struct TelemetryPacket + * + * @brief format of the telemetry packet +*/ +struct TelemetryPacket { + + // GPS + int32_t lat; + int32_t lon; + uint16_t alt; //15 bit meters, 1 bit command ack + uint16_t baro_alt; + + /** @brief High-G accelerations mapped into fixed-point integer spanning (-64G, 64G]. **/ + uint16_t highg_ax; //16 bit accel (-64G, 64G] + uint16_t highg_ay; //16 bit ay (-64G, 64G] + uint16_t highg_az; //16 bit az (-64G, 64G] + + /** @brief Composite state: Upper 12 bits represent normalized tilt (0 to PI). Lower 4 bits hold the current FSMState enum. */ + uint16_t tilt_fsm; + /** @brief Main system battery voltage mapped using inv_convert_range up to MAX_TELEM_VOLTAGE_V. */ + uint8_t batt_volt; + + // If callsign bit (highest bit of fsm_callsign_satcount) is set, the callsign is KD9ZMJ + // + // If callsign bit (highest bit of fsm_callsign_satcount) is not set, the callsign is KD9ZPM + + + uint8_t gpsfix_satcount; // lower 3 bits gps fix type, upper 5 bits total count of satellites in view + uint8_t serial; // MIDAS Serial number + /** @brief KF estimated horizontal X-axis velocity mapped onto MAX_KF_XVELOCITY_MS. */ + uint16_t kf_vx; // 16 bit meters/second + /** @brief KF estimated horizontal Y-axis velocity mapped onto MAX_KF_XVELOCITY_MS. */ + uint16_t kf_vy; // 16 bit meters/second + /** @brief KF estimated X-position mapped relative to MAX_KF_VPOSITION_M. */ + uint16_t kf_px; // 16 bit meters + /** @brief KF estimated Y-position mapped relative to MAX_KF_LPOSITION_M. */ + uint16_t kf_py; // 16 bit meters + /** @brief KF estimated Z-position mapped relative to MAX_KF_LPOSITION_M. */ + uint16_t kf_pz; // 16 bit meters + + uint32_t pyro; // 8 bit continuity x 4 channels (A, B, C, D), tracking pyrotechnic continuity in channels + + /** @brief Comprehensive bitwise system exception and error tracking collection flags. */ + MErrorFlags error_flags; + + /** @brief Normalized absolute roll rate scaled relative to MAX_ROLL_RATE_HZ, spanning 0x00 to 0xFF. */ + uint8_t roll_rate; + /** @brief Operating status byte originating from the on-board subsystem camera data. */ + uint8_t camera_state; + /** @brief Derived representation of camera battery level computed via fractional scale relative to 9V. */ + uint8_t camera_batt_volt; + +}; + + + +/** @brief Ground station command action flags parsed by the telemetry subsystem. */ +enum class CommandType: uint8_t { RESET_KF, SWITCH_TO_SAFE, SWITCH_TO_PYRO_TEST, SWITCH_TO_ARMED, FIRE_PYRO_A, FIRE_PYRO_B, FIRE_PYRO_C, FIRE_PYRO_D, CAM_ON, CAM_OFF, TOGGLE_CAM_VMUX, CALIB_ACCEL, CALIB_MAG }; + +/** + * @struct TelemetryCommand + * + * @brief format of the packet that telemetry receives +*/ +struct TelemetryCommand { + CommandType command; + uint8_t serial; + uint8_t serial_check; + + /** + * @brief Performs a structural parity verification check on the received command envelope. + * @details Compares the target hardware serial against the obfuscated bit-swapped check field. + */ + + bool valid() { + return serial == (serial_check ^ 0xF2); + } +}; diff --git a/MIDAS/src/telemetry_packet.h b/MIDAS/src/telemetry_packet.h deleted file mode 100644 index d96f3ced..00000000 --- a/MIDAS/src/telemetry_packet.h +++ /dev/null @@ -1,77 +0,0 @@ -#pragma once - -#include -#include -#include "rocket_state.h" - -#define MAX_TELEM_VOLTAGE_V 6.0f -#define MAX_TELEM_CONT_I 0.2f -#define MAX_KF_VPOSITION_M 100000.0f // Max vertical position of KF data (m) -#define MAX_KF_LPOSITION_M 50000.0f // Max lateral position of KF data (m) -#define MAX_ROLL_RATE_HZ 10.0f -#define MAX_ABS_ACCEL_RANGE_G 64 -#define MAX_KF_XVELOCITY_MS 2000.0f - -/** - * @struct TelemetryPacket - * - * @brief format of the telemetry packet -*/ -struct TelemetryPacket { - - // GPS - int32_t lat; - int32_t lon; - uint16_t alt; //15 bit meters, 1 bit command ack - uint16_t baro_alt; - - // High-G - uint16_t highg_ax; //16 bit accel (-64G, 64G] - uint16_t highg_ay; //16 bit ay (-64G, 64G] - uint16_t highg_az; //16 bit az (-64G, 64G] - - uint16_t tilt_fsm; //12 bits tilt | 4 bits FSM - uint8_t batt_volt; - - // If callsign bit (highest bit of fsm_callsign_satcount) is set, the callsign is KD9ZMJ - // - // If callsign bit (highest bit of fsm_callsign_satcount) is not set, the callsign is KD9ZPM - - - uint8_t gpsfix_satcount; // 3 bits gpsfix, 5 bits sat count - uint8_t serial; // MIDAS Serial no - uint16_t kf_vx; // 16 bit meters/second - uint16_t kf_px; // 16 bit meters - uint16_t kf_py; // 16 bit meters - uint16_t kf_pz; // 16 bit meters - - uint32_t pyro; // 8 bit continuity x 4 channels - - // Global error flags - MErrorFlags error_flags; - - uint8_t roll_rate; - uint8_t camera_state; - uint8_t camera_batt_volt; - -}; - - - -// Commands transmitted from ground station to rocket -enum class CommandType: uint8_t { RESET_KF, SWITCH_TO_SAFE, SWITCH_TO_PYRO_TEST, SWITCH_TO_ARMED, FIRE_PYRO_A, FIRE_PYRO_B, FIRE_PYRO_C, FIRE_PYRO_D, CAM_ON, CAM_OFF, TOGGLE_CAM_VMUX, CALIB_ACCEL, CALIB_MAG }; - -/** - * @struct TelemetryCommand - * - * @brief format of the packet that telemetry receives -*/ -struct TelemetryCommand { - CommandType command; - uint8_t serial; - uint8_t serial_check; - - bool valid() { - return serial == (serial_check ^ 0xF2); - } -}; diff --git a/MIDAS/src/Buffer.h b/MIDAS/src/util/Buffer.h similarity index 100% rename from MIDAS/src/Buffer.h rename to MIDAS/src/util/Buffer.h diff --git a/MIDAS/src/Mutex.h b/MIDAS/src/util/Mutex.h similarity index 100% rename from MIDAS/src/Mutex.h rename to MIDAS/src/util/Mutex.h diff --git a/MIDAS/src/Queue.h b/MIDAS/src/util/Queue.h similarity index 100% rename from MIDAS/src/Queue.h rename to MIDAS/src/util/Queue.h diff --git a/MIDAS/src/b2b_interface.cpp b/MIDAS/src/util/b2b_interface.cpp similarity index 97% rename from MIDAS/src/b2b_interface.cpp rename to MIDAS/src/util/b2b_interface.cpp index f164dfa7..296df362 100644 --- a/MIDAS/src/b2b_interface.cpp +++ b/MIDAS/src/util/b2b_interface.cpp @@ -1,6 +1,6 @@ #include "b2b_interface.h" -#include "sensor_data.h" -#include "rocket_state.h" +#include "flight-systems/sensor_data.h" +#include "flight-systems/rocket_state.h" ErrorCode B2BInterface::init() { // No special init diff --git a/MIDAS/src/b2b_interface.h b/MIDAS/src/util/b2b_interface.h similarity index 94% rename from MIDAS/src/b2b_interface.h rename to MIDAS/src/util/b2b_interface.h index 29bb3368..82e03314 100644 --- a/MIDAS/src/b2b_interface.h +++ b/MIDAS/src/util/b2b_interface.h @@ -3,8 +3,8 @@ #include #include "errors.h" #include "hal.h" -#include "sensor_data.h" -#include "rocket_state.h" +#include "flight-systems/sensor_data.h" +#include "flight-systems/rocket_state.h" // Which b2b communication we should use #define B2B_I2C diff --git a/MIDAS/src/buzzer.cpp b/MIDAS/src/util/buzzer.cpp similarity index 99% rename from MIDAS/src/buzzer.cpp rename to MIDAS/src/util/buzzer.cpp index a409d495..c68def4f 100644 --- a/MIDAS/src/buzzer.cpp +++ b/MIDAS/src/util/buzzer.cpp @@ -1,4 +1,4 @@ -#include "buzzer.h" +#include "util/buzzer.h" #include #include diff --git a/MIDAS/src/buzzer.h b/MIDAS/src/util/buzzer.h similarity index 100% rename from MIDAS/src/buzzer.h rename to MIDAS/src/util/buzzer.h diff --git a/MIDAS/src/errors.cpp b/MIDAS/src/util/errors.cpp similarity index 100% rename from MIDAS/src/errors.cpp rename to MIDAS/src/util/errors.cpp diff --git a/MIDAS/src/errors.h b/MIDAS/src/util/errors.h similarity index 100% rename from MIDAS/src/errors.h rename to MIDAS/src/util/errors.h diff --git a/MIDAS/src/hal.h b/MIDAS/src/util/hal.h similarity index 100% rename from MIDAS/src/hal.h rename to MIDAS/src/util/hal.h diff --git a/MIDAS/src/led.cpp b/MIDAS/src/util/led.cpp similarity index 100% rename from MIDAS/src/led.cpp rename to MIDAS/src/util/led.cpp diff --git a/MIDAS/src/led.h b/MIDAS/src/util/led.h similarity index 100% rename from MIDAS/src/led.h rename to MIDAS/src/util/led.h diff --git a/MIDAS/test/fsm_test/src/main.cpp b/MIDAS/test/fsm_test/src/main.cpp index 74689d99..5ea5567d 100644 --- a/MIDAS/test/fsm_test/src/main.cpp +++ b/MIDAS/test/fsm_test/src/main.cpp @@ -10,10 +10,10 @@ #include "csv.hpp" #include "config_parser.h" -#include "fsm.h" -#include "pyro_eval.h" -#include "sensor_data.h" -#include "command_flags.h" +#include "finite-state-machines/fsm.h" +#include "finite-state-machines/pyro_eval.h" +#include "flight-systems/sensor_data.h" +#include "finite-state-machines/command_flags.h" static void print_usage(const char* prog) { fprintf(stderr, diff --git a/MIDAS/tools/extra_script.py b/MIDAS/tools/extra_script.py index 9f39de7c..ddc83352 100644 --- a/MIDAS/tools/extra_script.py +++ b/MIDAS/tools/extra_script.py @@ -7,9 +7,9 @@ Import("env") -log_format_text = (Path("src") / "log_format.h").read_text() -sensor_data_text = (Path("src") / "sensor_data.h").read_text(errors="replace") -eeprom_format_text = (Path("src") / "esp_eeprom_format.h").read_text() +log_format_text = (Path("src") / "logging" / "log_format.h").read_text() +sensor_data_text = (Path("src") / "flight-systems" / "sensor_data.h").read_text(errors="replace") +eeprom_format_text = (Path("src") / "logging" / "esp_eeprom_format.h").read_text() git_command = ["git", "log", "-n", "1", "--pretty=format:%H"] git_hash = subprocess.run(git_command, stdout=subprocess.PIPE).stdout.decode('utf-8') @@ -28,8 +28,8 @@ #define EEPROM_CHECKSUM (0x{checksum_eeprom:08x}) """ -with Path("src/log_checksum.h").open("w") as checksum_file: +with Path("src/logging/log_checksum.h").open("w") as checksum_file: checksum_file.write(file) -with Path("src/esp_eeprom_checksum.h").open("w") as checksum_file_eeprom: +with Path("src/logging/esp_eeprom_checksum.h").open("w") as checksum_file_eeprom: checksum_file_eeprom.write(file_eeprom) diff --git a/MIDAS/tools/log_enc.py b/MIDAS/tools/log_enc.py index 507d2101..8f79e82b 100644 --- a/MIDAS/tools/log_enc.py +++ b/MIDAS/tools/log_enc.py @@ -19,6 +19,7 @@ from lark.visitors import Interpreter, v_args from pathlib import Path import struct +from lark.exceptions import UnexpectedCharacters, UnexpectedToken __all__ = ['parse_file', 'Context', 'Type', 'Struct', 'Enum', 'Float', 'Union', 'Integer'] @@ -747,8 +748,13 @@ def preprocess(self, file_path: str, text: str) -> str: raise Exception(f"Malformed line {i+1} of file {file_path}") include_path = parts[1] if include_path.startswith("\"") and include_path.endswith("\""): - include_path = Path(file_path).absolute().parent / include_path[1:-1] - processed += self.include_file(include_path) + include_name = include_path[1:-1] + candidate = Path(file_path).absolute().parent / include_name + if not candidate.exists(): + candidate = Path("src") / include_name + processed += self.include_file(candidate) + + elif include_path in STD_HEADERS: processed += self.preprocess(include_path[1:-1], STD_HEADERS[include_path]) else: @@ -773,8 +779,14 @@ def parse_file(file: Path) -> tuple[Context, dict[str, str]]: parser = Lark(grammar, parser="earley") try: tree = parser.parse(text) - except Exception as e: - raise Exception(f"Could not parse {file}") from e + except UnexpectedCharacters as e: + print(f"{file}:{e.line}:{e.column}") + print(e.get_context(text)) + raise + except UnexpectedToken as e: + print(f"{file}:{e.line}:{e.column}") + print(e.get_context(text)) + raise ctxt = BASE_CTXT.clone() ctxt.names.update(preprocessor.defines) Calculate(ctxt).visit(tree) @@ -889,8 +901,8 @@ def write_autogen(entries, eeprom_entries, variant_table, out_path: Path): def main(): - ctxt, associations = parse_file(Path("src") / "log_format.h") - ctxt_eeprom, assoc_eeprom = parse_file(Path("src") / "esp_eeprom_format.h") + ctxt, associations = parse_file(Path("src") / "logging" / "log_format.h") + ctxt_eeprom, assoc_eeprom = parse_file(Path("src") / "logging" / "esp_eeprom_format.h") eeprom_format = ctxt_eeprom.types["MIDASEEPROM"] eeprom_entries = flatten("MIDASEEPROM", eeprom_format) @@ -900,7 +912,7 @@ def main(): entries = flatten("LoggedReading", logged_reading) vtable = build_variant_table(entries, associations, disc_enum) - autogen_file = Path("src") / "log_format_AUTOGEN.h" + autogen_file = Path("src") / "logging" / "log_format_AUTOGEN.h" write_autogen(entries, eeprom_entries, vtable, autogen_file) print(f"Successfully wrote log format to {str(autogen_file)}!")