finished GPS handler refactoring
Some checks failed
EIVE/eive-obsw/pipeline/pr-develop There was a failure building this commit
Some checks failed
EIVE/eive-obsw/pipeline/pr-develop There was a failure building this commit
This commit is contained in:
parent
858fd07c4a
commit
37ae508af5
|
|
@ -4,6 +4,10 @@
|
|||
#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>
|
||||
|
|
@ -18,67 +22,7 @@ GPSHyperionHandler::GPSHyperionHandler(object_id_t objectId, object_id_t parentI
|
|||
GPSHyperionHandler::~GPSHyperionHandler() {}
|
||||
|
||||
void GPSHyperionHandler::performControlOperation() {
|
||||
// The data from the device will generally be read all at once. Therefore, we
|
||||
// can set all field here
|
||||
// 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(gpsData.is_valid) {
|
||||
// // Set all entries valid now, set invalid on case basis if values are sanitized
|
||||
// gpsSet.setValidity(true, true);
|
||||
// }
|
||||
// // Negative latitude -> South direction
|
||||
// gpsSet.latitude.value = gpsData.latitude;
|
||||
// // Negative longitude -> West direction
|
||||
// gpsSet.longitude.value = gpsData.longitude;
|
||||
// if(gpsData.altitude > 600000.0 or gpsData.altitude < 400000.0) {
|
||||
// gpsSet.altitude.setValid(false);
|
||||
// }
|
||||
// else {
|
||||
// gpsSet.altitude.setValid(true);
|
||||
// gpsSet.altitude.value = gpsData.altitude;
|
||||
// }
|
||||
// gpsSet.fixMode.value = gpsData.fix_mode;
|
||||
// gpsSet.satInUse.value = gpsData.sats_in_use;
|
||||
// Clock::TimeOfDay_t timeStruct = {};
|
||||
// timeStruct.day = gpsData.date;
|
||||
// timeStruct.hour = gpsData.hours;
|
||||
// timeStruct.minute = gpsData.minutes;
|
||||
// timeStruct.month = gpsData.month;
|
||||
// timeStruct.second = gpsData.seconds;
|
||||
// // Convert two-digit year to full year (AD)
|
||||
// timeStruct.year = gpsData.year + 2000;
|
||||
// timeval timeval = {};
|
||||
// Clock::convertTimeOfDayToTimeval(&timeStruct, &timeval);
|
||||
// gpsSet.year = timeStruct.year;
|
||||
// gpsSet.month = gpsData.month;
|
||||
// gpsSet.day = gpsData.date;
|
||||
// gpsSet.hours = gpsData.hours;
|
||||
// gpsSet.minutes = gpsData.minutes;
|
||||
// gpsSet.seconds = gpsData.seconds;
|
||||
// gpsSet.unixSeconds = timeval.tv_sec;
|
||||
// if(debugHyperionGps) {
|
||||
// sif::info << "GPS Data" << std::endl;
|
||||
// printf("Valid status: %d\n", gpsData.is_valid);
|
||||
// printf("Latitude: %f degrees\n", gpsData.latitude);
|
||||
// printf("Longitude: %f degrees\n", gpsData.longitude);
|
||||
// printf("Altitude: %f meters\n", gpsData.altitude);
|
||||
// }
|
||||
//#if FSFW_DEV_HYPERION_GPS_CREATE_NMEA_CSV == 1
|
||||
// std::string filename = "/mnt/sd0/gps_log.txt";
|
||||
// std::ofstream gpsFile;
|
||||
// if(not std::filesystem::exists(filename)) {
|
||||
// gpsFile.open(filename, std::ofstream::out);
|
||||
// }
|
||||
// gpsFile.open(filename, std::ofstream::out | std::ofstream::app);
|
||||
// gpsFile.write("\n", 1);
|
||||
// gpsFile.write(reinterpret_cast<const char*>(start), len);
|
||||
//#endif
|
||||
readGpsDataFromGpsd();
|
||||
}
|
||||
|
||||
LocalPoolDataSetBase* GPSHyperionHandler::getDataSetHandle(sid_t sid) {
|
||||
|
|
@ -143,3 +87,88 @@ ReturnValue_t GPSHyperionHandler::initialize() {
|
|||
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;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -5,7 +5,6 @@
|
|||
#include "fsfw/devicehandlers/DeviceHandlerBase.h"
|
||||
#include "fsfw/controller/ExtendedControllerBase.h"
|
||||
#include "devicedefinitions/GPSDefinitions.h"
|
||||
#include "lwgps/lwgps.h"
|
||||
|
||||
/**
|
||||
* @brief Device handler for the Hyperion HT-GPS200 device
|
||||
|
|
@ -43,6 +42,8 @@ protected:
|
|||
private:
|
||||
GpsPrimaryDataset gpsSet;
|
||||
bool debugHyperionGps = false;
|
||||
|
||||
void readGpsDataFromGpsd();
|
||||
};
|
||||
|
||||
#endif /* MISSION_DEVICES_GPSHYPERIONHANDLER_H_ */
|
||||
|
|
|
|||
|
|
@ -15,15 +15,17 @@ enum GpsPoolIds: lp_id_t {
|
|||
LATITUDE = 0,
|
||||
LONGITUDE = 1,
|
||||
ALTITUDE = 2,
|
||||
FIX_MODE = 3,
|
||||
SATS_IN_USE = 4,
|
||||
UNIX_SECONDS = 5,
|
||||
YEAR = 6,
|
||||
MONTH = 7,
|
||||
DAY = 8,
|
||||
HOURS = 9,
|
||||
MINUTES = 10,
|
||||
SECONDS = 11
|
||||
SPEED = 3,
|
||||
FIX_MODE = 4,
|
||||
SATS_IN_USE = 5,
|
||||
SATS_IN_VIEW = 6,
|
||||
UNIX_SECONDS = 7,
|
||||
YEAR = 8,
|
||||
MONTH = 9,
|
||||
DAY = 10,
|
||||
HOURS = 11,
|
||||
MINUTES = 12,
|
||||
SECONDS = 13
|
||||
};
|
||||
|
||||
enum GpsFixModes: uint8_t {
|
||||
|
|
@ -47,8 +49,10 @@ public:
|
|||
lp_var_t<double> longitude = lp_var_t<double>(sid.objectId,
|
||||
GpsHyperion::LONGITUDE, this);
|
||||
lp_var_t<double> altitude = lp_var_t<double>(sid.objectId, GpsHyperion::ALTITUDE, this);
|
||||
lp_var_t<double> speed = lp_var_t<double>(sid.objectId, GpsHyperion::SPEED, this);
|
||||
lp_var_t<uint8_t> fixMode = lp_var_t<uint8_t>(sid.objectId, GpsHyperion::FIX_MODE, this);
|
||||
lp_var_t<uint8_t> satInUse = lp_var_t<uint8_t>(sid.objectId, GpsHyperion::SATS_IN_USE, this);
|
||||
lp_var_t<uint8_t> satInView = lp_var_t<uint8_t>(sid.objectId, GpsHyperion::SATS_IN_VIEW, this);
|
||||
lp_var_t<uint16_t> year = lp_var_t<uint16_t>(sid.objectId, GpsHyperion::YEAR, this);
|
||||
lp_var_t<uint8_t> month = lp_var_t<uint8_t>(sid.objectId, GpsHyperion::MONTH, this);
|
||||
lp_var_t<uint8_t> day = lp_var_t<uint8_t>(sid.objectId, GpsHyperion::DAY, this);
|
||||
|
|
|
|||
2
tmtc
2
tmtc
|
|
@ -1 +1 @@
|
|||
Subproject commit 8ed7a4e90f392f4f60e63659fa0115a2fe6765e1
|
||||
Subproject commit c86cd1874f605a89277e1b8fcf4496f9302c941e
|
||||
Loading…
Reference in New Issue
Block a user