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: vehicle_status_flags add attitude and angular velocity

Browse Source
This commit is contained in:
Daniel Agar 2021-02-05 12:20:46 -05:00 committed by Lorenz Meier
parent 168027ac3d
commit 6633ff5089
3 changed files with 26 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
msg/vehicle_status_flags.msg
View File

@ -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

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

@ -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()

6
src/modules/commander/Commander.hpp
View File

@ -81,7 +81,8 @@
#include <uORB/topics/safety.h>
#include <uORB/topics/system_power.h>
#include <uORB/topics/telemetry_status.h>
#include <uORB/topics/vehicle_acceleration.h>
#include <uORB/topics/vehicle_angular_velocity.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_land_detected.h>
@ -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};