diff --git a/bsp_q7s/core/ObjectFactory.cpp b/bsp_q7s/core/ObjectFactory.cpp index ffc9693b..14f1b34b 100644 --- a/bsp_q7s/core/ObjectFactory.cpp +++ b/bsp_q7s/core/ObjectFactory.cpp @@ -577,7 +577,6 @@ void ObjectFactory::createAcsBoardComponents(LinuxLibgpioIF *gpioComIF, UartComI auto gpsHandler0 = new GPSHyperionHandler(objects::GPS0_HANDLER, objects::UART_COM_IF, uartCookieGps0, debugGps); gpsHandler0->setResetPinTriggerFunction(gps::triggerGpioResetPin, &resetArgsGnss0); - gpsHandler0->setStartUpImmediately(); } void ObjectFactory::createHeaterComponents() { diff --git a/mission/devices/GPSHyperionHandler.cpp b/mission/devices/GPSHyperionHandler.cpp index 8944d17a..58491332 100644 --- a/mission/devices/GPSHyperionHandler.cpp +++ b/mission/devices/GPSHyperionHandler.cpp @@ -4,8 +4,6 @@ #include "fsfw/datapool/PoolReadGuard.h" #include "fsfw/timemanager/Clock.h" -#include "lwgps/lwgps.h" - #if FSFW_DEV_HYPERION_GPS_CREATE_NMEA_CSV == 1 #include #include @@ -13,48 +11,88 @@ GPSHyperionHandler::GPSHyperionHandler(object_id_t objectId, object_id_t deviceCommunication, CookieIF *comCookie, bool debugHyperionGps): - DeviceHandlerBase(objectId, deviceCommunication, comCookie), gpsSet(this), + ExtendedControllerBase(objectId, objects::NO_OBJECT), gpsSet(this), debugHyperionGps(debugHyperionGps) { - lwgps_init(&gpsData); } GPSHyperionHandler::~GPSHyperionHandler() {} -void GPSHyperionHandler::doStartUp() { - if(internalState == InternalStates::NONE) { - commandExecuted = false; - internalState = InternalStates::WAIT_FIRST_MESSAGE; - } - - if(internalState == InternalStates::WAIT_FIRST_MESSAGE) { - if(commandExecuted) { - internalState = InternalStates::IDLE; - setMode(MODE_ON); - commandExecuted = false; - } - } +void GPSHyperionHandler::performControlOperation() { + // The data from the device will generally be read all at once. Therefore, we + // can set all field here +// PoolReadGuard pg(&gpsSet); +// if(pg.getReadResult() != HasReturnvaluesIF::RETURN_OK) { +//#if FSFW_VERBOSE_LEVEL >= 1 +// sif::warning << "GPSHyperionHandler::scanForReply: Reading dataset failed" +// << std::endl; +//#endif +// } +// // Print messages +// if(gpsData.is_valid) { +// // Set all entries valid now, set invalid on case basis if values are sanitized +// gpsSet.setValidity(true, true); +// } +// // Negative latitude -> South direction +// gpsSet.latitude.value = gpsData.latitude; +// // Negative longitude -> West direction +// gpsSet.longitude.value = gpsData.longitude; +// if(gpsData.altitude > 600000.0 or gpsData.altitude < 400000.0) { +// gpsSet.altitude.setValid(false); +// } +// else { +// gpsSet.altitude.setValid(true); +// gpsSet.altitude.value = gpsData.altitude; +// } +// gpsSet.fixMode.value = gpsData.fix_mode; +// gpsSet.satInUse.value = gpsData.sats_in_use; +// Clock::TimeOfDay_t timeStruct = {}; +// timeStruct.day = gpsData.date; +// timeStruct.hour = gpsData.hours; +// timeStruct.minute = gpsData.minutes; +// timeStruct.month = gpsData.month; +// timeStruct.second = gpsData.seconds; +// // Convert two-digit year to full year (AD) +// timeStruct.year = gpsData.year + 2000; +// timeval timeval = {}; +// Clock::convertTimeOfDayToTimeval(&timeStruct, &timeval); +// gpsSet.year = timeStruct.year; +// gpsSet.month = gpsData.month; +// gpsSet.day = gpsData.date; +// gpsSet.hours = gpsData.hours; +// gpsSet.minutes = gpsData.minutes; +// gpsSet.seconds = gpsData.seconds; +// gpsSet.unixSeconds = timeval.tv_sec; +// if(debugHyperionGps) { +// sif::info << "GPS Data" << std::endl; +// printf("Valid status: %d\n", gpsData.is_valid); +// printf("Latitude: %f degrees\n", gpsData.latitude); +// printf("Longitude: %f degrees\n", gpsData.longitude); +// printf("Altitude: %f meters\n", gpsData.altitude); +// } +//#if FSFW_DEV_HYPERION_GPS_CREATE_NMEA_CSV == 1 +// std::string filename = "/mnt/sd0/gps_log.txt"; +// std::ofstream gpsFile; +// if(not std::filesystem::exists(filename)) { +// gpsFile.open(filename, std::ofstream::out); +// } +// gpsFile.open(filename, std::ofstream::out | std::ofstream::app); +// gpsFile.write("\n", 1); +// gpsFile.write(reinterpret_cast(start), len); +//#endif } -void GPSHyperionHandler::doShutDown() { - internalState = InternalStates::NONE; - commandExecuted = false; - setMode(_MODE_POWER_DOWN); +LocalPoolDataSetBase* GPSHyperionHandler::getDataSetHandle(sid_t sid) { + return nullptr; } -ReturnValue_t GPSHyperionHandler::buildTransitionDeviceCommand(DeviceCommandId_t *id) { - return NOTHING_TO_SEND; +ReturnValue_t GPSHyperionHandler::checkModeCommand(Mode_t mode, Submode_t submode, + uint32_t *msToReachTheMode) { + return HasReturnvaluesIF::RETURN_OK; } -ReturnValue_t GPSHyperionHandler::buildNormalDeviceCommand(DeviceCommandId_t *id) { - return NOTHING_TO_SEND; -} - -ReturnValue_t GPSHyperionHandler::buildCommandFromCommand( - DeviceCommandId_t deviceCommand, const uint8_t *commandData, - size_t commandDataLen) { - // By default, send nothing - rawPacketLen = 0; - switch(deviceCommand) { +ReturnValue_t GPSHyperionHandler::executeAction(ActionId_t actionId, MessageQueueId_t commandedBy, + const uint8_t *data, size_t size) { + switch(actionId) { case(GpsHyperion::TRIGGER_RESET_PIN): { if(resetCallback != nullptr) { PoolReadGuard pg(&gpsSet); @@ -69,99 +107,6 @@ ReturnValue_t GPSHyperionHandler::buildCommandFromCommand( return HasReturnvaluesIF::RETURN_OK; } -ReturnValue_t GPSHyperionHandler::scanForReply(const uint8_t *start, size_t len, - DeviceCommandId_t *foundId, size_t *foundLen) { - // Pass data to GPS library - if(len > 0) { - // sif::debug << "GPSHandler::scanForReply: Received " << len << " bytes" << std::endl; - if (internalState == InternalStates::WAIT_FIRST_MESSAGE) { - // TODO: Check whether data is valid by checking whether NMEA start string is valid? - commandExecuted = true; - } - int result = lwgps_process(&gpsData, start, len); - if(result != 1) { - sif::warning << "GPSHandler::scanForReply: Issue processing GPS data with lwgps" - << std::endl; - } - else { - // The data from the device will generally be read all at once. Therefore, we - // can set all field here - PoolReadGuard pg(&gpsSet); - if(pg.getReadResult() != HasReturnvaluesIF::RETURN_OK) { -#if FSFW_VERBOSE_LEVEL >= 1 - sif::warning << "GPSHyperionHandler::scanForReply: Reading dataset failed" - << std::endl; -#endif - } - // Print messages - if(gpsData.is_valid) { - // Set all entries valid now, set invalid on case basis if values are sanitized - gpsSet.setValidity(true, true); - } - // Negative latitude -> South direction - gpsSet.latitude.value = gpsData.latitude; - // Negative longitude -> West direction - gpsSet.longitude.value = gpsData.longitude; - if(gpsData.altitude > 600000.0 or gpsData.altitude < 400000.0) { - gpsSet.altitude.setValid(false); - } - else { - gpsSet.altitude.setValid(true); - gpsSet.altitude.value = gpsData.altitude; - } - gpsSet.fixMode.value = gpsData.fix_mode; - gpsSet.satInUse.value = gpsData.sats_in_use; - Clock::TimeOfDay_t timeStruct = {}; - timeStruct.day = gpsData.date; - timeStruct.hour = gpsData.hours; - timeStruct.minute = gpsData.minutes; - timeStruct.month = gpsData.month; - timeStruct.second = gpsData.seconds; - // Convert two-digit year to full year (AD) - timeStruct.year = gpsData.year + 2000; - timeval timeval = {}; - Clock::convertTimeOfDayToTimeval(&timeStruct, &timeval); - gpsSet.year = timeStruct.year; - gpsSet.month = gpsData.month; - gpsSet.day = gpsData.date; - gpsSet.hours = gpsData.hours; - gpsSet.minutes = gpsData.minutes; - gpsSet.seconds = gpsData.seconds; - gpsSet.unixSeconds = timeval.tv_sec; - if(debugHyperionGps) { - sif::info << "GPS Data" << std::endl; - printf("Valid status: %d\n", gpsData.is_valid); - printf("Latitude: %f degrees\n", gpsData.latitude); - printf("Longitude: %f degrees\n", gpsData.longitude); - printf("Altitude: %f meters\n", gpsData.altitude); - } -#if FSFW_DEV_HYPERION_GPS_CREATE_NMEA_CSV == 1 - std::string filename = "/mnt/sd0/gps_log.txt"; - std::ofstream gpsFile; - if(not std::filesystem::exists(filename)) { - gpsFile.open(filename, std::ofstream::out); - } - gpsFile.open(filename, std::ofstream::out | std::ofstream::app); - gpsFile.write("\n", 1); - gpsFile.write(reinterpret_cast(start), len); -#endif - } - *foundLen = len; - *foundId = GpsHyperion::GPS_REPLY; - } - return HasReturnvaluesIF::RETURN_OK; - -} - -ReturnValue_t GPSHyperionHandler::interpretDeviceReply(DeviceCommandId_t id, - const uint8_t *packet) { - return HasReturnvaluesIF::RETURN_OK; -} - -uint32_t GPSHyperionHandler::getTransitionDelayMs(Mode_t from, Mode_t to) { - return 5000; -} - ReturnValue_t GPSHyperionHandler::initializeLocalDataPool( localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) { localDataPoolMap.emplace(GpsHyperion::ALTITUDE, new PoolEntry({0.0})); @@ -180,35 +125,21 @@ ReturnValue_t GPSHyperionHandler::initializeLocalDataPool( return HasReturnvaluesIF::RETURN_OK; } -void GPSHyperionHandler::fillCommandAndReplyMap() { - // Reply length does not matter, packets should always arrive periodically - insertInReplyMap(GpsHyperion::GPS_REPLY, 4, &gpsSet, 0, true); - insertInCommandMap(GpsHyperion::TRIGGER_RESET_PIN); -} - -void GPSHyperionHandler::modeChanged() { - internalState = InternalStates::NONE; -} - void GPSHyperionHandler::setResetPinTriggerFunction(gpioResetFunction_t resetCallback, void *args) { this->resetCallback = resetCallback; resetCallbackArgs = args; } -void GPSHyperionHandler::debugInterface(uint8_t positionTracker, object_id_t objectId, - uint32_t parameter) { -} ReturnValue_t GPSHyperionHandler::initialize() { - ReturnValue_t result = DeviceHandlerBase::initialize(); + ReturnValue_t result = ExtendedControllerBase::initialize(); if(result != HasReturnvaluesIF::RETURN_OK) { return result; } - // Enable reply immediately for now - return updatePeriodicReply(true, GpsHyperion::GPS_REPLY); + return result; } -ReturnValue_t GPSHyperionHandler::acceptExternalDeviceCommands() { - return DeviceHandlerBase::acceptExternalDeviceCommands(); +ReturnValue_t GPSHyperionHandler::handleCommandMessage(CommandMessage *message) { + return ExtendedControllerBase::handleCommandMessage(message); } diff --git a/mission/devices/GPSHyperionHandler.h b/mission/devices/GPSHyperionHandler.h index e7c925be..62ec4d01 100644 --- a/mission/devices/GPSHyperionHandler.h +++ b/mission/devices/GPSHyperionHandler.h @@ -3,6 +3,7 @@ #include "fsfw/FSFW.h" #include "fsfw/devicehandlers/DeviceHandlerBase.h" +#include "fsfw/controller/ExtendedControllerBase.h" #include "devicedefinitions/GPSDefinitions.h" #include "lwgps/lwgps.h" @@ -12,7 +13,7 @@ * Flight manual: * https://egit.irs.uni-stuttgart.de/redmine/projects/eive-flight-manual/wiki/Hyperion_HT-GPS200 */ -class GPSHyperionHandler: public DeviceHandlerBase { +class GPSHyperionHandler: public ExtendedControllerBase { public: GPSHyperionHandler(object_id_t objectId, object_id_t deviceCommunication, @@ -22,46 +23,24 @@ public: using gpioResetFunction_t = ReturnValue_t (*) (void* args); void setResetPinTriggerFunction(gpioResetFunction_t resetCallback, void*args); - ReturnValue_t acceptExternalDeviceCommands() override; - + ReturnValue_t handleCommandMessage(CommandMessage *message) override; + void performControlOperation() override; + LocalPoolDataSetBase* getDataSetHandle(sid_t sid) override; + ReturnValue_t checkModeCommand(Mode_t mode, Submode_t submode, + uint32_t *msToReachTheMode) override; + ReturnValue_t executeAction(ActionId_t actionId, + MessageQueueId_t commandedBy, const uint8_t* data, + size_t size) override; ReturnValue_t initialize() override; protected: gpioResetFunction_t resetCallback = nullptr; void* resetCallbackArgs = nullptr; - enum class InternalStates { - NONE, - WAIT_FIRST_MESSAGE, - IDLE - }; - InternalStates internalState = InternalStates::NONE; - bool commandExecuted = false; - - /* DeviceHandlerBase overrides */ - ReturnValue_t buildTransitionDeviceCommand( - DeviceCommandId_t *id) override; - void doStartUp() override; - void doShutDown() override; - ReturnValue_t buildNormalDeviceCommand( - DeviceCommandId_t *id) override; - ReturnValue_t buildCommandFromCommand( - DeviceCommandId_t deviceCommand, const uint8_t *commandData, - size_t commandDataLen) override; - ReturnValue_t scanForReply(const uint8_t *start, size_t len, - DeviceCommandId_t *foundId, size_t *foundLen) override; - ReturnValue_t interpretDeviceReply(DeviceCommandId_t id, - const uint8_t *packet) override; - - void fillCommandAndReplyMap() override; - void modeChanged() override; - uint32_t getTransitionDelayMs(Mode_t from, Mode_t to) override; ReturnValue_t initializeLocalDataPool(localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) override; - virtual void debugInterface(uint8_t positionTracker = 0, - object_id_t objectId = 0, uint32_t parameter = 0) override; + private: - lwgps_t gpsData = {}; GpsPrimaryDataset gpsSet; bool debugHyperionGps = false; };