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() {

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 <filesystem>
 #include <fstream>
@@ -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() {
+void GPSHyperionHandler::performControlOperation() {
-    if(internalState == InternalStates::NONE) {
+    // The data from the device will generally be read all at once. Therefore, we
-        commandExecuted = false;
+    // can set all field here
-        internalState = InternalStates::WAIT_FIRST_MESSAGE;
+//    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<const char*>(start), len);
+//#endif
 }
 
-    if(internalState == InternalStates::WAIT_FIRST_MESSAGE) {
+LocalPoolDataSetBase* GPSHyperionHandler::getDataSetHandle(sid_t sid) {
-        if(commandExecuted) {
+    return nullptr;
-            internalState = InternalStates::IDLE;
-            setMode(MODE_ON);
-            commandExecuted = false;
-        }
-    }
 }
 
-void GPSHyperionHandler::doShutDown() {
+ReturnValue_t GPSHyperionHandler::checkModeCommand(Mode_t mode, Submode_t submode,
-    internalState = InternalStates::NONE;
+    uint32_t *msToReachTheMode) {
-    commandExecuted = false;
+    return HasReturnvaluesIF::RETURN_OK;
-    setMode(_MODE_POWER_DOWN);
 }
 
-ReturnValue_t GPSHyperionHandler::buildTransitionDeviceCommand(DeviceCommandId_t *id) {
+ReturnValue_t GPSHyperionHandler::executeAction(ActionId_t actionId, MessageQueueId_t commandedBy,
-    return NOTHING_TO_SEND;
+    const uint8_t *data, size_t size) {
-}
+    switch(actionId) {
-
-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) {
     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<const char*>(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<double>({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 result;
-    return updatePeriodicReply(true, GpsHyperion::GPS_REPLY);
 }
 
-ReturnValue_t GPSHyperionHandler::acceptExternalDeviceCommands() {
+ReturnValue_t GPSHyperionHandler::handleCommandMessage(CommandMessage *message) {
-    return DeviceHandlerBase::acceptExternalDeviceCommands();
+    return ExtendedControllerBase::handleCommandMessage(message);
 }

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;
 };