diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 143b83c803..9b4bbd1ab5 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -1027,9 +1027,9 @@ Commander::handle_command(const vehicle_command_s &cmd) cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; /* update home position on arming if at least 500 ms from commander start spent to avoid setting home on in-air restart */ - if ((arming_action == vehicle_command_s::ARMING_ACTION_ARM) && (arming_res == TRANSITION_CHANGED) && - (hrt_absolute_time() > (_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL)) && !_home_pub.get().manual_home) { - + if ((arming_action == vehicle_command_s::ARMING_ACTION_ARM) && (arming_res == TRANSITION_CHANGED) + && (hrt_absolute_time() > (_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL)) + && (_param_com_home_en.get() && !_home_pub.get().manual_home)) { set_home_position(); } } @@ -1798,6 +1798,52 @@ void Commander::executeActionRequest(const action_request_s &action_request) } } +bool +Commander::hasMovedFromCurrentHomeLocation() +{ + float home_dist_xy = -1.f; + float home_dist_z = -1.f; + float eph = 0.f; + float epv = 0.f; + + if (_home_pub.get().valid_lpos && _local_position_sub.get().xy_valid && _local_position_sub.get().z_valid) { + mavlink_wpm_distance_to_point_local(_home_pub.get().x, _home_pub.get().y, _home_pub.get().z, + _local_position_sub.get().x, _local_position_sub.get().y, _local_position_sub.get().z, + &home_dist_xy, &home_dist_z); + + eph = _local_position_sub.get().eph; + epv = _local_position_sub.get().epv; + + } else if (_home_pub.get().valid_hpos && _home_pub.get().valid_alt) { + if (_status_flags.global_position_valid) { + const vehicle_global_position_s &gpos = _global_position_sub.get(); + + get_distance_to_point_global_wgs84(_home_pub.get().lat, _home_pub.get().lon, _home_pub.get().alt, + gpos.lat, gpos.lon, gpos.alt, + &home_dist_xy, &home_dist_z); + + eph = gpos.eph; + epv = gpos.epv; + + } else if (_status_flags.gps_position_valid) { + vehicle_gps_position_s gps; + _vehicle_gps_position_sub.copy(&gps); + const double lat = static_cast(gps.lat) * 1e-7; + const double lon = static_cast(gps.lon) * 1e-7; + const float alt = static_cast(gps.alt) * 1e-3f; + + get_distance_to_point_global_wgs84(_home_pub.get().lat, _home_pub.get().lon, _home_pub.get().alt, + lat, lon, alt, + &home_dist_xy, &home_dist_z); + + eph = gps.eph; + epv = gps.epv; + } + } + + return (home_dist_xy > eph * 2.f) || (home_dist_z > epv * 2.f); +} + /** * @brief This function initializes the home position an altitude of the vehicle. This happens first time we get a good GPS fix and each * time the vehicle is armed with a good GPS fix. @@ -1805,83 +1851,86 @@ void Commander::executeActionRequest(const action_request_s &action_request) bool Commander::set_home_position() { - // Need global and local position fix to be able to set home - // but already set the home position in local coordinates if available - // in case the global position is only valid after takeoff - if (_param_com_home_en.get() && _status_flags.local_position_valid) { + bool updated = false; + home_position_s home{}; + if (_status_flags.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; + _heading_reset_counter = lpos.heading_reset_counter; // TODO: should not be here - home_position_s home{}; - home.timestamp = hrt_absolute_time(); - home.manual_home = false; fillLocalHomePos(home, lpos); - - if (_status_flags.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); + updated = true; } - return _status_flags.home_position_valid; + if (_status_flags.global_position_valid) { + // Set home using the global position estimate (fused INS/GNSS) + const vehicle_global_position_s &gpos = _global_position_sub.get(); + fillGlobalHomePos(home, gpos); + setHomePosValid(); + updated = true; + + } else if (_status_flags.gps_position_valid) { + // Set home using GNSS position + vehicle_gps_position_s gps_pos; + _vehicle_gps_position_sub.copy(&gps_pos); + const double lat = static_cast(gps_pos.lat) * 1e-7; + const double lon = static_cast(gps_pos.lon) * 1e-7; + const float alt = static_cast(gps_pos.alt) * 1e-3f; + fillGlobalHomePos(home, lat, lon, alt); + setHomePosValid(); + updated = true; + + } else if (_local_position_sub.get().z_global) { + // handle special case where we are setting only altitude using local position reference + // This might be overwritten by altitude from global or GNSS altitude + home.alt = _local_position_sub.get().ref_alt; + home.valid_alt = true; + + updated = true; + } + + if (updated) { + home.timestamp = hrt_absolute_time(); + home.manual_home = false; + updated = _home_pub.update(home); + } + + return updated; } -bool +void Commander::set_in_air_home_position() { - if (_param_com_home_en.get() - && _status_flags.local_position_valid + if (_status_flags.local_position_valid && _status_flags.global_position_valid) { const vehicle_global_position_s &gpos = _global_position_sub.get(); home_position_s home{}; + home = _home_pub.get(); + const vehicle_local_position_s &lpos = _local_position_sub.get(); - // Ensure that the GPS accuracy is good enough for intializing home - if (isGPosGoodForInitializingHomePos(gpos)) { - home = _home_pub.get(); - home.timestamp = hrt_absolute_time(); - const vehicle_local_position_s &lpos = _local_position_sub.get(); + if (home.valid_lpos) { + // Back-compute lon, lat and alt of home position given the home + // and current positions in local frame + MapProjection ref_pos{gpos.lat, gpos.lon}; + double home_lat; + double home_lon; + ref_pos.reproject(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); - if (_home_pub.get().valid_lpos) { - // Back-compute lon, lat and alt of home position given the home - // and current positions in local frame - MapProjection ref_pos{gpos.lat, gpos.lon}; - double home_lat; - double home_lon; - ref_pos.reproject(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); - - } else { - // Home position in local frame is unknowm, set - // home as current position - fillLocalHomePos(home, lpos); - fillGlobalHomePos(home, gpos); - } - - setHomePosValid(); - _home_pub.update(home); + } else { + // Home position in local frame is unknowm, set + // home as current position + fillLocalHomePos(home, lpos); + fillGlobalHomePos(home, gpos); } + + setHomePosValid(); + home.timestamp = hrt_absolute_time(); + _home_pub.update(home); } - - return _status_flags.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 @@ -1926,25 +1975,6 @@ void Commander::setHomePosValid() _status_flags.home_position_valid = true; } -bool -Commander::set_home_position_alt_only() -{ - const vehicle_local_position_s &lpos = _local_position_sub.get(); - - if (_param_com_home_en.get() && !_home_pub.get().valid_alt && lpos.z_global) { - // handle special case where we are setting only altitude using local position reference - home_position_s home{}; - home.alt = lpos.ref_alt; - home.valid_alt = true; - - home.timestamp = hrt_absolute_time(); - - return _home_pub.update(home); - } - - return false; -} - void Commander::updateHomePositionYaw(float yaw) { @@ -2191,13 +2221,12 @@ Commander::run() if (_param_com_home_en.get() && !_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 && !_vehicle_land_detected.landed && - (hrt_elapsed_time(&_boot_timestamp) > INAIR_RESTART_HOLDOFF_INTERVAL)) { + if (!_vehicle_land_detected.landed && (hrt_elapsed_time(&_boot_timestamp) > INAIR_RESTART_HOLDOFF_INTERVAL)) { if (was_landed) { - _should_set_home_on_takeoff = !set_home_position(); + set_home_position(); - } else if (_param_com_home_in_air.get()) { - _should_set_home_on_takeoff = !set_in_air_home_position(); + } else if (!_status_flags.home_position_valid && _param_com_home_in_air.get()) { + set_in_air_home_position(); } } } @@ -2839,35 +2868,17 @@ Commander::run() // automatically set or update home position if (_param_com_home_en.get() && !_home_pub.get().manual_home) { - const vehicle_local_position_s &local_position = _local_position_sub.get(); + if (!_armed.armed && _vehicle_land_detected.landed) { + const bool can_set_home_lpos_first_time = (!_home_pub.get().valid_lpos && _status_flags.local_position_valid); + const bool can_set_home_gpos_first_time = ((!_home_pub.get().valid_hpos || !_home_pub.get().valid_alt) + && (_status_flags.global_position_valid || _status_flags.gps_position_valid)); + const bool can_set_home_alt_first_time = (!_home_pub.get().valid_alt && _local_position_sub.get().z_global); - if (!_armed.armed) { - if (_home_pub.get().valid_lpos) { - if (_vehicle_land_detected.landed && local_position.xy_valid && local_position.z_valid) { - /* distance from home */ - float home_dist_xy = -1.0f; - float home_dist_z = -1.0f; - mavlink_wpm_distance_to_point_local(_home_pub.get().x, _home_pub.get().y, _home_pub.get().z, - local_position.x, local_position.y, local_position.z, - &home_dist_xy, &home_dist_z); - - if ((home_dist_xy > local_position.eph * 2.0f) || (home_dist_z > local_position.epv * 2.0f)) { - - /* update when disarmed, landed and moved away from current home position */ - set_home_position(); - } - } - - } else { - /* First time home position update - but only if disarmed */ + if (can_set_home_lpos_first_time + || can_set_home_gpos_first_time + || can_set_home_alt_first_time + || hasMovedFromCurrentHomeLocation()) { set_home_position(); - - /* Set home position altitude to EKF origin height if home is not set and the EKF has a global origin. - * This allows home altitude to be used in the calculation of height above takeoff location when GPS - * use has commenced after takeoff. */ - if (!_status_flags.home_position_valid) { - set_home_position_alt_only(); - } } } } @@ -2887,8 +2898,6 @@ Commander::run() _param_flight_uuid.commit_no_notification(); _last_disarmed_timestamp = hrt_absolute_time(); - - _should_set_home_on_takeoff = true; } } diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index a71ac6ba70..2f24a5e2cc 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -163,10 +163,10 @@ private: void print_reject_mode(uint8_t main_state); + bool hasMovedFromCurrentHomeLocation(); 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 set_in_air_home_position(); void fillLocalHomePos(home_position_s &home, const vehicle_local_position_s &lpos) const; void fillLocalHomePos(home_position_s &home, float x, float y, float z, float heading) const; void fillGlobalHomePos(home_position_s &home, const vehicle_global_position_s &gpos) const; @@ -199,8 +199,6 @@ private: (ParamInt) _param_com_rcl_except, (ParamBool) _param_com_home_en, - (ParamFloat) _param_com_home_h_t, - (ParamFloat) _param_com_home_v_t, (ParamBool) _param_com_home_in_air, (ParamFloat) _param_com_pos_fs_eph, @@ -386,7 +384,6 @@ private: bool _was_armed{false}; bool _failsafe_old{false}; ///< check which state machines for changes, clear "changed" flag bool _have_taken_off_since_arming{false}; - bool _should_set_home_on_takeoff{true}; bool _system_power_usb_connected{false}; geofence_result_s _geofence_result{}; diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 17e2ba0254..55e5ae48de 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -213,34 +213,6 @@ PARAM_DEFINE_FLOAT(COM_RCL_ACT_T, 15.0f); */ PARAM_DEFINE_INT32(COM_HOME_EN, 1); -/** - * Home set horizontal threshold - * - * The home position will be set if the estimated positioning accuracy is below the threshold. - * - * @group Commander - * @unit m - * @min 2 - * @max 15 - * @decimal 2 - * @increment 0.5 - */ -PARAM_DEFINE_FLOAT(COM_HOME_H_T, 5.0f); - -/** - * Home set vertical threshold - * - * The home position will be set if the estimated positioning accuracy is below the threshold. - * - * @group Commander - * @unit m - * @min 5 - * @max 25 - * @decimal 2 - * @increment 0.5 - */ -PARAM_DEFINE_FLOAT(COM_HOME_V_T, 10.0f); - /** * Allows setting the home position after takeoff *