more ACS cleaning
- STR code is now in mission folder, but still only conpiled for Q7S code because wire library is proprietary
This commit is contained in:
382
linux/acs/GpsHyperionLinuxController.cpp
Normal file
382
linux/acs/GpsHyperionLinuxController.cpp
Normal file
@ -0,0 +1,382 @@
|
||||
#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) {}
|
||||
|
||||
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;
|
||||
ReturnValue_t result = handleGpsReadData();
|
||||
if (result == returnvalue::OK) {
|
||||
return true;
|
||||
} else {
|
||||
return false;
|
||||
}
|
||||
noModeSetCntr = 0;
|
||||
} else {
|
||||
return false;
|
||||
}
|
||||
} else if (readMode == ReadModes::SHM) {
|
||||
sif::error << "GpsHyperionLinuxController::readGpsDataFromGpsdPermanentLoop: "
|
||||
"SHM read not implemented"
|
||||
<< std::endl;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
ReturnValue_t GpsHyperionLinuxController::handleGpsReadData() {
|
||||
bool modeIsSet = true;
|
||||
if (MODE_SET != (MODE_SET & gps.set)) {
|
||||
if (mode != MODE_OFF) {
|
||||
if (maxTimeToReachFix.hasTimedOut() and oneShotSwitches.cantGetFixSwitch) {
|
||||
sif::warning << "GpsHyperionLinuxController: No mode could be set in allowed "
|
||||
<< maxTimeToReachFix.getTimeoutMs() / 1000 << " seconds" << std::endl;
|
||||
triggerEvent(GpsHyperion::CANT_GET_FIX, maxTimeToReachFix.getTimeoutMs());
|
||||
oneShotSwitches.cantGetFixSwitch = false;
|
||||
}
|
||||
modeIsSet = false;
|
||||
} else {
|
||||
// GPS device is off anyway, so do other handling
|
||||
return returnvalue::FAILED;
|
||||
}
|
||||
}
|
||||
|
||||
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 == 2 or gps.fix.mode == 3) {
|
||||
validFix = true;
|
||||
}
|
||||
newFix = gps.fix.mode;
|
||||
if (newFix == 0 or newFix == 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;
|
||||
}
|
||||
}
|
||||
}
|
||||
if (gpsSet.fixMode.value != newFix) {
|
||||
triggerEvent(GpsHyperion::GPS_FIX_CHANGE, gpsSet.fixMode.value, newFix);
|
||||
}
|
||||
gpsSet.fixMode = newFix;
|
||||
gpsSet.fixMode.setValid(modeIsSet);
|
||||
|
||||
// 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 {
|
||||
satNotSetCounter++;
|
||||
if (gpsSet.satInUse.isValid() and satNotSetCounter >= 10) {
|
||||
gpsSet.satInUse.setValid(false);
|
||||
gpsSet.satInView.setValid(false);
|
||||
}
|
||||
}
|
||||
|
||||
// LATLON is set for every message, no need for a counter
|
||||
bool latValid = false;
|
||||
bool longValid = false;
|
||||
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 >= 2) {
|
||||
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 >= 2) {
|
||||
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 (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 == 3) {
|
||||
altitudeValid = true;
|
||||
}
|
||||
}
|
||||
gpsSet.altitude.setValid(altitudeValid);
|
||||
|
||||
// SPEED is set for every message, no need for a counter
|
||||
bool speedValid = false;
|
||||
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;
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user