yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 01:12:31 +00: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
Download Patch File Download Diff File Expand all files Collapse all files

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};