dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Navigator: take vehicle position for first-WP-distance-check 

And only use Home position if current vehicle position is unknown.

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer 2023-03-07 22:26:09 +01:00
parent 4d2a31afe3
commit d8ed35c422
3 changed files with 88 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

38
src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2022 PX4 Development Team. All rights reserved.
* Copyright (c) 2022-2023 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
@ -50,6 +50,7 @@ void FeasibilityChecker::reset()
_is_landed = false;
_home_alt_msl = NAN;
_home_lat_lon = matrix::Vector2d((double)NAN, (double)NAN);
_current_position_lat_lon = matrix::Vector2d((double)NAN, (double)NAN);
_vehicle_type = VehicleType::RotaryWing;
_mission_validity_failed = false;
@ -119,6 +120,12 @@ void FeasibilityChecker::updateData()
_is_landed = land_detected.landed;
}
if (_vehicle_global_position_sub.updated()) {
vehicle_global_position_s vehicle_global_position = {};
_vehicle_global_position_sub.copy(&vehicle_global_position);
_current_position_lat_lon = matrix::Vector2d(vehicle_global_position.lat, vehicle_global_position.lon);
}
param_t handle = param_find("FW_LND_ANG");
if (handle != PARAM_INVALID) {
@ -189,7 +196,7 @@ void FeasibilityChecker::doCommonChecks(mission_item_s &mission_item, const int
}
if (!_distance_first_waypoint_failed) {
_distance_first_waypoint_failed = !checkDistanceToFirstWaypoint(mission_item);
_distance_first_waypoint_failed = !checkHorizontalDistanceToFirstWaypoint(mission_item);
}
if (!_below_home_alt_failed) {
@ -595,21 +602,29 @@ bool FeasibilityChecker::checkTakeoffLandAvailable()
}
bool FeasibilityChecker::checkDistanceToFirstWaypoint(mission_item_s &mission_item)
bool FeasibilityChecker::checkHorizontalDistanceToFirstWaypoint(mission_item_s &mission_item)
{
if (_param_mis_dist_1wp <= 0.0f || !_home_lat_lon.isAllFinite()) {
/* param not set, check is ok */
return true;
matrix::Vector2d position_reference = matrix::Vector2d((double)NAN, (double)NAN);
// take last known vehicle global_position, or Home position if not available
if (_current_position_lat_lon.isAllFinite()) {
position_reference = _current_position_lat_lon;
} else if (_home_lat_lon.isAllFinite()) {
position_reference = _home_lat_lon;
}
if (!_first_waypoint_found && MissionBlock::item_contains_position(mission_item)) {
if (_param_mis_dist_1wp > FLT_EPSILON &&
position_reference.isAllFinite() &&
!_first_waypoint_found &&
MissionBlock::item_contains_position(mission_item)) {
_first_waypoint_found = true;
/* check distance from current position to item */
float dist_to_1wp = get_distance_to_next_waypoint(
mission_item.lat, mission_item.lon,
_home_lat_lon(0), _home_lat_lon(1));
const float dist_to_1wp = get_distance_to_next_waypoint(
mission_item.lat, mission_item.lon,
position_reference(0), position_reference(1));
if (dist_to_1wp < _param_mis_dist_1wp) {
@ -627,7 +642,6 @@ bool FeasibilityChecker::checkDistanceToFirstWaypoint(mission_item_s &mission_it
}
}
/* no waypoints found in mission, then we will not fly far away */
return true;
}

12
src/modules/navigator/MissionFeasibility/FeasibilityChecker.hpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2022 PX4 Development Team. All rights reserved.
* Copyright (c) 2022-2023 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
@ -37,6 +37,7 @@
#include <mathlib/mathlib.h>
#include <uORB/topics/home_position.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/Subscription.hpp>
#include <px4_platform_common/module_params.h>
@ -96,6 +97,7 @@ private:
uORB::Subscription _home_pos_sub{ORB_ID(home_position)};
uORB::Subscription _status_sub{ORB_ID(vehicle_status)};
uORB::Subscription _land_detector_sub{ORB_ID(vehicle_land_detected)};
uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position)};
// parameters
float _param_fw_lnd_ang{0.f};
@ -106,6 +108,7 @@ private:
bool _is_landed{false};
float _home_alt_msl{NAN};
matrix::Vector2d _home_lat_lon = matrix::Vector2d((double)NAN, (double)NAN);
matrix::Vector2d _current_position_lat_lon = matrix::Vector2d((double)NAN, (double)NAN);
VehicleType _vehicle_type{VehicleType::RotaryWing};
// internal flags to keep track of which checks failed
@ -129,7 +132,7 @@ private:
int _landing_approach_index{-1};
mission_item_s _mission_item_previous = {};
// internal checkDistanceToFirstWaypoint variables
// internal checkHorizontalDistanceToFirstWaypoint variables
bool _first_waypoint_found{false};
// internal checkDistancesBetweenWaypoints variables
@ -171,12 +174,13 @@ private:
bool checkLandPatternValidity(mission_item_s &mission_item, const int current_index, const int last_index);
/**
* @brief Check distance to first waypoint.
* @brief Check distance to first waypoint from current vehicle position.
* Use Home position instead of vehicle position if vehicle position is invalid.
*
* @param mission_item The current mission item
* @return False if the check failed.
*/
bool checkDistanceToFirstWaypoint(mission_item_s &mission_item);
bool checkHorizontalDistanceToFirstWaypoint(mission_item_s &mission_item);
/**
* @brief Check distances between waypoints

64
src/modules/navigator/MissionFeasibility/FeasibilityCheckerTest.cpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2022 PX4 Development Team. All rights reserved.
* Copyright (C) 2022-2023 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
@ -33,8 +33,6 @@
#include <gtest/gtest.h>
#include "FeasibilityChecker.hpp"
#include <uORB/topics/home_position.h>
#include <uORB/topics/vehicle_land_detected.h>
#include <lib/geo/geo.h>
@ -67,6 +65,15 @@ public:
orb_publish(ORB_ID(home_position), home_pub, &home);
}
void publishCurrentPosition(double lat, double lon)
{
vehicle_global_position_s gpos = {};
gpos.lat = lat;
gpos.lon = lon;
orb_advert_t gpos_pub = orb_advertise(ORB_ID(vehicle_global_position), &gpos);
orb_publish(ORB_ID(vehicle_global_position), gpos_pub, &gpos);
}
void publishLanded(bool landed)
{
vehicle_land_detected_s land_detected = {};
@ -125,35 +132,72 @@ TEST_F(FeasibilityCheckerTest, mission_item_validity)
TEST_F(FeasibilityCheckerTest, check_dist_first_waypoint)
{
// GIVEN: MIS_DIST_1WP set to 500m
TestFeasibilityChecker checker;
checker.publishLanded(true);
checker.publishHomePosition(0, 0, 0);
param_t param = param_handle(px4::params::MIS_DIST_1WP);
float max_dist = 500.0f;
param_set(param, &max_dist);
checker.paramsChanged();
mission_item_s mission_item = {};
mission_item.nav_cmd = NAV_CMD_WAYPOINT;
double lat_new, lon_new;
// GIVEN: no valid Current position, no valid Home
// THEN: always pass
checker.processNextItem(mission_item, 0, 1);
ASSERT_EQ(checker.someCheckFailed(), false);
// GIVEN: no valid current position, but valid Home. First WP 501m away from Home
checker.reset();
checker.publishLanded(true);
checker.publishHomePosition(0, 0, 0);
waypoint_from_heading_and_distance(mission_item.lat, mission_item.lon, 0, 501, &lat_new, &lon_new);
mission_item.lat = lat_new;
mission_item.lon = lon_new;
checker.processNextItem(mission_item, 0, 1);
// THEN: check should fail
checker.processNextItem(mission_item, 0, 1);
ASSERT_EQ(checker.someCheckFailed(), true);
// BUT WHEN: no valid current position, but valid Home. First WP 499m away from Home
checker.reset();
checker.publishLanded(true);
checker.publishHomePosition(0, 0, 0);
waypoint_from_heading_and_distance(0, 0, 0, 499, &lat_new, &lon_new);
mission_item.lat = lat_new;
mission_item.lon = lon_new;
checker.processNextItem(mission_item, 0, 1);
// THEN: pass
checker.processNextItem(mission_item, 0, 1);
ASSERT_EQ(checker.someCheckFailed(), false);
// BUT WHEN: valid current position, valid Home, first WP 501m away from current pos
checker.reset();
checker.publishLanded(true);
checker.publishHomePosition(0, 0, 0);
checker.publishCurrentPosition(0, 0);
waypoint_from_heading_and_distance(0, 0, 0, 501, &lat_new, &lon_new);
mission_item.lat = lat_new;
mission_item.lon = lon_new;
// THEN: fail
checker.processNextItem(mission_item, 0, 1);
ASSERT_EQ(checker.someCheckFailed(), true);
// BUT WHEN: valid current position, valid Home, fist WP 499m away from current but more from Home
checker.reset();
checker.publishLanded(true);
checker.publishHomePosition(10, 20, 0); // random position far away
checker.publishCurrentPosition(0, 0);
waypoint_from_heading_and_distance(0, 0, 0, 499, &lat_new, &lon_new);
mission_item.lat = lat_new;
mission_item.lon = lon_new;
// THEN: pass
checker.processNextItem(mission_item, 0, 1);
ASSERT_EQ(checker.someCheckFailed(), false);
}