Rover: set ahrs flyforward if thr over 15% for 2 seconds

or if throttle is positive and desired speed over 0.5m/s

APMrover2/APMrover2.cpp

@@ -150,11 +150,8 @@ void Rover::ahrs_update()
     gcs_update();
 #endif
 
-    // when in reverse we need to tell AHRS not to assume we are a
-    // 'fly forward' vehicle, otherwise it will see a large
-    // discrepancy between the mag and the GPS heading and will try to
-    // correct for it, leading to a large yaw error
-    ahrs.set_fly_forward(!in_reverse);
+    // AHRS may use movement to calculate heading
+    update_ahrs_flyforward();
 
     ahrs.update();
 

APMrover2/Rover.h

@@ -327,8 +327,8 @@ private:
     // This is the time between calls to the DCM algorithm and is the Integration time for the gyros.
     float G_Dt;
 
-    // set if we are driving backwards
-    bool in_reverse;
+    // flyforward timer
+    uint32_t flyforward_start_ms;
 
     // true if pivoting (set by use_pivot_steering)
     bool pivot_steering_active;
@@ -525,7 +525,7 @@ private:
     // system.cpp
     void init_ardupilot();
     void startup_ground(void);
-    void set_reverse(bool reverse);
+    void update_ahrs_flyforward();
     bool set_mode(Mode &new_mode, mode_reason_t reason);
     bool mavlink_set_mode(uint8_t mode);
     void startup_INS_ground(void);

APMrover2/commands_logic.cpp

@@ -420,5 +420,4 @@ void Rover::do_digicam_control(const AP_Mission::Mission_Command& cmd)
 void Rover::do_set_reverse(const AP_Mission::Mission_Command& cmd)
 {
     mode_auto.set_reversed(cmd.p1 == 1);
-    set_reverse(cmd.p1 == 1);
 }

APMrover2/mode_acro.cpp

@@ -11,15 +11,11 @@ void ModeAcro::update()
         get_pilot_desired_steering_and_throttle(desired_steering, desired_throttle);
         // no valid speed, just use the provided throttle
         g2.motors.set_throttle(desired_throttle);
-        // set reverse flag if negative throttle
-        rover.set_reverse(is_negative(desired_throttle));
     } else {
         float desired_speed;
         // convert pilot stick input into desired steering and speed
         get_pilot_desired_steering_and_speed(desired_steering, desired_speed);
         calc_throttle(desired_speed, false, true);
-        // set reverse flag if negative desired speed
-        rover.set_reverse(is_negative(desired_speed));
     }
 
     // convert pilot steering input to desired turn rate in radians/sec

APMrover2/mode_auto.cpp

@@ -24,9 +24,6 @@ bool ModeAuto::_enter()
     // other initialisation
     auto_triggered = false;
 
-    // initialise reversed to be false
-    set_reversed(false);
-
     // restart mission processing
     mission.start_or_resume();
     return true;
@@ -148,7 +145,6 @@ void ModeAuto::set_reversed(bool value)
 {
     if (_reversed != value) {
         _reversed = value;
-        rover.set_reverse(_reversed);
     }
 }
 

APMrover2/mode_guided.cpp

@@ -9,9 +9,6 @@ bool ModeGuided::_enter()
     // when entering guided mode we set the target as the current location.
     set_desired_location(rover.current_loc);
 
-    // guided mode never travels in reverse
-    rover.set_reverse(false);
-
     return true;
 }
 

APMrover2/mode_hold.cpp

@@ -6,7 +6,4 @@ void ModeHold::update()
     // hold position - stop motors and center steering
     g2.motors.set_throttle(0.0f);
     g2.motors.set_steering(0.0f);
-
-    // hold mode never reverses
-    rover.set_reverse(false);
 }

APMrover2/mode_loiter.cpp

@@ -15,9 +15,6 @@ bool ModeLoiter::_enter()
     _desired_yaw_cd = ahrs.yaw_sensor;
     _yaw_error_cd = 0.0f;
 
-    // set reversed based on speed
-    rover.set_reverse(is_negative(_desired_speed));
-
     return true;
 }
 
@@ -54,8 +51,4 @@ void ModeLoiter::update()
     // run steering and throttle controllers
     calc_steering_to_heading(_desired_yaw_cd, _desired_speed < 0);
     calc_throttle(_desired_speed, false, true);
-
-    // mark us as in_reverse when using a negative throttle
-    // To-Do: only in reverse if vehicle is actually travelling backwards?
-    rover.set_reverse(_desired_speed < 0);
 }

APMrover2/mode_manual.cpp

@@ -9,7 +9,4 @@ void ModeManual::update()
     // copy RC scaled inputs to outputs
     g2.motors.set_throttle(desired_throttle);
     g2.motors.set_steering(desired_steering, false);
-
-    // mark us as in_reverse when using a negative throttle to stop AHRS getting off
-    rover.set_reverse(is_negative(g2.motors.get_throttle()));
 }

APMrover2/mode_rtl.cpp

@@ -14,9 +14,6 @@ bool ModeRTL::_enter()
     // set destination
     set_desired_location(rover.home);
 
-    // RTL never reverses
-    rover.set_reverse(false);
-
     return true;
 }
 

APMrover2/mode_smart_rtl.cpp

@@ -20,9 +20,6 @@ bool ModeSmartRTL::_enter()
     // init location target
     set_desired_location(rover.current_loc);
 
-    // RTL never reverses
-    rover.set_reverse(false);
-
     // init state
     smart_rtl_state = SmartRTL_WaitForPathCleanup;
     _reached_destination = false;

APMrover2/mode_steering.cpp

@@ -48,9 +48,6 @@ void ModeSteering::update()
         calc_steering_from_lateral_acceleration(desired_lat_accel, reversed);
     }
 
-    // mark us as in_reverse when using a negative throttle
-    rover.set_reverse(reversed);
-
     // run speed to throttle controller
     calc_throttle(desired_speed, false, true);
 }

APMrover2/system.cpp

@@ -203,16 +203,33 @@ void Rover::startup_ground(void)
     gcs().send_text(MAV_SEVERITY_INFO, "Ready to drive");
 }
 
-/*
-  set the in_reverse flag
-  reset the throttle integrator if this changes in_reverse
- */
-void Rover::set_reverse(bool reverse)
+// update the ahrs flyforward setting which can allow
+// the vehicle's movements to be used to estimate heading
+void Rover::update_ahrs_flyforward()
 {
-    if (in_reverse == reverse) {
-        return;
+    bool flyforward = false;
+
+    // boats never use movement to estimate heading
+    if (!is_boat()) {
+        // throttle threshold is 15% or 1/2 cruise throttle
+        bool throttle_over_thresh = g2.motors.get_throttle() > MIN(g.throttle_cruise * 0.50f, 15.0f);
+        // desired speed threshold of 1m/s
+        bool desired_speed_over_thresh = g2.attitude_control.speed_control_active() && (g2.attitude_control.get_desired_speed() > 0.5f);
+        if (throttle_over_thresh || (is_positive(g2.motors.get_throttle()) && desired_speed_over_thresh)) {
+            uint32_t now = AP_HAL::millis();
+            // if throttle over threshold start timer
+            if (flyforward_start_ms == 0) {
+                flyforward_start_ms = now;
+            }
+            // if throttle over threshold for 2 seconds set flyforward to true
+            flyforward = (now - flyforward_start_ms > 2000);
+        } else {
+            // reset timer
+            flyforward_start_ms = 0;
+        }
     }
-    in_reverse = reverse;
+
+    ahrs.set_fly_forward(flyforward);
 }
 
 bool Rover::set_mode(Mode &new_mode, mode_reason_t reason)