AP_L1 - add a stale flag

threading bug fix. When a mission wp updates, but the L1 controller had not yet, the data is stale. Example, On Plane when NAV_LAND starts for a moment your xtrack and bearing is most likely bear zero regardless if you have a big turn or not until 10 Hz later when the update() gets called and updates those values with correct values for the new waypoint.

libraries/AP_L1_Control/AP_L1_Control.cpp

@@ -255,6 +255,8 @@ void AP_L1_Control::update_waypoint(const struct Location &prev_WP, const struct
 	_WPcircle = false;
 	
 	_bearing_error = Nu; // bearing error angle (radians), +ve to left of track
+
+	_data_is_stale = false; // status are correctly updated with current waypoint data
 }
 
 // update L1 control for loitering
@@ -361,6 +363,8 @@ void AP_L1_Control::update_loiter(const struct Location &center_WP, float radius
 		_bearing_error = 0.0f; // bearing error (radians), +ve to left of track
 		_nav_bearing = atan2f(-A_air_unit.y , -A_air_unit.x); // bearing (radians)from AC to L1 point
 	}
+
+    _data_is_stale = false; // status are correctly updated with current waypoint data
 }
 
 
@@ -401,6 +405,8 @@ void AP_L1_Control::update_heading_hold(int32_t navigation_heading_cd)
 	// Limit Nu to +-pi
 	Nu = constrain_float(Nu, -M_PI_2, M_PI_2);
 	_latAccDem = 2.0f*sinf(Nu)*VomegaA;
+
+    _data_is_stale = false; // status are correctly updated with current waypoint data
 }
 
 // update L1 control for level flight on current heading
@@ -416,4 +422,6 @@ void AP_L1_Control::update_level_flight(void)
 	_WPcircle = false;
 
 	_latAccDem = 0;
+
+    _data_is_stale = false; // status are correctly updated with current waypoint data
 }

libraries/AP_L1_Control/AP_L1_Control.h

@@ -55,6 +55,13 @@ public:
         _L1_period.set_default(period);
     }
 
+    void set_data_is_stale(void)  {
+        _data_is_stale = true;
+    }
+    bool data_is_stale(void) const {
+        return _data_is_stale;
+    }
+
 	// this supports the NAVl1_* user settable parameters
     static const struct AP_Param::GroupInfo var_info[];
 
@@ -104,4 +111,5 @@ private:
     AP_Float _L1_xtrack_i_gain;
     float _L1_xtrack_i_gain_prev = 0;
     uint32_t _last_update_waypoint_us;
+    bool _data_is_stale = true;
 };

libraries/AP_Navigation/AP_Navigation.h

@@ -102,6 +102,17 @@ public:
 	// the update_loiter() method is used
 	virtual bool reached_loiter_target(void) = 0;
 
+	// notify Navigation controller that a new waypoint has just been
+	// processed. This means that until we handle an update_XXX() function
+	// the data is stale with old navigation information.
+    virtual void set_data_is_stale(void) = 0;
+
+    // return true if a new waypoint has been processed by mission
+    // controller but the navigation controller still has old stale data
+    // from previous waypoint navigation handling. This gets cleared on
+    // every update_XXXXXX() call.
+    virtual bool data_is_stale(void) const = 0;
+
 	// add new navigation controllers to this enum. Users can then
 	// select which navigation controller to use by setting the
 	// NAV_CONTROLLER parameter