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://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: Copter: Guided pos and vel controller doesn't hold z position, it descends slowly · Issue #6735 · ArduPilot/ardupilot · GitHub
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: Copter: Guided pos and vel controller doesn't react to velocity.z commands · Issue #6736 · ArduPilot/ardupilot · GitHub
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 Copter: Guided pos and vel controller doesn't react to velocity.z commands · Issue #6736 · ArduPilot/ardupilot · GitHub. 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