Add airspeed control to AP_DDS for plane in GUIDED

[Ryanf55]

Purpose

I want to control plane's airspeed from DDS while in GUIDED mode, similar to MAV_CMD_GUIDED_CHANGE_SPEED.

Description

We already have low level velocity control in the DDS interface, currently implemented on copter. We can use the same topic for airspeed commands, assuming the linear Y and Z components are NaN, and it's in the body frame obeying the ROS REP-103 FLU convention. I assume that ROS users will want airspeed control in plane rather than local GPS speed control.

The guided code was pulled from GCS_Mavlink_Plane::handle_command_int_guided_slew_commands where it handled MAV_CMD_GUIDED_CHANGE_SPEED:

ardupilot/ArduPlane/GCS_MAVLink_Plane.cpp

Lines 672 to 709 in 8e34fd1

	case MAV_CMD_GUIDED_CHANGE_SPEED: {
	// command is only valid in guided mode
	if (plane.control_mode != &plane.mode_guided) {
	return MAV_RESULT_FAILED;
	}
	// only airspeed commands are supported right now...
	if (int(packet.param1) != SPEED_TYPE_AIRSPEED) { // since SPEED_TYPE is int in range 0-1 and packet.param1 is a *float* this works.
	return MAV_RESULT_DENIED;
	}
	// reject airspeeds that are outside of the tuning envelope
	if (packet.param2 > plane.aparm.airspeed_max || packet.param2 < plane.aparm.airspeed_min) {
	return MAV_RESULT_DENIED;
	}
	// no need to process any new packet/s with the
	// same airspeed any further, if we are already doing it.
	float new_target_airspeed_cm = packet.param2 * 100;
	if ( is_equal(new_target_airspeed_cm,plane.guided_state.target_airspeed_cm)) {
	return MAV_RESULT_ACCEPTED;
	}
	plane.guided_state.target_airspeed_cm = new_target_airspeed_cm;
	plane.guided_state.target_airspeed_time_ms = AP_HAL::millis();
	if (is_zero(packet.param3)) {
	// the user wanted /maximum acceleration, pick a large value as close enough
	plane.guided_state.target_airspeed_accel = 1000.0f;
	} else {
	plane.guided_state.target_airspeed_accel = fabsf(packet.param3);
	}
	// assign an acceleration direction
	if (plane.guided_state.target_airspeed_cm < plane.target_airspeed_cm) {
	plane.guided_state.target_airspeed_accel *= -1.0f;
	}
	return MAV_RESULT_ACCEPTED;
	}

Example Command

header:
  stamp:
    sec: 0
    nanosec: 0
  frame_id: base_link
twist:
  linear:
    x: 30.0
    y: .nan
    z: .nan
  angular:
    x: 0.0
    y: 0.0
    z: 0.0

Tests Performed

colcon build --packages-up-to ardupilot_sitl
source install/setup.bash
ros2 launch ardupilot_sitl sitl_dds_udp.launch.py command:=arduplane model:=plane defaults:=$(ros2 pkg prefix --share ardupilot_sitl)/config/models/plane.parm

In another terminal, take off and get the vehicle into GUIDED so it's ready to accept airspeed commands

mavproxy.py --master :14551
mode takeoff
arm throttle
# wait till at altitude
mode guided

In another terminal, send the airspeed command to 26 m/s.

source install/setup.bash
ros2 topic pub /ap/cmd_vel geometry_msgs/msg/TwistStamped "{header: {stamp: now, frame_id: base_link}, twist: {linear: {x: 26.0, y: .nan, z: .nan}}}" --once

Observe the plane speed up from 23 to 26 m/s in mavproxy's console.

Now, slow it down to 15 m/S.

ros2 topic pub /ap/cmd_vel geometry_msgs/msg/TwistStamped "{header: {stamp: now, frame_id: base_link}, twist: {linear: {x: 15.0, y: .nan, z: .nan}}}" --once

Observe it slow down.

YOu can also try with 9 to see it's rejected (too slow) and 31 (too fast), or in another mode like FBWB to see it's rejected.

[MatthewFehl365]

It would be nice if support for climb rate control was also added! otherwise great feature.

[peterbarker]

Time for a rebase

[Ryanf55]

Time for a rebase

Done, retestested with the same test cases. Ready for final review.