From b7a748df88a43e9b3cd27526da7dc2f4571ec66d Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 11 Jul 2019 11:11:46 +0900 Subject: [PATCH] Copter: auto's wp_start accepts terrain alts --- ArduCopter/mode.h | 2 +- ArduCopter/mode_auto.cpp | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 07ef41adc1..234eb0ff1c 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -313,7 +313,7 @@ public: bool loiter_start(); void rtl_start(); void takeoff_start(const Location& dest_loc); - void wp_start(const Vector3f& destination); + void wp_start(const Vector3f& destination, bool terrain_alt); void wp_start(const Location& dest_loc); void land_start(); void land_start(const Vector3f& destination); diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index 142886dcc2..656b2ea95e 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -189,12 +189,12 @@ void ModeAuto::takeoff_start(const Location& dest_loc) } // auto_wp_start - initialises waypoint controller to implement flying to a particular destination -void ModeAuto::wp_start(const Vector3f& destination) +void ModeAuto::wp_start(const Vector3f& destination, bool terrain_alt) { _mode = Auto_WP; // initialise wpnav (no need to check return status because terrain data is not used) - wp_nav->set_wp_destination(destination, false); + wp_nav->set_wp_destination(destination, terrain_alt); // initialise yaw // To-Do: reset the yaw only when the previous navigation command is not a WP. this would allow removing the special check for ROI @@ -741,7 +741,7 @@ void ModeAuto::takeoff_run() auto_takeoff_run(); if (wp_nav->reached_wp_destination()) { const Vector3f target = wp_nav->get_wp_destination(); - wp_start(target); + wp_start(target, wp_nav->origin_and_destination_are_terrain_alt()); } }