Copter: Guided mode velocity controller and pos_and_vel controller have significant drift

Hi,

I’m sending velocity commands from a companion computer to the PixHawk running ArduCopter 3.4 firmware using the set_position_target_local_ned message:

master.mav.set_position_target_local_ned_send(master.target_system, master.target_component, 0, 8, 1991, 1, 2, 3, data.vx_desired, data.vy_desired, data.vz_desired, 7, 8, 9, 10 ,data.psid_desired)

It gets received well and at relatively high frequency (20Hz) by ArduPilot which is shown in the logs. The desired velocities get send into the guided velocity controller (not pos nor posvel). I also implemented a yaw rate controller into the guided mode. All of this works and the UAV responds in flight to the commands, although very slowly and with some considerable delay, which on its own is quite strange given the high update rate.

However, there is a clear difference in flight behaviour if it is in loiter or guided mode when both are receiving zero input from pilot/companion computer (so hover). The flights take place outside with good GPS quality and no wind. During loiter, the drift is around 10-20cm from initial position (first half of image), while for guided, this is 60cm (second halve of image).

I already tuned the roll, pitch and loiter PIDs and set the Loiter accel settings higher for crisper response and less drift, which is working really well in loiter. I get good response and short stopping distance. However, in guided mode, the UAV flies like a freight train, with very slow response and stopping distances of +50cm instead of 10cm. I found another post on this forum (@vasarhelyi ) complaining about the slow response of guided mode and how to implement the accelerations similar to loiter mode as apparently, the accel settings in loiter are not used in guided:

https://discuss.ardupilot.org/t/guided-mode-independent-params-for-acceleration-velocity-limits-and-smoothing/14772

https://github.com/ArduPilot/ardupilot/commit/b7134a6f2338ab6ed8972f4da51aeee882009759

Nonetheless, this helps a little its performance, but not much. The drift is still significant. I think the reason is that it is trying to match the desired velocity, but due to inaccuracies, the position drifts.
.not for less drift nor in better response (less stopping distance).

I then tried to use the pos_and_vel controller, sending both pos and vel commands equal to zero. Again this should result in hover and even track current position. However, its behaviour does not show this. There are two issues:

When sending (pos.x,pos.y,pos.z,vel.x,vel.y,vel.z) = (0,0,0,0,0,0), the drone slowly descends at constant rate (around 5cm/s). One can overrule it, but its annoying. I’ve written an issue report: https://github.com/ArduPilot/ardupilot/issues/6735

When sending (pos.x,pos.y,pos.z,vel.x,vel.y,vel.z) = (0,0,0,0,0,0.5) to arducopter it should climb with 0.5m/s. However, it does not, except for descending due to ,the possibly related, issue #6735. I’ve also written an issue report for this: https://github.com/ArduPilot/ardupilot/issues/6736
When sending (pos.x,pos.y,pos.z,vel.x,vel.y,vel.z) = (0,0,-1.5,0,0,0), it responds immediately.
The pos_and_vel controller works by integrating the desired velocity and adding it to the desired position target in control.guided.cpp. This works for xy_controller but doesn’t have effect for the z-controller which is called separately:

// advance position target using velocity target
guided_pos_target_cm += guided_vel_target_cms * dt;

When printing out values, it looks like the position error is indeed corrected for the desired velocity, but the next iteration of the update_z_controller, this correction is no longer present. As the correction only happens at pos update rate, most position_target outputs from guided_posvel_control_run() are not compensated for the desired velocity. In addition, when update_z_controller() is called, it calls pos_to_rate_z() which only looks at the position error, not the velocity error as _flags.use_desvel_ff_z is false:

// add feed forward component
if (_flags.use_desvel_ff_z) {
_vel_target.z += _vel_desired.z;
}

I solved this by changing two items in the code:

Every time new position and velocity target message arrives, the new position is set with pos_control.set_pos_target(position). This function sets _flags.use_desvel_ff_z to false each time and sets _vel_desired.z to zero. These latter two are commented.
Similar to pos controller in guided mode and loiter, but not present in the pos_and_vel controller, the alt target should be compensated by feedforward for the desired climb rate right before calling update_z_controller() using:

// update altitude target
set_alt_target_from_climb_rate_ff(_vel_desired.z, _dt, false);

With all of these changes, although not fully solved, the pos_and_vel controller works quite well. The accuracy of the XY position is still not great. I believe this is due to me using local position commands. Even though I always send pos = (0,0,0), the position is added to the current position. But when the droen is drifting due to wind or other reasons, it takes the new position as the new target position. As such, it still drifts around. If (pos.x,pos.y,pos.z,vel.x,vel.y,vel.z) = (0,0,0,0,0,0) it should track the last (global) position the drone had right before the velocities were set to zero. I’m going to adapt the code for this and when I tested it, I will post it here also.

Update 8/8/2017: I implemented the change in code described above and tested it and it was much better and very similar to drift accuracy of loiter. I even flew it in confined space with obstacle avoidance active and it had gentle response with none of the oscillating behaviour I had with the pure velocity controller. I now have nearly no drift in XY at all when XY-velocities are zero. I’m going to refine the code and robustness of it. I still have the slow descend described in https://github.com/ArduPilot/ardupilot/issues/6736. I’m going to try some new solutions for that as well. Unfortunately the weather is not good for flight testing the next few days, so my next update might take some time.

Feedback is welcome! I’m searching my way through this rather in the dark.
My goal is to have to command drone (pixhawk) through companion computer in confined space with guided flight mode. I want the minimal drift of the loiter mode when in hover while having fast response to velocity commands if applied.

Cheers,

Jon

3 Likes

Copter v3.5 was released recently, have you tried it yet?

Hi,

No I haven’t. I adapted the ArduCopter 3.4 code for my own obstacle avoidance mission quite a lot (in other places). I don’t want (and have the time) to upgrade to the latest version and test everything again as I’m approaching my deadline. However, I explored the master version thoroughly as I was hoping it would contain code changes which could improve the behaviour, but there are no significant changes in pos_and_vel and pure velocity controllers of guided mode which would solve this. As discussed above, I already implemented the addition of the loiter accel and speed settings, which is present in 3.5. That is the only significant difference found.

Cheers,

Jon

Hi @jverbeke,

Thank you for this great and detailed post !
I’ve been strugglling with position and velocity commands for some time now and never reallly got satisfying results…
Setting a (Lon, Lat) target position seems to work well for me, but doesn’t seem to work well in the Z direction.
I’ve seen you add some correction to the master but do you know if and when these will be available ?

WIth your experience, is there any way to have a good altitude control without modifying the arducopter code and upload a custom one ? If there is, what would be the best approach in your opinion ?

Do I understand it right that you managed to have good results even indoors ? If so, how do you manage to enter guided mode if you don’t have any GPS signal ?

I thank you again for this great post and for any extra info you can provide,

Regards,

Hi @Thomasss,

You can find the pull request here with the updated code:

The pull-request isn’t added to the master yet, it still awaits confirmation and the lead developers want it coded more generic.

The pos-vel controller doesn’t handle the Z-velocity commands well and a feed-forward flag should be enabled to solve this. Also the height controller doesn’t take into account the terrain slope, which is now corrected.
I only tested the guided mode outdoor (within the corridors of fruit orchards) as you indeed need GPS. The altitude response now is very good.

So if you want to solve it short-term, then you will need to adapt the code yourself and upload it to your drone.
Sorry.

Cheers,

Jon

2 Likes

Any update on this topic?

Is the suggested changes and the pull request now merged into master branch of versions 3.4, 3.5 and/or 3.6?

As a starter I need to know that in guided mode if I send a command and then due to a problem the connection between my companion board or GCS and the flight controller is lost, then is there a way that for example after a few seconds of sending no guided commands, the quad returns to home (RTL/H)?

What about availability of manual user intervention by RC in guided mode? Is it possible?

Thanks in advance.

1 Like