fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-29 15:20:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

vehicle_local_position: rename yaw -> heading and add reset logic

Browse Source 
- vehicle_global_position yaw removed (redundant)
This commit is contained in:
Daniel Agar
2020-08-07 14:06:38 -04:00
committed by Mathieu Bresciani
parent 33fb9d0c62
commit 97fc1db768
24 changed files with 75 additions and 86 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/Commander.cpp
+12 -20
View File
@@ -1199,7 +1199,8 @@ Commander::set_home_position()
home.y = lpos.y;
home.z = lpos.z;
home.yaw = lpos.yaw;
home.yaw = lpos.heading;
_heading_reset_counter = lpos.heading_reset_counter;
home.manual_home = false;
@@ -1248,20 +1249,6 @@ Commander::updateHomePositionYaw(float yaw)
_home_pub.update(home);
}
void
Commander::checkEkfResetCounters()
{
if (_attitude_sub.get().quat_reset_counter != _quat_reset_counter) {
const float delta_psi = matrix::Eulerf(matrix::Quatf(_attitude_sub.get().delta_q_reset)).psi();
if (!_home_pub.get().manual_home) {
updateHomePositionYaw(matrix::wrap_pi(_home_pub.get().yaw + delta_psi));
}
_quat_reset_counter = _attitude_sub.get().quat_reset_counter;
}
}
void
Commander::run()
{
@@ -1629,7 +1616,7 @@ Commander::run()
}
}
estimator_check();
estimator_check(status_flags);
/* Update land detector */
if (_land_detector_sub.updated()) {
@@ -2279,8 +2266,6 @@ Commander::run()
/* Get current timestamp */
const hrt_abstime now = hrt_absolute_time();
checkEkfResetCounters();
// automatically set or update home position
if (!_home_pub.get().manual_home) {
const vehicle_local_position_s &local_position = _local_position_sub.get();
@@ -4048,18 +4033,25 @@ void Commander::battery_status_check()
}
}
void Commander::estimator_check()
void Commander::estimator_check(const vehicle_status_flags_s &vstatus_flags)
{
// Check if quality checking of position accuracy and consistency is to be performed
const bool run_quality_checks = !status_flags.circuit_breaker_engaged_posfailure_check;
_local_position_sub.update();
_global_position_sub.update();
_attitude_sub.update();
const vehicle_local_position_s &lpos = _local_position_sub.get();
const vehicle_global_position_s &gpos = _global_position_sub.get();
if (lpos.heading_reset_counter != _heading_reset_counter) {
if (vstatus_flags.condition_home_position_valid) {
updateHomePositionYaw(_home_pub.get().yaw + lpos.delta_heading);
}
_heading_reset_counter = lpos.heading_reset_counter;
}
const bool mag_fault_prev = (_estimator_status_sub.get().control_mode_flags & (1 << estimator_status_s::CS_MAG_FAULT));
if (_estimator_status_sub.update()) {