Copter/AC_PosControl/Scripting: slung payload oscillation suppression

[rmackay9]

This adds slung payload oscillation suppression using a Lua script together with a new AC_PosControl feature that allows external callers (e.g. scripts) to add horizontal position or velocity offsets to the vehicle while flying in Auto mode.

The payload oscillation suppression only operates while the vehicle is in Auto mode executing a SCRIPT_TIME or PAYLOAD_PLACE command. E.g. it does not trying to reduce oscillation while the vehicle if flying towards a waypoint nor in any other flight mode. A blog post including video is here.

Changes include:

1. SITL's slung payload simulator is enhance so that wind (e.g. SIM_WIND_SPD, SIM_WIND_DIR) affects the payload. This allows us to create an offsets between the slung payload and vehicle
2. AC_PosControl gets new get/set methods for position and velocity offsets. External callers (including scripting) can use these to add offsets to the vehicle's target position or velocity
3. AP_Vehicle/AP_Copter/AC_WPNav/AP_Scripting get equivalent new methods/bindings to set the position and velocity offsets (e.g. set_pos_offset, set_vel_offset). Perhaps we should rename/replace these if we want to support more modes besides Auto
4. AP_Scripting's modules directory gets message definitions for GLOBAL_POSITION_INT and HEARTBEAT. We haven't normally added these but without them it makes it very difficult for normal users to use scripts that consume or sends these mavlink messages
5. slung-payload.lua applet is added to AP_Scripting. This script only operates while the vehicle is in Auto mode executing a SCRIPT_TIME or PAYLOAD_PLACE command. It moves the vehicle towards the payload to reduce its oscillation with the speed of movement controlled by the SLUP_POS_P parameter. It also tries to estimate the payload's long-term position offset from the vehicle and slowly moves the vehicle in the opposite direction so that the package lands right on the waypoint. The speed of this movement can be adjusted using the SLUP_WP_POS_P parameter

This has been well tested in SITL and moderately well tested on a real vehicle:

• checked that AC_PosControl position and velocity targets result in the expected behaviour. See screenshots below
  
• checked that the simulated payload lands precisely on the waypoint
• checked that simulated wind affects the payload as expected (e.g. payload is pushed by the wind)
• checked every navigation mission command with an offset of x=20m,y=20m to confirm they worked as expected

Video of SITL test: https://www.youtube.com/watch?v=jL2YUByYv2Y

Known issues we should fix before merging:

• when switching between SCurve navigation and pure position control navigation the offsets are lost leading to a jump in the vehicle's position target. This happens in Auto mode when switching from a WAYPOINT command and PAYLOAD_PLACE
• when switching from takeoff to SCurve navigation the SCurves seem to use twice the expected position offset

[lthall]

I have done my first pass and my initial focus is on just the position controller work. I think I have two general concerns:

1. We don't handle xy and z in the same way. I think this can be handled with a small change.
2. We have limited the input to just position or velocity when both these can be provided with higher order inputs ie. We can specify a moving position offset by specifying position, velocity and acceleration to achieve a more responsive offset target movement.

[lthall]

I am thinking we should not pass pos_offset_z into update_pos_offset_z and instead just use _pos_offset_target.z in that function like we do update_pos_offset_xy.

We would then set _pos_offset_target.z at the top of this function.

This would be more consistent between xy and z.

[rmackay9]

ok thanks. My concern about changing those lines is it would mean I would need to re-test surface tracking and/or terrain following

[IamPete1]

Do we want to log the offsets all the time? For the PD scale stuff we added a new message that was only logged if they were set (not one in that case, not zero in your case).

[rmackay9]

This was discussed on the dev call and I'll make the following changes:

• PSCx offsets will be logged to a new message PSCO (?)
• Scripting bindings will use a new position controller object (instead of going through AP_Vehicle)
• Check / change places where a reference has been replaced with an object to ensure the code is not doubling up on calculations. also consider moving from .h to .cpp

[rmackay9]

Here is the flash cost of this feature

Binary Name      Text [B]         Data [B]     BSS (B)        Total Flash Change [B] (%)      Flash Free After PR (B)
---------------  ---------------  -----------  -------------  ----------------------------  -------------------------
arducopter       1420 (+0.1463%)  0 (0.0000%)  4 (+0.0020%)   1420 (+0.1461%)                                   58872
antennatracker   132 (+0.0185%)   0 (0.0000%)  4 (+0.0020%)   132 (+0.0184%)                                   315792
arducopter-heli  1424 (+0.1460%)  0 (0.0000%)  0 (0.0000%)    1424 (+0.1458%)                                   53936
ardusub          1260 (+0.1410%)  0 (0.0000%)  4 (+0.0020%)   1260 (+0.1408%)                                  135928
ardurover        132 (+0.0149%)   0 (0.0000%)  -4 (-0.0020%)  132 (+0.0148%)                                   142440
blimp            120 (+0.0163%)   0 (0.0000%)  0 (0.0000%)    120 (+0.0163%)                                   296720
arduplane        1148 (+0.1133%)  0 (0.0000%)  -4 (-0.0020%)  1148 (+0.1132%)                                   16752