aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AC_WPNav: use AC_Avoidance to stop at fence during Loiter

Browse Source
This commit is contained in:
Daniel Ricketts 2016-06-20 10:13:57 +09:00 committed by Randy Mackay
parent ff7bc7c0cb
commit 8a6aa24525
2 changed files with 10 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
libraries/AC_WPNav/AC_WPNav.cpp
View File

@ -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);
}

5
libraries/AC_WPNav/AC_WPNav.h
View File

@ -9,6 +9,7 @@
#include <AC_AttitudeControl/AC_PosControl.h> // Position control library
#include <AC_AttitudeControl/AC_AttitudeControl.h> // Attitude control library
#include <AP_Terrain/AP_Terrain.h>
#include <AC_Avoidance/AC_Avoid.h> // 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