From b27bc945430cd8ebbf9e216da01fd7051c626b1b Mon Sep 17 00:00:00 2001
From: Thorsten von Eicken <tve@voneicken.com>
Date: Sat, 14 Sep 2024 16:19:09 -0700
Subject: [PATCH] lr1110: added script to eval position using LoRaCloud

---
 .../LR11x0_Gnss_Scan/LR11x0_Gnss_Scan.ino     |  47 ++++++--
 examples/LR11x0/LR11x0_Gnss_Scan/README.md    |  67 ++++++++----
 .../LR11x0/LR11x0_Gnss_Scan/location-sample.h |   4 +
 .../LR11x0/LR11x0_Gnss_Scan/lora-cloud.sample |   4 +
 .../LR11x0/LR11x0_Gnss_Scan/lora-cloud.sh     | 103 ++++++++++++++++++
 src/modules/LR11x0/LR11x0.cpp                 |   3 +-
 6 files changed, 198 insertions(+), 30 deletions(-)
 create mode 100644 examples/LR11x0/LR11x0_Gnss_Scan/location-sample.h
 create mode 100644 examples/LR11x0/LR11x0_Gnss_Scan/lora-cloud.sample
 create mode 100755 examples/LR11x0/LR11x0_Gnss_Scan/lora-cloud.sh

diff --git a/examples/LR11x0/LR11x0_Gnss_Scan/LR11x0_Gnss_Scan.ino b/examples/LR11x0/LR11x0_Gnss_Scan/LR11x0_Gnss_Scan.ino
index 45bbca1ed..91ddf364a 100644
--- a/examples/LR11x0/LR11x0_Gnss_Scan/LR11x0_Gnss_Scan.ino
+++ b/examples/LR11x0/LR11x0_Gnss_Scan/LR11x0_Gnss_Scan.ino
@@ -1,10 +1,7 @@
 // Semtech LR11x0 GNSS scanner, by Thorsten von Eicken, derived from WiFi scanner example
 // The configuration is for a Seeed WIO Tracker 1110 dev board (red)
 #include <RadioLib.h>
-
-// Real position of GPS in order to calculate real error (assumes stationary GPS)
-#define REAL_LAT (34.499360)
-#define REAL_LON (-119.818190)
+#include "location.h"
 
 // From https://github.com/jgromes/RadioLib/discussions/1101#discussioncomment-9576099
 //                        cs, irq, rst busy
@@ -139,7 +136,6 @@ void printPowerInfo(uint32_t *timing, uint8_t constDemod) {
 // ===== Geographic distance
 
 float haversine(float lat1, float lon1, float lat2, float lon2) { // radians -> meters
-  constexpr float r = 6371; // km
   float avgLat = sin((lat1-lat2)/2);
   float avgLon = sin((lon1-lon2)/2);
   float a = avgLat*avgLat + cos(lat1)*cos(lat2)*avgLon*avgLon;
@@ -150,6 +146,7 @@ float haversine(float lat1, float lon1, float lat2, float lon2) { // radians ->
 
 bool startScan; // time to start next scan
 volatile bool scanFlag = false; // scan completed flag
+uint32_t scanStartAt = 0; // millis() when starting scan
 
 void configRadio() {
   Serial.println(F("\n[LR1110] Initializing ... "));
@@ -234,10 +231,10 @@ void loop() {
       return;
     }
 
-    // ===== Print various information about the scan
-
+    // see whether we have the time
     uint8_t timeErr;
     uint32_t gpsTime, nbUs, timeAcc;
+    uint32_t gotTimeAt = millis();
     state = radio.gnssReadTime(&timeErr, &gpsTime, &nbUs, &timeAcc);
     if (state == RADIOLIB_ERR_NONE) {
       if (timeErr == 0) {
@@ -249,6 +246,41 @@ void loop() {
       }
     }
 
+    uint16_t rsize=0;
+    state = radio.gnssGetResultSize(&rsize);
+    if (state == RADIOLIB_ERR_NONE && rsize > 0) {
+      uint8_t *buf = (uint8_t *)calloc(rsize, 1);
+      state = radio.gnssReadResults(buf, rsize); // handles buf==nullptr
+      if (state == RADIOLIB_ERR_NONE) {
+        switch (buf[0]) {
+        case 1:
+          if (timeErr == 0 && timeAcc < 2000000) {
+            // we got reasonably accurate time info, include in printf
+            // adjust to start of scan with 1s fudget, LoRaCloud is not explicit when to measure
+            // and this seems to get very close to the time it ends up reporting
+            uint32_t adjTime = gpsTime - (gotTimeAt-scanStartAt)/1000 + 1;
+            printf("NAV @%d: ", adjTime);
+          } else {
+            printf("NAV: ");
+          }
+          for (int i=1; i<rsize; i++) printf("%02x", buf[i]);
+          printf("\n");
+          break;
+        case 0:
+          printf("Scan result status: %d\n", buf[1]);
+          break;
+        case 2:
+          printf("Almanac update message (%d bytes)\n", rsize-1);
+          break;
+        default:
+          printf("Unknown result message (dest=%d, size=%d)\n", buf[0], rsize);
+          break;
+        }
+      }
+    }
+
+    // ===== Print various information about the scan
+
     // print position
     {
       uint8_t error=9;
@@ -329,6 +361,7 @@ void loop() {
     // start scanning again
     while (true) {
       Serial.println(F("\n[LR1110] Starting GNSS scan ... "));
+      scanStartAt = millis();
       state = radio.gnssScan(&gnssCount);
       if (state == RADIOLIB_ERR_NONE) {
         scanFlag = 1;
diff --git a/examples/LR11x0/LR11x0_Gnss_Scan/README.md b/examples/LR11x0/LR11x0_Gnss_Scan/README.md
index df4b740f2..dcc894c8b 100644
--- a/examples/LR11x0/LR11x0_Gnss_Scan/README.md
+++ b/examples/LR11x0/LR11x0_Gnss_Scan/README.md
@@ -8,7 +8,7 @@ some of the results.
 It primarily loops performing GNSS scans but also checks the almanac status and asks the LR11x0
 to receive almanac updates from satellite transmissions.
 
-The sketch is configured to work as-is on a Seeed WIO Tracker 1110 dev board (red in color)
+The sketch is configured to work as-is on a Seeed WIO Tracker 1110 dev board (red PCB)
 and using the sketch on different hardware requires small adaptations.
 The LR11x0 almost always uses external RF switches that are controlled through some of its
 pins and which pin controls which switch is board dependent.
@@ -16,38 +16,61 @@ On the WIO Tracker board this sketch uses the Seeed Arduino core.
 
 Currently the sketch also requires `#define RADIOLIB_GODMODE 1` in `BuildOptUser.h` to access
 otherwise private functions in the LR11x0 class.
+In addition, you can set the location of your device in location.h so it prints the actual
+error (expect tens of km since this it's based on almanac data only).
+
+## LoRaCloud evaluation shell script
+
+The sketch outputs NAV lines that can be sent to LoRaCloud in order to resolve the precise
+location (this is what the LR1110 is actually about).
+A relatively simple bash shell script `lora-cloud.sh` reads the sketch output from the serial port,
+detects the NAV lines, sends the information to LoRaCloud, and prints the result interspersed
+with the sketch output.
+
+For the script to work you need to put your creds into `lora-cloud.auth`, see the sample file.
+You should also place your device's coordinates there so it can display the actual position error.
 
 ### Sample output
 
 ```
 [LR1110] Starting GNSS scan ... 
+
 RLB_DBG: GNSS scan start 'medium effort'
 RLB_DBG: Starting GNSS scan with effort=1 resMask=3c nbsvMax=8
-RLB_DBG: GNSS scan done in 12598 ms
-RLB_DBG: Demod status 'Word sync found' (2), info 01
+RLB_DBG: Status: 5 CMD_OK, INTR, mode=1
+RLB_DBG: IRQstatus=00080000 errors=0x0000
+RLB_DBG:     IRQ bit 19 set
+RLB_DBG: GNSS scan done in 10946 ms
+RLB_DBG: Demod status 'Time Of Week found' (3), info 03
 [LR1110] Reading GNSS scan result ...
 RLB_DBG: Last scan mode 'Assisted mode (Time and Assisted Position known)' (3)
-RLB_DBG: Detected 8 SVs:
-RLB_DBG:   SV 0: SNR 13 dB, Doppler -1221 Hz
-RLB_DBG:   SV 1: SNR 10 dB, Doppler 2049 Hz
-RLB_DBG:   SV 2: SNR 10 dB, Doppler 4481 Hz
-RLB_DBG:   SV 3: SNR 7 dB, Doppler 3997 Hz
-RLB_DBG:   SV 4: SNR 6 dB, Doppler 2906 Hz
-RLB_DBG:   SV 5: SNR 5 dB, Doppler -1196 Hz
-RLB_DBG:   SV 6: SNR 4 dB, Doppler 4275 Hz
-RLB_DBG:   SV 7: SNR 2 dB, Doppler -1046 Hz
-GPS time=1726180161 180720us accuracy=242256us unix=2042144943
-Demodulated position (using 8 SVs):
-  One-shot: 34.760742 -119.091797 | accuracy:42[?] xtal:974ppb | real err: 72538m
-  Filtered: 34.497070 -119.707031 | accuracy:29[?] xtal:981ppb | real err: 10189m
+RLB_DBG: Detected 7 SVs:
+RLB_DBG:   SV 0: SNR 12 dB, Doppler 300 Hz
+RLB_DBG:   SV 1: SNR 12 dB, Doppler 3983 Hz
+RLB_DBG:   SV 2: SNR 10 dB, Doppler 3959 Hz
+RLB_DBG:   SV 3: SNR 5 dB, Doppler -931 Hz
+RLB_DBG:   SV 4: SNR 5 dB, Doppler 2529 Hz
+RLB_DBG:   SV 5: SNR 3 dB, Doppler 1815 Hz
+RLB_DBG:   SV 6: SNR 3 dB, Doppler 2698 Hz
+GPS time=1410390673 407205us accuracy=11397us unix=1726355455
+NAV @1410390664: 631052c00000000c94e1b0000007dcc35d722e5565bbe78df146009
+LoRaCloud:
+  Position: 35.12345 -119.12345 alt: 560.39
+  Real error: 26m
+  Claimed accuracy: 110.5m
+  GPS time: 1410390665.293538 -- delta=1s
+  UTC time: 1726355447.293538 -- 2024-09-14T23:10:47+00:00
+Demodulated position (using 7 SVs):
+  One-shot: 35.999999 -119.999999 | accuracy:39[?] xtal:1046ppb | real err: 14721m
+  Filtered: 35.999999 -119.888888 | accuracy:51[?] xtal:1035ppb | real err: 13937m
 Last scan timing, power, and % of power:
-  GPS: total 4.3s/56.6mJ | P1:44.1% Adv:55.9% | capture:38.2% cpu:52.6% sleep:9.2%
+  GPS: total 4.3s/55.8mJ | P1:45.5% Adv:54.5% | capture:38.7% cpu:51.2% sleep:10.0%
   Beidou: total 0.0s/0.0mJ | P1:nan% Adv:nan% | capture:nan% cpu:nan% sleep:nan%
-  GPS demod: total 8.3s/78.8mJ | capture:47.3% cpu:6.6% sleep:46.0%
-  TOTAL: 12.6s/135.5mJ | GPS:41.8% Beidou:0.0% Demod:58.2% Init:0.0%
-GPS almanac status: 'No satellite to update' (0)
-  SVs needing update: 0 (SVs 0 0)
-  Update in 0.0s, first subframe 0, 0 subframes
+  GPS demod: total 6.7s/56.2mJ | capture:46.0% cpu:8.2% sleep:45.8%
+  TOTAL: 11.0s/112.0mJ | GPS:49.8% Beidou:0.0% Demod:50.1% Init:0.0%
+GPS almanac status: 'At least 1 satellite must be updated' (1)
+  SVs needing update: 1 (SVs 63 25)
+  Update in 364.7s, first subframe 4, 2 subframes
 BDU almanac status: 'No satellite to update' (0)
   SVs needing update: 0 (SVs 0 0)
   Update in 0.0s, first subframe 0, 0 subframes
diff --git a/examples/LR11x0/LR11x0_Gnss_Scan/location-sample.h b/examples/LR11x0/LR11x0_Gnss_Scan/location-sample.h
new file mode 100644
index 000000000..be9fb5440
--- /dev/null
+++ b/examples/LR11x0/LR11x0_Gnss_Scan/location-sample.h
@@ -0,0 +1,4 @@
+// Real position of GPS in order to calculate real error (assumes stationary GPS)
+// Rename this file to location.h
+#define REAL_LAT (35.12345)
+#define REAL_LON (-119.12345)
diff --git a/examples/LR11x0/LR11x0_Gnss_Scan/lora-cloud.sample b/examples/LR11x0/LR11x0_Gnss_Scan/lora-cloud.sample
new file mode 100644
index 000000000..e967087a4
--- /dev/null
+++ b/examples/LR11x0/LR11x0_Gnss_Scan/lora-cloud.sample
@@ -0,0 +1,4 @@
+# Replace the data below with yours and rename this file to lora-cloud.auth, used by lora-cloud.sh
+AUTH_TOKEN='AQE000000000000000000000/0000000CQ'
+REAL_LAT=35.12345
+REAL_LON=-119.12345
diff --git a/examples/LR11x0/LR11x0_Gnss_Scan/lora-cloud.sh b/examples/LR11x0/LR11x0_Gnss_Scan/lora-cloud.sh
new file mode 100755
index 000000000..4f7cd9e81
--- /dev/null
+++ b/examples/LR11x0/LR11x0_Gnss_Scan/lora-cloud.sh
@@ -0,0 +1,103 @@
+#! /usr/bin/bash -e
+# Parse the output of the sketch and send the NAV data to LoRaCloud to get the position
+# Uses `curl`` to perform the request, `date`` to convert unix timestamp to readable date, and
+# `bc` to calculate real error
+# This script was developed on Linux, adapting it to MacOS or Windows will require some changes to paths
+# For the auth tokens, create a .lora-cloud.auth file assigning the AUTH_TOKEN variable
+
+SERIAL=$(echo /dev/serial/by-id/usb-Seeed_Wio_Tracker_1110_*)
+BAUD=115200
+
+# URL="https://gls.loracloud.com/api/v3/solve/gnss_lr1110_singleframe" # deprecated
+URL="https://mgs.loracloud.com/api/v1/solve/gnss_lora_edge_singleframe"
+source lora-cloud.auth
+
+# haversine formula for distance
+function haversine() {
+    local lat1=$1
+    local lon1=$2
+    local lat2=$REAL_LAT
+    local lon2=$REAL_LON
+    local expr="
+      torad = 3.141593/180
+      avglat = s(($lat1- $lat2)*torad/2)
+      avglon = s(($lon1- $lon2)*torad/2)
+      a = avglat*avglat + c($lat1*torad)*c($lat2*torad)*avglon*avglon
+      dist = 12742000 * a(sqrt(a) / sqrt(1-a)) *3
+      scale=0
+      dist/3
+    "
+    # echo "  Expr: $expr"m
+    echo "  Real error: $(bc -l <<<"$expr")m"
+}
+
+# query LoRaCloud
+function query() {
+    local data=$1
+    local time=$2
+    local accuracy=$3
+    echo "LoRaCloud:"
+    # add gnss_capture_time to body if available
+    if [[ -n "$time" ]]; then
+        # LoRaCloud is not explicit about when to measure the acquisition time in the device
+        # Specifying an accuracy <16s causes it to use the specified time as initial estimate
+        # Lower values causes it to reject solutions outside that error bound
+        # Higher values causes it to use the time encoded in the payload (16s granularity)
+        # Best is probably accuracy=15 or omitting this altogether
+        # Note that the docs are self-contradictory, saying that the default time is the server's...
+        local body='{"payload":"'$data'",
+            "gnss_capture_time":'$time',"gnss_capture_time_accuracy":15}'
+            # "gnss_assist_position":'$ASSIST_POS'}'
+    else
+        local body='{"payload":"'$data'"}'
+    fi
+    local header=="Accept: application/json;Ocp-Apim-Subscription-Key: $AUTH_TOKEN;"
+    local res=$(curl -s -S -d "$body" \
+        -HAccept:application/json -HOcp-Apim-Subscription-Key:$AUTH_TOKEN \
+        $URL)
+    # echo "  Result: $res"
+    local POS_RE='"llh":\[([-0-9.]*),([-0-9.]*),([-0-9.]*)'
+    local GPS_RE='"capture_time_gps":([0-9.]*)'
+    local UTC_RE='"capture_time_utc":([0-9.]*)'
+    local ACC_RE='"accuracy":([0-9.]*)'
+    local ERR_RE='"errors": *[[](.*)]'
+    if [[ "$res" =~ $ERR_RE ]]; then
+        echo "  *** Location error: ${BASH_REMATCH[1]}"
+        # echo "  Request body: $body"
+    else
+        if [[ "$res" =~ $POS_RE ]]; then
+            local lat=${BASH_REMATCH[1]}
+            local lon=${BASH_REMATCH[2]}
+            local alt=${BASH_REMATCH[3]}
+            echo "  Position: $lat $lon alt: $alt"
+            haversine $lat $lon
+        fi
+        if [[ "$res" =~ $ACC_RE ]]; then
+            local acc=${BASH_REMATCH[1]}
+            if [[ "$acc" = 0* ]]; then echo "  Claimed accuracy: unknown"; else echo "  Claimed accuracy: ${acc}m"; fi
+        fi
+        if [[ "$res" =~ $GPS_RE ]]; then
+            local gps=${BASH_REMATCH[1]}
+            local delta=$(( ${gps%.*} - $time )) # note: integer only...
+            echo "  GPS time: $gps -- delta=${delta}s" # delta to what we submitted in the request
+        fi
+        if [[ "$res" =~ $UTC_RE ]]; then
+            local txt=$(date -u -Iseconds -d "@${BASH_REMATCH[1]}")
+            echo "  UTC time: ${BASH_REMATCH[1]} -- $txt"
+        fi
+    fi
+}
+
+RE='^NAV( @([0-9]*))?: *([0-9a-zA-Z]*)' # regular expression for NAV line output by sketch
+
+stty -F$SERIAL 115200
+while IFS=\b read -r line; do
+    if [[ "$line" =~ $RE ]]; then
+        echo "$line"
+        time=${BASH_REMATCH[2]}
+        data=${BASH_REMATCH[3]}
+        query "$data" "$time"
+    else
+        echo "$line"
+    fi
+done <$SERIAL
diff --git a/src/modules/LR11x0/LR11x0.cpp b/src/modules/LR11x0/LR11x0.cpp
index af9b90271..c1e2ba26c 100644
--- a/src/modules/LR11x0/LR11x0.cpp
+++ b/src/modules/LR11x0/LR11x0.cpp
@@ -2045,6 +2045,7 @@ int16_t LR11x0::SPIparseStatus(uint8_t in) {
   if((in & 0b00001110) == RADIOLIB_LR11X0_STAT_1_CMD_PERR) {
     return(RADIOLIB_ERR_SPI_CMD_INVALID);
   } else if((in & 0b00001110) == RADIOLIB_LR11X0_STAT_1_CMD_FAIL) {
+    RADIOLIB_DEBUG_BASIC_PRINTLN("Stat1=CMD_FAIL: 0x%x", in);
     return(RADIOLIB_ERR_SPI_CMD_FAILED);
   } else if((in == 0x00) || (in == 0xFF)) {
     return(RADIOLIB_ERR_CHIP_NOT_FOUND);
@@ -3433,7 +3434,7 @@ int16_t LR11x0::gnssReadTime(uint8_t* err, uint32_t* time, uint32_t* nbUs, uint3
   if(time) {
     *time = ((uint32_t)(buff[1]) << 24) | ((uint32_t)(buff[2]) << 16) | ((uint32_t)(buff[3]) << 8) | (uint32_t)buff[4];
     *time += 2UL*1024UL*7UL*24UL*3600UL; // assume WN rollover is at 2, this will fail sometime in 2038
-    *time += 315964800UL; // convert to UTC
+    // *time += 315964800UL; // convert to UTC
   }
 
   if(nbUs) {