ardupilot/libraries/AP_Math/SplineCurve.h

69 lines
3.8 KiB
C++

#pragma once
#include <AP_Common/AP_Common.h>
class SplineCurve {
public:
// set maximum speed and acceleration
void set_speed_accel(float speed_xy, float speed_up, float speed_down,
float accel_xy, float accel_z);
// set origin and destination using position vectors (offset from EKF origin)
// origin_vel is vehicle velocity at origin (in NEU frame)
// destination_vel is vehicle velocity at destination (in NEU frame)
void set_origin_and_destination(const Vector3f &origin, const Vector3f &destination, const Vector3f &origin_vel, const Vector3f &destination_vel);
// move target location along track from origin to destination
// target_pos is updated with the target position from EKF origin in NEU frame
// target_vel is updated with the target velocity in NEU frame
void advance_target_along_track(float dt, Vector3f &target_pos, Vector3f &target_vel);
// returns true if vehicle has reached destination
bool reached_destination() const WARN_IF_UNUSED { return _reached_destination; }
// returns the unscaled destination velocity vector
const Vector3f& get_destination_vel() WARN_IF_UNUSED { return _destination_vel; }
// returns maximum speed at origin
float get_origin_speed_max() const WARN_IF_UNUSED { return _origin_speed_max; }
// get or set maximum speed at destination
float get_destination_speed_max() const WARN_IF_UNUSED { return _destination_speed_max; }
void set_destination_speed_max(float destination_speed_max) { _destination_speed_max = MIN(_destination_speed_max, destination_speed_max); }
private:
// calculate the spline delta time for a given delta distance
// returns the spline position and velocity and maximum speed and acceleration the vehicle can travel without exceeding acceleration limits
void calc_dt_speed_max(float time, float distance_delta, float &spline_dt, Vector3f &target_pos, Vector3f &spline_vel_unit, float &speed_max, float &accel_max);
// recalculate hermite_spline_solution grid
void update_solution(const Vector3f &origin, const Vector3f &dest, const Vector3f &origin_vel, const Vector3f &dest_vel);
// calculate target position and velocity from given spline time
// time is a value from 0 to 1
// position is updated with target position as an offset from EKF origin in NEU frame
// velocity is updated with the unscaled velocity
// relies on set_origin_and_destination having been called to update_solution
void calc_target_pos_vel(float time, Vector3f &position, Vector3f &velocity, Vector3f &acceleration, Vector3f &jerk);
// interval variables
Vector3f _origin; // origin offset (in NEU frame) from EKF
Vector3f _destination; // destination offset (in NEU frame) from EKF
Vector3f _origin_vel; // the target velocity vector in NEU frame at the origin of the spline segment
Vector3f _destination_vel; // the target velocity vector in NEU frame at the destination point of the spline segment
Vector3f _hermite_solution[4]; // array describing path between origin and destination
float _time; // current spline time (between 0 and 1) between origin and destination
float _speed_xy; // maximum horizontal speed
float _speed_up; // maximum speed upwards
float _speed_down; // maximum speed downwards
float _accel_xy; // maximum horizontal acceleration
float _accel_z; // maximum vertical acceleration
float _origin_speed_max; // maximum speed at origin
float _destination_speed_max; // maximum speed at destination
bool _reached_destination; // true once vehicle has reached destination
bool _zero_length; // true if spline is zero length
};