Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

RTK MAvsdk - Implementation #543

Closed
Murali-IsPagro opened this issue Jan 16, 2023 · 23 comments
Closed

RTK MAvsdk - Implementation #543

Murali-IsPagro opened this issue Jan 16, 2023 · 23 comments

Comments

@Murali-IsPagro
Copy link

Hi,

We tested Here3+ RTK in our drone by following the below link from Px4 official website.

RTK Link

In QGCS:

We could able to establish the connection with RTK , The GPS Lock was changed from 3D LOCK to 3D RTK GPSLock(Float)

WhatsApp Image 2023-01-16 at 10 48 36

Problem statement:

I want to test my MAVSDK -Python code by using RTK . For this I closed the QGCS and tested the drone using the below code.

`

import asyncio
from mavsdk import System
from array import *

async def run():
    # Init the drone
    drone = System()
    await drone.connect(system_address="serial:///dev/ttyUSB0")
    asyncio.ensure_future(print_gps_info(drone))

async def print_gps_info(drone):
    async for gps_info in drone.telemetry.gps_info():
        print(f"GPS info: {gps_info}")

if __name__ == "__main__":
    # Start the main function

    asyncio.ensure_future(run())

    # Runs the event loop until the program is canceled with e.g. CTRL-C
    asyncio.get_event_loop().run_forever()`

I could able to read the GPS Info as RTK_FLOAT but within some time it changed to FIX_DGPS and slowly changed to FIX_3D
WhatsApp Image 2023-01-16 at 11 19 31 (1)
WhatsApp Image 2023-01-16 at 11 19 31

Next, I stopped the code and opened the QGCS , the RTK status was active. Upon testing the code again , same problem persist, ie, initially it shows RTK_FLOAT and changing to FIX_DGPS and to FIX_3D

Where I am missing, is there any workaround to use RTK data in MAVSDk-Python or should i use two telemetry.

Ubuntu:18.04 LTS
Name: mavsdk
Version: 0.22.0-4-g92ed890
Summary: Python wrapper for MAVSDK
Home-page: https://github.com/mavlink/MAVSDK-Python
Author:
Author-email:
License: UNKNOWN
Location: /home/murali/MAVSDK-Python
Requires: aiogrpc, grpcio, importlib_resources, protobuf
Required-by:

@dayjaby
Copy link
Contributor

dayjaby commented Jan 16, 2023

This is not a MAVSDK related issue, but more about your RTK connection.

I assume you use PX4? Is the GPS device connected via UAVCAN or UART?

You can check whether the PX4 receives enough RTK RTCM messages by typing in the mavlink shell in QGC:

listener gps_inject_data
gps status

gps_inject_data is the uORB topic containing the RTCM messages being passthroughed from QGC to the GPS module.
gps status only works if the GPS device is connected via UART.

@Murali-IsPagro
Copy link
Author

Murali-IsPagro commented Jan 16, 2023

Yes, I am using Px4, We connected GPS to UAVCAN port in Cube Orange.

These things are showing in MAVLINK Console.

nsh> listener gps_inject_data TOPIC: gps_inject_data gps_inject_data_s len: 12 flags: 200 (0b1100'1000) data: [211, 0, 6, 76, 224, 0, 136, 16, 151, 194, 68, 139, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]

nsh> gps status INFO [gps] Main GPS INFO [gps] protocol: UBX INFO [gps] status: NOT OK, port: /dev/ttyS2, baudrate: 0 INFO [gps] sat info: disabled INFO [gps] rate reading: 0 B/s

@julianoes
Copy link
Collaborator

Are you sure this has nothing to do with your (old?) MAVSDK version?

Version: 0.22.0-4-g92ed890

There was a fix at some point: mavlink/MAVSDK#1808

@Murali-IsPagro
Copy link
Author

Hello ,

I have updated MAVSDK to latest one using pip3 install --user --upgrade mavsdk

then pip3 show mavsdk

Name: mavsdk
Version: 1.4.1
Summary: Python wrapper for MAVSDK
Home-page: https://github.com/mavlink/MAVSDK-Python
Author:
Author-email:
License: UNKNOWN
Location: /home/murali/.local/lib/python3.6/site-packages
Requires: aiogrpc, grpcio, importlib-resources, protobuf
Required-by:

QGCS:(MAVLINK CONSOLE):

gps_inject_data_s len: 25 flags: 152 (0b1001'1000) data: [211, 0, 19, 62, 208, 0, 3, 3, 200, 224, 222, 34, 141, 249, 104, 8, 66, 3, 75, 76, 30, 138, 245, 0, 207, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]

Also, I happened to see the issue no. #491

Kindly direct me how to move on from here.

Additionally, One thing i observed was when i connected my drone and QGC using mavlink-router, and tested my telemetry.py code to read GPS_fix. It is continuosly reading as RTK_FLOAT. That means a source should continuously publish RTCM data, Is that correct?

@julianoes
Copy link
Collaborator

julianoes commented Jan 17, 2023

Ok, so the version is good. What's the PX4 version? There have been some changes there as well unfortunately, so I'm wondering if this is working correctly with v1.13.2.

With main (going to be v1.14) you need to set https://docs.px4.io/main/en/advanced_config/parameter_reference.html#UAVCAN_PUB_RTCM.

@Murali-IsPagro
Copy link
Author

Murali-IsPagro commented Jan 18, 2023

Px4 version is v1.13.2,

Enabled UAVCAN_PUB_RTCM

Without opening QGCS , can i able to use RTCM data in MAVSDK Python?

I have included below line in my code.

from mavsdk.rtk import RtcmData

and tested but same FIX_3D only showing in terminal.

Edit 2:

I have created a code by including rtk import by referring from another issue #419 . Please cross check if this is correct . I didn't modify the rtcm_data in the code.

import asyncio
from mavsdk import System
from mavsdk.rtk import (RtcmData,RtkError)
from array import *

async def run():
    # Init the drone
    drone = System()
    await drone.connect(system_address="serial:///dev/ttyUSB0")
    asyncio.ensure_future(print_gps_info(drone))
    asyncio.ensure_future(send_data(RtcmData(str(rtcm_data))))


async def send_data(drone):
    await drone.rtk.send_rtcm_data(data)

async def print_gps_info(drone):
    async for gps_info in drone.telemetry.gps_info():
        print(f"GPS info: {gps_info}")



if __name__ == "__main__":
    # Start the main function
    rtcm_data = bytearray(b'\xd3\x00mCP\x00\x8c2\x16\x82\x00\x00,@\x88\x00\x00\x00\x00\x00 \x00\x00\x00~\x9c\xa4\x9a\x90\xa2\x8c\x00\x00\x01\xa7\xa2\x1e=gv\x8f\x1fq{\\x13_\xc9\xdf\x17\x02L$\xb6\xdd\x17\x9a.\xe8\xba\x94\x02U6^\xa2^\x08\xac\xf5\xf4\x1d\xcc\n\x9d\xe7\xeb\x04R\x15\x92\x93\xf9o\xf2\xc1\xb5-j\xba\xf12`@\r\x83\xc0\xe8B\x0f\x05\xec\x8c\xfc\xc4\x88l\xac\x7f\xf1\x1aR\xc2\xbc\x87')
    asyncio.ensure_future(run())

    # Runs the event loop until the program is canceled with e.g. CTRL-C
    asyncio.get_event_loop().run_forever()

@julianoes
Copy link
Collaborator

await drone.connect(system_address="serial:///dev/ttyUSB0")

I think you're missing the baudrate there.

And then you need to of course actually get that Rtcm data from somewhere.

@Murali-IsPagro
Copy link
Author

Included the above said suggestion , also rtcm_data obtained in listener gps_inject_data after converting to bytearray using the below small python code

list = [211, 0, 19, 62, 208, 0, 3, 3, 200, 224, 114, 188, 141, 249, 104, 71, 224, 3, 75, 75, 178, 150, 224, 28, 140, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 ]
# iterable as source
array = bytearray(list)
print(array)
print("Count of bytes:", len(array))

Output:

bytearray(b'\xd3\x00\x13>\xd0\x00\x03\x03\xc8\xe0r\xbc\x8d\xf9hG\xe0\x03KK\xb2\x96\xe0\x1c\x8c\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00')
Count of bytes: 180

Now Modified the code and it looks like:

#!/usr/bin/env python3

import asyncio
from mavsdk import System
from mavsdk.rtk import (RtcmData,RtkError)
from array import *

async def run():
    # Init the drone
    drone = System()
    await drone.connect(system_address="serial:///dev/ttyUSB0:57600")
    asyncio.ensure_future(print_gps_info(drone))

    try:
        await drone.rtk.send_rtcm_data(RtcmData(str(rtcm_data)))
    except RtkError as error:
        print(f"Starting RTK failed with error code: \
              {error._result.result}")


async def print_gps_info(drone):
    async for gps_info in drone.telemetry.gps_info():
        print(f"GPS info: {gps_info}")



if __name__ == "__main__":
    # Start the main function
    rtcm_data = bytearray(b'\xd3\x00\x13>\xd0\x00\x03\x03\xc8\xe0r\xbc\x8d\xf9hG\xe0\x03KK\xb2\x96\xe0\x1c\x8c\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00')
    asyncio.ensure_future(run())

    # Runs the event loop until the program is canceled with e.g. CTRL-C
    asyncio.get_event_loop().run_forever()

Output:

Same 3D-FIX.

Any example of MAVSDK-Python?

@dayjaby
Copy link
Contributor

dayjaby commented Jan 19, 2023

Sorry to step in again, but just like I tried to convey with my first comment: There are a dozen of possibilities why RTK can go wrong. E.g. from my experience you need the RTCM msg ids 1005, 1077, 1087, 1210 at least (so four different types of messages) from a live(!!) source to actually get into RTK float/fixed. Sending the same RTCM message over and over again won't do you any good.

According to https://github.com/semuconsulting/pyrtcm/blob/main/pyrtcm/rtcmreader.py you can read the msg id quite easily: Skip the first 3 bytes and then read the next 12 bits. So I'd expect something like 0x3ED in your message.

So maybe let's talk about your setup first? Where does your RTCM data come from? From an own base station? From an NTRIP source?

@Murali-IsPagro
Copy link
Author

@dayjaby RTCM data is coming from Own base station. I have not used NTRIP source. I will try the rtcmreader.py and verify the RTCM msg id.

@Murali-IsPagro
Copy link
Author

Murali-IsPagro commented Jan 21, 2023

@dayjaby I have tried reading msg id from pyrtcm, the result is

>>> from serial import Serial
>>> from pyrtcm import RTCMReader
>>> msg = RTCMReader.parse(b"\xd3\x00\x13>\xd0\x00\x03\x03\xc8\xe0r\xbc\x8d\xf9hG\xe0\x03KK\xb2\x96\xe0\x1c\x8c\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00")
>>> print(msg)
<RTCM(1005, DF002=1005, DF003=0, DF021=0, DF022=1, DF023=1, DF024=0, DF141=0, DF025=1625505.4524, DF142=1, DF001_1=0, DF026=6001893.5776, DF364=0, DF027=1414815.4006)>
>>> msg.identity
'1005'

I think ,the message id is 1005
in hex format 0X3ED

Direct me from here, please.

@dayjaby
Copy link
Contributor

dayjaby commented Jan 21, 2023

Just 1005 won't be enough. Is it always the same msg id that you receive?

Did you configure your ublox device that you use as base station to also send the other message IDs like 1077? You can do it with the software from ublox itself or use something like pyubx2 to configure it dynamically, e.g. by sending the following messages:

                UBXMessage('CFG', 'CFG-MSG', SET, msgClass=0xF5, msgID=0x05, rateUSB=10), # RTCM 1005, 0.1 Hz
                UBXMessage('CFG', 'CFG-MSG', SET, msgClass=0xF5, msgID=0x4D, rateUSB=1),  # RTCM 1077, 1 Hz
                UBXMessage('CFG', 'CFG-MSG', SET, msgClass=0xF5, msgID=0x57, rateUSB=1),  # RTCM 1087, 1 Hz
                UBXMessage('CFG', 'CFG-MSG', SET, msgClass=0xF5, msgID=0xE6, rateUSB=1),  # RTCM 1230, 1 Hz

See https://github.com/semuconsulting/pyubx2#serializing how to send such a message

@Murali-IsPagro
Copy link
Author

@dayjaby Sorry for late response,

I have tested the following code based on your input.

>>> from serial import Serial
>>> serialOut = Serial('/dev/ttyACM0', 9600, timeout=5)
>>> from pyubx2 import UBXMessage, SET
>>> msg =  UBXMessage('CFG', 'CFG-MSG', SET, msgClass=0xF5, msgID=0x05, rateUSB=10), UBXMessage('CFG', 'CFG-MSG', SET, msgClass=0xF5, msgID=0x4D, rateUSB=1),UBXMessage('CFG', 'CFG-MSG', SET, msgClass=0xF5, msgID=0x57, rateUSB=1),UBXMessage('CFG', 'CFG-MSG', SET, msgClass=0xF5, msgID=0xE6, rateUSB=1)
>>> print(msg)
(UBXMessage(b'\x06', b'\x01', 1, payload=b'\xf5\x05\x00\x00\x00\n\x00\x00'), UBXMessage(b'\x06', b'\x01', 1, payload=b'\xf5M\x00\x00\x00\x01\x00\x00'), UBXMessage(b'\x06', b'\x01', 1, payload=b'\xf5W\x00\x00\x00\x01\x00\x00'), UBXMessage(b'\x06', b'\x01', 1, payload=b'\xf5\xe6\x00\x00\x00\x01\x00\x00'))

@dayjaby
Copy link
Contributor

dayjaby commented Jan 31, 2023

You are doing msg.serialize and serialOut.write as well? Do you receive more messages now? 1005, 1077 etc?

@Murali-IsPagro
Copy link
Author

Murali-IsPagro commented Jan 31, 2023

@dayjaby
I could not able to print serialize, it is giving me an error.

>>> from pyubx2 import UBXReader
>>> serialOut = Serial('/dev/ttyACM0', 9600, timeout=5)
>>> ubr = UBXReader(serialOut)
>>> (raw_data, parsed_data) = ubr.read()
>>> print(parsed_data)
<UBX(NAV-PVT, iTOW=10:47:24, year=2023, month=1, day=31, hour=10, min=47, second=24, validDate=1, validTime=1, fullyResolved=1, validMag=0, tAcc=3, nano=-331045, fixType=5, gnssFixOk=1, difSoln=0, psmState=0, headVehValid=0, carrSoln=0, confirmedAvai=1, confirmedDate=1, confirmedTime=1, numSV=14, lon=74.8459864, lat=12.9018684, height=-32051, hMSL=52947, hAcc=490, vAcc=346, velN=0, velE=0, velD=0, gSpeed=0, headMot=0.0, sAcc=10, headAcc=180.0, pDOP=99.99, invalidLlh=0, lastCorrectionAge=0, reserved0=992352530, headVeh=0.0, magDec=0.0, magAcc=0.0)>
>>> from pyubx2 import UBXMessage, SET
>>> msg =  UBXMessage('CFG', 'CFG-MSG', SET, msgClass=0xF5, msgID=0x05, rateUSB=10), UBXMessage('CFG', 'CFG-MSG', SET, msgClass=0xF5, msgID=0x4D, rateUSB=1),UBXMessage('CFG', 'CFG-MSG', SET, msgClass=0xF5, msgID=0x57, rateUSB=1),UBXMessage('CFG', 'CFG-MSG', SET, msgClass=0xF5, msgID=0xE6, rateUSB=1)
>>> print(msg)
(UBXMessage(b'\x06', b'\x01', 1, payload=b'\xf5\x05\x00\x00\x00\n\x00\x00'), UBXMessage(b'\x06', b'\x01', 1, payload=b'\xf5M\x00\x00\x00\x01\x00\x00'), UBXMessage(b'\x06', b'\x01', 1, payload=b'\xf5W\x00\x00\x00\x01\x00\x00'), UBXMessage(b'\x06', b'\x01', 1, payload=b'\xf5\xe6\x00\x00\x00\x01\x00\x00'))
>>> output = msg.serialize()
Traceback (most recent call last):
  File "<stdin>", line 1, in <module>
AttributeError: 'tuple' object has no attribute 'serialize'

But when I tried to SET individually that error was not popping

>>> msg = UBXMessage('CFG', 'CFG-MSG', SET, msgClass=0xF5, msgID=0x05, rateUSB=10)
>>> output = msg.serialize()
>>> output
b'\xb5b\x06\x01\x08\x00\xf5\x05\x00\x00\x00\n\x00\x00\x13\x8c'
>>> msg = UBXMessage('CFG', 'CFG-MSG', SET, msgClass=0xF5, msgID=0x4D, rateUSB=1)
>>> output = msg.serialize()
>>> output
b'\xb5b\x06\x01\x08\x00\xf5M\x00\x00\x00\x01\x00\x00Ri'
>>> msg = UBXMessage('CFG', 'CFG-MSG', SET, msgClass=0xF5, msgID=0x57, rateUSB=1)
>>> output = msg.serialize()
>>> output
b'\xb5b\x06\x01\x08\x00\xf5W\x00\x00\x00\x01\x00\x00\\\xaf'
>>> msg = UBXMessage('CFG', 'CFG-MSG', SET, msgClass=0xF5, msgID=0xE6, rateUSB=1)
>>> output = msg.serialize()
>>> output
b'\xb5b\x06\x01\x08\x00\xf5\xe6\x00\x00\x00\x01\x00\x00\xeb\x98'
>>> serialOut.write(output)
16

You are doing msg.serialize and serialOut.write as well? Do you receive more messages now? 1005, 1077 etc?

I don't think I am receiving 1077 and other msg id. I am receiving only 1005.

Please suggest me the testing sequence.

@Murali-IsPagro
Copy link
Author

I did a work around, I connected 2 telemetry to my drone, Had 2 individual laptop, One running QGCS with RTK base connected and another one having MAVSDK-Python running. This time, my MAVSDK-Python code able to read RTK-FIX/FLOAT continuously.

I tested my Offboard code too, it was moving with precise accuracy.

@Murali-IsPagro
Copy link
Author

@dayjaby , any inputs for this?

@dayjaby I could not able to print serialize, it is giving me an error.

>>> from pyubx2 import UBXReader
>>> serialOut = Serial('/dev/ttyACM0', 9600, timeout=5)
>>> ubr = UBXReader(serialOut)
>>> (raw_data, parsed_data) = ubr.read()
>>> print(parsed_data)
<UBX(NAV-PVT, iTOW=10:47:24, year=2023, month=1, day=31, hour=10, min=47, second=24, validDate=1, validTime=1, fullyResolved=1, validMag=0, tAcc=3, nano=-331045, fixType=5, gnssFixOk=1, difSoln=0, psmState=0, headVehValid=0, carrSoln=0, confirmedAvai=1, confirmedDate=1, confirmedTime=1, numSV=14, lon=74.8459864, lat=12.9018684, height=-32051, hMSL=52947, hAcc=490, vAcc=346, velN=0, velE=0, velD=0, gSpeed=0, headMot=0.0, sAcc=10, headAcc=180.0, pDOP=99.99, invalidLlh=0, lastCorrectionAge=0, reserved0=992352530, headVeh=0.0, magDec=0.0, magAcc=0.0)>
>>> from pyubx2 import UBXMessage, SET
>>> msg =  UBXMessage('CFG', 'CFG-MSG', SET, msgClass=0xF5, msgID=0x05, rateUSB=10), UBXMessage('CFG', 'CFG-MSG', SET, msgClass=0xF5, msgID=0x4D, rateUSB=1),UBXMessage('CFG', 'CFG-MSG', SET, msgClass=0xF5, msgID=0x57, rateUSB=1),UBXMessage('CFG', 'CFG-MSG', SET, msgClass=0xF5, msgID=0xE6, rateUSB=1)
>>> print(msg)
(UBXMessage(b'\x06', b'\x01', 1, payload=b'\xf5\x05\x00\x00\x00\n\x00\x00'), UBXMessage(b'\x06', b'\x01', 1, payload=b'\xf5M\x00\x00\x00\x01\x00\x00'), UBXMessage(b'\x06', b'\x01', 1, payload=b'\xf5W\x00\x00\x00\x01\x00\x00'), UBXMessage(b'\x06', b'\x01', 1, payload=b'\xf5\xe6\x00\x00\x00\x01\x00\x00'))
>>> output = msg.serialize()
Traceback (most recent call last):
  File "<stdin>", line 1, in <module>
AttributeError: 'tuple' object has no attribute 'serialize'

But when I tried to SET individually that error was not popping

>>> msg = UBXMessage('CFG', 'CFG-MSG', SET, msgClass=0xF5, msgID=0x05, rateUSB=10)
>>> output = msg.serialize()
>>> output
b'\xb5b\x06\x01\x08\x00\xf5\x05\x00\x00\x00\n\x00\x00\x13\x8c'
>>> msg = UBXMessage('CFG', 'CFG-MSG', SET, msgClass=0xF5, msgID=0x4D, rateUSB=1)
>>> output = msg.serialize()
>>> output
b'\xb5b\x06\x01\x08\x00\xf5M\x00\x00\x00\x01\x00\x00Ri'
>>> msg = UBXMessage('CFG', 'CFG-MSG', SET, msgClass=0xF5, msgID=0x57, rateUSB=1)
>>> output = msg.serialize()
>>> output
b'\xb5b\x06\x01\x08\x00\xf5W\x00\x00\x00\x01\x00\x00\\\xaf'
>>> msg = UBXMessage('CFG', 'CFG-MSG', SET, msgClass=0xF5, msgID=0xE6, rateUSB=1)
>>> output = msg.serialize()
>>> output
b'\xb5b\x06\x01\x08\x00\xf5\xe6\x00\x00\x00\x01\x00\x00\xeb\x98'
>>> serialOut.write(output)
16

You are doing msg.serialize and serialOut.write as well? Do you receive more messages now? 1005, 1077 etc?

I don't think I am receiving 1077 and other msg id. I am receiving only 1005.

Please suggest me the testing sequence.

@dayjaby
Copy link
Contributor

dayjaby commented Feb 6, 2023

You are trying to serialize a tuple. You have to serialize each element of the tuple:

msgs = UBXMessage('CFG', 'CFG-MSG', SET, msgClass=0xF5, msgID=0x05, rateUSB=10), UBXMessage('CFG', 'CFG-MSG', SET, msgClass=0xF5, msgID=0x4D, rateUSB=1),UBXMessage('CFG', 'CFG-MSG', SET, msgClass=0xF5, msgID=0x57, rateUSB=1),UBXMessage('CFG', 'CFG-MSG', SET, msgClass=0xF5, msgID=0xE6, rateUSB=1)
for msg in msgs:
   # do serialize and write

@Murali-IsPagro
Copy link
Author

@dayjaby , I did individually also. This is the output

>>> msg = UBXMessage('CFG', 'CFG-MSG', SET, msgClass=0xF5, msgID=0x05, rateUSB=10)
>>> output = msg.serialize()
>>> output
b'\xb5b\x06\x01\x08\x00\xf5\x05\x00\x00\x00\n\x00\x00\x13\x8c'
>>> msg = UBXMessage('CFG', 'CFG-MSG', SET, msgClass=0xF5, msgID=0x4D, rateUSB=1)
>>> output = msg.serialize()
>>> output
b'\xb5b\x06\x01\x08\x00\xf5M\x00\x00\x00\x01\x00\x00Ri'
>>> msg = UBXMessage('CFG', 'CFG-MSG', SET, msgClass=0xF5, msgID=0x57, rateUSB=1)
>>> output = msg.serialize()
>>> output
b'\xb5b\x06\x01\x08\x00\xf5W\x00\x00\x00\x01\x00\x00\\\xaf'
>>> msg = UBXMessage('CFG', 'CFG-MSG', SET, msgClass=0xF5, msgID=0xE6, rateUSB=1)
>>> output = msg.serialize()
>>> output
b'\xb5b\x06\x01\x08\x00\xf5\xe6\x00\x00\x00\x01\x00\x00\xeb\x98'
>>> serialOut.write(output)
16

@Murali-IsPagro
Copy link
Author

@dayjaby , Please , check this.

@dayjaby , I did individually also. This is the output

>>> msg = UBXMessage('CFG', 'CFG-MSG', SET, msgClass=0xF5, msgID=0x05, rateUSB=10)
>>> output = msg.serialize()
>>> output
b'\xb5b\x06\x01\x08\x00\xf5\x05\x00\x00\x00\n\x00\x00\x13\x8c'
>>> msg = UBXMessage('CFG', 'CFG-MSG', SET, msgClass=0xF5, msgID=0x4D, rateUSB=1)
>>> output = msg.serialize()
>>> output
b'\xb5b\x06\x01\x08\x00\xf5M\x00\x00\x00\x01\x00\x00Ri'
>>> msg = UBXMessage('CFG', 'CFG-MSG', SET, msgClass=0xF5, msgID=0x57, rateUSB=1)
>>> output = msg.serialize()
>>> output
b'\xb5b\x06\x01\x08\x00\xf5W\x00\x00\x00\x01\x00\x00\\\xaf'
>>> msg = UBXMessage('CFG', 'CFG-MSG', SET, msgClass=0xF5, msgID=0xE6, rateUSB=1)
>>> output = msg.serialize()
>>> output
b'\xb5b\x06\x01\x08\x00\xf5\xe6\x00\x00\x00\x01\x00\x00\xeb\x98'
>>> serialOut.write(output)
16

@dayjaby
Copy link
Contributor

dayjaby commented Feb 15, 2023

Well, next step would be to perform a survey in. Have you done that?

UBXMessage('CFG', 'CFG-TMODE3', SET,
    rcvrMode=1, # Survey In    
    svinMinDur=duration_in_seconds,
    svinAccLimit=int(accuracy_in_meters * 10000
)

To receive some feedback during survey in:

UBXMessage('CFG', 'CFG-MSG', SET, msgClass=0x01, msgID=0x3B, rateUSB=1)  # NAV-SVIN, 1 Hz

@Murali-IsPagro
Copy link
Author

No, I haven't done survey-in. I will do once and update you.

@julianoes
Copy link
Collaborator

The MAVSDK-Python Rtk plugin should now work.

This is the fix on the C++ side.

You need mavsdk_server version v1.4.16 and MAVSDK-Python v1.4.8.

Example how to use the Rtk Plugin in Python: rtk.py.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

No branches or pull requests

3 participants