eive-obsw/linux/devices/GpsHyperionLinuxController.cpp

#include "GpsHyperionLinuxController.h"

#include <fsfw/tasks/TaskFactory.h>
#include <fsfw/timemanager/Stopwatch.h>

#include "OBSWConfig.h"
#include "fsfw/FSFW.h"
#include "fsfw/datapool/PoolReadGuard.h"
#include "fsfw/timemanager/Clock.h"
#include "linux/utility/utility.h"
#include "mission/utility/compileTime.h"

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

GpsHyperionLinuxController::GpsHyperionLinuxController(object_id_t objectId, object_id_t parentId,
                                                       bool debugHyperionGps)
    : ExtendedControllerBase(objectId), gpsSet(this), debugHyperionGps(debugHyperionGps) {
  timeUpdateCd.resetTimer();
}

GpsHyperionLinuxController::~GpsHyperionLinuxController() {
  gps_stream(&gps, WATCH_DISABLE, nullptr);
  gps_close(&gps);
}

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

ReturnValue_t GpsHyperionLinuxController::checkModeCommand(Mode_t mode, Submode_t submode,
                                                           uint32_t *msToReachTheMode) {
  if (not modeCommanded) {
    if (mode == MODE_ON or mode == MODE_OFF) {
      // 5h time to reach fix
      *msToReachTheMode = MAX_SECONDS_TO_REACH_FIX;
      maxTimeToReachFix.resetTimer();
      modeCommanded = true;
    } else if (mode == MODE_NORMAL) {
      return HasModesIF::INVALID_MODE;
    }
  }
  if (mode == MODE_OFF) {
    PoolReadGuard pg(&gpsSet);
    gpsSet.setValidity(false, true);
    // There can't be a fix with a device that is off.
    triggerEvent(GpsHyperion::GPS_FIX_CHANGE, gpsSet.fixMode.value, 0);
    gpsSet.fixMode.value = 0;
    oneShotSwitches.reset();
    modeCommanded = false;
  }
  return returnvalue::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_GNSS): {
      if (resetCallback != nullptr) {
        PoolReadGuard pg(&gpsSet);
        // Set HK entries invalid
        gpsSet.setValidity(false, true);
        resetCallback(data, size, resetCallbackArgs);
        return HasActionsIF::EXECUTION_FINISHED;
      }
      return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED;
    }
  }
  return returnvalue::OK;
}

ReturnValue_t GpsHyperionLinuxController::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::SPEED, 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::SATS_IN_VIEW, new PoolEntry<uint8_t>());
  localDataPoolMap.emplace(GpsHyperion::FIX_MODE, new PoolEntry<uint8_t>());
  poolManager.subscribeForRegularPeriodicPacket({gpsSet.getSid(), false, 30.0});
  return returnvalue::OK;
}

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

ReturnValue_t GpsHyperionLinuxController::performOperation(uint8_t opCode) {
  handleQueue();
  poolManager.performHkOperation();
  while (true) {
#if OBSW_THREAD_TRACING == 1
    trace::threadTrace(opCounter, "GPS CTRL");
#endif
    bool callAgainImmediately = readGpsDataFromGpsd();
    if (not callAgainImmediately) {
      handleQueue();
      poolManager.performHkOperation();
      TaskFactory::delayTask(250);
    }
  }
  // Should never be reached.
  return returnvalue::OK;
}

ReturnValue_t GpsHyperionLinuxController::initialize() {
  ReturnValue_t result = ExtendedControllerBase::initialize();
  if (result != returnvalue::OK) {
    return result;
  }
  auto openError = [&](const char *type, int error) {
  // Opening failed
#if FSFW_VERBOSE_LEVEL >= 1
    sif::warning << "GPSHyperionHandler::readGpsDataFromGpsd: Opening GPSMM " << type
                 << " failed | Error " << error << " | " << gps_errstr(error) << std::endl;
#endif
  };
  if (readMode == ReadModes::SOCKET) {
    int retval = gps_open("localhost", DEFAULT_GPSD_PORT, &gps);
    if (retval != 0) {
      openError("Socket", retval);
      return ObjectManager::CHILD_INIT_FAILED;
    }
    gps_stream(&gps, WATCH_ENABLE | WATCH_JSON, nullptr);
  } else if (readMode == ReadModes::SHM) {
    int retval = gps_open(GPSD_SHARED_MEMORY, "", &gps);
    if (retval != 0) {
      openError("SHM", retval);
      return ObjectManager::CHILD_INIT_FAILED;
    }
  }
  return result;
}

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

void GpsHyperionLinuxController::performControlOperation() {}

bool GpsHyperionLinuxController::readGpsDataFromGpsd() {
  auto readError = [&]() {
    if (oneShotSwitches.gpsReadFailedSwitch) {
      oneShotSwitches.gpsReadFailedSwitch = false;
      sif::warning << "GPSHyperionHandler::readGpsDataFromGpsd: Reading GPS data failed | "
                      "Error "
                   << errno << " | " << gps_errstr(errno) << std::endl;
    }
  };
  // GPS is off, no point in reading data from GPSD.
  if (mode == MODE_OFF) {
    return false;
  }
  if (readMode == ReadModes::SOCKET) {
    // Poll the GPS.
    if (gps_waiting(&gps, 0)) {
      if (-1 == gps_read(&gps)) {
        readError();
        return false;
      }
      oneShotSwitches.gpsReadFailedSwitch = true;
      // did not event get mode, nothing to see.
      if (MODE_SET != (MODE_SET & gps.set)) {
        if (mode != MODE_OFF) {
          if (maxTimeToReachFix.hasTimedOut() and oneShotSwitches.cantGetFixSwitch) {
            sif::warning
                << "GPSHyperionHandler::readGpsDataFromGpsd: No mode could be set in allowed "
                << maxTimeToReachFix.timeout / 1000 << " seconds" << std::endl;
            triggerEvent(GpsHyperion::CANT_GET_FIX, maxTimeToReachFix.timeout);
            oneShotSwitches.cantGetFixSwitch = false;
          }
          // Mode is on, so do next read immediately
          return true;
        }
        // GPS device is off anyway, so do other handling
        return false;
      }
      noModeSetCntr = 0;
    } else {
      return false;
    }
  } else if (readMode == ReadModes::SHM) {
    sif::error << "GpsHyperionLinuxController::readGpsDataFromGpsdPermanentLoop: "
                  "SHM read not implemented"
               << std::endl;
  }
  handleGpsReadData();
  return true;
}

ReturnValue_t GpsHyperionLinuxController::handleGpsReadData() {
  PoolReadGuard pg(&gpsSet);
  if (pg.getReadResult() != returnvalue::OK) {
#if FSFW_VERBOSE_LEVEL >= 1
    sif::warning << "GPSHyperionHandler::readGpsDataFromGpsd: Reading dataset failed" << std::endl;
#endif
    return returnvalue::FAILED;
  }

  bool validFix = false;
  // 0: Not seen, 1: No fix, 2: 2D-Fix, 3: 3D-Fix
  if (gps.fix.mode == 2 or gps.fix.mode == 3) {
    validFix = true;
  }
  if (gpsSet.fixMode.value != gps.fix.mode) {
    triggerEvent(GpsHyperion::GPS_FIX_CHANGE, gpsSet.fixMode.value, gps.fix.mode);
  }
  gpsSet.fixMode.value = gps.fix.mode;
  if (gps.fix.mode == 0 or gps.fix.mode == 1) {
    if (modeCommanded and maxTimeToReachFix.hasTimedOut()) {
      // We are supposed to be on and functioning, but no fix was found
      if (mode == MODE_ON or mode == MODE_NORMAL) {
        mode = MODE_OFF;
      }
      modeCommanded = false;
    }
    gpsSet.setValidity(false, true);
  } else if (gps.satellites_used > 0 && validFix && mode != MODE_OFF) {
    gpsSet.setValidity(true, true);
  }

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

  bool latValid = false;
  if (std::isfinite(gps.fix.latitude)) {
    // Negative latitude -> South direction
    gpsSet.latitude.value = gps.fix.latitude;
    if (gps.fix.mode >= 2) {
      latValid = true;
    }
  }
  gpsSet.latitude.setValid(latValid);

  bool longValid = false;
  if (std::isfinite(gps.fix.longitude)) {
    // Negative longitude -> West direction
    gpsSet.longitude.value = gps.fix.longitude;
    if (gps.fix.mode >= 2) {
      longValid = true;
    }
  }
  gpsSet.latitude.setValid(longValid);

  bool altitudeValid = false;
  if (std::isfinite(gps.fix.altitude)) {
    gpsSet.altitude.value = gps.fix.altitude;
    if (gps.fix.mode == 3) {
      altitudeValid = true;
    }
  }
  gpsSet.altitude.setValid(altitudeValid);

  if (std::isfinite(gps.fix.speed)) {
    gpsSet.speed.value = gps.fix.speed;
  } else {
    gpsSet.speed.setValid(false);
  }

  if (TIME_SET == (TIME_SET & gps.set)) {
    timeval time = {};
#if LIBGPS_VERSION_MINOR <= 17
    gpsSet.unixSeconds.value = std::floor(gps.fix.time);
    double fractionalPart = gps.fix.time - gpsSet.unixSeconds.value;
    time.tv_usec = fractionalPart * 1000.0 * 1000.0;
#else
    gpsSet.unixSeconds.value = gps.fix.time.tv_sec;
    time.tv_usec = gps.fix.time.tv_nsec / 1000;
#endif
    time.tv_sec = gpsSet.unixSeconds.value;
    // If the time is totally wrong (e.g. year 2000 after system reset because we do not have a RTC
    // and no time file available) we set it with the roughly valid time from the GPS.
    // NTP might only work if the time difference between sys time and current time is not too
    // large.
    overwriteTimeIfNotSane(time, validFix);
    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;
  } else {
    gpsSet.unixSeconds.setValid(false);
    gpsSet.year.setValid(false);
    gpsSet.month.setValid(false);
    gpsSet.day.setValid(false);
    gpsSet.hours.setValid(false);
    gpsSet.minutes.setValid(false);
    gpsSet.seconds.setValid(false);
  }

  if (debugHyperionGps) {
    sif::info << "-- Hyperion GPS Data --" << std::endl;
#if LIBGPS_VERSION_MINOR <= 17
    time_t timeRaw = gpsSet.unixSeconds.value;
#else
    time_t timeRaw = gps.fix.time.tv_sec;
#endif
    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;
#if LIBGPS_VERSION_MINOR <= 17
    std::cout << "Altitude(MSL): " << gps.fix.altitude << std::endl;
#else
    std::cout << "Altitude(MSL): " << gps.fix.altMSL << std::endl;
#endif
    std::cout << "Speed(m/s): " << gps.fix.speed << std::endl;
    std::time_t t = std::time(nullptr);
    std::tm tm = *std::gmtime(&t);
    std::cout << "C Time: " << std::put_time(&tm, "%c") << std::endl;
  }
  return returnvalue::OK;
}

void GpsHyperionLinuxController::overwriteTimeIfNotSane(timeval time, bool validFix) {
  if (not timeInit and validFix) {
    if (not utility::timeSanityCheck()) {
#if OBSW_VERBOSE_LEVEL >= 1
      time_t timeRaw = time.tv_sec;
      std::tm *timeTm = std::gmtime(&timeRaw);
      sif::info << "Overwriting invalid system time from GPS data directly: "
                << std::put_time(timeTm, "%c %Z") << std::endl;
#endif
      // For some reason, the clock needs to be somewhat correct for NTP to work. Really dumb..
      Clock::setClock(&time);
    }
    timeInit = true;
  }
}