HAPSRG / GPS_HAPS

GPS GNSS

Dependencies:   Vector3

GPS_HAPS.cpp@4:708e05b10257, 2021-09-08 (annotated)

Committer:
cocorlow
Date:
Wed Sep 08 13:02:17 2021 +0000
Revision:
4:708e05b10257
Parent:
3:b0d594e5d9e2
Child:
5:f23265e8d4aa
Calcurate->Calculate

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 3:b0d594e5d9e2 78 if (abs(latitude) < ERROR_F)
cocorlow 3:b0d594e5d9e2 79 {
cocorlow 3:b0d594e5d9e2 80 Latitude = -1.0f;
cocorlow 3:b0d594e5d9e2 81 }
cocorlow 3:b0d594e5d9e2 82 else
cocorlow 3:b0d594e5d9e2 83 {
cocorlow 3:b0d594e5d9e2 84 float lat = (int)latitude/100;
cocorlow 3:b0d594e5d9e2 85 Latitude = lat + (latitude-lat*100)/60.0f;
cocorlow 3:b0d594e5d9e2 86 }
cocorlow 3:b0d594e5d9e2 87 if (abs(longitude) < ERROR_F)
cocorlow 3:b0d594e5d9e2 88 {
cocorlow 3:b0d594e5d9e2 89 Longitude = -1.0f;
cocorlow 3:b0d594e5d9e2 90 }
cocorlow 3:b0d594e5d9e2 91 else
cocorlow 3:b0d594e5d9e2 92 {
cocorlow 3:b0d594e5d9e2 93 float lon = (int)longitude/100;
cocorlow 3:b0d594e5d9e2 94 Longitude = lon + (longitude-lon*100)/60.0f;
cocorlow 3:b0d594e5d9e2 95 }
cocorlow 0:6c7f7ed85f2c 96 N_S = ns;
cocorlow 0:6c7f7ed85f2c 97 E_W = ew;
cocorlow 0:6c7f7ed85f2c 98 Quality = fq;
cocorlow 0:6c7f7ed85f2c 99 Satellites = nst;
cocorlow 3:b0d594e5d9e2 100 if (abs(altitude) < ERROR_F)
cocorlow 3:b0d594e5d9e2 101 {
cocorlow 3:b0d594e5d9e2 102 Altitude = -1.0f;
cocorlow 3:b0d594e5d9e2 103 }
cocorlow 3:b0d594e5d9e2 104 {
cocorlow 3:b0d594e5d9e2 105 Altitude = altitude;
cocorlow 3:b0d594e5d9e2 106 }
cocorlow 0:6c7f7ed85f2c 107 // pc.printf("%s\n", cmd);
cocorlow 0:6c7f7ed85f2c 108 // 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 109 }
cocorlow 0:6c7f7ed85f2c 110
cocorlow 0:6c7f7ed85f2c 111 // // Satellite status
cocorlow 0:6c7f7ed85f2c 112 // else if(strncmp(cmd,"$GPGSA", 6) == 0)
cocorlow 0:6c7f7ed85f2c 113 // {
cocorlow 0:6c7f7ed85f2c 114 // sscanf(cmd, "$GPGSA,%c,%d,%d", &tf, &fix, &nst);
cocorlow 0:6c7f7ed85f2c 115 // pc.printf("GPGSA Type fix: %c, 3D fix: %d, number of sat: %d\r\n", tf, fix, nst);
cocorlow 0:6c7f7ed85f2c 116 // }
cocorlow 0:6c7f7ed85f2c 117
cocorlow 0:6c7f7ed85f2c 118 // Geographic position, Latitude and Longitude
cocorlow 0:6c7f7ed85f2c 119 // else if(strncmp(cmd,"$GPGLL", 6) == 0)
cocorlow 0:6c7f7ed85f2c 120 // {
cocorlow 0:6c7f7ed85f2c 121 // sscanf(cmd, "$GPGLL,%f,%c,%f,%c,%f", &latitude, &ns, &longitude, &ew, &timefix);
cocorlow 0:6c7f7ed85f2c 122 // pc.printf("GPGLL Latitude: %f %c, Longitude: %f %c, Fix taken at: %f\n", latitude, ns, longitude, ew, timefix);
cocorlow 0:6c7f7ed85f2c 123 // }
cocorlow 0:6c7f7ed85f2c 124
cocorlow 0:6c7f7ed85f2c 125 // Geographic position, Latitude and Longitude
cocorlow 0:6c7f7ed85f2c 126 // $GNRMC,035344.00,A,3542.91153,N,13945.74153,E,0.239,,070921,,,A,V*15
cocorlow 0:6c7f7ed85f2c 127 else if(strstr(cmd,"RMC") != NULL)
cocorlow 0:6c7f7ed85f2c 128 {
cocorlow 0:6c7f7ed85f2c 129 sscanf(cmd, "%[^,],%f,%c,%f,%c,%f,%c,%f,,%d", s, &timefix, &status, &latitude, &ns, &longitude, &ew, &speed, &date);
cocorlow 0:6c7f7ed85f2c 130 // pc.printf("%s,%f,%c,%f,%c,%f,%c,%f,,%d", s, timefix, status, latitude, ns, longitude, ew, speed, date);
cocorlow 3:b0d594e5d9e2 131 if (abs(timefix) > ERROR_F)
cocorlow 3:b0d594e5d9e2 132 {
cocorlow 3:b0d594e5d9e2 133 float time = timefix + 90000.00f;
cocorlow 3:b0d594e5d9e2 134 Hours = int(time) / 10000;
cocorlow 3:b0d594e5d9e2 135 Minutes = (int(time) % 10000) / 100;
cocorlow 3:b0d594e5d9e2 136 Seconds = int(time) % 100;
cocorlow 3:b0d594e5d9e2 137 Milliseconds = (int)(((time - (Hours*10000+Minutes*100+Seconds)))/0.001f);
cocorlow 3:b0d594e5d9e2 138 Time = Hours*3600000+Minutes*60000+Seconds*1000+Milliseconds;
cocorlow 3:b0d594e5d9e2 139 if (abs(speed) < ERROR_F)
cocorlow 3:b0d594e5d9e2 140 {
cocorlow 3:b0d594e5d9e2 141 Speed = -1.0f;
cocorlow 3:b0d594e5d9e2 142 }
cocorlow 3:b0d594e5d9e2 143 else
cocorlow 3:b0d594e5d9e2 144 {
cocorlow 3:b0d594e5d9e2 145 Speed = speed * 0.514444f;
cocorlow 3:b0d594e5d9e2 146 }
cocorlow 3:b0d594e5d9e2 147 Year = date % 100;
cocorlow 3:b0d594e5d9e2 148 date /= 100;
cocorlow 3:b0d594e5d9e2 149 Month = date % 100;
cocorlow 3:b0d594e5d9e2 150 date /= 100;
cocorlow 3:b0d594e5d9e2 151 Day = date;
cocorlow 3:b0d594e5d9e2 152 }
cocorlow 0:6c7f7ed85f2c 153 // pc.printf("%s\n", cmd);
cocorlow 0:6c7f7ed85f2c 154 // 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 155 }
cocorlow 0:6c7f7ed85f2c 156
cocorlow 0:6c7f7ed85f2c 157 //
cocorlow 0:6c7f7ed85f2c 158 // $GNVTG,,T,,M,0.444,N,0.822,K,A*31
cocorlow 0:6c7f7ed85f2c 159 else if(strstr(cmd,"VTG") != NULL)
cocorlow 0:6c7f7ed85f2c 160 {
cocorlow 0:6c7f7ed85f2c 161 // pc.printf("%s\n", cmd);
cocorlow 0:6c7f7ed85f2c 162 Direction = -1.0f;
cocorlow 0:6c7f7ed85f2c 163 if(strstr(cmd, "VTG,,") == NULL){
cocorlow 0:6c7f7ed85f2c 164 sscanf(cmd, "%[^,],%f", s, &Direction);
cocorlow 0:6c7f7ed85f2c 165 // pc.printf("<<%f\r\n", Direction);
cocorlow 0:6c7f7ed85f2c 166 // pc.printf("its a Vector Track message.\n");
cocorlow 0:6c7f7ed85f2c 167 }
cocorlow 0:6c7f7ed85f2c 168 }
cocorlow 0:6c7f7ed85f2c 169
cocorlow 0:6c7f7ed85f2c 170 // else if(strncmp(cmd,"$GNGGA", 6) == 0)
cocorlow 0:6c7f7ed85f2c 171 // {
cocorlow 0:6c7f7ed85f2c 172 // sscanf(cmd, "$GNGGA,%f,%f,%c,%f,%c,%d,%d,%*f,%f", &timefix, &latitude, &ns, &longitude, &ew, &fq, &nst, &altitude);
cocorlow 0:6c7f7ed85f2c 173 // 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 174 // parseTime(timefix);
cocorlow 0:6c7f7ed85f2c 175 // pc.printf("Time: %d:%d:%d\n", h_time, m_time, s_time);
cocorlow 0:6c7f7ed85f2c 176 // }
cocorlow 0:6c7f7ed85f2c 177
cocorlow 0:6c7f7ed85f2c 178 // else if(strncmp(cmd,"$GNGSA", 6) == 0)
cocorlow 0:6c7f7ed85f2c 179 // {
cocorlow 0:6c7f7ed85f2c 180 // sscanf(cmd, "$GNGSA,%c,%d,%d", &tf, &fix, &nst);
cocorlow 0:6c7f7ed85f2c 181 // pc.printf("GNGSA Type fix: %c, 3D fix: %d, number of sat: %d\r\n", tf, fix, nst);
cocorlow 0:6c7f7ed85f2c 182 // }
cocorlow 0:6c7f7ed85f2c 183
cocorlow 0:6c7f7ed85f2c 184 // else if(strncmp(cmd,"$GPGSV", 6) == 0)
cocorlow 0:6c7f7ed85f2c 185 // {
cocorlow 0:6c7f7ed85f2c 186 // // pc.printf("its a Satellite details message.\n");
cocorlow 0:6c7f7ed85f2c 187 // }
cocorlow 0:6c7f7ed85f2c 188
cocorlow 0:6c7f7ed85f2c 189 // else if(strncmp(cmd,"$GNGLL", 6) == 0)
cocorlow 0:6c7f7ed85f2c 190 // {
cocorlow 0:6c7f7ed85f2c 191 // sscanf(cmd, "$GNGLL,%f,%c,%f,%c,%f", &latitude, &ns, &longitude, &ew, &timefix);
cocorlow 0:6c7f7ed85f2c 192 // pc.printf("GNGLL Latitude: %f %c, Longitude: %f %c, Fix taken at: %f\n", latitude, ns, longitude, ew, timefix);
cocorlow 0:6c7f7ed85f2c 193 // }
cocorlow 0:6c7f7ed85f2c 194
cocorlow 0:6c7f7ed85f2c 195 // else
cocorlow 0:6c7f7ed85f2c 196 // {
cocorlow 0:6c7f7ed85f2c 197 // pc.printf("Unknown message type\n");
cocorlow 0:6c7f7ed85f2c 198 // }
cocorlow 0:6c7f7ed85f2c 199 }
cocorlow 0:6c7f7ed85f2c 200
cocorlow 0:6c7f7ed85f2c 201 void GPS_HAPS::Punctuate()
cocorlow 0:6c7f7ed85f2c 202 {
cocorlow 0:6c7f7ed85f2c 203 }
cocorlow 0:6c7f7ed85f2c 204
cocorlow 0:6c7f7ed85f2c 205 void GPS_HAPS::Attach()
cocorlow 0:6c7f7ed85f2c 206 {
cocorlow 0:6c7f7ed85f2c 207 serial.attach(this, &GPS_HAPS::Receive, Serial::RxIrq);
cocorlow 0:6c7f7ed85f2c 208 }
cocorlow 0:6c7f7ed85f2c 209
cocorlow 0:6c7f7ed85f2c 210 void GPS_HAPS::Update()
cocorlow 0:6c7f7ed85f2c 211 {
cocorlow 0:6c7f7ed85f2c 212 }
cocorlow 0:6c7f7ed85f2c 213
cocorlow 0:6c7f7ed85f2c 214 void GPS_HAPS::Initialize()
cocorlow 0:6c7f7ed85f2c 215 {
cocorlow 0:6c7f7ed85f2c 216 Satellites = 0;
cocorlow 0:6c7f7ed85f2c 217 while (Satellites <= 4 || Latitude == 0.0f || Longitude == 0.0f || Elevation == 0.0f)
cocorlow 0:6c7f7ed85f2c 218 {
cocorlow 0:6c7f7ed85f2c 219 }
cocorlow 4:708e05b10257 220 CalculateUnit();
cocorlow 0:6c7f7ed85f2c 221 }
cocorlow 0:6c7f7ed85f2c 222
cocorlow 0:6c7f7ed85f2c 223 Vector3 GPS_HAPS::ToUniversalUnit()
cocorlow 0:6c7f7ed85f2c 224 {
cocorlow 0:6c7f7ed85f2c 225 // 東経180度、北緯0度で精度最大
cocorlow 0:6c7f7ed85f2c 226 float pi_2_theta = (Latitude * ((N_S == 'N') ? 1.0f : -1.0f)) * M_PI / 180.0f;
cocorlow 0:6c7f7ed85f2c 227 float pi_phi = ((E_W == 'E') ? Longitude - 180.0f : 180.0f - Longitude) * M_PI / 180.0f;
cocorlow 0:6c7f7ed85f2c 228 float x = - cosf(pi_2_theta) * cosf(pi_phi);
cocorlow 0:6c7f7ed85f2c 229 float y = - cosf(pi_2_theta) * sinf(pi_phi);
cocorlow 0:6c7f7ed85f2c 230 float z = sinf(pi_2_theta);
cocorlow 0:6c7f7ed85f2c 231 Vector3 v(x, y, z);
cocorlow 0:6c7f7ed85f2c 232 return v;
cocorlow 0:6c7f7ed85f2c 233 }
cocorlow 0:6c7f7ed85f2c 234
cocorlow 0:6c7f7ed85f2c 235 Vector3 GPS_HAPS::ToUniversal()
cocorlow 0:6c7f7ed85f2c 236 {
cocorlow 0:6c7f7ed85f2c 237 Vector3 v = ToUniversalUnit();
cocorlow 0:6c7f7ed85f2c 238 return (Radius + Elevation) * v;
cocorlow 0:6c7f7ed85f2c 239 }
cocorlow 0:6c7f7ed85f2c 240
cocorlow 4:708e05b10257 241 void GPS_HAPS::CalculateUnit()
cocorlow 0:6c7f7ed85f2c 242 {
cocorlow 0:6c7f7ed85f2c 243 Vector3 _d = -1.0f * ToUniversalUnit();
cocorlow 0:6c7f7ed85f2c 244
cocorlow 0:6c7f7ed85f2c 245 UniversalZeroPosition = -(Radius+Elevation)*_d;
cocorlow 0:6c7f7ed85f2c 246 Vector3 _z(0.0f, 0.0f, 1.0f);
cocorlow 0:6c7f7ed85f2c 247 Vector3 _e = _d * _z;
cocorlow 0:6c7f7ed85f2c 248 Vector3 _n = _e * _d;
cocorlow 0:6c7f7ed85f2c 249 UniversalZeroUnitN = _n;
cocorlow 0:6c7f7ed85f2c 250 UniversalZeroUnitE = _e;
cocorlow 0:6c7f7ed85f2c 251 UniversalZeroUnitD = _d;
cocorlow 0:6c7f7ed85f2c 252 }
cocorlow 0:6c7f7ed85f2c 253
cocorlow 4:708e05b10257 254 void GPS_HAPS::Calculate()
cocorlow 0:6c7f7ed85f2c 255 {
cocorlow 0:6c7f7ed85f2c 256 UniversalPosition = ToUniversal();
cocorlow 0:6c7f7ed85f2c 257 Vector3 relative = UniversalPosition - UniversalZeroPosition;
cocorlow 0:6c7f7ed85f2c 258 Vector3 _position(relative % UniversalZeroUnitN, relative % UniversalZeroUnitE, relative % UniversalZeroUnitD);
cocorlow 0:6c7f7ed85f2c 259 Position = _position;
cocorlow 0:6c7f7ed85f2c 260 }
cocorlow 0:6c7f7ed85f2c 261
cocorlow 4:708e05b10257 262 Vector3 GPS_HAPS::Calculate(Vector3 position)
cocorlow 0:6c7f7ed85f2c 263 {
cocorlow 0:6c7f7ed85f2c 264 UniversalPosition = ToUniversal();
cocorlow 0:6c7f7ed85f2c 265 Vector3 relative = position - UniversalZeroPosition;
cocorlow 0:6c7f7ed85f2c 266 Vector3 _position(relative % UniversalZeroUnitN, relative % UniversalZeroUnitE, relative % UniversalZeroUnitD);
cocorlow 0:6c7f7ed85f2c 267 return _position;
cocorlow 0:6c7f7ed85f2c 268 }