Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-02 03:49:12 +08:00
Code Issues Actions 6 Packages Projects Releases Wiki Activity

refactor commander: split out home position

Browse Source
This commit is contained in:
Beat Küng
2022-08-09 22:01:32 +02:00
committed by Daniel Agar
parent f17f38197d
commit c9037f115b
5 changed files with 432 additions and 324 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/CMakeLists.txt
+1
View File
@@ -50,6 +50,7 @@ px4_add_module(
esc_calibration.cpp esc_calibration.cpp
factory_calibration_storage.cpp factory_calibration_storage.cpp
gyro_calibration.cpp gyro_calibration.cpp
HomePosition.cpp
level_calibration.cpp level_calibration.cpp
lm_fit.cpp lm_fit.cpp
mag_calibration.cpp mag_calibration.cpp
src/modules/commander/Commander.cpp
+10 -303
View File
@@ -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 */ /* 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) if ((arming_action == vehicle_command_s::ARMING_ACTION_ARM) && (arming_res == TRANSITION_CHANGED)
&& (hrt_absolute_time() > (_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL)) && (hrt_absolute_time() > (_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL))
&& (_param_com_home_en.get() && !_home_position_pub.get().manual_home)) { && (_param_com_home_en.get())) {
set_home_position(); _home_position.setHomePosition();
} }
} }
} }
@@ -1044,7 +1044,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
if (use_current) { if (use_current) {
/* use current position */ /* use current position */
if (set_home_position()) { if (_home_position.setHomePosition(true)) {
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED; cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
} else { } else {
@@ -1059,27 +1059,8 @@ Commander::handle_command(const vehicle_command_s &cmd)
const float alt = cmd.param7; const float alt = cmd.param7;
if (PX4_ISFINITE(lat) && PX4_ISFINITE(lon) && PX4_ISFINITE(alt)) { 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) { if (_home_position.setManually(lat, lon, alt, yaw)) {
/* 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);
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED; 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() void Commander::updateParameters()
{ {
@@ -2189,17 +1932,15 @@ Commander::run()
} }
// automatically set or update home position // 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 // 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 (!_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) { if (was_landed) {
set_home_position(); _home_position.setHomePosition();
} else if (_param_com_home_in_air.get() } else if (_param_com_home_in_air.get()) {
&& (!_home_position_pub.get().valid_lpos || !_home_position_pub.get().valid_hpos _home_position.setInAirHomePosition();
|| !_home_position_pub.get().valid_alt)) {
set_in_air_home_position();
} }
} }
} }
@@ -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 // Auto disarm when landed or kill switch engaged
if (_arm_state_machine.isArmed()) { if (_arm_state_machine.isArmed()) {
@@ -2777,24 +2519,6 @@ Commander::run()
"Maximum flight time reached, abort operation and RTL"); "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 // check for arming state changes
if (_was_armed != _arm_state_machine.isArmed()) { if (_was_armed != _arm_state_machine.isArmed()) {
_status_changed = true; _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() void Commander::manual_control_check()
{ {
manual_control_setpoint_s manual_control_setpoint; manual_control_setpoint_s manual_control_setpoint;
src/modules/commander/Commander.hpp
+2 -21
View File
@@ -40,6 +40,7 @@
#include "state_machine_helper.h" #include "state_machine_helper.h"
#include "worker_thread.hpp" #include "worker_thread.hpp"
#include "HealthAndArmingChecks/HealthAndArmingChecks.hpp" #include "HealthAndArmingChecks/HealthAndArmingChecks.hpp"
#include "HomePosition.hpp"
#include <lib/controllib/blocks.hpp> #include <lib/controllib/blocks.hpp>
#include <lib/hysteresis/hysteresis.h> #include <lib/hysteresis/hysteresis.h>
@@ -53,7 +54,6 @@
#include <uORB/topics/actuator_armed.h> #include <uORB/topics/actuator_armed.h>
#include <uORB/topics/actuator_test.h> #include <uORB/topics/actuator_test.h>
#include <uORB/topics/failure_detector_status.h> #include <uORB/topics/failure_detector_status.h>
#include <uORB/topics/home_position.h>
#include <uORB/topics/test_motor.h> #include <uORB/topics/test_motor.h>
#include <uORB/topics/vehicle_command_ack.h> #include <uORB/topics/vehicle_command_ack.h>
#include <uORB/topics/vehicle_control_mode.h> #include <uORB/topics/vehicle_control_mode.h>
@@ -138,8 +138,6 @@ private:
void esc_status_check(); void esc_status_check();
void estimator_check();
void manual_control_check(); void manual_control_check();
bool handle_command(const vehicle_command_s &cmd); bool handle_command(const vehicle_command_s &cmd);
@@ -153,17 +151,6 @@ private:
void print_reject_mode(uint8_t main_state); 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(); void update_control_mode();
bool shutdown_if_allowed(); bool shutdown_if_allowed();
@@ -335,8 +322,6 @@ private:
hrt_abstime _last_termination_message_sent{0}; hrt_abstime _last_termination_message_sent{0};
uint8_t _heading_reset_counter{0};
bool _status_changed{true}; bool _status_changed{true};
bool _arm_tune_played{false}; bool _arm_tune_played{false};
bool _was_armed{false}; bool _was_armed{false};
@@ -370,7 +355,6 @@ private:
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
uORB::Subscription _system_power_sub{ORB_ID(system_power)}; uORB::Subscription _system_power_sub{ORB_ID(system_power)};
uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)}; 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 _vtol_vehicle_status_sub{ORB_ID(vtol_vehicle_status)};
uORB::Subscription _wind_sub{ORB_ID(wind)}; 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<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<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 // Publications
uORB::Publication<actuator_armed_s> _actuator_armed_pub{ORB_ID(actuator_armed)}; 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_flags_s> _vehicle_status_flags_pub{ORB_ID(vehicle_status_flags)};
uORB::Publication<vehicle_status_s> _vehicle_status_pub{ORB_ID(vehicle_status)}; 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)}; uORB::Publication<vehicle_command_ack_s> _vehicle_command_ack_pub{ORB_ID(vehicle_command_ack)};
orb_advert_t _mavlink_log_pub{nullptr}; 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 _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
perf_counter_t _preflight_check_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": preflight check")}; perf_counter_t _preflight_check_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": preflight check")};
HealthAndArmingChecks _health_and_arming_checks; HealthAndArmingChecks _health_and_arming_checks;
HomePosition _home_position{_vehicle_status_flags};
}; };
src/modules/commander/HomePosition.cpp
+341
View File
@@ -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();
}
}
}
src/modules/commander/HomePosition.hpp
+78
View File
@@ -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;
};