#pragma once #include /* * SCurves calculate paths between waypoints (including the corners) using specified speed, acceleration and jerk limits * * How to use: * 1. create three SCurve objects called something like "prev_leg", "this_leg" and "next_leg" * 2. call this_leg.calculate_track() to calculate the path from the origin to the destination for the given speed, accel and jerk limits * 3. if the vehicle will fly past the destination to another "next destination": * a) call next_leg.calculate_track() with the appropriate arguments * b) set a "fast_waypoint" boolean to true. this will be passed into "advance_target_along_track()" in the next step * if there is no "next destination" * a) call next_leg.init() * b) set the "fast_waypoint" boolean to false * 4. call this_leg.advance_target_along_track() with a small "dt" value and retrieve the resulting target position, velocity and acceleration * Note: the target_pos should be set to the segments's earth frame origin before this function is called * 5. pass the target position, velocity and acceleration into the position controller * 6. repeat steps 4 and 5 until finished() returns true * 7. promote the legs: * a) set prev_leg = this_leg * b) set this_leg = next_leg * c) jump back to step 3 * * Other features: * 1. set_speed_max() allows changing the max speeds mid path. The path will be recalculated * 2. set_origin_speed_max() and set_destination_speed_max() allows setting the speed along the path at the beginning and end of the leg * this is used to smoothly integrate with spline segments * * This library works with any units (meters, cm, etc) as long as they are used consistently. * e.g. if origin and destination are meters, speeds should be in m/s, accel in m/s/s, etc. * * Terminology: * position: a point in space * velocity: rate of change of position. aka speed * acceleration: rate of change of speed * jerk: rate of change of acceleration * jerk time: the time (in seconds) for jerk to increase from zero to its maximum value * jounce: rate of change of jerk * track: 3D path that the vehicle will follow * path: position, velocity, accel and jerk kinematic profile that this library generates */ class SCurve { public: // constructor SCurve(); // initialise and clear the path void init(); // generate a trigonometric track in 3D space that moves over a straight line // between two points defined by the origin and destination void calculate_track(const Vector3f &origin, const Vector3f &destination, float speed_xy, float speed_up, float speed_down, float accel_xy, float accel_z, float jerk_time_sec, float jerk_maximum); // set maximum velocity and re-calculate the path using these limits void set_speed_max(float speed_xy, float speed_up, float speed_down); // set the maximum vehicle speed at the origin // returns the expected speed at the origin which will always be equal or lower than speed float set_origin_speed_max(float speed); // set the maximum vehicle speed at the destination void set_destination_speed_max(float speed); // move target location along path from origin to destination // prev_leg and next_leg are the paths before and after this path // wp_radius is max distance from the waypoint at the apex of the turn // fast_waypoint should be true if vehicle will not stop at end of this leg // dt is the time increment the vehicle will move along the path // target_pos should be set to this segment's origin and it will be updated to the current position target // target_vel and target_accel are updated with new targets // returns true if vehicle has passed the apex of the corner bool advance_target_along_track(SCurve &prev_leg, SCurve &next_leg, float wp_radius, bool fast_waypoint, float dt, Vector3f &target_pos, Vector3f &target_vel, Vector3f &target_accel) WARN_IF_UNUSED; // time has reached the end of the sequence bool finished() const WARN_IF_UNUSED; private: // increment time and return the position, velocity and acceleration vectors relative to the origin void move_from_pos_vel_accel(float dt, Vector3f &pos, Vector3f &vel, Vector3f &accel); // increment time and return the position, velocity and acceleration vectors relative to the destination void move_to_pos_vel_accel(float dt, Vector3f &pos, Vector3f &vel, Vector3f &accel); // return the position, velocity and acceleration vectors relative to the origin at a specified time along the path void move_from_time_pos_vel_accel(float t, Vector3f &pos, Vector3f &vel, Vector3f &accel); // get desired maximum speed along track float get_speed_along_track() const WARN_IF_UNUSED { return vel_max; } // get desired maximum acceleration along track float get_accel_along_track() const WARN_IF_UNUSED { return accel_max; } // return the change in position from origin to destination const Vector3f& get_track() const WARN_IF_UNUSED { return track; }; // return the current time elapsed float get_time_elapsed() const WARN_IF_UNUSED { return time; } // time at the end of the sequence float time_end() const WARN_IF_UNUSED; // time left before sequence will complete float get_time_remaining() const WARN_IF_UNUSED; // time when acceleration section of the sequence will complete float get_accel_finished_time() const WARN_IF_UNUSED; // return true if the sequence is braking to a stop bool braking() const WARN_IF_UNUSED; // increment the internal time void advance_time(float dt); // calculate the jerk, acceleration, velocity and position at time t void get_jerk_accel_vel_pos_at_time(float time_now, float &Jt_out, float &At_out, float &Vt_out, float &Pt_out) const; // calculate the jerk, acceleration, velocity and position at time t when running the constant jerk time segment void calc_javp_for_segment_const_jerk(float time_now, float J0, float A0, float V0, float P0, float &Jt, float &At, float &Vt, float &Pt) const; // Calculate the jerk, acceleration, velocity and position at time t when running the increasing jerk magnitude time segment based on a raised cosine profile void calc_javp_for_segment_incr_jerk(float time_now, float tj, float Jm, float A0, float V0, float P0, float &Jt, float &At, float &Vt, float &Pt) const; // Calculate the jerk, acceleration, velocity and position at time t when running the decreasing jerk magnitude time segment based on a raised cosine profile void calc_javp_for_segment_decr_jerk(float time_now, float tj, float Jm, float A0, float V0, float P0, float &Jt, float &At, float &Vt, float &Pt) const; // generate time segments for straight segment void add_segments(float L); // calculate the segment times for the trigonometric S-Curve path defined by: // tj - duration of the raised cosine jerk profile (aka jerk time) // Jm - maximum value of the raised cosine jerk profile (aka jerk max) // V0 - initial velocity magnitude // Am - maximum constant acceleration // Vm - maximum constant velocity // L - Length of the path void calculate_path(float tj, float Jm, float V0, float Am, float Vm, float L, float &Jm_out, float &t2_out, float &t4_out, float &t6_out) const; // generate three time segments forming the jerk profile void add_segments_jerk(uint8_t &seg_pnt, float tj, float Jm, float Tcj); // generate constant jerk time segment void add_segment_const_jerk(uint8_t &seg_pnt, float tin, float J0); // generate increasing jerk magnitude time segment based on a raised cosine profile void add_segment_incr_jerk(uint8_t &seg_pnt, float tj, float Jm); // generate decreasing jerk magnitude time segment based on a raised cosine profile void add_segment_decr_jerk(uint8_t &seg_pnt, float tj, float Jm); // set speed and acceleration limits for the path // origin and destination are offsets from EKF origin // speed and acceleration parameters are given in horizontal, up and down. void set_kinematic_limits(const Vector3f &origin, const Vector3f &destination, float speed_xy, float speed_up, float speed_down, float accel_xy, float accel_z); // return true if the curve is valid. Used to identify and protect against code errors bool valid() const WARN_IF_UNUSED; #if CONFIG_HAL_BOARD == HAL_BOARD_SITL // debugging messages void debug() const; #endif // segment types enum class SegmentType { CONSTANT_JERK, POSITIVE_JERK, NEGATIVE_JERK }; // add single segment void add_segment(uint8_t &seg_pnt, float end_time, SegmentType seg_type, float jerk_ref, float end_accel, float end_vel, float end_pos); // members float jerk_time; // duration of jerk raised cosine time segment float jerk_max; // maximum jerk magnitude float accel_max; // maximum acceleration magnitude float vel_max; // maximum velocity magnitude float time; // time that defines position on the path float position_sq; // position (squared) on the path at the last time step (used to detect finish) // segment 0 is the initial segment and holds the vehicle's initial position and velocity // segments 1 to 7 are the acceleration segments // segments 8 to 14 are the speed change segments // segment 15 is the constant velocity segment // segment 16 to 22 is the deceleration segment const static uint8_t segments_max = 23; // maximum number of time segments uint8_t num_segs; // number of time segments being used struct { float jerk_ref; // jerk reference value for time segment (the jerk at the beginning, middle or end depending upon the segment type) SegmentType seg_type; // segment type (jerk is constant, increasing or decreasing) float end_time; // final time value for segment float end_accel; // final acceleration value for segment float end_vel; // final velocity value for segment float end_pos; // final position value for segment } segment[segments_max]; Vector3f track; // total change in position from origin to destination Vector3f delta_unit; // reference direction vector for path };