Plane: support off-board terrain navigation

[srmainwaring]

This is a proof-of-concept implementation of off-board control for ArduPlane for use with the terrain navigation library.

The implementation uses the SET_POSITION_TARGET_GLOBAL_INT mavlink message to match the interface provided by terrain_navigation and adds a custom guided mode (TERRAIN_NAVIGATION = Mode(26)) to minimise the changes in current code. The longer term objective is to use the DDS interface and integrate more closely with the existing controller and guided mode.

There are two choices of controller at compile time. Selection is by the variable in mode_terrain_navigation.cpp:

#define ENABLE_NPFG_CONTROLLER 0

Setting ENABLE_NPFG_CONTROLLER to 1 selects an experimental integration of the Nonlinear Path Following Guidance (NPFG) controller. Unfortunately this is not yet working correctly. The other approach co-opts the existing L1 controller to follow the Dubins path provided by the external controller.

Interpreting setpoint information

The planner populates the fields of SET_POSITION_TARGET_GLOBAL_INT as follows:

The position (lat_int, lon_int, alt) is the closest point on the planned Dubins path to the current vehicle location, where the vehicle location is provided to the planner by the flight controller using the GLOBAL_POSITION_INT message. The alt is always provided as MAV_FRAME_GLOBAL_RELATIVE_ALT_INT, so this is the only case handled.

The horizontal velocity components (vx, vy) comprise a unit vector tangential to the Dubins path at the closest point. The vertical component (vz) is the desired climb rate along the flightpath.

The horizontal acceleration components determine the desired path curvature. The path radius and direction are calculated from the lateral acceleration using r = |v|^2 / |a_lat|.

If the acceleration is zero, the curvature is assumed to be zero (a radius of zero is interpreted as zero curvature).

terrain navigation mode

When using the L1 controller, the terrain navigation mode uses update_loiter if the curvature is non-zero (radius non-zero) and update_waypoint if the curvature is zero (radius zero). When using update_loiter the setpoint position, tangent and turn radius are used to set a center_wp, turn radius and direction. When using update_waypoint the setpoint position and tangent are used to set a next_wp 50m ahead of the setpoint position.

See also

• https://github.com/ethz-asl/terrain-navigation/tree/ros2
• integrate optional NPFG library for wind-aware fixed-wing guidance PX4/PX4-Autopilot#18810
• Plane: handle guided path requests in GUIDED mode #26328

Usage

In the following we assume the colcon workspace for the ROS 2 packages is called ros2-aerial.

Build dependencies

Follow the instructions to set up ArduPilot for ROS 2 Humble.

Build and install the ros2 branch of terrain navigation and any dependencies.

Run

Launch ArduPilot SITL. This launch file simulates a quadplane in a park in Davosdorf.

ros2 launch terrain_navigation_ros ardupilot.launch.py

Connect a mavproxy session to the running simulation:

mavproxy.py --master 127.0.0.1:14550 --console --map

Open the mission editor and load the misson:

ros2-aerial/src/terrain_navigation/terrain_navigation_ros/config/davosdorf_mission.txt

Update a couple of parameters

param set SCALING_SPEED 25.0 # to match AIRSPEED_CRUISE
param set WP_LOITER_RAD 80.0

Run the mission

FBWA> arm throttle
FBWA> auto

The aircraft will takeoff then loiter indefinitely.

Launch mavros configured for ardupilot:

ros2 launch terrain_navigation_ros mavros.launch.py flight_stack:=ardupilot

Launch the terrain planner:

ros2 launch terrain_navigation_ros terrain_planner.launch.py rviz:=false cruise_speed:=25.0 minimum_turn_radius:=80

Launch rviz:

ros2 launch terrain_navigation_ros rviz.launch.py

• Click the Load Terrain button.
• Select Interact in the top tool bar.
• Move the interactive marker over where the plane is loitering and click Update Start.
• Move the interactive marker to another location and click Update Goal.
  A green circle will appear if the goal location is valid. A red circle indicates the goal location is not suitable.
• Click Plan.
• Click Engage Planner.
• Click NAVIGATE.

In MAVProxy you should see the mode has switched to the custom mode (Mode(26) = TERRAIN_NAVIGATION)

AUTO>
Mode(26)>

Testing

NPFG

Is not working as expected and is disabled.

L1

Is satisfactory, although the turns are not tight (the vehicle rate of turn is less than desired).

Figure: The vehicle turn is wider than the setpoint. The red arrow is the desired turn radius and direction. The blue arrow is the path tangent.

Figure: The final loiter circle is wider than the setpoint.

Figure: Logged flight path

[IamPete1]

I guess your radius's are turning out wrong because of the altitude scale we apply for constant load factor.

ardupilot/libraries/AP_L1_Control/AP_L1_Control.cpp

Lines 148 to 178 in 2a67cbe

	float AP_L1_Control::loiter_radius(const float radius) const
	{
	// prevent an insane loiter bank limit
	float sanitized_bank_limit = constrain_float(_loiter_bank_limit, 0.0f, 89.0f);
	float lateral_accel_sea_level = tanf(radians(sanitized_bank_limit)) * GRAVITY_MSS;
	float nominal_velocity_sea_level = 0.0f;
	if(_tecs != nullptr) {
	nominal_velocity_sea_level = _tecs->get_target_airspeed();
	}
	float eas2tas_sq = sq(_ahrs.get_EAS2TAS());
	if (is_zero(sanitized_bank_limit) || is_zero(nominal_velocity_sea_level) ||
	is_zero(lateral_accel_sea_level)) {
	// Missing a sane input for calculating the limit, or the user has
	// requested a straight scaling with altitude. This will always vary
	// with the current altitude, but will at least protect the airframe
	return radius * eas2tas_sq;
	} else {
	float sea_level_radius = sq(nominal_velocity_sea_level) / lateral_accel_sea_level;
	if (sea_level_radius > radius) {
	// If we've told the plane that its sea level radius is unachievable fallback to
	// straight altitude scaling
	return radius * eas2tas_sq;
	} else {
	// select the requested radius, or the required altitude scale, whichever is safer
	return MAX(sea_level_radius * eas2tas_sq, radius);
	}
	}
	}

There has been some discussion on this recently. I think the general view is that we should remove it (by default anyway) because it makes mission planning very unintuitive.

[srmainwaring]

I guess your radius's are turning out wrong because of the altitude scale we apply for constant load factor.

Thanks @IamPete1, I'll try disabling.

Update

Confirmed - the scaling in

float AP_L1_Control::loiter_radius(const float radius) const

is the cause.