dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

commander: improve angular velocity validity and message

This commit is contained in:
Daniel Agar 2021-11-19 19:20:51 -05:00
parent f058cbd0e0
commit 577d926639
1 changed files with 14 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -3821,12 +3821,22 @@ void Commander::estimator_check()
// angular velocity
vehicle_angular_velocity_s angular_velocity{};
_vehicle_angular_velocity_sub.copy(&angular_velocity);
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]);
const bool condition_angular_velocity_time_valid = (angular_velocity.timestamp != 0)
&& (hrt_elapsed_time(&angular_velocity.timestamp) < 1_s);
const bool condition_angular_velocity_finite = PX4_ISFINITE(angular_velocity.xyz[0])
&& PX4_ISFINITE(angular_velocity.xyz[1]) && PX4_ISFINITE(angular_velocity.xyz[2]);
const bool condition_angular_velocity_valid = condition_angular_velocity_time_valid
&& condition_angular_velocity_finite;
if (_status_flags.condition_angular_velocity_valid && !condition_angular_velocity_valid) {
PX4_ERR("angular velocity no longer valid");
const char err_str[] {"angular velocity no longer valid"};
if (!condition_angular_velocity_time_valid) {
PX4_ERR("%s (timeout)", err_str);
} else if (!condition_angular_velocity_finite) {
PX4_ERR("%s (non-finite values)", err_str);
}
}
_status_flags.condition_angular_velocity_valid = condition_angular_velocity_valid;