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

Commander: set home position in air

Browse Source 
en/dis-able in-air home position via parameter COM_HOME_IN_AIR
This commit is contained in:
bresch
2020-10-19 11:00:31 +02:00
committed by Daniel Agar
parent e8cefcafbc
commit f3e5b86b06
4 changed files with 134 additions and 39 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/Commander.cpp
+114 -39
View File
@@ -821,6 +821,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
map_projection_init(&ref_pos, local_pos.ref_lat, local_pos.ref_lon);
map_projection_project(&ref_pos, lat, lon, &home.x, &home.y);
home.z = -(alt - local_pos.ref_alt);
home.valid_lpos = true;
/* mark home position as set */
_status_flags.condition_home_position_valid = _home_pub.update(home);
@@ -1316,49 +1317,118 @@ bool
Commander::set_home_position()
{
// Need global and local position fix to be able to set home
if (_status_flags.condition_global_position_valid && _status_flags.condition_local_position_valid) {
// but already set the home position in local coordinates if available
// in case the global position is only valid after takeoff
if (_status_flags.condition_local_position_valid) {
// Set home position in local coordinates
const vehicle_local_position_s &lpos = _local_position_sub.get();
_heading_reset_counter = lpos.heading_reset_counter;
home_position_s home{};
home.timestamp = hrt_absolute_time();
home.manual_home = false;
fillLocalHomePos(home, lpos);
if (_status_flags.condition_global_position_valid) {
const vehicle_global_position_s &gpos = _global_position_sub.get();
// Ensure that the GPS accuracy is good enough for intializing home
if (isGPosGoodForInitializingHomePos(gpos)) {
fillGlobalHomePos(home, gpos);
setHomePosValid();
}
}
_home_pub.update(home);
}
return _status_flags.condition_home_position_valid;
}
bool
Commander::set_in_air_home_position()
{
if (_status_flags.condition_local_position_valid
&& _status_flags.condition_global_position_valid) {
const vehicle_global_position_s &gpos = _global_position_sub.get();
home_position_s home{};
// Ensure that the GPS accuracy is good enough for intializing home
if ((gpos.eph <= _param_com_home_h_t.get()) && (gpos.epv <= _param_com_home_v_t.get())) {
if (isGPosGoodForInitializingHomePos(gpos)) {
home = _home_pub.get();
home.timestamp = hrt_absolute_time();
const vehicle_local_position_s &lpos = _local_position_sub.get();
// Set home position
home_position_s home{};
if (_home_pub.get().valid_lpos) {
// Back-compute lon, lat and alt of home position given the home
// and current positions in local frame
map_projection_reference_s ref_pos;
double home_lat;
double home_lon;
map_projection_init(&ref_pos, gpos.lat, gpos.lon);
map_projection_reproject(&ref_pos, home.x - lpos.x, home.y - lpos.y, &home_lat, &home_lon);
const float home_alt = gpos.alt + home.z;
fillGlobalHomePos(home, home_lat, home_lon, home_alt);
home.timestamp = hrt_absolute_time();
home.lat = gpos.lat;
home.lon = gpos.lon;
home.valid_hpos = true;
home.alt = gpos.alt;
home.valid_alt = true;
home.x = lpos.x;
home.y = lpos.y;
home.z = lpos.z;
home.yaw = lpos.heading;
_heading_reset_counter = lpos.heading_reset_counter;
home.manual_home = false;
// play tune first time we initialize HOME
if (!_status_flags.condition_home_position_valid) {
tune_home_set(true);
} else {
// Home position in local frame is unknowm, set
// home as current position
fillLocalHomePos(home, lpos);
fillGlobalHomePos(home, gpos);
}
// mark home position as set
_status_flags.condition_home_position_valid = _home_pub.update(home);
return _status_flags.condition_home_position_valid;
setHomePosValid();
_home_pub.update(home);
}
}
return false;
return _status_flags.condition_home_position_valid;
}
bool
Commander::isGPosGoodForInitializingHomePos(const vehicle_global_position_s &gpos) const
{
return (gpos.eph <= _param_com_home_h_t.get())
&& (gpos.epv <= _param_com_home_v_t.get());
}
void
Commander::fillLocalHomePos(home_position_s &home, const vehicle_local_position_s &lpos) const
{
home.x = lpos.x;
home.y = lpos.y;
home.z = lpos.z;
home.valid_lpos = true;
home.yaw = lpos.heading;
}
void Commander::fillGlobalHomePos(home_position_s &home, const vehicle_global_position_s &gpos) const
{
fillGlobalHomePos(home, gpos.lat, gpos.lon, gpos.alt);
}
void Commander::fillGlobalHomePos(home_position_s &home, double lat, double lon, float alt) const
{
home.lat = lat;
home.lon = lon;
home.valid_hpos = true;
home.alt = alt;
home.valid_alt = true;
}
void Commander::setHomePosValid()
{
// play tune first time we initialize HOME
if (!_status_flags.condition_home_position_valid) {
tune_home_set(true);
}
// mark home position as set
_status_flags.condition_home_position_valid = true;
}
bool
@@ -1655,14 +1725,19 @@ Commander::run()
_gpos_probation_time_us = _param_com_pos_fs_prob.get() * 1_s;
_lpos_probation_time_us = _param_com_pos_fs_prob.get() * 1_s;
_lvel_probation_time_us = _param_com_pos_fs_prob.get() * 1_s;
}
// automatically set or update home position
if (!_home_pub.get().manual_home) {
// set the home position when taking off, but only if we were previously disarmed
// and at least 500 ms from commander start spent to avoid setting home on in-air restart
if (_should_set_home_on_takeoff && (hrt_elapsed_time(&_boot_timestamp) > INAIR_RESTART_HOLDOFF_INTERVAL)) {
_should_set_home_on_takeoff = false;
set_home_position();
// automatically set or update home position
if (!_home_pub.get().manual_home) {
// set the home position when taking off, but only if we were previously disarmed
// and at least 500 ms from commander start spent to avoid setting home on in-air restart
if (_should_set_home_on_takeoff && !_land_detector.landed &&
(hrt_elapsed_time(&_boot_timestamp) > INAIR_RESTART_HOLDOFF_INTERVAL)) {
if (was_landed) {
_should_set_home_on_takeoff = !set_home_position();
} else if (_param_com_home_in_air.get()) {
_should_set_home_on_takeoff = !set_in_air_home_position();
}
}
}
@@ -2373,7 +2448,7 @@ Commander::run()
const vehicle_local_position_s &local_position = _local_position_sub.get();
if (!_armed.armed) {
if (_status_flags.condition_home_position_valid) {
if (_home_pub.get().valid_lpos) {
if (_land_detector.landed && local_position.xy_valid && local_position.z_valid) {
/* distance from home */
float home_dist_xy = -1.0f;