Realtime PID Tuning with message stream from each axis (RPY) using pymavlink

ArduCopter Copter 4.3
, ,
1

Hi,

I am currently setting up my raspberry pi to communicate with my cube orange via pymavlink and I have been following this documentation.

It is really great and have all I need and I am able to basically access most parameters or even messages stream that I need. However, I am currently a bit stuck here where I am planning on performing some real time PID tuning in which I am able to access each axis’s desired and achieved rate along with its PID parameters in a single stream messages. I know that this is possible as I have found the following xml file containing the PID_TUNING_AXIS in line 786 as well as the message structure for it which contains all the data I need in line 1213.

Some configurations that to I have done to my cubepilot is:

SR0_EXTRA1 : 4 Hz
GCS_PID_MASK: 15.0 (Tick all Roll, Pitch, Yaw, Accz)

When I run the following python script:

while True:

    # Raspberry Pi GND, GPIO 14(RX), GPIO 15(TX)
    # You will need to enable "Serial Port" Interface and you can do so with the following command
    # `sudo raspi-config`
    # Once this is enabled, by default, the data is transmitted over /dev/serial0
    # You should also check the baudrate set on SERIAL2_BAUD parameter in Mission Planner and ensure that it corresponds to what you have set here
    # You can also use SITL to test development, and you can do so by downloading the sitl, and running it with the `sim_vehicle.py` command
    # Run SITL first, choose the sitl connection instead of the serial connection
    # TODO(Khalid): Use args parser so you can flexibly allocate different baudrate or serial port
    # TODO(Khalid): Use logger
    sitl = "127.0.0.1:14550"
    serial = "/dev/serial0"
    mav = mavutil.mavlink_connection(sitl, baud=57600)
    heartbeatStatus = mav.wait_heartbeat(timeout=5)

    for key in mav.messages.keys():
        print(key)

I get the following output:

....
....
LOCAL_POSITION_NED
PID_TUNING
PID_TUNING[1]
PID_TUNING[2]
PID_TUNING[3]
PID_TUNING[4]
VIBRATION
POSITION_TARGET_GLOBAL_INT
....
....

As far as I understand it here, 1 corresponds to Roll, 2: Pitch, 3: Yaw etc. but whenever I try to access those values, it returns None and I couldnt understand why? However, accessing the PID_TUNING data returns me an axis of 1 (Roll), with a few other data such as desired rate, achieved rate, PID etc.

I dont understand why it wouldnt work for the others and I hope someone can point me in the right direction.

Edit:
My quick implementation is as follow:

    PIDTuning = mav.recv_match(type='PID_TUNING', blocking=True, timeout=2)
    PIDTuning1 = mav.recv_match(type='PID_TUNING[1]', blocking=True, timeout=2)

It outputs:

PID_TUNING {axis : 1, desired : -0.011007029563188553, achieved : -0.006042619235813618, FF : -0.0, P : -0.000670195440761745, I : -0.00024550777743570507, D : 3.076280336244963e-05, SRate : 0.053419217467308044, PDmod : 1.0}
None
2

So, I’ve had a play around with the following and I dont really like my current solution to the problem that I am facing. Apparently, for some reason, whenever I set the ‘GCS_PID_MASK’ to only a single axis, it returns me the axis that I need by accessing the ‘PID_TUNING’ messages:

Screenshot from 2023-06-22 17-35-47
Screenshot from 2023-06-22 17-35-47835×141 12.3 KB

e.g I did the following:

# Bitmask val corresponds to the value you set in GCS_PID_MASK
# 1: Only Roll
# 2: Only Pitch
# 4: Only Yaw
# 8: Only Accz
master.param_set_send('GCS_PID_MASK', bitmask_val)
master.recv_match(type='PID_TUNING', blocking=True, timeout=timeout)

So, whenever I implement the following code for each axis (eg use a for loop to set different params each time [not ideal]), I am able to access the PID data coming from each axis. I really hate this solution cause setting the parameters will actually decrease my performance. It took me at least 0.8 seconds to access each data which is really bad since I want to stream it to my computer at 1 Hz.

I have also used the request_message_interval() function to request the PID tuning data to be streamed at a higher frequency (I set it to 10 Hz for now) but same thing.

I hope someone can point me in the right direction