From d921833ee64326d9ae64420a6fd201345f81c1f3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E2=80=9CChristy?= <“[christy.guirguis@ucalgary.ca]”> Date: Sat, 12 Sep 2026 14:07:46 -0600 Subject: [PATCH] remove polling task, keep submodule name "SoarDrivers" in project conf --- .project | 2 +- PollingTask.cpp | 419 ------------------------------------------------ PollingTask.hpp | 179 --------------------- 3 files changed, 1 insertion(+), 599 deletions(-) delete mode 100644 PollingTask.cpp delete mode 100644 PollingTask.hpp diff --git a/.project b/.project index 9f6a3ed..a2d19b2 100644 --- a/.project +++ b/.project @@ -1,6 +1,6 @@ - PeripheralDriversSubmodule + SoarDrivers diff --git a/PollingTask.cpp b/PollingTask.cpp deleted file mode 100644 index e30c3a0..0000000 --- a/PollingTask.cpp +++ /dev/null @@ -1,419 +0,0 @@ -/* - * PollingTask.cpp - * - * Created on: Apr 8, 2026 - * Author: jaddina - */ - -#include "PollingTask.hpp" -#include "FreeRTOS.h" -#include "timers.h" -#include "DataBroker.hpp" -#include "LoggingService.hpp" -#include "CanAutoNodeDaughter.hpp" - -extern FDCAN_HandleTypeDef hfdcan1; - -namespace { -constexpr uint32_t kLaunchPollMs = 20; -constexpr uint32_t kBoostPollMs = 50; -constexpr uint32_t kCoastPollMs = 100; -constexpr uint32_t kDescentPollMs = 100; -constexpr uint32_t kRecoveryPollMs = 250; -constexpr uint32_t kGroundPollMs = 300; -constexpr uint32_t kCanServicePeriodMs = 20; - - -constexpr uint8_t kDaqBoardType = 1; -constexpr uint8_t kDaqSlotNumber = 1; -constexpr uint8_t kRocketStateRxLogIndex = 0; -constexpr uint16_t kRocketStateRxLogSizeBytes = sizeof(uint8_t); -} - -PollingTask::PollingTask():Task(TASK_LOGGING_QUEUE_DEPTH_OBJS), pollTimerHandle(nullptr), rocketState(RocketState::RS_TEST), imu16(), imu32(), magnetometer() -{ - -} - -/** - * @brief Initialize the PollingTask - * Do not modify this function aside from adding the task name - */ -void PollingTask::InitTask() -{ - // Make sure the task is not already initialized - SOAR_ASSERT(rtTaskHandle == nullptr, "Cannot initialize watchdog task twice"); - - BaseType_t rtValue = - xTaskCreate((TaskFunction_t)PollingTask::RunTask, - (const char*)"PollingTask", - (uint16_t)TASK_LOGGING_QUEUE_DEPTH_WORDS, - (void*)this, - (UBaseType_t)TASK_LOGGING_PRIORITY, - (TaskHandle_t*)&rtTaskHandle); - - SOAR_ASSERT(rtValue == pdPASS, "PollingTask::InitTask() - xTaskCreate() failed"); - - //Init drivers - imu16.Init(hspi6_, LSM6DSO_CS_PIN, LSM6DSO_CS_PORT ); - imu32.Init(hspi2_, LSM6DSO32_CS_PORT, LSM6DSO32_CS_PIN); - magnetometer.Init(hspi4_, MMC_CS_PORT, MMC_CS_PIN); - - pollTimerHandle = xTimerCreate("PollingTimer", - pdMS_TO_TICKS(kLaunchPollMs), - pdFALSE, - (void*)this, - PollingTask::PollTimerCallback); - - SOAR_ASSERT(pollTimerHandle != nullptr, "PollingTask::InitTask() - xTimerCreate() failed"); - - CanAutoNodeDaughter::LogInit daughterLogs[] = { - {kRocketStateRxLogSizeBytes}, - }; - canNode = new CanAutoNodeDaughter(&hfdcan1, - daughterLogs, - sizeof(daughterLogs) / sizeof(daughterLogs[0]), - kDaqBoardType, - kDaqSlotNumber, - "DAQ"); - SOAR_ASSERT(canNode != nullptr, "PollingTask::InitTask() - CAN node alloc failed"); - - ApplyRocketState(); -} - -void PollingTask::SetRocketState(RocketState newState) -{ - rocketState = newState; - ApplyRocketState(); -} - -uint32_t PollingTask::GetPollingPeriodMs(RocketState state) -{ - switch (state) - { - - case RocketState::RS_NONE: - case RocketState::RS_ABORT: - LoggingService::StopLogging(); - return 0; - case RocketState::RS_FILL: - LoggingService::StartLogging(); - return kGroundPollMs; - case RocketState::RS_PRELAUNCH: - LoggingService::StartLogging(); - return kGroundPollMs; - case RocketState::RS_ARM: - LoggingService::StartLogging(); - return kGroundPollMs; - case RocketState::RS_TEST: - LoggingService::StopLogging(); - return 0; - case RocketState::RS_IGNITION: - LoggingService::StartLogging(); - return kLaunchPollMs; - case RocketState::RS_LAUNCH: - LoggingService::StartLogging(); - return kLaunchPollMs; - case RocketState::RS_BURN: - LoggingService::StartLogging(); - return kLaunchPollMs; - case RocketState::RS_COAST: - LoggingService::StartLogging(); - return kCoastPollMs; - case RocketState::RS_DESCENT: - LoggingService::StartLogging(); - return kDescentPollMs; - case RocketState::RS_RECOVERY: - LoggingService::StartLogging(); - return kRecoveryPollMs; - - default: - return 0; - } -} - -void PollingTask::ApplyRocketState() -{ - if (pollTimerHandle == nullptr) - { - return; - } - - const uint32_t pollPeriodMs = GetPollingPeriodMs(rocketState); - if (pollPeriodMs == 0) - { - (void)xTimerStop(pollTimerHandle, 0); - return; - } - - (void)xTimerChangePeriod(pollTimerHandle, pdMS_TO_TICKS(pollPeriodMs), 0); -} - -void PollingTask::Run(void * pvParams){ - // Allow peripherals to settle after scheduler starts, then initialize GPS. - vTaskDelay(pdMS_TO_TICKS(kGroundPollMs)); - gpsInitialized = gps.Init(hspi6_, GPS_CS_PORT, GPS_CS_PIN); - - while (1) { - - Command cm; - bool res = qEvtQueue->Receive(cm, kCanServicePeriodMs); - if(res){ - - HandleCommand(cm); - } - ServiceCanNetwork(); - - } -} - -bool PollingTask::DecodeRocketStateFromCan(uint8_t rawState, RocketState& outState) -{ - switch (rawState) - { - case static_cast(RocketState::RS_PRELAUNCH): - outState = RocketState::RS_PRELAUNCH; - return true; - case static_cast(RocketState::RS_FILL): - outState = RocketState::RS_FILL; - return true; - case static_cast(RocketState::RS_ARM): - outState = RocketState::RS_ARM; - return true; - case static_cast(RocketState::RS_IGNITION): - outState = RocketState::RS_IGNITION; - return true; - case static_cast(RocketState::RS_LAUNCH): - outState = RocketState::RS_LAUNCH; - return true; - case static_cast(RocketState::RS_BURN): - outState = RocketState::RS_BURN; - return true; - case static_cast(RocketState::RS_COAST): - outState = RocketState::RS_COAST; - return true; - case static_cast(RocketState::RS_DESCENT): - outState = RocketState::RS_DESCENT; - return true; - case static_cast(RocketState::RS_RECOVERY): - outState = RocketState::RS_RECOVERY; - return true; - case static_cast(RocketState::RS_ABORT): - outState = RocketState::RS_ABORT; - return true; - case static_cast(RocketState::RS_TEST): - outState = RocketState::RS_TEST; - return true; - case static_cast(RocketState::RS_NONE): - outState = RocketState::RS_NONE; - return true; - default: - return false; - } -} - -void PollingTask::ServiceCanNetwork() -{ - if (canNode == nullptr) - { - SOAR_PRINT("null cannode\n"); - return; - } - if(canNode->GetCurrentState() == CanAutoNodeDaughter::ERROR){ - //SOAR_PRINT("error can\n"); - return; - } - - (void)canNode->CheckCANCommands(); - - if(canNode->GetCurrentState() == CanAutoNodeDaughter::UNINITIALIZED) { - canNode->TryRequestingJoiningNetwork(); - } - - if(canNode->GetCurrentState() == CanAutoNodeDaughter::READY) { - - uint8_t rawRocketState = 0; - if (canNode->ReadMessageByLogIndex(kRocketStateRxLogIndex, &rawRocketState, sizeof(rawRocketState))) - { - RocketState decodedState = RocketState::RS_PRELAUNCH; - if (!DecodeRocketStateFromCan(rawRocketState, decodedState)) - { - SOAR_PRINT("PollingTask CAN | Unknown rocket state byte: %u\n", (unsigned int)rawRocketState); - return; - } - - if (decodedState != rocketState) - { - SetRocketState(decodedState); - SOAR_PRINT("PollingTask CAN | Rocket state updated to %u\n", (unsigned int)rawRocketState); - } - } - } -} - -void PollingTask::HandleCommand(Command& cm){ - switch(cm.GetCommand()){ - case DATA_COMMAND: - HandleRequestCommand(cm.GetTaskCommand()); - break; - - case TASK_SPECIFIC_COMMAND: - break; - } - - cm.Reset(); - -} - -void PollingTask::HandleRequestCommand(uint16_t taskCommand){ - switch(taskCommand){ - case POLL_SENSORS_AND_LOG: - PollSensors(); - LogData(); - break; - case GPS_TEST: - if (gpsInitialized) - { - if (gps.getGGALine(gpsData.buffer_)) - { - SOAR_PRINT("GPS RAW | %s\n", gpsData.buffer_); - GPSData rawGps = gpsData; - DataBroker::Publish(&rawGps); - gps.ParseGpsData(&gpsData); - SOAR_PRINT("GPS GGA | time=%d lat_deg=%d lat_min=%d lon_deg=%d lon_min=%d antAlt=%d antUnit=%d geoidAlt=%d geoidUnit=%d totalAlt=%d totalUnit=%d\n", - (int32_t)gpsData.time_, - (int32_t)gpsData.latitude_.degrees_, - (int32_t)gpsData.latitude_.minutes_, - (int32_t)gpsData.longitude_.degrees_, - (int32_t)gpsData.longitude_.minutes_, - (int32_t)gpsData.antennaAltitude_.altitude_, - (int32_t)gpsData.antennaAltitude_.unit_, - (int32_t)gpsData.geoidAltitude_.altitude_, - (int32_t)gpsData.geoidAltitude_.unit_, - (int32_t)gpsData.totalAltitude_.altitude_, - (int32_t)gpsData.totalAltitude_.unit_); - } - - } - - default: - break; - } -} - -void PollingTask::PollTimerCallback(TimerHandle_t xTimer) -{ - PollingTask* task = static_cast(pvTimerGetTimerID(xTimer)); - if ((task == nullptr) || (task->GetEventQueue() == nullptr)) { - return; - } - - Command cmd(DATA_COMMAND, PollingTask::POLL_SENSORS_AND_LOG); - task->GetEventQueue()->Send(cmd, true); - task->ApplyRocketState(); -} - - -void PollingTask::PollSensors() -{ - uint8_t data[14] = {0}; - - baro07Data = barometer07.getSample(); - baro07Data.id = 0; - - baro11Data = barometer11.getSample(); - baro11Data.id = 1; - - imu16.readSensors(data); - imu16Data = imu16.bytesToStruct(data, true, true, true); - imu16Data.id = 0; - - imu32.ReadSensors(data); - imu32Data = imu32.ConvertRawMeasurementToStruct(data); - imu32Data.id = 1; - - magnetometer.triggerMeasurement(); - magnetometer.readData(driverData); - - magData.magX = driverData.scaledX; - magData.magY = driverData.scaledY; - magData.magZ = driverData.scaledZ; - - static const char kMockGga[83] ="$GNGGA,123519,4807.038,N,01131.000,E,1,08,0.9,545.4,M,46.9,M,,*59\r\n"; - memset(gpsData.buffer_, 0, sizeof(gpsData.buffer_)); - strncpy(gpsData.buffer_, kMockGga, sizeof(gpsData.buffer_) - 1); - GPSData parsedGps = gpsData; - gps.ParseGpsData(&parsedGps); - - -} - - -void PollingTask::LogData(){ - TickType_t nowTick = xTaskGetTickCount(); - - if(!pollingTimerStarted){ - pollingStartTick = nowTick; - previousLogTick = nowTick; - pollingTimerStarted = true; - } - - TickType_t elapsedTicks = nowTick - pollingStartTick; - TickType_t deltaTicks = nowTick - previousLogTick; - - float elapsedSec = (float)elapsedTicks / configTICK_RATE_HZ; - float deltaSec = (float)deltaTicks / configTICK_RATE_HZ; - uint32_t rateHz = (deltaSec > 0.0f) ? (1.0f / deltaSec) : 0.0f; - - uint32_t elapsedMs = (elapsedTicks * 1000U) / configTICK_RATE_HZ; - uint32_t deltaMs = (deltaTicks * 1000U) / configTICK_RATE_HZ; - - SOAR_PRINT("PollingTask Timing | elapsed=%d ms dt=%d ms rate=%d Hz\n", - elapsedMs, - deltaMs, - rateHz); - - previousLogTick = nowTick; - - SOAR_PRINT("Log data polled"); -// SOAR_PRINT("PollingTask Log | Baro07 id=%u temp=%d pressure=%lu\n", -// (unsigned int)baro07Data.id, -// (int)baro07Data.temp, -// (unsigned long)baro07Data.pressure); -// SOAR_PRINT("PollingTask Log | Baro11 id=%u temp=%d pressure=%lu\n", -// (unsigned int)baro11Data.id, -// (int)baro11Data.temp, -// (unsigned long)baro11Data.pressure); -// -// SOAR_PRINT("PollingTask Log | IMU16 id=%u accel=(%d,%d,%d) gyro=(%d,%d,%d) temp=%d\n", -// (unsigned int)imu16Data.id, -// (int)imu16Data.accel.x, -// (int)imu16Data.accel.y, -// (int)imu16Data.accel.z, -// (int)imu16Data.gyro.x, -// (int)imu16Data.gyro.y, -// (int)imu16Data.gyro.z, -// (int)imu16Data.temp); -// -// SOAR_PRINT("PollingTask Log | IMU32 id=%u accel=(%d,%d,%d) gyro=(%d,%d,%d) temp=%d\n", -// (unsigned int)imu32Data.id, -// (int)imu32Data.accel.x, -// (int)imu32Data.accel.y, -// (int)imu32Data.accel.z, -// (int)imu32Data.gyro.x, -// (int)imu32Data.gyro.y, -// (int)imu32Data.gyro.z, -// (int)imu32Data.temp); -// -// SOAR_PRINT("PollingTask Log | MAG scaled=(%ld,%ld,%ld)\n", -// (long)magData.magX, -// (long)magData.magY, -// (long)magData.magZ); - - DataBroker::Publish(&imu16Data); - DataBroker::Publish(&imu32Data); - DataBroker::Publish(&baro07Data); - DataBroker::Publish(&baro11Data); - DataBroker::Publish(&magData); - DataBroker::Publish(&gpsData); -} diff --git a/PollingTask.hpp b/PollingTask.hpp deleted file mode 100644 index a5bafea..0000000 --- a/PollingTask.hpp +++ /dev/null @@ -1,179 +0,0 @@ -/* - * PollingTask.hpp - * - * Created on: Apr 8, 2026 - * Author: jaddina - */ -#ifndef COMPONENTS_POLLINGTASK_HPP_ -#define COMPONENTS_POLLINGTASK_HPP_ - -#include "Task.hpp" -#include "MS5607Driver.hpp" -#include "MS5611Driver.hpp" -#include "lsm6dso.hpp" -#include -#include "mmc5983ma.hpp" -#include "NEO-M9N-00BDriver.hpp" - -#include "SensorDataTypes.hpp" -#include "main.h" - -class CanAutoNodeDaughter; - -extern SPI_HandleTypeDef hspi1; -extern SPI_HandleTypeDef hspi2; -extern SPI_HandleTypeDef hspi3; -extern SPI_HandleTypeDef hspi4; -extern SPI_HandleTypeDef hspi5; -extern SPI_HandleTypeDef hspi6; - -enum RocketState -{ - - //-- GROUND -- - // Manual venting allowed at all times - RS_PRELAUNCH = 0, // Idle state, waiting for command to proceeding sequences - RS_FILL, // N2 Prefill/Purge/Leak-check/Load-cell Tare check sub-sequences, full control of valves (except MEV) allowed - RS_ARM, // We don't allow fill etc. 1-2 minutes before launch : Cannot fill rocket with N2 etc. unless you return to FILL - // Power Transition, Fill-Arm Disconnect Sub-sequences (you should be able to revert the power transition) - - //-- IGNITION -- Manual venting NOT ALLOWED - RS_IGNITION, // Ignition of the ignitors - RS_LAUNCH, // Launch triggered by confirmation of ignition (from ignitor) is nominal : MEV Open Sequence - - //-- BURN -- - // Vents should stay closed, manual venting NOT ALLOWED - // !vents open is definitely not ideal for abort! Best to keep it closed with manual override if we fail here - // (can we maybe have the code change the true default state by overwriting EEPROM?) - // Ideally we don't want to EVER exceed 7 seconds of burn time (we should store this time at the very least - split into 1 sub-stage for each second if necessary). - // For timing we want ~1/10th of a second or better. - RS_BURN, // Main burn (vents closed MEV open) - 5-6 seconds (TBD) : - - - //-- COAST -- - // Manual venting NOT ALLOWED -- Note: MEV never closes! - RS_COAST, // Coasting (MEV closed, vents closed) - 30 seconds (TBD) ^ Vents closed applies here too, in part. Includes APOGEE - - //-- DESCENT / POSTAPOGEE -- - // Automatic Venting AND Vent Control ALLOWED - RS_DESCENT, // Vents open (well into the descent) - RS_RECOVERY, // Vents open, MEV closed, transmit all data over radio and accept vent commands - // Supports general commands (e.g. venting) and logs/transmits slowly (maybe stop logging after close to full memory?) - - //-- RECOVERY / TECHNICAL -- - RS_ABORT, // Abort sequence, vents open, MEV closed, ignitors off - RS_TEST, // Test, between ABORT and PRE-LAUNCH, has full control of all GPIOs - - RS_NONE // Invalid state, must be last -}; - - - - -class PollingTask: public Task -{ - public: - - enum PollingTaskCommands : uint16_t { - POLL_SENSORS_AND_LOG = 1, - GPS_TEST, - }; - - static PollingTask& Inst() { - static PollingTask inst; - return inst; - } - - void InitTask(); - void SetRocketState(RocketState newState); - RocketState GetRocketState() const { return rocketState; } - - - - protected: - - static void RunTask(void* pvParams) { PollingTask::Inst().Run(pvParams); } // Static Task Interface, passes control to the instance Run(); - void Run(void * pvParams); // Main run code - void HandleCommand(Command& cm); - void HandleRequestCommand(uint16_t taskCommand); - static void PollTimerCallback(TimerHandle_t xTimer); - - - - private: - // Private Functions - PollingTask(); // Private constructor - PollingTask(const PollingTask&); // Prevent copy-construction - PollingTask& operator=(const PollingTask&); // Prevent assignment - void LogData(); - void PollSensors(); - void ApplyRocketState(); - void ServiceCanNetwork(); - static bool DecodeRocketStateFromCan(uint8_t rawState, RocketState& outState); - static uint32_t GetPollingPeriodMs(RocketState state); - TimerHandle_t pollTimerHandle; - - //to count time elapsed and time taken per poll and Hz - TickType_t pollingStartTick = 0; - TickType_t previousLogTick = 0; - bool pollingTimerStarted = false; - bool gpsInitialized = false; - bool canNetworkReady = false; - TickType_t lastCanJoinRetryTick = 0; - - RocketState rocketState; - //sensor data structs - BaroData baro07Data; - BaroData baro11Data; - IMUData imu32Data; - IMUData imu16Data; - MagData magData; - MagDriverData driverData; - GPSData gpsData; - - //All sensor gpios and spi - //Baro 07 config - GPIO_TypeDef* MS5607_CS_PORT = BARO07_CS_GPIO_Port; - const uint16_t MS5607_CS_PIN = BARO07_CS_Pin; - SPI_HandleTypeDef* hspi3_= &hspi3; - - //Baro 11 config - GPIO_TypeDef* MS5611_CS_PORT = BARO11_CS_GPIO_Port; - const uint16_t MS5611_CS_PIN = BARO11_CS_Pin; - SPI_HandleTypeDef* hspi1_ = &hspi1; - - //IMU 32 config - GPIO_TypeDef *LSM6DSO32_CS_PORT = IMU32_CS_GPIO_Port; - const uint16_t LSM6DSO32_CS_PIN = IMU32_CS_Pin; - SPI_HandleTypeDef *hspi2_ = &hspi2; - - //IMU 16 config - GPIO_TypeDef *LSM6DSO_CS_PORT = IMU16_CS_GPIO_Port; - const uint16_t LSM6DSO_CS_PIN = IMU16_CS_Pin; - SPI_HandleTypeDef *hspi6_ = &hspi6; - - //Mag config - SPI_HandleTypeDef *hspi4_ = &hspi4; - GPIO_TypeDef *MMC_CS_PORT = MAG_CS_GPIO_Port; - const uint16_t MMC_CS_PIN = MAG_CS_Pin; - - //GPS config - GPIO_TypeDef *GPS_CS_PORT = GPS_CS_GPIO_Port; - const uint16_t GPS_CS_PIN = GPS_CS_Pin; - - - //Drivers - LSM6DSO_Driver imu16; - LSM6DO32_Driver imu32; - MS5607_Driver barometer07{hspi3_, MS5607_CS_PORT, MS5607_CS_PIN}; - MS5611_Driver barometer11{hspi1_, MS5611_CS_PORT, MS5611_CS_PIN}; - MMC5983MA magnetometer; - NEOM9N00B gps; - - CanAutoNodeDaughter* canNode = nullptr; - -}; - - - -#endif /* COMPONENTS_POLLINGTASK_HPP_ */