eive-obsw/linux/acs/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 enableHkSets, bool debugHyperionGps)
    : ExtendedControllerBase(objectId),
      gpsSet(this),
      skyviewSet(this),
      enableHkSets(enableHkSets),
      debugHyperionGps(debugHyperionGps) {}

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

LocalPoolDataSetBase *GpsHyperionLinuxController::getDataSetHandle(sid_t sid) {
  switch (sid.ownerSetId) {
    case GpsHyperion::CORE_DATASET:
      return &gpsSet;
    case GpsHyperion::SKYVIEW_DATASET:
      return &skyviewSet;
    default:
      return nullptr;
  }
  return nullptr;
}

ReturnValue_t GpsHyperionLinuxController::checkModeCommand(Mode_t mode, Submode_t submode,
                                                           uint32_t *msToReachTheMode) {
  if (mode == MODE_ON) {
    maxTimeToReachFix.resetTimer();
  } else if (mode == MODE_NORMAL) {
    return HasModesIF::INVALID_MODE;
  }
  if (mode == MODE_OFF) {
    maxTimeToReachFix.timeOut();
    gainedNewFix.timeOut();
    PoolReadGuard pg(&gpsSet);
    gpsSet.setValidity(false, true);
    // The ctrl is off, so it cannot detect the data from the devices.
    handleFixChangedEvent(GpsHyperion::FixMode::NOT_SEEN);
    gpsSet.fixMode.value = GpsHyperion::FixMode::NOT_SEEN;
    oneShotSwitches.reset();
  }
  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);
        ReturnValue_t result = resetCallback(data, size, resetCallbackArgs);
        if (result != returnvalue::OK) {
          return result;
        }
        return HasActionsIF::EXECUTION_FINISHED;
      }
      return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED;
    }
  }
  return HasActionsIF::INVALID_ACTION_ID;
}

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(), enableHkSets, 30.0});
  localDataPoolMap.emplace(GpsHyperion::SKYVIEW_UNIX_SECONDS, new PoolEntry<double>());
  localDataPoolMap.emplace(GpsHyperion::PRN_ID, new PoolEntry<int16_t>());
  localDataPoolMap.emplace(GpsHyperion::AZIMUTH, new PoolEntry<int16_t>());
  localDataPoolMap.emplace(GpsHyperion::ELEVATION, new PoolEntry<int16_t>());
  localDataPoolMap.emplace(GpsHyperion::SIGNAL2NOISE, new PoolEntry<double>());
  localDataPoolMap.emplace(GpsHyperion::USED, new PoolEntry<uint8_t>());
  poolManager.subscribeForRegularPeriodicPacket({skyviewSet.getSid(), false, 120.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;
  }
  unsigned int readIdx = 0;
  if (readMode == ReadModes::SOCKET) {
    // Poll the GPS.
    while (gps_waiting(&gps, 0)) {
      int retval = gps_read(&gps);
      if (retval < 0) {
        readError();
        return false;
      }
      readIdx++;
      if (readIdx >= 40) {
        sif::warning << "GpsHyperionLinuxController: Received " << readIdx
                     << " GPSD message consecutively" << std::endl;
        break;
      }
    }
    if (readIdx > 0) {
      oneShotSwitches.gpsReadFailedSwitch = true;
      handleGpsReadData();
    }
  } else if (readMode == ReadModes::SHM) {
    sif::error << "GpsHyperionLinuxController::readGpsDataFromGpsdPermanentLoop: "
                  "SHM read not implemented"
               << std::endl;
  }
  return false;
}

ReturnValue_t GpsHyperionLinuxController::handleGpsReadData() {
  bool modeIsSet = true;
  if (MODE_SET != (MODE_SET & gps.set)) {
    if (mode != MODE_OFF) {
      modeIsSet = false;
    } else {
      // GPS ctrl is off anyway, so do other handling
      return returnvalue::FAILED;
    }
  }
  ReturnValue_t result = handleCoreTelemetry(modeIsSet);
  if (result != returnvalue::OK) {
    return result;
  }
  result = handleSkyviewTelemetry();
  return result;
}

ReturnValue_t GpsHyperionLinuxController::handleCoreTelemetry(bool modeIsSet) {
  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;
  uint8_t newFix = 0;
  if (modeIsSet) {
    // 0: Not seen, 1: No fix, 2: 2D-Fix, 3: 3D-Fix
    if (gps.fix.mode == GpsHyperion::FixMode::FIX_2D or
        gps.fix.mode == GpsHyperion::FixMode::FIX_3D) {
      validFix = true;
      maxTimeToReachFix.resetTimer();
    }
    newFix = gps.fix.mode;
  }
  if (gpsSet.fixMode.value != newFix) {
    handleFixChangedEvent(newFix);
  }
  gpsSet.fixMode = newFix;
  gpsSet.fixMode.setValid(modeIsSet);
  // We are supposed to be on and functioning, but no fix was found
  if (not validFix) {
    if (maxTimeToReachFix.hasTimedOut()) {
      // Set HK entries invalid
      gpsSet.setValidity(false, true);
      if (oneShotSwitches.cantGetFixSwitch) {
        sif::warning << "GpsHyperionLinuxController: No fix detected in allowed "
                     << maxTimeToReachFix.getTimeoutMs() / 1000 << " seconds" << std::endl;
        triggerEvent(GpsHyperion::CANT_GET_FIX, maxTimeToReachFix.getTimeoutMs());
        oneShotSwitches.cantGetFixSwitch = false;
        // Try resetting the devices
        if (resetCallback != nullptr) {
          uint8_t chip = GpsHyperion::GnssChip::A_SIDE;
          ReturnValue_t result = resetCallback(&chip, 1, resetCallbackArgs);
          if (result != returnvalue::OK) {
            triggerEvent(GpsHyperion::RESET_FAIL, chip);
          }
          chip = GpsHyperion::GnssChip::B_SIDE;
          result = resetCallback(&chip, 1, resetCallbackArgs);
          if (result != returnvalue::OK) {
            triggerEvent(GpsHyperion::RESET_FAIL, chip);
          }
        }
      }
    }
  }

  // Only set on specific messages, so only set a valid flag to invalid
  // if not set for more than a full message set (10 messages here)
  if (SATELLITE_SET == (SATELLITE_SET & gps.set)) {
    gpsSet.satInUse.value = gps.satellites_used;
    gpsSet.satInView.value = gps.satellites_visible;
    if (not gpsSet.satInUse.isValid()) {
      gpsSet.satInUse.setValid(true);
      gpsSet.satInView.setValid(true);
    }
    satNotSetCounter = 0;
  } else {
    if (satNotSetCounter < 10) {
      satNotSetCounter++;
    } else {
      gpsSet.satInUse.value = 0;
      gpsSet.satInUse.setValid(false);
      gpsSet.satInView.value = 0;
      gpsSet.satInView.setValid(false);
    }
  }

  // LATLON is set for every message, no need for a counter
  bool latValid = false;
  bool longValid = false;
  if (modeIsSet) {
    if (LATLON_SET == (LATLON_SET & gps.set)) {
      if (std::isfinite(gps.fix.latitude)) {
        // Negative latitude -> South direction
        gpsSet.latitude.value = gps.fix.latitude;
        // As specified in gps.h: Only valid if mode >= 2
        if (gps.fix.mode >= GpsHyperion::FixMode::FIX_2D) {
          latValid = true;
        }
      }

      if (std::isfinite(gps.fix.longitude)) {
        // Negative longitude -> West direction
        gpsSet.longitude.value = gps.fix.longitude;
        // As specified in gps.h: Only valid if mode >= 2
        if (gps.fix.mode >= GpsHyperion::FixMode::FIX_2D) {
          longValid = true;
        }
      }
    }
  }
  gpsSet.latitude.setValid(latValid);
  gpsSet.longitude.setValid(longValid);

  // ALTITUDE is set for every message, no need for a counter
  bool altitudeValid = false;
  if (modeIsSet) {
    if (ALTITUDE_SET == (ALTITUDE_SET & gps.set) && std::isfinite(gps.fix.altitude)) {
      gpsSet.altitude.value = gps.fix.altitude;
      // As specified in gps.h: Only valid if mode == 3
      if (gps.fix.mode == GpsHyperion::FixMode::FIX_3D) {
        altitudeValid = true;
      }
    }
  }
  gpsSet.altitude.setValid(altitudeValid);

  // SPEED is set for every message, no need for a counter
  bool speedValid = false;
  if (modeIsSet) {
    if (SPEED_SET == (SPEED_SET & gps.set) && std::isfinite(gps.fix.speed)) {
      gpsSet.speed.value = gps.fix.speed;
      speedValid = true;
    }
  }
  gpsSet.speed.setValid(speedValid);

  // TIME is set for every message, no need for a counter
  bool timeValid = false;
  if (TIME_SET == (TIME_SET & gps.set)) {
    // To prevent totally incorrect times from being declared valid.
    if (gpsSet.satInView.isValid() and gpsSet.satInView.value >= 1) {
      timeValid = true;
    }
    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;
  }
  gpsSet.unixSeconds.setValid(timeValid);
  gpsSet.year.setValid(timeValid);
  gpsSet.month.setValid(timeValid);
  gpsSet.day.setValid(timeValid);
  gpsSet.hours.setValid(timeValid);
  gpsSet.minutes.setValid(timeValid);
  gpsSet.seconds.setValid(timeValid);

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

ReturnValue_t GpsHyperionLinuxController::handleSkyviewTelemetry() {
  PoolReadGuard pg(&skyviewSet);
  if (pg.getReadResult() != returnvalue::OK) {
    return returnvalue::FAILED;
  }
  skyviewSet.unixSeconds.value = gps.skyview_time;
  for (int sat = 0; sat < GpsHyperion::MAX_SATELLITES; sat++) {
    skyviewSet.prn_id.value[sat] = gps.skyview[sat].PRN;
    skyviewSet.azimuth.value[sat] = gps.skyview[sat].azimuth;
    skyviewSet.elevation.value[sat] = gps.skyview[sat].elevation;
    skyviewSet.signal2noise.value[sat] = gps.skyview[sat].ss;
    skyviewSet.used.value[sat] = gps.skyview[sat].used;
  }
  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;
  }
}

void GpsHyperionLinuxController::handleFixChangedEvent(uint8_t newFix) {
  if (gainedNewFix.hasTimedOut()) {
    triggerEvent(GpsHyperion::GPS_FIX_CHANGE, newFix, fixChangeCounter);
    fixChangeCounter = 0;
  }
  fixChangeCounter++;
  gainedNewFix.resetTimer();
}