diff --git a/mission/devices/GPSHyperionHandler.cpp b/mission/devices/GPSHyperionHandler.cpp index 3cc95db6..bd34591a 100644 --- a/mission/devices/GPSHyperionHandler.cpp +++ b/mission/devices/GPSHyperionHandler.cpp @@ -7,9 +7,9 @@ #include "lwgps/lwgps.h" GPSHyperionHandler::GPSHyperionHandler(object_id_t objectId, object_id_t deviceCommunication, - CookieIF *comCookie, bool debugHyperionGps): - DeviceHandlerBase(objectId, deviceCommunication, comCookie), gpsSet(this), - debugHyperionGps(debugHyperionGps) { + CookieIF *comCookie, bool debugHyperionGps): + DeviceHandlerBase(objectId, deviceCommunication, comCookie), gpsSet(this), + debugHyperionGps(debugHyperionGps) { lwgps_init(&gpsData); } @@ -37,21 +37,21 @@ void GPSHyperionHandler::doShutDown() { } ReturnValue_t GPSHyperionHandler::buildTransitionDeviceCommand(DeviceCommandId_t *id) { - return HasReturnvaluesIF::RETURN_OK; + return HasReturnvaluesIF::RETURN_OK; } ReturnValue_t GPSHyperionHandler::buildNormalDeviceCommand(DeviceCommandId_t *id) { - return HasReturnvaluesIF::RETURN_OK; + return HasReturnvaluesIF::RETURN_OK; } ReturnValue_t GPSHyperionHandler::buildCommandFromCommand( - DeviceCommandId_t deviceCommand, const uint8_t *commandData, - size_t commandDataLen) { - return HasReturnvaluesIF::RETURN_OK; + 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) { + DeviceCommandId_t *foundId, size_t *foundLen) { // Pass data to GPS library if(len > 0) { sif::info << "GPSHandler::scanForReply: Received " << len << " bytes" << std::endl; @@ -111,21 +111,21 @@ ReturnValue_t GPSHyperionHandler::scanForReply(const uint8_t *start, size_t len, } *foundLen = len; } - return HasReturnvaluesIF::RETURN_OK; + return HasReturnvaluesIF::RETURN_OK; } ReturnValue_t GPSHyperionHandler::interpretDeviceReply(DeviceCommandId_t id, - const uint8_t *packet) { - return HasReturnvaluesIF::RETURN_OK; + const uint8_t *packet) { + return HasReturnvaluesIF::RETURN_OK; } uint32_t GPSHyperionHandler::getTransitionDelayMs(Mode_t from, Mode_t to) { - return 5000; + return 5000; } ReturnValue_t GPSHyperionHandler::initializeLocalDataPool( - localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) { + localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) { localDataPoolMap.emplace(GpsHyperion::ALTITUDE, new PoolEntry({0.0})); localDataPoolMap.emplace(GpsHyperion::LONGITUDE, new PoolEntry({0.0})); localDataPoolMap.emplace(GpsHyperion::LATITUDE, new PoolEntry({0.0})); @@ -138,7 +138,8 @@ ReturnValue_t GPSHyperionHandler::initializeLocalDataPool( localDataPoolMap.emplace(GpsHyperion::UNIX_SECONDS, new PoolEntry()); localDataPoolMap.emplace(GpsHyperion::SATS_IN_USE, new PoolEntry()); localDataPoolMap.emplace(GpsHyperion::FIX_MODE, new PoolEntry()); - return HasReturnvaluesIF::RETURN_OK; + poolManager.subscribeForPeriodicPacket(gpsSet.getSid(), true, 2.0, false); + return HasReturnvaluesIF::RETURN_OK; } void GPSHyperionHandler::fillCommandAndReplyMap() { diff --git a/mission/devices/GPSHyperionHandler.h b/mission/devices/GPSHyperionHandler.h index feb99a6a..49d8f416 100644 --- a/mission/devices/GPSHyperionHandler.h +++ b/mission/devices/GPSHyperionHandler.h @@ -20,6 +20,7 @@ public: virtual ~GPSHyperionHandler(); protected: + enum class InternalStates { NONE, WAIT_FIRST_MESSAGE,