feat: add ATTITUDE_QUATERNION_COV message#33751
Conversation
peterbarker
left a comment
peterbarker
left a comment
There was a problem hiding this comment.
This must be no-compiler-output-change when not compiled in. Which it should not be by default.
Needs an autotest. With non-zero covariances being output (EK3_GYRO_P_NSE maybe?)
CI must pass.
| bool airspeed_TAS_vec_ok; | ||
| Quaternion quat; | ||
| bool quat_ok; | ||
| Matrix3f attitude_covariance; |
There was a problem hiding this comment.
Must be guarded with AP_AHRS_ATTITUDE_COVARIANCE_ENABLED
| bool get_attitude_covariance(Matrix3f &cov) const { | ||
| if (!attitude_covariance_valid) { | ||
| return false; | ||
| } | ||
| cov = attitude_covariance; | ||
| return true; | ||
| } |
There was a problem hiding this comment.
Remove this. The accessor pattern has not proven to be useful yet.
| ret = outputDataNew.quat.tofloat(); | ||
| } | ||
|
|
||
| bool NavEKF3_core::getEulerCovariance(Matrix3f &cov) const |
There was a problem hiding this comment.
Is this really the most compact way this can be done?!
| #if AP_MAVLINK_UTM_GLOBAL_POSITION_SENDING_ENABLED | ||
| MSG_UTM_GLOBAL_POSITION = 101, | ||
| #endif // AP_MAVLINK_UTM_GLOBAL_POSITION_SENDING_ENABLED | ||
| #if AP_AHRS_ENABLED |
There was a problem hiding this comment.
| #if AP_AHRS_ENABLED | |
| #if AP_MAVLINK_ATTITUDE_QUATERNION_COV_SENDING_ENABLED |
|
|
||
| void GCS_MAVLINK::send_attitude_quaternion_cov() const | ||
| { | ||
| #if AP_AHRS_ENABLED |
There was a problem hiding this comment.
Remove the entire function, not just the body. And use the new define.
| EKF3.getQuaternion(results.quaternion); | ||
| results.quaternion.rotate(-AP::ahrs().get_trim()); | ||
|
|
||
| results.attitude_covariance_valid = EKF3.getEulerCovariance(results.attitude_covariance); |
There was a problem hiding this comment.
get_results must be fast - you can see everything else in here just copies from one place to another.
You're also unconditionally doing all of this maths at the main loop rate regardless of whether it will be used. We don't have a good mechanism for preventing that at the moment. Most people won't have this message compiled inso it won't be aproblem for them, but if you are using this message we'll compute it at (e.g.) 400Hz and send it at a rather lower rate than that, even to a companion computer.
| EKF3.getQuaternion(results.quaternion); | ||
| results.quaternion.rotate(-AP::ahrs().get_trim()); | ||
|
|
||
| results.attitude_covariance_valid = EKF3.getEulerCovariance(results.attitude_covariance); |
There was a problem hiding this comment.
There is a frame problem on your covariance. It's still in "sensor body frame" ("autopilot body frame"). The attitude being returned is in "fuselage body frame" ("vehicle body frame").
The difference is the trims.
There was a problem hiding this comment.
There's also a timing issue with the quaternion.
The attitude we send is from the output predictor, the P matrix (by definition) is at the fusion time horizon. This is probably OK, but should at least be commented.
| } | ||
| } | ||
| } else { | ||
| covariance[0] = nanf(""); |
There was a problem hiding this comment.
| covariance[0] = nanf(""); | |
| covariance[0] = NaNf; |
| } | ||
| } | ||
| } else { | ||
| covariance[0] = nanf(""); |
There was a problem hiding this comment.
| covariance[0] = nanf(""); | |
| covariance.zero(); | |
| covariance[0] = nanf(""); |
(uninitialised data being sent on wire)
|
Hey @peterbarker sorry I didn't mean for this to be ready for review! Just something I'm playing with. I'm happy to close until it's in a good spot |
It's absolutely not terrible :-) |
2 participants
Summary
Enables sending of the
ATTITUDE_QUATERNION_COVmavlink message (docs)Classification & Testing (check all that apply and add your own)
Description