mirror of https://github.com/ArduPilot/ardupilot
AP_SmartRTL: renamed from SafeRTL
This commit is contained in:
squilter
Randy Mackay
parent
4415a6b052
commit
cb0f50a1c8
|
|
@ -11,18 +11,18 @@
|
|||
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include "AP_SafeRTL.h"
|
||||
#include "AP_SmartRTL.h"
|
||||
|
||||
extern const AP_HAL::HAL& hal;
|
||||
|
||||
const AP_Param::GroupInfo AP_SafeRTL::var_info[] = {
|
||||
const AP_Param::GroupInfo AP_SmartRTL::var_info[] = {
|
||||
// @Param: ACCURACY
|
||||
// @DisplayName: SafeRTL accuracy
|
||||
// @Description: SafeRTL accuracy. The minimum distance between points.
|
||||
// @Units: m
|
||||
// @Range: 0 10
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("ACCURACY", 0, AP_SafeRTL, _accuracy, SAFERTL_ACCURACY_DEFAULT),
|
||||
AP_GROUPINFO("ACCURACY", 0, AP_SmartRTL, _accuracy, SAFERTL_ACCURACY_DEFAULT),
|
||||
|
||||
// @Param: POINTS
|
||||
// @DisplayName: SafeRTL maximum number of points on path
|
||||
|
|
@ -30,7 +30,7 @@ const AP_Param::GroupInfo AP_SafeRTL::var_info[] = {
|
|||
// @Range: 0 500
|
||||
// @User: Advanced
|
||||
// @RebootRequired: True
|
||||
AP_GROUPINFO("POINTS", 1, AP_SafeRTL, _points_max, SAFERTL_POINTS_DEFAULT),
|
||||
AP_GROUPINFO("POINTS", 1, AP_SmartRTL, _points_max, SAFERTL_POINTS_DEFAULT),
|
||||
|
||||
AP_GROUPEND
|
||||
};
|
||||
|
|
@ -72,7 +72,7 @@ const AP_Param::GroupInfo AP_SafeRTL::var_info[] = {
|
|||
* seconds before initiating the return journey.
|
||||
*/
|
||||
|
||||
AP_SafeRTL::AP_SafeRTL(const AP_AHRS& ahrs, bool example_mode) :
|
||||
AP_SmartRTL::AP_SmartRTL(const AP_AHRS& ahrs, bool example_mode) :
|
||||
_ahrs(ahrs),
|
||||
_example_mode(example_mode)
|
||||
{
|
||||
|
|
@ -81,7 +81,7 @@ AP_SafeRTL::AP_SafeRTL(const AP_AHRS& ahrs, bool example_mode) :
|
|||
}
|
||||
|
||||
// initialise safe rtl including setting up background processes
|
||||
void AP_SafeRTL::init()
|
||||
void AP_SmartRTL::init()
|
||||
{
|
||||
// protect against repeated call to init
|
||||
if (_path != nullptr) {
|
||||
|
|
@ -126,18 +126,18 @@ void AP_SafeRTL::init()
|
|||
// when running the example sketch, we want the cleanup tasks to run when we tell them to, not in the background (so that they can be timed.)
|
||||
if (!_example_mode){
|
||||
// register background cleanup to run in IO thread
|
||||
hal.scheduler->register_io_process(FUNCTOR_BIND_MEMBER(&AP_SafeRTL::run_background_cleanup, void));
|
||||
hal.scheduler->register_io_process(FUNCTOR_BIND_MEMBER(&AP_SmartRTL::run_background_cleanup, void));
|
||||
}
|
||||
}
|
||||
|
||||
// returns number of points on the path
|
||||
uint16_t AP_SafeRTL::get_num_points() const
|
||||
uint16_t AP_SmartRTL::get_num_points() const
|
||||
{
|
||||
return _path_points_count;
|
||||
}
|
||||
|
||||
// get next point on the path to home, returns true on success
|
||||
bool AP_SafeRTL::pop_point(Vector3f& point)
|
||||
bool AP_SmartRTL::pop_point(Vector3f& point)
|
||||
{
|
||||
// check we are active
|
||||
if (!_active) {
|
||||
|
|
@ -167,14 +167,14 @@ bool AP_SafeRTL::pop_point(Vector3f& point)
|
|||
}
|
||||
|
||||
// clear return path and set home location. This should be called as part of the arming procedure
|
||||
void AP_SafeRTL::reset_path(bool position_ok)
|
||||
void AP_SmartRTL::reset_path(bool position_ok)
|
||||
{
|
||||
Vector3f current_pos;
|
||||
position_ok &= _ahrs.get_relative_position_NED_origin(current_pos);
|
||||
reset_path(position_ok, current_pos);
|
||||
}
|
||||
|
||||
void AP_SafeRTL::reset_path(bool position_ok, const Vector3f& current_pos)
|
||||
void AP_SmartRTL::reset_path(bool position_ok, const Vector3f& current_pos)
|
||||
{
|
||||
if (_path == nullptr) {
|
||||
return;
|
||||
|
|
@ -206,7 +206,7 @@ void AP_SafeRTL::reset_path(bool position_ok, const Vector3f& current_pos)
|
|||
}
|
||||
|
||||
// call this at 3hz (or higher) regardless of what mode the vehicle is in
|
||||
void AP_SafeRTL::update(bool position_ok, bool save_position)
|
||||
void AP_SmartRTL::update(bool position_ok, bool save_position)
|
||||
{
|
||||
if (!_active || !save_position) {
|
||||
return;
|
||||
|
|
@ -217,7 +217,7 @@ void AP_SafeRTL::update(bool position_ok, bool save_position)
|
|||
update(position_ok, current_pos);
|
||||
}
|
||||
|
||||
void AP_SafeRTL::update(bool position_ok, const Vector3f& current_pos)
|
||||
void AP_SmartRTL::update(bool position_ok, const Vector3f& current_pos)
|
||||
{
|
||||
if (!_active) {
|
||||
return;
|
||||
|
|
@ -245,7 +245,7 @@ void AP_SafeRTL::update(bool position_ok, const Vector3f& current_pos)
|
|||
// request thorough cleanup including simplification, pruning and removal of all unnecessary points
|
||||
// returns true if the thorough cleanup was completed, false if it has not yet completed
|
||||
// this method should be called repeatedly until it returns true before initiating the return journey
|
||||
bool AP_SafeRTL::request_thorough_cleanup(ThoroughCleanupType clean_type)
|
||||
bool AP_SmartRTL::request_thorough_cleanup(ThoroughCleanupType clean_type)
|
||||
{
|
||||
// this should never happen but just in case
|
||||
if (!_active) {
|
||||
|
|
@ -271,7 +271,7 @@ bool AP_SafeRTL::request_thorough_cleanup(ThoroughCleanupType clean_type)
|
|||
}
|
||||
|
||||
// cancel request for thorough cleanup
|
||||
void AP_SafeRTL::cancel_request_for_thorough_cleanup()
|
||||
void AP_SmartRTL::cancel_request_for_thorough_cleanup()
|
||||
{
|
||||
_thorough_clean_request_ms = 0;
|
||||
}
|
||||
|
|
@ -281,7 +281,7 @@ void AP_SafeRTL::cancel_request_for_thorough_cleanup()
|
|||
//
|
||||
|
||||
// add point to end of path (if necessary), returns true on success
|
||||
bool AP_SafeRTL::add_point(const Vector3f& point)
|
||||
bool AP_SmartRTL::add_point(const Vector3f& point)
|
||||
{
|
||||
// get semaphore
|
||||
if (!_path_sem->take_nonblocking()) {
|
||||
|
|
@ -314,7 +314,7 @@ bool AP_SafeRTL::add_point(const Vector3f& point)
|
|||
}
|
||||
|
||||
// run background cleanup - should be run regularly from the IO thread
|
||||
void AP_SafeRTL::run_background_cleanup()
|
||||
void AP_SmartRTL::run_background_cleanup()
|
||||
{
|
||||
if (!_active) {
|
||||
return;
|
||||
|
|
@ -355,7 +355,7 @@ void AP_SafeRTL::run_background_cleanup()
|
|||
// simplifies the path after SAFERTL_CLEANUP_POINT_TRIGGER points (50 points) have been added OR
|
||||
// SAFERTL_CLEANUP_POINT_MIN (10 points) have been added and the path has less than SAFERTL_CLEANUP_START_MARGIN spaces (10 spaces) remaining
|
||||
// prunes the path if the path has less than SAFERTL_CLEANUP_START_MARGIN spaces (10 spaces) remaining
|
||||
void AP_SafeRTL::routine_cleanup(uint16_t path_points_count, uint16_t path_points_completed_limit)
|
||||
void AP_SmartRTL::routine_cleanup(uint16_t path_points_count, uint16_t path_points_completed_limit)
|
||||
{
|
||||
// if simplify is running, let it run to completion
|
||||
if (!_simplify.complete) {
|
||||
|
|
@ -406,7 +406,7 @@ void AP_SafeRTL::routine_cleanup(uint16_t path_points_count, uint16_t path_point
|
|||
|
||||
// thorough cleanup simplifies and prunes all loops. returns true if the cleanup was completed.
|
||||
// path_points_count is _path_points_count but passed in to avoid having to take the semaphore
|
||||
bool AP_SafeRTL::thorough_cleanup(uint16_t path_points_count, ThoroughCleanupType clean_type)
|
||||
bool AP_SmartRTL::thorough_cleanup(uint16_t path_points_count, ThoroughCleanupType clean_type)
|
||||
{
|
||||
if (clean_type != THOROUGH_CLEAN_PRUNE_ONLY) {
|
||||
// restart simplify if new points have appeared on path
|
||||
|
|
@ -446,7 +446,7 @@ bool AP_SafeRTL::thorough_cleanup(uint16_t path_points_count, ThoroughCleanupTyp
|
|||
|
||||
// Simplifies a 3D path, according to the Ramer-Douglas-Peucker algorithm.
|
||||
// _simplify.complete is set to true when all simplifications on the path have been identified
|
||||
void AP_SafeRTL::detect_simplifications()
|
||||
void AP_SmartRTL::detect_simplifications()
|
||||
{
|
||||
// complete immediately if only one segment
|
||||
if (_simplify.path_points_count < 3) {
|
||||
|
|
@ -520,7 +520,7 @@ void AP_SafeRTL::detect_simplifications()
|
|||
*
|
||||
* reset_pruning should have been called at least once before this function is called to setup the indexes (_prune.i, etc)
|
||||
*/
|
||||
void AP_SafeRTL::detect_loops()
|
||||
void AP_SmartRTL::detect_loops()
|
||||
{
|
||||
// if there are less than 4 points (3 segments), mark complete
|
||||
if (_prune.path_points_count < 4) {
|
||||
|
|
@ -565,7 +565,7 @@ void AP_SafeRTL::detect_loops()
|
|||
|
||||
// restart simplify if new points have been added to path
|
||||
// path_points_count is _path_points_count but passed in to avoid having to take the semaphore
|
||||
void AP_SafeRTL::restart_simplify_if_new_points(uint16_t path_points_count)
|
||||
void AP_SmartRTL::restart_simplify_if_new_points(uint16_t path_points_count)
|
||||
{
|
||||
// any difference in the number of points is because of new points being added to path
|
||||
if (_simplify.path_points_count != path_points_count) {
|
||||
|
|
@ -574,7 +574,7 @@ void AP_SafeRTL::restart_simplify_if_new_points(uint16_t path_points_count)
|
|||
}
|
||||
|
||||
// reset pruning if new points have been simplified
|
||||
void AP_SafeRTL::restart_pruning_if_new_points()
|
||||
void AP_SmartRTL::restart_pruning_if_new_points()
|
||||
{
|
||||
// any difference in the number of points is because of new points being added to path
|
||||
if (_prune.path_points_count != _simplify.path_points_completed) {
|
||||
|
|
@ -583,7 +583,7 @@ void AP_SafeRTL::restart_pruning_if_new_points()
|
|||
}
|
||||
|
||||
// restart simplification algorithm so that it will check new points in the path
|
||||
void AP_SafeRTL::restart_simplification(uint16_t path_points_count)
|
||||
void AP_SmartRTL::restart_simplification(uint16_t path_points_count)
|
||||
{
|
||||
_simplify.complete = false;
|
||||
_simplify.removal_required = false;
|
||||
|
|
@ -593,14 +593,14 @@ void AP_SafeRTL::restart_simplification(uint16_t path_points_count)
|
|||
}
|
||||
|
||||
// reset simplification algorithm so that it will re-check all points in the path
|
||||
void AP_SafeRTL::reset_simplification()
|
||||
void AP_SmartRTL::reset_simplification()
|
||||
{
|
||||
restart_simplification(0);
|
||||
_simplify.path_points_completed = 0;
|
||||
}
|
||||
|
||||
// restart pruning algorithm to check new points that have arrived
|
||||
void AP_SafeRTL::restart_pruning(uint16_t path_points_count)
|
||||
void AP_SmartRTL::restart_pruning(uint16_t path_points_count)
|
||||
{
|
||||
_prune.complete = false;
|
||||
_prune.i = (path_points_count > 0) ? path_points_count - 1 : 0;
|
||||
|
|
@ -609,7 +609,7 @@ void AP_SafeRTL::restart_pruning(uint16_t path_points_count)
|
|||
}
|
||||
|
||||
// reset pruning algorithm so that it will re-check all points in the path
|
||||
void AP_SafeRTL::reset_pruning()
|
||||
void AP_SmartRTL::reset_pruning()
|
||||
{
|
||||
restart_pruning(0);
|
||||
_prune.loops_count = 0; // clear the loops that we've recorded
|
||||
|
|
@ -617,7 +617,7 @@ void AP_SafeRTL::reset_pruning()
|
|||
}
|
||||
|
||||
// remove all simplify-able points from the path
|
||||
void AP_SafeRTL::remove_points_by_simplify_bitmask()
|
||||
void AP_SmartRTL::remove_points_by_simplify_bitmask()
|
||||
{
|
||||
// get semaphore before modifying path
|
||||
if (!_path_sem->take_nonblocking()) {
|
||||
|
|
@ -655,7 +655,7 @@ void AP_SafeRTL::remove_points_by_simplify_bitmask()
|
|||
// remove loops until at least num_point_to_delete have been removed from path
|
||||
// does not necessarily prune all loops
|
||||
// returns false if it failed to remove points (because it could not take semaphore)
|
||||
bool AP_SafeRTL::remove_points_by_loops(uint16_t num_points_to_remove)
|
||||
bool AP_SmartRTL::remove_points_by_loops(uint16_t num_points_to_remove)
|
||||
{
|
||||
// exit immediately if no loops to prune
|
||||
if (_prune.loops_count == 0) {
|
||||
|
|
@ -716,7 +716,7 @@ bool AP_SafeRTL::remove_points_by_loops(uint16_t num_points_to_remove)
|
|||
// add loop to loops array
|
||||
// returns true if loop added successfully, false if loop array is full
|
||||
// checks if loop overlaps with an existing loop, keeps only the longer loop
|
||||
bool AP_SafeRTL::add_loop(uint16_t start_index, uint16_t end_index, const Vector3f& midpoint)
|
||||
bool AP_SmartRTL::add_loop(uint16_t start_index, uint16_t end_index, const Vector3f& midpoint)
|
||||
{
|
||||
// if the buffer is full, return failure
|
||||
if (_prune.loops_count >= _prune.loops_max) {
|
||||
|
|
@ -779,7 +779,7 @@ bool AP_SafeRTL::add_loop(uint16_t start_index, uint16_t end_index, const Vector
|
|||
* This does not matter for the path cleanup algorithm because the pruning will still occur fine between the first
|
||||
* parallel segment and a segment which is directly before or after the second segment.
|
||||
*/
|
||||
AP_SafeRTL::dist_point AP_SafeRTL::segment_segment_dist(const Vector3f &p1, const Vector3f &p2, const Vector3f &p3, const Vector3f &p4)
|
||||
AP_SmartRTL::dist_point AP_SmartRTL::segment_segment_dist(const Vector3f &p1, const Vector3f &p2, const Vector3f &p3, const Vector3f &p4)
|
||||
{
|
||||
const Vector3f line1 = p2-p1;
|
||||
const Vector3f line2 = p4-p3;
|
||||
|
|
@ -817,7 +817,7 @@ AP_SafeRTL::dist_point AP_SafeRTL::segment_segment_dist(const Vector3f &p1, cons
|
|||
}
|
||||
|
||||
// de-activate SafeRTL, send warning to GCS and log to dataflash
|
||||
void AP_SafeRTL::deactivate(SRTL_Actions action, const char *reason)
|
||||
void AP_SmartRTL::deactivate(SRTL_Actions action, const char *reason)
|
||||
{
|
||||
_active = false;
|
||||
log_action(action);
|
||||
|
|
@ -825,7 +825,7 @@ void AP_SafeRTL::deactivate(SRTL_Actions action, const char *reason)
|
|||
}
|
||||
|
||||
// logging
|
||||
void AP_SafeRTL::log_action(SRTL_Actions action, const Vector3f &point)
|
||||
void AP_SmartRTL::log_action(SRTL_Actions action, const Vector3f &point)
|
||||
{
|
||||
if (!_example_mode) {
|
||||
DataFlash_Class::instance()->Log_Write_SRTL(_active, _path_points_count, _path_points_max, action, point);
|
||||
|
|
@ -833,7 +833,7 @@ void AP_SafeRTL::log_action(SRTL_Actions action, const Vector3f &point)
|
|||
}
|
||||
|
||||
// returns true if the two loops overlap (used within add_loop to determine which loops to keep or throw away)
|
||||
bool AP_SafeRTL::loops_overlap(const prune_loop_t &loop1, const prune_loop_t &loop2) const
|
||||
bool AP_SmartRTL::loops_overlap(const prune_loop_t &loop1, const prune_loop_t &loop2) const
|
||||
{
|
||||
// check if loop1 within loop2
|
||||
if (loop1.start_index >= loop2.start_index && loop1.end_index <= loop2.end_index) {
|
||||
|
|
@ -25,17 +25,17 @@
|
|||
#define SAFERTL_PRUNING_LOOP_BUFFER_LEN_MULT 0.25f // pruning loop buffer size as compared to maximum number of points
|
||||
#define SAFERTL_PRUNING_LOOP_TIME_US 200 // maximum time (in microseconds) that the loop finding algorithm will run before returning
|
||||
|
||||
class AP_SafeRTL {
|
||||
class AP_SmartRTL {
|
||||
|
||||
public:
|
||||
|
||||
// constructor, destructor
|
||||
AP_SafeRTL(const AP_AHRS& ahrs, bool example_mode = false);
|
||||
AP_SmartRTL(const AP_AHRS& ahrs, bool example_mode = false);
|
||||
|
||||
// initialise safe rtl including setting up background processes
|
||||
void init();
|
||||
|
||||
// return true if safe_rtl is usable (it may become unusable if the user took off without GPS lock or the path became too long)
|
||||
// return true if smart_rtl is usable (it may become unusable if the user took off without GPS lock or the path became too long)
|
||||
bool is_active() const { return _active; }
|
||||
|
||||
// returns number of points on the path
|
||||
|
|
@ -1,4 +1,4 @@
|
|||
#include "SafeRTL_test.h"
|
||||
#include "SmartRTL_test.h"
|
||||
#include <AP_AHRS/AP_AHRS.h>
|
||||
#include <AP_Baro/AP_Baro.h>
|
||||
#include <AP_Compass/AP_Compass.h>
|
||||
|
|
@ -30,7 +30,7 @@ public:
|
|||
static DummyVehicle vehicle;
|
||||
|
||||
AP_AHRS_NavEKF ahrs(vehicle.ahrs);
|
||||
AP_SafeRTL safe_rtl {ahrs, true};
|
||||
AP_SmartRTL smart_rtl {ahrs, true};
|
||||
|
||||
void setup();
|
||||
void loop();
|
||||
|
|
@ -41,7 +41,7 @@ void setup()
|
|||
{
|
||||
hal.console->printf("SafeRTL test\n");
|
||||
AP_BoardConfig{}.init();
|
||||
safe_rtl.init();
|
||||
smart_rtl.init();
|
||||
}
|
||||
|
||||
void loop()
|
||||
|
|
@ -53,7 +53,7 @@ void loop()
|
|||
|
||||
hal.console->printf("--------------------\n");
|
||||
|
||||
// reset path and upload "test_path_before" to safe_rtl
|
||||
// reset path and upload "test_path_before" to smart_rtl
|
||||
reference_time = AP_HAL::micros();
|
||||
reset();
|
||||
run_time = AP_HAL::micros() - reference_time;
|
||||
|
|
@ -63,8 +63,8 @@ void loop()
|
|||
|
||||
// test simplifications
|
||||
reference_time = AP_HAL::micros();
|
||||
while (!safe_rtl.request_thorough_cleanup(AP_SafeRTL::THOROUGH_CLEAN_SIMPLIFY_ONLY)) {
|
||||
safe_rtl.run_background_cleanup();
|
||||
while (!smart_rtl.request_thorough_cleanup(AP_SmartRTL::THOROUGH_CLEAN_SIMPLIFY_ONLY)) {
|
||||
smart_rtl.run_background_cleanup();
|
||||
}
|
||||
run_time = AP_HAL::micros() - reference_time;
|
||||
check_path(test_path_after_simplifying, "simplify", run_time);
|
||||
|
|
@ -73,8 +73,8 @@ void loop()
|
|||
hal.scheduler->delay(5); // delay 5 milliseconds because request_through_cleanup uses millisecond timestamps
|
||||
reset();
|
||||
reference_time = AP_HAL::micros();
|
||||
while (!safe_rtl.request_thorough_cleanup(AP_SafeRTL::THOROUGH_CLEAN_ALL)) {
|
||||
safe_rtl.run_background_cleanup();
|
||||
while (!smart_rtl.request_thorough_cleanup(AP_SmartRTL::THOROUGH_CLEAN_ALL)) {
|
||||
smart_rtl.run_background_cleanup();
|
||||
}
|
||||
run_time = AP_HAL::micros() - reference_time;
|
||||
check_path(test_path_complete, "simplify and pruning", run_time);
|
||||
|
|
@ -83,27 +83,27 @@ void loop()
|
|||
hal.scheduler->delay(5e3); // 5 seconds
|
||||
}
|
||||
|
||||
// reset path (i.e. clear path and add home) and upload "test_path_before" to safe_rtl
|
||||
// reset path (i.e. clear path and add home) and upload "test_path_before" to smart_rtl
|
||||
void reset()
|
||||
{
|
||||
safe_rtl.reset_path(true, Vector3f{0.0f, 0.0f, 0.0f});
|
||||
smart_rtl.reset_path(true, Vector3f{0.0f, 0.0f, 0.0f});
|
||||
for (Vector3f v : test_path_before) {
|
||||
safe_rtl.update(true, v);
|
||||
smart_rtl.update(true, v);
|
||||
}
|
||||
}
|
||||
|
||||
// compare the vector array passed in with the path held in the safe_rtl object
|
||||
// compare the vector array passed in with the path held in the smart_rtl object
|
||||
void check_path(const std::vector<Vector3f>& correct_path, const char* test_name, uint32_t time_us)
|
||||
{
|
||||
// check number of points
|
||||
bool num_points_match = correct_path.size() == safe_rtl.get_num_points();
|
||||
uint16_t points_to_compare = MIN(correct_path.size(), safe_rtl.get_num_points());
|
||||
bool num_points_match = correct_path.size() == smart_rtl.get_num_points();
|
||||
uint16_t points_to_compare = MIN(correct_path.size(), smart_rtl.get_num_points());
|
||||
|
||||
// check all points match
|
||||
bool points_match = true;
|
||||
uint16_t failure_index = 0;
|
||||
for (uint16_t i = 0; i < points_to_compare; i++) {
|
||||
if (safe_rtl.get_point(i) != correct_path[i]) {
|
||||
if (smart_rtl.get_point(i) != correct_path[i]) {
|
||||
failure_index = i;
|
||||
points_match = false;
|
||||
}
|
||||
|
|
@ -113,12 +113,12 @@ void check_path(const std::vector<Vector3f>& correct_path, const char* test_name
|
|||
hal.console->printf("%s: %s time:%u us\n", test_name, (num_points_match && points_match) ? "success" : "fail", time_us);
|
||||
|
||||
// display number of points
|
||||
hal.console->printf(" expected %u points, got %u\n", (unsigned)correct_path.size(), (unsigned)safe_rtl.get_num_points());
|
||||
hal.console->printf(" expected %u points, got %u\n", (unsigned)correct_path.size(), (unsigned)smart_rtl.get_num_points());
|
||||
|
||||
// display the first failed point and all subsequent points
|
||||
if (!points_match) {
|
||||
for (uint16_t j = failure_index; j < points_to_compare; j++) {
|
||||
const Vector3f& safertl_point = safe_rtl.get_point(j);
|
||||
const Vector3f& safertl_point = smart_rtl.get_point(j);
|
||||
hal.console->printf(" expected point %d to be %4.2f,%4.2f,%4.2f, got %4.2f,%4.2f,%4.2f\n",
|
||||
(int)j,
|
||||
(double)correct_path[j].x,
|
||||
|
|
@ -3,7 +3,7 @@
|
|||
#include <AP_BoardConfig/AP_BoardConfig.h>
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
#include <AP_Math/AP_Math.h>
|
||||
#include <AP_SafeRTL/AP_SafeRTL.h>
|
||||
#include <AP_SmartRTL/AP_SmartRTL.h>
|
||||
#include "../../../../ArduCopter/GCS_Copter.h"
|
||||
|
||||
// vectors defined below:
|
||||
Loading…
Reference in New Issue