From 5c535784e984fa3970180ce6bd2d02290202532a Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 26 Jan 2022 17:59:31 +0100 Subject: [PATCH] added back old GPS handler --- bsp_q7s/boardconfig/busConf.h | 12 +- bsp_q7s/core/ObjectFactory.cpp | 4 +- linux/obc/PapbVcInterface.cpp | 1 + linux/obc/PapbVcInterface.h | 1 + linux/obc/PdecHandler.cpp | 5 +- linux/obc/Ptme.cpp | 1 + linux/obc/PtmeAxiConfig.cpp | 57 ++-- linux/obc/PtmeAxiConfig.h | 44 +-- linux/obc/PtmeRateSetter.h | 3 +- mission/devices/CMakeLists.txt | 2 +- mission/devices/GPSHyperionHandler.cpp | 251 ++++++++++-------- mission/devices/GPSHyperionHandler.h | 55 ++-- .../devices/GPSHyperionLinuxController.cpp | 178 +++++++++++++ mission/devices/GPSHyperionLinuxController.h | 55 ++++ mission/devices/GomspaceDeviceHandler.cpp | 6 +- mission/devices/GomspaceDeviceHandler.h | 2 +- mission/devices/GyroADIS1650XHandler.cpp | 8 +- mission/devices/PDU1Handler.cpp | 5 +- mission/devices/PDU2Handler.cpp | 5 +- .../devicedefinitions/GPSDefinitions.h | 2 +- mission/tmtc/CCSDSHandler.cpp | 2 +- mission/tmtc/CCSDSHandler.h | 4 +- 22 files changed, 487 insertions(+), 216 deletions(-) create mode 100644 mission/devices/GPSHyperionLinuxController.cpp create mode 100644 mission/devices/GPSHyperionLinuxController.h diff --git a/bsp_q7s/boardconfig/busConf.h b/bsp_q7s/boardconfig/busConf.h index 3e8a4b49..7f8453c9 100644 --- a/bsp_q7s/boardconfig/busConf.h +++ b/bsp_q7s/boardconfig/busConf.h @@ -21,12 +21,12 @@ static constexpr char UIO_PTME[] = "/dev/uio1"; static constexpr int MAP_ID_PTME_CONFIG = 3; namespace uiomapids { - static const int PTME_VC0 = 0; - static const int PTME_VC1 = 1; - static const int PTME_VC2 = 2; - static const int PTME_VC3 = 3; - static const int PTME_CONFIG = 4; -} +static const int PTME_VC0 = 0; +static const int PTME_VC1 = 1; +static const int PTME_VC2 = 2; +static const int PTME_VC3 = 3; +static const int PTME_CONFIG = 4; +} // namespace uiomapids namespace gpioNames { static constexpr char GYRO_0_ADIS_CS[] = "gyro_0_adis_chip_select"; diff --git a/bsp_q7s/core/ObjectFactory.cpp b/bsp_q7s/core/ObjectFactory.cpp index 1b82d206..49c5f209 100644 --- a/bsp_q7s/core/ObjectFactory.cpp +++ b/bsp_q7s/core/ObjectFactory.cpp @@ -45,7 +45,7 @@ #include "linux/devices/devicedefinitions/SusDefinitions.h" #include "mission/core/GenericFactory.h" #include "mission/devices/ACUHandler.h" -#include "mission/devices/GPSHyperionHandler.h" +#include "mission/devices/GPSHyperionLinuxController.h" #include "mission/devices/GyroADIS1650XHandler.h" #include "mission/devices/HeaterHandler.h" #include "mission/devices/IMTQHandler.h" @@ -78,10 +78,10 @@ #include #include #include +#include #include #include #include -#include ResetArgs resetArgsGnss0; ResetArgs resetArgsGnss1; diff --git a/linux/obc/PapbVcInterface.cpp b/linux/obc/PapbVcInterface.cpp index 45358ee1..5636975a 100644 --- a/linux/obc/PapbVcInterface.cpp +++ b/linux/obc/PapbVcInterface.cpp @@ -1,5 +1,6 @@ #include #include + #include "fsfw/serviceinterface/ServiceInterface.h" PapbVcInterface::PapbVcInterface(LinuxLibgpioIF* gpioComIF, gpioId_t papbBusyId, diff --git a/linux/obc/PapbVcInterface.h b/linux/obc/PapbVcInterface.h index d4df659e..0d6382e3 100644 --- a/linux/obc/PapbVcInterface.h +++ b/linux/obc/PapbVcInterface.h @@ -3,6 +3,7 @@ #include #include + #include "OBSWConfig.h" #include "fsfw/returnvalues/HasReturnvaluesIF.h" #include "linux/obc/VcInterfaceIF.h" diff --git a/linux/obc/PdecHandler.cpp b/linux/obc/PdecHandler.cpp index 7dfcfd66..f97c3965 100644 --- a/linux/obc/PdecHandler.cpp +++ b/linux/obc/PdecHandler.cpp @@ -1,9 +1,12 @@ +#include "PdecHandler.h" + #include #include + #include #include + #include "OBSWConfig.h" -#include "PdecHandler.h" #include "fsfw/ipc/QueueFactory.h" #include "fsfw/objectmanager/ObjectManager.h" #include "fsfw/serviceinterface/ServiceInterface.h" diff --git a/linux/obc/Ptme.cpp b/linux/obc/Ptme.cpp index 8258fea0..68ba3924 100644 --- a/linux/obc/Ptme.cpp +++ b/linux/obc/Ptme.cpp @@ -2,6 +2,7 @@ #include #include #include + #include "PtmeConfig.h" #include "fsfw/serviceinterface/ServiceInterface.h" diff --git a/linux/obc/PtmeAxiConfig.cpp b/linux/obc/PtmeAxiConfig.cpp index 3750b831..0619ca38 100644 --- a/linux/obc/PtmeAxiConfig.cpp +++ b/linux/obc/PtmeAxiConfig.cpp @@ -1,41 +1,40 @@ #include "PtmeAxiConfig.h" + #include "fsfw/serviceinterface/ServiceInterface.h" #include "fsfw_hal/linux/uio/UioMapper.h" -PtmeAxiConfig::PtmeAxiConfig(object_id_t objectId, std::string configAxiUio, int mapNum) : - SystemObject(objectId), configAxiUio(configAxiUio), mapNum(mapNum) { - mutex = MutexFactory::instance()->createMutex(); - if (mutex == nullptr) { - sif::warning << "Failed to create mutex" << std::endl; - } +PtmeAxiConfig::PtmeAxiConfig(object_id_t objectId, std::string configAxiUio, int mapNum) + : SystemObject(objectId), configAxiUio(configAxiUio), mapNum(mapNum) { + mutex = MutexFactory::instance()->createMutex(); + if (mutex == nullptr) { + sif::warning << "Failed to create mutex" << std::endl; + } } -PtmeAxiConfig::~PtmeAxiConfig() { -} +PtmeAxiConfig::~PtmeAxiConfig() {} ReturnValue_t PtmeAxiConfig::initialize() { - ReturnValue_t result = HasReturnvaluesIF::RETURN_OK; - UioMapper uioMapper(configAxiUio, mapNum); - result = uioMapper.getMappedAdress(&baseAddress, UioMapper::Permissions::READ_WRITE); - if (result != HasReturnvaluesIF::RETURN_OK) { - return result; - } - return HasReturnvaluesIF::RETURN_OK; + ReturnValue_t result = HasReturnvaluesIF::RETURN_OK; + UioMapper uioMapper(configAxiUio, mapNum); + result = uioMapper.getMappedAdress(&baseAddress, UioMapper::Permissions::READ_WRITE); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + return HasReturnvaluesIF::RETURN_OK; } ReturnValue_t PtmeAxiConfig::writeCaduRateReg(uint8_t rateVal) { - ReturnValue_t result = HasReturnvaluesIF::RETURN_OK; - result = mutex->lockMutex(timeoutType, mutexTimeout); - if (result != HasReturnvaluesIF::RETURN_OK) { - sif::warning << "PtmeAxiConfig::writeCaduRateReg: Failed to lock mutex" << std::endl; - return HasReturnvaluesIF::RETURN_FAILED; - } - *(baseAddress + CADU_BITRATE_REG) = static_cast(rateVal); - result = mutex->unlockMutex(); - if (result != HasReturnvaluesIF::RETURN_OK) { - sif::warning << "PtmeAxiConfig::writeCaduRateReg: Failed to unlock mutex" << std::endl; - return HasReturnvaluesIF::RETURN_FAILED; - } - return HasReturnvaluesIF::RETURN_OK; + ReturnValue_t result = HasReturnvaluesIF::RETURN_OK; + result = mutex->lockMutex(timeoutType, mutexTimeout); + if (result != HasReturnvaluesIF::RETURN_OK) { + sif::warning << "PtmeAxiConfig::writeCaduRateReg: Failed to lock mutex" << std::endl; + return HasReturnvaluesIF::RETURN_FAILED; + } + *(baseAddress + CADU_BITRATE_REG) = static_cast(rateVal); + result = mutex->unlockMutex(); + if (result != HasReturnvaluesIF::RETURN_OK) { + sif::warning << "PtmeAxiConfig::writeCaduRateReg: Failed to unlock mutex" << std::endl; + return HasReturnvaluesIF::RETURN_FAILED; + } + return HasReturnvaluesIF::RETURN_OK; } - diff --git a/linux/obc/PtmeAxiConfig.h b/linux/obc/PtmeAxiConfig.h index 1ea6c295..4638d0c0 100644 --- a/linux/obc/PtmeAxiConfig.h +++ b/linux/obc/PtmeAxiConfig.h @@ -2,9 +2,10 @@ #define LINUX_OBC_PTMEAXICONFIG_H_ #include + #include "fsfw/ipc/MutexIF.h" -#include "fsfw/returnvalues/HasReturnvaluesIF.h" #include "fsfw/objectmanager/SystemObject.h" +#include "fsfw/returnvalues/HasReturnvaluesIF.h" /** * @brief Class providing low level access to the configuration interface of the PTME. @@ -12,31 +13,30 @@ * @author J. Meier */ class PtmeAxiConfig : public SystemObject { -public: - /** - * @brief Constructor - * @param configAxiUio Device file of UIO belonging to the AXI configuration interface. - * @param mapNum Number of map belonging to axi configuration interface. - */ - PtmeAxiConfig(object_id_t objectId, std::string configAxiUio, int mapNum); - virtual ~PtmeAxiConfig(); + public: + /** + * @brief Constructor + * @param configAxiUio Device file of UIO belonging to the AXI configuration interface. + * @param mapNum Number of map belonging to axi configuration interface. + */ + PtmeAxiConfig(object_id_t objectId, std::string configAxiUio, int mapNum); + virtual ~PtmeAxiConfig(); - virtual ReturnValue_t initialize() override; - ReturnValue_t writeCaduRateReg(uint8_t rateVal); + virtual ReturnValue_t initialize() override; + ReturnValue_t writeCaduRateReg(uint8_t rateVal); -private: - // Address of register storing the bitrate configuration parameter - static const uint32_t CADU_BITRATE_REG = 0x0; + private: + // Address of register storing the bitrate configuration parameter + static const uint32_t CADU_BITRATE_REG = 0x0; - std::string configAxiUio; - std::string uioMap; - int mapNum = 0; - MutexIF* mutex = nullptr; - MutexIF::TimeoutType timeoutType = MutexIF::TimeoutType::WAITING; - uint32_t mutexTimeout = 20; - - uint32_t* baseAddress = nullptr; + std::string configAxiUio; + std::string uioMap; + int mapNum = 0; + MutexIF* mutex = nullptr; + MutexIF::TimeoutType timeoutType = MutexIF::TimeoutType::WAITING; + uint32_t mutexTimeout = 20; + uint32_t* baseAddress = nullptr; }; #endif /* LINUX_OBC_PTMEAXICONFIG_H_ */ diff --git a/linux/obc/PtmeRateSetter.h b/linux/obc/PtmeRateSetter.h index a5b1a8db..44ac9b48 100644 --- a/linux/obc/PtmeRateSetter.h +++ b/linux/obc/PtmeRateSetter.h @@ -2,10 +2,10 @@ #define LINUX_OBC_PTMERATESETTER_H_ #include "TxRateSetterIF.h" +#include "fsfw/objectmanager/SystemObject.h" #include "fsfw/returnvalues/HasReturnvaluesIF.h" #include "linux/obc/PtmeAxiConfig.h" #include "linux/obc/PtmeConfig.h" -#include "fsfw/objectmanager/SystemObject.h" /** * @brief Class to set the downlink bit rate by writing to the AXI configuration interface of the @@ -31,7 +31,6 @@ class PtmeRateSetter : public TxRateSetterIF, public SystemObject, public HasRet virtual ReturnValue_t setRate(uint32_t bitRate); private: - static const uint8_t INTERFACE_ID = CLASS_ID::RATE_SETTER; //! [EXPORT] : [COMMENT] The commanded rate is not supported by the current FPGA design diff --git a/mission/devices/CMakeLists.txt b/mission/devices/CMakeLists.txt index ed811ed3..9491a3aa 100644 --- a/mission/devices/CMakeLists.txt +++ b/mission/devices/CMakeLists.txt @@ -1,5 +1,5 @@ target_sources(${TARGET_NAME} PUBLIC - GPSHyperionHandler.cpp + GPSHyperionLinuxController.cpp GomspaceDeviceHandler.cpp Tmp1075Handler.cpp PCDUHandler.cpp diff --git a/mission/devices/GPSHyperionHandler.cpp b/mission/devices/GPSHyperionHandler.cpp index 0517f296..d6928bb5 100644 --- a/mission/devices/GPSHyperionHandler.cpp +++ b/mission/devices/GPSHyperionHandler.cpp @@ -3,41 +3,58 @@ #include "devicedefinitions/GPSDefinitions.h" #include "fsfw/datapool/PoolReadGuard.h" #include "fsfw/timemanager/Clock.h" - -#include +#include "lwgps/lwgps.h" #if FSFW_DEV_HYPERION_GPS_CREATE_NMEA_CSV == 1 #include #include #endif -GPSHyperionHandler::GPSHyperionHandler(object_id_t objectId, object_id_t parentId, - bool debugHyperionGps) - : ExtendedControllerBase(objectId, objects::NO_OBJECT), +GPSHyperionHandler::GPSHyperionHandler(object_id_t objectId, object_id_t deviceCommunication, + CookieIF *comCookie, bool debugHyperionGps) + : DeviceHandlerBase(objectId, deviceCommunication, comCookie), gpsSet(this), - myGpsmm(GPSD_SHARED_MEMORY, nullptr), - debugHyperionGps(debugHyperionGps) {} + debugHyperionGps(debugHyperionGps) { + lwgps_init(&gpsData); +} GPSHyperionHandler::~GPSHyperionHandler() {} -void GPSHyperionHandler::performControlOperation() { -#ifdef FSFW_OSAL_LINUX - readGpsDataFromGpsd(); -#endif +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; + } + } } -LocalPoolDataSetBase *GPSHyperionHandler::getDataSetHandle(sid_t sid) { - return &gpsSet; +void GPSHyperionHandler::doShutDown() { + internalState = InternalStates::NONE; + commandExecuted = false; + setMode(_MODE_POWER_DOWN); } -ReturnValue_t GPSHyperionHandler::checkModeCommand(Mode_t mode, Submode_t submode, - uint32_t *msToReachTheMode) { - return HasReturnvaluesIF::RETURN_OK; +ReturnValue_t GPSHyperionHandler::buildTransitionDeviceCommand(DeviceCommandId_t *id) { + return NOTHING_TO_SEND; } -ReturnValue_t GPSHyperionHandler::executeAction(ActionId_t actionId, MessageQueueId_t commandedBy, - 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); @@ -52,12 +69,97 @@ ReturnValue_t GPSHyperionHandler::executeAction(ActionId_t actionId, MessageQueu 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})); localDataPoolMap.emplace(GpsHyperion::LONGITUDE, new PoolEntry({0.0})); localDataPoolMap.emplace(GpsHyperion::LATITUDE, new PoolEntry({0.0})); - localDataPoolMap.emplace(GpsHyperion::SPEED, new PoolEntry({0.0})); localDataPoolMap.emplace(GpsHyperion::YEAR, new PoolEntry()); localDataPoolMap.emplace(GpsHyperion::MONTH, new PoolEntry()); localDataPoolMap.emplace(GpsHyperion::DAY, new PoolEntry()); @@ -66,113 +168,36 @@ ReturnValue_t GPSHyperionHandler::initializeLocalDataPool(localpool::DataPool &l localDataPoolMap.emplace(GpsHyperion::SECONDS, new PoolEntry()); localDataPoolMap.emplace(GpsHyperion::UNIX_SECONDS, new PoolEntry()); localDataPoolMap.emplace(GpsHyperion::SATS_IN_USE, new PoolEntry()); - localDataPoolMap.emplace(GpsHyperion::SATS_IN_VIEW, new PoolEntry()); localDataPoolMap.emplace(GpsHyperion::FIX_MODE, new PoolEntry()); poolManager.subscribeForPeriodicPacket(gpsSet.getSid(), true, 2.0, false); 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 = ExtendedControllerBase::initialize(); + ReturnValue_t result = DeviceHandlerBase::initialize(); if (result != HasReturnvaluesIF::RETURN_OK) { return result; } - return result; + // Enable reply immediately for now + return updatePeriodicReply(true, GpsHyperion::GPS_REPLY); } -ReturnValue_t GPSHyperionHandler::handleCommandMessage(CommandMessage *message) { - return ExtendedControllerBase::handleCommandMessage(message); +ReturnValue_t GPSHyperionHandler::acceptExternalDeviceCommands() { + return DeviceHandlerBase::acceptExternalDeviceCommands(); } - -#ifdef FSFW_OSAL_LINUX -void GPSHyperionHandler::readGpsDataFromGpsd() { - // The data from the device will generally be read all at once. Therefore, we - // can set all field here - if(not myGpsmm.is_open()) { - // Opening failed -#if FSFW_VERBOSE_LEVEL >= 1 - sif::warning << "GPSHyperionHandler::readGpsDataFromGpsd: Opening GPSMM failed" << std::endl; -#endif - } - gps_data_t *gps = nullptr; - gps = myGpsmm.read(); - if (gps == nullptr) { - sif::warning << "GPSHyperionHandler::readGpsDataFromGpsd: Reading GPS data failed" << std::endl; - } - PoolReadGuard pg(&gpsSet); - if (pg.getReadResult() != HasReturnvaluesIF::RETURN_OK) { -#if FSFW_VERBOSE_LEVEL >= 1 - sif::warning << "GPSHyperionHandler::readGpsDataFromGpsd: Reading dataset failed" << std::endl; -#endif - } - - // 0: Not seen, 1: No fix, 2: 2D-Fix, 3: 3D-Fix - gpsSet.fixMode.value = gps->fix.mode; - if(gps->fix.mode == 0 or gps->fix.mode == 1) { - gpsSet.setValidity(false, true); - } else if (gps->satellites_used > 0) { - gpsSet.setValidity(true, true); - } - - gpsSet.satInUse.value = gps->satellites_used; - gpsSet.satInView.value = gps->satellites_visible; - - if (std::isfinite(gps->fix.latitude)) { - // Negative latitude -> South direction - gpsSet.latitude.value = gps->fix.latitude; - } else { - gpsSet.latitude.setValid(false); - } - - if (std::isfinite(gps->fix.longitude)) { - // Negative longitude -> West direction - gpsSet.longitude.value = gps->fix.longitude; - } else { - gpsSet.longitude.setValid(false); - } - - if (std::isfinite(gps->fix.altitude)) { - gpsSet.altitude.value = gps->fix.altitude; - } else { - gpsSet.altitude.setValid(false); - } - - if (std::isfinite(gps->fix.speed)) { - gpsSet.speed.value = gps->fix.speed; - } else { - gpsSet.speed.setValid(false); - } - - gpsSet.unixSeconds.value = gps->fix.time.tv_sec; - timeval time = {}; - time.tv_sec = gpsSet.unixSeconds.value; - time.tv_usec = gps->fix.time.tv_nsec / 1000; - Clock::TimeOfDay_t timeOfDay = {}; - Clock::convertTimevalToTimeOfDay(&time, &timeOfDay); - gpsSet.year = timeOfDay.year; - gpsSet.month = timeOfDay.month; - gpsSet.day = timeOfDay.day; - gpsSet.hours = timeOfDay.hour; - gpsSet.minutes = timeOfDay.minute; - gpsSet.seconds = timeOfDay.second; - debugHyperionGps = true; - if (debugHyperionGps) { - sif::info << "-- Hyperion GPS Data --" << std::endl; - time_t timeRaw = gps->fix.time.tv_sec; - std::tm *time = gmtime(&timeRaw); - std::cout << "Time: " << std::put_time(time, "%c %Z") << std::endl; - std::cout << "Visible satellites: " << gps->satellites_visible << std::endl; - std::cout << "Satellites used: " << gps->satellites_used << std::endl; - std::cout << "Fix (0:Not Seen|1:No Fix|2:2D|3:3D): " << gps->fix.mode << std::endl; - std::cout << "Latitude: " << gps->fix.latitude << std::endl; - std::cout << "Longitude: " << gps->fix.longitude << std::endl; - std::cout << "Altitude(MSL): " << gps->fix.altMSL << std::endl; - std::cout << "Speed(m/s): " << gps->fix.speed << std::endl; - } -} -#endif diff --git a/mission/devices/GPSHyperionHandler.h b/mission/devices/GPSHyperionHandler.h index c5251794..b5734f23 100644 --- a/mission/devices/GPSHyperionHandler.h +++ b/mission/devices/GPSHyperionHandler.h @@ -3,13 +3,8 @@ #include "devicedefinitions/GPSDefinitions.h" #include "fsfw/FSFW.h" -#include "fsfw/controller/ExtendedControllerBase.h" #include "fsfw/devicehandlers/DeviceHandlerBase.h" - -#ifdef FSFW_OSAL_LINUX -#include -#include -#endif +#include "lwgps/lwgps.h" /** * @brief Device handler for the Hyperion HT-GPS200 device @@ -17,36 +12,50 @@ * Flight manual: * https://egit.irs.uni-stuttgart.de/redmine/projects/eive-flight-manual/wiki/Hyperion_HT-GPS200 */ -class GPSHyperionHandler : public ExtendedControllerBase { +class GPSHyperionHandler : public DeviceHandlerBase { public: - GPSHyperionHandler(object_id_t objectId, object_id_t parentId, bool debugHyperionGps = false); + GPSHyperionHandler(object_id_t objectId, object_id_t deviceCommunication, CookieIF *comCookie, + bool debugHyperionGps = false); virtual ~GPSHyperionHandler(); - using gpioResetFunction_t = ReturnValue_t (*)(void* args); + using gpioResetFunction_t = ReturnValue_t (*)(void *args); + + void setResetPinTriggerFunction(gpioResetFunction_t resetCallback, void *args); + ReturnValue_t acceptExternalDeviceCommands() override; - void setResetPinTriggerFunction(gpioResetFunction_t resetCallback, void* args); - 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; + void *resetCallbackArgs = nullptr; - ReturnValue_t initializeLocalDataPool(localpool::DataPool& localDataPoolMap, - LocalDataPoolManager& poolManager) override; + 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; - gpsmm myGpsmm; bool debugHyperionGps = false; - - void readGpsDataFromGpsd(); }; #endif /* MISSION_DEVICES_GPSHYPERIONHANDLER_H_ */ diff --git a/mission/devices/GPSHyperionLinuxController.cpp b/mission/devices/GPSHyperionLinuxController.cpp new file mode 100644 index 00000000..0955e1c7 --- /dev/null +++ b/mission/devices/GPSHyperionLinuxController.cpp @@ -0,0 +1,178 @@ +#include "GPSHyperionLinuxController.h" + +#include + +#include "devicedefinitions/GPSDefinitions.h" +#include "fsfw/datapool/PoolReadGuard.h" +#include "fsfw/timemanager/Clock.h" + +#if FSFW_DEV_HYPERION_GPS_CREATE_NMEA_CSV == 1 +#include +#include +#endif + +GPSHyperionLinuxController::GPSHyperionLinuxController(object_id_t objectId, object_id_t parentId, + bool debugHyperionGps) + : ExtendedControllerBase(objectId, objects::NO_OBJECT), + gpsSet(this), + myGpsmm(GPSD_SHARED_MEMORY, nullptr), + debugHyperionGps(debugHyperionGps) {} + +GPSHyperionLinuxController::~GPSHyperionLinuxController() {} + +void GPSHyperionLinuxController::performControlOperation() { +#ifdef FSFW_OSAL_LINUX + readGpsDataFromGpsd(); +#endif +} + +LocalPoolDataSetBase *GPSHyperionLinuxController::getDataSetHandle(sid_t sid) { return &gpsSet; } + +ReturnValue_t GPSHyperionLinuxController::checkModeCommand(Mode_t mode, Submode_t submode, + uint32_t *msToReachTheMode) { + return HasReturnvaluesIF::RETURN_OK; +} + +ReturnValue_t GPSHyperionLinuxController::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); + // Set HK entries invalid + gpsSet.setValidity(false, true); + resetCallback(resetCallbackArgs); + return HasActionsIF::EXECUTION_FINISHED; + } + return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED; + } + } + return HasReturnvaluesIF::RETURN_OK; +} + +ReturnValue_t GPSHyperionLinuxController::initializeLocalDataPool( + 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})); + localDataPoolMap.emplace(GpsHyperion::SPEED, new PoolEntry({0.0})); + localDataPoolMap.emplace(GpsHyperion::YEAR, new PoolEntry()); + localDataPoolMap.emplace(GpsHyperion::MONTH, new PoolEntry()); + localDataPoolMap.emplace(GpsHyperion::DAY, new PoolEntry()); + localDataPoolMap.emplace(GpsHyperion::HOURS, new PoolEntry()); + localDataPoolMap.emplace(GpsHyperion::MINUTES, new PoolEntry()); + localDataPoolMap.emplace(GpsHyperion::SECONDS, new PoolEntry()); + localDataPoolMap.emplace(GpsHyperion::UNIX_SECONDS, new PoolEntry()); + localDataPoolMap.emplace(GpsHyperion::SATS_IN_USE, new PoolEntry()); + localDataPoolMap.emplace(GpsHyperion::SATS_IN_VIEW, new PoolEntry()); + localDataPoolMap.emplace(GpsHyperion::FIX_MODE, new PoolEntry()); + poolManager.subscribeForPeriodicPacket(gpsSet.getSid(), true, 2.0, false); + return HasReturnvaluesIF::RETURN_OK; +} + +void GPSHyperionLinuxController::setResetPinTriggerFunction(gpioResetFunction_t resetCallback, + void *args) { + this->resetCallback = resetCallback; + resetCallbackArgs = args; +} + +ReturnValue_t GPSHyperionLinuxController::initialize() { + ReturnValue_t result = ExtendedControllerBase::initialize(); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + return result; +} + +ReturnValue_t GPSHyperionLinuxController::handleCommandMessage(CommandMessage *message) { + return ExtendedControllerBase::handleCommandMessage(message); +} + +#ifdef FSFW_OSAL_LINUX +void GPSHyperionLinuxController::readGpsDataFromGpsd() { + // The data from the device will generally be read all at once. Therefore, we + // can set all field here + if (not myGpsmm.is_open()) { + // Opening failed +#if FSFW_VERBOSE_LEVEL >= 1 + sif::warning << "GPSHyperionHandler::readGpsDataFromGpsd: Opening GPSMM failed" << std::endl; +#endif + } + gps_data_t *gps = nullptr; + gps = myGpsmm.read(); + if (gps == nullptr) { + sif::warning << "GPSHyperionHandler::readGpsDataFromGpsd: Reading GPS data failed" << std::endl; + } + PoolReadGuard pg(&gpsSet); + if (pg.getReadResult() != HasReturnvaluesIF::RETURN_OK) { +#if FSFW_VERBOSE_LEVEL >= 1 + sif::warning << "GPSHyperionHandler::readGpsDataFromGpsd: Reading dataset failed" << std::endl; +#endif + } + + // 0: Not seen, 1: No fix, 2: 2D-Fix, 3: 3D-Fix + gpsSet.fixMode.value = gps->fix.mode; + if (gps->fix.mode == 0 or gps->fix.mode == 1) { + gpsSet.setValidity(false, true); + } else if (gps->satellites_used > 0) { + gpsSet.setValidity(true, true); + } + + gpsSet.satInUse.value = gps->satellites_used; + gpsSet.satInView.value = gps->satellites_visible; + + if (std::isfinite(gps->fix.latitude)) { + // Negative latitude -> South direction + gpsSet.latitude.value = gps->fix.latitude; + } else { + gpsSet.latitude.setValid(false); + } + + if (std::isfinite(gps->fix.longitude)) { + // Negative longitude -> West direction + gpsSet.longitude.value = gps->fix.longitude; + } else { + gpsSet.longitude.setValid(false); + } + + if (std::isfinite(gps->fix.altitude)) { + gpsSet.altitude.value = gps->fix.altitude; + } else { + gpsSet.altitude.setValid(false); + } + + if (std::isfinite(gps->fix.speed)) { + gpsSet.speed.value = gps->fix.speed; + } else { + gpsSet.speed.setValid(false); + } + + gpsSet.unixSeconds.value = gps->fix.time.tv_sec; + timeval time = {}; + time.tv_sec = gpsSet.unixSeconds.value; + time.tv_usec = gps->fix.time.tv_nsec / 1000; + Clock::TimeOfDay_t timeOfDay = {}; + Clock::convertTimevalToTimeOfDay(&time, &timeOfDay); + gpsSet.year = timeOfDay.year; + gpsSet.month = timeOfDay.month; + gpsSet.day = timeOfDay.day; + gpsSet.hours = timeOfDay.hour; + gpsSet.minutes = timeOfDay.minute; + gpsSet.seconds = timeOfDay.second; + debugHyperionGps = true; + if (debugHyperionGps) { + sif::info << "-- Hyperion GPS Data --" << std::endl; + time_t timeRaw = gps->fix.time.tv_sec; + std::tm *time = gmtime(&timeRaw); + std::cout << "Time: " << std::put_time(time, "%c %Z") << std::endl; + std::cout << "Visible satellites: " << gps->satellites_visible << std::endl; + std::cout << "Satellites used: " << gps->satellites_used << std::endl; + std::cout << "Fix (0:Not Seen|1:No Fix|2:2D|3:3D): " << gps->fix.mode << std::endl; + std::cout << "Latitude: " << gps->fix.latitude << std::endl; + std::cout << "Longitude: " << gps->fix.longitude << std::endl; + std::cout << "Altitude(MSL): " << gps->fix.altMSL << std::endl; + std::cout << "Speed(m/s): " << gps->fix.speed << std::endl; + } +} +#endif diff --git a/mission/devices/GPSHyperionLinuxController.h b/mission/devices/GPSHyperionLinuxController.h new file mode 100644 index 00000000..57da40e6 --- /dev/null +++ b/mission/devices/GPSHyperionLinuxController.h @@ -0,0 +1,55 @@ +#ifndef MISSION_DEVICES_GPSHYPERIONHANDLER_H_ +#define MISSION_DEVICES_GPSHYPERIONHANDLER_H_ + +#include "devicedefinitions/GPSDefinitions.h" +#include "fsfw/FSFW.h" +#include "fsfw/controller/ExtendedControllerBase.h" +#include "fsfw/devicehandlers/DeviceHandlerBase.h" + +#ifdef FSFW_OSAL_LINUX +#include +#include +#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 + * This device handler can only be used on Linux system where the gpsd daemon with shared memory + * export is running. + */ +class GPSHyperionLinuxController : public ExtendedControllerBase { + public: + GPSHyperionLinuxController(object_id_t objectId, object_id_t parentId, + bool debugHyperionGps = false); + virtual ~GPSHyperionLinuxController(); + + using gpioResetFunction_t = ReturnValue_t (*)(void* args); + + void setResetPinTriggerFunction(gpioResetFunction_t resetCallback, void* args); + 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; + + ReturnValue_t initializeLocalDataPool(localpool::DataPool& localDataPoolMap, + LocalDataPoolManager& poolManager) override; + + private: + GpsPrimaryDataset gpsSet; + gpsmm myGpsmm; + bool debugHyperionGps = false; + + void readGpsDataFromGpsd(); +}; + +#endif /* MISSION_DEVICES_GPSHYPERIONHANDLER_H_ */ diff --git a/mission/devices/GomspaceDeviceHandler.cpp b/mission/devices/GomspaceDeviceHandler.cpp index ce86d13a..4cec651e 100644 --- a/mission/devices/GomspaceDeviceHandler.cpp +++ b/mission/devices/GomspaceDeviceHandler.cpp @@ -187,8 +187,8 @@ void GomspaceDeviceHandler::setNormalDatapoolEntriesInvalid() {} ReturnValue_t GomspaceDeviceHandler::generateSetParamCommand(const uint8_t* commandData, size_t commandDataLen) { - ReturnValue_t result = setParamCacher.deSerialize(&commandData, &commandDataLen, - SerializeIF::Endianness::BIG); + ReturnValue_t result = + setParamCacher.deSerialize(&commandData, &commandDataLen, SerializeIF::Endianness::BIG); if (result != HasReturnvaluesIF::RETURN_OK) { sif::error << "GomspaceDeviceHandler: Failed to deserialize set parameter " "message" @@ -346,7 +346,7 @@ ReturnValue_t GomspaceDeviceHandler::childCommandHook(DeviceCommandId_t cmd, } ReturnValue_t GomspaceDeviceHandler::setParamCallback(SetParamMessageUnpacker& unpacker, - bool afterExecution) { + bool afterExecution) { return HasReturnvaluesIF::RETURN_OK; } diff --git a/mission/devices/GomspaceDeviceHandler.h b/mission/devices/GomspaceDeviceHandler.h index efe18ee5..3d50b27b 100644 --- a/mission/devices/GomspaceDeviceHandler.h +++ b/mission/devices/GomspaceDeviceHandler.h @@ -126,7 +126,7 @@ class GomspaceDeviceHandler : public DeviceHandlerBase { * execution * @return */ - virtual ReturnValue_t setParamCallback(SetParamMessageUnpacker& unpacker, bool afterExecution); + virtual ReturnValue_t setParamCallback(SetParamMessageUnpacker &unpacker, bool afterExecution); /** * @brief Function to generate the command to get a parameter from a diff --git a/mission/devices/GyroADIS1650XHandler.cpp b/mission/devices/GyroADIS1650XHandler.cpp index 717a1bfd..0965e265 100644 --- a/mission/devices/GyroADIS1650XHandler.cpp +++ b/mission/devices/GyroADIS1650XHandler.cpp @@ -55,7 +55,7 @@ void GyroADIS1650XHandler::doStartUp() { } if (internalState == InternalState::IDLE) { - if(goToNormalMode) { + if (goToNormalMode) { setMode(MODE_NORMAL); } else { setMode(MODE_ON); @@ -211,7 +211,7 @@ ReturnValue_t GyroADIS1650XHandler::interpretDeviceReply(DeviceCommandId_t id, if (((adisType == ADIS1650X::Type::ADIS16507) and (readProdId != ADIS1650X::PROD_ID_16507)) or ((adisType == ADIS1650X::Type::ADIS16505) and (readProdId != ADIS1650X::PROD_ID_16505))) { #if OBSW_VERBOSE_LEVEL >= 1 - if(warningSwitch) { + if (warningSwitch) { sif::warning << "GyroADIS1650XHandler::interpretDeviceReply: Invalid product ID " << readProdId << std::endl; } @@ -327,9 +327,7 @@ ReturnValue_t GyroADIS1650XHandler::handleSensorData(const uint8_t *packet) { return HasReturnvaluesIF::RETURN_OK; } -uint32_t GyroADIS1650XHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { - return 6000; -} +uint32_t GyroADIS1650XHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return 6000; } void GyroADIS1650XHandler::prepareWriteCommand(uint8_t startReg, uint8_t valueOne, uint8_t valueTwo) { diff --git a/mission/devices/PDU1Handler.cpp b/mission/devices/PDU1Handler.cpp index d1276c7f..48b79551 100644 --- a/mission/devices/PDU1Handler.cpp +++ b/mission/devices/PDU1Handler.cpp @@ -73,10 +73,11 @@ void PDU1Handler::assignChannelHookFunction(GOMSPACE::ChannelSwitchHook hook, vo this->hookArgs = args; } -ReturnValue_t PDU1Handler::setParamCallback(SetParamMessageUnpacker &unpacker, bool afterExecution) { +ReturnValue_t PDU1Handler::setParamCallback(SetParamMessageUnpacker &unpacker, + bool afterExecution) { using namespace PDU1; GOMSPACE::Pdu pdu = GOMSPACE::Pdu::PDU1; - if(not afterExecution) { + if (not afterExecution) { return HasReturnvaluesIF::RETURN_OK; } if (channelSwitchHook != nullptr and unpacker.getParameterSize() == 1) { diff --git a/mission/devices/PDU2Handler.cpp b/mission/devices/PDU2Handler.cpp index 61e9bb6a..776cae58 100644 --- a/mission/devices/PDU2Handler.cpp +++ b/mission/devices/PDU2Handler.cpp @@ -427,10 +427,11 @@ void PDU2Handler::printHkTable() { << std::endl; } -ReturnValue_t PDU2Handler::setParamCallback(SetParamMessageUnpacker &unpacker, bool afterExecution) { +ReturnValue_t PDU2Handler::setParamCallback(SetParamMessageUnpacker &unpacker, + bool afterExecution) { using namespace PDU2; GOMSPACE::Pdu pdu = GOMSPACE::Pdu::PDU2; - if(not afterExecution) { + if (not afterExecution) { return HasReturnvaluesIF::RETURN_OK; } if (channelSwitchHook != nullptr and unpacker.getParameterSize() == 1) { diff --git a/mission/devices/devicedefinitions/GPSDefinitions.h b/mission/devices/devicedefinitions/GPSDefinitions.h index ce58f680..5a565fea 100644 --- a/mission/devices/devicedefinitions/GPSDefinitions.h +++ b/mission/devices/devicedefinitions/GPSDefinitions.h @@ -55,7 +55,7 @@ class GpsPrimaryDataset : public StaticLocalDataSet<18> { lp_var_t(sid.objectId, GpsHyperion::UNIX_SECONDS, this); private: - friend class GPSHyperionHandler; + friend class GPSHyperionLinuxController; GpsPrimaryDataset(HasLocalDataPoolIF* hkOwner) : StaticLocalDataSet(hkOwner, GpsHyperion::DATASET_ID) {} }; diff --git a/mission/tmtc/CCSDSHandler.cpp b/mission/tmtc/CCSDSHandler.cpp index 5c7e19ff..bb31723b 100644 --- a/mission/tmtc/CCSDSHandler.cpp +++ b/mission/tmtc/CCSDSHandler.cpp @@ -218,7 +218,7 @@ ReturnValue_t CCSDSHandler::executeAction(ActionId_t actionId, MessageQueueId_t return COMMAND_NOT_IMPLEMENTED; } if (result != RETURN_OK) { - return result; + return result; } return EXECUTION_FINISHED; } diff --git a/mission/tmtc/CCSDSHandler.h b/mission/tmtc/CCSDSHandler.h index d2782630..95c22a78 100644 --- a/mission/tmtc/CCSDSHandler.h +++ b/mission/tmtc/CCSDSHandler.h @@ -103,8 +103,8 @@ class CCSDSHandler : public SystemObject, // syrlinks must not be transmitting more than 15 minutes (according to datasheet) static const uint32_t TRANSMITTER_TIMEOUT = 900000; // 900000 ms = 15 min #else - // Set to high value when not sending via syrlinks - static const uint32_t TRANSMITTER_TIMEOUT = 86400000; // 1 day + // Set to high value when not sending via syrlinks + static const uint32_t TRANSMITTER_TIMEOUT = 86400000; // 1 day #endif /* OBSW_SYRLINKS_DOWNLINK == 0 */ static const bool UP = true;