From 1e3b91789c9aa5148e8a176580a630ca50cf833e Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 24 Jun 2021 11:42:40 +0200 Subject: [PATCH] continued gps handler --- bsp_linux_board/ObjectFactory.cpp | 4 +- mission/devices/CMakeLists.txt | 3 +- ...{GPSHandler.cpp => GPSHyperionHandler.cpp} | 52 +++++++++++++------ .../{GPSHandler.h => GPSHyperionHandler.h} | 20 ++++--- .../devicedefinitions/GPSDefinitions.h | 24 ++++++++- 5 files changed, 74 insertions(+), 29 deletions(-) rename mission/devices/{GPSHandler.cpp => GPSHyperionHandler.cpp} (55%) rename mission/devices/{GPSHandler.h => GPSHyperionHandler.h} (72%) diff --git a/bsp_linux_board/ObjectFactory.cpp b/bsp_linux_board/ObjectFactory.cpp index f4ae00bf..c5e12868 100644 --- a/bsp_linux_board/ObjectFactory.cpp +++ b/bsp_linux_board/ObjectFactory.cpp @@ -1,6 +1,6 @@ #include #include -#include +#include #include "ObjectFactory.h" #include "objects/systemObjectList.h" @@ -157,7 +157,7 @@ void ObjectFactory::produce(void* args){ UartModes::CANONICAL, 9600, 1024); uartCookie->setToFlushInput(true); uartCookie->setReadCycles(6); - GPSHandler* gpsHandler = new GPSHandler(objects::GPS0_HANDLER, + GPSHyperionHandler* gpsHandler = new GPSHyperionHandler(objects::GPS0_HANDLER, objects::UART_COM_IF, uartCookie); gpsHandler->setStartUpImmediately(); #endif diff --git a/mission/devices/CMakeLists.txt b/mission/devices/CMakeLists.txt index 735e595b..8eb96538 100644 --- a/mission/devices/CMakeLists.txt +++ b/mission/devices/CMakeLists.txt @@ -1,6 +1,5 @@ target_sources(${TARGET_NAME} PUBLIC - GPSHandler.cpp - # GyroL3GD20Handler.cpp + GPSHyperionHandler.cpp MGMHandlerLIS3MDL.cpp MGMHandlerRM3100.cpp GomspaceDeviceHandler.cpp diff --git a/mission/devices/GPSHandler.cpp b/mission/devices/GPSHyperionHandler.cpp similarity index 55% rename from mission/devices/GPSHandler.cpp rename to mission/devices/GPSHyperionHandler.cpp index 3fdb6ab1..248b7193 100644 --- a/mission/devices/GPSHandler.cpp +++ b/mission/devices/GPSHyperionHandler.cpp @@ -1,17 +1,19 @@ -#include "GPSHandler.h" +#include "GPSHyperionHandler.h" #include "devicedefinitions/GPSDefinitions.h" +#include "fsfw/datapool/PoolReadGuard.h" + #include "lwgps/lwgps.h" -GPSHandler::GPSHandler(object_id_t objectId, object_id_t deviceCommunication, +GPSHyperionHandler::GPSHyperionHandler(object_id_t objectId, object_id_t deviceCommunication, CookieIF *comCookie): - DeviceHandlerBase(objectId, deviceCommunication, comCookie) { + DeviceHandlerBase(objectId, deviceCommunication, comCookie), gpsSet(this) { lwgps_init(&gpsData); } -GPSHandler::~GPSHandler() {} +GPSHyperionHandler::~GPSHyperionHandler() {} -void GPSHandler::doStartUp() { +void GPSHyperionHandler::doStartUp() { if(internalState == InternalStates::NONE) { commandExecuted = false; internalState = InternalStates::WAIT_FIRST_MESSAGE; @@ -26,27 +28,27 @@ void GPSHandler::doStartUp() { } } -void GPSHandler::doShutDown() { +void GPSHyperionHandler::doShutDown() { internalState = InternalStates::NONE; commandExecuted = false; setMode(MODE_OFF); } -ReturnValue_t GPSHandler::buildTransitionDeviceCommand(DeviceCommandId_t *id) { +ReturnValue_t GPSHyperionHandler::buildTransitionDeviceCommand(DeviceCommandId_t *id) { return HasReturnvaluesIF::RETURN_OK; } -ReturnValue_t GPSHandler::buildNormalDeviceCommand(DeviceCommandId_t *id) { +ReturnValue_t GPSHyperionHandler::buildNormalDeviceCommand(DeviceCommandId_t *id) { return HasReturnvaluesIF::RETURN_OK; } -ReturnValue_t GPSHandler::buildCommandFromCommand( +ReturnValue_t GPSHyperionHandler::buildCommandFromCommand( DeviceCommandId_t deviceCommand, const uint8_t *commandData, size_t commandDataLen) { return HasReturnvaluesIF::RETURN_OK; } -ReturnValue_t GPSHandler::scanForReply(const uint8_t *start, size_t len, +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) { @@ -61,12 +63,30 @@ ReturnValue_t GPSHandler::scanForReply(const uint8_t *start, size_t len, << std::endl; } else { - sif::info << "GPS Data" << std::endl; + // 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; } @@ -74,25 +94,25 @@ ReturnValue_t GPSHandler::scanForReply(const uint8_t *start, size_t len, } -ReturnValue_t GPSHandler::interpretDeviceReply(DeviceCommandId_t id, +ReturnValue_t GPSHyperionHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) { return HasReturnvaluesIF::RETURN_OK; } -uint32_t GPSHandler::getTransitionDelayMs(Mode_t from, Mode_t to) { +uint32_t GPSHyperionHandler::getTransitionDelayMs(Mode_t from, Mode_t to) { return 5000; } -ReturnValue_t GPSHandler::initializeLocalDataPool( +ReturnValue_t GPSHyperionHandler::initializeLocalDataPool( localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) { return HasReturnvaluesIF::RETURN_OK; } -void GPSHandler::fillCommandAndReplyMap() { +void GPSHyperionHandler::fillCommandAndReplyMap() { // Reply length does not matter, packets should always arrive periodically insertInReplyMap(GpsHyperion::GPS_REPLY, 4, nullptr, 0, true); } -void GPSHandler::modeChanged() { +void GPSHyperionHandler::modeChanged() { internalState = InternalStates::NONE; } diff --git a/mission/devices/GPSHandler.h b/mission/devices/GPSHyperionHandler.h similarity index 72% rename from mission/devices/GPSHandler.h rename to mission/devices/GPSHyperionHandler.h index 298871e4..5685ac86 100644 --- a/mission/devices/GPSHandler.h +++ b/mission/devices/GPSHyperionHandler.h @@ -1,20 +1,25 @@ -#ifndef MISSION_DEVICES_GPSHANDLER_H_ -#define MISSION_DEVICES_GPSHANDLER_H_ +#ifndef MISSION_DEVICES_GPSHYPERIONHANDLER_H_ +#define MISSION_DEVICES_GPSHYPERIONHANDLER_H_ -#include +#include "fsfw/devicehandlers/DeviceHandlerBase.h" +#include "devicedefinitions/GPSDefinitions.h" #include "lwgps/lwgps.h" +#ifndef FSFW_HAL_DEBUG_HYPERION_GPS +#define FSFW_HAL_DEBUG_HYPERION_GPS 0 +#endif + /** * @brief Device handler for the Hyperion HT-GPS200 device * @details * Flight manual: * https://egit.irs.uni-stuttgart.de/redmine/projects/eive-flight-manual/wiki/Hyperion_HT-GPS200 */ -class GPSHandler: public DeviceHandlerBase { +class GPSHyperionHandler: public DeviceHandlerBase { public: - GPSHandler(object_id_t objectId, object_id_t deviceCommunication, + GPSHyperionHandler(object_id_t objectId, object_id_t deviceCommunication, CookieIF* comCookie); - virtual ~GPSHandler(); + virtual ~GPSHyperionHandler(); protected: enum class InternalStates { @@ -48,6 +53,7 @@ protected: private: lwgps_t gpsData = {}; + GpsPrimaryDataset gpsSet; }; -#endif /* MISSION_DEVICES_GPSHANDLER_H_ */ +#endif /* MISSION_DEVICES_GPSHYPERIONHANDLER_H_ */ diff --git a/mission/devices/devicedefinitions/GPSDefinitions.h b/mission/devices/devicedefinitions/GPSDefinitions.h index cb3cfd1a..72433f66 100644 --- a/mission/devices/devicedefinitions/GPSDefinitions.h +++ b/mission/devices/devicedefinitions/GPSDefinitions.h @@ -1,21 +1,41 @@ #ifndef MISSION_DEVICES_DEVICEDEFINITIONS_GPSDEFINITIONS_H_ #define MISSION_DEVICES_DEVICEDEFINITIONS_GPSDEFINITIONS_H_ -#include +#include "fsfw/devicehandlers/DeviceHandlerIF.h" +#include "fsfw/datapoollocal/StaticLocalDataSet.h" namespace GpsHyperion { static constexpr DeviceCommandId_t GPS_REPLY = 0; -enum GpsPoolIds: lp_id_t { +static constexpr uint32_t DATASET_ID = 0; +enum GpsPoolIds: lp_id_t { + LATITUDE = 0, + LONGITUDE = 1, + ALTITUDE = 2, + FIX_MODE = 3, }; } class GpsPrimaryDataset: public StaticLocalDataSet<5> { public: + GpsPrimaryDataset(object_id_t gpsId): + StaticLocalDataSet(sid_t(gpsId, GpsHyperion::DATASET_ID)) { + setAllVariablesReadOnly(); + } + + lp_var_t latitude = lp_var_t(sid.objectId, + GpsHyperion::LATITUDE, this); + lp_var_t longitude = lp_var_t(sid.objectId, + GpsHyperion::LONGITUDE, this); + lp_var_t altitude = lp_var_t(sid.objectId, GpsHyperion::ALTITUDE, this); + lp_var_t fixMode = lp_var_t(sid.objectId, GpsHyperion::FIX_MODE, this); private: + friend class GPSHyperionHandler; + GpsPrimaryDataset(HasLocalDataPoolIF* hkOwner): + StaticLocalDataSet(hkOwner, GpsHyperion::DATASET_ID) {} }; #endif /* MISSION_DEVICES_DEVICEDEFINITIONS_GPSDEFINITIONS_H_ */