From 8a6aa24525b2725ea073fd3806946ca00522ba24 Mon Sep 17 00:00:00 2001 From: Daniel Ricketts Date: Mon, 20 Jun 2016 10:13:57 +0900 Subject: [PATCH] AC_WPNav: use AC_Avoidance to stop at fence during Loiter --- libraries/AC_WPNav/AC_WPNav.cpp | 5 +++++ libraries/AC_WPNav/AC_WPNav.h | 5 +++++ 2 files changed, 10 insertions(+) diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp index ca4f1985da..55ee045bff 100644 --- a/libraries/AC_WPNav/AC_WPNav.cpp +++ b/libraries/AC_WPNav/AC_WPNav.cpp @@ -304,6 +304,11 @@ void AC_WPNav::calc_loiter_desired_velocity(float nav_dt, float ekfGndSpdLimit) desired_vel.y = desired_vel.y * gnd_speed_limit_cms / horizSpdDem; } + // Limit the velocity to prevent fence violations + if (_avoid != NULL) { + _avoid->adjust_velocity(_pos_control.get_pos_xy_kP(), _loiter_accel_cmss, desired_vel); + } + // send adjusted feed forward velocity back to position controller _pos_control.set_desired_velocity_xy(desired_vel.x,desired_vel.y); } diff --git a/libraries/AC_WPNav/AC_WPNav.h b/libraries/AC_WPNav/AC_WPNav.h index 15fab96caa..af0fbbc79a 100644 --- a/libraries/AC_WPNav/AC_WPNav.h +++ b/libraries/AC_WPNav/AC_WPNav.h @@ -9,6 +9,7 @@ #include // Position control library #include // Attitude control library #include +#include // Stop at fence library // loiter maximum velocities and accelerations #define WPNAV_ACCELERATION 100.0f // defines the default velocity vs distant curve. maximum acceleration in cm/s/s that position controller asks for from acceleration controller @@ -59,6 +60,9 @@ public: /// provide pointer to terrain database void set_terrain(AP_Terrain* terrain_ptr) { _terrain = terrain_ptr; } + /// provide pointer to avoidance library + void set_avoidance(AC_Avoid* avoid_ptr) { _avoid = avoid_ptr; } + /// provide rangefinder altitude void set_rangefinder_alt(bool use, bool healthy, float alt_cm) { _rangefinder_use = use; _rangefinder_healthy = healthy; _rangefinder_alt_cm = alt_cm; } @@ -315,6 +319,7 @@ protected: AC_PosControl& _pos_control; const AC_AttitudeControl& _attitude_control; AP_Terrain *_terrain = NULL; + AC_Avoid *_avoid = NULL; // parameters AP_Float _loiter_speed_cms; // maximum horizontal speed in cm/s while in loiter