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

libraries/AC_WPNav/AC_WPNav.cpp

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

libraries/AC_WPNav/AC_WPNav.h

@@ -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: