mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-06-01 19:07:45 +08:00
refactor commander: split out home position
This commit is contained in:
Beat Küng
Daniel Agar
@@ -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
|
||||
|
||||
@@ -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<double>(gps.lat) * 1e-7;
|
||||
const double lon = static_cast<double>(gps.lon) * 1e-7;
|
||||
const float alt = static_cast<float>(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<double>(gps_pos.lat) * 1e-7;
|
||||
const double lon = static_cast<double>(gps_pos.lon) * 1e-7;
|
||||
const float alt = static_cast<float>(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<double>(gps.lat) * 1e-7;
|
||||
const double lon = static_cast<double>(gps.lon) * 1e-7;
|
||||
const float alt = static_cast<float>(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;
|
||||
|
||||
@@ -40,6 +40,7 @@
|
||||
#include "state_machine_helper.h"
|
||||
#include "worker_thread.hpp"
|
||||
#include "HealthAndArmingChecks/HealthAndArmingChecks.hpp"
|
||||
#include "HomePosition.hpp"
|
||||
|
||||
#include <lib/controllib/blocks.hpp>
|
||||
#include <lib/hysteresis/hysteresis.h>
|
||||
@@ -53,7 +54,6 @@
|
||||
#include <uORB/topics/actuator_armed.h>
|
||||
#include <uORB/topics/actuator_test.h>
|
||||
#include <uORB/topics/failure_detector_status.h>
|
||||
#include <uORB/topics/home_position.h>
|
||||
#include <uORB/topics/test_motor.h>
|
||||
#include <uORB/topics/vehicle_command_ack.h>
|
||||
#include <uORB/topics/vehicle_control_mode.h>
|
||||
@@ -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_s> _mission_result_sub{ORB_ID(mission_result)};
|
||||
uORB::SubscriptionData<offboard_control_mode_s> _offboard_control_mode_sub{ORB_ID(offboard_control_mode)};
|
||||
uORB::SubscriptionData<vehicle_global_position_s> _global_position_sub{ORB_ID(vehicle_global_position)};
|
||||
uORB::SubscriptionData<vehicle_local_position_s> _local_position_sub{ORB_ID(vehicle_local_position)};
|
||||
|
||||
// Publications
|
||||
uORB::Publication<actuator_armed_s> _actuator_armed_pub{ORB_ID(actuator_armed)};
|
||||
@@ -397,8 +379,6 @@ private:
|
||||
uORB::Publication<vehicle_status_flags_s> _vehicle_status_flags_pub{ORB_ID(vehicle_status_flags)};
|
||||
uORB::Publication<vehicle_status_s> _vehicle_status_pub{ORB_ID(vehicle_status)};
|
||||
|
||||
uORB::PublicationData<home_position_s> _home_position_pub{ORB_ID(home_position)};
|
||||
|
||||
uORB::Publication<vehicle_command_ack_s> _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};
|
||||
};
|
||||
|
||||
@@ -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 <lib/geo/geo.h>
|
||||
#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<double>(gps.lat) * 1e-7;
|
||||
const double lon = static_cast<double>(gps.lon) * 1e-7;
|
||||
const float alt = static_cast<float>(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<double>(gps_pos.lat) * 1e-7;
|
||||
const double lon = static_cast<double>(gps_pos.lon) * 1e-7;
|
||||
const float alt = static_cast<float>(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<double>(gps.lat) * 1e-7;
|
||||
const double lon = static_cast<double>(gps.lon) * 1e-7;
|
||||
const float alt = static_cast<float>(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();
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -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 <uORB/Publication.hpp>
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/topics/home_position.h>
|
||||
#include <uORB/topics/sensor_gps.h>
|
||||
#include <uORB/topics/vehicle_global_position.h>
|
||||
#include <uORB/topics/vehicle_local_position.h>
|
||||
#include <uORB/topics/vehicle_status_flags.h>
|
||||
|
||||
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<vehicle_global_position_s> _global_position_sub{ORB_ID(vehicle_global_position)};
|
||||
uORB::SubscriptionData<vehicle_local_position_s> _local_position_sub{ORB_ID(vehicle_local_position)};
|
||||
|
||||
uORB::PublicationData<home_position_s> _home_position_pub{ORB_ID(home_position)};
|
||||
|
||||
uint8_t _heading_reset_counter{0};
|
||||
bool _valid{false};
|
||||
const vehicle_status_flags_s &_vehicle_status_flags;
|
||||
};
|
||||
Reference in New Issue
Block a user