DroneResponse / hardware-tests

0 stars 2 forks source link

Gimbal Convention #30

Open tawfiq1200 opened 2 years ago

tawfiq1200 commented 2 years ago

We were discussing the convention of the gimbal and I am copying some of the things we discussed for reference:

I had a confusion about the convention the gimbal and Gazebo are having and then @murphym18 said this:

So far I’ve seen three conventions (but six are possible):

  1. The FRD convention. This is used in aviation. This is what gazebo uses:

gazeb_convention

  1. The convention Ankit showed us : The “pitch” axis points right. The “roll” axis points backwards. The “yaw” axis points down.

    However, This webpage does not specify a vector for any of the axes. So we don’t know what direction the vector (1, 0, 0) maps to.

    My best guess is that (1, 0, 0) is the roll access, (0, 1, 0) is the yaw axis and (0, 0, 1) is the pitch axis. But that could be wrong. In fact, they list yaw first. So if we assume that (1, 0, 0) is yaw, and we apply right hand rule, then (0, 1, 0) is the pitch axis and (0, 0, 1) points backwards… I’m not sure.

  1. The convention used by the SD-HX10 gimbal. This convention isn’t clear to me. Someone at SD said they would update the manual. Here’s my best guess: This convention is like FRD except we need to convert to euler angles, multiply by -1, then convert back to a Quaternion. I’m not sure, but could we interpret this as: the x-axis points backwards? (b/c -1 forward == backwards) the y-axis points left? (b/c -1 right == left) the z-axis points up? (b/c -1 * down == up)

    I have seen some evidence that this Interpretation might be correct:

    PX4 issue #19933 - gimbal orientation broken in case of pitch == -90 and yaw != 0

Specifically, I’m referring to this image from the github issue:

image_gimbal

The axes are labeled with blue rotation arrows. And I think the side with the blue label is the positive direction. If this is the case, then my interpretation above seems correct (x=backwards, y=left, z=down). But I wouldn’t say this is strong evidence.

The core question

For me, it would be most helpful if we could answer the following questions:

what direction does this vector point: (1, 0, 0)?
what direction does this vector point: (0, 1, 0)?
what direction does this vector point: (0, 0, 1)?The SD-HX10 gamble convention isn’t clear to me either. 

Then at some point, @murphym18 said this:

According to the gimbal controller:

what direction does this vector point: (1, 0, 0)? If we rotate around this axis what does the gimbal do? Put another way, if we build a quaternion using the axis angle formula, and we give this vector as the axis and we give a positive angle, what does the gimbal do?
what direction does this vector point: (0, 1, 0)? If we rotate around this axis what does the gimbal do? 
what direction does this vector point: (0, 0, 1)? If we rotate around this axis what does the gimbal do? 

I think, the answers are:

(1, 0, 0) == -1 * the drone's compass heading. This is the "backwards" axis but it's level with the horizon. If we rotate around this axis, the gimbal rolls left.
(0, 0, 1) == up. It points away from the ground (that is to say, -1 * the direction a plumbline points). If we rotate around this axis, the gimbal yaws left.
(0, 1, 0) is determined by the right hand rule. We can find this direction by taking the cross product: (0, 0, 1) X (1, 0, 0). But intuitively this points to the "left" since our neutral gimbal frame is level with the horizon.  the gimbal would look down if we were to rotate around this axis by a positive angle.

They said on the phone that the gimbal frame doesn't follow the drone's body frame exactly. Nate gave a good reason as to why: The gimbal tries to keep the camera level with the horizon where down in the image points towards the ground. And to do so, the gimbal updates its orientation very quickly using an IMU. This way the video will look stable even if the drone is moving around. This is good because if we had to command the gimbal to keep the image level with the horizon, then we'd record worse video because our sensor data is older and our commands arrive later. The controller has the newest data since it's connected to the sensors and its commands arrive earlier because it's directly connected to the motors.

Then at some point, I made the following assumption which may not be accurate:

Okay, then just to confirm, this is the Gazebo convention and drone is the exact opposite:

roll (+ angles=rotate right, - angles=rotate left) pitch (+ angles=look up, - angles=look down) yaw (+ angles=turn right, - angles=turn left)

So, drone is:

roll (- angles=rotate right, + angles=rotate left) pitch (- angles=look up, + angles=look down) yaw (- angles=turn right, + angles=turn left)

The drone should match Ankit's simulator's convention however.

Then @murphym18 replied to that:

I think it's confusing to say that the gimbal convention for gazebo is opposite from the SD-HX10 convention. And I don't think Ankit's convention is the same as the SD-HX10's convention.

To describe each gimbal convention we need to know:

The neutral orientation. This is how the neutral frame moves with the drone. The spatial direction of each principal axis. We know this property when we can point in the direction of the "roll," "pitch," and "yaw" axes The numerical vector associated with each principal axis. We know this property when we can assign numbers to the roll pitch and yaw axes.

For the SD-HX10 gimbal convention

The neutral orientation follows the body frame of the drone partially (the gimbal frame moves when the drone changes its compass heading) spatially: It seems like roll is around the "backwards" axis, pitch is around the "left" axis, and yaw is around the "up" axis. numerical: the roll axis is (1, 0, 0), the pitch axis is (0, 1, 0) and the yaw axis is (0, 0, 1)

For the Gazebo gimbal convention

The neutral orientation follows the body frame exactly spatially: the roll axis points forward, the pitch axis points right, the yaw axis points down (this one property might be opposite from the SD-HX10, however) numerically: the roll axis is (1, 0, 0), the pitch axis is (0, 1, 0), the yaw axis is (0, 0, 1) For the convention Ankit showed us, I don't have much information. I have this link: https://compsci290-s2016.github.io/CoursePage/Materials/EulerAnglesViz/

From this link, I can say: It looks like the neutral orientation follows the body frame exactly. Is this true? spatially: roll axis points backwards, the pitch axis points right, the yaw axis points down numerically: I have no information. But I would guess that the roll axis is (1, 0, 0), the pitch axis is (0, 1, 0) and the yaw axis is (0, 0, 1)

It seems like there are differences between all three conventions.

murphym18 commented 2 years ago

I've been reading the SimpleBGC 32bit software user manual. A few things jumped out at me.

On page 46:

Order of Euler angles

The control of the gimbal from RC transmitter or joystick is made by 3 separate angles (called “Euler angles”): ROLL (to control horizon), PITCH (to tilt up-down), and YAW (to turn left-right). To rotate camera from one direction to another, we can make three separate rotations by three axes. But the order of rotations plays a role. You can change the order of rotations in the parameter “Order of Euler angles”. The difference is displayed in the picture below:

The default order is “Camera →PITCH →ROLL →YAW →Frame”. As you can see, it does not allow to roll camera to angle greater that ±90 degrees, because in this case PITCH axis becomes equal to YAW axis and the PITCH angle can not be distinguished from the YAW angle. it's recommended to set Min.angle=-85, Max.angle=85 for ROLL axis in the “RC” tab, to not allow this case.

Looking at the hardware:

image

I can see that:

Therefore we have a Camera →PITCH →ROLL →YAW →Frame gimbal.

With that in mind, on page 101 it says:

There are two ways to control [the] gimbal:

  1. Sending an angle setpoint in COMMAND_LONG:
    • DO_MOUNT_CONTROL – sets the provided angles as a target
    • DO_MOUNT_CONFIGURE – set mode of operation
      • MAV_MOUNT_MODE_NEUTRAL – go to home position
      • MAV_MOUNT_MODE_MAVLINK_TARGETING – get target angles from the DO_MOUNT_CONTROL command
      • MAV_MOUNT_MODE_RC_TARGETING – return control to the RC or "Follow" mode (MavLink control is turned OFF)

NOTE: By default, YAW angle is considered relative to frame (legacy behavior in ArduPilot FC), so we add YAW angle of the frame as received in the message ATTITUDE. But it can be changed by setting the flag “YAW angle is absolute” in the GUI.

  1. Passing RC signal using message RC_CHANNELS_RAW or RC_CHANNELS and assigning RC channels API_VIRT_CH_1-16 to the functions in GUI.

So it looks like the gimbal accepts DO_MOUNT_CONTROL commands. Since we're sending gimbal v2 messages, PX4 is translating our commands into MAVLink V1 messages.

As a side note: there is bug in PX4 related to gimbals like ours. When pitch is +/-90 degrees there is gimbal lock. We're probably going to want to merge this fixes ASAP: https://github.com/PX4/PX4-Autopilot/pull/19951

But until that fix is applied, I take it that our quaternion is converted into Euler angles using the 3-2-1 convention.

And I'm not sure what to say about the problems in this video: https://drive.google.com/file/d/1T53yEPoDFjyx29tSguazxO6zlD1KEMAp/edit

I think the worst case scenario would be us having to send DO_MOUNT_CONTROL commands where we convert to Euler angles using the 3-1-2 convention.

With all that said, the software manual helped us constrain the possible gimbal conventions:

For the neutral orientation, it looks like the yaw angle is relative to the [body] frame. It also seems like the a roll and pitch angles are indeed "absolute"

For the axes of rotation:

If we go by our experiments then it's probably true that:

But please correct me if I'm wrong.

Also it looks like the community needs an open source gimbal controller because STORM32 and SimpleBGC still use Euler angles and that's causing issues.

murphym18 commented 2 years ago

We should consider enabling the YAW angle is absolute setting in SimpleBGC 32bit.

tawfiq1200 commented 2 years ago

@n-snyder may take note of this.

JaneClelandHuang commented 2 years ago

Everyone, I discussed the gimbal configuration with SD (Nate and Adam) today to find out if there were any reference documents behind their decisions for how to configure +ve and -ve for yaw, pitch, and roll. There are no reference documents. What I think this might mean is that if we want DR to be sufficiently flexible to work with diverse gimbal configurations, we might need some sort of configuration file that tells us how the currently installed gimbal is configured. @murphym18 how hard would it be to integrate configuration information into the math calculations?

murphym18 commented 2 years ago

I created https://github.com/DroneResponse/DR-OnboardAutonomy/issues/125 with my thoughts on how we might integrate gimbal configuration.

So long as the SD-HX10 gimbal can accept a quaternion, then the approach I described there seems like it will work.

But are we sure we can send a quaternion? This video at 45 seconds shows a problem: https://drive.google.com/file/d/1T53yEPoDFjyx29tSguazxO6zlD1KEMAp/edit

Was the problem caused by the wires getting twisted? Or is there something wrong with our code?

There is a known bug in PX4 that effects our gimbal. We're probably going to want to cherry pick https://github.com/PX4/PX4-Autopilot/pull/19951 or update PX4 after they merge it. But this bug should not have caused the problem in the video.

n-snyder commented 2 years ago

Thank you for sharing this video. There are acceptance tests that we perform for the gimbal (without using quaternions), and I'll ensure that ours includes combined pitch and yaw with angles similar to this.

murphym18 commented 2 years ago

thank you @n-snyder

In case it’s helpful, the gimbal is yawing left 90 degrees and pitching up 45 degrees

JaneClelandHuang commented 2 years ago

@n-snyder fixed this in MC. @n-snyder can you explain how you fixed it please? We were really busy when you were explaining at MC. I believe you changed a parameter?? Anything else??

n-snyder commented 2 years ago

@JaneClelandHuang, I fixed the issue where the gimbal seemed to point upward all the time. The issue was that the knob on the RC for pitch was slightly out of calibration, so that the center position was demanding upward pitch. I calibrated the RC knob on the Taranis, and in PX4 I added deadzone to the channel controlling pitch. I have an improvement to this fix planned for the next set of aircraft.

I suspect that the issue in the video that @murphym18 posted is unrelated.