diff --git a/msg/home_position.msg b/msg/home_position.msg index 4106aba538..15bb16af09 100644 --- a/msg/home_position.msg +++ b/msg/home_position.msg @@ -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 diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 56238472c2..ef8752da89 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -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(); + } else { + // Home position in local frame is unknowm, set + // home as current position + fillLocalHomePos(home, lpos); + fillGlobalHomePos(home, gpos); + } - home.lat = gpos.lat; - home.lon = gpos.lon; - home.valid_hpos = true; + setHomePosValid(); + _home_pub.update(home); + } + } - home.alt = gpos.alt; - home.valid_alt = true; + return _status_flags.condition_home_position_valid; +} - home.x = lpos.x; - home.y = lpos.y; - home.z = lpos.z; +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()); +} - home.yaw = lpos.heading; - _heading_reset_counter = lpos.heading_reset_counter; +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.manual_home = false; + home.yaw = lpos.heading; +} - // play tune first time we initialize HOME - if (!_status_flags.condition_home_position_valid) { - tune_home_set(true); - } +void Commander::fillGlobalHomePos(home_position_s &home, const vehicle_global_position_s &gpos) const +{ + fillGlobalHomePos(home, gpos.lat, gpos.lon, gpos.alt); +} - // mark home position as set - _status_flags.condition_home_position_valid = _home_pub.update(home); +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; +} - return _status_flags.condition_home_position_valid; - } +void Commander::setHomePosValid() +{ + // play tune first time we initialize HOME + if (!_status_flags.condition_home_position_valid) { + tune_home_set(true); } - return false; + // 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; diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index 9afdf19827..be2ec95465 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -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) _param_com_home_h_t, (ParamFloat) _param_com_home_v_t, + (ParamBool) _param_com_home_in_air, (ParamFloat) _param_com_pos_fs_eph, (ParamFloat) _param_com_pos_fs_epv, /*Not realy used for now*/ diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 570c7de98e..7c7a42eb81 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -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 *