AP_Mount:Add GIMBAL_MANAGER_SET_ATTITUDE support

[khanasif786]

Task from Gimbal enhancement list #20985
consumes GIMBAL_MANAGER_SET_ATTITUDE messages

This needs to be merged before it
#308

[peterbarker]

Seem about right.

[khanasif786]

@peterbarker @amilcarlucas Thanks for the suggestions.

[khanasif786]

Sorry. I was thinking we can request review from two people at the same time. Please ignore this.

[rmackay9]

Looks good to me. After this is merged we should add a new item to the developer gimbal control page too. https://ardupilot.org/dev/docs/mavlink-gimbal-mount.html. Not critical of course though.

[khanasif786]

@rmackay9 okay will make a pull on wiki also afterward merging.

[peterbarker]

Looks good to me. After this is merged we should add a new item to the developer gimbal control page too. https://ardupilot.org/dev/docs/mavlink-gimbal-mount.html. Not critical of course though.

I have a concern here that we are not treating this mavlink message in the same way that we treat the vehicle SET_ATTITUDE_TARGET message. @lthall went to a lot of trouble to unscramble that egg for us to we ended up with correct behaviour there. It would be a shame to put another scrambled egg in.

The basic difference between this implementation and the vehicle handling of SET_ATTITUDE_TARGET is that with the vehicle message you are not specifying the rate at which to come to the new attitude, you are specifying the rate at which the vehicle should be moving when it gets to that attitude.

[peterbarker]

I've merged the mavlink part of this.

[peterbarker]

I've tested this on a T3v3 and it does control the gimbal.

The attitude always takes precedent over the rate, so if your quaternion is valid you will come to that attitude as fast as the gimbal allows (pretty damned fast in the T3 case!)

If you have an invalid quaternion then it honours the rates (I have not checked the rate directions).

These were the MAVProxy commands I was using to test:

message GIMBAL_MANAGER_SET_ATTITUDE 0 0 0 0 [float("NAN"),float("NAN"),float("NAN"),float("NAN")] 0.01 0.01 0.01

message GIMBAL_MANAGER_SET_ATTITUDE 0 0 0 0 [1,0,0,0] 0.01 0.01 0.01

message GIMBAL_MANAGER_SET_ATTITUDE 0 0 0 0 [1,0.1,0.1,0.1] 0.001 0.001 0.001

[peterbarker]

I suggest we ignore (and probably throw a warning periodically) if we ever receive a SET_GIMBAL_ATTITUDE message that has both the attitude and the rate set. We don't sensibly handle that case right now, so it's a reasonable precaution and will allow us to implement the same semantics as the vehicle equivalent at some stage.

[rmackay9]

@peterbarker,

Re callers who provide both rate and angle controls, I think that for now at least the correct behaviour is what we have in this PR which is to consume the angle targets and ignore the rate targets. There are no gimbals that support accepting both at the same time so it's a bit different than what Leonard did with our attitude controllers.

[rmackay9]

@peterbarker thanks very much for the review by the way!

[peterbarker]

@peterbarker,

Re callers who provide both rate and angle controls, I think that for now at least the correct behaviour is what we have in this PR which is to consume the angle targets and ignore the rate targets. There are no gimbals that support accepting both at the same time so it's a bit different than what Leonard did with our attitude controllers.

I'm suggesting we enforce that somehow so we don't run into problems when we do accept both later.

It's only a couple of lines of code, pretty sure.

[rmackay9]

@peterbarker,

OK, I don't think it's necessary but it's a team effort so maybe you can provide a suggestion on those few extra lines.

[rmackay9]

I've rebased this on master after merging the mavlink submodule update.

[peterbarker]

@peterbarker,

OK, I don't think it's necessary but it's a team effort so maybe you can provide a suggestion on those few extra lines.

https://github.com/peterbarker/ardupilot/pull/new/pr/no-att-and-rate-together

[lthall]

You could reject messages that provide both angle and rate. That way we wouldn't find ourselves in a situation where we start supporting both and break someone's system.

[peterbarker]

@khanasif786 did you want to cherry-pick my suggested change?

[khanasif786]

@khanasif786 did you want to cherry-pick my suggested change?

@peterbarker. Yeah I have added your commit.

[rmackay9]

Looks like PeterB's suggestion had a little typo in it. So might need to fix that somehow.

Maybe it is "is_nan()" instead of "isn_nan()"?

[peterbarker]

Looks like PeterB's suggestion had a little typo in it. So might need to fix that somehow.

Maybe it is "is_nan()" instead of "isn_nan()"?

Fixed.

[peterbarker]

@rmackay9 if you're happy I'm happy now :-)

Fixed

[khanasif786]

@peterbarker @rmackay9 can it be merged?

[peterbarker]

@peterbarker @rmackay9 can it be merged?

I've marked it for DevCall to make sure there's a deadline it gets discussed for merging. Randy may choose to merge earlier.

[peterbarker]

Merged, thanks!