Line data    Source code

       1             : #pragma once
       2             : 
       3             : #include <AP_Common/AP_Common.h>
       4             : 
       5             : class SplineCurve {
       6             : 
       7             : public:
       8             : 
       9             :     // set maximum speed and acceleration
      10             :     void set_speed_accel(float speed_xy, float speed_up, float speed_down,
      11             :                          float accel_xy, float accel_z);
      12             : 
      13             :     // set origin and destination using position vectors (offset from EKF origin)
      14             :     // origin_vel is vehicle velocity at origin (in NED frame)
      15             :     // destination_vel is vehicle velocity at destination (in NED frame)
      16             :     void set_origin_and_destination(const Vector3p &origin, const Vector3p &destination, const Vector3f &origin_vel, const Vector3f &destination_vel);
      17             : 
      18             :     // move target location along track from origin to destination
      19             :     // target_pos is updated with the target position from EKF origin in NED frame
      20             :     // target_vel is updated with the target velocity in NED frame
      21             :     void advance_target_along_track(float dt, Vector3p &target_pos, Vector3f &target_vel);
      22             : 
      23             :     // returns true if vehicle has reached destination
      24      107717 :     bool reached_destination() const WARN_IF_UNUSED { return _reached_destination; }
      25             : 
      26             :     // returns the unscaled destination velocity vector
      27          24 :     const Vector3f& get_destination_vel() WARN_IF_UNUSED { return _destination_vel; }
      28             : 
      29             :     // returns maximum speed at origin
      30          14 :     float get_origin_speed_max() const WARN_IF_UNUSED { return _origin_speed_max; }
      31             : 
      32             :     // get or set maximum speed at destination
      33           5 :     float get_destination_speed_max() const WARN_IF_UNUSED { return _destination_speed_max; }
      34          17 :     void set_destination_speed_max(float destination_speed_max) { _destination_speed_max = MIN(_destination_speed_max, destination_speed_max); }
      35             : 
      36             : private:
      37             : 
      38             :     // calculate the spline delta time for a given delta distance
      39             :     // returns the spline position and velocity and maximum speed and acceleration the vehicle can travel without exceeding acceleration limits
      40             :     void calc_dt_speed_max(float time, float distance_delta, float &spline_dt, Vector3p &target_pos, Vector3f &spline_vel_unit, float &speed_max, float &accel_max);
      41             : 
      42             :     // recalculate hermite_spline_solution grid
      43             :     void update_solution(const Vector3p &origin, const Vector3p &dest, const Vector3f &origin_vel, const Vector3f &dest_vel);
      44             : 
      45             :     // calculate target position and velocity from given spline time
      46             :     // time is a value from 0 to 1
      47             :     // position is updated with target position as an offset from EKF origin in NED frame
      48             :     // velocity is updated with the unscaled velocity
      49             :     // relies on set_origin_and_destination having been called to update_solution
      50             :     void calc_target_pos_vel(float time, Vector3p &position, Vector3f &velocity, Vector3f &acceleration, Vector3f &jerk);
      51             : 
      52             :     // interval variables
      53             :     Vector3p    _origin;                // origin offset (in NED frame) from EKF
      54             :     Vector3p    _destination;           // destination offset (in NED frame) from EKF
      55             :     Vector3f    _origin_vel;            // the target velocity vector in NED frame at the origin of the spline segment
      56             :     Vector3f    _destination_vel;       // the target velocity vector in NED frame at the destination point of the spline segment
      57             :     Vector3p    _hermite_solution[4];   // array describing path between origin and destination
      58             :     float       _time;                  // current spline time (between 0 and 1) between origin and destination
      59             :     float       _speed_xy;              // maximum horizontal speed
      60             :     float       _speed_up;              // maximum speed upwards
      61             :     float       _speed_down;            // maximum speed downwards
      62             :     float       _accel_xy;              // maximum horizontal acceleration
      63             :     float       _accel_z;               // maximum vertical acceleration
      64             :     float       _origin_speed_max;      // maximum speed at origin
      65             :     float       _destination_speed_max; // maximum speed at destination
      66             :     bool        _reached_destination;   // true once vehicle has reached destination
      67             :     bool        _zero_length;           // true if spline is zero length
      68             : };