[Bug] Terrain Following in Position mode doesn't work as expected

[mars2860]

Describe the bug

Terrain Following works when we set MPC_ALT_MODE=Terrain Following and MPC_POS_MODE=Direct Velocity. Collision Prevention works when we set CP_DIST > 0 and MPC_POS_MODE=Acceleration based (since Commit 0cd6a55). Collision prevention stops to work if I set MPC_POS_MODE=Direct Velocity. And Terrain Following stops to work if I set MPC_POS_MODE=Acceleration Based. Thus these two modes can't work simultaneously.

To Reproduce

make px4_sitl gz_x500_lidar_down

Set MPC_ALT_MODE=Terrain Following and MPC_POS_MODE=Acceleration based. Terrain following will not work.

make px4_sitl gz_x500_lidar_2d_walls

Set CP_DIST=3 and MPC_POS_MODE=Direct Velocity. Collision Prevention will not work.

Expected behavior

No response

Screenshot / Media

No response

Flight Log

No response

Software Version

No response

Flight controller

No response

Vehicle type

None

How are the different components wired up (including port information)

No response

Additional context

No response

[bresch]

Thanks for reporting. Collision prevention is not implemented for the direct velocity mode, however, everything should work in "Acceleration based" pos mode (terrain following too).

[mars2860]

At this time terrain following doesn't work in "Acceleration based" position mode. I have investigated the code and found a bug.

void FlightTaskManualAltitudeSmoothVel::_setOutputState()
{
	_jerk_setpoint(2) = _smoothing.getCurrentJerk();
	_acceleration_setpoint(2) = _smoothing.getCurrentAcceleration();
	_velocity_setpoint(2) = _smoothing.getCurrentVelocity();

	if (!_terrain_hold) {
		if (_terrain_hold_previous) {
			// Reset position setpoint to current position when switching from terrain hold to non-terrain hold
			_smoothing.setCurrentPosition(_position(2));
		}

                // the bug is here: this line set altitude setpoint as NAN when terrain following works
		_position_setpoint(2) = _smoothing.getCurrentPosition();
	}

	_terrain_hold_previous = _terrain_hold;
}

[bresch]

the bug is here: this line set altitude setpoint as NAN when terrain following works

Sorry but I don't get it, when terrain following is active, _terrain_hold is true, so it won't enter this condition, right?

[mars2860]

No, when terrain following is active _terrain_hold is false. This flag is true only when a vehicle stops. You can see it in the code below

void FlightTaskManualAltitude::_updateAltitudeLock() {
...

bool not_moving = spd_xy < 0.5f * _param_mpc_hold_max_xy.get();

			if (!stick_input && not_moving && PX4_ISFINITE(_dist_to_bottom)) {
				// Start using distance to ground
				_terrain_hold = true;

...
}

[dirksavage88]

@mars2860 is this still an issue for you? There have been some updates to the terrain state but not sure if it still breaks with acceleration based input.

[mars2860]

I have fixed this on my local copy of PX4 as I have mentioned above and it works

[farhangnaderi]

@mars2860 Could you specify how this can be reproduced? Should there be any terrain added to the SIM Environment? Would be great if you could also provide a log when the issue is triggered.

[MaEtUgR]

Could you specify how this can be reproduced?

@mars2860 We just talked about this in the dev call. You filled out the how to reproduce but not in detail. Specifically how do you see "Terrain following will not work."?

Collision prevention is not supported in MPC_POS_MODE=Direct Velocity. That setting is only intended for tuning the velocity controller, not for running an actual use case. So the remaining issue I see is you don't get what you want from terrain following with normal Position mode (I adjusted the title accordingly). Could you describe what you expect and what it does different from that?

I did a quick non-scientific test of running make px4_sitl gz_x500_lidar_down and setting MPC_ALT_MODE=Terrain Following.

The default world is empty but if I manually add a dummy cone it seems to go up when I manage to fly over it. I think there should be a better world to test this because the single cone is pretty small and hard to fly over manually.

Here are some screenshots:

[mars2860]

PX4 v1.16.0alpha2, last commit 8d296a50f9. To reproduce set MPC_POS_MODE=Acceleration based, MPC_ALT_MODE=Terrain Following, EKF2_RNG_CNTRL=Enabled, make px4_sitl gz_x500_lidar_down, add to world this model. Expected behavior = the drone goes up in position mode when it flies over a hill. Actual behavior = it doesn't