Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-22 22:32:11 +08:00
Code Issues Actions 7 Packages Projects Releases Wiki Activity

Commander: Home Position: move gps checks on when to allow setting home pos inside HomePosition class

Browse Source 
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2023-02-21 16:21:05 +01:00
committed by Beat Küng
parent 352f773ec4
commit e6af8b9aa6
2 changed files with 27 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/HomePosition.cpp
+20 -5
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2022 PX4 Development Team. All rights reserved.
* Copyright (c) 2022-2023 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -37,6 +37,8 @@
#include <lib/geo/geo.h>
#include "commander_helper.h"
using namespace time_literals;
HomePosition::HomePosition(const failsafe_flags_s &failsafe_flags)
: _failsafe_flags(failsafe_flags)
{
@@ -69,7 +71,7 @@ bool HomePosition::hasMovedFromCurrentHomeLocation()
eph = gpos.eph;
epv = gpos.epv;
} else if (!_failsafe_flags.gps_position_invalid) {
} else if (_gps_position_for_home_valid) {
sensor_gps_s gps;
_vehicle_gps_position_sub.copy(&gps);
const double lat = static_cast<double>(gps.lat) * 1e-7;
@@ -114,7 +116,7 @@ bool HomePosition::setHomePosition(bool force)
setHomePosValid();
updated = true;
} else if (!_failsafe_flags.gps_position_invalid) {
} else if (_gps_position_for_home_valid) {
// Set home using GNSS position
sensor_gps_s gps_pos;
_vehicle_gps_position_sub.copy(&gps_pos);
@@ -203,7 +205,7 @@ void HomePosition::setInAirHomePosition()
home.timestamp = hrt_absolute_time();
_home_position_pub.update();
} else if (!_failsafe_flags.local_position_invalid && !_failsafe_flags.gps_position_invalid) {
} else if (!_failsafe_flags.local_position_invalid && _gps_position_for_home_valid) {
// 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();
@@ -314,6 +316,19 @@ void HomePosition::update(bool set_automatically, bool check_if_changed)
_local_position_sub.update();
_global_position_sub.update();
if (_vehicle_gps_position_sub.updated()) {
sensor_gps_s vehicle_gps_position;
_vehicle_gps_position_sub.copy(&vehicle_gps_position);
const hrt_abstime now = hrt_absolute_time();
const bool time = (now < vehicle_gps_position.timestamp + 1_s);
const bool fix = vehicle_gps_position.fix_type >= kHomePositionGPSRequiredFixType;
const bool eph = vehicle_gps_position.eph < kHomePositionGPSRequiredEPH;
const bool epv = vehicle_gps_position.epv < kHomePositionGPSRequiredEPV;
const bool evh = vehicle_gps_position.s_variance_m_s < kHomePositionGPSRequiredEVH;
_gps_position_for_home_valid = time && fix && eph && epv && evh;
}
const vehicle_local_position_s &lpos = _local_position_sub.get();
const auto &home = _home_position_pub.get();
@@ -328,7 +343,7 @@ 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 && !_failsafe_flags.local_position_invalid;
const bool can_set_home_gpos_first_time = ((!home.valid_hpos || !home.valid_alt)
&& (!_failsafe_flags.global_position_invalid || !_failsafe_flags.gps_position_invalid));
&& (!_failsafe_flags.global_position_invalid || _gps_position_for_home_valid));
const bool can_set_home_alt_first_time = (!home.valid_alt && lpos.z_global);
if (can_set_home_lpos_first_time
src/modules/commander/HomePosition.hpp
+7 -1
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2022 PX4 Development Team. All rights reserved.
* Copyright (c) 2022-2023 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -41,6 +41,11 @@
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/failsafe_flags.h>
static constexpr int kHomePositionGPSRequiredFixType = 2;
static constexpr float kHomePositionGPSRequiredEPH = 5.f;
static constexpr float kHomePositionGPSRequiredEPV = 10.f;
static constexpr float kHomePositionGPSRequiredEVH = 1.f;
class HomePosition
{
public:
@@ -75,4 +80,5 @@ private:
uint8_t _heading_reset_counter{0};
bool _valid{false};
const failsafe_flags_s &_failsafe_flags;
bool _gps_position_for_home_valid{false};
};