dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Commander: set home position in air 

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
msg/home_position.msg
View File

@ -14,5 +14,6 @@ float32 yaw # Yaw angle in radians
bool valid_alt # true when the altitude has been set
bool valid_hpos # true when the latitude and longitude have been set
bool valid_lpos # true when the local position (xyz) has been set
bool manual_home # true when home position was set manually

153
src/modules/commander/Commander.cpp
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;

7
src/modules/commander/Commander.hpp
View File

@ -157,6 +157,12 @@ private:
bool set_home_position();
bool set_home_position_alt_only();
bool set_in_air_home_position();
bool isGPosGoodForInitializingHomePos(const vehicle_global_position_s &gpos) const;
void fillLocalHomePos(home_position_s &home, const vehicle_local_position_s &lpos) const;
void fillGlobalHomePos(home_position_s &home, const vehicle_global_position_s &gpos) const;
void fillGlobalHomePos(home_position_s &home, double lat, double lon, float alt) const;
void setHomePosValid();
void updateHomePositionYaw(float yaw);
void update_control_mode();
@ -187,6 +193,7 @@ private:
(ParamFloat<px4::params::COM_HOME_H_T>) _param_com_home_h_t,
(ParamFloat<px4::params::COM_HOME_V_T>) _param_com_home_v_t,
(ParamBool<px4::params::COM_HOME_IN_AIR>) _param_com_home_in_air,
(ParamFloat<px4::params::COM_POS_FS_EPH>) _param_com_pos_fs_eph,
(ParamFloat<px4::params::COM_POS_FS_EPV>) _param_com_pos_fs_epv, /*Not realy used for now*/

12
src/modules/commander/commander_params.c
View File

@ -213,6 +213,18 @@ PARAM_DEFINE_FLOAT(COM_HOME_H_T, 5.0f);
*/
PARAM_DEFINE_FLOAT(COM_HOME_V_T, 10.0f);
/**
* Allows setting the home position after takeoff
*
* If set to true, the autopilot is allowed to set its home position after takeoff
* The true home position is back-computed if a local position is estimate if available.
* If no local position is available, home is set to the current position.
*
* @boolean
* @group Commander
*/
PARAM_DEFINE_INT32(COM_HOME_IN_AIR, 0);
/**
* RC control input mode
*