@@ -182,7 +182,7 @@ class NPFG
182
182
183
183
/*
184
184
* @return Feed-forward lateral acceleration command increment for tracking
185
- * path curvature [m/s^2]
185
+ * path curvature [m/s^2]
186
186
*/
187
187
float getLateralAccelFF () const { return lateral_accel_ff_; }
188
188
@@ -266,28 +266,28 @@ class NPFG
266
266
267
267
/*
268
268
* Keep the wings level.
269
- *
270
- * @param[in] heading Heading angle [rad]
269
+ *
270
+ * @param[in] heading Heading angle [rad]
271
271
*/
272
272
void navigateLevelFlight (const float heading);
273
273
274
274
/*
275
- * [Copied directly from ECL_L1_Pos_Controller]
276
- *
275
+ * [Copied directly from ECL_L1_Pos_Controller]
276
+ *
277
277
* Set the maximum roll angle output in radians
278
278
*/
279
279
void setRollLimit (float roll_lim_rad) { roll_lim_rad_ = roll_lim_rad; }
280
280
281
281
/*
282
- * [Copied directly from ECL_L1_Pos_Controller]
283
- *
282
+ * [Copied directly from ECL_L1_Pos_Controller]
283
+ *
284
284
* Set roll angle slew rate. Set to zero to deactivate.
285
285
*/
286
286
void setRollSlewRate (float roll_slew_rate) { roll_slew_rate_ = roll_slew_rate; }
287
287
288
288
/*
289
- * [Copied directly from ECL_L1_Pos_Controller]
290
- *
289
+ * [Copied directly from ECL_L1_Pos_Controller]
290
+ *
291
291
* Set control loop dt. The value will be used to apply roll angle setpoint slew rate limiting.
292
292
*/
293
293
void setDt (const float dt) { dt_ = dt; }
@@ -410,7 +410,7 @@ class NPFG
410
410
411
411
/*
412
412
* Adapts the controller period considering user defined inputs, current flight
413
- * condition, path properties, and stability bounds.
413
+ * condition, path properties, and stability bounds.
414
414
*
415
415
* @param[in] ground_speed Vehicle ground speed [m/s]
416
416
* @param[in] airspeed Vehicle airspeed [m/s]
@@ -420,8 +420,8 @@ class NPFG
420
420
* @param[in] wind_vel Wind velocity vector in inertial frame [m/s]
421
421
* @param[in] unit_path_tangent Unit vector tangent to path at closest point
422
422
* in direction of path
423
- * @param[in] feas_on_track Bearing feasibility on track at the closest point
424
- * @return Adapted period [s]
423
+ * @param[in] feas_on_track Bearing feasibility on track at the closest point
424
+ * @return Adapted period [s]
425
425
*/
426
426
float adaptPeriod (const float ground_speed, const float airspeed, const float wind_ratio,
427
427
const float track_error, const float path_curvature, const matrix::Vector2f &wind_vel,
@@ -484,35 +484,36 @@ class NPFG
484
484
* track error vector to that of the path tangent vector.
485
485
*
486
486
* @param[in] ground_speed Vehicle ground speed [m/s]
487
- * ADSFASDFSAFDSF
487
+ * @param[in] time_const Controller time constant [s]
488
488
* @return Track error boundary [m]
489
489
*/
490
490
float trackErrorBound (const float ground_speed, const float time_const) const ;
491
491
492
492
/*
493
- * Calculates the required controller proportional gain to achieve the desired
494
- * system period and damping ratio. NOTE: actual period and damping will vary
495
- * when following paths with curvature in wind.
493
+ * Calculates the required controller proportional gain to achieve the desired
494
+ * system period and damping ratio. NOTE: actual period and damping will vary
495
+ * when following paths with curvature in wind.
496
496
*
497
497
* @param[in] period Desired system period [s]
498
- * @param[in] damping Desired system damping ratio
499
- * @return Proportional gain [rad/s]
498
+ * @param[in] damping Desired system damping ratio
499
+ * @return Proportional gain [rad/s]
500
500
*/
501
501
float pGain (const float period, const float damping) const ;
502
502
503
503
/*
504
504
* Calculates the required controller time constant to achieve the desired
505
- * system period and damping ratio. NOTE: actual period and damping will vary
506
- * when following paths with curvature in wind.
505
+ * system period and damping ratio. NOTE: actual period and damping will vary
506
+ * when following paths with curvature in wind.
507
507
*
508
508
* @param[in] period Desired system period [s]
509
- * @param[in] damping Desired system damping ratio
510
- * @return Time constant [s]
509
+ * @param[in] damping Desired system damping ratio
510
+ * @return Time constant [s]
511
511
*/
512
512
float timeConst (const float period, const float damping) const ;
513
513
514
514
/*
515
- * Cacluates the look ahead angle as a function of the normalized track error.
515
+ * Cacluates the look ahead angle as a quadratic function of the normalized
516
+ * track error.
516
517
*
517
518
* @param[in] normalized_track_error Normalized track error (track error / track error boundary)
518
519
* @return Look ahead angle [rad]
@@ -523,14 +524,12 @@ class NPFG
523
524
* Calculates the bearing vector and track proximity transitioning variable
524
525
* from the look-ahead angle mapping.
525
526
*
526
- * @param[out] track_proximity Smoothing parameter based on vehicle proximity
527
- * to track with values between 0 (at track error boundary) and 1 (on track)
528
- * @param[in] unit_track_error Unit vector in direction from vehicle to
529
- * closest point on path
530
527
* @param[in] unit_path_tangent Unit vector tangent to path at closest point
531
528
* in direction of path
532
529
* @param[in] look_ahead_ang The bearing vector lies at this angle from
533
530
* the path normal vector [rad]
531
+ * @param[in] signed_track_error Signed error to track at closest point (sign
532
+ * determined by path normal direction) [m]
534
533
* @return Unit bearing vector
535
534
*/
536
535
matrix::Vector2f bearingVec (const matrix::Vector2f &unit_path_tangent, const float look_ahead_ang,
@@ -541,7 +540,7 @@ class NPFG
541
540
* ground speed maintanence as well as track keeping logic.
542
541
*
543
542
* @param[in] normalized_track_error Normalized track error (track error / track error boundary)
544
- * @param[in] feas Bearing feasibility
543
+ * @param[in] feas Bearing feasibility
545
544
* @return Minimum forward ground speed demand [m/s]
546
545
*/
547
546
float minGroundSpeed (const float normalized_track_error, const float feas);
@@ -626,7 +625,7 @@ class NPFG
626
625
627
626
/*
628
627
* Calculates an additional feed-forward lateral acceleration demand considering
629
- * the path curvature. The full effect of the acceleration increment is smoothly
628
+ * the path curvature. The full effect of the acceleration increment is smoothly
630
629
* ramped in as the vehicle approaches the track and is further smoothly
631
630
* zeroed out as the bearing becomes infeasible.
632
631
*
@@ -692,8 +691,8 @@ class NPFG
692
691
matrix::Vector2f getLocalPlanarVector (const matrix::Vector2d &origin, const matrix::Vector2d &target) const ;
693
692
694
693
/* *
695
- * [Copied directly from ECL_L1_Pos_Controller]
696
- *
694
+ * [Copied directly from ECL_L1_Pos_Controller]
695
+ *
697
696
* Update roll angle setpoint. This will also apply slew rate limits if set.
698
697
*/
699
698
void updateRollSetpoint ();
0 commit comments