From 6633ff5089eda29b2c5980a0532058ca175b9733 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Fri, 5 Feb 2021 12:20:46 -0500 Subject: [PATCH] commander: vehicle_status_flags add attitude and angular velocity --- msg/vehicle_status_flags.msg | 9 ++++++--- src/modules/commander/Commander.cpp | 16 ++++++++++++++++ src/modules/commander/Commander.hpp | 6 ++++-- 3 files changed, 26 insertions(+), 5 deletions(-) diff --git a/msg/vehicle_status_flags.msg b/msg/vehicle_status_flags.msg index 851bf96b81..c8d397d151 100644 --- a/msg/vehicle_status_flags.msg +++ b/msg/vehicle_status_flags.msg @@ -7,14 +7,17 @@ bool condition_system_sensors_initialized bool condition_system_hotplug_timeout # true if the hotplug sensor search is over bool condition_system_returned_to_home bool condition_auto_mission_available -bool condition_global_position_valid # set to true by the commander app if the quality of the global position estimate is good enough to use for navigation -bool condition_home_position_valid # indicates a valid home position (a valid home position is not always a valid launch) +bool condition_angular_velocity_valid +bool condition_attitude_valid +bool condition_local_altitude_valid bool condition_local_position_valid # set to true by the commander app if the quality of the local position estimate is good enough to use for navigation bool condition_local_velocity_valid # set to true by the commander app if the quality of the local horizontal velocity data is good enough to use for navigation -bool condition_local_altitude_valid +bool condition_global_position_valid # set to true by the commander app if the quality of the global position estimate is good enough to use for navigation +bool condition_home_position_valid # indicates a valid home position (a valid home position is not always a valid launch) bool condition_power_input_valid # set if input power is valid bool condition_battery_healthy # set if battery is available and not low bool condition_escs_error # set to true if one or more ESCs reporting esc_status are offline + bool circuit_breaker_engaged_power_check bool circuit_breaker_engaged_airspd_check bool circuit_breaker_engaged_enginefailure_check diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index bda82b74f4..a5c35b5c48 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -4049,6 +4049,22 @@ void Commander::estimator_check() _status_flags.condition_local_altitude_valid = lpos.z_valid && (hrt_elapsed_time(&lpos.timestamp) < (_param_com_pos_fs_delay.get() * 1_s)); + + + // attitude + vehicle_attitude_s attitude{}; + _vehicle_attitude_sub.copy(&attitude); + const matrix::Quatf q{attitude.q}; + const matrix::Quatf q_norm{q.unit()}; + _status_flags.condition_attitude_valid = (hrt_elapsed_time(&attitude.timestamp) < 1_s) + && (matrix::Quatf(q - q_norm).length() < FLT_EPSILON); + + // 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) + && PX4_ISFINITE(angular_velocity.xyz[0]) && PX4_ISFINITE(angular_velocity.xyz[1]) + && PX4_ISFINITE(angular_velocity.xyz[2]); } void Commander::UpdateEstimateValidity() diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index e171fc0199..65d2ae24f2 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -81,7 +81,8 @@ #include #include #include -#include +#include +#include #include #include #include @@ -414,7 +415,8 @@ private: uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; uORB::Subscription _manual_control_switches_sub{ORB_ID(manual_control_switches)}; uORB::Subscription _system_power_sub{ORB_ID(system_power)}; - uORB::Subscription _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)}; + uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)}; + uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; uORB::Subscription _vtol_vehicle_status_sub{ORB_ID(vtol_vehicle_status)}; uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};