-
Notifications
You must be signed in to change notification settings - Fork 1.1k
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
fixed ref reset on mode switch heading issue #2536
fixed ref reset on mode switch heading issue #2536
Conversation
I guess we should fix this for the other controllers as well by the way. |
I guess you might want to properly set the ref when entering a mode that is using it. But I this might generate discontinuities in some cases. |
That would probably be the best thing to do. |
|
implementation Still needs to be tested.
@@ -59,7 +59,7 @@ struct AttRefQuatFloat { | |||
|
|||
|
|||
extern void attitude_ref_quat_float_init(struct AttRefQuatFloat *ref); | |||
extern void attitude_ref_quat_float_enter(struct AttRefQuatFloat *ref, float psi); | |||
extern void attitude_ref_quat_float_enter(struct AttRefQuatFloat *ref, struct FloatEulers *state_euler); |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Why passing an euler format here, rather than a quat ?
With the current implementation, it means that you'll still have to make the conversion at some point, but if we replace it by a ref really based on quaternions, this would make a transformation that is not needed.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Ah I figured that since it is there in the struct, it would be best to compute both, but it is also already computed with every update. Does it still make sense to keep the euler angles in the reference struct though?
Just setting the ref attiude to the current one and the ref rate and acceleration to zero should be pretty robust.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Did you get a chance to test it in real flight ?
attitude_ref_quat_float_enter(&att_ref_quat_f, stab_att_sp_euler.psi); | ||
struct FloatQuat *state_quat = stateGetNedToBodyQuat_f(); | ||
|
||
attitude_ref_quat_float_enter(&att_ref_quat_f, state_euler); |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
it should be state_quat
as second argument
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Ah of course, no as you can see not tested yet 😅
I can test everything in simulation, will do that right away.
Tested and works |
I think this is ready to be merged, or are there any other points? |
I wanted to test it, but couldn't find the time :( |
Upon some mode switches, the reference attitude is reset to the current attitude. A quaternion is made from the current attitude in euler angles, but for this a 'pitch-corrected' heading is used (a heading that points in the same direction if you pass 90 deg pitch. This is a problem, because if interpreted as a normal ZYX euler angle set the reference attitude is incorrectly set. The result is a 180 yaw movement when switching between FWD and ATT with pitch < -90 deg. The real unedited ZYX euler angles should be used instead.
Actually, looking at it again, should the ref psi be reset at all? Ref phi and theta are also not reset...