diff --git a/.gitignore b/.gitignore index fd9c771bea2..9b72fa8305a 100644 --- a/.gitignore +++ b/.gitignore @@ -40,4 +40,9 @@ make/local.mk launch.json .vscode/tasks.json .vscode/c_cpp_properties.json + /cmake-build-debug/ + +# Assitnow token and files for test script +tokens.yaml +*.ubx diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index acbf0c2dcb4..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 @@ -87,6 +88,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" @@ -3287,6 +3289,15 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) } break; #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; #ifdef USE_EZ_TUNE @@ -4134,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/io/gps_ublox.c b/src/main/io/gps_ublox.c index 958de158d3e..52b6329d9f2 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,20 @@ static uint8_t gpsMapFixType(bool fixValid, uint8_t ubloxFixType) return GPS_NO_FIX; } +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); + + _ack_waiting_msg = sb->message.header.msg_id; + _ack_state = UBX_ACK_WAITING; + + return true; +} + static void sendConfigMessageUBLOX(void) { uint8_t ck_a=0, ck_b=0; @@ -1118,4 +1132,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 8ec9be16d09..87cffa9c50b 100644 --- a/src/main/io/gps_ublox.h +++ b/src/main/io/gps_ublox.h @@ -424,6 +424,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 diff --git a/src/main/msp/msp_protocol_v2_inav.h b/src/main/msp/msp_protocol_v2_inav.h index 7c10a4df990..8b4a09e87af 100755 --- a/src/main/msp/msp_protocol_v2_inav.h +++ b/src/main/msp/msp_protocol_v2_inav.h @@ -96,6 +96,8 @@ #define MSP2_INAV_FW_APPROACH 0x204A #define MSP2_INAV_SET_FW_APPROACH 0x204B +#define MSP2_INAV_GPS_UBLOX_COMMAND 0x2050 + #define MSP2_INAV_RATE_DYNAMICS 0x2060 #define MSP2_INAV_SET_RATE_DYNAMICS 0x2061 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); } diff --git a/src/utils/assistnow.py b/src/utils/assistnow.py new file mode 100755 index 00000000000..4968a85a9dd --- /dev/null +++ b/src/utils/assistnow.py @@ -0,0 +1,331 @@ +#!/usr/bin/env 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 socket +import selectors +import types +import serial +import getopt +import io +import time + +online_token = "" +offline_token = "" +passthrough = False +token_file = "tokens.yaml" +serial_port = None +dry_run = False + + +# 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 + #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: + lenLow = True + payloadLen = int(ubxBytes[i]) + currentCommand.append(ubxBytes[i]) + continue + 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 - 1: + skipped = skipped + 1 + currentCommand.append(ubxBytes[i]) + continue + if skipped == payloadLen - 1: + skipped = skipped + 1 + currentCommand.append(ubxBytes[i]) + ubxCommands.append(currentCommand) + resetUbloxState() + continue; + + return ubxCommands + +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 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<\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]) + + #print ("msp: %s" % (bytes(msp))) + + msp.append(crc & 0xFF) + print ("CRC: %i" % (crc)) + + return bytes(msp) + + +def usage(): + print ("assistnow.py -s /dev/ttyS0 [-t tokens.yaml]") + +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(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.sendall(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: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" + 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 + 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 and not dry_run: +# usage() +# sys.exit(2) + +loadTokens(token_file) + + +#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)) +online = io.BytesIO(online_req.content) +online_bytes = online.read() +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() + +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) +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))) + +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() + +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: + 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)