eive-obsw/mission/devices/GPSHandler.cpp

#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;
}