mirror of
https://github.com/peterantypas/maiana.git
synced 2025-05-31 23:00:16 -07:00
395 lines
11 KiB
C++
395 lines
11 KiB
C++
/*
|
|
* GPS.cpp
|
|
*
|
|
* Created on: Nov 9, 2015
|
|
* Author: peter
|
|
*/
|
|
|
|
#include "GPS.hpp"
|
|
#include "stm32f30x.h"
|
|
#include <cstdio>
|
|
#include <cassert>
|
|
#include <cstring>
|
|
|
|
#include "DataTerminal.hpp"
|
|
#include "NMEASentence.hpp"
|
|
#include "Utils.hpp"
|
|
#include "EventQueue.hpp"
|
|
#include "printf2.h"
|
|
#include "globals.h"
|
|
//#include "LEDManager.hpp"
|
|
|
|
|
|
GPS &
|
|
GPS::instance()
|
|
{
|
|
static GPS __instance;
|
|
return __instance;
|
|
}
|
|
|
|
GPS::GPS()
|
|
: mBuffPos(0), mUTC(0), mLat(0), mLng(0), mStarted(false), mSlotNumber(0), mDelegate(NULL), mCOG(511), mSpeed(0)
|
|
{
|
|
mPeriod = (SystemCoreClock / 37.5) - 1;
|
|
memset(&mTime, 0, sizeof(mTime));
|
|
}
|
|
|
|
GPS::~GPS()
|
|
{
|
|
|
|
}
|
|
|
|
time_t GPS::UTC()
|
|
{
|
|
return mUTC;
|
|
}
|
|
|
|
struct tm &GPS::time()
|
|
{
|
|
return mTime;
|
|
}
|
|
|
|
uint32_t GPS::aisSlot()
|
|
{
|
|
return mSlotNumber;
|
|
}
|
|
|
|
double GPS::lat()
|
|
{
|
|
return mLat;
|
|
}
|
|
|
|
double GPS::lng()
|
|
{
|
|
return mLng;
|
|
}
|
|
|
|
void GPS::setDelegate(GPSDelegate *delegate)
|
|
{
|
|
mDelegate = delegate;
|
|
}
|
|
|
|
void GPS::init()
|
|
{
|
|
GPIO_InitTypeDef GPIO_InitStruct;
|
|
USART_InitTypeDef USART_InitStruct;
|
|
NVIC_InitTypeDef NVIC_InitStruct;
|
|
|
|
RCC_AHBPeriphClockCmd(RCC_AHBPeriph_GPIOA, ENABLE); // For RX
|
|
RCC_APB2PeriphClockCmd(RCC_APB2Periph_SYSCFG, ENABLE);
|
|
GPIO_PinAFConfig(GPIOA, GPIO_PinSource10, GPIO_AF_7);
|
|
|
|
// Initialize pins as alternative function
|
|
GPIO_InitStruct.GPIO_Pin = GPIO_Pin_10;
|
|
GPIO_InitStruct.GPIO_Mode = GPIO_Mode_AF;
|
|
GPIO_InitStruct.GPIO_OType = GPIO_OType_PP;
|
|
GPIO_InitStruct.GPIO_PuPd = GPIO_PuPd_NOPULL;
|
|
GPIO_InitStruct.GPIO_Speed = GPIO_Speed_Level_1;
|
|
GPIO_Init(GPIOA, &GPIO_InitStruct);
|
|
|
|
/* PA0 is connected to EXTI_Line0 -- currently the PPS signal from the GPS */
|
|
GPIO_InitStruct.GPIO_Pin = GPIO_Pin_0;
|
|
GPIO_InitStruct.GPIO_Mode = GPIO_Mode_IN;
|
|
GPIO_InitStruct.GPIO_Speed = GPIO_Speed_Level_1;
|
|
GPIO_InitStruct.GPIO_OType = GPIO_OType_PP;
|
|
GPIO_InitStruct.GPIO_PuPd = GPIO_PuPd_NOPULL;
|
|
GPIO_Init(GPIOA, &GPIO_InitStruct);
|
|
|
|
SYSCFG_EXTILineConfig(EXTI_PortSourceGPIOA, EXTI_PinSource0);
|
|
EXTI_InitTypeDef EXTI_InitStruct;
|
|
EXTI_InitStruct.EXTI_Line = EXTI_Line0;
|
|
EXTI_InitStruct.EXTI_LineCmd = ENABLE;
|
|
EXTI_InitStruct.EXTI_Mode = EXTI_Mode_Interrupt;
|
|
EXTI_InitStruct.EXTI_Trigger = EXTI_Trigger_Rising;
|
|
EXTI_Init(&EXTI_InitStruct);
|
|
|
|
|
|
|
|
/**
|
|
* Enable clock for USART1 peripheral
|
|
*/
|
|
RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1, ENABLE);
|
|
|
|
USART_InitStruct.USART_BaudRate = 9600;
|
|
USART_InitStruct.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
|
|
USART_InitStruct.USART_Mode = USART_Mode_Rx;
|
|
USART_InitStruct.USART_Parity = USART_Parity_No;
|
|
USART_InitStruct.USART_StopBits = USART_StopBits_1;
|
|
USART_InitStruct.USART_WordLength = USART_WordLength_8b;
|
|
USART_Init(USART1, &USART_InitStruct);
|
|
USART_Cmd(USART1, ENABLE);
|
|
|
|
EventQueue::instance().addObserver(this, GPS_NMEA_SENTENCE);
|
|
|
|
|
|
/**
|
|
* Enable RX interrupt
|
|
*/
|
|
USART_ITConfig(USART1, USART_IT_RXNE, ENABLE);
|
|
NVIC_InitStruct.NVIC_IRQChannel = USART1_IRQn;
|
|
NVIC_InitStruct.NVIC_IRQChannelCmd = ENABLE;
|
|
NVIC_InitStruct.NVIC_IRQChannelPreemptionPriority = 1;
|
|
NVIC_Init(&NVIC_InitStruct);
|
|
|
|
|
|
/*
|
|
* Enable clock for TIM2 on APB1
|
|
*/
|
|
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE);
|
|
resetTimer();
|
|
|
|
NVIC_InitStruct.NVIC_IRQChannel = TIM2_IRQn;
|
|
NVIC_InitStruct.NVIC_IRQChannelPreemptionPriority = 1;
|
|
NVIC_InitStruct.NVIC_IRQChannelCmd = ENABLE;
|
|
NVIC_Init(&NVIC_InitStruct);
|
|
|
|
NVIC_InitStruct.NVIC_IRQChannel = EXTI0_IRQn;
|
|
NVIC_InitStruct.NVIC_IRQChannelPreemptionPriority = 1;
|
|
NVIC_InitStruct.NVIC_IRQChannelCmd = ENABLE;
|
|
NVIC_Init(&NVIC_InitStruct);
|
|
|
|
printf2("GPS Initialized\r\n");
|
|
}
|
|
|
|
void GPS::onRX(char c)
|
|
{
|
|
mBuff[mBuffPos++] = c;
|
|
if (mBuffPos > 90) {
|
|
// We screwed up!
|
|
mBuffPos = 0;
|
|
mBuff[mBuffPos] = 0;
|
|
}
|
|
else if (c == '\n') {
|
|
mBuff[mBuffPos] = 0;
|
|
GPSNMEASentence *event = static_cast<GPSNMEASentence*>(EventPool::instance().newEvent(GPS_NMEA_SENTENCE));
|
|
if ( event ) {
|
|
memcpy(event->mSentence, mBuff, sizeof event->mSentence);
|
|
EventQueue::instance ().push (event);
|
|
}
|
|
mBuffPos = 0;
|
|
mBuff[mBuffPos] = 0;
|
|
}
|
|
}
|
|
|
|
void GPS::onPPS()
|
|
{
|
|
if (mUTC) {
|
|
++mUTC; // PPS := advance clock by one second!
|
|
localtime_r (&mUTC, &mTime); // Now we know exactly what UTC second it is, with only microseconds of latency
|
|
if (!mStarted) {
|
|
// To keep things simple, we only start the AIS slot timer if we're on an even second (it has a 37.5 Hz frequency)
|
|
mSlotNumber = (mTime.tm_sec % 60) * 2250; // We know what AIS slot number we're in
|
|
if (!(mTime.tm_sec & 0x00000001))
|
|
startTimer ();
|
|
}
|
|
else {
|
|
// The timer is on, now let's re-calibrate ...
|
|
if (mTime.tm_sec & 0x00000001) {
|
|
// On odd seconds, we expect the timer value to be half its period. Just correct it.
|
|
uint32_t nominalTimerValue = mPeriod / 2 - 1;
|
|
TIM2->CNT = nominalTimerValue;
|
|
}
|
|
else {
|
|
// On even seconds, things are a bit more tricky ...
|
|
uint32_t currentTimerValue = TIM2->CNT;
|
|
if (currentTimerValue >= mPeriod / 2 - 1) {
|
|
// The timer is a little behind, so kick it forward
|
|
TIM2->CNT = mPeriod - 1;
|
|
}
|
|
else {
|
|
// The timer is a little ahead, so pull it back
|
|
TIM2->CNT = 0;
|
|
}
|
|
}
|
|
}
|
|
|
|
ClockEvent *event = static_cast<ClockEvent*>(EventPool::instance().newEvent(CLOCK_EVENT));
|
|
if ( event ) {
|
|
event->mTime = mUTC;
|
|
EventQueue::instance ().push(event);
|
|
}
|
|
} // We have a valid UTC timestamp
|
|
|
|
}
|
|
|
|
void GPS::processEvent(Event *event)
|
|
{
|
|
//printf2("-> GPS::processEvent()\r\n");
|
|
GPSNMEASentence *s = static_cast<GPSNMEASentence*>(event);
|
|
processLine(s->mSentence);
|
|
//printf2("<- GPS::processEvent()\r\n");
|
|
}
|
|
|
|
void GPS::processLine(const char* buff)
|
|
{
|
|
if ( buff[0] == '$' && buff[1] != '$' ) {
|
|
unsigned reportedHash;
|
|
char *starPos = strstr(buff, "*");
|
|
if ( starPos && sscanf(starPos + 1, "%x", &reportedHash) == 1 ) {
|
|
unsigned actualHash = 0;
|
|
for ( const char* c = buff + 1; c < starPos; ++c )
|
|
actualHash ^= *c;
|
|
|
|
if ( reportedHash == actualHash ) {
|
|
parseSentence(buff);
|
|
}
|
|
|
|
}
|
|
}
|
|
}
|
|
|
|
void GPS::parseSentence(const char *buff)
|
|
{
|
|
//printf2(buff);
|
|
//if ( strstr(buff, "RMC") != buff+3 )
|
|
// return;
|
|
|
|
NMEASentence sentence(buff);
|
|
if ( sentence.code().find("RMC") == 2 ) {
|
|
#ifdef ENABLE_TERMINAL
|
|
#ifdef OUTPUT_GPS_NMEA
|
|
#ifdef MULTIPLEXED_OUTPUT
|
|
DataTerminal::instance().write("NMEA", buff);
|
|
#else
|
|
DataTerminal::instance().write(buff);
|
|
#endif
|
|
#endif
|
|
#endif
|
|
|
|
// This is the time that corresponds to the previous PPS
|
|
const vector<string> &fields = sentence.fields();
|
|
|
|
/*
|
|
* Sometimes the GPS indicates errors with sentences like
|
|
* $GPRMC,1420$0*74\r\n
|
|
* Although the sentence structure is valid, its content is not what we expect.
|
|
*
|
|
* TODO: Should we consider the GPS non-functioning at this point and thus prevent transmission until it recovers?
|
|
*/
|
|
if ( fields.size() < 10 )
|
|
return;
|
|
|
|
// GPS updates arrive even with no time or fix information
|
|
if ( fields[1].length() < 6 || fields[9].length() < 6 )
|
|
return;
|
|
|
|
//printf2(buff);
|
|
//LEDManager::instance().blink();
|
|
|
|
if ( mUTC == 0 ) {
|
|
const string ×tr = fields[1].substr(0, 6);
|
|
const string &datestr = fields[9].substr(0, 6);
|
|
mTime.tm_hour = Utils::toInt(timestr.substr(0, 2));
|
|
mTime.tm_min = Utils::toInt(timestr.substr(2, 2));
|
|
mTime.tm_sec = Utils::toInt(timestr.substr(4, 2));
|
|
mTime.tm_mday = Utils::toInt(datestr.substr(0, 2));
|
|
mTime.tm_mon = Utils::toInt(datestr.substr(2, 2)) - 1; // Month is 0-based
|
|
mTime.tm_year = 100 + Utils::toInt(datestr.substr(4, 2)); // Year is 1900-based
|
|
mUTC = mktime(&mTime);
|
|
//printf2("UTC=%d\r\n", mUTC);
|
|
}
|
|
|
|
// Do we have a fix?
|
|
if ( mUTC && sentence.fields()[3].length() > 0 && sentence.fields()[5].length() > 0 ) {
|
|
mLat = Utils::latitudeFromNMEA(sentence.fields()[3], sentence.fields()[4]);
|
|
mLng = Utils::longitudeFromNMEA(sentence.fields()[5], sentence.fields()[6]);
|
|
mSpeed = Utils::toDouble(sentence.fields()[7]);
|
|
mCOG = Utils::toDouble(sentence.fields()[8]);
|
|
GPSFIXEvent *event = static_cast<GPSFIXEvent*>(EventPool::instance().newEvent(GPS_FIX_EVENT));
|
|
if ( event ) {
|
|
event->mUTC = mUTC;
|
|
event->mLat = mLat;
|
|
event->mLng = mLng;
|
|
event->mSpeed = mSpeed;
|
|
event->mCOG = mCOG;
|
|
EventQueue::instance().push(event);
|
|
}
|
|
//printf2("Lat: %f, Lng: %f\r\n", mLat, mLng);
|
|
}
|
|
}
|
|
}
|
|
|
|
void GPS::resetTimer()
|
|
{
|
|
TIM_TimeBaseInitTypeDef timerInitStructure;
|
|
timerInitStructure.TIM_Prescaler = 0;
|
|
timerInitStructure.TIM_CounterMode = TIM_CounterMode_Up;
|
|
timerInitStructure.TIM_Period = mPeriod;
|
|
timerInitStructure.TIM_ClockDivision = TIM_CKD_DIV1;
|
|
|
|
TIM_TimeBaseInit(TIM2, &timerInitStructure);
|
|
TIM_ITConfig(TIM2, TIM_IT_Update, ENABLE);
|
|
}
|
|
|
|
void GPS::startTimer()
|
|
{
|
|
TIM_Cmd(TIM2, ENABLE);
|
|
mStarted = true;
|
|
printf2("Started SOTDMA timer\r\n");
|
|
}
|
|
|
|
void GPS::stopTimer()
|
|
{
|
|
mStarted = false;
|
|
TIM_Cmd(TIM2, DISABLE);
|
|
printf2("Stopped SOTDMA timer\r\n");
|
|
}
|
|
|
|
|
|
void GPS::reset()
|
|
{
|
|
stopTimer();
|
|
resetTimer();
|
|
startTimer();
|
|
}
|
|
|
|
void GPS::onTimerIRQ()
|
|
{
|
|
if ( mStarted ) {
|
|
++mSlotNumber;
|
|
if ( mSlotNumber == 2250 )
|
|
mSlotNumber = 0;
|
|
|
|
// Delegates need real-time information
|
|
if ( mDelegate )
|
|
mDelegate->timeSlotStarted(mSlotNumber);
|
|
/*
|
|
if ( mSlotNumber % 2 )
|
|
GPIO_SetBits(GPIOB, GPIO_Pin_3);
|
|
else
|
|
GPIO_ResetBits(GPIOB, GPIO_Pin_3);
|
|
*/
|
|
}
|
|
}
|
|
|
|
extern "C" {
|
|
|
|
|
|
void TIM2_IRQHandler(void)
|
|
{
|
|
if ( TIM_GetITStatus(TIM2, TIM_IT_Update) != RESET ) {
|
|
TIM_ClearITPendingBit(TIM2, TIM_IT_Update);
|
|
GPS::instance().onTimerIRQ();
|
|
}
|
|
}
|
|
|
|
void EXTI0_IRQHandler(void)
|
|
{
|
|
if ( EXTI_GetITStatus(EXTI_Line0) != RESET ) {
|
|
EXTI_ClearITPendingBit(EXTI_Line0);
|
|
GPS::instance().onPPS();
|
|
}
|
|
}
|
|
|
|
void USART1_IRQHandler(void)
|
|
{
|
|
if ( USART_GetITStatus(USART1, USART_IT_RXNE) ) {
|
|
char c = (char) USART1->RDR; // This clears the interrupt right away
|
|
GPS::instance().onRX(c);
|
|
}
|
|
}
|
|
|
|
|
|
}
|
|
|