HAPSRG / GPS_HAPS

GPS GNSS

Dependencies:   Vector3

GPS_HAPS.cpp@2:abd509e1f4d1, 2021-09-07 (annotated)

Committer:
cocorlow
Date:
Tue Sep 07 06:58:56 2021 +0000
Revision:
2:abd509e1f4d1
Parent:
1:543687550091
Child:
3:b0d594e5d9e2
Time modified;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
cocorlow 0:6c7f7ed85f2c 1 #include "mbed.h"
cocorlow 0:6c7f7ed85f2c 2 #include "GPS_HAPS.hpp"
cocorlow 0:6c7f7ed85f2c 3 #include <string.h>
cocorlow 0:6c7f7ed85f2c 4 #include <stdlib.h>
cocorlow 0:6c7f7ed85f2c 5 #include "Vector3.hpp"
cocorlow 0:6c7f7ed85f2c 6 #include <math.h>
cocorlow 0:6c7f7ed85f2c 7 #define M_PI 3.14159265358979f
cocorlow 0:6c7f7ed85f2c 8
cocorlow 0:6c7f7ed85f2c 9 GPS_HAPS::GPS_HAPS(PinName tx, PinName rx, int baud)
cocorlow 0:6c7f7ed85f2c 10 :serial(tx, rx, baud), receive_start(0), receive_index(0), receive_request(0)
cocorlow 0:6c7f7ed85f2c 11 {
cocorlow 0:6c7f7ed85f2c 12 }
cocorlow 0:6c7f7ed85f2c 13
cocorlow 0:6c7f7ed85f2c 14 void GPS_HAPS::Receive()
cocorlow 0:6c7f7ed85f2c 15 {
cocorlow 0:6c7f7ed85f2c 16 while (serial.readable())
cocorlow 0:6c7f7ed85f2c 17 {
cocorlow 0:6c7f7ed85f2c 18 char c;
cocorlow 0:6c7f7ed85f2c 19 c = serial.getc();
cocorlow 0:6c7f7ed85f2c 20 // pc.putc(c);
cocorlow 0:6c7f7ed85f2c 21 receive_buffer[receive_index] = c;
cocorlow 0:6c7f7ed85f2c 22 if (c == '\r')
cocorlow 0:6c7f7ed85f2c 23 {
cocorlow 0:6c7f7ed85f2c 24 receive_request++;
cocorlow 0:6c7f7ed85f2c 25 }
cocorlow 0:6c7f7ed85f2c 26 receive_index = (receive_index + 1) % RECEIVE_SIZE;
cocorlow 0:6c7f7ed85f2c 27 }
cocorlow 0:6c7f7ed85f2c 28 }
cocorlow 0:6c7f7ed85f2c 29
cocorlow 0:6c7f7ed85f2c 30 void GPS_HAPS::Loop()
cocorlow 0:6c7f7ed85f2c 31 {
cocorlow 0:6c7f7ed85f2c 32 if (receive_request > 0)
cocorlow 0:6c7f7ed85f2c 33 {
cocorlow 0:6c7f7ed85f2c 34 if (receive_buffer[receive_start] == '$')
cocorlow 0:6c7f7ed85f2c 35 {
cocorlow 0:6c7f7ed85f2c 36 for (int i = 0; i < SENTENCE_SIZE; i++)
cocorlow 0:6c7f7ed85f2c 37 {
cocorlow 0:6c7f7ed85f2c 38 char c;
cocorlow 0:6c7f7ed85f2c 39 c = receive_buffer[(receive_start + i) % RECEIVE_SIZE];
cocorlow 0:6c7f7ed85f2c 40 sentence_buffer[i] = c;
cocorlow 0:6c7f7ed85f2c 41 if (c == '\r')
cocorlow 0:6c7f7ed85f2c 42 {
cocorlow 0:6c7f7ed85f2c 43 receive_start = (receive_start + i + 1) % RECEIVE_SIZE;
cocorlow 0:6c7f7ed85f2c 44 sentence_buffer[i+1] = 0;
cocorlow 0:6c7f7ed85f2c 45 break;
cocorlow 0:6c7f7ed85f2c 46 }
cocorlow 0:6c7f7ed85f2c 47 }
cocorlow 0:6c7f7ed85f2c 48 Parse(sentence_buffer);
cocorlow 0:6c7f7ed85f2c 49 receive_request--;
cocorlow 0:6c7f7ed85f2c 50 }
cocorlow 0:6c7f7ed85f2c 51 else
cocorlow 0:6c7f7ed85f2c 52 {
cocorlow 0:6c7f7ed85f2c 53 while (receive_start != receive_index)
cocorlow 0:6c7f7ed85f2c 54 {
cocorlow 0:6c7f7ed85f2c 55 receive_start = (receive_start + 1) % RECEIVE_SIZE;
cocorlow 0:6c7f7ed85f2c 56 if (receive_buffer[receive_start] == '$')
cocorlow 0:6c7f7ed85f2c 57 {
cocorlow 0:6c7f7ed85f2c 58 break;
cocorlow 0:6c7f7ed85f2c 59 }
cocorlow 0:6c7f7ed85f2c 60 }
cocorlow 0:6c7f7ed85f2c 61 }
cocorlow 0:6c7f7ed85f2c 62 }
cocorlow 0:6c7f7ed85f2c 63 }
cocorlow 0:6c7f7ed85f2c 64
cocorlow 0:6c7f7ed85f2c 65 void GPS_HAPS::Parse(char *cmd)
cocorlow 0:6c7f7ed85f2c 66 {
cocorlow 0:6c7f7ed85f2c 67 char ns, ew, tf, status;
cocorlow 0:6c7f7ed85f2c 68 int fq, nst, fix, date; // fix quality, Number of satellites being tracked, 3D fix
cocorlow 0:6c7f7ed85f2c 69 float latitude, longitude, timefix, speed, altitude;
cocorlow 0:6c7f7ed85f2c 70 char s[16];
cocorlow 0:6c7f7ed85f2c 71
cocorlow 0:6c7f7ed85f2c 72 // Global Positioning System Fix Data
cocorlow 0:6c7f7ed85f2c 73 // $GNGGA,030503.00,3542.92568,N,13945.76087,E,1,05,10.02,225.2,M,39.4,M,,*7C
cocorlow 0:6c7f7ed85f2c 74 // if(strncmp(cmd,"$GPGGA", 6) == 0)
cocorlow 0:6c7f7ed85f2c 75 if(strstr(cmd,"GGA") != NULL)
cocorlow 0:6c7f7ed85f2c 76 {
cocorlow 0:6c7f7ed85f2c 77 sscanf(cmd, "%[^,],%f,%f,%c,%f,%c,%d,%d,%f", s, &timefix, &latitude, &ns, &longitude, &ew, &fq, &nst, &altitude);
cocorlow 0:6c7f7ed85f2c 78 float lat = (int)latitude/100;
cocorlow 0:6c7f7ed85f2c 79 Latitude = lat + (latitude-lat*100)/60.0f;
cocorlow 0:6c7f7ed85f2c 80 float lon = (int)longitude/100;
cocorlow 0:6c7f7ed85f2c 81 Longitude = lon + (longitude-lon*100)/60.0f;
cocorlow 0:6c7f7ed85f2c 82 N_S = ns;
cocorlow 0:6c7f7ed85f2c 83 E_W = ew;
cocorlow 0:6c7f7ed85f2c 84 Quality = fq;
cocorlow 0:6c7f7ed85f2c 85 Satellites = nst;
cocorlow 0:6c7f7ed85f2c 86 Altitude = altitude;
cocorlow 0:6c7f7ed85f2c 87 // pc.printf("%s\n", cmd);
cocorlow 0:6c7f7ed85f2c 88 // pc.printf("##GPGGA Fix taken at: %f, Latitude: %f %c, Longitude: %f %c, Fix quality: %d, Number of sat: %d, Altitude: %f M\r\n", timefix, latitude, ns, longitude, ew, fq, nst, altitude);
cocorlow 0:6c7f7ed85f2c 89 }
cocorlow 0:6c7f7ed85f2c 90
cocorlow 0:6c7f7ed85f2c 91 // // Satellite status
cocorlow 0:6c7f7ed85f2c 92 // else if(strncmp(cmd,"$GPGSA", 6) == 0)
cocorlow 0:6c7f7ed85f2c 93 // {
cocorlow 0:6c7f7ed85f2c 94 // sscanf(cmd, "$GPGSA,%c,%d,%d", &tf, &fix, &nst);
cocorlow 0:6c7f7ed85f2c 95 // pc.printf("GPGSA Type fix: %c, 3D fix: %d, number of sat: %d\r\n", tf, fix, nst);
cocorlow 0:6c7f7ed85f2c 96 // }
cocorlow 0:6c7f7ed85f2c 97
cocorlow 0:6c7f7ed85f2c 98 // Geographic position, Latitude and Longitude
cocorlow 0:6c7f7ed85f2c 99 // else if(strncmp(cmd,"$GPGLL", 6) == 0)
cocorlow 0:6c7f7ed85f2c 100 // {
cocorlow 0:6c7f7ed85f2c 101 // sscanf(cmd, "$GPGLL,%f,%c,%f,%c,%f", &latitude, &ns, &longitude, &ew, &timefix);
cocorlow 0:6c7f7ed85f2c 102 // pc.printf("GPGLL Latitude: %f %c, Longitude: %f %c, Fix taken at: %f\n", latitude, ns, longitude, ew, timefix);
cocorlow 0:6c7f7ed85f2c 103 // }
cocorlow 0:6c7f7ed85f2c 104
cocorlow 0:6c7f7ed85f2c 105 // Geographic position, Latitude and Longitude
cocorlow 0:6c7f7ed85f2c 106 // $GNRMC,035344.00,A,3542.91153,N,13945.74153,E,0.239,,070921,,,A,V*15
cocorlow 0:6c7f7ed85f2c 107 else if(strstr(cmd,"RMC") != NULL)
cocorlow 0:6c7f7ed85f2c 108 {
cocorlow 0:6c7f7ed85f2c 109 sscanf(cmd, "%[^,],%f,%c,%f,%c,%f,%c,%f,,%d", s, &timefix, &status, &latitude, &ns, &longitude, &ew, &speed, &date);
cocorlow 0:6c7f7ed85f2c 110 // pc.printf("%s,%f,%c,%f,%c,%f,%c,%f,,%d", s, timefix, status, latitude, ns, longitude, ew, speed, date);
cocorlow 0:6c7f7ed85f2c 111 float time = timefix + 90000.00f;
cocorlow 0:6c7f7ed85f2c 112 Hours = int(time) / 10000;
cocorlow 0:6c7f7ed85f2c 113 Minutes = (int(time) % 10000) / 100;
cocorlow 0:6c7f7ed85f2c 114 Seconds = int(time) % 100;
cocorlow 0:6c7f7ed85f2c 115 Milliseconds = (int)(((time - (Hours*10000+Minutes*100+Seconds)))/0.001f);
cocorlow 2:abd509e1f4d1 116 Time = Hours*3600000+Minutes*60000+Seconds*1000+Milliseconds;
cocorlow 0:6c7f7ed85f2c 117 Speed = speed * 0.514444f;
cocorlow 0:6c7f7ed85f2c 118 Year = date % 100;
cocorlow 0:6c7f7ed85f2c 119 date /= 100;
cocorlow 0:6c7f7ed85f2c 120 Month = date % 100;
cocorlow 0:6c7f7ed85f2c 121 date /= 100;
cocorlow 0:6c7f7ed85f2c 122 Day = date;
cocorlow 0:6c7f7ed85f2c 123 // pc.printf("%s\n", cmd);
cocorlow 0:6c7f7ed85f2c 124 // pc.printf("##GPRMC Fix taken at: %f, Status: %c, Latitude: %f %c, Longitude: %f %c, Speed: %f, Date: %d\r\n", timefix, status, latitude, ns, longitude, ew, speed, date);
cocorlow 0:6c7f7ed85f2c 125 }
cocorlow 0:6c7f7ed85f2c 126
cocorlow 0:6c7f7ed85f2c 127 //
cocorlow 0:6c7f7ed85f2c 128 // $GNVTG,,T,,M,0.444,N,0.822,K,A*31
cocorlow 0:6c7f7ed85f2c 129 else if(strstr(cmd,"VTG") != NULL)
cocorlow 0:6c7f7ed85f2c 130 {
cocorlow 0:6c7f7ed85f2c 131 // pc.printf("%s\n", cmd);
cocorlow 0:6c7f7ed85f2c 132 Direction = -1.0f;
cocorlow 0:6c7f7ed85f2c 133 if(strstr(cmd, "VTG,,") == NULL){
cocorlow 0:6c7f7ed85f2c 134 sscanf(cmd, "%[^,],%f", s, &Direction);
cocorlow 0:6c7f7ed85f2c 135 // pc.printf("<<%f\r\n", Direction);
cocorlow 0:6c7f7ed85f2c 136 // pc.printf("its a Vector Track message.\n");
cocorlow 0:6c7f7ed85f2c 137 }
cocorlow 0:6c7f7ed85f2c 138 }
cocorlow 0:6c7f7ed85f2c 139
cocorlow 0:6c7f7ed85f2c 140 // else if(strncmp(cmd,"$GNGGA", 6) == 0)
cocorlow 0:6c7f7ed85f2c 141 // {
cocorlow 0:6c7f7ed85f2c 142 // sscanf(cmd, "$GNGGA,%f,%f,%c,%f,%c,%d,%d,%*f,%f", &timefix, &latitude, &ns, &longitude, &ew, &fq, &nst, &altitude);
cocorlow 0:6c7f7ed85f2c 143 // pc.printf("GNGGA Fix taken at: %f, Latitude: %f %c, Longitude: %f %c, Fix quality: %d, Number of sat: %d, Altitude: %f M\n", timefix, latitude, ns, longitude, ew, fq, nst, altitude);
cocorlow 0:6c7f7ed85f2c 144 // parseTime(timefix);
cocorlow 0:6c7f7ed85f2c 145 // pc.printf("Time: %d:%d:%d\n", h_time, m_time, s_time);
cocorlow 0:6c7f7ed85f2c 146 // }
cocorlow 0:6c7f7ed85f2c 147
cocorlow 0:6c7f7ed85f2c 148 // else if(strncmp(cmd,"$GNGSA", 6) == 0)
cocorlow 0:6c7f7ed85f2c 149 // {
cocorlow 0:6c7f7ed85f2c 150 // sscanf(cmd, "$GNGSA,%c,%d,%d", &tf, &fix, &nst);
cocorlow 0:6c7f7ed85f2c 151 // pc.printf("GNGSA Type fix: %c, 3D fix: %d, number of sat: %d\r\n", tf, fix, nst);
cocorlow 0:6c7f7ed85f2c 152 // }
cocorlow 0:6c7f7ed85f2c 153
cocorlow 0:6c7f7ed85f2c 154 // else if(strncmp(cmd,"$GPGSV", 6) == 0)
cocorlow 0:6c7f7ed85f2c 155 // {
cocorlow 0:6c7f7ed85f2c 156 // // pc.printf("its a Satellite details message.\n");
cocorlow 0:6c7f7ed85f2c 157 // }
cocorlow 0:6c7f7ed85f2c 158
cocorlow 0:6c7f7ed85f2c 159 // else if(strncmp(cmd,"$GNGLL", 6) == 0)
cocorlow 0:6c7f7ed85f2c 160 // {
cocorlow 0:6c7f7ed85f2c 161 // sscanf(cmd, "$GNGLL,%f,%c,%f,%c,%f", &latitude, &ns, &longitude, &ew, &timefix);
cocorlow 0:6c7f7ed85f2c 162 // pc.printf("GNGLL Latitude: %f %c, Longitude: %f %c, Fix taken at: %f\n", latitude, ns, longitude, ew, timefix);
cocorlow 0:6c7f7ed85f2c 163 // }
cocorlow 0:6c7f7ed85f2c 164
cocorlow 0:6c7f7ed85f2c 165 // else
cocorlow 0:6c7f7ed85f2c 166 // {
cocorlow 0:6c7f7ed85f2c 167 // pc.printf("Unknown message type\n");
cocorlow 0:6c7f7ed85f2c 168 // }
cocorlow 0:6c7f7ed85f2c 169 }
cocorlow 0:6c7f7ed85f2c 170
cocorlow 0:6c7f7ed85f2c 171 void GPS_HAPS::Punctuate()
cocorlow 0:6c7f7ed85f2c 172 {
cocorlow 0:6c7f7ed85f2c 173 }
cocorlow 0:6c7f7ed85f2c 174
cocorlow 0:6c7f7ed85f2c 175 void GPS_HAPS::Attach()
cocorlow 0:6c7f7ed85f2c 176 {
cocorlow 0:6c7f7ed85f2c 177 serial.attach(this, &GPS_HAPS::Receive, Serial::RxIrq);
cocorlow 0:6c7f7ed85f2c 178 }
cocorlow 0:6c7f7ed85f2c 179
cocorlow 0:6c7f7ed85f2c 180 void GPS_HAPS::Update()
cocorlow 0:6c7f7ed85f2c 181 {
cocorlow 0:6c7f7ed85f2c 182 }
cocorlow 0:6c7f7ed85f2c 183
cocorlow 0:6c7f7ed85f2c 184 void GPS_HAPS::Initialize()
cocorlow 0:6c7f7ed85f2c 185 {
cocorlow 0:6c7f7ed85f2c 186 Satellites = 0;
cocorlow 0:6c7f7ed85f2c 187 while (Satellites <= 4 || Latitude == 0.0f || Longitude == 0.0f || Elevation == 0.0f)
cocorlow 0:6c7f7ed85f2c 188 {
cocorlow 0:6c7f7ed85f2c 189 }
cocorlow 0:6c7f7ed85f2c 190 CalcurateUnit();
cocorlow 0:6c7f7ed85f2c 191 }
cocorlow 0:6c7f7ed85f2c 192
cocorlow 0:6c7f7ed85f2c 193 Vector3 GPS_HAPS::ToUniversalUnit()
cocorlow 0:6c7f7ed85f2c 194 {
cocorlow 0:6c7f7ed85f2c 195 // 東経180度、北緯0度で精度最大
cocorlow 0:6c7f7ed85f2c 196 float pi_2_theta = (Latitude * ((N_S == 'N') ? 1.0f : -1.0f)) * M_PI / 180.0f;
cocorlow 0:6c7f7ed85f2c 197 float pi_phi = ((E_W == 'E') ? Longitude - 180.0f : 180.0f - Longitude) * M_PI / 180.0f;
cocorlow 0:6c7f7ed85f2c 198 float x = - cosf(pi_2_theta) * cosf(pi_phi);
cocorlow 0:6c7f7ed85f2c 199 float y = - cosf(pi_2_theta) * sinf(pi_phi);
cocorlow 0:6c7f7ed85f2c 200 float z = sinf(pi_2_theta);
cocorlow 0:6c7f7ed85f2c 201 Vector3 v(x, y, z);
cocorlow 0:6c7f7ed85f2c 202 return v;
cocorlow 0:6c7f7ed85f2c 203 }
cocorlow 0:6c7f7ed85f2c 204
cocorlow 0:6c7f7ed85f2c 205 Vector3 GPS_HAPS::ToUniversal()
cocorlow 0:6c7f7ed85f2c 206 {
cocorlow 0:6c7f7ed85f2c 207 Vector3 v = ToUniversalUnit();
cocorlow 0:6c7f7ed85f2c 208 return (Radius + Elevation) * v;
cocorlow 0:6c7f7ed85f2c 209 }
cocorlow 0:6c7f7ed85f2c 210
cocorlow 0:6c7f7ed85f2c 211 void GPS_HAPS::CalcurateUnit()
cocorlow 0:6c7f7ed85f2c 212 {
cocorlow 0:6c7f7ed85f2c 213 Vector3 _d = -1.0f * ToUniversalUnit();
cocorlow 0:6c7f7ed85f2c 214
cocorlow 0:6c7f7ed85f2c 215 UniversalZeroPosition = -(Radius+Elevation)*_d;
cocorlow 0:6c7f7ed85f2c 216 Vector3 _z(0.0f, 0.0f, 1.0f);
cocorlow 0:6c7f7ed85f2c 217 Vector3 _e = _d * _z;
cocorlow 0:6c7f7ed85f2c 218 Vector3 _n = _e * _d;
cocorlow 0:6c7f7ed85f2c 219 UniversalZeroUnitN = _n;
cocorlow 0:6c7f7ed85f2c 220 UniversalZeroUnitE = _e;
cocorlow 0:6c7f7ed85f2c 221 UniversalZeroUnitD = _d;
cocorlow 0:6c7f7ed85f2c 222 }
cocorlow 0:6c7f7ed85f2c 223
cocorlow 0:6c7f7ed85f2c 224 void GPS_HAPS::Calcurate()
cocorlow 0:6c7f7ed85f2c 225 {
cocorlow 0:6c7f7ed85f2c 226 UniversalPosition = ToUniversal();
cocorlow 0:6c7f7ed85f2c 227 Vector3 relative = UniversalPosition - UniversalZeroPosition;
cocorlow 0:6c7f7ed85f2c 228 Vector3 _position(relative % UniversalZeroUnitN, relative % UniversalZeroUnitE, relative % UniversalZeroUnitD);
cocorlow 0:6c7f7ed85f2c 229 Position = _position;
cocorlow 0:6c7f7ed85f2c 230 }
cocorlow 0:6c7f7ed85f2c 231
cocorlow 0:6c7f7ed85f2c 232 Vector3 GPS_HAPS::Calcurate(Vector3 position)
cocorlow 0:6c7f7ed85f2c 233 {
cocorlow 0:6c7f7ed85f2c 234 UniversalPosition = ToUniversal();
cocorlow 0:6c7f7ed85f2c 235 Vector3 relative = position - UniversalZeroPosition;
cocorlow 0:6c7f7ed85f2c 236 Vector3 _position(relative % UniversalZeroUnitN, relative % UniversalZeroUnitE, relative % UniversalZeroUnitD);
cocorlow 0:6c7f7ed85f2c 237 return _position;
cocorlow 0:6c7f7ed85f2c 238 }