Add terrain hold altitude control mode #9854

priseborough · 2018-07-07T02:51:24Z

This PR adds a height control mode that uses a ground reference when not moving horizontally and a local position datum reference when moving. An example application is when operating indoors using optical flow. The vehicle can be flown horizontally over obstacles without height increases that could cause it to to collide with overhead obstacles.

Relevant to #9793.

The upper altitude limit has been modified so that the height error gain is consistent with control loop tuning.

This has been tested in SITL using the following branch which contains modifications to simulate baro drift and print transitions: https://github.com/priseborough/Firmware/tree/altControlMode-wip

Test log here: https://logs.px4.io/plot_app?log=05d598f6-181d-421e-a686-164225065079

Note: The upper optical flow height limit is not applied when the vehicle is moving horizontally. This behaviour can be changed, however it will cause a terrain following behaviour under some conditions if we do so.

The reason for adding this altitude control mode is that although an equivalent behaviour can be achieved via use of the EKF2_RNG_AID parameter, this should be avoided if possible. EKF2_RNG_AID was originally developed for an application where the ground effect interference from rotor wash was so severe that it affected vertical velocity estimates and safe takeoffs and landings were not possible.

Range finders are accurate, but not reliable sensors (they can experience large random readings), the terrain under the vehicle cannot be relied on to be flat and although the EKF can handle slow drift and variation in height reference, anything more than that can cause estimation problems that go beyond the vertical position state.

For these reasons, unless baro errors are severe, we should be meeting the requirement for accurate ground relative height control in the position controller when possible.

LorenzMeier · 2018-07-09T06:55:35Z

@RomanBapst FYI

RomanBapst · 2018-07-09T07:59:58Z

src/modules/mc_pos_control/mc_pos_control_params.c

+ * with terrain height variation. Requires a distance to ground sensor. The height controller will
+ * revert to using height above origin if the distance to ground estimate becomes invalid as indicated
+ * by the local_position.distance_bottom_valid message being false.
+ * Set to 2 to control height relative to earth frame origin when stationary and relative to ground


@priseborough Shouldn't it be the other way around? Control height relative to earth frame when moving and relative to ground frame when stationary?

RomanBapst · 2018-07-09T08:03:03Z

@priseborough I will test this using a distance sensor but with no optical flow activated due to PX4/PX4-ECL#474

RomanBapst · 2018-07-09T20:22:16Z

@priseborough I tested this outdoors and it seems to work fine. I forgot to upload the log but will do it tomorrow.

priseborough · 2018-07-10T07:07:16Z

I need to rebase this on the latest flight task PR

RomanBapst · 2018-07-10T08:45:33Z

@priseborough Here's a log from the flight I did. The performance was actually not really nice because the baro is very much affected by wind when moving horizontally. However, I verified that the functionality was as described.

@Stifael @MaEtUgR This is the same log in which I saw the yaw jump at the very beginning.
The strange thing is that I cannot find the log in which I was not able to takeoff in altctl mode. It was the very first log but there is no such log in the SD card. Maybe something else screwed up. Anyway, I'm heading out to test again and will report if it occurs again.

RomanBapst · 2018-07-10T11:40:55Z

Here's the log:
https://review.px4.io/plot_app?log=f06f9dcb-2fff-47d3-b427-4015b794630e

priseborough · 2018-07-11T00:36:04Z

@MaEtUgR I have added you as a reviewer given it is modifying your PR

Stifael · 2018-07-11T11:45:07Z

src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp

+	if (MPC_ALT_MODE.get() == 2) {
+		// terrain hold
+		float spd_xy = Vector2f(&_velocity(0)).length();
+		bool stick_input = Vector2f(&_velocity_setpoint(0)).length() > 0.5f * MPC_ALT_MODE_SPD.get();


can we not use _sticks instead (https://github.com/PX4/Firmware/pull/9731/files#diff-7dfb661344d42aeefa71f4afd9b85be8R60), which would make it scaling independent.
In addition, there is already a parameter for horizontal direction that defines the speed threshold at which the vehicle is considered at rest: https://github.com/PX4/Firmware/blob/master/src/modules/mc_pos_control/mc_pos_control_params.c#L375

Good idea. Will fix.

Stifael · 2018-07-11T12:01:30Z

src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp

 		if (PX4_ISFINITE(_constraints.max_distance_to_ground)) {
-			_constraints.speed_up = math::gradual(_dist_to_bottom, min_distance_to_ground, _constraints.max_distance_to_ground,
-							      _max_speed_up, 0.0f);
+			_constraints.speed_up = math::constrain(MPC_Z_P.get() * (_constraints.max_distance_to_ground - _dist_to_bottom),


I personally prefer to avoid core-position control parameters like MPC_Z_P. Why can we not just limit the speed upwards linearly from max velocity to 0 at maximum altitude?

Because that is a de-facto position error gain than can either be too small/sluggish if the distance between min alt and max alt is large or too high resulting in overshoot if the distance is small. By using the gain we guarantee the fastest ascent possible without excessive overshoot and consistent behaviour regardless of height limit separation.

Stifael · 2018-07-12T06:33:06Z

I don't know how to proceed. Should we wait with merge until pr-flight-task-iteration is in master or should we already merge this additional feature already.
Merging this PR into pr-flight-task-iteration has the drawback that the flighttask PR continues to increase in size and and functionality which are not yet legacy code. The drawback of not merging this PR is obviously the uncertainty of when the flighttask PR gets merged

priseborough · 2018-07-12T07:34:33Z

If we don't merge this now, then it will rot given the churn in pr-flight-task-iteration.

Working this feature into master requires a different implementation and I can do that as a separate PR if pr-flight-task-iteration is going to take too long. We can then include the patches required to revert that PR in pr-flight-task-iteration

priseborough · 2018-07-14T04:58:51Z

@Stifael and @MaEtUgR I've had to rebase it again. If you are not comfortable to merge this, then I will re-write it for master and let you manage the conflicts at the next rebase of pr-flight-task-iteration.

Stifael · 2018-07-16T12:45:44Z

@priseborough, I prefer to merge first pr-flight-task-iteration, then merge this PR and @mrivi PR for avoidance on top. Otherwise pr-flight-task-iteration never comes to an end

This new mode of altitude control uses terrain following when holding position and normal altitude control when moving.

…2 squashed commits) Squashed commits: [ed2a243] FlightTasks: Preserve control loop tuning when applying max altitude limit [b33b947] FlightTasks: Add terrain hold function This new mode of altitude control uses terrain following when holding position and normal altitude control when moving.

Use baro for height in EKF. Use terrain hold mode in height controller.

Use pre-existing MPC_HOLD_MAX_XY parameter for speed threshold. Use _stick_expo variable for stick movement check. Update documentation.

priseborough · 2018-07-20T08:21:33Z

Rebased on master now that pr-flight-task-iteration has been merged.

priseborough mentioned this pull request Jul 7, 2018

mc_pos_control: switch to use bottom distance when stationary; when moving horizontally use local position #9793

Closed

RomanBapst reviewed Jul 9, 2018

View reviewed changes

Stifael force-pushed the pr-flight-task-iteration branch from 300e6e2 to 84c72b4 Compare July 9, 2018 12:30

priseborough force-pushed the pr-altControlMode branch from 4b627ef to ac8e75d Compare July 11, 2018 00:14

priseborough requested a review from MaEtUgR July 11, 2018 00:34

Stifael reviewed Jul 11, 2018

View reviewed changes

Stifael approved these changes Jul 12, 2018

View reviewed changes

MaEtUgR force-pushed the pr-flight-task-iteration branch from e3f92e9 to da794e7 Compare July 12, 2018 10:05

priseborough force-pushed the pr-altControlMode branch from 655d130 to 57f6d2d Compare July 14, 2018 04:56

MaEtUgR force-pushed the pr-flight-task-iteration branch 2 times, most recently from 32084b7 to 7917a1f Compare July 17, 2018 09:42

priseborough added 7 commits July 20, 2018 18:13

mc_pos_control: Add parameter support for terrain hold function

e28d7df

This new mode of altitude control uses terrain following when holding position and normal altitude control when moving.

posix-configs: Update optical flow SITL test parameter defaults

3a71cbc

Use baro for height in EKF. Use terrain hold mode in height controller.

ekf2: Update documentation for EKF2_RNG_AID parameter

d6cadfe

mc_pos_control: Fix parameter description error

8c89105

FlightTasks: update terrain hold transition logic.

1724271

Use pre-existing MPC_HOLD_MAX_XY parameter for speed threshold. Use _stick_expo variable for stick movement check. Update documentation.

mc_pos_control: remove un-used parameter

638ce2d

priseborough force-pushed the pr-altControlMode branch from 57f6d2d to 638ce2d Compare July 20, 2018 08:18

priseborough changed the base branch from pr-flight-task-iteration to master July 20, 2018 08:20

priseborough merged commit a37580c into PX4:master Jul 22, 2018

priseborough deleted the pr-altControlMode branch July 22, 2018 07:44

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Add terrain hold altitude control mode #9854

Add terrain hold altitude control mode #9854

priseborough commented Jul 7, 2018 •

edited by AuterionWrikeBot

Loading

LorenzMeier commented Jul 9, 2018

RomanBapst Jul 9, 2018

priseborough Jul 9, 2018

RomanBapst commented Jul 9, 2018

RomanBapst commented Jul 9, 2018

priseborough commented Jul 10, 2018

RomanBapst commented Jul 10, 2018 •

edited

Loading

RomanBapst commented Jul 10, 2018

priseborough commented Jul 11, 2018

Stifael Jul 11, 2018

priseborough Jul 12, 2018

Stifael Jul 11, 2018 •

edited

Loading

priseborough Jul 12, 2018 •

edited

Loading

Stifael commented Jul 12, 2018

priseborough commented Jul 12, 2018

priseborough commented Jul 14, 2018

Stifael commented Jul 16, 2018

priseborough commented Jul 20, 2018

Add terrain hold altitude control mode #9854

Add terrain hold altitude control mode #9854

Conversation

priseborough commented Jul 7, 2018 • edited by AuterionWrikeBot Loading

LorenzMeier commented Jul 9, 2018

RomanBapst Jul 9, 2018

Choose a reason for hiding this comment

priseborough Jul 9, 2018

Choose a reason for hiding this comment

RomanBapst commented Jul 9, 2018

RomanBapst commented Jul 9, 2018

priseborough commented Jul 10, 2018

RomanBapst commented Jul 10, 2018 • edited Loading

RomanBapst commented Jul 10, 2018

priseborough commented Jul 11, 2018

Stifael Jul 11, 2018

Choose a reason for hiding this comment

priseborough Jul 12, 2018

Choose a reason for hiding this comment

Stifael Jul 11, 2018 • edited Loading

Choose a reason for hiding this comment

priseborough Jul 12, 2018 • edited Loading

Choose a reason for hiding this comment

Stifael commented Jul 12, 2018

priseborough commented Jul 12, 2018

priseborough commented Jul 14, 2018

Stifael commented Jul 16, 2018

priseborough commented Jul 20, 2018

priseborough commented Jul 7, 2018 •

edited by AuterionWrikeBot

Loading

RomanBapst commented Jul 10, 2018 •

edited

Loading

Stifael Jul 11, 2018 •

edited

Loading

priseborough Jul 12, 2018 •

edited

Loading