Rover: aux switch to learn cruise throttle and speed

APMrover2/APMrover2.cpp

@@ -80,6 +80,7 @@ const AP_Scheduler::Task Rover::scheduler_tasks[] = {
     SCHED_TASK(button_update,           5,    100),
     SCHED_TASK(stats_update,            1,    100),
     SCHED_TASK(crash_check,            10,   1000),
+    SCHED_TASK(cruise_learn_update,    50,     50),
 #if ADVANCED_FAILSAFE == ENABLED
     SCHED_TASK(afs_fs_check,           10,    100),
 #endif

APMrover2/Parameters.cpp

@@ -129,7 +129,7 @@ const AP_Param::Info Rover::var_info[] = {
     // @Param: CH7_OPTION
     // @DisplayName: Channel 7 option
     // @Description: What to do use channel 7 for
-    // @Values: 0:Nothing,1:SaveWaypoint
+    // @Values: 0:Nothing,1:SaveWaypoint,2:LearnCruiseSpeed
     // @User: Standard
     GSCALAR(ch7_option,             "CH7_OPTION",          CH7_OPTION),
 

APMrover2/Rover.h

@@ -71,6 +71,7 @@
 #include <Filter/AverageFilter.h>                   // Mode Filter from Filter library
 #include <Filter/Butter.h>                          // Filter library - butterworth filter
 #include <Filter/Filter.h>                          // Filter library
+#include <Filter/LowPassFilter.h>
 #include <Filter/ModeFilter.h>                      // Mode Filter from Filter library
 #include <RC_Channel/RC_Channel.h>                  // RC Channel Library
 #include <StorageManager/StorageManager.h>
@@ -380,6 +381,13 @@ private:
     ModeSteering mode_steering;
     ModeRTL mode_rtl;
 
+    // cruise throttle and speed learning
+    struct {
+        bool learning;
+        LowPassFilterFloat speed_filt = LowPassFilterFloat(2.0f);
+        LowPassFilterFloat throttle_filt = LowPassFilterFloat(2.0f);
+    } cruise_learn;
+
 private:
 
     // APMrover2.cpp
@@ -452,6 +460,11 @@ private:
     // crash_check.cpp
     void crash_check();
 
+    // cruise_learn.cpp
+    void cruise_learn_start();
+    void cruise_learn_update();
+    void cruise_learn_complete();
+
     // events.cpp
     void update_events(void);
 

APMrover2/control_modes.cpp

@@ -174,6 +174,15 @@ void Rover::read_aux_switch()
             }
         }
         break;
+
+    // learn cruise speed and throttle
+    case CH7_LEARN_CRUISE:
+        if (aux_ch7 == AUX_SWITCH_HIGH) {
+            cruise_learn_start();
+        } else if (aux_ch7 == AUX_SWITCH_LOW) {
+            cruise_learn_complete();
+        }
+        break;
     }
 }
 

APMrover2/cruise_learn.cpp

@@ -0,0 +1,51 @@
+#include "Rover.h"
+
+// start cruise throttle and speed learning
+void Rover::cruise_learn_start()
+{
+    // if disarmed do nothing
+    if (!arming.is_armed()) {
+        cruise_learn.learning = false;
+        gcs().send_text(MAV_SEVERITY_CRITICAL, "Cruise Learning NOT stated");
+        return;
+    }
+    // when switch is high, start learning
+    float speed;
+    if (g2.attitude_control.get_forward_speed(speed)) {
+        cruise_learn.learning = true;
+        cruise_learn.speed_filt.reset(speed);
+        cruise_learn.throttle_filt.reset(g2.motors.get_throttle());
+        gcs().send_text(MAV_SEVERITY_CRITICAL, "Cruise Learning started");
+    }
+}
+
+// update cruise learning with latest speed and throttle
+// should be called at 50hz
+void Rover::cruise_learn_update()
+{
+    float speed;
+    if (cruise_learn.learning && g2.attitude_control.get_forward_speed(speed)) {
+        // update filters with latest speed and throttle
+        cruise_learn.speed_filt.apply(speed, 0.02f);
+        cruise_learn.throttle_filt.apply(g2.motors.get_throttle(), 0.02f);
+        return;
+    }
+}
+
+// complete cruise learning and save results
+void Rover::cruise_learn_complete()
+{
+    // when switch is moved low, save learned cruise value
+    if (cruise_learn.learning) {
+        float thr = cruise_learn.throttle_filt.get();
+        float speed = cruise_learn.speed_filt.get();
+        if (thr >= 10.0f && thr <= 100.0f && is_positive(speed)) {
+            g.throttle_cruise.set_and_save(thr);
+            g.speed_cruise.set_and_save(speed);
+            gcs().send_text(MAV_SEVERITY_CRITICAL, "Cruise Learned: Thr:%d Speed:%3.1f", (int)g.throttle_cruise, (double)g.speed_cruise);
+        } else {
+            gcs().send_text(MAV_SEVERITY_CRITICAL, "Cruise Learning failed");
+        }
+        cruise_learn.learning = false;
+    }
+}

APMrover2/defines.h

@@ -15,8 +15,9 @@
 
 // CH 7 control
 enum ch7_option {
-    CH7_DO_NOTHING = 0,
-    CH7_SAVE_WP    = 1
+    CH7_DO_NOTHING      = 0,
+    CH7_SAVE_WP         = 1,
+    CH7_LEARN_CRUISE    = 2
 };
 
 // HIL enumerations