From 3daedae2da9a9b012694addcdc605015a6f42220 Mon Sep 17 00:00:00 2001 From: pkkrusty <79770016+pkkrusty@users.noreply.github.com> Date: Sat, 25 Mar 2023 14:37:38 +0000 Subject: [PATCH] Add speed and heading info to GPS driver (#18035) * Add speed heading to driver Added NAV-VELNED messages to driver, allowing speed and heading info on the main web page and JSON messages. Probably could be optimized; mostly another exercise in copy/paste * wrap velocity code in #define USE_GPS_VELOCITY Code space increases by 0k4, not sure about RAM (don't know how to measure). But added a struct with 320 bits, so maybe 40 bytes? * Add documentation up top --------- Co-authored-by: Theo Arends <11044339+arendst@users.noreply.github.com> --- tasmota/tasmota_xsns_sensor/xsns_60_GPS.ino | 109 ++++++++++++++++++-- 1 file changed, 102 insertions(+), 7 deletions(-) diff --git a/tasmota/tasmota_xsns_sensor/xsns_60_GPS.ino b/tasmota/tasmota_xsns_sensor/xsns_60_GPS.ino index f284418ba..fc53bd920 100644 --- a/tasmota/tasmota_xsns_sensor/xsns_60_GPS.ino +++ b/tasmota/tasmota_xsns_sensor/xsns_60_GPS.ino @@ -48,6 +48,7 @@ Driver is tested on a NEO-6m and a Beitian-220. Series 7 should work too. This a - Web-UI - simplified NTP-server and UART-over-TCP/IP-bridge (virtual serial port) - command interface +- velocity and heading information with #define USE_GPS_VELOCITY ## Usage: The serial pins are GPS_RX and GPS_TX, no further installation steps needed. To get more debug information compile it with option "DEBUG_TASMOTA_SENSOR". @@ -129,7 +130,7 @@ const char S_JSON_UBX_COMMAND_NVALUE[] PROGMEM = "{\"" D_CMND_UBX "%s\":%d}"; const char kUBXTypes[] PROGMEM = "UBX"; -#define UBX_LAT_LON_THRESHOLD 1000 // filter out some noise of local drift +#define UBX_LAT_LON_THRESHOLD 100 // filter out some noise of local drift #define UBX_SERIAL_BUFFER_SIZE 256 #define UBX_TCP_PORT 1234 @@ -153,13 +154,18 @@ const char UBLOX_INIT[] PROGMEM = { 0xB5,0x62,0x06,0x01,0x08,0x00,0x01,0x02,0x00,0x00,0x00,0x00,0x00,0x00,0x12,0xB9, //NAV-POSLLH off 0xB5,0x62,0x06,0x01,0x08,0x00,0x01,0x03,0x00,0x00,0x00,0x00,0x00,0x00,0x13,0xC0, //NAV-STATUS off 0xB5,0x62,0x06,0x01,0x08,0x00,0x01,0x21,0x00,0x00,0x00,0x00,0x00,0x00,0x31,0x92, //NAV-TIMEUTC off +#ifdef USE_GPS_VELOCITY + 0xB5,0x62,0x06,0x01,0x08,0x00,0x01,0x12,0x00,0x00,0x00,0x00,0x00,0x00,0x22,0x29, //NAV-VELNED off +#endif // Enable UBX // 0xB5,0x62,0x06,0x01,0x08,0x00,0x01,0x07,0x00,0x01,0x00,0x00,0x00,0x00,0x18,0xE1, //NAV-PVT on 0xB5,0x62,0x06,0x01,0x08,0x00,0x01,0x02,0x00,0x01,0x00,0x00,0x00,0x00,0x13,0xBE, //NAV-POSLLH on 0xB5,0x62,0x06,0x01,0x08,0x00,0x01,0x03,0x00,0x01,0x00,0x00,0x00,0x00,0x14,0xC5, //NAV-STATUS on 0xB5,0x62,0x06,0x01,0x08,0x00,0x01,0x21,0x00,0x01,0x00,0x00,0x00,0x00,0x32,0x97, //NAV-TIMEUTC on - +#ifdef USE_GPS_VELOCITY + 0xB5,0x62,0x06,0x01,0x08,0x00,0x01,0x12,0x00,0x01,0x00,0x00,0x00,0x00,0x23,0x2E, //NAV-VELNED on +#endif // Rate - we will not reset it for the moment after restart // 0xB5,0x62,0x06,0x08,0x06,0x00,0x64,0x00,0x01,0x00,0x01,0x00,0x7A,0x12, //(10Hz) // 0xB5,0x62,0x06,0x08,0x06,0x00,0xC8,0x00,0x01,0x00,0x01,0x00,0xDE,0x6A, //(5Hz) @@ -174,6 +180,9 @@ struct UBX_t { const char NAV_POSLLH_HEADER[2] = { 0x01, 0x02 }; const char NAV_STATUS_HEADER[2] = { 0x01, 0x03 }; const char NAV_TIME_HEADER[2] = { 0x01, 0x21 }; +#ifdef USE_GPS_VELOCITY + const char NAV_VEL_HEADER[2] = { 0x01, 0x12 }; +#endif struct entry_t { int32_t lat; //raw sensor value @@ -238,7 +247,22 @@ struct UBX_t { uint8_t padding:5; } valid; }; - +#ifdef USE_GPS_VELOCITY + struct NAV_VEL { + uint8_t cls; + uint8_t id; + uint16_t len; + uint32_t iTOW; + int32_t velN; + int32_t velE; //bit 0 - gpsfix valid + int32_t velD; + uint32_t speed; + uint32_t gSpeed; + int32_t heading; + uint32_t sAcc; + uint32_t cAcc; + }; +#endif struct CFG_RATE { uint8_t cls; //0x06 uint8_t id; //0x08 @@ -276,6 +300,9 @@ struct UBX_t { NAV_POSLLH navPosllh; NAV_STATUS navStatus; NAV_TIME_UTC navTime; +#ifdef USE_GPS_VELOCITY + NAV_VEL navVel; +#endif POLL_MSG pollMsg; CFG_RATE cfgRate; } Message; @@ -291,6 +318,9 @@ enum UBXMsgType { MT_NAV_POSLLH, MT_NAV_STATUS, MT_NAV_TIME, +#ifdef USE_GPS_VELOCITY + MT_NAV_VEL, +#endif MT_POLL }; @@ -426,6 +456,13 @@ uint32_t UBXprocessGPS() payloadSize = sizeof(UBX_t::NAV_TIME_UTC); DEBUG_SENSOR_LOG(PSTR("UBX: got NAV_TIME_UTC")); } +#ifdef USE_GPS_VELOCITY + else if ( UBXcompareMsgHeader(UBX.NAV_VEL_HEADER) ) { + currentMsgType = MT_NAV_VEL; + payloadSize = sizeof(UBX_t::NAV_VEL); + DEBUG_SENSOR_LOG(PSTR("UBX: got NAV_VEL")); + } +#endif else { // unknown message type, bail fpos = 0; @@ -506,7 +543,7 @@ void UBXsendRecord(uint8_t *buf) void UBXsendFooter(void) { - Webserver->sendContent(F("\n\n")); + Webserver->sendContent(F("\n\n")); Webserver->sendContent(""); Rtc.user_time_entry = false; // we have blocked the main loop and want a new valid time } @@ -590,8 +627,15 @@ void UBXSelectMode(uint16_t mode) break; case 10: UBX.mode.runningNTP = false; +#ifdef USE_GPS_VELOCITY + UBXsendCFGLine(11); //NAV-POSLLH on + UBXsendCFGLine(12); //NAV-STATUS on + UBXsendCFGLine(14); //NAV-VELNED on +#endif +#ifndef USE_GPS_VELOCITY UBXsendCFGLine(10); //NAV-POSLLH on UBXsendCFGLine(11); //NAV-STATUS on +#endif break; case 11: UBX.mode.forceUTCupdate = true; @@ -646,7 +690,11 @@ bool UBXHandlePOSLLH() if (UBX.mode.runningNTP){ // after receiving pos-data at least once -> go to pure NTP-mode UBXsendCFGLine(7); //NAV-POSLLH off UBXsendCFGLine(8); //NAV-STATUS off +#ifdef USE_GPS_VELOCITY + UBXsendCFGLine(10); //NAV-VELNED off +#endif } + //UBX_LAT_LON_THRESHOLD = 20 * UBX.Message.navPosllh.hAcc; return true; // new position } else { DEBUG_SENSOR_LOG(PSTR("UBX: no valid position data")); @@ -654,6 +702,20 @@ bool UBXHandlePOSLLH() return false; // no GPS-fix } +#ifdef USE_GPS_VELOCITY +void UBXHandleVEL() + { + DEBUG_SENSOR_LOG(PSTR("UBX: iTOWvel: %u"),UBX.Message.navVel.iTOW); + if (UBX.state.gpsFix>1) { + DEBUG_SENSOR_LOG(PSTR("UBX: speed: %d"), UBX.Message.navVel.gSpeed); + DEBUG_SENSOR_LOG(PSTR("UBX: heading: %i"), UBX.Message.navVel.heading); + DEBUG_SENSOR_LOG(PSTR("UBX: spd accuracy: %i"), UBX.Message.navVel.sAcc); + DEBUG_SENSOR_LOG(PSTR("UBX: hdng accuracy: %i"), UBX.Message.navVel.cAcc); + } + +} +#endif + void UBXHandleSTATUS() { DEBUG_SENSOR_LOG(PSTR("UBX: gpsFix: %u, valid: %u"), UBX.Message.navStatus.gpsFix, (UBX.Message.navStatus.flags)&1); @@ -667,7 +729,7 @@ void UBXHandleSTATUS() void UBXHandleTIME() { DEBUG_SENSOR_LOG(PSTR("UBX: UTC-Time: %u-%u-%u %u:%u:%u"), UBX.Message.navTime.year, UBX.Message.navTime.month ,UBX.Message.navTime.day,UBX.Message.navTime.hour,UBX.Message.navTime.min,UBX.Message.navTime.sec); - if ((UBX.Message.navTime.valid.UTC == 1) && (UBX.Message.navTime.year >= 2023)) { + if ((UBX.Message.navTime.valid.UTC == 1) && (UBX.Message.navTime.year >= 2023)) { UBX.state.timeOffset = millis(); // iTOW%1000 should be 0 here, when NTP-server is enabled and in "pure mode" DEBUG_SENSOR_LOG(PSTR("UBX: UTC-Time is valid")); bool resync = (Rtc.utc_time > UBX.utc_time); // Sync local time every hour @@ -744,6 +806,11 @@ void UBXLoop(void) case MT_NAV_TIME: UBXHandleTIME(); break; +#ifdef USE_GPS_VELOCITY + case MT_NAV_VEL: + UBXHandleVEL(); + break; +#endif default: UBXHandleOther(); break; @@ -791,7 +858,14 @@ void UBXLoop(void) "{s} GPS altitude {m}%s m{e}" "{s} GPS hor. Accuracy {m}%s m{e}" "{s} GPS vert. Accuracy {m}%s m{e}" - "{s} GPS sat-fix status {m}%s{e}"; + "{s} GPS sat-fix status {m}%s{e}" +#ifdef USE_GPS_VELOCITY + "{s} GPS Speed {m}%s{e}" + "{s} GPS Heading {m}%s{e}" + "{s} GPS Heading Acc {m}%s{e}" + "{s} GPS Speed Acc {m}%s{e}" +#endif + ; const char kGPSFix0[] PROGMEM = "no fix"; const char kGPSFix1[] PROGMEM = "dead reckoning only"; @@ -815,11 +889,23 @@ void UBXShow(bool json) char alt[12]; char hAcc[12]; char vAcc[12]; + #ifdef USE_GPS_VELOCITY + char spd[12]; + char hdng[12]; + char cAcc[12]; + char sAcc[12]; + #endif dtostrfd((double)UBX.rec_buffer.values.lat/10000000.0f,7,lat); dtostrfd((double)UBX.rec_buffer.values.lon/10000000.0f,7,lon); dtostrfd((double)UBX.state.last_alt/1000.0f,3,alt); dtostrfd((double)UBX.state.last_vAcc/1000.0f,3,hAcc); dtostrfd((double)UBX.state.last_hAcc/1000.0f,3,vAcc); + #ifdef USE_GPS_VELOCITY + dtostrfd((double)UBX.Message.navVel.gSpeed/27.778f,1,spd); + dtostrfd((double)UBX.Message.navVel.heading/100000.0f,1,hdng); + dtostrfd((double)UBX.Message.navVel.cAcc/100000.0f,2,cAcc); + dtostrfd((double)UBX.Message.navVel.sAcc/100000.0f,2,sAcc); + #endif if (json) { ResponseAppend_P(PSTR(",\"GPS\":{")); @@ -827,7 +913,11 @@ void UBXShow(bool json) uint32_t i = UBX.state.log_interval / 10; ResponseAppend_P(PSTR("\"fil\":%u,\"int\":%u}"), UBX.mode.filter_noise, i); } else { - ResponseAppend_P(PSTR("\"lat\":%s,\"lon\":%s,\"alt\":%s,\"hAcc\":%s,\"vAcc\":%s,\"fix\":\"%s\"}"), lat, lon, alt, hAcc, vAcc, kGPSFix[UBX.state.gpsFix]); + ResponseAppend_P(PSTR("\"lat\":%s,\"lon\":%s,\"alt\":%s,\"hAcc\":%s,\"vAcc\":%s,\"fix\":\"%s\""), lat, lon, alt, hAcc, vAcc, kGPSFix[UBX.state.gpsFix]); +#ifdef USE_GPS_VELOCITY + ResponseAppend_P(PSTR(,\"spd\":%s,\"hdng\":%s,\"cAcc\":%s,\"sAcc\":%s"), spd, hdng, cAcc, sAcc); +#endif + ResponseAppend_P(PSTR("}")); } #ifdef USE_FLOG ResponseAppend_P(PSTR(",\"FLOG\":{\"rec\":%u,\"mode\":%u,\"sec\":%u}"), Flog->recording, Flog->mode, Flog->sectors_left); @@ -835,7 +925,12 @@ void UBXShow(bool json) UBX.mode.send_UI_only = false; #ifdef USE_WEBSERVER } else { +#ifdef USE_GPS_VELOCITY + WSContentSend_PD(HTTP_SNS_GPS, lat, lon, alt, hAcc, vAcc, kGPSFix[UBX.state.gpsFix], spd, hdng, cAcc, sAcc); +#endif +#ifndef USE_GPS_VELOCITY WSContentSend_PD(HTTP_SNS_GPS, lat, lon, alt, hAcc, vAcc, kGPSFix[UBX.state.gpsFix]); +#endif //WSContentSend_P(UBX_GOOGLE_MAPS, lat, lon); #ifdef DEBUG_TASMOTA_SENSOR #ifdef USE_FLOG