WPNav: add spline support

libraries/AC_WPNav/AC_WPNav.cpp

@@ -83,7 +83,10 @@ AC_WPNav::AC_WPNav(const AP_InertialNav* inav, const AP_AHRS* ahrs, AC_PosContro
     _track_desired(0.0),
     _track_accel(0.0),
     _track_speed(0.0),
-    _track_leash_length(0.0)
+    _track_leash_length(0.0),
+    _spline_time(0.0),
+    _spline_vel_scaler(0.0),
+    _spline_slow_down_dist(0.0)
 {
     AP_Param::setup_object_defaults(this, var_info);
 }
@@ -222,15 +225,15 @@ void AC_WPNav::update_loiter()
         if (dt >= 1.0f) {
             dt = 0.0;
         }
-            // capture time since last iteration
-            _loiter_last_update = now;
-            // translate any adjustments from pilot to loiter target
+        // capture time since last iteration
+        _loiter_last_update = now;
+        // translate any adjustments from pilot to loiter target
         calc_loiter_desired_velocity(dt);
         // trigger position controller on next update
         _pos_control.trigger_xy();
     }else{
-            // run loiter's position to velocity step
-            _pos_control.update_pos_controller(true);
+        // run loiter's position to velocity step
+        _pos_control.update_pos_controller(true);
     }
 }
 
@@ -242,17 +245,19 @@ void AC_WPNav::update_loiter()
 /// set_destination - set destination using cm from home
 void AC_WPNav::set_wp_destination(const Vector3f& destination)
 {
+	Vector3f origin;
+
     // if waypoint controller is active and copter has reached the previous waypoint use it for the origin
     if( _flags.reached_destination && ((hal.scheduler->millis() - _wp_last_update) < 1000) ) {
-        _origin = _destination;
+        origin = _destination;
     }else{
         // otherwise calculate origin from the current position and velocity
-        _pos_control.get_stopping_point_xy(_origin);
-        _pos_control.get_stopping_point_z(_origin);
+        _pos_control.get_stopping_point_xy(origin);
+        _pos_control.get_stopping_point_z(origin);
     }
 
     // set origin and destination
-    set_wp_origin_and_destination(_origin, destination);
+    set_wp_origin_and_destination(origin, destination);
 }
 
 /// set_origin_and_destination - set origin and destination using lat/lon coordinates
@@ -290,6 +295,7 @@ void AC_WPNav::set_wp_origin_and_destination(const Vector3f& origin, const Vecto
     _track_desired = 0;             // target is at beginning of track
     _flags.reached_destination = false;
     _flags.fast_waypoint = false;   // default waypoint back to slow
+    _flags.segment_type = SEGMENT_STRAIGHT;
 
     // initialise the limited speed to current speed along the track
     const Vector3f &curr_vel = _inav->get_velocity();
@@ -435,15 +441,15 @@ void AC_WPNav::update_wpnav()
         if (dt >= 1.0f) {
             dt = 0.0;
         }
-            // capture time since last iteration
+        // capture time since last iteration
         _wp_last_update = now;
 
-            // advance the target if necessary
+        // advance the target if necessary
         advance_wp_target_along_track(dt);
         _pos_control.trigger_xy();
     }else{
         // run position controller
-            _pos_control.update_pos_controller(false);
+        _pos_control.update_pos_controller(false);
     }
 }
 
@@ -484,6 +490,265 @@ void AC_WPNav::calculate_wp_leash_length()
     }
 }
 
+///
+/// spline methods
+///
+
+/// set_spline_destination waypoint using position vector (distance from home in cm)
+///     seg_type should be calculated by calling function based on the mission
+void AC_WPNav::set_spline_destination(const Vector3f& destination, bool stopped_at_start, spline_segment_end_type seg_end_type, const Vector3f& next_destination)
+{
+    Vector3f origin;
+
+    // if waypoint controller is active and copter has reached the previous waypoint use it for the origin
+    if( _flags.reached_destination && ((hal.scheduler->millis() - _wp_last_update) < 1000) ) {
+        origin = _destination;
+    }else{
+        // otherwise calculate origin from the current position and velocity
+        _pos_control.get_stopping_point_xy(origin);
+        _pos_control.get_stopping_point_z(origin);
+    }
+
+    // set origin and destination
+    set_spline_origin_and_destination(origin, destination, stopped_at_start, seg_end_type, next_destination);
+}
+
+/// set_spline_origin_and_destination - set origin and destination waypoints using position vectors (distance from home in cm)
+///     seg_type should be calculated by calling function based on the mission
+void AC_WPNav::set_spline_origin_and_destination(const Vector3f& origin, const Vector3f& destination, bool stopped_at_start, spline_segment_end_type seg_end_type, const Vector3f& next_destination)
+{
+    // mission is "active" if wpnav has been called recently and vehicle reached the previous waypoint
+    bool prev_segment_exists = (_flags.reached_destination && ((hal.scheduler->millis() - _wp_last_update) < 1000));
+
+    // segment start types
+    // stop - vehicle is not moving at origin
+    // straight-fast - vehicle is moving, previous segment is straight.  vehicle will fly straight through the waypoint before beginning it's spline path to the next wp
+    //     _flag.segment_type holds whether prev segment is straight vs spline but we don't know if it has a delay
+    // spline-fast - vehicle is moving, previous segment is splined, vehicle will fly through waypoint but previous segment should have it flying in the correct direction (i.e. exactly parallel to position difference vector from previous segment's origin to this segment's destination)
+
+    // calculate spline velocity at origin
+    if (stopped_at_start) {
+    	// if vehicle is stopped at the origin, set origin velocity to 0.1 * distance vector from origin to destination
+    	_spline_origin_vel = (destination - origin) * 0.1f;
+    	_spline_time = 0.0f;
+    	_spline_vel_scaler = 0.0f;
+    }else{
+    	// look at previous segment to determine velocity at origin
+        if (prev_segment_exists) {
+            if (_flags.segment_type == SEGMENT_STRAIGHT) {
+                // previous segment is straight, vehicle is moving so vehicle should fly straight through the origin
+                // before beginning it's spline path to the next waypoint. Note: we are using the previous segment's origin and destination
+                _spline_origin_vel = (_destination - _origin);
+            	_spline_time = 0.0f;	// To-Do: this should be set based on how much overrun there was from straight segment?
+            	_spline_vel_scaler = 0.0f;    // To-Do: this should be set based on speed at end of prev straight segment?
+            }else{
+                // previous segment is splined, vehicle will fly through origin
+                // we can use the previous segment's destination velocity as this segment's origin velocity
+                // Note: previous segment will leave destination velocity parallel to position difference vector
+                //       from previous segment's origin to this segment's destination)
+                _spline_origin_vel = _spline_destination_vel;
+                if (_spline_time > 1.0f && _spline_time < 1.1f) {    // To-Do: remove hard coded 1.1f
+                    _spline_time -= 1.0f;
+                }else{
+                    _spline_time = 0.0f;
+                }
+                _spline_vel_scaler = 0.0f;
+            }
+        }
+    }
+
+    // calculate spline velocity at destination
+    switch (seg_end_type) {
+
+    case SEGMENT_END_STOP:
+        // if vehicle stops at the destination set destination velocity to 0.1 * distance vector from origin to destination
+        _spline_destination_vel = (destination - origin) * 0.1f;
+        _flags.fast_waypoint = false;
+        break;
+
+    case SEGMENT_END_STRAIGHT:
+        // if next segment is straight, vehicle's final velocity should face along the next segment's position
+        _spline_destination_vel = (next_destination - destination);
+        _flags.fast_waypoint = true;
+        break;
+
+    case SEGMENT_END_SPLINE:
+        // if next segment is splined, vehicle's final velocity should face parallel to the line from the origin to the next destination
+        _spline_destination_vel = (next_destination - origin);
+        _flags.fast_waypoint = true;
+        break;
+    }
+
+    // code below ensures we don't get too much overshoot when the next segment is short
+    float vel_len = (_spline_origin_vel + _spline_destination_vel).length();
+    float pos_len = (destination - origin).length() * 2.0f;
+    if (vel_len > pos_len) {
+        // if total start+stop velocity is more than twice position difference
+        // use a scaled down start and stop velocityscale the  start and stop velocities down
+        float vel_scaling = pos_len / vel_len;
+        // update spline calculator
+        update_spline_solution(origin, destination, _spline_origin_vel * vel_scaling, _spline_destination_vel * vel_scaling);
+    }else{
+        // update spline calculator
+        update_spline_solution(origin, destination, _spline_origin_vel, _spline_destination_vel);
+    }
+
+    // To-Do: handle case where this is a straight segment?
+    // if this is a straight segment, origin velocity is distance vector from origin to destination
+    // To-Do: this handles case of a fast waypoint?
+    // _spline_origin_vel = destination - origin;
+
+    // store origin and destination locations
+    _origin = origin;
+    _destination = destination;
+
+    // initialise position controller speed and acceleration
+    _pos_control.set_speed_xy(_wp_speed_cms);
+    _pos_control.set_accel_xy(_wp_accel_cms);
+    _pos_control.set_speed_z(-_wp_speed_down_cms, _wp_speed_up_cms);
+    _pos_control.calc_leash_length_xy();
+    _pos_control.calc_leash_length_z();
+
+    // calculate leash lengths
+    calculate_wp_leash_length();
+
+    // calculate slow down distance
+    // To-Do: this should be used for straight segments as well
+    // To-Do: should we use a combination of horizontal and vertical speeds?
+    // To-Do: update this automatically when speed or acceleration is changed
+    _spline_slow_down_dist = _wp_speed_cms * _wp_speed_cms / (2.0f*_wp_accel_cms);
+
+    // initialise intermediate point to the origin
+    _pos_control.set_pos_target(origin);
+    _flags.reached_destination = false;
+    _flags.segment_type = SEGMENT_SPLINE;
+}
+
+/// update_spline - update spline controller
+void AC_WPNav::update_spline()
+{
+    // exit immediately if this is not a spline segment
+    if (_flags.segment_type != SEGMENT_SPLINE) {
+        return;
+    }
+
+    // calculate dt
+    uint32_t now = hal.scheduler->millis();
+    float dt = (now - _wp_last_update) / 1000.0f;
+
+    // reset step back to 0 if 0.1 seconds has passed and we completed the last full cycle
+    if (dt > 0.095f) {
+        // double check dt is reasonable
+        if (dt >= 1.0f) {
+            dt = 0.0;
+        }
+        // capture time since last iteration
+        _wp_last_update = now;
+
+        // advance the target if necessary
+        advance_spline_target_along_track(dt);
+        _pos_control.trigger_xy();
+    }else{
+        // run position controller
+        _pos_control.update_pos_controller(false);
+    }
+}
+
+/// update_spline_solution - recalculates hermite_spline_solution grid
+///		relies on _spline_origin_vel, _spline_destination_vel and _origin and _destination
+void AC_WPNav::update_spline_solution(const Vector3f& origin, const Vector3f& dest, const Vector3f& origin_vel, const Vector3f& dest_vel)
+{
+    _hermite_spline_solution[0] = origin;
+    _hermite_spline_solution[1] = origin_vel;
+    _hermite_spline_solution[2] = -origin*3.0f -origin_vel*2.0f + dest*3.0f - dest_vel;
+    _hermite_spline_solution[3] = origin*2.0f + origin_vel -dest*2.0f + dest_vel;
+ }
+
+/// advance_spline_target_along_track - move target location along track from origin to destination
+void AC_WPNav::advance_spline_target_along_track(float dt)
+{
+    if (!_flags.reached_destination) {
+        Vector3f target_pos, target_vel;
+
+        // update target position and velocity from spline calculator
+        calc_spline_pos_vel(_spline_time, target_pos, target_vel);
+
+        // update velocity
+        float spline_dist_to_wp = (_destination - target_pos).length();
+
+        // if within the stopping distance from destination, set target velocity to sqrt of distance * 2 * acceleration
+        if (!_flags.fast_waypoint && spline_dist_to_wp < _spline_slow_down_dist) {
+            _spline_vel_scaler = safe_sqrt(spline_dist_to_wp * 2.0f * _wp_accel_cms);
+        }else if(_spline_vel_scaler < _wp_speed_cms) {
+            // increase velocity using acceleration
+        	// To-Do: replace 0.1f below with update frequency passed in from main program
+            _spline_vel_scaler += _wp_accel_cms* 0.1f;
+        }
+
+        // constrain target velocity
+        if (_spline_vel_scaler > _wp_speed_cms) {
+            _spline_vel_scaler = _wp_speed_cms;
+        }
+
+        // scale the spline_time by the velocity we've calculated vs the velocity that came out of the spline calculator
+        float spline_time_scale = _spline_vel_scaler/target_vel.length();
+
+        // update target position
+        _pos_control.set_pos_target(target_pos);
+
+        // advance spline time to next step
+        _spline_time += spline_time_scale*0.1f;
+
+        // we will reach the next waypoint in the next step so set reached_destination flag
+        // To-Do: is this one step too early?
+        if (_spline_time >= 1.0f) {
+            _flags.reached_destination = true;
+        }
+    }
+}
+
+// calc_spline_pos_vel_accel - calculates target position, velocity and acceleration for the given "spline_time"
+//  To-Do: explain what is "spline_time"
+/// calc_spline_pos_vel - update position and velocity from given spline time
+/// 	relies on update_spline_solution being called since the previous
+void AC_WPNav::calc_spline_pos_vel(float spline_time, Vector3f& position, Vector3f& velocity)
+{
+    float spline_time_sqrd = spline_time * spline_time;
+    float spline_time_cubed = spline_time_sqrd * spline_time;
+
+    position = _hermite_spline_solution[0] + \
+               _hermite_spline_solution[1] * spline_time + \
+               _hermite_spline_solution[2] * spline_time_sqrd + \
+               _hermite_spline_solution[3] * spline_time_cubed;
+
+    velocity = _hermite_spline_solution[1] + \
+               _hermite_spline_solution[2] * 2.0f * spline_time + \
+               _hermite_spline_solution[3] * 3.0f * spline_time_sqrd;
+
+    /*position.x = _hermite_spline_solution[1].x + \
+    		     _hermite_spline_solution[2].x * spline_time + \
+    		     _hermite_spline_solution[3].x * spline_time_sqrd + \
+    		     _hermite_spline_solution[4].x * spline_time_cubed;*/
+
+    /*position.y = _hermite_spline_solution[1].y + \
+    		     _hermite_spline_solution[2].y * u(aa) + \
+    		     hss(3,2)*u(aa)^2 + \
+    		     hss(4,2)*u(aa)^3;
+
+    position.z = _hermite_spline_solution[1].z + _hermite_spline_solution[2].z * u(aa) + hss(3,3)*u(aa)^2 + hss(4,3)*u(aa)^3;
+    */
+
+    //velocity.x = hss(2,1) + hss(3,1)*2*u(aa) + hss(4,1)*3*u(aa)^2;
+    //velocity.y = hss(2,2) + hss(3,2)*2*u(aa) + hss(4,2)*3*u(aa)^2;
+    //velocity.z = hss(2,3) + hss(3,3)*2*u(aa) + hss(4,3)*3*u(aa)^2;
+
+    /*for aa = 1:length(u)
+        pos(aa,:)   = [1, u(aa), u(aa)^2, u(aa)^3] * hermite_spline_solution;
+        vel(aa,:)   = [0, 1, 2*u(aa), 3*u(aa)^2] * hermite_spline_solution;
+        accel(aa,:) = [0, 0, 2, 6*u(aa)] * hermite_spline_solution;
+    end*/
+}
+
 
 ///
 /// shared methods

libraries/AC_WPNav/AC_WPNav.h

@@ -37,6 +37,13 @@ class AC_WPNav
 {
 public:
 
+    // spline segment end types enum
+    enum spline_segment_end_type {
+        SEGMENT_END_STOP = 0,
+        SEGMENT_END_STRAIGHT,
+        SEGMENT_END_SPLINE
+    };
+
     /// Constructor
     AC_WPNav(const AP_InertialNav* inav, const AP_AHRS* ahrs, AC_PosControl& pos_control);
 
@@ -124,6 +131,39 @@ public:
     /// calculate_wp_leash_length - calculates track speed, acceleration and leash lengths for waypoint controller
     void calculate_wp_leash_length();
 
+    ///
+    /// spline methods
+    ///
+
+    // segment start types
+    // stop - vehicle is not moving at origin
+    // straight-fast - vehicle is moving, previous segment is straight.  vehicle will fly straight through the waypoint before beginning it's spline path to the next wp
+    //     _flag.segment_type holds whether prev segment is straight vs spline but we don't know if it has a delay
+    // spline-fast - vehicle is moving, previous segment is splined, vehicle will fly through waypoint but previous segment should have it flying in the correct direction (i.e. exactly parallel to position difference vector from previous segment's origin to this segment's destination)
+
+    // segment end types
+    // stop - vehicle is not moving at destination
+    // straight-fast - next segment is straight, vehicle's destination velocity should be directly along track from this segment's destination to next segment's destination
+    // spline-fast - next segment is spline, vehicle's destination velocity should be parallel to position difference vector from previous segment's origin to this segment's destination
+
+    /// set_spline_destination waypoint using position vector (distance from home in cm)
+    ///     stopped_at_start should be set to true if vehicle is stopped at the origin
+    ///     seg_end_type should be set to stopped, straight or spline depending upon the next segment's type
+    ///     next_destination should be set to the next segment's destination if the seg_end_type is SEGMENT_END_STRAIGHT or SEGMENT_END_SPLINE
+    void set_spline_destination(const Vector3f& destination, bool stopped_at_start, spline_segment_end_type seg_end_type, const Vector3f& next_destination);
+
+    /// set_spline_origin_and_destination - set origin and destination waypoints using position vectors (distance from home in cm)
+    ///     stopped_at_start should be set to true if vehicle is stopped at the origin
+    ///     seg_end_type should be set to stopped, straight or spline depending upon the next segment's type
+    ///     next_destination should be set to the next segment's destination if the seg_end_type is SEGMENT_END_STRAIGHT or SEGMENT_END_SPLINE
+    void set_spline_origin_and_destination(const Vector3f& origin, const Vector3f& destination, bool stopped_at_start, spline_segment_end_type seg_end_type, const Vector3f& next_destination);
+
+    /// reached_spline_destination - true when we have come within RADIUS cm of the waypoint
+    bool reached_spline_destination() const { return _flags.reached_destination; }
+
+    /// update_spline - update spline controller
+    void update_spline();
+
     ///
     /// shared methods
     ///
@@ -137,16 +177,25 @@ public:
 
     /// set_desired_alt - set desired altitude (in cm above home)
     void set_desired_alt(float desired_alt) { _pos_control.set_alt_target(desired_alt); }
+
     /// advance_wp_target_along_track - move target location along track from origin to destination
     void advance_wp_target_along_track(float dt);
 
     static const struct AP_Param::GroupInfo var_info[];
 
 protected:
+
+    // segment types, either straight or spine
+    enum SegmentType {
+        SEGMENT_STRAIGHT = 0,
+        SEGMENT_SPLINE = 1
+    };
+
     // flags structure
     struct wpnav_flags {
         uint8_t reached_destination     : 1;    // true if we have reached the destination
         uint8_t fast_waypoint           : 1;    // true if we should ignore the waypoint radius and consider the waypoint complete once the intermediate target has reached the waypoint
+        SegmentType segment_type        : 1;    // active segment is either straight or spline
     } _flags;
 
     /// calc_loiter_desired_velocity - updates desired velocity (i.e. feed forward) with pilot requested acceleration and fake wind resistance
@@ -156,6 +205,18 @@ protected:
     /// get_bearing_cd - return bearing in centi-degrees between two positions
     float get_bearing_cd(const Vector3f &origin, const Vector3f &destination) const;
 
+    /// spline protected functions
+
+    /// update_spline_solution - recalculates hermite_spline_solution grid
+    void update_spline_solution(const Vector3f& origin, const Vector3f& dest, const Vector3f& origin_vel, const Vector3f& dest_vel);
+
+    /// advance_spline_target_along_track - move target location along track from origin to destination
+    void advance_spline_target_along_track(float dt);
+
+    /// calc_spline_pos_vel - update position and velocity from given spline time
+    /// 	relies on update_spline_solution being called since the previous
+    void calc_spline_pos_vel(float spline_time, Vector3f& position, Vector3f& velocity);
+
     // references to inertial nav and ahrs libraries
     const AP_InertialNav* const _inav;
     const AP_AHRS*        const _ahrs;
@@ -188,5 +249,14 @@ protected:
     float       _track_accel;           // acceleration along track
     float       _track_speed;           // speed in cm/s along track
     float       _track_leash_length;    // leash length along track
+
+    // spline variables
+    float		_spline_time;			// current spline time between origin and destination
+    Vector3f    _spline_origin_vel;     // the target velocity vector at the origin of the spline segment
+    Vector3f    _spline_destination_vel;// the target velocity vector at the destination point of the spline segment
+    Vector3f    _hermite_spline_solution[4]; // array describing spline path between origin and destination
+    float       _spline_vel_scaler;		//
+    float       _spline_slow_down_dist; // vehicle should begin to slow down once it is within this distance from the destination
+                                        // To-Do: this should be used for straight segments as well
 };
 #endif	// AC_WPNAV_H