From 44c2029cd39b3e4907efa62d51c3739105caa423 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Sat, 7 Oct 2023 12:24:19 +0200 Subject: [PATCH 01/12] Add MSP commands for ublox passthrough --- src/main/msp/msp_protocol_v2_inav.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/msp/msp_protocol_v2_inav.h b/src/main/msp/msp_protocol_v2_inav.h index f746f5c8ff3..2c69d404f57 100755 --- a/src/main/msp/msp_protocol_v2_inav.h +++ b/src/main/msp/msp_protocol_v2_inav.h @@ -91,4 +91,5 @@ #define MSP2_INAV_LED_STRIP_CONFIG_EX 0x2048 #define MSP2_INAV_SET_LED_STRIP_CONFIG_EX 0x2049 - +#define MSP2_INAV_GPS_UBLOX_COMMAND 0x2050 +#define MSP2_INAV_GPS_UBLOX_COMMAND_RESULT 0x2051 From 70aa0e34db97b966d4e5574a7384831caaef34b0 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Sat, 7 Oct 2023 13:39:40 +0200 Subject: [PATCH 02/12] Foundation for passthrough ublox commands --- src/main/fc/fc_msp.c | 8 ++++++++ src/main/io/gps_ublox.c | 28 +++++++++++++++++++++++++++- src/main/io/gps_ublox.h | 3 +++ 3 files changed, 38 insertions(+), 1 deletion(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 8b62fdba391..bf82ab59cc4 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -85,6 +85,7 @@ #include "io/asyncfatfs/asyncfatfs.h" #include "io/flashfs.h" #include "io/gps.h" +#include "io/gps_ublox.h" #include "io/opflow.h" #include "io/rangefinder.h" #include "io/ledstrip.h" @@ -3039,6 +3040,13 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) } break; #endif + case MSP2_INAV_GPS_UBLOX_COMMAND: + if(dataSize < 8 || !isGpsUblox()) { + return MSP_RESULT_ERROR; + } + + gpsUbloxSendCommand(src->ptr, dataSize, 0); + break; default: return MSP_RESULT_ERROR; diff --git a/src/main/io/gps_ublox.c b/src/main/io/gps_ublox.c index d03c3390776..7125afcd41b 100755 --- a/src/main/io/gps_ublox.c +++ b/src/main/io/gps_ublox.c @@ -128,7 +128,7 @@ static struct { // Send buffer -static union { +static union send_buffer_t { ubx_message message; uint8_t bytes[58]; } send_buffer; @@ -212,6 +212,23 @@ static uint8_t gpsMapFixType(bool fixValid, uint8_t ubloxFixType) return GPS_NO_FIX; } +bool gpsUbloxSendCommand(uint8_t *rawCommand, uint16_t commandLen, uint16_t timeout) +{ + serialWriteBuf(gpsState.gpsPort, rawCommand, commandLen); + + union send_buffer_t *sb = (union send_buffer_t *)(rawCommand); + + _ack_waiting_msg = sb->message.header.msg_id; + _ack_state = UBX_ACK_WAITING; + + UNUSED(timeout); + //if (timeout > 0) { + // ptWait(_ack_state == UBX_ACK_GOT_ACK); + //} + + return true; +} + static void sendConfigMessageUBLOX(void) { uint8_t ck_a=0, ck_b=0; @@ -1100,4 +1117,13 @@ void gpsHandleUBLOX(void) } } +bool isGpsUblox(void) +{ + if(gpsState.gpsConfig->provider == GPS_UBLOX || gpsState.gpsConfig->provider == GPS_UBLOX7PLUS) { + return true; + } + + return false; +} + #endif diff --git a/src/main/io/gps_ublox.h b/src/main/io/gps_ublox.h index 00b42eeb2b7..232bbf8697e 100644 --- a/src/main/io/gps_ublox.h +++ b/src/main/io/gps_ublox.h @@ -422,6 +422,9 @@ bool gpsUbloxGlonassDefault(void); bool gpsUbloxGalileoEnabled(void); bool gpsUbloxBeidouEnabled(void); bool gpsUbloxGlonassEnabled(void); +bool gpsUbloxSendCommand(uint8_t *rawCommand, uint16_t commandLen, uint16_t timeout); + +bool isGpsUblox(void); #ifdef __cplusplus From 69a0ad14aaf02cba24ec3ce5aef0b4d0fa2242e8 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Sat, 7 Oct 2023 18:36:48 +0200 Subject: [PATCH 03/12] Add a script to test firmware --- .gitignore | 3 ++ src/utils/assistnow.py | 102 +++++++++++++++++++++++++++++++++++++++++ 2 files changed, 105 insertions(+) create mode 100755 src/utils/assistnow.py diff --git a/.gitignore b/.gitignore index b2c3b9ff546..e89a159c49c 100644 --- a/.gitignore +++ b/.gitignore @@ -35,3 +35,6 @@ make/local.mk launch.json .vscode/tasks.json .vscode/c_cpp_properties.json + +# Assitnow token files for test script +tokens.yaml diff --git a/src/utils/assistnow.py b/src/utils/assistnow.py new file mode 100755 index 00000000000..f498da39d12 --- /dev/null +++ b/src/utils/assistnow.py @@ -0,0 +1,102 @@ +#!/usr/bin/python3 +# https://developer.thingstream.io/guides/location-services/assistnow-user-guide +# +# Create a file named tokens.yaml +# Add your assist now tokens +# assistnow_online: XXXXXXXXXXX +# assistnow_offline: XXXXXXXXXXX + +import requests +import yaml +import sys +import serial +import getopt + +online_token = "" +offline_token = "" +token_file = "tokens.yaml" +serial_port = None + +def loadTokens(file): + global online_token + global offline_token + yaml_config = {} + + with open(file, "r") as stream: + try: + yaml_config = yaml.safe_load(stream) + online_token = yaml_config['assistnow_online'] + offline_token = yaml_config['assistnow_online'] + except yaml.YAMLError as exc: + print(exc) + stream.close() + + +def usage(): + print ("assist-now.py -s /dev/ttyS0 [-t tokens.yaml]") + +try: + opts, args = getopt.getopt(sys.argv[1:], "s:t:", ["serial=", "tokens="]) +except getopt.GetoptError as err: + # print help information and exit: + print(err) # will print something like "option -a not recognized" + usage() + sys.exit(2) + +for o, a in opts: + if o in ("-s", "--serial"): + serial_port = a + elif o in ("-t", "--tokens="): + token_file = a + else: + usage() + sys.exit(2) + +if serial_port == None: + usage() + sys.exit(2) + +loadTokens(token_file) + +#s = serial.Serial(serial_port, 115200) + +#s.write(b"#\r\n") +#s.write(b"gpspassrhtrough\r\n"); + +#m8 only +fmt="mga" +gnss="gps,gal,bds,glo,qzss" +offline_gnss="gps,gal,bds,glo" +#m8 only +alm="gps,qzss,gal,bds,glo" + +online_servers = ['online-live1.services.u-blox.com', 'online-live2.services.u-blox.com'] +#m8 format = mga +#m7 format = aid +url = "https://online-live1.services.u-blox.com/GetOnlineData.ashx?token=" + online_token + ";gnss=" + gnss + ";datatype=eph,alm,aux,pos;format=" + fmt + ";" +online_req = requests.get(url) + +print (online_req) +#print (online_req.content) +print (len(online_req.content)) +#s.write(online_req.content); +of = open("aon.ubx", "wb") +of.write(online_req.content) +of.close() + +period = "5" + +offline_servers = ['offline-live1.services.u-blox.com', 'offline-live2.services.u-blox.com'] +offline_url = "https://offline-live1.services.u-blox.com/GetOfflineData.ashx?token=" + offline_token + ";gnss=" + offline_gnss + ";format=" + fmt + ";period=" + period + ";resolution=1;alm=" + alm + ";" +offline_req = requests.get(offline_url) + +print(offline_req) +#print(offline_req.content) +print(len(offline_req.content)) + +#s.write(offline_req.content); + +of = open("aoff.ubx", "wb") +of.write(offline_req.content) +of.close() + From edf2c3d186faeeda370329807789e13437c1facb Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Sat, 7 Oct 2023 20:14:04 +0200 Subject: [PATCH 04/12] Parse ublox commands so we can send them over msp --- .gitignore | 3 +- src/utils/assistnow.py | 121 ++++++++++++++++++++++++++++++++++++++++- 2 files changed, 120 insertions(+), 4 deletions(-) diff --git a/.gitignore b/.gitignore index e89a159c49c..f0fc36f5026 100644 --- a/.gitignore +++ b/.gitignore @@ -36,5 +36,6 @@ launch.json .vscode/tasks.json .vscode/c_cpp_properties.json -# Assitnow token files for test script +# Assitnow token and files for test script tokens.yaml +*.ubx diff --git a/src/utils/assistnow.py b/src/utils/assistnow.py index f498da39d12..0e3a13345b6 100755 --- a/src/utils/assistnow.py +++ b/src/utils/assistnow.py @@ -1,4 +1,4 @@ -#!/usr/bin/python3 +#!/usr/local/bin/python3 # https://developer.thingstream.io/guides/location-services/assistnow-user-guide # # Create a file named tokens.yaml @@ -11,12 +11,119 @@ import sys import serial import getopt +import io online_token = "" offline_token = "" token_file = "tokens.yaml" serial_port = None + +# UBX frame +# 0xB5 +# 0x62 +# 0x?? class +# 0x?? id +# 0x?? len low +# 0x?? len high +# 0x?? len bytes payload +# 0x?? crc1 +# 0x?? crc2 +# Total len = 8 + len + +hasFirstHeader = False +hasSecondHeader = False +ubxClass = False +ubxId = False +lenLow = False +lenHigh = False +payloadLen = 0 +skipped = 0 +currentCommand = [] + +def resetUbloxState(): + global hasFirstHeader + global hasSecondHeader + global ubxClass + global ubxId + global lenLow + global lenHigh + global payloadLen + global skipped + global currentCommand + + hasFirstHeader = False + hasSecondHeader = False + ubxClass = False + ubxId = False + lenLow = False + lenHigh = False + payloadLen = 0 + skipped = 0 + currentCommand = [] + +def splitUbloxCommands(ubxBytes): + ubxCommands = [] + global hasFirstHeader + global hasSecondHeader + global ubxClass + global ubxId + global lenLow + global lenHigh + global payloadLen + global skipped + global currentCommand + + resetUbloxState() + + print("%s" % (type(ubxBytes))) + print("len: %i" % (len(ubxBytes))) + + for i in range(len(ubxBytes)): + if not hasFirstHeader: + if ubxBytes[i] == 0xb5: + hasFirstHeader = True + currentCommand.append(ubxBytes[i]) + continue + else: + resetUbloxState() + continue + if not hasSecondHeader: + if ubxBytes[i] == 0x62: + hasSecondHeader = True + currentCommand.append(ubxBytes[i]) + continue + else: + resetUbloxState() + continue + if not ubxClass: + ubxClass = True + currentCommand.append(ubxBytes[i]) + continue + if not ubxId: + ubxId = True + currentCommand.append(ubxBytes[i]) + continue + if not lenLow: + lenLow = True + payloadLen = int(ubxBytes[i]) + currentCommand.append(ubxBytes[i]) + continue + if not lenHigh: + lenHigh = True + payloadLen = (int(ubxBytes[i]) << 8) | payloadLen + payloadLen += 2 # add crc bytes + currentCommand.append(ubxBytes[i]) + continue + if skipped < payloadLen: + skipped = skipped + 1 + currentCommand.append(ubxBytes[i]) + continue + ubxCommands.append(currentCommand) + resetUbloxState() + + return ubxCommands + def loadTokens(file): global online_token global offline_token @@ -33,7 +140,7 @@ def loadTokens(file): def usage(): - print ("assist-now.py -s /dev/ttyS0 [-t tokens.yaml]") + print ("assistnow.py -s /dev/ttyS0 [-t tokens.yaml]") try: opts, args = getopt.getopt(sys.argv[1:], "s:t:", ["serial=", "tokens="]) @@ -78,7 +185,11 @@ def usage(): print (online_req) #print (online_req.content) -print (len(online_req.content)) +#print (len(online_req.content)) +online = io.BytesIO(online_req.content) +online_bytes = online.read() +online_cmds = splitUbloxCommands(online_bytes) +print ("AssitnowOnline: %i" % (len(online_cmds))) #s.write(online_req.content); of = open("aon.ubx", "wb") of.write(online_req.content) @@ -92,7 +203,11 @@ def usage(): print(offline_req) #print(offline_req.content) +off = io.BytesIO(offline_req.content) print(len(offline_req.content)) +offline_bytes = off.read() +offline_cmds = splitUbloxCommands(offline_bytes) +print ("AssitnowOffline: %i" % (len(offline_cmds))) #s.write(offline_req.content); From 524563d5a1bf51cd47a09e95b7a00d87c0e78243 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Sat, 7 Oct 2023 23:44:49 +0200 Subject: [PATCH 05/12] almost working. Getting some random disconnects from usb port that needs to be investigated. Also need to check if gps is getting sent the correct ublox messages. --- src/utils/assistnow.py | 51 +++++++++++++++++++++++++++++++++++------- 1 file changed, 43 insertions(+), 8 deletions(-) diff --git a/src/utils/assistnow.py b/src/utils/assistnow.py index 0e3a13345b6..2a042b08cc3 100755 --- a/src/utils/assistnow.py +++ b/src/utils/assistnow.py @@ -12,6 +12,7 @@ import serial import getopt import io +import time online_token = "" offline_token = "" @@ -138,10 +139,47 @@ def loadTokens(file): print(exc) stream.close() +def crc8_dvb_s2( crc:int, b:int) -> int: + crc ^= b + for ii in range(8): + if (crc & 0x80) == 0x80: + crc = ((crc << 1) ^ 0xD5) & 0xFF + else: + crc = (crc << 1) & 0xFF + + return int(crc & 0xFF) + +def ubloxToMsp(ubxCmd): + ubloxLen = len(ubxCmd) + msp = bytearray(b'$X>\x00\x50\x20') + msp.append(ubloxLen & 0xFF) + msp.append((ubloxLen >> 8) & 0xFF) + + for i in range(ubloxLen): + msp.append(ubxCmd[i]) + + crc = 0 + for i in range(ubloxLen + 5): + crc = crc8_dvb_s2(crc, int(msp[i + 3])) + + #print ("msp: %s" % (bytes(msp))) + + return bytes(msp) + def usage(): print ("assistnow.py -s /dev/ttyS0 [-t tokens.yaml]") +def sendMessages(s, ubxMessages): + printed = 0 + for cmd in ubxMessages: + msp = ubloxToMsp(cmd) + printed += 1 + if(len(msp) > 8): + print ("%i/%i msp: %i ubx: %i" % (printed, len(ubxMessages), len(msp), len(cmd))) + s.write(msp) + time.sleep(0.2) + try: opts, args = getopt.getopt(sys.argv[1:], "s:t:", ["serial=", "tokens="]) except getopt.GetoptError as err: @@ -165,10 +203,6 @@ def usage(): loadTokens(token_file) -#s = serial.Serial(serial_port, 115200) - -#s.write(b"#\r\n") -#s.write(b"gpspassrhtrough\r\n"); #m8 only fmt="mga" @@ -185,12 +219,12 @@ def usage(): print (online_req) #print (online_req.content) -#print (len(online_req.content)) +print (len(online_req.content)) online = io.BytesIO(online_req.content) online_bytes = online.read() online_cmds = splitUbloxCommands(online_bytes) print ("AssitnowOnline: %i" % (len(online_cmds))) -#s.write(online_req.content); + of = open("aon.ubx", "wb") of.write(online_req.content) of.close() @@ -209,9 +243,10 @@ def usage(): offline_cmds = splitUbloxCommands(offline_bytes) print ("AssitnowOffline: %i" % (len(offline_cmds))) -#s.write(offline_req.content); - of = open("aoff.ubx", "wb") of.write(offline_req.content) of.close() +s = serial.Serial(serial_port, 230400) +sendMessages(s, online_cmds) +sendMessages(s, offline_cmds) \ No newline at end of file From 49208dc581d59cb50eb58a5847dbd365fbee9659 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Sun, 8 Oct 2023 13:02:50 +0200 Subject: [PATCH 06/12] Debugging why I get disconnects with my speedybeef405wing --- src/utils/assistnow.py | 18 ++++++++++++------ 1 file changed, 12 insertions(+), 6 deletions(-) diff --git a/src/utils/assistnow.py b/src/utils/assistnow.py index 2a042b08cc3..c3498d7a821 100755 --- a/src/utils/assistnow.py +++ b/src/utils/assistnow.py @@ -77,8 +77,8 @@ def splitUbloxCommands(ubxBytes): resetUbloxState() - print("%s" % (type(ubxBytes))) - print("len: %i" % (len(ubxBytes))) + #print("%s" % (type(ubxBytes))) + #print("len: %i" % (len(ubxBytes))) for i in range(len(ubxBytes)): if not hasFirstHeader: @@ -177,8 +177,14 @@ def sendMessages(s, ubxMessages): printed += 1 if(len(msp) > 8): print ("%i/%i msp: %i ubx: %i" % (printed, len(ubxMessages), len(msp), len(cmd))) - s.write(msp) - time.sleep(0.2) + try: + s.write(msp) + except serial.SerialException as err: + print (err) + print (cmd) + print (msp) + break + time.sleep(1.0) try: opts, args = getopt.getopt(sys.argv[1:], "s:t:", ["serial=", "tokens="]) @@ -248,5 +254,5 @@ def sendMessages(s, ubxMessages): of.close() s = serial.Serial(serial_port, 230400) -sendMessages(s, online_cmds) -sendMessages(s, offline_cmds) \ No newline at end of file +sendMessages(s, offline_cmds) +sendMessages(s, online_cmds) \ No newline at end of file From f75da29e464f3aadb5411766d3f222a09507d3bd Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Mon, 9 Oct 2023 12:58:40 +0200 Subject: [PATCH 07/12] Add gpspassthrough option --- src/utils/assistnow.py | 37 +++++++++++++++++++++++++++++++------ 1 file changed, 31 insertions(+), 6 deletions(-) diff --git a/src/utils/assistnow.py b/src/utils/assistnow.py index c3498d7a821..f87208781ff 100755 --- a/src/utils/assistnow.py +++ b/src/utils/assistnow.py @@ -1,4 +1,4 @@ -#!/usr/local/bin/python3 +#!/usr/bin/env python3 # https://developer.thingstream.io/guides/location-services/assistnow-user-guide # # Create a file named tokens.yaml @@ -16,6 +16,7 @@ online_token = "" offline_token = "" +passthrough = False token_file = "tokens.yaml" serial_port = None @@ -170,7 +171,22 @@ def ubloxToMsp(ubxCmd): def usage(): print ("assistnow.py -s /dev/ttyS0 [-t tokens.yaml]") -def sendMessages(s, ubxMessages): +def sendUbxMessages(s, ubxMessages): + printed = 0 + for cmd in ubxMessages: + printed += 1 + if(len(msp) > 8): + print ("%i/%i ubx: %i" % (printed, len(ubxMessages), len(cmd))) + try: + s.write(cmd) + except serial.SerialException as err: + print (err) + print (cmd) + break + time.sleep(1.0) + + +def sendMspMessages(s, ubxMessages): printed = 0 for cmd in ubxMessages: msp = ubloxToMsp(cmd) @@ -187,7 +203,7 @@ def sendMessages(s, ubxMessages): time.sleep(1.0) try: - opts, args = getopt.getopt(sys.argv[1:], "s:t:", ["serial=", "tokens="]) + opts, args = getopt.getopt(sys.argv[1:], "s:t:p", ["serial=", "tokens=", "passthrough"]) except getopt.GetoptError as err: # print help information and exit: print(err) # will print something like "option -a not recognized" @@ -195,10 +211,12 @@ def sendMessages(s, ubxMessages): sys.exit(2) for o, a in opts: - if o in ("-s", "--serial"): + if o in ("-s", "--serial="): serial_port = a elif o in ("-t", "--tokens="): token_file = a + elif o in ("-p", "--passthrough"): + passthrough = True else: usage() sys.exit(2) @@ -254,5 +272,12 @@ def sendMessages(s, ubxMessages): of.close() s = serial.Serial(serial_port, 230400) -sendMessages(s, offline_cmds) -sendMessages(s, online_cmds) \ No newline at end of file + +if not passthrough: + sendMspMessages(s, offline_cmds) + sendMspMessages(s, online_cmds) +else: + serial.write('#\r\n') + serial.write('gpspassthrough\r\n') + sendUbxMessages(s, offline_cmds) + sendUbxMessages(s, online_cmds) From d993c26f1c074e8a038c3a0c358b9c4038b283bf Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Tue, 5 Mar 2024 17:02:34 +0100 Subject: [PATCH 08/12] Fix ublox splitting --- src/utils/assistnow.py | 55 ++++++++++++++++++++++++++++++++---------- 1 file changed, 42 insertions(+), 13 deletions(-) diff --git a/src/utils/assistnow.py b/src/utils/assistnow.py index f87208781ff..066029b4320 100755 --- a/src/utils/assistnow.py +++ b/src/utils/assistnow.py @@ -19,6 +19,7 @@ passthrough = False token_file = "tokens.yaml" serial_port = None +dry_run = False # UBX frame @@ -100,10 +101,12 @@ def splitUbloxCommands(ubxBytes): continue if not ubxClass: ubxClass = True + #print ("ubxClass: %02x" % (ubxBytes[i])) currentCommand.append(ubxBytes[i]) continue if not ubxId: ubxId = True + #print ("ubxId: %02x" % (ubxBytes[i])) currentCommand.append(ubxBytes[i]) continue if not lenLow: @@ -114,15 +117,20 @@ def splitUbloxCommands(ubxBytes): if not lenHigh: lenHigh = True payloadLen = (int(ubxBytes[i]) << 8) | payloadLen + #print ("Payload len %i" % (payloadLen)) payloadLen += 2 # add crc bytes currentCommand.append(ubxBytes[i]) continue - if skipped < payloadLen: + if skipped < payloadLen - 1: skipped = skipped + 1 currentCommand.append(ubxBytes[i]) continue - ubxCommands.append(currentCommand) - resetUbloxState() + if skipped == payloadLen - 1: + skipped = skipped + 1 + currentCommand.append(ubxBytes[i]) + ubxCommands.append(currentCommand) + resetUbloxState() + continue; return ubxCommands @@ -203,7 +211,7 @@ def sendMspMessages(s, ubxMessages): time.sleep(1.0) try: - opts, args = getopt.getopt(sys.argv[1:], "s:t:p", ["serial=", "tokens=", "passthrough"]) + opts, args = getopt.getopt(sys.argv[1:], "s:t:pd", ["serial=", "tokens=", "passthrough", "dry-run"]) except getopt.GetoptError as err: # print help information and exit: print(err) # will print something like "option -a not recognized" @@ -217,11 +225,13 @@ def sendMspMessages(s, ubxMessages): token_file = a elif o in ("-p", "--passthrough"): passthrough = True + elif o in ("-d", "--dry-run"): + dry_run = True else: usage() sys.exit(2) -if serial_port == None: +if serial_port == None and not dry_run: usage() sys.exit(2) @@ -249,6 +259,15 @@ def sendMspMessages(s, ubxMessages): online_cmds = splitUbloxCommands(online_bytes) print ("AssitnowOnline: %i" % (len(online_cmds))) +max_payload = 0 +max_msp_payload = 0 +for cmd in online_cmds: + if len(cmd) > max_payload: + max_payload = len(cmd) + max_msp_payload = len(ubloxToMsp(cmd)) + print ("Max ubx payload: %i" % (max_payload)) + print ("Max msp payload: %i" % (max_msp_payload)) + of = open("aon.ubx", "wb") of.write(online_req.content) of.close() @@ -267,17 +286,27 @@ def sendMspMessages(s, ubxMessages): offline_cmds = splitUbloxCommands(offline_bytes) print ("AssitnowOffline: %i" % (len(offline_cmds))) +for cmd in offline_cmds: + if len(cmd) > max_payload: + max_payload = len(cmd) + print ("Max ubx payload: %i" % (max_payload)) + max_msp_payload = len(ubloxToMsp(cmd)) + print ("Max msp payload: %i" % (max_msp_payload)) + of = open("aoff.ubx", "wb") of.write(offline_req.content) of.close() s = serial.Serial(serial_port, 230400) -if not passthrough: - sendMspMessages(s, offline_cmds) - sendMspMessages(s, online_cmds) -else: - serial.write('#\r\n') - serial.write('gpspassthrough\r\n') - sendUbxMessages(s, offline_cmds) - sendUbxMessages(s, online_cmds) +if not dry_run: + if not passthrough: + print ("Offline cmds...") + sendMspMessages(s, offline_cmds) + print ("Online cmds...") + sendMspMessages(s, online_cmds) + else: + serial.write('#\r\n') + serial.write('gpspassthrough\r\n') + sendUbxMessages(s, offline_cmds) + sendUbxMessages(s, online_cmds) From bb91342e5dbc3d5ba3da9be042a4b44f0ce9bac4 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Sat, 22 Jun 2024 20:40:48 +0200 Subject: [PATCH 09/12] Fix ublox crc calculation. --- src/utils/assistnow.py | 47 +++++++++++++++++++++++++++++------------- 1 file changed, 33 insertions(+), 14 deletions(-) diff --git a/src/utils/assistnow.py b/src/utils/assistnow.py index 066029b4320..c9f19fe6efd 100755 --- a/src/utils/assistnow.py +++ b/src/utils/assistnow.py @@ -9,6 +9,9 @@ import requests import yaml import sys +import socket +import selectors +import types import serial import getopt import io @@ -160,19 +163,29 @@ def crc8_dvb_s2( crc:int, b:int) -> int: def ubloxToMsp(ubxCmd): ubloxLen = len(ubxCmd) - msp = bytearray(b'$X>\x00\x50\x20') + #msp = bytearray(b"$X<\x00d\x00\x00\x00\x8F") + crc = 0 + msp = bytearray(b"$X<\x00\x50\x20") + crc = crc8_dvb_s2(crc, 0x00) + crc = crc8_dvb_s2(crc, 0x50) + crc = crc8_dvb_s2(crc, 0x20) msp.append(ubloxLen & 0xFF) + crc = crc8_dvb_s2(crc, ubloxLen & 0xFF) msp.append((ubloxLen >> 8) & 0xFF) + crc = crc8_dvb_s2(crc, (ubloxLen >> 8) & 0xFF) + + if(len(msp) != 8): + print ("Wrong size") for i in range(ubloxLen): msp.append(ubxCmd[i]) + crc = crc8_dvb_s2(crc, ubxCmd[i]) - crc = 0 - for i in range(ubloxLen + 5): - crc = crc8_dvb_s2(crc, int(msp[i + 3])) - #print ("msp: %s" % (bytes(msp))) + msp.append(crc & 0xFF) + print ("CRC: %i" % (crc)) + return bytes(msp) @@ -191,24 +204,26 @@ def sendUbxMessages(s, ubxMessages): print (err) print (cmd) break - time.sleep(1.0) + #time.sleep(0.1) def sendMspMessages(s, ubxMessages): printed = 0 for cmd in ubxMessages: msp = ubloxToMsp(cmd) + #msp = bytearray(b"$X<\x00d\x00\x00\x00\x8F") printed += 1 if(len(msp) > 8): print ("%i/%i msp: %i ubx: %i" % (printed, len(ubxMessages), len(msp), len(cmd))) try: - s.write(msp) + #s.write(msp) + s.sendall(msp) except serial.SerialException as err: print (err) print (cmd) print (msp) break - time.sleep(1.0) + #time.sleep(1.0) try: opts, args = getopt.getopt(sys.argv[1:], "s:t:pd", ["serial=", "tokens=", "passthrough", "dry-run"]) @@ -231,9 +246,9 @@ def sendMspMessages(s, ubxMessages): usage() sys.exit(2) -if serial_port == None and not dry_run: - usage() - sys.exit(2) +#if serial_port == None and not dry_run: +# usage() +# sys.exit(2) loadTokens(token_file) @@ -297,7 +312,11 @@ def sendMspMessages(s, ubxMessages): of.write(offline_req.content) of.close() -s = serial.Serial(serial_port, 230400) +print ("Connecting...") +#s = serial.Serial(serial_port, 230400) +s = socket.socket(socket.AF_INET, socket.SOCK_STREAM) +s.connect(('localhost', 5760)) +print ("Connected.") if not dry_run: if not passthrough: @@ -306,7 +325,7 @@ def sendMspMessages(s, ubxMessages): print ("Online cmds...") sendMspMessages(s, online_cmds) else: - serial.write('#\r\n') - serial.write('gpspassthrough\r\n') + #serial.write('#\r\n') + #serial.write('gpspassthrough\r\n') sendUbxMessages(s, offline_cmds) sendUbxMessages(s, online_cmds) From 2a409138a9e030b6a01cd5081a5d5f0e6f7af1e8 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Sat, 22 Jun 2024 20:41:47 +0200 Subject: [PATCH 10/12] Switch back to serial comms --- src/utils/assistnow.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/utils/assistnow.py b/src/utils/assistnow.py index c9f19fe6efd..4968a85a9dd 100755 --- a/src/utils/assistnow.py +++ b/src/utils/assistnow.py @@ -216,8 +216,8 @@ def sendMspMessages(s, ubxMessages): if(len(msp) > 8): print ("%i/%i msp: %i ubx: %i" % (printed, len(ubxMessages), len(msp), len(cmd))) try: - #s.write(msp) - s.sendall(msp) + s.write(msp) + #s.sendall(msp) except serial.SerialException as err: print (err) print (cmd) @@ -313,9 +313,9 @@ def sendMspMessages(s, ubxMessages): of.close() print ("Connecting...") -#s = serial.Serial(serial_port, 230400) -s = socket.socket(socket.AF_INET, socket.SOCK_STREAM) -s.connect(('localhost', 5760)) +s = serial.Serial(serial_port, 230400) +#s = socket.socket(socket.AF_INET, socket.SOCK_STREAM) +#s.connect(('localhost', 5760)) print ("Connected.") if not dry_run: From b6e7a4bb063146758f0eb551190636b96017885f Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Sat, 22 Jun 2024 20:48:59 +0200 Subject: [PATCH 11/12] Add some debug messages to SITL Great to diagnose your msp messages are actually being delivered. :) --- src/main/fc/fc_msp.c | 4 ++++ src/main/msp/msp_serial.c | 8 ++++++++ 2 files changed, 12 insertions(+) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 8427f94b9c4..c8ae27f270d 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -18,6 +18,7 @@ #include #include #include +#include #include #include @@ -3290,9 +3291,11 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) #endif case MSP2_INAV_GPS_UBLOX_COMMAND: if(dataSize < 8 || !isGpsUblox()) { + SD(fprintf(stderr, "[GPS] Not ublox!\n")); return MSP_RESULT_ERROR; } + SD(fprintf(stderr, "[GPS] Sending ubx command: %i!\n", dataSize)); gpsUbloxSendCommand(src->ptr, dataSize, 0); break; @@ -4142,6 +4145,7 @@ mspResult_e mspFcProcessCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostPro // initialize reply by default reply->cmd = cmd->cmd; + SD(fprintf(stderr, "[MSP] CommandId: 0x%04x bytes: %i!\n", cmdMSP, sbufBytesRemaining(src))); if (MSP2_IS_SENSOR_MESSAGE(cmdMSP)) { ret = mspProcessSensorCommand(cmdMSP, src); } else if (mspFcProcessOutCommand(cmdMSP, dst, mspPostProcessFn)) { diff --git a/src/main/msp/msp_serial.c b/src/main/msp/msp_serial.c index 0aaecba2d7d..0dbc1768b8a 100644 --- a/src/main/msp/msp_serial.c +++ b/src/main/msp/msp_serial.c @@ -17,6 +17,8 @@ #include #include +#include +#include #include #include "platform.h" @@ -171,8 +173,10 @@ static bool mspSerialProcessReceivedData(mspPort_t *mspPort, uint8_t c) case MSP_CHECKSUM_V1: if (mspPort->checksum1 == c) { mspPort->c_state = MSP_COMMAND_RECEIVED; + SD(fprintf(stderr, "[MSPV1] Command received\n")); } else { mspPort->c_state = MSP_IDLE; + SD(fprintf(stderr, "[MSPV1] Checksum error!\n")); } break; @@ -225,6 +229,7 @@ static bool mspSerialProcessReceivedData(mspPort_t *mspPort, uint8_t c) // Check for potential buffer overflow if (hdrv2->size > MSP_PORT_INBUF_SIZE) { mspPort->c_state = MSP_IDLE; + SD(fprintf(stderr, "[MSPV2] Potential buffer overflow!\n")); } else { mspPort->dataSize = hdrv2->size; @@ -248,7 +253,9 @@ static bool mspSerialProcessReceivedData(mspPort_t *mspPort, uint8_t c) case MSP_CHECKSUM_V2_NATIVE: if (mspPort->checksum2 == c) { mspPort->c_state = MSP_COMMAND_RECEIVED; + SD(fprintf(stderr, "[MSPV2] command received!\n")); } else { + SD(fprintf(stderr, "[MSPV2] Checksum error!\n")); mspPort->c_state = MSP_IDLE; } break; @@ -472,6 +479,7 @@ void mspSerialProcessOnePort(mspPort_t * const mspPort, mspEvaluateNonMspData_e const uint8_t c = serialRead(mspPort->port); const bool consumed = mspSerialProcessReceivedData(mspPort, c); + //SD(fprintf(stderr, "[MSP]: received char: %02x (%c) state: %i\n", c, isprint(c) ? c : '.', mspPort->c_state)); if (!consumed && evaluateNonMspData == MSP_EVALUATE_NON_MSP_DATA) { mspEvaluateNonMspData(mspPort, c); } From d9d6f9ff85fd4fb4c495906e6981c141fdcbf28a Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Sat, 22 Jun 2024 23:06:08 +0200 Subject: [PATCH 12/12] Cleanup before merge --- src/main/io/gps_ublox.c | 7 ++----- src/main/msp/msp_protocol_v2_inav.h | 1 - 2 files changed, 2 insertions(+), 6 deletions(-) diff --git a/src/main/io/gps_ublox.c b/src/main/io/gps_ublox.c index f21b02f114c..52b6329d9f2 100755 --- a/src/main/io/gps_ublox.c +++ b/src/main/io/gps_ublox.c @@ -214,6 +214,8 @@ static uint8_t gpsMapFixType(bool fixValid, uint8_t ubloxFixType) bool gpsUbloxSendCommand(uint8_t *rawCommand, uint16_t commandLen, uint16_t timeout) { + UNUSED(timeout); + serialWriteBuf(gpsState.gpsPort, rawCommand, commandLen); union send_buffer_t *sb = (union send_buffer_t *)(rawCommand); @@ -221,11 +223,6 @@ bool gpsUbloxSendCommand(uint8_t *rawCommand, uint16_t commandLen, uint16_t time _ack_waiting_msg = sb->message.header.msg_id; _ack_state = UBX_ACK_WAITING; - UNUSED(timeout); - //if (timeout > 0) { - // ptWait(_ack_state == UBX_ACK_GOT_ACK); - //} - return true; } diff --git a/src/main/msp/msp_protocol_v2_inav.h b/src/main/msp/msp_protocol_v2_inav.h index 2d9663041e3..8b4a09e87af 100755 --- a/src/main/msp/msp_protocol_v2_inav.h +++ b/src/main/msp/msp_protocol_v2_inav.h @@ -97,7 +97,6 @@ #define MSP2_INAV_SET_FW_APPROACH 0x204B #define MSP2_INAV_GPS_UBLOX_COMMAND 0x2050 -#define MSP2_INAV_GPS_UBLOX_COMMAND_RESULT 0x2051 #define MSP2_INAV_RATE_DYNAMICS 0x2060 #define MSP2_INAV_SET_RATE_DYNAMICS 0x2061