99 lines
3.0 KiB
C++
99 lines
3.0 KiB
C++
#include "GPSHandler.h"
|
|
#include "devicedefinitions/GPSDefinitions.h"
|
|
|
|
#include "lwgps/lwgps.h"
|
|
|
|
GPSHandler::GPSHandler(object_id_t objectId, object_id_t deviceCommunication,
|
|
CookieIF *comCookie):
|
|
DeviceHandlerBase(objectId, deviceCommunication, comCookie) {
|
|
lwgps_init(&gpsData);
|
|
}
|
|
|
|
GPSHandler::~GPSHandler() {}
|
|
|
|
void GPSHandler::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 GPSHandler::doShutDown() {
|
|
internalState = InternalStates::NONE;
|
|
commandExecuted = false;
|
|
setMode(MODE_OFF);
|
|
}
|
|
|
|
ReturnValue_t GPSHandler::buildTransitionDeviceCommand(DeviceCommandId_t *id) {
|
|
return HasReturnvaluesIF::RETURN_OK;
|
|
}
|
|
|
|
ReturnValue_t GPSHandler::buildNormalDeviceCommand(DeviceCommandId_t *id) {
|
|
return HasReturnvaluesIF::RETURN_OK;
|
|
}
|
|
|
|
ReturnValue_t GPSHandler::buildCommandFromCommand(
|
|
DeviceCommandId_t deviceCommand, const uint8_t *commandData,
|
|
size_t commandDataLen) {
|
|
return HasReturnvaluesIF::RETURN_OK;
|
|
}
|
|
|
|
ReturnValue_t GPSHandler::scanForReply(const uint8_t *start, size_t len,
|
|
DeviceCommandId_t *foundId, size_t *foundLen) {
|
|
// Pass data to GPS library
|
|
if(len > 0) {
|
|
sif::info << "GPSHandler::scanForReply: Received " << len << " bytes" << std::endl;
|
|
if (internalState == InternalStates::WAIT_FIRST_MESSAGE) {
|
|
// TODO: Check whether data is valid by chcking 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 {
|
|
sif::info << "GPS Data" << std::endl;
|
|
// Print messages
|
|
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);
|
|
}
|
|
*foundLen = len;
|
|
}
|
|
return HasReturnvaluesIF::RETURN_OK;
|
|
|
|
}
|
|
|
|
ReturnValue_t GPSHandler::interpretDeviceReply(DeviceCommandId_t id,
|
|
const uint8_t *packet) {
|
|
return HasReturnvaluesIF::RETURN_OK;
|
|
}
|
|
|
|
uint32_t GPSHandler::getTransitionDelayMs(Mode_t from, Mode_t to) {
|
|
return 5000;
|
|
}
|
|
|
|
ReturnValue_t GPSHandler::initializeLocalDataPool(
|
|
localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) {
|
|
return HasReturnvaluesIF::RETURN_OK;
|
|
}
|
|
|
|
void GPSHandler::fillCommandAndReplyMap() {
|
|
// Reply length does not matter, packets should always arrive periodically
|
|
insertInReplyMap(GpsHyperion::GPS_REPLY, 4, nullptr, 0, true);
|
|
}
|
|
|
|
void GPSHandler::modeChanged() {
|
|
internalState = InternalStates::NONE;
|
|
}
|