52 lines
1.9 KiB
C++
52 lines
1.9 KiB
C++
#include "Telemetry.hpp"
|
|
#include "NetMgr.hpp"
|
|
|
|
namespace Telemetry {
|
|
|
|
bool readGNSS(double& lat, double& lng, double& alt, double& speed, double& heading) {
|
|
lat=lng=alt=speed=heading=0;
|
|
String r = NetMgr::sendAT("AT+CGNSINF", 3000);
|
|
// +CGNSINF: 1,1,yyyyMMddhhmmss.sss,lat,lng,alt, ... ,speed_over_ground, ... ,course
|
|
int p = r.indexOf("+CGNSINF:");
|
|
if (p < 0) return false;
|
|
// crude split
|
|
int first = r.indexOf('\n', p); // skip header if present
|
|
String line = (first!=-1) ? r.substring(p, first) : r.substring(p);
|
|
// safer: take last line
|
|
int lastNl = r.lastIndexOf('\n');
|
|
line = (lastNl!=-1) ? r.substring(lastNl+1) : line;
|
|
line.trim();
|
|
|
|
// tokenization
|
|
int commas[10]; int count=0;
|
|
for (int i=0;i<(int)line.length() && count<10;i++) if (line[i]==',') commas[count++]=i;
|
|
if (count < 6) return false;
|
|
|
|
lat = line.substring(commas[1]+1, commas[2]).toDouble();
|
|
lng = line.substring(commas[2]+1, commas[3]).toDouble();
|
|
alt = line.substring(commas[3]+1, commas[4]).toDouble();
|
|
// speed (km/h) field may be later; do a find:
|
|
int next = line.indexOf(',', commas[4]+1);
|
|
next = line.indexOf(',', next+1);
|
|
next = line.indexOf(',', next+1);
|
|
int speedStart = next+1;
|
|
int speedEnd = line.indexOf(',', speedStart);
|
|
if (speedStart>0 && speedEnd>speedStart) speed = line.substring(speedStart, speedEnd).toDouble();
|
|
// heading/course similarly if needed
|
|
return true;
|
|
}
|
|
|
|
String buildJson(const String& imei, double lat, double lng, double alt, double speed, double heading) {
|
|
String j = String("{\"device_id\":\"") + imei + "\","
|
|
+ "\"lat\":" + String(lat,6) + ","
|
|
+ "\"lng\":" + String(lng,6) + ","
|
|
+ "\"speed\":" + String(speed,2) + ","
|
|
+ "\"altitude\":" + String(alt,2) + "}";
|
|
return j;
|
|
}
|
|
|
|
String buildNoPowerJson(const String& imei) {
|
|
return String("{\"device_id\":\"") + imei + "\",\"car_power\":false}";
|
|
}
|
|
|
|
} // namespace Telemetry namespace Telemetry
|