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.
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.
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.
| 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.
it should be state_quat as second argument
There was a problem hiding this comment.
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...