fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-02 05:04:08 +08:00
Code Issues Packages Projects Releases Wiki Activity

Made a separate precision landing library. Now, the Precision Landing mode can use it the same as the RTL and the mission mode.

Browse Source
This commit is contained in:
Konrad 2022-11-16 13:08:20 +01:00
parent 5910f8982a
commit a5663d4d3c
11 changed files with 400 additions and 184 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
src/modules/navigator/CMakeLists.txt
View File

@ -46,6 +46,7 @@ px4_add_module(
takeoff.cpp
land.cpp
precland.cpp
precland_mode.cpp
mission_feasibility_checker.cpp
geofence.cpp
vtol_takeoff.cpp

45
src/modules/navigator/mission.cpp
View File

@ -176,10 +176,6 @@ Mission::on_inactivation()
_navigator->stop_capturing_images();
_navigator->release_gimbal_control();
if (_navigator->get_precland()->is_activated()) {
_navigator->get_precland()->on_inactivation();
}
_time_mission_deactivated = hrt_absolute_time();
/* reset so current mission item gets restarted if mission was paused */
@ -298,10 +294,25 @@ Mission::on_active()
}
if (_work_item_type == WORK_ITEM_TYPE_PRECISION_LAND) {
_navigator->get_precland()->on_active();
// Update the position in the setpoint triplet.
_precland.setAcceptanceRadius(_navigator->get_acceptance_radius());
_precland.update();
PrecLand::Output prec_land_output{_precland.getOutput()};
} else if (_navigator->get_precland()->is_activated()) {
_navigator->get_precland()->on_inactivation();
_mission_item.altitude = prec_land_output.alt;
_mission_item.lat = prec_land_output.pos_hor.lat;
_mission_item.lon = prec_land_output.pos_hor.lon;
_mission_item.nav_cmd = prec_land_output.nav_cmd;
mission_apply_limitation(_mission_item);
position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
pos_sp_triplet->previous.valid = false;
pos_sp_triplet->next.valid = false;
if (mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current)) {
_navigator->set_position_setpoint_triplet_updated();
}
}
}
@ -983,13 +994,17 @@ Mission::set_mission_items()
new_work_item_type = WORK_ITEM_TYPE_PRECISION_LAND;
if (_mission_item.land_precision == 1) {
_navigator->get_precland()->set_mode(PrecLandMode::Opportunistic);
_precland.setMode(PrecLand::PrecLandMode::Opportunistic);
} else { //_mission_item.land_precision == 2
_navigator->get_precland()->set_mode(PrecLandMode::Required);
_precland.setMode(PrecLand::PrecLandMode::Required);
}
_navigator->get_precland()->on_activation();
PrecLand::LandingPosition2D approximate_landing_pos{
.lat = _mission_item.lat,
.lon = _mission_item.lon,
};
_precland.initialize(approximate_landing_pos);
}
}
@ -1002,13 +1017,17 @@ Mission::set_mission_items()
new_work_item_type = WORK_ITEM_TYPE_PRECISION_LAND;
if (_mission_item.land_precision == 1) {
_navigator->get_precland()->set_mode(PrecLandMode::Opportunistic);
_precland.setMode(PrecLand::PrecLandMode::Opportunistic);
} else { //_mission_item.land_precision == 2
_navigator->get_precland()->set_mode(PrecLandMode::Required);
_precland.setMode(PrecLand::PrecLandMode::Required);
}
_navigator->get_precland()->on_activation();
PrecLand::LandingPosition2D approximate_landing_pos{
.lat = _mission_item.lat,
.lon = _mission_item.lon,
};
_precland.initialize(approximate_landing_pos);
}

3
src/modules/navigator/mission.h
View File

@ -49,6 +49,7 @@
#include "mission_block.h"
#include "mission_feasibility_checker.h"
#include "navigator_mode.h"
#include "precland.h"
#include <float.h>
@ -270,6 +271,8 @@ private:
hrt_abstime _time_mission_deactivated{0};
PrecLand _precland;
enum {
MISSION_TYPE_NONE,
MISSION_TYPE_MISSION

6
src/modules/navigator/navigator.h
View File

@ -43,7 +43,7 @@
#include "geofence.h"
#include "land.h"
#include "precland.h"
#include "precland_mode.h"
#include "loiter.h"
#include "mission.h"
#include "navigator_mode.h"
@ -172,8 +172,6 @@ public:
vehicle_local_position_s *get_local_position() { return &_local_pos; }
vehicle_status_s *get_vstatus() { return &_vstatus; }
PrecLand *get_precland() { return &_precland; } /**< allow others, e.g. Mission, to use the precision land block */
const vehicle_roi_s &get_vroi() { return _vroi; }
void reset_vroi() { _vroi = {}; }
@ -391,7 +389,7 @@ private:
Takeoff _takeoff; /**< class for handling takeoff commands */
VtolTakeoff _vtol_takeoff; /**< class for handling VEHICLE_CMD_NAV_VTOL_TAKEOFF command */
Land _land; /**< class for handling land commands */
PrecLand _precland; /**< class for handling precision land commands */
PrecLandMode _precland; /**< class for handling precision land commands */
RTL _rtl; /**< class that handles RTL */
NavigatorMode *_navigation_mode{nullptr}; /**< abstract pointer to current navigation mode class */

2
src/modules/navigator/navigator_main.cpp
View File

@ -706,7 +706,7 @@ void Navigator::run()
case vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND:
_pos_sp_triplet_published_invalid_once = false;
navigation_mode_new = &_precland;
_precland.set_mode(PrecLandMode::Required);
_precland.set_mode(PrecLand::PrecLandMode::Required);
break;
case vehicle_status_s::NAVIGATION_STATE_MANUAL:

218
src/modules/navigator/precland.cpp
View File

@ -39,31 +39,18 @@
*/
#include "precland.h"
#include "navigator.h"
#include <string.h>
#include <stdlib.h>
#include <stdbool.h>
#include <math.h>
#include <fcntl.h>
#include <drivers/drv_hrt.h>
#include <systemlib/err.h>
#include <systemlib/mavlink_log.h>
#include <uORB/uORB.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_command.h>
using namespace time_literals;
#define SEC2USEC 1000000.0f
#define STATE_TIMEOUT 10000000 // [us] Maximum time to spend in any state
static constexpr hrt_abstime state_timeout{10_s}; // Maximum time to spend in any state
static constexpr const char *LOST_TARGET_ERROR_MESSAGE = "Lost landing target while landing";
PrecLand::PrecLand(Navigator *navigator) :
MissionBlock(navigator),
ModuleParams(navigator)
PrecLand::PrecLand() : ModuleParams(nullptr)
{
_handle_param_acceleration_hor = param_find("MPC_ACC_HOR");
_handle_param_xy_vel_cruise = param_find("MPC_XY_CRUISE");
@ -72,31 +59,17 @@ PrecLand::PrecLand(Navigator *navigator) :
}
void
PrecLand::on_activation()
PrecLand::initialize(const LandingPosition2D &approximate_landing_pos)
{
_approximate_landing_pos = approximate_landing_pos;
_local_pos_sub.update();
_state = PrecLandState::Start;
_search_cnt = 0;
_last_slewrate_time = 0;
vehicle_local_position_s *vehicle_local_position = _navigator->get_local_position();
_point_reached_time = 0u;
if (!_map_ref.isInitialized()) {
_map_ref.initReference(vehicle_local_position->ref_lat, vehicle_local_position->ref_lon);
}
position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
pos_sp_triplet->next.valid = false;
pos_sp_triplet->previous.valid = false;
// Check that the current position setpoint is valid, otherwise land at current position
if (!pos_sp_triplet->current.valid) {
PX4_WARN("Reset");
pos_sp_triplet->current.lat = _navigator->get_global_position()->lat;
pos_sp_triplet->current.lon = _navigator->get_global_position()->lon;
pos_sp_triplet->current.alt = _navigator->get_global_position()->alt;
pos_sp_triplet->current.valid = true;
pos_sp_triplet->current.timestamp = hrt_absolute_time();
_map_ref.initReference(_local_pos_sub.get().ref_lat, _local_pos_sub.get().ref_lon);
}
_sp_pev = matrix::Vector2f(0, 0);
@ -104,26 +77,27 @@ PrecLand::on_activation()
_last_slewrate_time = 0;
switch_to_state_start();
_is_activated = true;
}
void
PrecLand::on_active()
PrecLand::update()
{
// get new target measurement
_target_pose_updated = _target_pose_sub.update(&_target_pose);
// get new input measurement
_target_pose_updated = _target_pose_sub.update();
_land_detected_sub.update();
_global_pos_sub.update();
_local_pos_sub.update();
if (_target_pose_updated) {
_target_pose_valid = true;
}
if ((hrt_elapsed_time(&_target_pose.timestamp) / 1e6f) > _param_pld_btout.get()) {
if ((hrt_elapsed_time(&_target_pose_sub.get().timestamp) / 1e6f) > _param_pld_btout.get()) {
_target_pose_valid = false;
}
// stop if we are landed
if (_navigator->get_land_detected()->landed) {
if (_land_detected_sub.get().landed) {
switch_to_state_done();
}
@ -153,20 +127,13 @@ PrecLand::on_active()
break;
case PrecLandState::Done:
// nothing to do
run_state_done();
break;
default:
// unknown state
break;
}
}
void
PrecLand::on_inactivation()
{
_is_activated = false;
}
void
@ -186,6 +153,11 @@ PrecLand::updateParams()
void
PrecLand::run_state_start()
{
_output.nav_cmd = NAV_CMD_WAYPOINT;
_output.pos_hor.lat = _approximate_landing_pos.lat;
_output.pos_hor.lon = _approximate_landing_pos.lon;
_output.alt = _global_pos_sub.get().alt;
// check if target visible and go to horizontal approach
if (switch_to_state_horizontal_approach()) {
return;
@ -196,20 +168,19 @@ PrecLand::run_state_start()
switch_to_state_fallback();
}
position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
float dist = get_distance_to_next_waypoint(pos_sp_triplet->current.lat, pos_sp_triplet->current.lon,
_navigator->get_global_position()->lat, _navigator->get_global_position()->lon);
float dist = get_distance_to_next_waypoint(_approximate_landing_pos.lat, _approximate_landing_pos.lon,
_global_pos_sub.get().lat, _global_pos_sub.get().lon);
// check if we've reached the start point
if (dist < _navigator->get_acceptance_radius()) {
if (dist < _acceptance_radius) {
if (!_point_reached_time) {
_point_reached_time = hrt_absolute_time();
}
// if we don't see the target after 1 second, search for it
if (_param_pld_srch_tout.get() > 0) {
if (_param_pld_srch_tout.get() > FLT_EPSILON) {
if (hrt_absolute_time() - _point_reached_time > 2000000) {
if (hrt_absolute_time() - _point_reached_time > 1_s) {
if (!switch_to_state_search()) {
switch_to_state_fallback();
}
@ -224,16 +195,17 @@ PrecLand::run_state_start()
void
PrecLand::run_state_horizontal_approach()
{
position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
_output.nav_cmd = NAV_CMD_WAYPOINT;
_output.pos_hor.lat = _approximate_landing_pos.lat;
_output.pos_hor.lon = _approximate_landing_pos.lon;
_output.alt = _approach_alt;
// check if target visible, if not go to start
if (!check_state_conditions(PrecLandState::HorizontalApproach)) {
PX4_WARN("%s, state: %i", LOST_TARGET_ERROR_MESSAGE, (int) _state);
// Stay at current position for searching for the landing target
pos_sp_triplet->current.lat = _navigator->get_global_position()->lat;
pos_sp_triplet->current.lon = _navigator->get_global_position()->lon;
pos_sp_triplet->current.alt = _navigator->get_global_position()->alt;
// Stop at current altitude
_output.alt = _global_pos_sub.get().alt;
if (!switch_to_state_start()) {
switch_to_state_fallback();
@ -247,44 +219,41 @@ PrecLand::run_state_horizontal_approach()
_point_reached_time = hrt_absolute_time();
}
if (hrt_absolute_time() - _point_reached_time > 2000000) {
if (hrt_absolute_time() - _point_reached_time > 2_s) {
// if close enough for descent above target go to descend above target
if (switch_to_state_descend_above_target()) {
return;
}
}
}
float x = _target_pose.x_abs;
float y = _target_pose.y_abs;
float x = _target_pose_sub.get().x_abs;
float y = _target_pose_sub.get().y_abs;
slewrate(x, y);
// XXX need to transform to GPS coords because mc_pos_control only looks at that
_map_ref.reproject(x, y, pos_sp_triplet->current.lat, pos_sp_triplet->current.lon);
pos_sp_triplet->current.alt = _approach_alt;
pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_POSITION;
_navigator->set_position_setpoint_triplet_updated();
_map_ref.reproject(x, y, _output.pos_hor.lat, _output.pos_hor.lon);
_approximate_landing_pos.lat = _output.pos_hor.lat;
_approximate_landing_pos.lon = _output.pos_hor.lon;
}
void
PrecLand::run_state_descend_above_target()
{
position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
_output.nav_cmd = NAV_CMD_LAND;
_output.pos_hor.lat = _approximate_landing_pos.lat;
_output.pos_hor.lon = _approximate_landing_pos.lon;
_output.alt = _global_pos_sub.get().alt;
// check if target visible
if (!check_state_conditions(PrecLandState::DescendAboveTarget)) {
if (!switch_to_state_final_approach()) {
PX4_WARN("%s, state: %i", LOST_TARGET_ERROR_MESSAGE, (int) _state);
// Stay at current position for searching for the target
pos_sp_triplet->current.lat = _navigator->get_global_position()->lat;
pos_sp_triplet->current.lon = _navigator->get_global_position()->lon;
pos_sp_triplet->current.alt = _navigator->get_global_position()->alt;
// Stay at current altitude for searching for the target
_output.nav_cmd = NAV_CMD_WAYPOINT;
if (!switch_to_state_start()) {
switch_to_state_fallback();
@ -295,37 +264,44 @@ PrecLand::run_state_descend_above_target()
}
// XXX need to transform to GPS coords because mc_pos_control only looks at that
_map_ref.reproject(_target_pose.x_abs, _target_pose.y_abs, pos_sp_triplet->current.lat, pos_sp_triplet->current.lon);
pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_LAND;
_navigator->set_position_setpoint_triplet_updated();
_map_ref.reproject(_target_pose_sub.get().x_abs, _target_pose_sub.get().y_abs, _output.pos_hor.lat,
_output.pos_hor.lon);
_approximate_landing_pos.lat = _output.pos_hor.lat;
_approximate_landing_pos.lon = _output.pos_hor.lon;
}
void
PrecLand::run_state_final_approach()
{
// nothing to do, will land
_output.nav_cmd = NAV_CMD_LAND;
_output.pos_hor.lat = _approximate_landing_pos.lat;
_output.pos_hor.lon = _approximate_landing_pos.lon;
_output.alt = _global_pos_sub.get().alt;
}
void
PrecLand::run_state_search()
{
_output.nav_cmd = NAV_CMD_WAYPOINT;
_output.pos_hor.lat = _approximate_landing_pos.lat;
_output.pos_hor.lon = _approximate_landing_pos.lon;
// check if we can see the target
if (check_state_conditions(PrecLandState::HorizontalApproach)) {
if (!_target_acquired_time) {
// target just became visible. Stop climbing, but give it some margin so we don't stop too apruptly
// target just became visible. Stop climbing, but give it some margin so we don't stop too abruptly
_target_acquired_time = hrt_absolute_time();
position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
float new_alt = _navigator->get_global_position()->alt + 1.0f;
pos_sp_triplet->current.alt = new_alt < pos_sp_triplet->current.alt ? new_alt : pos_sp_triplet->current.alt;
_navigator->set_position_setpoint_triplet_updated();
float new_alt = _global_pos_sub.get().alt + 1.0f;
_output.alt = new_alt < _output.alt ? new_alt : _output.alt;
}
} else {
_target_acquired_time = 0u;
_output.alt = _local_pos_sub.get().ref_alt + _param_pld_srch_alt.get();
}
// stay at that height for a second to allow the vehicle to settle
if (_target_acquired_time && (hrt_absolute_time() - _target_acquired_time) > 1000000) {
if (_target_acquired_time && (hrt_absolute_time() - _target_acquired_time) > 1_s) {
// try to switch to horizontal approach
if (switch_to_state_horizontal_approach()) {
return;
@ -333,7 +309,7 @@ PrecLand::run_state_search()
}
// check if search timed out and go to fallback
if (hrt_absolute_time() - _state_start_time > _param_pld_srch_tout.get()*SEC2USEC) {
if (hrt_absolute_time() - _state_start_time > _param_pld_srch_tout.get() * 1_s) {
PX4_WARN("Search timed out");
switch_to_state_fallback();
@ -343,16 +319,24 @@ PrecLand::run_state_search()
void
PrecLand::run_state_fallback()
{
// nothing to do, will land
_output.nav_cmd = NAV_CMD_LAND;
_output.pos_hor.lat = _approximate_landing_pos.lat;
_output.pos_hor.lon = _approximate_landing_pos.lon;
}
void
PrecLand::run_state_done()
{
_output.nav_cmd = NAV_CMD_IDLE;
_output.pos_hor.lat = _global_pos_sub.get().lat;
_output.pos_hor.lon = _global_pos_sub.get().lon;
_output.alt = _global_pos_sub.get().alt;
}
bool
PrecLand::switch_to_state_start()
{
if (check_state_conditions(PrecLandState::Start)) {
position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_POSITION;
_navigator->set_position_setpoint_triplet_updated();
_search_cnt++;
_point_reached_time = 0;
@ -370,7 +354,7 @@ PrecLand::switch_to_state_horizontal_approach()
{
if (check_state_conditions(PrecLandState::HorizontalApproach)) {
print_state_switch_message("horizontal approach");
_approach_alt = _navigator->get_global_position()->alt;
_approach_alt = _global_pos_sub.get().alt;
_point_reached_time = 0;
@ -412,12 +396,6 @@ bool
PrecLand::switch_to_state_search()
{
PX4_INFO("Climbing to search altitude.");
vehicle_local_position_s *vehicle_local_position = _navigator->get_local_position();
position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
pos_sp_triplet->current.alt = vehicle_local_position->ref_alt + _param_pld_srch_alt.get();
pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_POSITION;
_navigator->set_position_setpoint_triplet_updated();
_target_acquired_time = 0;
@ -430,12 +408,6 @@ bool
PrecLand::switch_to_state_fallback()
{
print_state_switch_message("fallback");
position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
pos_sp_triplet->current.lat = _navigator->get_global_position()->lat;
pos_sp_triplet->current.lon = _navigator->get_global_position()->lon;
pos_sp_triplet->current.alt = _navigator->get_global_position()->alt;
pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_LAND;
_navigator->set_position_setpoint_triplet_updated();
_state = PrecLandState::Fallback;
_state_start_time = hrt_absolute_time();
@ -457,8 +429,6 @@ void PrecLand::print_state_switch_message(const char *state_name)
bool PrecLand::check_state_conditions(PrecLandState state)
{
vehicle_local_position_s *vehicle_local_position = _navigator->get_local_position();
switch (state) {
case PrecLandState::Start:
return _search_cnt <= _param_pld_max_srch.get();
@ -467,10 +437,10 @@ bool PrecLand::check_state_conditions(PrecLandState state)
// if we're already in this state, only want to make it invalid if we reached the target but can't see it anymore
if (_state == PrecLandState::HorizontalApproach) {
if (fabsf(_target_pose.x_abs - vehicle_local_position->x) < _param_pld_hacc_rad.get()
&& fabsf(_target_pose.y_abs - vehicle_local_position->y) < _param_pld_hacc_rad.get()) {
if (fabsf(_target_pose_sub.get().x_abs - _local_pos_sub.get().x) < _param_pld_hacc_rad.get()
&& fabsf(_target_pose_sub.get().y_abs - _local_pos_sub.get().y) < _param_pld_hacc_rad.get()) {
// we've reached the position where we last saw the target. If we don't see it now, we need to do something
return _target_pose_valid && _target_pose.abs_pos_valid;
return _target_pose_valid && _target_pose_sub.get().abs_pos_valid;
} else {
// We've seen the target sometime during horizontal approach.
@ -480,7 +450,7 @@ bool PrecLand::check_state_conditions(PrecLandState state)
}
// If we're trying to switch to this state, the target needs to be visible
return _target_pose_updated && _target_pose_valid && _target_pose.abs_pos_valid;
return _target_pose_updated && _target_pose_valid && _target_pose_sub.get().abs_pos_valid;
case PrecLandState::DescendAboveTarget:
@ -488,22 +458,22 @@ bool PrecLand::check_state_conditions(PrecLandState state)
if (_state == PrecLandState::DescendAboveTarget) {
// if we're close to the ground, we're more critical of target timeouts so we quickly go into descend
if (check_state_conditions(PrecLandState::FinalApproach)) {
return hrt_absolute_time() - _target_pose.timestamp < 500000; // 0.5s
return hrt_absolute_time() - _target_pose_sub.get().timestamp < 500_ms; // 0.5s
} else {
return _target_pose_valid && _target_pose.abs_pos_valid;
return _target_pose_valid && _target_pose_sub.get().abs_pos_valid;
}
} else {
// if not already in this state, need to be above target to enter it
return _target_pose_updated && _target_pose.abs_pos_valid
&& fabsf(_target_pose.x_abs - vehicle_local_position->x) < _param_pld_hacc_rad.get()
&& fabsf(_target_pose.y_abs - vehicle_local_position->y) < _param_pld_hacc_rad.get();
return _target_pose_updated && _target_pose_sub.get().abs_pos_valid
&& fabsf(_target_pose_sub.get().x_abs - _local_pos_sub.get().x) < _param_pld_hacc_rad.get()
&& fabsf(_target_pose_sub.get().y_abs - _local_pos_sub.get().y) < _param_pld_hacc_rad.get();
}
case PrecLandState::FinalApproach:
return _target_pose_valid && _target_pose.abs_pos_valid
&& (_target_pose.z_abs - vehicle_local_position->z) < _param_pld_fappr_alt.get();
return _target_pose_valid && _target_pose_sub.get().abs_pos_valid
&& (_target_pose_sub.get().z_abs - _local_pos_sub.get().z) < _param_pld_fappr_alt.get();
case PrecLandState::Search:
return true;
@ -537,10 +507,10 @@ void PrecLand::slewrate(float &sp_x, float &sp_y)
dt = 50000 / SEC2USEC;
// set a best guess for previous setpoints for smooth transition
_sp_pev = _map_ref.project(_navigator->get_position_setpoint_triplet()->current.lat,
_navigator->get_position_setpoint_triplet()->current.lon);
_sp_pev_prev(0) = _sp_pev(0) - _navigator->get_local_position()->vx * dt;
_sp_pev_prev(1) = _sp_pev(1) - _navigator->get_local_position()->vy * dt;
_sp_pev = _map_ref.project(_output.pos_hor.lat,
_output.pos_hor.lon);
_sp_pev_prev(0) = _sp_pev(0) - _local_pos_sub.get().vx * dt;
_sp_pev_prev(1) = _sp_pev(1) - _local_pos_sub.get().vy * dt;
}
_last_slewrate_time = now;

80
src/modules/navigator/precland.h
View File

@ -43,45 +43,61 @@
#include <matrix/math.hpp>
#include <lib/geo/geo.h>
#include <px4_platform_common/module_params.h>
#include <uORB/uORB.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/landing_target_pose.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/vehicle_local_position.h>
#include "navigator_mode.h"
#include "mission_block.h"
#include "navigator/navigation.h"
enum class PrecLandState {
Start, // Starting state
HorizontalApproach, // Positioning over landing target while maintaining altitude
DescendAboveTarget, // Stay over landing target while descending
FinalApproach, // Final landing approach, even without landing target
Search, // Search for landing target
Fallback, // Fallback landing method
Done // Done landing
};
enum class PrecLandMode {
Opportunistic = 1, // only do precision landing if landing target visible at the beginning
Required = 2 // try to find landing target if not visible at the beginning
};
class PrecLand : public MissionBlock, public ModuleParams
class PrecLand : public ModuleParams
{
public:
PrecLand(Navigator *navigator);
~PrecLand() override = default;
enum class PrecLandMode {
Opportunistic = 1, // only do precision landing if landing target visible at the beginning
Required = 2 // try to find landing target if not visible at the beginning
};
void on_activation() override;
void on_active() override;
void on_inactivation() override;
struct LandingPosition2D {
double lat;
double lon;
};
void set_mode(PrecLandMode mode) { _mode = mode; };
struct Output {
LandingPosition2D pos_hor;
float alt;
enum NAV_CMD nav_cmd;
};
PrecLandMode get_mode() { return _mode; };
public:
PrecLand();
~PrecLand() = default;
bool is_activated() { return _is_activated; };
void initialize(const LandingPosition2D &approximate_landing_pos);
void update();
Output getOutput() {return _output;};
void setMode(PrecLandMode mode) { _mode = mode; };
void setAcceptanceRadius(float acceptance_radius) {_acceptance_radius = acceptance_radius;};
PrecLandMode getMode() { return _mode; };
private:
enum class PrecLandState {
Start, // Starting state
HorizontalApproach, // Positioning over landing target while maintaining altitude
DescendAboveTarget, // Stay over landing target while descending
FinalApproach, // Final landing approach, even without landing target
Search, // Search for landing target
Fallback, // Fallback landing method
Done // Done landing
};
private:
void updateParams() override;
// run the control loop for each state
@ -91,6 +107,7 @@ private:
void run_state_final_approach();
void run_state_search();
void run_state_fallback();
void run_state_done();
// attempt to switch to a different state. Returns true if state change was successful, false otherwise
bool switch_to_state_start();
@ -107,9 +124,14 @@ private:
bool check_state_conditions(PrecLandState state);
void slewrate(float &sp_x, float &sp_y);
landing_target_pose_s _target_pose{}; /**< precision landing target position */
LandingPosition2D _approximate_landing_pos{}; /**< Global position in WGS84 at which to find the landing target.*/
float _acceptance_radius{};
uORB::SubscriptionData<landing_target_pose_s> _target_pose_sub{ORB_ID(landing_target_pose)};
uORB::SubscriptionData<vehicle_land_detected_s> _land_detected_sub{ORB_ID(vehicle_land_detected)}; /**< vehicle land detected subscription */
uORB::SubscriptionData<vehicle_global_position_s> _global_pos_sub{ORB_ID(vehicle_global_position)}; /**< global position subscription */
uORB::SubscriptionData<vehicle_local_position_s> _local_pos_sub{ORB_ID(vehicle_local_position)}; /**< local position subscription */
uORB::Subscription _target_pose_sub{ORB_ID(landing_target_pose)};
bool _target_pose_valid{false}; /**< whether we have received a landing target position message */
bool _target_pose_updated{false}; /**< wether the landing target position message is updated */
@ -130,7 +152,7 @@ private:
PrecLandMode _mode{PrecLandMode::Opportunistic};
bool _is_activated {false}; /**< indicates if precland is activated */
Output _output;
DEFINE_PARAMETERS(
(ParamFloat<px4::params::PLD_BTOUT>) _param_pld_btout,

101
src/modules/navigator/precland_mode.cpp Normal file
View File

@ -0,0 +1,101 @@
/****************************************************************************
*
* Copyright (c) 2017 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.
*
****************************************************************************/
/**
* @file precland_mode.cpp
*
* Helper class to do precision landing with a landing target
*
* @author Nicolas de Palezieux (Sunflower Labs) <ndepal@gmail.com>
*/
#include "precland_mode.h"
#include "navigator.h"
#include <uORB/topics/position_setpoint_triplet.h>
PrecLandMode::PrecLandMode(Navigator *navigator) :
MissionBlock(navigator)
{
}
void
PrecLandMode::on_activation()
{
_global_pos_sub.update();
// Get the landing position from current position_setpoint, else use the current vehicle position.
position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
PrecLand::LandingPosition2D approximate_landing_pos{ .lat = pos_sp_triplet->current.lat,
.lon = pos_sp_triplet->current.lon};
if (!pos_sp_triplet->current.valid) {
PX4_WARN("No valid landing position for precision landing. Using current position");
approximate_landing_pos.lat = _global_pos_sub.get().lat;
approximate_landing_pos.lon = _global_pos_sub.get().lon;
}
_prec_land.initialize(approximate_landing_pos);
}
void
PrecLandMode::on_active()
{
_local_pos_sub.update();
_prec_land.setAcceptanceRadius(_navigator->get_acceptance_radius());
_prec_land.update();
PrecLand::Output prec_land_output{_prec_land.getOutput()};
_mission_item.nav_cmd = prec_land_output.nav_cmd;
_mission_item.lat = prec_land_output.pos_hor.lat;
_mission_item.lon = prec_land_output.pos_hor.lon;
_mission_item.altitude = prec_land_output.alt;
_mission_item.altitude_is_relative = false;
_mission_item.yaw = _local_pos_sub.get().heading;
mission_apply_limitation(_mission_item);
position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
pos_sp_triplet->previous.valid = false;
pos_sp_triplet->next.valid = false;
if (mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current)) {
_navigator->set_position_setpoint_triplet_updated();
}
}

69
src/modules/navigator/precland_mode.h Normal file
View File

@ -0,0 +1,69 @@
/***************************************************************************
*
* Copyright (c) 2017 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.
*
****************************************************************************/
/**
* @file preclandMode.h
*
* Helper class to do precision landing with a landing target
*
* @author Nicolas de Palezieux (Sunflower Labs) <ndepal@gmail.com>
*/
#pragma once
#include "mission_block.h"
#include "precland.h"
#include <uORB/Subscription.hpp>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_global_position.h>
class PrecLandMode : public MissionBlock
{
public:
PrecLandMode(Navigator *navigator);
~PrecLandMode() override = default;
void on_activation() override;
void on_active() override;
void set_mode(PrecLand::PrecLandMode mode) { _prec_land.setMode(mode); };
PrecLand::PrecLandMode get_mode() { return _prec_land.getMode(); };
private:
PrecLand _prec_land;
uORB::SubscriptionData<vehicle_local_position_s> _local_pos_sub{ORB_ID(vehicle_local_position)};
uORB::SubscriptionData<vehicle_global_position_s> _global_pos_sub{ORB_ID(vehicle_global_position)};
};

54
src/modules/navigator/rtl.cpp
View File

@ -69,9 +69,7 @@ RTL::RTL(Navigator *navigator) :
void RTL::on_inactivation()
{
if (_navigator->get_precland()->is_activated()) {
_navigator->get_precland()->on_inactivation();
}
}
void RTL::on_inactive()
@ -303,10 +301,8 @@ void RTL::on_active()
}
if (_rtl_state == RTL_STATE_LAND && _param_rtl_pld_md.get() > 0) {
_navigator->get_precland()->on_active();
} else if (_navigator->get_precland()->is_activated()) {
_navigator->get_precland()->on_inactivation();
// Need to update the position and type on the current setpoint triplet.
set_prec_land_item();
}
// Limit rtl time calculation to 1Hz
@ -566,13 +562,22 @@ void RTL::set_rtl_item()
_mission_item.origin = ORIGIN_ONBOARD;
_mission_item.land_precision = _param_rtl_pld_md.get();
if (_mission_item.land_precision == 1) {
_navigator->get_precland()->set_mode(PrecLandMode::Opportunistic);
_navigator->get_precland()->on_activation();
if (_mission_item.land_precision > 0u && _mission_item.land_precision <= 2u) {
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
} else if (_mission_item.land_precision == 2) {
_navigator->get_precland()->set_mode(PrecLandMode::Required);
_navigator->get_precland()->on_activation();
if (_mission_item.land_precision == 1) {
_precland.setMode(PrecLand::PrecLandMode::Opportunistic);
} else if (_mission_item.land_precision == 2) {
_precland.setMode(PrecLand::PrecLandMode::Required);
}
PrecLand::LandingPosition2D approximate_landing_pos{
.lat = _mission_item.lat,
.lon = _mission_item.lon,
};
_precland.initialize(approximate_landing_pos);
}
mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: land at destination\t");
@ -605,6 +610,29 @@ void RTL::set_rtl_item()
}
}
void RTL::set_prec_land_item()
{
_precland.setAcceptanceRadius(_navigator->get_acceptance_radius());
_precland.update();
PrecLand::Output prec_land_output{_precland.getOutput()};
_mission_item.altitude = prec_land_output.alt;
_mission_item.lat = prec_land_output.pos_hor.lat;
_mission_item.lon = prec_land_output.pos_hor.lon;
_mission_item.nav_cmd = prec_land_output.nav_cmd;
// Convert mission item to current position setpoint and make it valid.
mission_apply_limitation(_mission_item);
position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
pos_sp_triplet->previous.valid = false;
pos_sp_triplet->next.valid = false;
if (mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current)) {
_navigator->set_position_setpoint_triplet_updated();
}
}
void RTL::advance_rtl()
{
// determines if the vehicle should loiter above land

5
src/modules/navigator/rtl.h
View File

@ -45,6 +45,7 @@
#include "navigator_mode.h"
#include "mission_block.h"
#include "precland.h"
#include <uORB/Subscription.hpp>
#include <uORB/topics/home_position.h>
@ -118,6 +119,8 @@ private:
void set_rtl_item();
void set_prec_land_item();
void advance_rtl();
float calculate_return_alt_from_cone_half_angle(float cone_half_angle_deg);
@ -135,6 +138,8 @@ private:
RTLState _rtl_state{RTL_STATE_NONE};
PrecLand _precland;
struct RTLPosition {
double lat;
double lon;