fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

commander: PX4_ERR if attitude or angular velocity become invalid

Browse Source
This commit is contained in:
Daniel Agar 2021-10-13 19:31:26 -04:00
parent 4559230de6
commit d35cf78e4a
1 changed files with 16 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
src/modules/commander/Commander.cpp
View File

@ -3958,16 +3958,28 @@ void Commander::estimator_check()
&& (fabsf(q(3)) <= 1.f);
const bool norm_in_tolerance = (fabsf(1.f - q.norm()) <= FLT_EPSILON);
_status_flags.condition_attitude_valid = (hrt_elapsed_time(&attitude.timestamp) < 1_s)
&& norm_in_tolerance
&& no_element_larger_than_one;
const bool condition_attitude_valid = (hrt_elapsed_time(&attitude.timestamp) < 1_s)
&& norm_in_tolerance && no_element_larger_than_one;
if (_status_flags.condition_attitude_valid && !condition_attitude_valid) {
PX4_ERR("attitude estimate no longer valid");
}
_status_flags.condition_attitude_valid = condition_attitude_valid;
// angular velocity
vehicle_angular_velocity_s angular_velocity{};
_vehicle_angular_velocity_sub.copy(&angular_velocity);
_status_flags.condition_angular_velocity_valid = (hrt_elapsed_time(&angular_velocity.timestamp) < 1_s)
const bool condition_angular_velocity_valid = (hrt_elapsed_time(&angular_velocity.timestamp) < 1_s)
&& PX4_ISFINITE(angular_velocity.xyz[0]) && PX4_ISFINITE(angular_velocity.xyz[1])
&& PX4_ISFINITE(angular_velocity.xyz[2]);
if (_status_flags.condition_angular_velocity_valid && !condition_angular_velocity_valid) {
PX4_ERR("angular velocity no longer valid");
}
_status_flags.condition_angular_velocity_valid = condition_angular_velocity_valid;
}
void Commander::UpdateEstimateValidity()