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

AC_WPNav: run loiter and wp nav at 50hz on Pixhawk

Browse Source
This commit is contained in:
Randy Mackay 2014-04-19 22:30:17 +09:00
parent 8ac14023b4
commit d382fa51ee
2 changed files with 11 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AC_WPNav/AC_WPNav.cpp
View File

@ -221,7 +221,7 @@ void AC_WPNav::update_loiter()
float dt = (now - _loiter_last_update) / 1000.0f;
// reset step back to 0 if 0.1 seconds has passed and we completed the last full cycle
if (dt > 0.095f) {
if (dt >= WPNAV_LOITER_UPDATE_TIME) {
// double check dt is reasonable
if (dt >= 1.0f) {
dt = 0.0;
@ -445,7 +445,7 @@ void AC_WPNav::update_wpnav()
float dt = (now - _wp_last_update) / 1000.0f;
// reset step back to 0 if 0.1 seconds has passed and we completed the last full cycle
if (dt > 0.095f) {
if (dt >= WPNAV_WP_UPDATE_TIME) {
// double check dt is reasonable
if (dt >= 1.0f) {
dt = 0.0;
@ -649,7 +649,7 @@ void AC_WPNav::update_spline()
float dt = (now - _wp_last_update) / 1000.0f;
// reset step back to 0 if 0.1 seconds has passed and we completed the last full cycle
if (dt > 0.095f) {
if (dt >= WPNAV_WP_UPDATE_TIME) {
// double check dt is reasonable
if (dt >= 1.0f) {
dt = 0.0;

8
libraries/AC_WPNav/AC_WPNav.h
View File

@ -33,6 +33,14 @@
#define WPNAV_MIN_LEASH_LENGTH 100.0f // minimum leash lengths in cm
#if HAL_CPU_CLASS >= HAL_CPU_CLASS_75
# define WPNAV_LOITER_UPDATE_TIME 0.020f // 50hz update rate on high speed CPUs (Pixhawk, Flymaple)
# define WPNAV_WP_UPDATE_TIME 0.020f // 50hz update rate on high speed CPUs (Pixhawk, Flymaple)
#else
# define WPNAV_LOITER_UPDATE_TIME 0.095f // 10hz update rate on low speed CPUs (APM1, APM2)
# define WPNAV_WP_UPDATE_TIME 0.095f // 10hz update rate on low speed CPUs (APM1, APM2)
#endif
class AC_WPNav
{
public: