From 6f97f567268d54e2f3c18278cd13f4af3507418c Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 9 May 2019 10:52:18 +1000 Subject: [PATCH] AR_WPNav: add WARN_IF_UNUSED to various methods, fix for same --- libraries/AR_WPNav/AR_WPNav.cpp | 3 +-- libraries/AR_WPNav/AR_WPNav.h | 8 ++++---- 2 files changed, 5 insertions(+), 6 deletions(-) diff --git a/libraries/AR_WPNav/AR_WPNav.cpp b/libraries/AR_WPNav/AR_WPNav.cpp index b14cb18433..b32386c181 100644 --- a/libraries/AR_WPNav/AR_WPNav.cpp +++ b/libraries/AR_WPNav/AR_WPNav.cpp @@ -179,8 +179,7 @@ bool AR_WPNav::set_desired_location_NED(const Vector3f& destination, float next_ // apply offset destination_ned.offset(destination.x, destination.y); - set_desired_location(destination_ned, next_leg_bearing_cd); - return true; + return set_desired_location(destination_ned, next_leg_bearing_cd); } // calculate vehicle stopping point using current location, velocity and maximum acceleration diff --git a/libraries/AR_WPNav/AR_WPNav.h b/libraries/AR_WPNav/AR_WPNav.h index 1979c1a040..797f4fd500 100644 --- a/libraries/AR_WPNav/AR_WPNav.h +++ b/libraries/AR_WPNav/AR_WPNav.h @@ -38,13 +38,13 @@ public: // set desired location // next_leg_bearing_cd should be heading to the following waypoint (used to slow the vehicle in order to make the turn) - bool set_desired_location(const struct Location& destination, float next_leg_bearing_cd = AR_WPNAV_HEADING_UNKNOWN); + bool set_desired_location(const struct Location& destination, float next_leg_bearing_cd = AR_WPNAV_HEADING_UNKNOWN) WARN_IF_UNUSED; // set desired location to a reasonable stopping point, return true on success - bool set_desired_location_to_stopping_location(); + bool set_desired_location_to_stopping_location() WARN_IF_UNUSED; // set desired location as offset from the EKF origin, return true on success - bool set_desired_location_NED(const Vector3f& destination, float next_leg_bearing_cd = AR_WPNAV_HEADING_UNKNOWN); + bool set_desired_location_NED(const Vector3f& destination, float next_leg_bearing_cd = AR_WPNAV_HEADING_UNKNOWN) WARN_IF_UNUSED; // true if vehicle has reached desired location. defaults to true because this is normally used by missions and we do not want the mission to become stuck bool reached_destination() const { return _reached_destination; } @@ -94,7 +94,7 @@ private: // calculate stopping location using current position and attitude controller provided maximum deceleration // returns true on success, false on failure - bool get_stopping_location(Location& stopping_loc); + bool get_stopping_location(Location& stopping_loc) WARN_IF_UNUSED; // returns true if vehicle should pivot turn at next waypoint bool use_pivot_steering_at_next_WP(float yaw_error_cd) const;