forked from Archive/PX4-Autopilot
Commander: set home position in air
en/dis-able in-air home position via parameter COM_HOME_IN_AIR
This commit is contained in:
parent
e8cefcafbc
commit
f3e5b86b06
|
@ -14,5 +14,6 @@ float32 yaw # Yaw angle in radians
|
||||||
|
|
||||||
bool valid_alt # true when the altitude has been set
|
bool valid_alt # true when the altitude has been set
|
||||||
bool valid_hpos # true when the latitude and longitude have 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
|
bool manual_home # true when home position was set manually
|
||||||
|
|
|
@ -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_init(&ref_pos, local_pos.ref_lat, local_pos.ref_lon);
|
||||||
map_projection_project(&ref_pos, lat, lon, &home.x, &home.y);
|
map_projection_project(&ref_pos, lat, lon, &home.x, &home.y);
|
||||||
home.z = -(alt - local_pos.ref_alt);
|
home.z = -(alt - local_pos.ref_alt);
|
||||||
|
home.valid_lpos = true;
|
||||||
|
|
||||||
/* mark home position as set */
|
/* mark home position as set */
|
||||||
_status_flags.condition_home_position_valid = _home_pub.update(home);
|
_status_flags.condition_home_position_valid = _home_pub.update(home);
|
||||||
|
@ -1316,49 +1317,118 @@ bool
|
||||||
Commander::set_home_position()
|
Commander::set_home_position()
|
||||||
{
|
{
|
||||||
// Need global and local position fix to be able to set home
|
// 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();
|
const vehicle_global_position_s &gpos = _global_position_sub.get();
|
||||||
|
|
||||||
// Ensure that the GPS accuracy is good enough for intializing 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)) {
|
||||||
|
fillGlobalHomePos(home, gpos);
|
||||||
|
setHomePosValid();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
const vehicle_local_position_s &lpos = _local_position_sub.get();
|
_home_pub.update(home);
|
||||||
|
}
|
||||||
|
|
||||||
// Set home position
|
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{};
|
home_position_s home{};
|
||||||
|
|
||||||
|
// Ensure that the GPS accuracy is good enough for intializing home
|
||||||
|
if (isGPosGoodForInitializingHomePos(gpos)) {
|
||||||
|
home = _home_pub.get();
|
||||||
home.timestamp = hrt_absolute_time();
|
home.timestamp = hrt_absolute_time();
|
||||||
|
const vehicle_local_position_s &lpos = _local_position_sub.get();
|
||||||
|
|
||||||
home.lat = gpos.lat;
|
if (_home_pub.get().valid_lpos) {
|
||||||
home.lon = gpos.lon;
|
// Back-compute lon, lat and alt of home position given the home
|
||||||
home.valid_hpos = true;
|
// 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.alt = gpos.alt;
|
} else {
|
||||||
home.valid_alt = true;
|
// Home position in local frame is unknowm, set
|
||||||
|
// home as current position
|
||||||
|
fillLocalHomePos(home, lpos);
|
||||||
|
fillGlobalHomePos(home, gpos);
|
||||||
|
}
|
||||||
|
|
||||||
|
setHomePosValid();
|
||||||
|
_home_pub.update(home);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
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.x = lpos.x;
|
||||||
home.y = lpos.y;
|
home.y = lpos.y;
|
||||||
home.z = lpos.z;
|
home.z = lpos.z;
|
||||||
|
home.valid_lpos = true;
|
||||||
|
|
||||||
home.yaw = lpos.heading;
|
home.yaw = lpos.heading;
|
||||||
_heading_reset_counter = lpos.heading_reset_counter;
|
}
|
||||||
|
|
||||||
home.manual_home = false;
|
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
|
// play tune first time we initialize HOME
|
||||||
if (!_status_flags.condition_home_position_valid) {
|
if (!_status_flags.condition_home_position_valid) {
|
||||||
tune_home_set(true);
|
tune_home_set(true);
|
||||||
}
|
}
|
||||||
|
|
||||||
// mark home position as set
|
// mark home position as set
|
||||||
_status_flags.condition_home_position_valid = _home_pub.update(home);
|
_status_flags.condition_home_position_valid = true;
|
||||||
|
|
||||||
return _status_flags.condition_home_position_valid;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
return false;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
bool
|
bool
|
||||||
|
@ -1655,14 +1725,19 @@ Commander::run()
|
||||||
_gpos_probation_time_us = _param_com_pos_fs_prob.get() * 1_s;
|
_gpos_probation_time_us = _param_com_pos_fs_prob.get() * 1_s;
|
||||||
_lpos_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;
|
_lvel_probation_time_us = _param_com_pos_fs_prob.get() * 1_s;
|
||||||
|
}
|
||||||
|
|
||||||
// automatically set or update home position
|
// automatically set or update home position
|
||||||
if (!_home_pub.get().manual_home) {
|
if (!_home_pub.get().manual_home) {
|
||||||
// set the home position when taking off, but only if we were previously disarmed
|
// 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
|
// 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)) {
|
if (_should_set_home_on_takeoff && !_land_detector.landed &&
|
||||||
_should_set_home_on_takeoff = false;
|
(hrt_elapsed_time(&_boot_timestamp) > INAIR_RESTART_HOLDOFF_INTERVAL)) {
|
||||||
set_home_position();
|
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();
|
const vehicle_local_position_s &local_position = _local_position_sub.get();
|
||||||
|
|
||||||
if (!_armed.armed) {
|
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) {
|
if (_land_detector.landed && local_position.xy_valid && local_position.z_valid) {
|
||||||
/* distance from home */
|
/* distance from home */
|
||||||
float home_dist_xy = -1.0f;
|
float home_dist_xy = -1.0f;
|
||||||
|
|
|
@ -157,6 +157,12 @@ private:
|
||||||
|
|
||||||
bool set_home_position();
|
bool set_home_position();
|
||||||
bool set_home_position_alt_only();
|
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 updateHomePositionYaw(float yaw);
|
||||||
|
|
||||||
void update_control_mode();
|
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_H_T>) _param_com_home_h_t,
|
||||||
(ParamFloat<px4::params::COM_HOME_V_T>) _param_com_home_v_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_EPH>) _param_com_pos_fs_eph,
|
||||||
(ParamFloat<px4::params::COM_POS_FS_EPV>) _param_com_pos_fs_epv, /*Not realy used for now*/
|
(ParamFloat<px4::params::COM_POS_FS_EPV>) _param_com_pos_fs_epv, /*Not realy used for now*/
|
||||||
|
|
|
@ -213,6 +213,18 @@ PARAM_DEFINE_FLOAT(COM_HOME_H_T, 5.0f);
|
||||||
*/
|
*/
|
||||||
PARAM_DEFINE_FLOAT(COM_HOME_V_T, 10.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
|
* RC control input mode
|
||||||
*
|
*
|
||||||
|
|
Loading…
Reference in New Issue