eive-obsw/mission/devices/GPSHyperionHandler.cpp

#include "GPSHyperionHandler.h"
#include "devicedefinitions/GPSDefinitions.h"

#include "fsfw/datapool/PoolReadGuard.h"
#include "fsfw/timemanager/Clock.h"

#include <gps.h>
#include <libgpsmm.h>
#include <cmath>

#if FSFW_DEV_HYPERION_GPS_CREATE_NMEA_CSV == 1
#include <filesystem>
#include <fstream>
#endif

GPSHyperionHandler::GPSHyperionHandler(object_id_t objectId, object_id_t parentId,
        bool debugHyperionGps):
        ExtendedControllerBase(objectId, objects::NO_OBJECT), gpsSet(this),
        debugHyperionGps(debugHyperionGps) {
}

GPSHyperionHandler::~GPSHyperionHandler() {}

void GPSHyperionHandler::performControlOperation() {
    readGpsDataFromGpsd();
}

LocalPoolDataSetBase* GPSHyperionHandler::getDataSetHandle(sid_t sid) {
    return nullptr;
}

ReturnValue_t GPSHyperionHandler::checkModeCommand(Mode_t mode, Submode_t submode,
    uint32_t *msToReachTheMode) {
    return HasReturnvaluesIF::RETURN_OK;
}

ReturnValue_t GPSHyperionHandler::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 GPSHyperionHandler::initializeLocalDataPool(
        localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) {
    localDataPoolMap.emplace(GpsHyperion::ALTITUDE, new PoolEntry<double>({0.0}));
    localDataPoolMap.emplace(GpsHyperion::LONGITUDE, new PoolEntry<double>({0.0}));
    localDataPoolMap.emplace(GpsHyperion::LATITUDE, new PoolEntry<double>({0.0}));
    localDataPoolMap.emplace(GpsHyperion::YEAR, new PoolEntry<uint16_t>());
    localDataPoolMap.emplace(GpsHyperion::MONTH, new PoolEntry<uint8_t>());
    localDataPoolMap.emplace(GpsHyperion::DAY, new PoolEntry<uint8_t>());
    localDataPoolMap.emplace(GpsHyperion::HOURS, new PoolEntry<uint8_t>());
    localDataPoolMap.emplace(GpsHyperion::MINUTES, new PoolEntry<uint8_t>());
    localDataPoolMap.emplace(GpsHyperion::SECONDS, new PoolEntry<uint8_t>());
    localDataPoolMap.emplace(GpsHyperion::UNIX_SECONDS, new PoolEntry<uint32_t>());
    localDataPoolMap.emplace(GpsHyperion::SATS_IN_USE, new PoolEntry<uint8_t>());
    localDataPoolMap.emplace(GpsHyperion::FIX_MODE, new PoolEntry<uint8_t>());
    poolManager.subscribeForPeriodicPacket(gpsSet.getSid(), true, 2.0, false);
    return HasReturnvaluesIF::RETURN_OK;
}

void GPSHyperionHandler::setResetPinTriggerFunction(gpioResetFunction_t resetCallback,
        void *args) {
    this->resetCallback = resetCallback;
    resetCallbackArgs = args;
}


ReturnValue_t GPSHyperionHandler::initialize() {
    ReturnValue_t result = ExtendedControllerBase::initialize();
    if(result != HasReturnvaluesIF::RETURN_OK) {
        return result;
    }
    return result;
}

ReturnValue_t GPSHyperionHandler::handleCommandMessage(CommandMessage *message) {
    return ExtendedControllerBase::handleCommandMessage(message);
}

void GPSHyperionHandler::readGpsDataFromGpsd() {
    // The data from the device will generally be read all at once. Therefore, we
    // can set all field here
    gpsmm gpsmm(GPSD_SHARED_MEMORY, 0);
    gps_data_t* gps;
    gps = gpsmm.read();
    if(gps == nullptr) {
        sif::warning << "Q7STestTask: Reading GPS data failed" << std::endl;
    }
    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((gps->set & MODE_SET) != MODE_SET) {
        // Could not even set mode
        gpsSet.setValidity(false, true);
        return;
    }

    if(gps->satellites_used > 0) {
        gpsSet.setValidity(true, true);
    }

    gpsSet.satInUse.value = gps->satellites_used;
    gpsSet.satInView.value = gps->satellites_visible;

    // 0: Not seen, 1: No fix, 2: 2D-Fix, 3: 3D-Fix
    gpsSet.fixMode = gps->fix.mode;
    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;
    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;
    }
}