eive-obsw/mission/devices/GPSHyperionHandler.cpp

203 lines
7.9 KiB
C++
Raw Normal View History

2021-06-24 11:42:40 +02:00
#include "GPSHyperionHandler.h"
2021-06-15 17:07:47 +02:00
#include "devicedefinitions/GPSDefinitions.h"
2020-12-22 00:32:11 +01:00
2021-06-24 11:42:40 +02:00
#include "fsfw/datapool/PoolReadGuard.h"
2021-06-24 13:55:51 +02:00
#include "fsfw/timemanager/Clock.h"
2021-06-24 11:42:40 +02:00
2021-06-16 17:19:14 +02:00
#include "lwgps/lwgps.h"
2021-09-07 16:11:02 +02:00
#if FSFW_DEV_HYPERION_GPS_CREATE_NMEA_CSV == 1
#include <filesystem>
#include <fstream>
#endif
2021-06-24 11:42:40 +02:00
GPSHyperionHandler::GPSHyperionHandler(object_id_t objectId, object_id_t deviceCommunication,
CookieIF *comCookie, bool debugHyperionGps):
DeviceHandlerBase(objectId, deviceCommunication, comCookie), gpsSet(this),
debugHyperionGps(debugHyperionGps) {
2021-06-16 17:19:14 +02:00
lwgps_init(&gpsData);
2020-12-22 00:32:11 +01:00
}
2021-06-24 11:42:40 +02:00
GPSHyperionHandler::~GPSHyperionHandler() {}
2020-12-22 00:32:11 +01:00
2021-06-24 11:42:40 +02:00
void GPSHyperionHandler::doStartUp() {
2021-06-15 16:44:31 +02:00
if(internalState == InternalStates::NONE) {
commandExecuted = false;
internalState = InternalStates::WAIT_FIRST_MESSAGE;
}
2020-12-22 00:32:11 +01:00
2021-06-15 16:44:31 +02:00
if(internalState == InternalStates::WAIT_FIRST_MESSAGE) {
if(commandExecuted) {
internalState = InternalStates::IDLE;
setMode(MODE_ON);
commandExecuted = false;
}
}
2020-12-22 00:32:11 +01:00
}
2021-06-24 11:42:40 +02:00
void GPSHyperionHandler::doShutDown() {
2021-06-15 16:44:31 +02:00
internalState = InternalStates::NONE;
commandExecuted = false;
setMode(_MODE_POWER_DOWN);
2020-12-22 00:32:11 +01:00
}
2021-06-24 11:42:40 +02:00
ReturnValue_t GPSHyperionHandler::buildTransitionDeviceCommand(DeviceCommandId_t *id) {
2021-08-22 20:25:05 +02:00
return NOTHING_TO_SEND;
2020-12-22 00:32:11 +01:00
}
2021-06-24 11:42:40 +02:00
ReturnValue_t GPSHyperionHandler::buildNormalDeviceCommand(DeviceCommandId_t *id) {
2021-08-22 20:25:05 +02:00
return NOTHING_TO_SEND;
2020-12-22 00:32:11 +01:00
}
2021-06-24 11:42:40 +02:00
ReturnValue_t GPSHyperionHandler::buildCommandFromCommand(
DeviceCommandId_t deviceCommand, const uint8_t *commandData,
size_t commandDataLen) {
2021-09-08 16:03:20 +02:00
// By default, send nothing
rawPacketLen = 0;
2021-09-07 16:11:02 +02:00
switch(deviceCommand) {
case(GpsHyperion::TRIGGER_RESET_PIN): {
2021-09-07 16:14:54 +02:00
if(resetCallback != nullptr) {
2021-09-07 16:11:02 +02:00
PoolReadGuard pg(&gpsSet);
// Set HK entries invalid
gpsSet.setValidity(false, true);
// The user needs to implement this. Don't touch states for now, the device should
// quickly reboot and send valid strings again.
2021-09-08 17:02:37 +02:00
actionHelper.finish(true, getCommanderQueueId(deviceCommand), deviceCommand);
2021-09-07 16:14:54 +02:00
return resetCallback(resetCallbackArgs);
2021-09-07 16:11:02 +02:00
}
return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED;
}
}
return HasReturnvaluesIF::RETURN_OK;
2020-12-22 00:32:11 +01:00
}
2021-06-24 11:42:40 +02:00
ReturnValue_t GPSHyperionHandler::scanForReply(const uint8_t *start, size_t len,
DeviceCommandId_t *foundId, size_t *foundLen) {
2021-06-16 17:19:14 +02:00
// Pass data to GPS library
2021-06-23 15:18:31 +02:00
if(len > 0) {
2021-09-07 16:11:02 +02:00
sif::debug << "GPSHandler::scanForReply: Received " << len << " bytes" << std::endl;
if (internalState == InternalStates::WAIT_FIRST_MESSAGE) {
2021-09-07 16:11:02 +02:00
// TODO: Check whether data is valid by checking whether NMEA start string is valid?
commandExecuted = true;
}
2021-06-23 15:18:31 +02:00
int result = lwgps_process(&gpsData, start, len);
if(result != 1) {
2021-06-23 15:18:31 +02:00
sif::warning << "GPSHandler::scanForReply: Issue processing GPS data with lwgps"
<< std::endl;
}
else {
2021-06-24 11:42:40 +02:00
// 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
2021-06-24 11:42:40 +02:00
if(gpsData.is_valid) {
// Set all entries valid now, set invalid on case basis if values are sanitized
gpsSet.setValidity(true, true);
}
2021-06-24 13:55:51 +02:00
// Negative latitude -> South direction
2021-06-24 11:42:40 +02:00
gpsSet.latitude.value = gpsData.latitude;
2021-06-24 13:55:51 +02:00
// Negative longitude -> West direction
2021-09-06 18:27:33 +02:00
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;
}
2021-06-24 11:42:40 +02:00
gpsSet.fixMode.value = gpsData.fix_mode;
2021-06-24 13:55:51 +02:00
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;
2021-08-11 18:52:33 +02:00
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);
}
2021-09-07 12:00:22 +02:00
#if FSFW_DEV_HYPERION_GPS_CREATE_NMEA_CSV == 1
2021-09-07 16:11:02 +02:00
std::string filename = "/mnt/sd0/gps_log_070921.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(reinterpret_cast<const char*>(start), len);
2021-09-07 12:00:22 +02:00
#endif
}
*foundLen = len;
2021-09-06 12:07:15 +02:00
*foundId = GpsHyperion::GPS_REPLY;
2021-06-16 17:19:14 +02:00
}
return HasReturnvaluesIF::RETURN_OK;
2021-06-16 17:19:14 +02:00
2020-12-22 00:32:11 +01:00
}
2021-06-24 11:42:40 +02:00
ReturnValue_t GPSHyperionHandler::interpretDeviceReply(DeviceCommandId_t id,
const uint8_t *packet) {
return HasReturnvaluesIF::RETURN_OK;
2020-12-22 00:32:11 +01:00
}
2021-06-24 11:42:40 +02:00
uint32_t GPSHyperionHandler::getTransitionDelayMs(Mode_t from, Mode_t to) {
2021-09-06 18:42:58 +02:00
return 5000;
2020-12-22 00:32:11 +01:00
}
2021-06-24 11:42:40 +02:00
ReturnValue_t GPSHyperionHandler::initializeLocalDataPool(
localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) {
2021-06-24 13:55:51 +02:00
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;
2020-12-22 00:32:11 +01:00
}
2021-06-24 11:42:40 +02:00
void GPSHyperionHandler::fillCommandAndReplyMap() {
2021-06-15 17:07:47 +02:00
// Reply length does not matter, packets should always arrive periodically
2021-08-11 19:27:17 +02:00
insertInReplyMap(GpsHyperion::GPS_REPLY, 4, &gpsSet, 0, true);
2021-09-08 16:03:20 +02:00
insertInCommandMap(GpsHyperion::TRIGGER_RESET_PIN);
2020-12-22 00:32:11 +01:00
}
2021-06-24 11:42:40 +02:00
void GPSHyperionHandler::modeChanged() {
2021-06-15 17:07:47 +02:00
internalState = InternalStates::NONE;
2020-12-22 00:32:11 +01:00
}
2021-08-20 20:18:56 +02:00
2021-09-07 16:14:54 +02:00
void GPSHyperionHandler::setResetPinTriggerFunction(gpioResetFunction_t resetCallback,
2021-09-07 16:11:02 +02:00
void *args) {
2021-09-07 16:14:54 +02:00
this->resetCallback = resetCallback;
resetCallbackArgs = args;
2021-09-07 16:11:02 +02:00
}
2021-08-20 20:18:56 +02:00
void GPSHyperionHandler::debugInterface(uint8_t positionTracker, object_id_t objectId,
uint32_t parameter) {
}