zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Convert L1 controller to local coordinates

v1.13.0-BW
Jaeyoung Lim 3 years ago committed by JaeyoungLim
parent
897506267d
commit
28d34bf095
4 changed files with 59 additions and 28 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 31
      src/lib/l1/ECL_L1_Pos_Controller.cpp
  2. 7
      src/lib/l1/ECL_L1_Pos_Controller.hpp
  3. 43
      src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
  4. 6
      src/modules/rover_pos_control/RoverPositionControl.cpp

31
src/lib/l1/ECL_L1_Pos_Controller.cpp
Unescape View File

@ -73,8 +73,8 @@ float ECL_L1_Pos_Controller::switch_distance(float wp_radius)
} }
void void
ECL_L1_Pos_Controller::navigate_waypoints(const Vector2d &vector_A, const Vector2d &vector_B, ECL_L1_Pos_Controller::navigate_waypoints(const Vector2f &vector_A, const Vector2f &vector_B,
const Vector2d &vector_curr_position, const Vector2f &ground_speed_vector) const Vector2f &vector_curr_position, const Vector2f &ground_speed_vector)
{ {
_has_guidance_updated = true; _has_guidance_updated = true;
@ -82,8 +82,9 @@ ECL_L1_Pos_Controller::navigate_waypoints(const Vector2d &vector_A, const Vector
float eta = 0.0f; float eta = 0.0f;
/* get the direction between the last (visited) and next waypoint */ /* get the direction between the last (visited) and next waypoint */
_target_bearing = get_bearing_to_next_waypoint(vector_curr_position(0), vector_curr_position(1), vector_B(0), Vector2f vector_B_to_P = vector_B - vector_curr_position;
vector_B(1)); Vector2f vector_B_to_P_unit = vector_B_to_P.normalized();
_target_bearing = atan2f(vector_B_to_P_unit(1), vector_B_to_P_unit(0));
/* enforce a minimum ground speed of 0.1 m/s to avoid singularities */ /* enforce a minimum ground speed of 0.1 m/s to avoid singularities */
float ground_speed = math::max(ground_speed_vector.length(), 0.1f); float ground_speed = math::max(ground_speed_vector.length(), 0.1f);
@ -92,20 +93,20 @@ ECL_L1_Pos_Controller::navigate_waypoints(const Vector2d &vector_A, const Vector
_L1_distance = _L1_ratio * ground_speed; _L1_distance = _L1_ratio * ground_speed;
/* calculate vector from A to B */ /* calculate vector from A to B */
Vector2f vector_AB = get_local_planar_vector(vector_A, vector_B); Vector2f vector_AB = vector_B - vector_A;
/* /*
* check if waypoints are on top of each other. If yes, * check if waypoints are on top of each other. If yes,
* skip A and directly continue to B * skip A and directly continue to B
*/ */
if (vector_AB.length() < 1.0e-6f) { if (vector_AB.length() < 1.0e-6f) {
vector_AB = get_local_planar_vector(vector_curr_position, vector_B); vector_AB = vector_B - vector_curr_position;
} }
vector_AB.normalize(); vector_AB.normalize();
/* calculate the vector from waypoint A to the aircraft */ /* calculate the vector from waypoint A to the aircraft */
Vector2f vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position); Vector2f vector_A_to_airplane = vector_curr_position - vector_A;
/* calculate crosstrack error (output only) */ /* calculate crosstrack error (output only) */
_crosstrack_error = vector_AB % vector_A_to_airplane; _crosstrack_error = vector_AB % vector_A_to_airplane;
@ -119,12 +120,13 @@ ECL_L1_Pos_Controller::navigate_waypoints(const Vector2d &vector_A, const Vector
float alongTrackDist = vector_A_to_airplane * vector_AB; float alongTrackDist = vector_A_to_airplane * vector_AB;
/* estimate airplane position WRT to B */ /* estimate airplane position WRT to B */
Vector2f vector_B_to_P_unit = get_local_planar_vector(vector_B, vector_curr_position).normalized(); Vector2f vector_P_to_B = vector_curr_position - vector_B;
Vector2f vector_P_to_B_unit = vector_P_to_B.normalized();
/* calculate angle of airplane position vector relative to line) */ /* calculate angle of airplane position vector relative to line) */
// XXX this could probably also be based solely on the dot product // XXX this could probably also be based solely on the dot product
float AB_to_BP_bearing = atan2f(vector_B_to_P_unit % vector_AB, vector_B_to_P_unit * vector_AB); float AB_to_BP_bearing = atan2f(vector_P_to_B_unit % vector_AB, vector_P_to_B_unit * vector_AB);
/* extension from [2], fly directly to A */ /* extension from [2], fly directly to A */
if (distance_A_to_airplane > _L1_distance && alongTrackDist / math::max(distance_A_to_airplane, 1.0f) < -0.7071f) { if (distance_A_to_airplane > _L1_distance && alongTrackDist / math::max(distance_A_to_airplane, 1.0f) < -0.7071f) {
@ -210,7 +212,7 @@ ECL_L1_Pos_Controller::navigate_waypoints(const Vector2d &vector_A, const Vector
} }
void void
ECL_L1_Pos_Controller::navigate_loiter(const Vector2d &vector_A, const Vector2d &vector_curr_position, float radius, ECL_L1_Pos_Controller::navigate_loiter(const Vector2f &vector_A, const Vector2f &vector_curr_position, float radius,
int8_t loiter_direction, const Vector2f &ground_speed_vector) int8_t loiter_direction, const Vector2f &ground_speed_vector)
{ {
_has_guidance_updated = true; _has_guidance_updated = true;
@ -222,10 +224,6 @@ ECL_L1_Pos_Controller::navigate_loiter(const Vector2d &vector_A, const Vector2d
float K_crosstrack = omega * omega; float K_crosstrack = omega * omega;
float K_velocity = 2.0f * _L1_damping * omega; float K_velocity = 2.0f * _L1_damping * omega;
/* update bearing to next waypoint */
_target_bearing = get_bearing_to_next_waypoint(vector_curr_position(0), vector_curr_position(1), vector_A(0),
vector_A(1));
/* ground speed, enforce minimum of 0.1 m/s to avoid singularities */ /* ground speed, enforce minimum of 0.1 m/s to avoid singularities */
float ground_speed = math::max(ground_speed_vector.length(), 0.1f); float ground_speed = math::max(ground_speed_vector.length(), 0.1f);
@ -233,7 +231,7 @@ ECL_L1_Pos_Controller::navigate_loiter(const Vector2d &vector_A, const Vector2d
_L1_distance = _L1_ratio * ground_speed; _L1_distance = _L1_ratio * ground_speed;
/* calculate the vector from waypoint A to current position */ /* calculate the vector from waypoint A to current position */
Vector2f vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position); Vector2f vector_A_to_airplane = vector_curr_position - vector_A;
Vector2f vector_A_to_airplane_unit; Vector2f vector_A_to_airplane_unit;
@ -246,6 +244,9 @@ ECL_L1_Pos_Controller::navigate_loiter(const Vector2d &vector_A, const Vector2d
vector_A_to_airplane_unit = vector_A_to_airplane; 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 */ /* calculate eta angle towards the loiter center */
/* velocity across / orthogonal to line from waypoint to current position */ /* velocity across / orthogonal to line from waypoint to current position */

7
src/lib/l1/ECL_L1_Pos_Controller.hpp
Unescape View File

@ -143,9 +143,8 @@ public:
* *
* @return sets _lateral_accel setpoint * @return sets _lateral_accel setpoint
*/ */
void navigate_waypoints(const matrix::Vector2d &vector_A, const matrix::Vector2d &vector_B, void navigate_waypoints(const matrix::Vector2f &vector_A, const matrix::Vector2f &vector_B,
const matrix::Vector2d &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. * Navigate on an orbit around a loiter waypoint.
* *
@ -154,7 +153,7 @@ public:
* *
* @return sets _lateral_accel setpoint * @return sets _lateral_accel setpoint
*/ */
void navigate_loiter(const matrix::Vector2d &vector_A, const matrix::Vector2d &vector_curr_position, float radius, void navigate_loiter(const matrix::Vector2f &vector_A, const matrix::Vector2f &vector_curr_position, float radius,
int8_t loiter_direction, const matrix::Vector2f &ground_speed_vector); int8_t loiter_direction, const matrix::Vector2f &ground_speed_vector);
/** /**

43
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
Unescape View File

@ -1208,7 +1208,10 @@ FixedwingPositionControl::control_auto_position(const hrt_abstime &now, const fl
target_airspeed = _npfg.getAirspeedRef() / _eas2tas; target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
} else { } else {
_l1_control.navigate_waypoints(prev_wp, curr_wp, curr_pos, get_nav_speed_2d(ground_speed)); Vector2f curr_pos_local{_local_pos.x, _local_pos.y};
Vector2f curr_wp_local = _global_local_proj_ref.project(curr_wp(0), curr_wp(1));
Vector2f prev_wp_local = _global_local_proj_ref.project(prev_wp(0), prev_wp(1));
_l1_control.navigate_waypoints(prev_wp_local, curr_wp_local, curr_pos_local, get_nav_speed_2d(ground_speed));
_att_sp.roll_body = _l1_control.get_roll_setpoint(); _att_sp.roll_body = _l1_control.get_roll_setpoint();
} }
@ -1377,7 +1380,10 @@ FixedwingPositionControl::control_auto_loiter(const hrt_abstime &now, const floa
target_airspeed = _npfg.getAirspeedRef() / _eas2tas; target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
} else { } else {
_l1_control.navigate_loiter(curr_wp, curr_pos, loiter_radius, loiter_direction, get_nav_speed_2d(ground_speed)); Vector2f curr_pos_local{_local_pos.x, _local_pos.y};
Vector2f curr_wp_local = _global_local_proj_ref.project(curr_wp(0), curr_wp(1));
_l1_control.navigate_loiter(curr_wp_local, curr_pos_local, loiter_radius, loiter_direction,
get_nav_speed_2d(ground_speed));
_att_sp.roll_body = _l1_control.get_roll_setpoint(); _att_sp.roll_body = _l1_control.get_roll_setpoint();
} }
@ -1480,7 +1486,11 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
target_airspeed = _npfg.getAirspeedRef() / _eas2tas; target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
} else { } else {
_l1_control.navigate_waypoints(_runway_takeoff.getStartWP(), curr_wp, curr_pos, ground_speed); Vector2f curr_pos_local{_local_pos.x, _local_pos.y};
Vector2f curr_wp_local = _global_local_proj_ref.project(curr_wp(0), curr_wp(1));
Vector2f prev_wp_local = _global_local_proj_ref.project(_runway_takeoff.getStartWP()(0),
_runway_takeoff.getStartWP()(1));
_l1_control.navigate_waypoints(prev_wp_local, curr_wp_local, curr_pos_local, ground_speed);
_att_sp.roll_body = _runway_takeoff.getRoll(_l1_control.get_roll_setpoint()); _att_sp.roll_body = _runway_takeoff.getRoll(_l1_control.get_roll_setpoint());
} }
@ -1550,7 +1560,11 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
target_airspeed = _npfg.getAirspeedRef() / _eas2tas; target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
} else { } else {
_l1_control.navigate_waypoints(prev_wp, curr_wp, curr_pos, ground_speed); Vector2f curr_pos_local{_local_pos.x, _local_pos.y};
Vector2f curr_wp_local = _global_local_proj_ref.project(curr_wp(0), curr_wp(1));
Vector2f prev_wp_local = _global_local_proj_ref.project(_runway_takeoff.getStartWP()(0),
_runway_takeoff.getStartWP()(1));
_l1_control.navigate_waypoints(prev_wp_local, curr_wp_local, curr_pos_local, ground_speed);
_att_sp.roll_body = _l1_control.get_roll_setpoint(); _att_sp.roll_body = _l1_control.get_roll_setpoint();
} }
@ -1807,7 +1821,11 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const flo
} else { } else {
// normal navigation // normal navigation
_l1_control.navigate_waypoints(prev_wp, curr_wp, curr_pos, ground_speed); Vector2f curr_pos_local{_local_pos.x, _local_pos.y};
Vector2f curr_wp_local = _global_local_proj_ref.project(curr_wp(0), curr_wp(1));
Vector2f prev_wp_local = _global_local_proj_ref.project(prev_wp(0),
prev_wp(1));
_l1_control.navigate_waypoints(prev_wp_local, curr_wp_local, curr_pos_local, ground_speed);
} }
_att_sp.roll_body = _l1_control.get_roll_setpoint(); _att_sp.roll_body = _l1_control.get_roll_setpoint();
@ -1922,7 +1940,11 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const flo
} else { } else {
// normal navigation // normal navigation
_l1_control.navigate_waypoints(prev_wp, curr_wp, curr_pos, ground_speed); Vector2f curr_pos_local{_local_pos.x, _local_pos.y};
Vector2f curr_wp_local = _global_local_proj_ref.project(curr_wp(0), curr_wp(1));
Vector2f prev_wp_local = _global_local_proj_ref.project(prev_wp(0),
prev_wp(1));
_l1_control.navigate_waypoints(prev_wp_local, curr_wp_local, curr_pos_local, ground_speed);
} }
_att_sp.roll_body = _l1_control.get_roll_setpoint(); _att_sp.roll_body = _l1_control.get_roll_setpoint();
@ -2086,7 +2108,11 @@ FixedwingPositionControl::control_manual_position(const hrt_abstime &now, const
} else { } else {
/* populate l1 control setpoint */ /* populate l1 control setpoint */
_l1_control.navigate_waypoints(prev_wp, curr_wp, curr_pos, ground_speed); Vector2f curr_pos_local{_local_pos.x, _local_pos.y};
Vector2f curr_wp_local = _global_local_proj_ref.project(curr_wp(0), curr_wp(1));
Vector2f prev_wp_local = _global_local_proj_ref.project(prev_wp(0),
prev_wp(1));
_l1_control.navigate_waypoints(prev_wp_local, curr_wp_local, curr_pos_local, ground_speed);
_att_sp.roll_body = _l1_control.get_roll_setpoint(); _att_sp.roll_body = _l1_control.get_roll_setpoint();
} }
@ -2220,7 +2246,8 @@ FixedwingPositionControl::Run()
// Convert Local setpoints to global setpoints // Convert Local setpoints to global setpoints
if (!_global_local_proj_ref.isInitialized() if (!_global_local_proj_ref.isInitialized()
|| (_global_local_proj_ref.getProjectionReferenceTimestamp() != _local_pos.ref_timestamp)) { || (_global_local_proj_ref.getProjectionReferenceTimestamp() != _local_pos.ref_timestamp)
|| (_local_pos.vxy_reset_counter != _pos_reset_counter)) {
_global_local_proj_ref.initReference(_local_pos.ref_lat, _local_pos.ref_lon, _global_local_proj_ref.initReference(_local_pos.ref_lat, _local_pos.ref_lon,
_local_pos.ref_timestamp); _local_pos.ref_timestamp);

6
src/modules/rover_pos_control/RoverPositionControl.cpp
Unescape View File

@ -281,7 +281,11 @@ RoverPositionControl::control_position(const matrix::Vector2d &current_position,
_pos_ctrl_state = STOPPING; // We are closer than loiter radius to waypoint, stop. _pos_ctrl_state = STOPPING; // We are closer than loiter radius to waypoint, stop.
} else { } else {
_gnd_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d); Vector2f curr_pos_local{_local_pos.x, _local_pos.y};
Vector2f curr_wp_local = _global_local_proj_ref.project(curr_wp(0), curr_wp(1));
Vector2f prev_wp_local = _global_local_proj_ref.project(prev_wp(0),
prev_wp(1));
_gnd_control.navigate_waypoints(prev_wp_local, curr_wp_local, curr_pos_local, ground_speed_2d);
_act_controls.control[actuator_controls_s::INDEX_THROTTLE] = mission_throttle; _act_controls.control[actuator_controls_s::INDEX_THROTTLE] = mission_throttle;

Write Preview
Loading…
Cancel
Save