AP_Mount Servo gimbal stabilization problem

[akhodakivskiy]

Bug report

Since 4.3 Mounts are configured with new set of parameters, and part of the functionality of the Servo gimbal has been removed. Specifically the Servo gimbal stabilization does not take into account gimbal yaw angle. In neutral position the gimbal correctly compensates for MAV pitch/roll angle, but if the mount is rotated 180 degrees around the Z axis (panned 180 degrees) it will pitch with the plane. In earth coordinates the mount will pitch 2x times of the plane AHRS pitch. If the mount is panned 90 degrees (pointing left or right relative to the MAV body) the pitch servo is expected to compensate for MAV roll, but it keeps compensating for MAV pitch.

While writing this bug report I realized that the problem might be explained by the construction of the servo gimbal. In my case the Pan servo is attached to the MAV body, then comes the Pitch servo, and then the Roll servo. This construction and current implementation activates undesired behavior of the stabilization implementation. If the order was Pitch->Roll->Pan then current implementation would probably work fine. The mount would stabilize by pitch and roll and then pan to desired position. But most gimbals follow the Pan->Pitch->Roll construction pattern (including the one on the photo in the documentation as it's more natural in most cases.It might be necessary to add more parameters that specify servo Gimbal construction order or instruct developers to construct Pitch->Rol->Pan

4.3

Platform [ x] All [ ] AntennaTracker [ ] Copter [ ] Plane [ ] Rover [ ] Submarine

Airframe type All

[rmackay9]

Thanks for the report. I'll think about this a bit. I don't actually have a servo gimbal but maybe I should get one.

[rmackay9]

@akhodakivskiy,

I've looked at the code again and I'm pretty certain you are right. I almost can't believe we have this fundamental a problem with our servo gimbal driver. Sorry about this, I hope to fix this soon.

[rmackay9]

@akhodakivskiy,

Thanks again for the report. I've created PR https://github.com/ArduPilot/ardupilot/pull/22689 which I think mostly resolves this issue but I wonder if you would be willing to test it for me? Please just tell me which autopilot you're using and I'll create a binary.

[akhodakivskiy]

with pleasure, matek h743 wing v3.

I reviewed the PR, and I feel like it still has issues. First of all, do I understand correctly that with this driver the goal is to stabilize the mount in the earth coordinates?

If so then we need to consider the construction of the gimbal in the first place. The gimbal I currently have at hand is Yaw->Pitch->Roll. Meaning that the mount yaw servo is attached to the MAV plane, the pitch servo is attached to the mount yaw plane, and the roll servo is attached to the pitch plane. There are altogether 6 variants of the construction:

1. Yaw->Pitch->Roll
2. Yaw->Roll->Pitch
3. Roll->Yaw->Pitch
4. Roll->Pitch->Yaw
5. Pitch->Roll->Yaw
6. Pitch->Yaw->Roll

One issue is that if we compensate the mount pitch and roll for yaw angle beforehand then it will break variants 3-5.

Another issue is that the servo pitch/roll angles (if the gimbal is set to be level) don't always equal AHRS pitch/roll angles. Consider a MAV that's rolling -20 and pitching -40 degrees. Now we command the roll servo to rotate 20 deg, and the pitch servo to rotate 40 deg. What is the gimbal plane now?

For a Pitch->Roll gimbal (no yaw, pitch servo is attached to the MAV plane, roll servo is attached to the gimbal pitch plane).

Consider the MAV plane normal vector in MAV coordinates (0, 0, 1). Rotate it 20 deg around MAV X axis: (0, -0.3420201433, 0.9396926208) Rotate it 40 deg around MAV Y axis: (0.6040227736, -0.3420201433, 0.7198463104) (1)

For a Roll->Pitch gimbal (no yaw, roll servo is attached to the MAV plane, pitch servo is attached to the gimbal roll plane):

Consider the MAV plane normal vector in MAV coordinates (0, 0, 1). Rotate it 40 deg around MAV Y axis: (0.6427876097, 0, 0.7660444431) Rotate it 20 deg around MAV X axis: (0.6427876097, -0.2620026302, 0.7198463104) (2)

Courtesy to this calculator

Now compare (1) to (2) - those are different vectors, and both don't equal the normal vector in the Earth coordinates. Pitch->Roll and Roll->Pitch gimbals will point in different directions after compensating for AHRS roll and pitch. Adding Yaw servo will further complicate the calculations.

I'll come back home in the evening and will try to figure out the math necessary to level the gimbal. But I strongly feel that this feature is quite broken at this point.

[rmackay9]

@akhodakivskiy,

I neglected to ask which vehicle you're using. In any case, here is a Copter-4.4.0-DEV firmware for the MatekH743 board.

If you're using Plane or Rover please tell me and I can make a binary for one of them as well.

[akhodakivskiy]

@rmackay9 I'm using Plane. But please check my comment above as well.

[rmackay9]

@akhodakivskiy,

Here's a Plane-4.4.0-DEV firmware for the MatekH743 board.

Thanks for the reminder on the gimbal axis order issue. AP's Matrix3 library (commonly used for rotations) allows converting to AP's regular euler angles or to the "312" convention (see code here) so perhaps this would help.

I'm not immediately planning to work on fixing the larger issue of accuracy at large angles though because I think 3-axis servo gimbals are not really very popular. Most users are using brushless gimbals so I'm planning on adding support for more of those in the near future. .. after that perhaps though I might come back to this.

[akhodakivskiy]

@rmackay9 From what I understand in AP_Mount_Servo::update_angle_outputs we need to calculate servo angles from the gimbal yaw/pitch/tilt angles of the AP (RC targeting, mavlink, neutral, etc.). The way I see the solution is below. (Although it's been a while since I last did algebra and the first time I'm poking with AP code, and this approach is probably the least optimal, and most likely is buggy)

1. We have yaw/pitch/tilt angles in angle_rad structure.
2. Create a vector of the gimbal in earth frame.
3. Take the body-to-earth rotation matrix and further rotate it by the gimbal vector.
4. Calculate Euler rotation angles with specific axis order from the rotation matrix. Reference implementation is available here

Once again this is specific for the particular gimbal construction.

    // compute rotation matrix from earth frame to gimbal frame
    Vector3f gimbal_earth = Vector3f(
        cosF(angle_rad.yaw) * cosF(angle_rad.pitch),
        sinF(angle_rad.yaw) * cosF(angle_rad.pitch),
        sinF(angle_rad.pitch),
    );

    // compute rotation matrix from body frame to gimbal frame
    Matrix3f rot_b2g = ahrs.get_rotation_body_to_ned();
    rot_b2g.rotate(gimbal_earth);

    //now compute Euler angles from the rotation matrix rot_b2g
    //per https://github.com/libigl/eigen/blob/master/Eigen/src/Geometry/EulerAngles.h
    ...

[akhodakivskiy]

I'm not immediately planning to work on fixing the larger issue of accuracy at large angles though because I think 3-axis servo gimbals are not really very popular. Most users are using brushless gimbals so I'm planning on adding support for more of those in the near future. .. after that perhaps though I might come back to this.

I feel link 22440 and this issue are related. I will investigate further and will try to craft a proper PR. Question though - are we limited in the amount of math we can afford in the gimbal driver?

[rmackay9]

@akhodakivskiy,

That sounds about right. Here is an alternative implementation involving Matrix maths. I originally thought this would be a good solution for the "errors at large angles" issue. So for example with this fix, the gimbal points in the correct direction even when the vehicle is upside-down. I noticed two potential problems though, the gimbal became shaky when pointed straight down which is a very commonly used attitude. This is surely gimbal lock and is a result of the final conversion to Euler angles. The other problem was dealing with a servo gimbal with less than 360 degrees of yaw movement. I don't think we can just ignore the body-frame yaw limits because if we do then the roll and pitch servos will point in the wrong direction when outside these limits. It got complex at this point and I gave up for now.

Here's a plane-440-dev-servo-mount-big-fix-20JAn2023 for a MatekHP743 using this alternative approach.

[akhodakivskiy]

@akhodakivskiy,

I noticed two potential problems though, the gimbal became shaky when pointed straight down which is a very commonly used attitude. This is surely gimbal lock and is a result of the final conversion to Euler angles.

And once again I think it might be caused by the Euler angles order.

The other problem was dealing with a servo gimbal with less than 360 degrees of yaw movement. I don't think we can just ignore the body-frame yaw limits because if we do then the roll and pitch servos will point in the wrong direction when outside these limits. It got complex at this point and I gave up for now.

I have 270 degrees servo and in 4.3.2 I'm seeing that when yaw lock is engaged the yaw range is limited (beyond the servo limits) on one of the ends. As if the MNT1_YAW_MAX/MNT1_YAW_MIN is applied in earth frame, then overlapped with physical limits in body frame. I will see if the firmware below still has this behavior.

Here's a plane-440-dev-servo-mount-big-fix-20JAn2023 for a MatekHP743 using this alternative approach.

I'll give it a try tomorrow

[akhodakivskiy]

Here's a plane-440-dev-servo-mount-big-fix-20JAn2023 for a MatekHP743 using this alternative approach.

@rmackay9, err.. how do I install .apj firmware on Matek board? Is it possible to extract .hex from it? or maybe build hex separately?

Thanks,

Anton

[rmackay9]

@akhodakivskiy,

If you're using Mission Planner's Firmware Install screen then the "Load custom firmware" screen will accept an .apj file. In fact, .apj should be used in general instead of the .hex file. I think the .apj file actually includes the .hex.

[akhodakivskiy]

so this experimental firmware isn't working as expected. in neutral position the gimbal is reacting as expected to MAV pitch change but it's yawing when mav is rolled. if i turn the gimbal 90 deg then it's reacting correctly to MAV roll, but is yawing if MAV pitches

[akhodakivskiy]

in the latest 4.4.0 dev firmware (485b86e2) the gimbal works as expected

[rmackay9]

@akhodakivskiy, great, thanks very much for testing. This fix will be included in 4.4.0 which will start beta testing in a few weeks.

I'm going to close this issue if that's OK. I'll still see replies though.

thanks again for the report and testing!