AP_Mount: Add parameter to control relative pan option for servo mounts

[spark404]

The current code in AP_Mount_Servo sets the bool relative_pan to false. As far as I understand it this makes it calculate angles without calculating the pan angle relative to the current angle of the vehicle. When setting gimbal ROI targets separate from the ROI targets of the drone this results unexpected angles. I expected the gimbal to point to the ROI, regardless of the current yaw of the vehicle.

To adjust the behaviour i added a parameter MNT(2)_REL_PAN to change this from hardcoded to configurable behaviour. The default remains false which is the current hardcoded setting.

Limited testing done by installing this firmware with patch on a quadcopter and feeding several coordinates to the gimbal using MAV_CMD_DO_MOUNT_CONTROL.

[amilcarlucas]

I'm all for this this fixes a bug that some users reported. But I think it would be bettter if you use a MNT()_OPTIONS flags parameter.
That way we can add future configuration options bits to it, without having to add extra parameters.

[Hwurzburg]

@spark404 We discussed in DevCall today...a few comments:

1. I will be running some SITL tests on this myself in the next couple of day to be sure we understand the changes to operation
2. We (@rmackay9 Randy MacKay and myself) were wondering if you would be open to an interactive conversation, perhaps on the ArduPilot discord server, to be sure we understand your goals and to discuss the ideas we discussed in the DevCall....is this something you could do? Randy is in Japan, I am in Texas ...how about 2PM your time, 10PM Japan time, and 7AM my time some day?

[spark404]

2. We (@rmackay9 Randy MacKay and myself) were wondering if you would be open to an interactive conversation, perhaps on the ArduPilot discord server, to be sure we understand your goals and to discuss the ideas we discussed in the DevCall....is this something you could do? Randy is in Japan, I am in Texas ...how about 2PM your time, 10PM Japan time, and 7AM my time some day?

Sure, no problem :-) I've registered on the discord server, so it's best to use that to find the best date. But today (jan 26th) 2 pm CEST could work, tomorrow as well.

[spark404]

I'm all for this this fixes a bug that some users reported. But I think it would be bettter if you use a MNT()_OPTIONS flags parameter.
That way we can add future configuration options bits to it, without having to add extra parameters.

That makes sense, I've made the changes. Please advice if this is the preferred way of doing this. Other discussions still pending on a meeting.

[rmackay9]

Reading this more closely this just seems like a bug to me so I don't think we even need a parameter. Can anyone imagine a case where the servo outputs wouldn't be relative to the vehicle's heading when doing ROI?

[spark404]

Reading this more closely this just seems like a bug to me so I don't think we even need a parameter. Can anyone imagine a case where the servo outputs wouldn't be relative to the vehicle's heading when doing ROI?

I can't think of any, but didn't want to risk overlooking something. So a parameter seemed the prudent action. I've looked through the other gimbals and I couldn't find a place where relative pan was set to false other than in AP_Mount_Servo.

[rmackay9]

@spark404,

For this PR I think you can just change those couple of falses to trues and we can merge it.

On a related note I think we will need a parameter and an auxiliary switch to allow users to control whether they want the yaw to remain locked in earth frame or in body frame when controlling the gimbal's pan via RC. This is scope creep though so not required to get this PR in.

[spark404]

For this PR I think you can just change those couple of falses to trues and we can merge it.

Done, just pushed the commit.

On a related note I think we will need a parameter and an auxiliary switch to allow users to control whether they want the yaw to remain locked in earth frame or in body frame when controlling the gimbal's pan via RC. This is scope creep though so not required to get this PR in.

Nice, sounds like a useful feature.

[spark404]

Reading this more closely this just seems like a bug to me so I don't think we even need a parameter. Can anyone imagine a case where the servo outputs wouldn't be relative to the vehicle's heading when doing ROI?

@rmackay9 I was planning a mission in QGroundControl and actually found a "use case". The gimbal orientation as set by QGC actually uses the earth reference to plan the gimbal angles. See attached screen grab. I'm still sure this behaviour would be troublesome when used with smarter gimbals that do use the vehicle orientation. Like storm gimbals in serial or MAVLink mode will probably not point in the right direction.

(Unless either QGC or ArduCopter is being smart about this and compensates the angles for the gimbal relative to the vehicle yaw somewhere in the code. I don't know enough about the internals of the mission planning stuff to answer that)

[spark404]

OK, I fell in the rabbit hole I guess, but here is a blurb on relative panning in AP_Mount and specifically from the AP_Mount_Servo point of view.

So, we have these different modes for Gimbals. Each mode has their own needs and assumptions about the world around it. Starting with the easy ones and mainly focusing on the yaw (or pan) angle.

Additional Notes (edited):

• This all assumes that stabilisation is done by the gimbal. There are some differences in the code path, but I think the principles will hold when servo is used with stabilisation.

MAV_MOUNT_MODE_RETRACT:
This mode is used to indicated that the gimbal should put itself in a safe position. It could be to retract it in the fuselage or to protect a camera lens from scratching while landing. The flight controller doesn’t know what a safe position is for a particular UAV. In case of servo move the gimbal to a neutral position (angle 0, 0, 0) relative to the UAV frame. The actual reference isn’t considering the vehicle frame at all, it sets angles to zero and that should move the gimbal back to the same position as initialized.

MAV_MOUNT_MODE_NEUTRAL:
Place the gimbal in a neutral position. Just as retract, the actual reference isn’t considering the vehicle frame at all, it sets angles to zero and that should move the gimbal back to the same position as initialized.

MAV_MOUNT_MODE_MAVLINK_TARGETING:
This becomes more interesting. Using the MOUNT_CONTROL message the flight controller sets angles for the gimbal. As per the comments in AP_Mount these angles are earth frame angles. The expected behavior is to make the gimbal point to an arbitrary angle regardless of the UAV frame. This allows a GCS to steer a camera independently of the UAV position. It’s worth discussing what the expected behaviour of a servo mount is in this case. I’m assuming that a servo mount moves relative to the initialized position and because its attached to the UAV its movement is always relative to a UAV. So, when receiving angles in earth frame the code should calculate the actual angles considering the UAV position and the requested angle. Say the vehicle is flying with a yaw angle of 105° and the received ef yaw angle is 45° the servo gimbal should rotate (45-105) -60°. (Assumption based on 0 yaw being equal to 0 yaw in the UAV. This obviously depends on the initialized position and might be a case for a parameter to allow for offsets in cases a servo gimbal is initialized at another angle)

MAV_MOUNT_MODE_RC_TARGETING:
This is a special case. Here we leave it up to the pilot and for servo mounts this means that we allow the pilot to set angles relative to the neutral position (effectively making all angles relative to the UAV frame. Again, depending on the initialized orientation of the gimbal.

MAV_MOUNT_MODE_GPS_POINT:
MAV_MOUNT_MODE_HOME_LOCATION:
MAV_MOUNT_MODE_SYSID_TARGET:

Taking these three together as they are from a gimbal orientation perspective quite similar. The goals it to make the gimbal point to a location for which we supply GPS coordinates. Considering only the yaw angle, this is done by calculating the bearing from the current position of the UAV to the indicated location. This bearing should be compared with the current heading of the UAV and the difference should be used as the input for the yaw angle. Another case that could be made is that as the bearing calculated is with respect to the earth frame the angle should be set according to the earth frame reference. In that case it’s left up to the gimbal to compensate for any yaw rotation.

After writing all this down, my takeaway is that whether or not the servo mount should use relative pan is upto the “intelligence” in the gimbal. If the gimbal can determine its own heading, providing earth frame angles would work. If the gimbal knows only about its own initialization state and rotates itself to around that position with the angles provided, the logical behaviour would be to have the UAV calculate the yaw angles relative to the UAV yaw. Assuming that a servo mount is in general not an intelligent mount (hope I’m not offending any mounts here) making all calculations relative to the vehicle provides the most consistent behaviour. This means that GCS and missions shouldn’t assume to provide angles in ef, or additional code would need to be in the fc to calculate from ef to UAV frame when determining angles.

I hope this makes any sense and would love some feedback from people that know a lot more about the code and the intensions behind it. I could be completely wrong about all of this and would love to understand why.

Semi-related food for thought; When running through the calculations in AP_Mount_Backend for yaw angles in noticed that an approximation is used for both the distance between GPS coordinates and bearing. I’m assuming this is to avoid an overload in floating point angle calculation. However by design this introduces and error, which on relatively short distances could be non-trivial. I calculated scenario with a target at 66 meters and bearing -116° and the mount code calculated 101 meters distance and -106° bearing. This roughly translates to the camera pointing a spot 37 meter away from the intended target. For a wide view camera this might be an acceptable error, but with (extreme) zoom cameras this error might place the target out of frame.

[spark404]

@Hwurzburg here is a Testflight (SITL/Gazebo with the patch) : https://youtu.be/5L8sH2xFCvs
The test sequence (for the gimbal) is;
@ WP 2: pitch 90°
@ WP 4: pitch 0°
@ WP 6: yaw -90°
@ WP 8: set ROI (to water_tower)
@ WP 11: neutral (0,0,0)

As far as I can test it works pretty well with planned missions including tracking a ROI.

This is the 3d gimbal used in the video: khancyr/ardupilot_gazebo#46

[amilcarlucas]

ping, should this be discussed in a DevCall?

[amilcarlucas]

@spark404 can you attend Ardupilot's EU Developer Call ?

[spark404]

@spark404 can you attend Ardupilot's EU Developer Call ?

@amilcarlucas Yes, I'll try to be there. I should be able to make it if I got the timezone conversion right :-)