continued gps handler

mission/devices/GPSHyperionHandler.cpp

@@ -0,0 +1,118 @@
+#include "GPSHyperionHandler.h"
+#include "devicedefinitions/GPSDefinitions.h"
+
+#include "fsfw/datapool/PoolReadGuard.h"
+
+#include "lwgps/lwgps.h"
+
+GPSHyperionHandler::GPSHyperionHandler(object_id_t objectId, object_id_t deviceCommunication,
+		CookieIF *comCookie):
+		DeviceHandlerBase(objectId, deviceCommunication, comCookie), gpsSet(this) {
+    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::doShutDown() {
+    internalState = InternalStates::NONE;
+    commandExecuted = false;
+    setMode(MODE_OFF);
+}
+
+ReturnValue_t GPSHyperionHandler::buildTransitionDeviceCommand(DeviceCommandId_t *id) {
+	return HasReturnvaluesIF::RETURN_OK;
+}
+
+ReturnValue_t GPSHyperionHandler::buildNormalDeviceCommand(DeviceCommandId_t *id) {
+	return HasReturnvaluesIF::RETURN_OK;
+}
+
+ReturnValue_t GPSHyperionHandler::buildCommandFromCommand(
+		DeviceCommandId_t deviceCommand, const uint8_t *commandData,
+		size_t commandDataLen) {
+	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::info << "GPSHandler::scanForReply: Received " << len << " bytes" << std::endl;
+        if (internalState == InternalStates::WAIT_FIRST_MESSAGE) {
+            // TODO: Check whether data is valid by chcking 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);
+            }
+            gpsSet.latitude.value = gpsData.latitude;
+            gpsSet.longitude.value = gpsData.latitude;
+            gpsSet.fixMode.value = gpsData.fix_mode;
+#if FSFW_HAL_DEBUG_HYPERION_GPS == 1
+            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);
+#endif
+        }
+        *foundLen = len;
+    }
+	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) {
+	return HasReturnvaluesIF::RETURN_OK;
+}
+
+void GPSHyperionHandler::fillCommandAndReplyMap() {
+    // Reply length does not matter, packets should always arrive periodically
+    insertInReplyMap(GpsHyperion::GPS_REPLY, 4, nullptr, 0, true);
+}
+
+void GPSHyperionHandler::modeChanged() {
+    internalState = InternalStates::NONE;
+}