Rover: improve cruise speed/throttle learning. fix #9777

APMrover2/Log.cpp

@@ -393,7 +393,7 @@ void Rover::Log_Write_Throttle() {}
 void Rover::Log_Write_Rangefinder() {}
 void Rover::Log_Write_RC(void) {}
 void Rover::Log_Write_Steering() {}
-void Rover::Log_Write_WheelEncoder() {}\
+void Rover::Log_Write_WheelEncoder() {}
 void Rover::Log_Write_Vehicle_Startup_Messages() {}
 
 #endif  // LOGGING_ENABLED

APMrover2/RC_Channel.cpp

@@ -120,8 +120,6 @@ void RC_Channel_Rover::do_aux_function(const aux_func_t ch_option, const aux_swi
     case LEARN_CRUISE:
         if (ch_flag == HIGH) {
             rover.cruise_learn_start();
-        } else if (ch_flag == LOW) {
-            rover.cruise_learn_complete();
         }
         break;
 

APMrover2/Rover.h

@@ -357,11 +357,13 @@ private:
     ModeSimple mode_simple;
 
     // cruise throttle and speed learning
-    struct {
-        bool learning;
+    typedef struct {
         LowPassFilterFloat speed_filt = LowPassFilterFloat(2.0f);
         LowPassFilterFloat throttle_filt = LowPassFilterFloat(2.0f);
-    } cruise_learn;
+        uint32_t learn_start_ms;
+        uint32_t log_count;
+    } cruise_learn_t;
+    cruise_learn_t cruise_learn;
 
     // sailboat variables
     enum Sailboat_Tack {
@@ -415,6 +417,7 @@ private:
     void cruise_learn_start();
     void cruise_learn_update();
     void cruise_learn_complete();
+    void log_write_cruise_learn();
 
     // ekf_check.cpp
     void ekf_check();

APMrover2/cruise_learn.cpp

@@ -6,16 +6,17 @@ void Rover::cruise_learn_start()
     // if disarmed or no speed available do nothing
     float speed;
     if (!arming.is_armed() || !g2.attitude_control.get_forward_speed(speed)) {
-        cruise_learn.learning = false;
+        cruise_learn.learn_start_ms = 0;
         gcs().send_text(MAV_SEVERITY_CRITICAL, "Cruise Learning NOT started");
         return;
     }
     // start learning
-    cruise_learn.learning = true;
     cruise_learn.speed_filt.reset(speed);
     cruise_learn.throttle_filt.reset(g2.motors.get_throttle());
+    cruise_learn.learn_start_ms = AP_HAL::millis();
+    cruise_learn.log_count = 0;
+    log_write_cruise_learn();
     gcs().send_text(MAV_SEVERITY_CRITICAL, "Cruise Learning started");
-    // To-Do: add dataflash logging of learning started event
 }
 
 // update cruise learning with latest speed and throttle
@@ -23,10 +24,19 @@ void Rover::cruise_learn_start()
 void Rover::cruise_learn_update()
 {
     float speed;
-    if (cruise_learn.learning && g2.attitude_control.get_forward_speed(speed)) {
+    if (cruise_learn.learn_start_ms > 0 && 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);
+        // 10Hz logging
+        if (cruise_learn.log_count % 5 == 0) {
+            log_write_cruise_learn();
+        }
+        cruise_learn.log_count += 1;
+        // check how long it took to learn
+        if (AP_HAL::millis() - cruise_learn.learn_start_ms >= 2000) {
+            cruise_learn_complete();
+        }
         return;
     }
 }
@@ -35,7 +45,7 @@ void Rover::cruise_learn_update()
 void Rover::cruise_learn_complete()
 {
     // when switch is moved low, save learned cruise value
-    if (cruise_learn.learning) {
+    if (cruise_learn.learn_start_ms > 0) {
         const float thr = cruise_learn.throttle_filt.get();
         const float speed = cruise_learn.speed_filt.get();
         if (thr >= 10.0f && thr <= 100.0f && is_positive(speed)) {
@@ -45,7 +55,17 @@ void Rover::cruise_learn_complete()
         } else {
             gcs().send_text(MAV_SEVERITY_CRITICAL, "Cruise Learning failed");
         }
-        cruise_learn.learning = false;
-        // To-Do: add dataflash logging of learning completion event
+        cruise_learn.learn_start_ms = 0;
+        log_write_cruise_learn();
     }
 }
+
+// logging for cruise learn
+void Rover::log_write_cruise_learn()
+{
+    DataFlash_Class::instance()->Log_Write("CRSE", "TimeUS,State,Speed,Throttle", "Qbff",
+                                            AP_HAL::micros64,
+                                            cruise_learn.learn_start_ms > 0,
+                                            cruise_learn.speed_filt.get(),
+                                            cruise_learn.throttle_filt.get());
+}