dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

L1: remove some functions that Rover doesn't need 

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer 2023-03-02 17:05:45 +01:00
parent 7edce94b93
commit feec8b2036
3 changed files with 0 additions and 289 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

195
src/lib/l1/ECL_L1_Pos_Controller.cpp
View File

@ -46,25 +46,7 @@
#include <float.h> #include <float.h>
using matrix::Vector2d;
using matrix::Vector2f; using matrix::Vector2f;
using matrix::wrap_pi;
void ECL_L1_Pos_Controller::update_roll_setpoint()
{
float roll_new = atanf(_lateral_accel * 1.0f / CONSTANTS_ONE_G);
roll_new = math::constrain(roll_new, -_roll_lim_rad, _roll_lim_rad);
if (_dt > 0.0f && _roll_slew_rate > 0.0f) {
// slew rate limiting active
roll_new = math::constrain(roll_new, _roll_setpoint - _roll_slew_rate * _dt, _roll_setpoint + _roll_slew_rate * _dt);
}
if (PX4_ISFINITE(roll_new)) {
_roll_setpoint = roll_new;
}
}
float ECL_L1_Pos_Controller::switch_distance(float wp_radius) float ECL_L1_Pos_Controller::switch_distance(float wp_radius)
{ {
@ -76,8 +58,6 @@ void
ECL_L1_Pos_Controller::navigate_waypoints(const Vector2f &vector_A, const Vector2f &vector_B, ECL_L1_Pos_Controller::navigate_waypoints(const Vector2f &vector_A, const Vector2f &vector_B,
const Vector2f &vector_curr_position, const Vector2f &ground_speed_vector) const Vector2f &vector_curr_position, const Vector2f &ground_speed_vector)
{ {
_has_guidance_updated = true;
/* this follows the logic presented in [1] */ /* this follows the logic presented in [1] */
float eta = 0.0f; float eta = 0.0f;
@ -202,181 +182,6 @@ ECL_L1_Pos_Controller::navigate_waypoints(const Vector2f &vector_A, const Vector
eta = math::constrain(eta, (-M_PI_F) / 2.0f, +M_PI_F / 2.0f); eta = math::constrain(eta, (-M_PI_F) / 2.0f, +M_PI_F / 2.0f);
_lateral_accel = _K_L1 * ground_speed * ground_speed / _L1_distance * sinf(eta); _lateral_accel = _K_L1 * ground_speed * ground_speed / _L1_distance * sinf(eta);
/* flying to waypoints, not circling them */
_circle_mode = false;
/* the bearing angle, in NED frame */
_bearing_error = eta;
update_roll_setpoint();
}
void
ECL_L1_Pos_Controller::navigate_loiter(const Vector2f &vector_A, const Vector2f &vector_curr_position, float radius,
const bool loiter_direction_counter_clockwise, const Vector2f &ground_speed_vector)
{
_has_guidance_updated = true;
const float loiter_direction_multiplier = loiter_direction_counter_clockwise ? -1.f : 1.f;
/* the complete guidance logic in this section was proposed by [2] */
/* calculate the gains for the PD loop (circle tracking) */
float omega = (2.0f * M_PI_F / _L1_period);
float K_crosstrack = omega * omega;
float K_velocity = 2.0f * _L1_damping * omega;
/* ground speed, enforce minimum of 0.1 m/s to avoid singularities */
float ground_speed = math::max(ground_speed_vector.length(), 0.1f);
/* calculate the L1 length required for the desired period */
_L1_distance = _L1_ratio * ground_speed;
/* calculate the vector from waypoint A to current position */
Vector2f vector_A_to_airplane = vector_curr_position - vector_A;
Vector2f vector_A_to_airplane_unit;
/* prevent NaN when normalizing */
if (vector_A_to_airplane.length() > FLT_EPSILON) {
/* store the normalized vector from waypoint A to current position */
vector_A_to_airplane_unit = vector_A_to_airplane.normalized();
} else {
vector_A_to_airplane_unit = vector_A_to_airplane;
}
/* update bearing to next waypoint */
_target_bearing = atan2f(-vector_A_to_airplane_unit(1), -vector_A_to_airplane_unit(0));
/* calculate eta angle towards the loiter center */
/* velocity across / orthogonal to line from waypoint to current position */
float xtrack_vel_center = vector_A_to_airplane_unit % ground_speed_vector;
/* velocity along line from waypoint to current position */
float ltrack_vel_center = - (ground_speed_vector * vector_A_to_airplane_unit);
float eta = atan2f(xtrack_vel_center, ltrack_vel_center);
/* limit eta to 90 degrees */
eta = math::constrain(eta, -M_PI_F / 2.0f, +M_PI_F / 2.0f);
/* calculate the lateral acceleration to capture the center point */
float lateral_accel_sp_center = _K_L1 * ground_speed * ground_speed / _L1_distance * sinf(eta);
/* for PD control: Calculate radial position and velocity errors */
/* radial velocity error */
float xtrack_vel_circle = -ltrack_vel_center;
/* radial distance from the loiter circle (not center) */
float xtrack_err_circle = vector_A_to_airplane.length() - radius;
/* cross track error for feedback */
_crosstrack_error = xtrack_err_circle;
/* calculate PD update to circle waypoint */
float lateral_accel_sp_circle_pd = (xtrack_err_circle * K_crosstrack + xtrack_vel_circle * K_velocity);
/* calculate velocity on circle / along tangent */
float tangent_vel = xtrack_vel_center * loiter_direction_multiplier;
/* prevent PD output from turning the wrong way when in circle mode */
const float l1_op_tan_vel = 2.f; // hard coded max tangential velocity in the opposite direction
if (tangent_vel < -l1_op_tan_vel && _circle_mode) {
lateral_accel_sp_circle_pd = math::max(lateral_accel_sp_circle_pd, 0.0f);
}
/* calculate centripetal acceleration setpoint */
float lateral_accel_sp_circle_centripetal = tangent_vel * tangent_vel / math::max((0.5f * radius),
(radius + xtrack_err_circle));
/* add PD control on circle and centripetal acceleration for total circle command */
float lateral_accel_sp_circle = loiter_direction_multiplier * (lateral_accel_sp_circle_pd +
lateral_accel_sp_circle_centripetal);
/*
* Switch between circle (loiter) and capture (towards waypoint center) mode when
* the commands switch over. Only fly towards waypoint if outside the circle.
*/
// XXX check switch over
if ((lateral_accel_sp_center < lateral_accel_sp_circle && !loiter_direction_counter_clockwise
&& xtrack_err_circle > 0.0f)
||
(lateral_accel_sp_center > lateral_accel_sp_circle && loiter_direction_counter_clockwise && xtrack_err_circle > 0.0f)) {
_lateral_accel = lateral_accel_sp_center;
_circle_mode = false;
/* angle between requested and current velocity vector */
_bearing_error = eta;
/* bearing from current position to L1 point */
_nav_bearing = atan2f(-vector_A_to_airplane_unit(1), -vector_A_to_airplane_unit(0));
} else {
_lateral_accel = lateral_accel_sp_circle;
_circle_mode = true;
_bearing_error = 0.0f;
/* bearing from current position to L1 point */
_nav_bearing = atan2f(-vector_A_to_airplane_unit(1), -vector_A_to_airplane_unit(0));
}
update_roll_setpoint();
}
void ECL_L1_Pos_Controller::navigate_heading(float navigation_heading, float current_heading,
const Vector2f &ground_speed_vector)
{
_has_guidance_updated = true;
/* the complete guidance logic in this section was proposed by [2] */
/*
* As the commanded heading is the only reference
* (and no crosstrack correction occurs),
* target and navigation bearing become the same
*/
_target_bearing = _nav_bearing = wrap_pi(navigation_heading);
float eta = wrap_pi(_target_bearing - wrap_pi(current_heading));
/* consequently the bearing error is exactly eta: */
_bearing_error = eta;
/* ground speed is the length of the ground speed vector */
float ground_speed = ground_speed_vector.length();
/* adjust L1 distance to keep constant frequency */
_L1_distance = ground_speed / _heading_omega;
float omega_vel = ground_speed * _heading_omega;
/* not circling a waypoint */
_circle_mode = false;
/* navigating heading means by definition no crosstrack error */
_crosstrack_error = 0;
/* limit eta to 90 degrees */
eta = math::constrain(eta, (-M_PI_F) / 2.0f, +M_PI_F / 2.0f);
_lateral_accel = 2.0f * sinf(eta) * omega_vel;
update_roll_setpoint();
}
void ECL_L1_Pos_Controller::navigate_level_flight(float current_heading)
{
_has_guidance_updated = true;
/* the logic in this section is trivial, but originally proposed by [2] */
/* reset all heading / error measures resulting in zero roll */
_target_bearing = current_heading;
_nav_bearing = current_heading;
_bearing_error = 0;
_crosstrack_error = 0;
_lateral_accel = 0;
/* not circling a waypoint when flying level */
_circle_mode = false;
update_roll_setpoint();
} }
void ECL_L1_Pos_Controller::set_l1_period(float period) void ECL_L1_Pos_Controller::set_l1_period(float period)

90
src/lib/l1/ECL_L1_Pos_Controller.hpp
View File

@ -83,14 +83,6 @@ public:
*/ */
float nav_lateral_acceleration_demand() { return _lateral_accel; } float nav_lateral_acceleration_demand() { return _lateral_accel; }
/**
* Heading error.
*
* The heading error is either compared to the current track
* or to the tangent of the current loiter radius.
*/
float bearing_error() { return _bearing_error; }
/** /**
* Bearing from aircraft to current target. * Bearing from aircraft to current target.
* *
@ -98,13 +90,6 @@ public:
*/ */
float target_bearing() { return _target_bearing; } float target_bearing() { return _target_bearing; }
/**
* Get roll angle setpoint for fixed wing.
*
* @return Roll angle (in NED frame)
*/
float get_roll_setpoint() { return _roll_setpoint; }
/** /**
* Get the current crosstrack error. * Get the current crosstrack error.
* *
@ -112,16 +97,6 @@ public:
*/ */
float crosstrack_error() { return _crosstrack_error; } float crosstrack_error() { return _crosstrack_error; }
/**
* Returns true if the loiter waypoint has been reached
*/
bool reached_loiter_target() { return _circle_mode; }
/**
* Returns true if following a circle (loiter)
*/
bool circle_mode() { return _circle_mode; }
/** /**
* Get the switch distance * Get the switch distance
* *
@ -145,35 +120,6 @@ public:
*/ */
void navigate_waypoints(const matrix::Vector2f &vector_A, const matrix::Vector2f &vector_B, void navigate_waypoints(const matrix::Vector2f &vector_A, const matrix::Vector2f &vector_B,
const matrix::Vector2f &vector_curr_position, const matrix::Vector2f &ground_speed); const matrix::Vector2f &vector_curr_position, const matrix::Vector2f &ground_speed);
/**
* Navigate on an orbit around a loiter waypoint.
*
* This allow orbits smaller than the L1 length,
* this modification was introduced in [2].
*
* @return sets _lateral_accel setpoint
*/
void navigate_loiter(const matrix::Vector2f &vector_A, const matrix::Vector2f &vector_curr_position, float radius,
const bool loiter_direction_counter_clockwise, const matrix::Vector2f &ground_speed_vector);
/**
* Navigate on a fixed bearing.
*
* This only holds a certain direction and does not perform cross
* track correction. Helpful for semi-autonomous modes. Introduced
* by [2].
*
* @return sets _lateral_accel setpoint
*/
void navigate_heading(float navigation_heading, float current_heading, const matrix::Vector2f &ground_speed);
/**
* Keep the wings level.
*
* This is typically needed for maximum-lift-demand situations,
* such as takeoff or near stall. Introduced in [2].
*/
void navigate_level_flight(float current_heading);
/** /**
* Set the L1 period. * Set the L1 period.
@ -187,32 +133,11 @@ public:
*/ */
void set_l1_damping(float damping); void set_l1_damping(float damping);
/**
* Set the maximum roll angle output in radians
*/
void set_l1_roll_limit(float roll_lim_rad) { _roll_lim_rad = roll_lim_rad; }
/**
* Set roll angle slew rate. Set to zero to deactivate.
*/
void set_roll_slew_rate(float roll_slew_rate) { _roll_slew_rate = roll_slew_rate; }
/**
* Set control loop dt. The value will be used to apply roll angle setpoint slew rate limiting.
*/
void set_dt(float dt) { _dt = dt;}
void reset_has_guidance_updated() { _has_guidance_updated = false; }
bool has_guidance_updated() { return _has_guidance_updated; }
private: private:
float _lateral_accel{0.0f}; ///< Lateral acceleration setpoint in m/s^2 float _lateral_accel{0.0f}; ///< Lateral acceleration setpoint in m/s^2
float _L1_distance{20.0f}; ///< L1 lead distance, defined by period and damping float _L1_distance{20.0f}; ///< L1 lead distance, defined by period and damping
bool _circle_mode{false}; ///< flag for loiter mode
float _nav_bearing{0.0f}; ///< bearing to L1 reference point float _nav_bearing{0.0f}; ///< bearing to L1 reference point
float _bearing_error{0.0f}; ///< bearing error
float _crosstrack_error{0.0f}; ///< crosstrack error in meters float _crosstrack_error{0.0f}; ///< crosstrack error in meters
float _target_bearing{0.0f}; ///< the heading setpoint float _target_bearing{0.0f}; ///< the heading setpoint
@ -221,21 +146,6 @@ private:
float _L1_ratio{5.0f}; ///< L1 ratio for navigation float _L1_ratio{5.0f}; ///< L1 ratio for navigation
float _K_L1{2.0f}; ///< L1 control gain for _L1_damping float _K_L1{2.0f}; ///< L1 control gain for _L1_damping
float _heading_omega{1.0f}; ///< Normalized frequency float _heading_omega{1.0f}; ///< Normalized frequency
float _roll_lim_rad{math::radians(30.0f)}; ///<maximum roll angle in radians
float _roll_setpoint{0.0f}; ///< current roll angle setpoint in radians
float _roll_slew_rate{0.0f}; ///< roll angle setpoint slew rate limit in rad/s
float _dt{0}; ///< control loop time in seconds
bool _has_guidance_updated =
false; ///< this flag is set to true by any of the guidance methods. This flag has to be manually reset using has_guidance_updated_reset()
/**
* Update roll angle setpoint. This will also apply slew rate limits if set.
*
*/
void update_roll_setpoint();
}; };

4
src/modules/rover_pos_control/RoverPositionControl.cpp
View File

@ -93,7 +93,6 @@ void RoverPositionControl::parameters_update(bool force)
_gnd_control.set_l1_damping(_param_l1_damping.get()); _gnd_control.set_l1_damping(_param_l1_damping.get());
_gnd_control.set_l1_period(_param_l1_period.get()); _gnd_control.set_l1_period(_param_l1_period.get());
_gnd_control.set_l1_roll_limit(math::radians(0.0f));
pid_init(&_speed_ctrl, PID_MODE_DERIVATIV_CALC, 0.01f); pid_init(&_speed_ctrl, PID_MODE_DERIVATIV_CALC, 0.01f);
pid_set_parameters(&_speed_ctrl, pid_set_parameters(&_speed_ctrl,
@ -215,9 +214,6 @@ RoverPositionControl::control_position(const matrix::Vector2d &current_position,
_control_mode_current = UGV_POSCTRL_MODE_AUTO; _control_mode_current = UGV_POSCTRL_MODE_AUTO;
/* get circle mode */
//bool was_circle_mode = _gnd_control.circle_mode();
/* current waypoint (the one currently heading for) */ /* current waypoint (the one currently heading for) */
matrix::Vector2d curr_wp(pos_sp_triplet.current.lat, pos_sp_triplet.current.lon); matrix::Vector2d curr_wp(pos_sp_triplet.current.lat, pos_sp_triplet.current.lon);