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

vehicle_status_flags: cleanup message, move non-failsafe flags to vehicle_status

Browse Source
This commit is contained in:
Beat Küng
2022-09-05 07:56:39 +02:00
committed by Daniel Agar
parent ae6377dfa0
commit e4bb219d10
30 changed files with 325 additions and 259 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/HomePosition.cpp
+9 -9
View File
@@ -69,7 +69,7 @@ bool HomePosition::hasMovedFromCurrentHomeLocation()
eph = gpos.eph;
epv = gpos.epv;
} else if (_vehicle_status_flags.gps_position_valid) {
} else if (!_vehicle_status_flags.gps_position_invalid) {
sensor_gps_s gps;
_vehicle_gps_position_sub.copy(&gps);
const double lat = static_cast<double>(gps.lat) * 1e-7;
@@ -98,7 +98,7 @@ bool HomePosition::setHomePosition(bool force)
bool updated = false;
home_position_s home{};
if (_vehicle_status_flags.local_position_valid) {
if (!_vehicle_status_flags.local_position_invalid) {
// Set home position in local coordinates
const vehicle_local_position_s &lpos = _local_position_sub.get();
_heading_reset_counter = lpos.heading_reset_counter; // TODO: should not be here
@@ -107,14 +107,14 @@ bool HomePosition::setHomePosition(bool force)
updated = true;
}
if (_vehicle_status_flags.global_position_valid) {
if (!_vehicle_status_flags.global_position_invalid) {
// Set home using the global position estimate (fused INS/GNSS)
const vehicle_global_position_s &gpos = _global_position_sub.get();
fillGlobalHomePos(home, gpos);
setHomePosValid();
updated = true;
} else if (_vehicle_status_flags.gps_position_valid) {
} else if (!_vehicle_status_flags.gps_position_invalid) {
// Set home using GNSS position
sensor_gps_s gps_pos;
_vehicle_gps_position_sub.copy(&gps_pos);
@@ -184,7 +184,7 @@ void HomePosition::setInAirHomePosition()
const bool local_home_valid = home.valid_lpos;
if (local_home_valid && !global_home_valid) {
if (_vehicle_status_flags.local_position_valid && _vehicle_status_flags.global_position_valid) {
if (!_vehicle_status_flags.local_position_invalid && !_vehicle_status_flags.global_position_invalid) {
// Back-compute lon, lat and alt of home position given the local home position
// and current positions in local and global (GNSS fused) frames
const vehicle_local_position_s &lpos = _local_position_sub.get();
@@ -203,7 +203,7 @@ void HomePosition::setInAirHomePosition()
home.timestamp = hrt_absolute_time();
_home_position_pub.update();
} else if (_vehicle_status_flags.local_position_valid && _vehicle_status_flags.gps_position_valid) {
} else if (!_vehicle_status_flags.local_position_invalid && !_vehicle_status_flags.gps_position_invalid) {
// Back-compute lon, lat and alt of home position given the local home position
// and current positions in local and global (GNSS raw) frames
const vehicle_local_position_s &lpos = _local_position_sub.get();
@@ -231,7 +231,7 @@ void HomePosition::setInAirHomePosition()
} else if (!local_home_valid && global_home_valid) {
const vehicle_local_position_s &lpos = _local_position_sub.get();
if (_vehicle_status_flags.local_position_valid && lpos.xy_global && lpos.z_global) {
if (!_vehicle_status_flags.local_position_invalid && lpos.xy_global && lpos.z_global) {
// Back-compute x, y and z of home position given the global home position
// and the global reference of the local frame
MapProjection ref_pos{lpos.ref_lat, lpos.ref_lon};
@@ -326,9 +326,9 @@ void HomePosition::update(bool set_automatically, bool check_if_changed)
}
if (check_if_changed && set_automatically) {
const bool can_set_home_lpos_first_time = !home.valid_lpos && _vehicle_status_flags.local_position_valid;
const bool can_set_home_lpos_first_time = !home.valid_lpos && !_vehicle_status_flags.local_position_invalid;
const bool can_set_home_gpos_first_time = ((!home.valid_hpos || !home.valid_alt)
&& (_vehicle_status_flags.global_position_valid || _vehicle_status_flags.gps_position_valid));
&& (!_vehicle_status_flags.global_position_invalid || !_vehicle_status_flags.gps_position_invalid));
const bool can_set_home_alt_first_time = (!home.valid_alt && lpos.z_global);
if (can_set_home_lpos_first_time