Line data Source code
1 : /*
2 : This program is free software: you can redistribute it and/or modify
3 : it under the terms of the GNU General Public License as published by
4 : the Free Software Foundation, either version 3 of the License, or
5 : (at your option) any later version.
6 :
7 : This program is distributed in the hope that it will be useful,
8 : but WITHOUT ANY WARRANTY; without even the implied warranty of
9 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 : GNU General Public License for more details.
11 :
12 : You should have received a copy of the GNU General Public License
13 : along with this program. If not, see <http://www.gnu.org/licenses/>.
14 : */
15 :
16 : #include <AP_HAL/AP_HAL.h>
17 : #include <AP_Math/AP_Math.h>
18 : #include <AP_InternalError/AP_InternalError.h>
19 : #include "SplineCurve.h"
20 :
21 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
22 : #include <stdio.h>
23 : #endif
24 :
25 : extern const AP_HAL::HAL &hal;
26 :
27 : #define SPLINE_FACTOR 4.0f // defines shape of curves. larger numbers result in longer spline curves, lower numbers take a direct path
28 : #define TANGENTIAL_ACCEL_SCALER 0.5f // the proportion of the maximum accel that can be used for tangential acceleration (aka in the direction of travel along the track)
29 : #define LATERAL_ACCEL_SCALER 0.5f // the proportion of the maximum accel that can be used for lateral acceleration (aka crosstrack acceleration)
30 :
31 : // limit the maximum speed along the track to that which will achieve a cornering (aka lateral) acceleration of LATERAL_SPEED_SCALER * acceleration limit
32 :
33 : // set maximum speed and acceleration
34 38 : void SplineCurve::set_speed_accel(float speed_xy, float speed_up, float speed_down,
35 : float accel_xy, float accel_z)
36 : {
37 38 : _speed_xy = fabsf(speed_xy);
38 38 : _speed_up = fabsf(speed_up);
39 38 : _speed_down = fabsf(speed_down);
40 38 : _accel_xy = fabsf(accel_xy);
41 38 : _accel_z = fabsf(accel_z);
42 38 : }
43 :
44 : // set origin and destination using position vectors (offset from EKF origin)
45 : // origin_vel is vehicle velocity at origin (in NED frame)
46 : // destination_vel is vehicle velocity at destination (in NED frame)
47 : // time is reset to zero
48 38 : void SplineCurve::set_origin_and_destination(const Vector3p &origin, const Vector3p &destination, const Vector3f &origin_vel, const Vector3f &destination_vel)
49 : {
50 : // store origin and destination locations
51 38 : _origin = origin;
52 38 : _destination = destination;
53 :
54 : // handle zero length track
55 38 : _zero_length = is_zero((destination - origin).length_squared());
56 38 : if (_zero_length) {
57 5 : _time = 1.0f;
58 5 : _origin_vel.zero();
59 5 : _destination_vel.zero();
60 5 : _reached_destination = true;
61 5 : _origin_speed_max = 0.0f;
62 5 : _destination_speed_max = 0.0f;
63 5 : return;
64 : }
65 :
66 33 : _origin_vel = origin_vel;
67 33 : _destination_vel = destination_vel;
68 33 : _reached_destination = false;
69 :
70 : // reset time
71 : // Note: _time could include left-over from previous waypoint
72 33 : _time = 0.0f;
73 :
74 : // code below ensures we don't get too much overshoot when the next segment is short
75 33 : const float vel_len = _origin_vel.length() + _destination_vel.length();
76 33 : const float pos_len = (_destination - _origin).length() * SPLINE_FACTOR;
77 33 : if (vel_len > pos_len) {
78 : // if total start+stop velocity is more than four times the position difference
79 : // use a scaled down start and stop velocity
80 13 : const float vel_scaling = pos_len / vel_len;
81 : // update spline calculator
82 13 : update_solution(_origin, _destination, _origin_vel * vel_scaling, _destination_vel * vel_scaling);
83 : } else {
84 : // update spline calculator
85 20 : update_solution(_origin, _destination, _origin_vel, _destination_vel);
86 : }
87 33 : Vector3p target_pos;
88 33 : Vector3f spline_vel_unit;
89 : float spline_dt;
90 : float accel_max;
91 33 : calc_dt_speed_max(0.0f, 0.0f, spline_dt, target_pos, spline_vel_unit, _origin_speed_max, accel_max);
92 33 : if (_destination_vel.is_zero()) {
93 3 : _destination_speed_max = 0.0f;
94 : } else {
95 30 : calc_dt_speed_max(1.0f, 0.0f, spline_dt, target_pos, spline_vel_unit, _destination_speed_max, accel_max);
96 : }
97 : }
98 :
99 : // move target location along track from origin to destination
100 : // target_pos is updated with the target position from EKF origin in NED frame
101 : // target_vel is updated with the target velocity in NED frame
102 107717 : void SplineCurve::advance_target_along_track(float dt, Vector3p &target_pos, Vector3f &target_vel)
103 : {
104 : // handle zero length track
105 107717 : if (_zero_length) {
106 4804 : target_pos = _destination;
107 4804 : target_vel.zero();
108 4804 : return;
109 : }
110 :
111 : // calculate target position and velocity using spline calculator
112 102913 : float speed_cms = target_vel.length();
113 102913 : const float distance_delta = speed_cms * dt;
114 : float spline_dt;
115 102913 : Vector3f spline_vel_unit;
116 : float speed_max;
117 : float accel_max;
118 :
119 102913 : calc_dt_speed_max(_time, distance_delta, spline_dt, target_pos, spline_vel_unit, speed_max, accel_max);
120 102913 : speed_cms = constrain_float(speed_max, speed_cms - accel_max * dt, speed_cms + accel_max * dt);
121 102913 : target_vel = spline_vel_unit * speed_cms;
122 :
123 102913 : _time += spline_dt;
124 :
125 : // we will reach the destination in the next step so set reached_destination flag
126 102913 : if (_time >= 1.0f) {
127 2420 : _time = 1.0f;
128 2420 : _reached_destination = true;
129 : }
130 : }
131 :
132 : // calculate the spline delta time for a given delta distance
133 : // returns the spline position and velocity and maximum speed and acceleration the vehicle can travel without exceeding acceleration limits
134 102976 : void SplineCurve::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)
135 : {
136 : // initialise outputs
137 102976 : spline_dt = 0.0f;
138 102976 : spline_vel_unit.zero();
139 102976 : speed_max = 0.0f;
140 102976 : accel_max = 0.0f;
141 :
142 : // calculate target position and velocity using spline calculator
143 102976 : Vector3f spline_vel;
144 102976 : Vector3f spline_accel;
145 102976 : Vector3f spline_jerk;
146 :
147 102976 : calc_target_pos_vel(time, target_pos, spline_vel, spline_accel, spline_jerk);
148 :
149 : // vel, accel and jerk should never all be zero
150 102976 : if (spline_vel.is_zero() && spline_accel.is_zero() && spline_jerk.is_zero()) {
151 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
152 0 : ::printf("SplineCurve::calc_dt_speed_max vel, accel and jerk are all zero\n");
153 : #endif
154 0 : INTERNAL_ERROR(AP_InternalError::error_t::invalid_arg_or_result);
155 0 : _reached_destination = true;
156 0 : return;
157 : }
158 :
159 : // aircraft velocity and acceleration along the spline will be defined based on the aircraft kinematic limits
160 : // aircraft velocity along the spline should be reduced to ensure normal accelerations do not exceed kinematic limits
161 102976 : const float spline_vel_length = spline_vel.length();
162 102976 : if (is_zero(spline_vel_length)) {
163 : // if spline velocity is zero then direction must be defined by acceleration or jerk
164 2418 : if (is_zero(spline_accel.length_squared())) {
165 : // if acceleration is zero then direction must be defined by jerk
166 0 : spline_vel_unit = spline_jerk.normalized();
167 0 : spline_dt = powf(6.0f * distance_delta / spline_jerk.length(), 1.0f/3.0f);
168 : } else {
169 : // all spline acceleration is in the direction of travel
170 2418 : spline_vel_unit = spline_accel.normalized();
171 2418 : spline_dt = safe_sqrt(2.0f * distance_delta / spline_accel.length());
172 : }
173 : } else {
174 100558 : spline_vel_unit = spline_vel.normalized();
175 100558 : spline_dt = distance_delta / spline_vel_length;
176 : }
177 :
178 : // calculate acceleration normal to the direction of travel
179 102976 : const float spline_accel_tangent_length = spline_accel.dot(spline_vel_unit);
180 102976 : Vector3f spline_accel_norm = spline_accel - (spline_vel_unit * spline_accel_tangent_length);
181 102976 : const float spline_accel_norm_length = spline_accel_norm.length();
182 :
183 : // limit the maximum speed along the track to that which will achieve a cornering (aka lateral) acceleration of LATERAL_SPEED_SCALER * acceleration limit
184 102976 : const float tangential_speed_max = kinematic_limit(spline_vel_unit, _speed_xy, _speed_up, _speed_down);
185 102976 : const float accel_norm_max = LATERAL_ACCEL_SCALER * kinematic_limit(spline_accel_norm, _accel_xy, _accel_z, _accel_z);
186 :
187 : // sanity check to avoid divide by zero
188 102976 : if (is_zero(tangential_speed_max)) {
189 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
190 0 : ::printf("SplineCurve::calc_dt_speed_max tangential_speed_max is zero\n");
191 : #endif
192 0 : INTERNAL_ERROR(AP_InternalError::error_t::invalid_arg_or_result);
193 0 : _reached_destination = true;
194 0 : return;
195 : }
196 :
197 193656 : if ((is_positive(accel_norm_max)) && is_positive(spline_accel_norm_length) && is_positive(spline_vel_length) &&
198 90680 : ((spline_accel_norm_length/accel_norm_max) > sq(spline_vel_length/tangential_speed_max))) {
199 35731 : speed_max = spline_vel_length / safe_sqrt(spline_accel_norm_length/accel_norm_max);
200 : } else {
201 67245 : speed_max = tangential_speed_max;
202 : }
203 :
204 : // calculate accel max and sanity check
205 102976 : accel_max = TANGENTIAL_ACCEL_SCALER * kinematic_limit(spline_vel_unit, _accel_xy, _accel_z, _accel_z);
206 102976 : if (is_zero(accel_max)) {
207 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
208 0 : ::printf("SplineCurve::calc_dt_speed_max accel_max is zero\n");
209 : #endif
210 0 : INTERNAL_ERROR(AP_InternalError::error_t::invalid_arg_or_result);
211 0 : _reached_destination = true;
212 0 : return;
213 : }
214 102976 : const float dist = (_destination - target_pos).length();
215 102976 : speed_max = MIN(speed_max, safe_sqrt(2.0f * accel_max * (dist + sq(_destination_speed_max) / (2.0f*accel_max))));
216 : }
217 :
218 : // recalculate hermite_solution grid
219 : // relies on _origin_vel, _destination_vel and _origin and _destination
220 33 : void SplineCurve::update_solution(const Vector3p &origin, const Vector3p &dest, const Vector3f &origin_vel, const Vector3f &dest_vel)
221 : {
222 33 : _hermite_solution[0] = origin;
223 33 : _hermite_solution[1] = origin_vel.topostype();
224 33 : _hermite_solution[2] = -origin * 3.0 - origin_vel.topostype() * 2.0 + dest * 3.0 - dest_vel.topostype();
225 33 : _hermite_solution[3] = origin * 2.0 + origin_vel.topostype() - dest * 2.0 + dest_vel.topostype();
226 33 : }
227 :
228 : // calculate target position and velocity from given spline time
229 : // time is a value from 0 to 1
230 : // position is updated with target position as an offset from EKF origin in NED frame
231 : // velocity is updated with the unscaled velocity
232 : // relies on set_origin_and_destination having been called to update_solution
233 102976 : void SplineCurve::calc_target_pos_vel(float time, Vector3p &position, Vector3f &velocity, Vector3f &acceleration, Vector3f &jerk)
234 : {
235 102976 : const float time_sq = sq(time);
236 102976 : const float time_cubed = time_sq * time;
237 :
238 102976 : position = _hermite_solution[0] + \
239 102976 : _hermite_solution[1] * time + \
240 205952 : _hermite_solution[2] * time_sq + \
241 205952 : _hermite_solution[3] * time_cubed;
242 :
243 102976 : velocity = _hermite_solution[1].tofloat() + \
244 205952 : _hermite_solution[2].tofloat() * 2.0 * time + \
245 205952 : _hermite_solution[3].tofloat() * 3.0 * time_sq;
246 :
247 102976 : acceleration = _hermite_solution[2].tofloat() * 2.0 + \
248 205952 : _hermite_solution[3].tofloat() * 6.0 * time;
249 :
250 102976 : jerk = _hermite_solution[3].tofloat() * 6.0;
251 102976 : }
252 :
|
