eive-obsw/mission/devices/GPSHyperionHandler.cpp

#include "GPSHyperionHandler.h"

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

#ifdef FSFW_OSAL_LINUX
#include <gps.h>
#include <libgpsmm.h>
#endif

#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() {
#ifdef FSFW_OSAL_LINUX
  readGpsDataFromGpsd();
#endif
}

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

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);
}

#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
  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;
  }
}
#endif