Copter: does not hold position after takeoff and WP_NAVALT_MIN = 3

[VDLJu]

Bug report

Issue details

Adjusting home location after the next waypoint target vector is calculated causes fly-away like behavior as the drone keeps going as it cannot reach the waypoint.

See the check below:

ardupilot/libraries/AC_WPNav/AC_WPNav.cpp

Line 444 in 1a7284c

Vector3f dist_to_dest = (curr_pos - Vector3f(0,0,terr_offset)) - _destination;

The curr_pos is given based on the updated home position, where as the _destination is using the previous origin (home position).

Also the curr_delta used by the waypoint navigation controller has the same problem where the curr_pos is using new origin and _origin is the old one.

ardupilot/libraries/AC_WPNav/AC_WPNav.cpp

Line 334 in 1a7284c

Vector3f curr_delta = (curr_pos - Vector3f(0,0,terr_offset)) - _origin;

The issue is probably more complicated what I have so far described here, but hopefully this is enough to clarify what the issue is.

.

Version
This behavior was observed on version 3.6.11, but is probably affecting many previous versions also.

Platform
[ ] All
[ ] AntennaTracker
[X] Copter
[X] Plane
[X] Rover
[X] Submarine

All platforms that uses WP navigation controller.

Logs
2019-11-22 12-18-12.zip

[rmackay9]

As a side note, the inertial_nav and AC_WPNav work mostly off the EKF origin which is different from the AHRS Home (which is where RTL returns to). I'm not saying we don't have a problem but I don't think it's correct that the curr_pos is being modified because home is moved.

[VDLJu]

Thanks for looking into this problem @rmackay9.
I don't have the proficiency or knowledge to propose any fixes to this problem.

Hopefully there is now enough information for someone with time, skills and interest to improve this part.

[AlexeyChernenko]

From a log: Copter kept its target Roll and Pitch as 0. It didn't fly anywhere by target position, but drifts with a wind.

[MikeCharikov]

Yes. Very strange behaviour after UAV was switching to AUTO mode. It was acting like it was in AltHold mode: it holds the altitude, but its desired Pitch and Roll was zero. So UAV was just drifting (may be because of wind). We can see it easily from logs.

Also if we look at position and target position we can see, that target position was following drone position. So global target position was correct but UAV just didn't follow it. It actually didn't follow anything [pitch, roll were constant]

Problem is that UAV was in AUTO mode.

[rmackay9]

@MikeCharikov, re just the last point, the position controller uses a "leash" so the target will never be too far from the drone. If the drone moves too far from the target, the target is dragged towards the drone.

[MikeCharikov]

@rmackay9 Yes, exactly.

One more thing. Modes were switched by RC.

Configured modes are:

PARM, 114599808, FLTMODE1, 5
PARM, 114599826, FLTMODE2, 6
PARM, 114599844, FLTMODE3, 2
PARM, 114599862, FLTMODE4, 6
PARM, 114599880, FLTMODE5, 3
PARM, 114599957, FLTMODE6, 6

So when mode was switched from LOITER to AUTO i think it also should pass AltHold. It wasn't logged though. But when mode was switched back from AUTO to LOITER - AltHold was reported.

My mad idea - could UAV report AUTO mode but left in unreported AltHold?

[rmackay9]

@MikeCharikov, If it reports it's in Auto then it really should be in auto.

I've never used the do-set-home mission command, I was actually surprised to see that we even supported this so I suspect that there is a bug but I can't immediately reproduce the issue in SITL. For now I would highly recommend removing the do-set-home command, it's normally not needed. The command also seems to have an altitude of 117.54m which means home is being set 117m above the vehicle's home at the time the command is run (i.e. 117m above the take-off location). If the mission was run a couple of times, it would increase the home position by 117m each time it was run which would cause it to climb very high.

For others readings this, please note that in this test the mission itself contains a do-set-home command, the user has not just moved the home position using the GCS which is a much more common thing to do.

[rmackay9]

I suspect the issue is related to the WP_NAVALT_MIN parameter which is set to "3m". The purpose of this parameter is to make sure the vehicle does not try and navigate until it has reached the specified height above home during takeoff.

The do-set-home command is being run before this and has an altitude of 117.54m. So this means the moment the mission starts, the home altitude will be set 117m higher meaning that the vehicle will think it's current altitude is -117m. This is well below the "3m" held in the WP_NAVALT_MIN parameter value.

The question then becomes why wasn't the vehicle climbing..

[VDLJu]

This is the second time this behavior was observed.

I found a log from earlier flight and it has the same symptoms of having roll and pitch angle targets set to zero while on AUTO mode. This time the user switched to Loiter and then back to AUTO which worked well after that. This is from different UAV unit and is running on AC 3.6.9.

That mission didn't have do_set_home command at all. The first command in mission was MAV_CMD_NAV_TAKEOFF. So it seems that it is not fully related to do_set_home command.

You can find download link to that log below.

Log uploaded to gdrive

[rmackay9]

@VDLJu, ok, thanks. I think I've reproduced it in SITL now doing this:

• param set SIM_WIND_SPD 2 (to create some artificial wind)
• param set WP_NAVALT_MIN 40
• create and run a simple mission with just a single takeoff command to 50m

The vehicle takes off but doesn't control it's horizontal position. This is expected at first but at about 36m it also stops climbing.

[VDLJu]

I suspect the issue is related to the WP_NAVALT_MIN parameter which is set to "3m". The purpose of this parameter is to make sure the vehicle does not try and navigate until it has reached the specified height above home during takeoff.

The do-set-home command is being run before this and has an altitude of 117.54m. So this means the moment the mission starts, the home altitude will be set 117m higher meaning that the vehicle will think it's current altitude is -117m. This is well below the "3m" held in the WP_NAVALT_MIN parameter value.

The question then becomes why wasn't the vehicle climbing..

On the log posted on my last comment the UAV was clearly above the WP_NAVALT_MIN when switched to AUTO. There was no do-set-home in use. So the above description doesn't apply to this one?

Is it possible that the problem you were able find with SITL and the ones shown on the logs are two different problems?

[rmackay9]

@VDLJu,

I think you're right that the do-set-home is not directly related except that it can cause the altitude to become very low which can trigger the underlying issue which is in the way the WP_NAVALT_MIN is implemented.

I'm quite certain that what I've reproduced in SITL is the same underlying issue. I think for now, it would be best if you set the WP_NAVALT_MIN parameter to zero and I'm 99% certain you'll find the problem goes away.

By the way, the CTUN.Alt is not actually the altitude above home but rather the altitude above the EKF origin. To see the altitude above home check the POS.RelHomeAlt value.

[MikeCharikov]

@MikeCharikov, If it reports it's in Auto then it really should be in auto.

I've never used the do-set-home mission command, I was actually surprised to see that we even supported this so I suspect that there is a bug but I can't immediately reproduce the issue in SITL. For now I would highly recommend removing the do-set-home command, it's normally not needed. The command also seems to have an altitude of 117.54m which means home is being set 117m above the vehicle's home at the time the command is run (i.e. 117m above the take-off location). If the mission was run a couple of times, it would increase the home position by 117m each time it was run which would cause it to climb very high.

@rmackay9

Hm. We have used mission DO_SET_HOME for ages and never had a problem with it.

Checked this. DO_SET_HOME mission command sets absolute [AMSL] home altitude, not relative to current home.

It calls ahrs.set_home() directly and ahrs.set_home uses location altitude as absolute [AMSL] value.

When HP is set with do_set_home command from GCS you can set location Frame and in case of MAV_FRAME_GLOBAL_RELATIVE_ALT or MAV_FRAME_GLOBAL_RELATIVE_ALT_INT new home alt is relative. But in common case it is absolute

`

          new_home_loc.alt = packet.z * 100;

            // handle relative altitude
            if (packet.frame == MAV_FRAME_GLOBAL_RELATIVE_ALT || packet.frame == MAV_FRAME_GLOBAL_RELATIVE_ALT_INT) {
                if (!AP::ahrs().home_is_set()) {
                    // cannot use relative altitude if home is not set
                    break;
                }
                new_home_loc.alt += copter.ahrs.get_home().alt;
            }`

Also if we look at attached logs for ORGN - we can see that home location alt was exactly 117.54, and not the previous home.alt+117.54

[rmackay9]

@MikeCharikov,

OK, thanks for checking. When testing I used a newer version of MP that allows setting the altitude type (aka frame) to relative, absolute or terrain. When I set it to "relative" (which seemed to be the default in MP) then it behaved badly. Perhaps in stable MP the default is absolute for this command. Perhaps we should add a change so that we reject the command if the altitude if it's non-zero and the frame is relative.

Note: that MP doesn't actually display the do-set-home command at all so it has to be added using the "UNKNOWN" option.

Can you describe why you're keen to use do-set-home?

[MikeCharikov]

Can you describe why you're keen to use do-set-home?

It was legacy function. We'll remove it in upcoming software version.

[MikeCharikov]

@VDLJu, ok, thanks. I think I've reproduced it in SITL now doing this:

• param set SIM_WIND_SPD 2 (to create some artificial wind)
• param set WP_NAVALT_MIN 40
• create and run a simple mission with just a single takeoff command to 50m

The vehicle takes off but doesn't control it's horizontal position. This is expected at first but at about 36m it also stops climbing.

AC_WPNav::advance_wp_target_along_track

In this case reached_leash_limit becomes true from some moment and _track_desired stops to update so _pos_target.z is fixed on some value. (Actually this is rather good because if target altitude continues to update after reached_leash_limit, it will start to descend obviously.)

[rmackay9]

@MikeCharikov, yes, exactly. It's reaching the end of the leash so it stops trying to advance the intermediate target point towards the final destination. This is the correct behaviour but there is still a real bug here that we need to fix.

I've talked with Leonard Hall and we think that we need to change how the vehicle handles the WP_NAVALT_MIN parameter. I've got a partial fix here but I haven't tested it at all yet. It needs more review before we can fly the fix.

[rmackay9]

After some delay, @lthall and I have come up with a solution that seems to work and I've raised a PR #13695.

Any and all feedback especially from @VDLJu and @MikeCharikov is very welcome!

[rmackay9]

I think this issue is fixed with the PR linked above so closing.

[VDLJu]

Hi,

Sorry I haven't replied to this and been able to test this.
The linked PR idea LGTM