diff --git a/src/modules/commander/CMakeLists.txt b/src/modules/commander/CMakeLists.txt index b59ec05fed..b3d634ac1e 100644 --- a/src/modules/commander/CMakeLists.txt +++ b/src/modules/commander/CMakeLists.txt @@ -50,6 +50,7 @@ px4_add_module( esc_calibration.cpp factory_calibration_storage.cpp gyro_calibration.cpp + HomePosition.cpp level_calibration.cpp lm_fit.cpp mag_calibration.cpp diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index f4df954e2a..8e7c0658b3 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -998,8 +998,8 @@ Commander::handle_command(const vehicle_command_s &cmd) /* 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)) - && (_param_com_home_en.get() && !_home_position_pub.get().manual_home)) { - set_home_position(); + && (_param_com_home_en.get())) { + _home_position.setHomePosition(); } } } @@ -1044,7 +1044,7 @@ Commander::handle_command(const vehicle_command_s &cmd) if (use_current) { /* use current position */ - if (set_home_position()) { + if (_home_position.setHomePosition(true)) { cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED; } else { @@ -1059,27 +1059,8 @@ Commander::handle_command(const vehicle_command_s &cmd) const float alt = cmd.param7; if (PX4_ISFINITE(lat) && PX4_ISFINITE(lon) && PX4_ISFINITE(alt)) { - const vehicle_local_position_s &local_pos = _local_position_sub.get(); - if (local_pos.xy_global && local_pos.z_global) { - /* use specified position */ - home_position_s home{}; - home.timestamp = hrt_absolute_time(); - - fillGlobalHomePos(home, lat, lon, alt); - - home.manual_home = true; - - // update local projection reference including altitude - MapProjection ref_pos{local_pos.ref_lat, local_pos.ref_lon}; - float home_x; - float home_y; - ref_pos.project(lat, lon, home_x, home_y); - const float home_z = -(alt - local_pos.ref_alt); - fillLocalHomePos(home, home_x, home_y, home_z, yaw); - - /* mark home position as set */ - _vehicle_status_flags.home_position_valid = _home_position_pub.update(home); + if (_home_position.setManually(lat, lon, alt, yaw)) { cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED; @@ -1779,244 +1760,6 @@ 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_position_pub.get().valid_lpos && _local_position_sub.get().xy_valid && _local_position_sub.get().z_valid) { - mavlink_wpm_distance_to_point_local(_home_position_pub.get().x, _home_position_pub.get().y, _home_position_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_position_pub.get().valid_hpos && _home_position_pub.get().valid_alt) { - if (_vehicle_status_flags.global_position_valid) { - const vehicle_global_position_s &gpos = _global_position_sub.get(); - - get_distance_to_point_global_wgs84(_home_position_pub.get().lat, _home_position_pub.get().lon, - _home_position_pub.get().alt, - gpos.lat, gpos.lon, gpos.alt, - &home_dist_xy, &home_dist_z); - - eph = gpos.eph; - epv = gpos.epv; - - } else if (_vehicle_status_flags.gps_position_valid) { - sensor_gps_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_position_pub.get().lat, _home_position_pub.get().lon, - _home_position_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. -**/ -bool -Commander::set_home_position() -{ - bool updated = false; - home_position_s home{}; - - if (_vehicle_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; // TODO: should not be here - - fillLocalHomePos(home, lpos); - updated = true; - } - - if (_vehicle_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 (_vehicle_status_flags.gps_position_valid) { - // Set home using GNSS position - sensor_gps_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_position_pub.update(home); - } - - return updated; -} - -void -Commander::set_in_air_home_position() -{ - home_position_s home{}; - home = _home_position_pub.get(); - const bool global_home_valid = home.valid_hpos && home.valid_alt; - const bool local_home_valid = home.valid_lpos; - - if (local_home_valid && !global_home_valid) { - if (_vehicle_status_flags.local_position_valid && _vehicle_status_flags.global_position_valid) { - // Back-compute lon, lat and alt of home position given the local home position - // and current positions in local and global (GNSS fused) frames - const vehicle_local_position_s &lpos = _local_position_sub.get(); - const vehicle_global_position_s &gpos = _global_position_sub.get(); - - 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); - - setHomePosValid(); - home.timestamp = hrt_absolute_time(); - _home_position_pub.update(home); - - } else if (_vehicle_status_flags.local_position_valid && _vehicle_status_flags.gps_position_valid) { - // Back-compute lon, lat and alt of home position given the local home position - // and current positions in local and global (GNSS raw) frames - const vehicle_local_position_s &lpos = _local_position_sub.get(); - sensor_gps_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; - - MapProjection ref_pos{lat, 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 = alt + home.z; - fillGlobalHomePos(home, home_lat, home_lon, home_alt); - - setHomePosValid(); - home.timestamp = hrt_absolute_time(); - _home_position_pub.update(home); - } - - } else if (!local_home_valid && global_home_valid) { - const vehicle_local_position_s &lpos = _local_position_sub.get(); - - if (_vehicle_status_flags.local_position_valid && lpos.xy_global && lpos.z_global) { - // Back-compute x, y and z of home position given the global home position - // and the global reference of the local frame - MapProjection ref_pos{lpos.ref_lat, lpos.ref_lon}; - - float home_x; - float home_y; - ref_pos.project(home.lat, home.lon, home_x, home_y); - - const float home_z = -(home.alt - lpos.ref_alt); - fillLocalHomePos(home, home_x, home_y, home_z, NAN); - - home.timestamp = hrt_absolute_time(); - _home_position_pub.update(home); - } - - } else if (!local_home_valid && !global_home_valid) { - // Home position is not known in any frame, set home at current position - set_home_position(); - - } else { - // nothing to do - } -} - -void -Commander::fillLocalHomePos(home_position_s &home, const vehicle_local_position_s &lpos) const -{ - fillLocalHomePos(home, lpos.x, lpos.y, lpos.z, lpos.heading); -} - -void -Commander::fillLocalHomePos(home_position_s &home, float x, float y, float z, float heading) const -{ - home.x = x; - home.y = y; - home.z = z; - home.valid_lpos = true; - - home.yaw = 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 (!_vehicle_status_flags.home_position_valid) { - tune_home_set(true); - } - - // mark home position as set - _vehicle_status_flags.home_position_valid = true; -} - -void -Commander::updateHomePositionYaw(float yaw) -{ - if (_param_com_home_en.get()) { - home_position_s home = _home_position_pub.get(); - - home.yaw = yaw; - home.timestamp = hrt_absolute_time(); - - _home_position_pub.update(home); - } -} void Commander::updateParameters() { @@ -2189,17 +1932,15 @@ Commander::run() } // automatically set or update home position - if (_param_com_home_en.get() && !_home_position_pub.get().manual_home) { + if (_param_com_home_en.get()) { // 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 (!_vehicle_land_detected.landed && (hrt_elapsed_time(&_boot_timestamp) > INAIR_RESTART_HOLDOFF_INTERVAL)) { if (was_landed) { - set_home_position(); + _home_position.setHomePosition(); - } else if (_param_com_home_in_air.get() - && (!_home_position_pub.get().valid_lpos || !_home_position_pub.get().valid_hpos - || !_home_position_pub.get().valid_alt)) { - set_in_air_home_position(); + } else if (_param_com_home_in_air.get()) { + _home_position.setInAirHomePosition(); } } } @@ -2301,7 +2042,8 @@ Commander::run() } } - estimator_check(); + _home_position.update(_param_com_home_en.get(), !_arm_state_machine.isArmed() && _vehicle_land_detected.landed); + _vehicle_status_flags.home_position_valid = _home_position.valid(); // Auto disarm when landed or kill switch engaged if (_arm_state_machine.isArmed()) { @@ -2777,24 +2519,6 @@ Commander::run() "Maximum flight time reached, abort operation and RTL"); } - // automatically set or update home position - if (_param_com_home_en.get() && !_home_position_pub.get().manual_home) { - if (!_arm_state_machine.isArmed() && _vehicle_land_detected.landed) { - const bool can_set_home_lpos_first_time = (!_home_position_pub.get().valid_lpos - && _vehicle_status_flags.local_position_valid); - const bool can_set_home_gpos_first_time = ((!_home_position_pub.get().valid_hpos || !_home_position_pub.get().valid_alt) - && (_vehicle_status_flags.global_position_valid || _vehicle_status_flags.gps_position_valid)); - const bool can_set_home_alt_first_time = (!_home_position_pub.get().valid_alt && _local_position_sub.get().z_global); - - if (can_set_home_lpos_first_time - || can_set_home_gpos_first_time - || can_set_home_alt_first_time - || hasMovedFromCurrentHomeLocation()) { - set_home_position(); - } - } - } - // check for arming state changes if (_was_armed != _arm_state_machine.isArmed()) { _status_changed = true; @@ -3699,23 +3423,6 @@ void Commander::battery_status_check() } } -void Commander::estimator_check() -{ - _local_position_sub.update(); - _global_position_sub.update(); - - const vehicle_local_position_s &lpos = _local_position_sub.get(); - - if (lpos.heading_reset_counter != _heading_reset_counter) { - if (_vehicle_status_flags.home_position_valid) { - updateHomePositionYaw(_home_position_pub.get().yaw + lpos.delta_heading); - } - - _heading_reset_counter = lpos.heading_reset_counter; - } - -} - void Commander::manual_control_check() { manual_control_setpoint_s manual_control_setpoint; diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index ec56cf4283..082eb06439 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -40,6 +40,7 @@ #include "state_machine_helper.h" #include "worker_thread.hpp" #include "HealthAndArmingChecks/HealthAndArmingChecks.hpp" +#include "HomePosition.hpp" #include #include @@ -53,7 +54,6 @@ #include #include #include -#include #include #include #include @@ -138,8 +138,6 @@ private: void esc_status_check(); - void estimator_check(); - void manual_control_check(); bool handle_command(const vehicle_command_s &cmd); @@ -153,17 +151,6 @@ private: void print_reject_mode(uint8_t main_state); - bool hasMovedFromCurrentHomeLocation(); - bool set_home_position(); - bool set_home_position_alt_only(); - 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; - void fillGlobalHomePos(home_position_s &home, double lat, double lon, float alt) const; - void setHomePosValid(); - void updateHomePositionYaw(float yaw); - void update_control_mode(); bool shutdown_if_allowed(); @@ -335,8 +322,6 @@ private: hrt_abstime _last_termination_message_sent{0}; - uint8_t _heading_reset_counter{0}; - bool _status_changed{true}; bool _arm_tune_played{false}; bool _was_armed{false}; @@ -370,7 +355,6 @@ private: uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; uORB::Subscription _system_power_sub{ORB_ID(system_power)}; uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)}; - uORB::Subscription _vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)}; uORB::Subscription _vtol_vehicle_status_sub{ORB_ID(vtol_vehicle_status)}; uORB::Subscription _wind_sub{ORB_ID(wind)}; @@ -384,8 +368,6 @@ private: uORB::SubscriptionData _mission_result_sub{ORB_ID(mission_result)}; uORB::SubscriptionData _offboard_control_mode_sub{ORB_ID(offboard_control_mode)}; - uORB::SubscriptionData _global_position_sub{ORB_ID(vehicle_global_position)}; - uORB::SubscriptionData _local_position_sub{ORB_ID(vehicle_local_position)}; // Publications uORB::Publication _actuator_armed_pub{ORB_ID(actuator_armed)}; @@ -397,8 +379,6 @@ private: uORB::Publication _vehicle_status_flags_pub{ORB_ID(vehicle_status_flags)}; uORB::Publication _vehicle_status_pub{ORB_ID(vehicle_status)}; - uORB::PublicationData _home_position_pub{ORB_ID(home_position)}; - uORB::Publication _vehicle_command_ack_pub{ORB_ID(vehicle_command_ack)}; orb_advert_t _mavlink_log_pub{nullptr}; @@ -406,4 +386,5 @@ private: perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; perf_counter_t _preflight_check_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": preflight check")}; HealthAndArmingChecks _health_and_arming_checks; + HomePosition _home_position{_vehicle_status_flags}; }; diff --git a/src/modules/commander/HomePosition.cpp b/src/modules/commander/HomePosition.cpp new file mode 100644 index 0000000000..ee23ad3994 --- /dev/null +++ b/src/modules/commander/HomePosition.cpp @@ -0,0 +1,341 @@ +/**************************************************************************** + * + * Copyright (c) 2022 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + + +#include "HomePosition.hpp" + +#include +#include "commander_helper.h" + +HomePosition::HomePosition(const vehicle_status_flags_s &vehicle_status_flags) + : _vehicle_status_flags(vehicle_status_flags) +{ +} + +bool HomePosition::hasMovedFromCurrentHomeLocation() +{ + float home_dist_xy = -1.f; + float home_dist_z = -1.f; + float eph = 0.f; + float epv = 0.f; + + if (_home_position_pub.get().valid_lpos && _local_position_sub.get().xy_valid && _local_position_sub.get().z_valid) { + mavlink_wpm_distance_to_point_local(_home_position_pub.get().x, _home_position_pub.get().y, _home_position_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_position_pub.get().valid_hpos && _home_position_pub.get().valid_alt) { + if (_valid) { + const vehicle_global_position_s &gpos = _global_position_sub.get(); + + get_distance_to_point_global_wgs84(_home_position_pub.get().lat, _home_position_pub.get().lon, + _home_position_pub.get().alt, + gpos.lat, gpos.lon, gpos.alt, + &home_dist_xy, &home_dist_z); + + eph = gpos.eph; + epv = gpos.epv; + + } else if (_vehicle_status_flags.gps_position_valid) { + sensor_gps_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_position_pub.get().lat, _home_position_pub.get().lon, + _home_position_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); +} + +bool HomePosition::setHomePosition(bool force) +{ + if (_home_position_pub.get().manual_home && !force) { + return false; + } + + bool updated = false; + home_position_s home{}; + + if (_vehicle_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; // TODO: should not be here + + fillLocalHomePos(home, lpos); + updated = true; + } + + if (_vehicle_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 (_vehicle_status_flags.gps_position_valid) { + // Set home using GNSS position + sensor_gps_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_position_pub.update(home); + } + + return updated; +} + +void HomePosition::fillLocalHomePos(home_position_s &home, const vehicle_local_position_s &lpos) +{ + fillLocalHomePos(home, lpos.x, lpos.y, lpos.z, lpos.heading); +} + +void HomePosition::fillLocalHomePos(home_position_s &home, float x, float y, float z, float heading) +{ + home.x = x; + home.y = y; + home.z = z; + home.valid_lpos = true; + + home.yaw = heading; +} + +void HomePosition::fillGlobalHomePos(home_position_s &home, const vehicle_global_position_s &gpos) +{ + fillGlobalHomePos(home, gpos.lat, gpos.lon, gpos.alt); +} + +void HomePosition::fillGlobalHomePos(home_position_s &home, double lat, double lon, float alt) +{ + home.lat = lat; + home.lon = lon; + home.valid_hpos = true; + home.alt = alt; + home.valid_alt = true; +} + +void HomePosition::setInAirHomePosition() +{ + auto &home = _home_position_pub.get(); + + if (home.manual_home || (home.valid_lpos && home.valid_hpos && home.valid_alt)) { + return; + } + + const bool global_home_valid = home.valid_hpos && home.valid_alt; + const bool local_home_valid = home.valid_lpos; + + if (local_home_valid && !global_home_valid) { + if (_vehicle_status_flags.local_position_valid && _vehicle_status_flags.global_position_valid) { + // Back-compute lon, lat and alt of home position given the local home position + // and current positions in local and global (GNSS fused) frames + const vehicle_local_position_s &lpos = _local_position_sub.get(); + const vehicle_global_position_s &gpos = _global_position_sub.get(); + + 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); + + setHomePosValid(); + home.timestamp = hrt_absolute_time(); + _home_position_pub.update(); + + } else if (_vehicle_status_flags.local_position_valid && _vehicle_status_flags.gps_position_valid) { + // Back-compute lon, lat and alt of home position given the local home position + // and current positions in local and global (GNSS raw) frames + const vehicle_local_position_s &lpos = _local_position_sub.get(); + sensor_gps_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; + + MapProjection ref_pos{lat, 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 = alt + home.z; + fillGlobalHomePos(home, home_lat, home_lon, home_alt); + + setHomePosValid(); + home.timestamp = hrt_absolute_time(); + _home_position_pub.update(); + } + + } else if (!local_home_valid && global_home_valid) { + const vehicle_local_position_s &lpos = _local_position_sub.get(); + + if (_vehicle_status_flags.local_position_valid && lpos.xy_global && lpos.z_global) { + // Back-compute x, y and z of home position given the global home position + // and the global reference of the local frame + MapProjection ref_pos{lpos.ref_lat, lpos.ref_lon}; + + float home_x; + float home_y; + ref_pos.project(home.lat, home.lon, home_x, home_y); + + const float home_z = -(home.alt - lpos.ref_alt); + fillLocalHomePos(home, home_x, home_y, home_z, NAN); + + home.timestamp = hrt_absolute_time(); + _home_position_pub.update(); + } + + } else if (!local_home_valid && !global_home_valid) { + // Home position is not known in any frame, set home at current position + setHomePosition(); + + } else { + // nothing to do + } +} + +bool HomePosition::setManually(double lat, double lon, float alt, float yaw) +{ + const auto &vehicle_local_position = _local_position_sub.get(); + + if (!vehicle_local_position.xy_global || !vehicle_local_position.z_global) { + return false; + } + + auto &home = _home_position_pub.get(); + home.manual_home = true; + + home.lat = lat; + home.lon = lon; + home.valid_hpos = true; + home.alt = alt; + home.valid_alt = true; + + // update local projection reference including altitude + MapProjection ref_pos{vehicle_local_position.ref_lat, vehicle_local_position.ref_lon}; + ref_pos.project(lat, lon, home.x, home.y); + home.z = -(alt - vehicle_local_position.ref_alt); + home.valid_lpos = vehicle_local_position.xy_valid && vehicle_local_position.z_valid; + + home.yaw = yaw; + + home.timestamp = hrt_absolute_time(); + _home_position_pub.update(); + setHomePosValid(); + return true; +} + + +void HomePosition::setHomePosValid() +{ + // play tune first time we initialize HOME + if (!_valid) { + tune_home_set(true); + } + + // mark home position as set + _valid = true; +} + +void HomePosition::updateHomePositionYaw(float yaw) +{ + home_position_s home = _home_position_pub.get(); + + home.yaw = yaw; + home.timestamp = hrt_absolute_time(); + + _home_position_pub.update(home); +} + +void HomePosition::update(bool set_automatically, bool check_if_changed) +{ + _local_position_sub.update(); + _global_position_sub.update(); + + const vehicle_local_position_s &lpos = _local_position_sub.get(); + const auto &home = _home_position_pub.get(); + + if (lpos.heading_reset_counter != _heading_reset_counter) { + if (_valid && set_automatically) { + updateHomePositionYaw(home.yaw + lpos.delta_heading); + } + + _heading_reset_counter = lpos.heading_reset_counter; + } + + if (check_if_changed && set_automatically) { + const bool can_set_home_lpos_first_time = !home.valid_lpos && _vehicle_status_flags.local_position_valid; + const bool can_set_home_gpos_first_time = ((!home.valid_hpos || !home.valid_alt) + && (_vehicle_status_flags.global_position_valid || _vehicle_status_flags.gps_position_valid)); + const bool can_set_home_alt_first_time = (!home.valid_alt && lpos.z_global); + + if (can_set_home_lpos_first_time + || can_set_home_gpos_first_time + || can_set_home_alt_first_time + || hasMovedFromCurrentHomeLocation()) { + setHomePosition(); + } + } +} diff --git a/src/modules/commander/HomePosition.hpp b/src/modules/commander/HomePosition.hpp new file mode 100644 index 0000000000..c6db4f646a --- /dev/null +++ b/src/modules/commander/HomePosition.hpp @@ -0,0 +1,78 @@ +/**************************************************************************** + * + * Copyright (c) 2022 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +class HomePosition +{ +public: + HomePosition(const vehicle_status_flags_s &vehicle_status_flags); + ~HomePosition() = default; + + bool setHomePosition(bool force = false); + void setInAirHomePosition(); + bool setManually(double lat, double lon, float alt, float yaw); + + void update(bool set_automatically, bool check_if_changed); + + bool valid() const { return _valid; } + +private: + bool hasMovedFromCurrentHomeLocation(); + void setHomePosValid(); + void updateHomePositionYaw(float yaw); + + static void fillLocalHomePos(home_position_s &home, const vehicle_local_position_s &lpos); + static void fillLocalHomePos(home_position_s &home, float x, float y, float z, float heading); + static void fillGlobalHomePos(home_position_s &home, const vehicle_global_position_s &gpos); + static void fillGlobalHomePos(home_position_s &home, double lat, double lon, float alt); + + uORB::Subscription _vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)}; + + uORB::SubscriptionData _global_position_sub{ORB_ID(vehicle_global_position)}; + uORB::SubscriptionData _local_position_sub{ORB_ID(vehicle_local_position)}; + + uORB::PublicationData _home_position_pub{ORB_ID(home_position)}; + + uint8_t _heading_reset_counter{0}; + bool _valid{false}; + const vehicle_status_flags_s &_vehicle_status_flags; +};