ArduPlane: Factor out guided airspeed cmd logic from GCS_MAVLinkPlane to ModeGuided

[Ryanf55]

• Precursor to Add airspeed control to AP_DDS for plane in GUIDED #29829 to reduce code duplication

[peterbarker]

Thanks for doing things this way.

[peterbarker]

exits early without setting acceleration and timestamp. I think you might have mentioned this

[Ryanf55]

Yea, do you want a functional change in this refactor too? I'd prefer to solve that in a separate PR with a test that verifies it's working.

[peterbarker]

What testing has this had?

[peterbarker]

remove

[Ryanf55]

What testing has this had?

Try this python script. I played around with changing speed, sending the same command multiple times, and putting a really slow acceleration (0.25). You can also try airspeeds of 9 and 31 which are rejected. Run this against default SITL, with an added UDP output on 14551.

#!/usr/bin/env python3
from pymavlink import mavutil

# Connect to the vehicle
master = mavutil.mavlink_connection('udp:127.0.0.1:14551')  # Change if needed

# Wait for heartbeat
master.wait_heartbeat()
print(f"Connected to system {master.target_system}, component {master.target_component}")

# Define constants
MAV_CMD_GUIDED_CHANGE_SPEED = 43000
SPEED_TYPE_AIRSPEED = 0
TARGET_SPEED = 24.0  # m/s
INSTANT_ACCEL = 0.0  # 0 = change speed instantly

# Send command
master.mav.command_long_send(
    master.target_system,
    master.target_component,
    MAV_CMD_GUIDED_CHANGE_SPEED,
    0,                      # confirmation
    SPEED_TYPE_AIRSPEED,    # param1: speed type
    TARGET_SPEED,           # param2: target speed (m/s)
    INSTANT_ACCEL,          # param3: acceleration (0 = instant)
    0, 0, 0, 0              # param4-7: empty
)

print(f"MAV_CMD_GUIDED_CHANGE_SPEED sent: airspeed {TARGET_SPEED} m/s")


import time
start_time = time.time()
timeout = 5  # seconds

while time.time() - start_time < timeout:
    msg = master.recv_match(type='COMMAND_ACK', blocking=True, timeout=1)
    if msg and msg.command == MAV_CMD_GUIDED_CHANGE_SPEED:
        result = mavutil.mavlink.enums['MAV_RESULT'][msg.result].name
        print(f"Received ACK: {result}")
        break
else:
    print("Timed out waiting for COMMAND_ACK")
    

[peterbarker]

What testing has this had?

Try this python script. I played around with changing speed, sending the same command multiple times, and putting a really slow acceleration (0.25). You can also try airspeeds of 9 and 31 which are rejected. Run this against default SITL, with an added UDP output on 14551.

#!/usr/bin/env python3
from pymavlink import mavutil

# Connect to the vehicle
master = mavutil.mavlink_connection('udp:127.0.0.1:14551')  # Change if needed

# Wait for heartbeat
master.wait_heartbeat()
print(f"Connected to system {master.target_system}, component {master.target_component}")

# Define constants
MAV_CMD_GUIDED_CHANGE_SPEED = 43000
SPEED_TYPE_AIRSPEED = 0
TARGET_SPEED = 24.0  # m/s
INSTANT_ACCEL = 0.0  # 0 = change speed instantly

# Send command
master.mav.command_long_send(
    master.target_system,
    master.target_component,
    MAV_CMD_GUIDED_CHANGE_SPEED,
    0,                      # confirmation
    SPEED_TYPE_AIRSPEED,    # param1: speed type
    TARGET_SPEED,           # param2: target speed (m/s)
    INSTANT_ACCEL,          # param3: acceleration (0 = instant)
    0, 0, 0, 0              # param4-7: empty
)

print(f"MAV_CMD_GUIDED_CHANGE_SPEED sent: airspeed {TARGET_SPEED} m/s")


import time
start_time = time.time()
timeout = 5  # seconds

while time.time() - start_time < timeout:
    msg = master.recv_match(type='COMMAND_ACK', blocking=True, timeout=1)
    if msg and msg.command == MAV_CMD_GUIDED_CHANGE_SPEED:
        result = mavutil.mavlink.enums['MAV_RESULT'][msg.result].name
        print(f"Received ACK: {result}")
        break
else:
    print("Timed out waiting for COMMAND_ACK")
    

An autotest would be more useful. And yes, chatgpt can generate those with a little more prompting :-)

[peterbarker]

Looks like code has been moved.

Just need to know it's been tested in its current form.

[Ryanf55]

Looks like code has been moved.

Just need to know it's been tested in its current form.

Retested, LGTM on c82247d

ryan@FRIEDMANR-W11:~/Dev/ardu_ws/src/ardupilot$ ./guided_airspeed.py 
Connected to system 1, component 0
MAV_CMD_GUIDED_CHANGE_SPEED sent: airspeed 18.0 m/s
Received ACK: MAV_RESULT_ACCEPTED
ryan@FRIEDMANR-W11:~/Dev/ardu_ws/src/ardupilot$ ./guided_airspeed.py 
Connected to system 1, component 0
MAV_CMD_GUIDED_CHANGE_SPEED sent: airspeed 35.0 m/s
Received ACK: MAV_RESULT_FAILED
ryan@FRIEDMANR-W11:~/Dev/ardu_ws/src/ardupilot$ git log -n 1
commit c82247dcbece28b4f5ba4d42744c51f61d0f704c (HEAD -> guided-factor-airspeed-command-to-mode-guided, origin/guided-factor-airspeed-command-to-mode-guided)
Author: Ryan Friedman <[email protected]>
Date:   Tue Apr 22 09:00:06 2025 -0600

    ArduPlane: Factor out guided airspeed change logic
    
    * Precursor to https://github.com/ArduPilot/ardupilot/pull/29829
    
    Signed-off-by: Ryan Friedman <[email protected]>
    Co-authored-by: Peter Barker <[email protected]>
    ```

[peterbarker]

LGTM

[IamPete1]

LGTM