Support Ardupilot

[mhl787156]

This PR adds support for ardupilot.

Primary change is for enabling GUIDED mode, and the requirement (I think) to takeoff before setpoint_position is accepted.

Still WIP, Ardupilot vehicle arms, switches to guided mode, but will not takeoff. Currently returns a 4 status on takeoff
pointing to Coordinate frame not supported (https://mavlink.io/en/messages/common.html#MAV_CMD_ACK_ERR_NOT_SUPPORTED)

[mhl787156]

[Note this is not actually complete yet xD I was hoping you might be able to provide some input as to how to get an AP drone flying 🤣 ]
Currently for AP:

1. ARM
2. Change to GUIDED
3. Use Takeoff -> it returns a code 4

[rob-clarke]

I suspect this is caused by the interference of the SITL default rangefinder. In GUIDED mode, any takeoff altitude less than the rangefinder maximum altitude (by default 50m) sets the frame of the takeoff waypoint to ABOVE_TERRAIN (Source). Then, when the autopilot attempts to set the destination, it fails. Potentially due to missing terrain data.

[rob-clarke]

However, one would think that this should be caught by the ...terrain_source() == ...TERRAIN_FROM_RANGEFINDER line.

[rob-clarke]

I think it's actually this part:

if (takeoff_alt_cm <= copter.rangefinder_state.alt_cm) {
            return false;
        }

The altitude reported by the SITL range finder under Gazebo seems to be constantly the maximum, here 5000cm or 50m

[mhl787156]

Is there even a rangefinder on the model? Where do the rangefinder measurements come from?

[rob-clarke]

Not entirely sure, I think there is a rangefinder simulator built in to the SITL binary itself which may be being used?

[rob-clarke]

Now that the vehicle can take off, there still appear to be problems... Lots of "Waiting for Navigate" messages.

I've added a branch to disable the rangefinder for the ArduPilot SITL in the main repo. Once that is built, you can use uobflightlabstarling/starling-sim-ardupilot-copter:disable-rangefinder to test.

Not yet working

[mhl787156]

The waiting for navigate messages are that thread blocking until the drone has reached the navigation target. If you want a quick way of running, call the navigate service directly with argument {"z: 2, auto_arm: true'}

[mhl787156]

Do you observe the drone moving around on gazebo?

[rob-clarke]

It gets off the floor but just stays hovering at 2m

[rob-clarke]

Now it takes off, then hovers in place while I assume the trajectory follower goes round the trajectory, then lands.

[mhl787156]

Hmm sometimes it takes a little bit to stabilize after takeoff for it to trigger the 'i'm here'...

[rob-clarke]

The not moving was likely caused by the (recently fixed) error in the bridge yaml files. However, using nightly where the fix is implemented results in no takeoff again. My guess is that sending the setpoints somehow disturbs the takeoff sequence. With no setpoints being sent, the takeoff completes, if setpoints are sent, the vehicle doesn't leave the ground...