clockworkpi/PicoCalc
1
0
Fork 0
mirror of https://github.com/clockworkpi/PicoCalc.git synced 2025-12-12 10:18:54 +01:00
Code Issues Actions Packages Projects Releases Wiki Activity
PicoCalc/Code/pico_multi_booter/picomite/GPS.c
cuu beb05b7ca7 add pico_multi_booter code
2025-05-19 14:50:19 +08:00

705 lines
22 KiB
C
Raw Blame History

/***********************************************************************************************************************
PicoMite MMBasic
GPS.c
<COPYRIGHT HOLDERS> Geoff Graham, Peter Mather
Copyright (c) 2021, <COPYRIGHT HOLDERS> All rights reserved.
Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer
in the documentation and/or other materials provided with the distribution.
3. The name MMBasic be used when referring to the interpreter in any documentation and promotional material and the original copyright message be displayed
on the console at startup (additional copyright messages may be added).
4. All advertising materials mentioning features or use of this software must display the following acknowledgement: This product includes software developed
by the <copyright holder>.
5. Neither the name of the <copyright holder> nor the names of its contributors may be used to endorse or promote products derived from this software
without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY <COPYRIGHT HOLDERS> AS IS AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDERS> BE LIABLE FOR ANY DIRECT,
INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
************************************************************************************************************************/
/**
* @file GPS.c
* @author Geoff Graham, Peter Mather
* @brief Source for GPS MMBasic function
*/
/**
* @cond
* The following section will be excluded from the documentation.
*/
/* ************************************************************************** */
/* ************************************************************************** */
/* Section: Included Files */
/* ************************************************************************** */
/* ************************************************************************** */
/* This section lists the other files that are included in this file.
*/
/* TODO: Include other files here if needed. */
#include <stdlib.h>
#include <string.h>
#include <ctype.h>
#include <stdarg.h>
#include <time.h>
#include "MMBasic_Includes.h"
#include "Hardware_Includes.h"
/* ************************************************************************** */
/* ************************************************************************** */
/* Section: File Scope or Global Data */
/* ************************************************************************** */
/* ************************************************************************** */
/* A brief description of a section can be given directly below the section
banner.
*/
/* ************************************************************************** */
/** Descriptive Data Item Name
@Summary
Brief one-line summary of the data item.
@Description
Full description, explaining the purpose and usage of data item.
<p>
Additional description in consecutive paragraphs separated by HTML
paragraph breaks, as necessary.
<p>
Type "JavaDoc" in the "How Do I?" IDE toolbar for more information on tags.
@Remarks
Any additional remarks
*/
//int global_data;
int GPSchannel=0;
volatile char gpsbuf1[128];
volatile char gpsbuf2[128];
volatile char * volatile gpsbuf;
volatile char *gpsready;
volatile char gpscount;
volatile int gpscurrent;
volatile int gpsmonitor;
MMFLOAT GPSlatitude=0;
MMFLOAT GPSlongitude=0;
MMFLOAT GPSspeed=0;
int GPSvalid=0;
char GPStime[9]="000:00:0";
char GPSdate[11]="000-00-200";
MMFLOAT GPStrack=0;
MMFLOAT GPSdop=0;
int GPSsatellites=0;
MMFLOAT GPSaltitude=0;
MMFLOAT GPSgeoid=0;
int GPSfix=0;
int GPSadjust=0;
void GPS_parse(char *nmea);
#define EPOCH_ADJUSTMENT_DAYS 719468L
/* year to which the adjustment was made */
#define ADJUSTED_EPOCH_YEAR 0
/* 1st March of year 0 is Wednesday */
#define ADJUSTED_EPOCH_WDAY 3
/* there are 97 leap years in 400-year periods. ((400 - 97) * 365 + 97 * 366) */
#define DAYS_PER_ERA 146097L
/* there are 24 leap years in 100-year periods. ((100 - 24) * 365 + 24 * 366) */
#define DAYS_PER_CENTURY 36524L
/* there is one leap year every 4 years */
#define DAYS_PER_4_YEARS (3 * 365 + 366)
/* number of days in a non-leap year */
#define DAYS_PER_YEAR 365
/* number of days in January */
#define DAYS_IN_JANUARY 31
/* number of days in non-leap February */
#define DAYS_IN_FEBRUARY 28
/* number of years per era */
#define YEARS_PER_ERA 400
#define SECSPERDAY 86400
#define SECSPERHOUR 3600
#define SECSPERMIN 60
#define DAYSPERWEEK 7
#define YEAR_BASE 1900
/* Number of days per month (except for February in leap years). */
static const int monoff[] = {
0, 31, 59, 90, 120, 151, 181, 212, 243, 273, 304, 334
};
static int
is_leap_year(int year)
{
return year % 4 == 0 && (year % 100 != 0 || year % 400 == 0);
}
static int
leap_days(int y1, int y2)
{
--y1;
--y2;
return (y2/4 - y1/4) - (y2/100 - y1/100) + (y2/400 - y1/400);
}
struct tm *
gmtime_r (const time_t *__restrict tim_p,
struct tm *__restrict res)
{
long days, rem;
const time_t lcltime = *tim_p;
int era, weekday, year;
unsigned erayear, yearday, month, day;
unsigned long eraday;
days = lcltime / SECSPERDAY + EPOCH_ADJUSTMENT_DAYS;
rem = lcltime % SECSPERDAY;
if (rem < 0)
{
rem += SECSPERDAY;
--days;
}
/* compute hour, min, and sec */
res->tm_hour = (int) (rem / SECSPERHOUR);
rem %= SECSPERHOUR;
res->tm_min = (int) (rem / SECSPERMIN);
res->tm_sec = (int) (rem % SECSPERMIN);
/* compute day of week */
if ((weekday = ((ADJUSTED_EPOCH_WDAY + days) % DAYSPERWEEK)) < 0)
weekday += DAYSPERWEEK;
res->tm_wday = weekday;
/* compute year, month, day & day of year */
/* for description of this algorithm see
* http://howardhinnant.github.io/date_algorithms.html#civil_from_days */
era = (days >= 0 ? days : days - (DAYS_PER_ERA - 1)) / DAYS_PER_ERA;
eraday = days - era * DAYS_PER_ERA; /* [0, 146096] */
erayear = (eraday - eraday / (DAYS_PER_4_YEARS - 1) + eraday / DAYS_PER_CENTURY -
eraday / (DAYS_PER_ERA - 1)) / 365; /* [0, 399] */
yearday = eraday - (DAYS_PER_YEAR * erayear + erayear / 4 - erayear / 100); /* [0, 365] */
month = (5 * yearday + 2) / 153; /* [0, 11] */
day = yearday - (153 * month + 2) / 5 + 1; /* [1, 31] */
month += month < 10 ? 2 : -10;
year = ADJUSTED_EPOCH_YEAR + erayear + era * YEARS_PER_ERA + (month <= 1);
res->tm_yday = yearday >= DAYS_PER_YEAR - DAYS_IN_JANUARY - DAYS_IN_FEBRUARY ?
yearday - (DAYS_PER_YEAR - DAYS_IN_JANUARY - DAYS_IN_FEBRUARY) :
yearday + DAYS_IN_JANUARY + DAYS_IN_FEBRUARY + is_leap_year(erayear);
res->tm_year = year - YEAR_BASE;
res->tm_mon = month;
res->tm_mday = day;
res->tm_isdst = 0;
return (res);
}
struct tm *
gmtime (const time_t * tim_p)
{
struct _reent *reent = _REENT;
_REENT_CHECK_TM(reent);
return gmtime_r (tim_p, (struct tm *)_REENT_TM(reent));
}
time_t
timegm(const struct tm *tm)
{
int year;
time_t days;
time_t hours;
time_t minutes;
time_t seconds;
year = 1900 + tm->tm_year;
days = 365 * (year - 1970) + leap_days(1970, year);
days += monoff[tm->tm_mon];
if (tm->tm_mon > 1 && is_leap_year(year))
++days;
days += tm->tm_mday - 1;
hours = days * 24 + tm->tm_hour;
minutes = hours * 60 + tm->tm_min;
seconds = minutes * 60 + tm->tm_sec;
return seconds;
}
/* ************************************************************************** */
/* ************************************************************************** */
// Section: Local Functions */
/* ************************************************************************** */
/* ************************************************************************** */
/* A brief description of a section can be given directly below the section
banner.
*/
/* ************************************************************************** */
/**
@Function
int ExampleLocalFunctionName ( int param1, int param2 )
@Summary
Brief one-line description of the function.
@Description
Full description, explaining the purpose and usage of the function.
<p>
Additional description in consecutive paragraphs separated by HTML
paragraph breaks, as necessary.
<p>
Type "JavaDoc" in the "How Do I?" IDE toolbar for more information on tags.
@Precondition
List and describe any required preconditions. If there are no preconditions,
enter "None."
@Parameters
@param param1 Describe the first parameter to the function.
@param param2 Describe the second parameter to the function.
@Returns
List (if feasible) and describe the return values of the function.
<ul>
<li>1 Indicates an error occurred
<li>0 Indicates an error did not occur
</ul>
@Remarks
Describe any special behavior not described above.
<p>
Any additional remarks.
@Example
@code
if(ExampleFunctionName(1, 2) == 0)
{
return 3;
}
*/
//static int ExampleLocalFunction(int param1, int param2) {
// return 0;
//}
#define INDENT_SPACES " "
/* ************************************************************************** */
/* ************************************************************************** */
// Section: Interface Functions */
/* ************************************************************************** */
/* ************************************************************************** */
/* A brief description of a section can be given directly below the section
banner.
*/
// *****************************************************************************
/**
@Function
int ExampleInterfaceFunctionName ( int param1, int param2 )
@Summary
Brief one-line description of the function.
@Remarks
Refer to the example_file.h interface header for function usage details.
*/
//int ExampleInterfaceFunction(int param1, int param2) {
// return 0;
//}
/* @endcond */
void fun_GPS(void){
sret = GetTempMemory(STRINGSIZE); // this will last for the life of the command
if(!GPSchannel) error("GPS not activated");
if(checkstring(ep, (unsigned char *)"LATITUDE") != NULL) {
fret = GPSlatitude;
targ = T_NBR;
}
else if(checkstring(ep, (unsigned char *)"LONGITUDE") != NULL) {
fret = GPSlongitude;
targ = T_NBR;
}
else if(checkstring(ep, (unsigned char *)"SPEED") != NULL) {
fret = GPSspeed;
targ = T_NBR;
}
else if(checkstring(ep, (unsigned char *)"TRACK") != NULL) {
fret = GPStrack;
targ = T_NBR;
}
else if(checkstring(ep, (unsigned char *)"VALID") != NULL) {
iret = GPSvalid;
targ = T_INT;
}
else if(checkstring(ep, (unsigned char *)"TIME") != NULL) {
sret = (unsigned char *)GPStime;
targ = T_STR;
}
else if(checkstring(ep, (unsigned char *)"DATE") != NULL) {
sret = (unsigned char *)GPSdate;
targ = T_STR;
}
else if(checkstring(ep, (unsigned char *)"SATELLITES") != NULL) {
iret = GPSsatellites;
targ = T_INT;
}
else if(checkstring(ep, (unsigned char *)"ALTITUDE") != NULL) {
fret = GPSaltitude;
targ = T_NBR;
}
else if(checkstring(ep, (unsigned char *)"DOP") != NULL) {
fret = GPSdop;
targ = T_NBR;
}
else if(checkstring(ep, (unsigned char *)"FIX") != NULL) {
iret = GPSfix;
targ = T_INT;
}
else if(checkstring(ep, (unsigned char *)"GEOID") != NULL) {
fret = GPSgeoid;
targ = T_NBR;
}
else error("Invalid command");
}
/*
* @cond
* The following section will be excluded from the documentation.
*/
void processgps(void){
if(GPSTimer>2000){
GPSvalid=0;
}
if(gpsready !=NULL){
GPS_parse((char *)gpsready);
GPSTimer=0;
gpsready=NULL;
}
}
uint8_t parseHex(char c) {
if (c < '0')
return 0;
if (c <= '9')
return c - '0';
if (c < 'A')
return 0;
if (c <= 'F')
return (c - 'A')+10;
// if (c > 'F')
return 0;
}
void GPS_parse(char *nmea) {
uint8_t hour, minute, seconds, year=0, month=0, day=0;
uint16_t __attribute__((unused)) milliseconds;
// Floating point latitude and longitude value in degrees.
MMFLOAT __attribute__((unused)) latitude, longitude;
// Fixed point latitude and longitude value with degrees stored in units of 1/100000 degrees,
// and minutes stored in units of 1/100000 degrees. See pull #13 for more details:
// https://github.com/adafruit/Adafruit-GPS-Library/pull/13
int32_t __attribute__((unused)) latitude_fixed, longitude_fixed;
MMFLOAT latitudeDegrees=0.0, longitudeDegrees=0.0;
MMFLOAT geoidheight, altitude;
MMFLOAT speed, angle, HDOP;
MMFLOAT __attribute__((unused)) magvariation;
char __attribute__((unused)) lat, lon;
uint8_t fixquality, satellites;
struct tm *tm;
struct tm tma;
tm=&tma;
if(gpsmonitor){
MMPrintString(nmea);
}
// do checksum check
// first look if we even have one
if (nmea[strlen(nmea)-4] == '*') {
uint16_t sum = parseHex(nmea[strlen(nmea)-3]) * 16;
sum += parseHex(nmea[strlen(nmea)-2]);
uint8_t i;
// check checksum
for (i=2; i < (strlen(nmea)-4); i++) {
sum ^= nmea[i];
}
if (sum != 0) {
// bad checksum :(
return;
}
}
int32_t degree;
long minutes;
char degreebuff[10];
// look for a few common sentences
if (strstr(nmea, "$GPGGA") || strstr(nmea, "$GNGGA")) {
// found GGA
char *p = nmea;
// get time
p = strchr((char *)p, ',')+1;
MMFLOAT timef = atof(p);
uint32_t time = timef;
hour = time / 10000;
minute = (time % 10000) / 100;
seconds = (time % 100);
milliseconds = fmod(timef, 1.0) * 1000;
// parse out latitude
p = strchr((char *)p, ',')+1;
if (',' != *p)
{
strncpy(degreebuff, p, 2);
p += 2;
degreebuff[2] = '\0';
degree = atol(degreebuff) * 10000000;
strncpy(degreebuff, p, 2); // minutes
p += 3; // skip decimal point
strncpy(degreebuff + 2, p, 4);
degreebuff[6] = '\0';
minutes = 50 * atol(degreebuff) / 3;
latitude_fixed = degree + minutes;
latitude = degree / 100000 + minutes * 0.000006F;
latitudeDegrees = (latitude-100*(MMFLOAT)((int)(latitude/100)))/60.0;
latitudeDegrees += (MMFLOAT)((int)(latitude/100));
}
p = strchr((char *)p, ',')+1;
if (',' != *p)
{
if (p[0] == 'S') latitudeDegrees *= -1.0;
if (p[0] == 'N') lat = 'N';
else if (p[0] == 'S') lat = 'S';
else if (p[0] == ',') lat = 0;
else return;
}
GPSlatitude=(MMFLOAT)latitudeDegrees;
// parse out longitude
p = strchr((char *)p, ',')+1;
if (',' != *p)
{
strncpy(degreebuff, p, 3);
p += 3;
degreebuff[3] = '\0';
degree = atol(degreebuff) * 10000000;
strncpy(degreebuff, p, 2); // minutes
p += 3; // skip decimal point
strncpy(degreebuff + 2, p, 4);
degreebuff[6] = '\0';
minutes = 50 * atol(degreebuff) / 3;
longitude_fixed = degree + minutes;
longitude = degree / 100000 + minutes * 0.000006F;
longitudeDegrees = (longitude-100*(MMFLOAT)((int)(longitude/100)))/60.0;
longitudeDegrees += (MMFLOAT)((int)(longitude/100));
}
p = strchr((char *)p, ',')+1;
if (',' != *p)
{
if (p[0] == 'W') longitudeDegrees *= -1.0;
if (p[0] == 'W') lon = 'W';
else if (p[0] == 'E') lon = 'E';
else if (p[0] == ',') lon = 0;
else return;
}
GPSlongitude=(MMFLOAT)longitudeDegrees;
p = strchr((char *)p, ',')+1;
if (',' != *p)
{
fixquality = atoi(p);
GPSfix=(int)fixquality;
}
p = strchr((char *)p, ',')+1;
if (',' != *p)
{
satellites = atoi(p);
GPSsatellites=(int)satellites;
}
p = strchr((char *)p, ',')+1;
if (',' != *p)
{
HDOP = atof(p);
GPSdop=(MMFLOAT)HDOP;
}
p = strchr((char *)p, ',')+1;
if (',' != *p)
{
altitude = atof(p);
GPSaltitude=(MMFLOAT)altitude;
}
p = strchr((char *)p, ',')+1;
p = strchr((char *)p, ',')+1;
if (',' != *p)
{
geoidheight = atof(p);
GPSgeoid=(MMFLOAT)geoidheight;
}
return;
}
if (strstr(nmea, "$GPRMC") || strstr(nmea, "$GNRMC")) {
// found RMC
char *p = nmea;
int i, localGPSvalid=0;
// get time
p = strchr((char *)p, ',')+1;
MMFLOAT timef = atof(p);
uint32_t time = timef;
hour = time / 10000;
minute = (time % 10000) / 100;
seconds = (time % 100);
milliseconds = fmod(timef, 1.0) * 1000;
i=tm->tm_hour;
GPStime[1]=(hour/10) + 48;
GPStime[2]=(hour % 10) + 48;
i=tm->tm_min;
GPStime[4]=(minute/10) + 48;
GPStime[5]=(minute % 10) + 48;
i=tm->tm_sec;
GPStime[7]=(seconds/10) + 48;
GPStime[8]=(seconds % 10) + 48;
p = strchr((char *)p, ',')+1;
if (p[0] == 'A')
localGPSvalid = 1;
else if (p[0] == 'V')
localGPSvalid = 0;
else
{
GPSvalid=0;
return;
}
// parse out latitude
p = strchr((char *)p, ',')+1;
if (',' != *p)
{
strncpy(degreebuff, p, 2);
p += 2;
degreebuff[2] = '\0';
long degree = atol(degreebuff) * 10000000;
strncpy(degreebuff, p, 2); // minutes
p += 3; // skip decimal point
strncpy(degreebuff + 2, p, 4);
degreebuff[6] = '\0';
long minutes = 50 * atol(degreebuff) / 3;
latitude_fixed = degree + minutes;
latitude = degree / 100000 + minutes * 0.000006F;
latitudeDegrees = (latitude-100*(MMFLOAT)((int)(latitude/100)))/60.0;
latitudeDegrees += (MMFLOAT)((int)(latitude/100));
}
p = strchr((char *)p, ',')+1;
if (',' != *p)
{
if (p[0] == 'S') latitudeDegrees *= -1.0;
if (p[0] == 'N') lat = 'N';
else if (p[0] == 'S') lat = 'S';
else if (p[0] == ',') lat = 0;
else return;
}
GPSlatitude=(MMFLOAT)latitudeDegrees;
// parse out longitude
p = strchr((char *)p, ',')+1;
if (',' != *p)
{
strncpy(degreebuff, p, 3);
p += 3;
degreebuff[3] = '\0';
degree = atol(degreebuff) * 10000000;
strncpy(degreebuff, p, 2); // minutes
p += 3; // skip decimal point
strncpy(degreebuff + 2, p, 4);
degreebuff[6] = '\0';
minutes = 50 * atol(degreebuff) / 3;
longitude_fixed = degree + minutes;
longitude = degree / 100000 + minutes * 0.000006F;
longitudeDegrees = (longitude-100*(MMFLOAT)((int)(longitude/100)))/60.0;
longitudeDegrees += (MMFLOAT)((int)(longitude/100));
}
p = strchr((char *)p, ',')+1;
if (',' != *p)
{
if (p[0] == 'W') longitudeDegrees *= -1.0;
if (p[0] == 'W') lon = 'W';
else if (p[0] == 'E') lon = 'E';
else if (p[0] == ',') lon = 0;
else return;
}
GPSlongitude=(MMFLOAT)longitudeDegrees;
// speed
p = strchr((char *)p, ',')+1;
if (',' != *p)
{
speed = atof(p);
GPSspeed=(MMFLOAT)speed;
}
// angle
p = strchr((char *)p, ',')+1;
if (',' != *p)
{
angle = atof(p);
GPStrack=(MMFLOAT)angle;
}
p = strchr((char *)p, ',')+1;
if (',' != *p && p[6]==',')
{
uint32_t fulldate = atoi(p);
day = fulldate / 10000;
month = (fulldate % 10000) / 100;
year = (fulldate % 100);
GPStime[0]=8;
tm->tm_year = year + 100;
tm->tm_mon = month - 1;
tm->tm_mday = day;
tm->tm_hour = hour;
tm->tm_min = minute;
tm->tm_sec = seconds;
time_t timestamp = timegm(tm); /* See README.md if your system lacks timegm(). */
timestamp+=GPSadjust;
tm=gmtime(&timestamp);
i=tm->tm_hour;
GPStime[1]=(i/10) + 48;
GPStime[2]=(i % 10) + 48;
i=tm->tm_min;
GPStime[4]=(i/10) + 48;
GPStime[5]=(i % 10) + 48;
i=tm->tm_sec;
GPStime[7]=(i/10) + 48;
GPStime[8]=(i % 10) + 48;
i=tm->tm_mday;
GPSdate[0]=10;
GPSdate[1]=(i/10) + 48;
GPSdate[2]=(i % 10) + 48;
i=tm->tm_mon+1;
GPSdate[4]=(i/10) + 48;
GPSdate[5]=(i % 10) + 48;
i=tm->tm_year % 100;
GPSdate[9]=(i/10) + 48;
GPSdate[10]=(i % 10) + 48;
// we don't parse the remaining, yet!
GPSvalid=localGPSvalid;
return;
}
}
return;
}
/* @endcond */
Reference in New Issue View Git Blame Copy Permalink