AC_WPNav: add distance and bearing to target methods

libraries/AC_WPNav/AC_WPNav.cpp

@@ -35,14 +35,6 @@ AC_WPNav::AC_WPNav(AP_InertialNav* inav, APM_PI* pid_pos_lat, APM_PI* pid_pos_lo
 /// simple loiter controller
 ///
 
-/// set_loiter_target in cm from home
-void AC_WPNav::set_loiter_target(const Vector3f& position)
-{
-    _state = WPNAV_STATE_LOITER;
-    _target = position;
-}
-
-
 /// move_loiter_target - move loiter target by velocity provided in front/right directions in cm/s
 void AC_WPNav::move_loiter_target(int16_t vel_forward_cms, int16_t vel_right_cms, float dt)
 {
@@ -65,6 +57,35 @@ void AC_WPNav::move_loiter_target(int16_t vel_forward_cms, int16_t vel_right_cms
     _target.y += vel_lon * dt;
 }
 
+/// get_distance_to_target - get horizontal distance to loiter target in cm
+float AC_WPNav::get_distance_to_target()
+{
+    return _distance_to_target;
+}
+
+/// get_bearing_to_target - get bearing to loiter target in centi-degrees
+int32_t AC_WPNav::get_bearing_to_target()
+{
+    return get_bearing_cd(_inav->get_position(), _target);
+}
+
+/// update_loiter - run the loiter controller - should be called at 10hz
+void AC_WPNav::update_loiter()
+{
+    uint32_t now = hal.scheduler->millis();
+    float dt = now - _last_update;
+    _last_update = now;
+
+    // catch if we've just been started
+    if( dt >= 1.0 ) {
+        dt = 0.0;
+        reset_I();
+    }
+
+    // run loiter position controller
+    get_loiter_pos_lat_lon(_target.x, _target.y, dt);
+}
+
 ///
 /// waypoint navigation
 ///
@@ -78,7 +99,6 @@ void AC_WPNav::set_destination(const Vector3f& destination)
 /// set_origin_and_destination - set origin and destination using lat/lon coordinates
 void AC_WPNav::set_origin_and_destination(const Vector3f& origin, const Vector3f& destination)
 {
-    _state = WPNAV_STATE_WPNAV;
     _origin = origin;
     _destination = destination;
     _pos_delta = _destination - _origin;
@@ -148,20 +168,33 @@ void AC_WPNav::advance_target_along_track(float velocity_cms, float dt)
     _target.y = _origin.y + _pos_delta.y * track_length_pct;
 }
 
-///
-/// shared methods
-///
-
-/// update - run the loiter and wpnav controllers - should be called at 10hz
-void AC_WPNav::update()
+/// get_distance_to_destination - get horizontal distance to destination in cm
+float AC_WPNav::get_distance_to_destination()
 {
-    float dt = hal.scheduler->millis() - _last_update;
+    // get current location
+    Vector3f curr = _inav->get_position();
+    return pythagorous2(_destination.x-curr.x,_destination.y-curr.y);
+}
 
-    // prevent run up in dt value
-    dt = min(dt, 1.0f);
+/// get_bearing_to_destination - get bearing to next waypoint in centi-degrees
+int32_t AC_WPNav::get_bearing_to_destination()
+{
+    return get_bearing_cd(_inav->get_position(), _destination);
+}
 
-    // advance the target if necessary
-    if( _state == WPNAV_STATE_WPNAV ) {
+/// update_wpnav - run the wp controller - should be called at 10hz
+void AC_WPNav::update_wpnav()
+{
+    uint32_t now = hal.scheduler->millis();
+    float dt = now - _last_update;
+    _last_update = now;
+
+    // catch if we've just been started
+    if( dt >= 1.0 ) {
+        dt = 0.0;
+        reset_I();
+    }else{
+        // advance the target if necessary
         advance_target_along_track(_speed_cms, dt);
     }
 
@@ -169,6 +202,10 @@ void AC_WPNav::update()
     get_loiter_pos_lat_lon(_target.x, _target.y, dt);
 }
 
+///
+/// shared methods
+///
+
 // get_loiter_pos_lat_lon - loiter position controller
 //     converts desired position provided as distance from home in lat/lon directions to desired velocity
 void AC_WPNav::get_loiter_pos_lat_lon(int32_t target_lat_from_home, int32_t target_lon_from_home, float dt)
@@ -191,6 +228,7 @@ void AC_WPNav::get_loiter_pos_lat_lon(int32_t target_lat_from_home, int32_t targ
     dist_error_lon = target_lon_from_home - _inav->get_longitude_diff();
 
     linear_distance = MAX_LOITER_POS_ACCEL/(2*_pid_pos_lat->kP()*_pid_pos_lat->kP());
+    _distance_to_target = linear_distance;      // for reporting purposes
 
     dist_error_total = safe_sqrt(dist_error_lat*dist_error_lat + dist_error_lon*dist_error_lon);
     if( dist_error_total > 2*linear_distance ) {
@@ -261,6 +299,8 @@ void AC_WPNav::get_loiter_accel_lat_lon(int16_t accel_lat, int16_t accel_lon)
     float accel_forward;
     float accel_right;
 
+    // To-Do: add 1hz filter to accel_lat, accel_lon
+
     // rotate accelerations into body forward-right frame
     accel_forward = accel_lat*_cos_yaw + accel_lon*_sin_yaw;
     accel_right = -accel_lat*_sin_yaw + accel_lon*_cos_yaw;
@@ -269,3 +309,23 @@ void AC_WPNav::get_loiter_accel_lat_lon(int16_t accel_lat, int16_t accel_lon)
     _desired_roll = constrain((accel_right/(-z_accel_meas))*(18000/M_PI), -4500, 4500);
     _desired_pitch = constrain((-accel_forward/(-z_accel_meas*_cos_roll))*(18000/M_PI), -4500, 4500);
 }
+
+// get_bearing_cd - return bearing in centi-degrees between two positions
+// To-Do: move this to math library
+float AC_WPNav::get_bearing_cd(const Vector3f origin, const Vector3f destination)
+{
+    float bearing = 9000 + atan2f(-(destination.x-origin.x), destination.y-origin.y) * 5729.57795f;
+    if (bearing < 0) {
+        bearing += 36000;
+    }
+    return bearing;
+}
+
+/// reset_I - clears I terms from loiter PID controller
+void AC_WPNav::reset_I()
+{
+    _pid_pos_lon->reset_I();
+    _pid_pos_lat->reset_I();
+    _pid_rate_lon->reset_I();
+    _pid_rate_lat->reset_I();
+}

libraries/AC_WPNav/AC_WPNav.h

@@ -20,11 +20,6 @@
 #define WPINAV_MAX_POS_ERROR            2000.0f     // maximum distance (in cm) that the desired track can stray from our current location.
 #define WP_SPEED                        500         // default horizontal speed betwen waypoints in cm/s
 
-// possible states
-#define WPNAV_STATE_INACTIVE    0
-#define WPNAV_STATE_LOITER 	    1
-#define WPNAV_STATE_WPNAV       2
-
 class AC_WPNav
 {
 public:
@@ -36,32 +31,50 @@ public:
     /// simple loiter controller
     ///
 
+    /// get_loiter_target - get loiter target as position vector (from home in cm)
+    Vector3f get_loiter_target() { return _target; }
+
     /// set_loiter_target in cm from home
-    void set_loiter_target(const Vector3f& position);
+    void set_loiter_target(const Vector3f& position) { _target = position; }
 
     /// move_loiter_target - move destination using forward and right velocities in cm/s
     void move_loiter_target(int16_t vel_forward_cms, int16_t vel_right_cms, float dt);
 
+    /// get_distance_to_target - get horizontal distance to loiter target in cm
+    float get_distance_to_target();
+
+    /// get_bearing_to_target - get bearing to loiter target in centi-degrees
+    int32_t get_bearing_to_target();
+
+    /// update_loiter - run the loiter controller - should be called at 10hz
+    void update_loiter();
+
     ///
     /// waypoint navigation
     ///
 
-    /// set_destination with distance from home in cm
+    /// set_destination waypoint using position vector (distance from home in cm)
     void set_destination(const Vector3f& destination);
 
-    /// set_origin_and_destination - set origin and destination using lat/lon coordinates
+    /// set_origin_and_destination - set origin and destination waypoints using position vectors (distance from home in cm)
     void set_origin_and_destination(const Vector3f& origin, const Vector3f& destination);
 
     /// advance_target_along_track - move target location along track from origin to destination
     void advance_target_along_track(float velocity_cms, float dt);
 
+    /// get_distance_to_destination - get horizontal distance to destination in cm
+    float get_distance_to_destination();
+
+    /// get_bearing_to_destination - get bearing to next waypoint in centi-degrees
+    int32_t get_bearing_to_destination();
+
+    /// update_wp - update waypoint controller
+    void update_wpnav();
+
     ///
     /// shared methods
     ///
 
-    /// update - run the loiter and wpnav controllers - should be called at 10hz
-    void update(void);
-
     /// get desired roll, pitch and altitude which should be fed into stabilize controllers
     int32_t get_desired_roll() { return _desired_roll; };
     int32_t get_desired_pitch() { return _desired_pitch; };
@@ -92,9 +105,11 @@ protected:
     ///    converts desired accelerations provided in lat/lon frame to roll/pitch angles
     void get_loiter_accel_lat_lon(int16_t accel_lat, int16_t accel_lon);
 
-    /// waypoint controller
-    /// get_wpinav_pos - wpinav position controller with desired position held in wpinav_destination
-    void get_wpinav_pos(float dt);
+    /// get_bearing_cd - return bearing in centi-degrees between two positions
+    float get_bearing_cd(const Vector3f origin, const Vector3f destination);
+
+    /// reset_I - clears I terms from loiter PID controller
+    void reset_I();
 
     // pointers to inertial nav library
     AP_InertialNav*	_inav;
@@ -107,23 +122,31 @@ protected:
 
     // parameters
     AP_Float    _speed_cms;         // default horizontal speed in cm/s
-    uint8_t		_state;				// records whether we are loitering or navigating towards a waypoint
     uint32_t	_last_update;		// time of last update call
     float       _cos_yaw;           // short-cut to save on calcs required to convert roll-pitch frame to lat-lon frame
     float       _sin_yaw;
     float       _cos_roll;
 
     // output from controller
-    int32_t     _desired_roll;
-    int32_t     _desired_pitch;
-    int32_t     _desired_altitude;
+    int32_t     _desired_roll;          // fed to stabilize controllers at 50hz
+    int32_t     _desired_pitch;         // fed to stabilize controllers at 50hz
+    int32_t     _desired_altitude;      // fed to alt hold controller at 50hz
 
+    // internal variables
     Vector3f    _target;   		        // loiter's target location in cm from home
     Vector3f    _origin;                // starting point of trip to next waypoint in cm from home (equivalent to next_WP)
     Vector3f    _destination;           // target destination in cm from home (equivalent to next_WP)
-    Vector3f    _wpinav_target;         // the intermediate target location in cm from home
     Vector3f    _pos_delta;             // position difference between origin and destination
     float       _track_length;          // distance in cm between origin and destination
     float       _track_desired;         // our desired distance along the track in cm
+    float       _distance_to_target;    // distance to loiter target
+
+    // To-Do: add split of fast (100hz for accel->angle) and slow (10hz for navigation)
+    //float       _desired_accel_fwd;     // updated from loiter controller at 10hz, consumed by accel->angle controller at 50hz
+    //float       _desired_accel_rgt;
+    /// update - run the loiter and wpnav controllers - should be called at 100hz
+    //void update_100hz(void);
+    /// update - run the loiter and wpnav controllers - should be called at 10hz
+    //void update_10hz(void);
 };
 #endif	// AC_WPNAV_H