|
|
|
@@ -4,8 +4,6 @@
|
|
|
|
|
#include "fsfw/datapool/PoolReadGuard.h"
|
|
|
|
|
#include "fsfw/timemanager/Clock.h"
|
|
|
|
|
|
|
|
|
|
#include "lwgps/lwgps.h"
|
|
|
|
|
|
|
|
|
|
#if FSFW_DEV_HYPERION_GPS_CREATE_NMEA_CSV == 1
|
|
|
|
|
#include <filesystem>
|
|
|
|
|
#include <fstream>
|
|
|
|
@@ -13,48 +11,88 @@
|
|
|
|
|
|
|
|
|
|
GPSHyperionHandler::GPSHyperionHandler(object_id_t objectId, object_id_t deviceCommunication,
|
|
|
|
|
CookieIF *comCookie, bool debugHyperionGps):
|
|
|
|
|
DeviceHandlerBase(objectId, deviceCommunication, comCookie), gpsSet(this),
|
|
|
|
|
ExtendedControllerBase(objectId, objects::NO_OBJECT), gpsSet(this),
|
|
|
|
|
debugHyperionGps(debugHyperionGps) {
|
|
|
|
|
lwgps_init(&gpsData);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
GPSHyperionHandler::~GPSHyperionHandler() {}
|
|
|
|
|
|
|
|
|
|
void GPSHyperionHandler::doStartUp() {
|
|
|
|
|
if(internalState == InternalStates::NONE) {
|
|
|
|
|
commandExecuted = false;
|
|
|
|
|
internalState = InternalStates::WAIT_FIRST_MESSAGE;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
if(internalState == InternalStates::WAIT_FIRST_MESSAGE) {
|
|
|
|
|
if(commandExecuted) {
|
|
|
|
|
internalState = InternalStates::IDLE;
|
|
|
|
|
setMode(MODE_ON);
|
|
|
|
|
commandExecuted = false;
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
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
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
void GPSHyperionHandler::doShutDown() {
|
|
|
|
|
internalState = InternalStates::NONE;
|
|
|
|
|
commandExecuted = false;
|
|
|
|
|
setMode(_MODE_POWER_DOWN);
|
|
|
|
|
LocalPoolDataSetBase* GPSHyperionHandler::getDataSetHandle(sid_t sid) {
|
|
|
|
|
return nullptr;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
ReturnValue_t GPSHyperionHandler::buildTransitionDeviceCommand(DeviceCommandId_t *id) {
|
|
|
|
|
return NOTHING_TO_SEND;
|
|
|
|
|
ReturnValue_t GPSHyperionHandler::checkModeCommand(Mode_t mode, Submode_t submode,
|
|
|
|
|
uint32_t *msToReachTheMode) {
|
|
|
|
|
return HasReturnvaluesIF::RETURN_OK;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
ReturnValue_t GPSHyperionHandler::buildNormalDeviceCommand(DeviceCommandId_t *id) {
|
|
|
|
|
return NOTHING_TO_SEND;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
ReturnValue_t GPSHyperionHandler::buildCommandFromCommand(
|
|
|
|
|
DeviceCommandId_t deviceCommand, const uint8_t *commandData,
|
|
|
|
|
size_t commandDataLen) {
|
|
|
|
|
// By default, send nothing
|
|
|
|
|
rawPacketLen = 0;
|
|
|
|
|
switch(deviceCommand) {
|
|
|
|
|
ReturnValue_t GPSHyperionHandler::executeAction(ActionId_t actionId, MessageQueueId_t commandedBy,
|
|
|
|
|
const uint8_t *data, size_t size) {
|
|
|
|
|
switch(actionId) {
|
|
|
|
|
case(GpsHyperion::TRIGGER_RESET_PIN): {
|
|
|
|
|
if(resetCallback != nullptr) {
|
|
|
|
|
PoolReadGuard pg(&gpsSet);
|
|
|
|
@@ -69,99 +107,6 @@ ReturnValue_t GPSHyperionHandler::buildCommandFromCommand(
|
|
|
|
|
return HasReturnvaluesIF::RETURN_OK;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
ReturnValue_t GPSHyperionHandler::scanForReply(const uint8_t *start, size_t len,
|
|
|
|
|
DeviceCommandId_t *foundId, size_t *foundLen) {
|
|
|
|
|
// Pass data to GPS library
|
|
|
|
|
if(len > 0) {
|
|
|
|
|
// sif::debug << "GPSHandler::scanForReply: Received " << len << " bytes" << std::endl;
|
|
|
|
|
if (internalState == InternalStates::WAIT_FIRST_MESSAGE) {
|
|
|
|
|
// TODO: Check whether data is valid by checking whether NMEA start string is valid?
|
|
|
|
|
commandExecuted = true;
|
|
|
|
|
}
|
|
|
|
|
int result = lwgps_process(&gpsData, start, len);
|
|
|
|
|
if(result != 1) {
|
|
|
|
|
sif::warning << "GPSHandler::scanForReply: Issue processing GPS data with lwgps"
|
|
|
|
|
<< std::endl;
|
|
|
|
|
}
|
|
|
|
|
else {
|
|
|
|
|
// 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
|
|
|
|
|
}
|
|
|
|
|
*foundLen = len;
|
|
|
|
|
*foundId = GpsHyperion::GPS_REPLY;
|
|
|
|
|
}
|
|
|
|
|
return HasReturnvaluesIF::RETURN_OK;
|
|
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
ReturnValue_t GPSHyperionHandler::interpretDeviceReply(DeviceCommandId_t id,
|
|
|
|
|
const uint8_t *packet) {
|
|
|
|
|
return HasReturnvaluesIF::RETURN_OK;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
uint32_t GPSHyperionHandler::getTransitionDelayMs(Mode_t from, Mode_t to) {
|
|
|
|
|
return 5000;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
ReturnValue_t GPSHyperionHandler::initializeLocalDataPool(
|
|
|
|
|
localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) {
|
|
|
|
|
localDataPoolMap.emplace(GpsHyperion::ALTITUDE, new PoolEntry<double>({0.0}));
|
|
|
|
@@ -180,35 +125,21 @@ ReturnValue_t GPSHyperionHandler::initializeLocalDataPool(
|
|
|
|
|
return HasReturnvaluesIF::RETURN_OK;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
void GPSHyperionHandler::fillCommandAndReplyMap() {
|
|
|
|
|
// Reply length does not matter, packets should always arrive periodically
|
|
|
|
|
insertInReplyMap(GpsHyperion::GPS_REPLY, 4, &gpsSet, 0, true);
|
|
|
|
|
insertInCommandMap(GpsHyperion::TRIGGER_RESET_PIN);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
void GPSHyperionHandler::modeChanged() {
|
|
|
|
|
internalState = InternalStates::NONE;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
void GPSHyperionHandler::setResetPinTriggerFunction(gpioResetFunction_t resetCallback,
|
|
|
|
|
void *args) {
|
|
|
|
|
this->resetCallback = resetCallback;
|
|
|
|
|
resetCallbackArgs = args;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
void GPSHyperionHandler::debugInterface(uint8_t positionTracker, object_id_t objectId,
|
|
|
|
|
uint32_t parameter) {
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
ReturnValue_t GPSHyperionHandler::initialize() {
|
|
|
|
|
ReturnValue_t result = DeviceHandlerBase::initialize();
|
|
|
|
|
ReturnValue_t result = ExtendedControllerBase::initialize();
|
|
|
|
|
if(result != HasReturnvaluesIF::RETURN_OK) {
|
|
|
|
|
return result;
|
|
|
|
|
}
|
|
|
|
|
// Enable reply immediately for now
|
|
|
|
|
return updatePeriodicReply(true, GpsHyperion::GPS_REPLY);
|
|
|
|
|
return result;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
ReturnValue_t GPSHyperionHandler::acceptExternalDeviceCommands() {
|
|
|
|
|
return DeviceHandlerBase::acceptExternalDeviceCommands();
|
|
|
|
|
ReturnValue_t GPSHyperionHandler::handleCommandMessage(CommandMessage *message) {
|
|
|
|
|
return ExtendedControllerBase::handleCommandMessage(message);
|
|
|
|
|
}
|
|
|
|
|