From 28d34bf095e54b350b7926192c1adc3a0c36eb41 Mon Sep 17 00:00:00 2001 From: Jaeyoung Lim Date: Thu, 17 Mar 2022 21:04:02 +0100 Subject: [PATCH] Convert L1 controller to local coordinates --- src/lib/l1/ECL_L1_Pos_Controller.cpp | 31 ++++++------- src/lib/l1/ECL_L1_Pos_Controller.hpp | 7 ++- .../FixedwingPositionControl.cpp | 43 +++++++++++++++---- .../RoverPositionControl.cpp | 6 ++- 4 files changed, 59 insertions(+), 28 deletions(-) diff --git a/src/lib/l1/ECL_L1_Pos_Controller.cpp b/src/lib/l1/ECL_L1_Pos_Controller.cpp index 02f7eae830..8e1880ab77 100644 --- a/src/lib/l1/ECL_L1_Pos_Controller.cpp +++ b/src/lib/l1/ECL_L1_Pos_Controller.cpp @@ -73,8 +73,8 @@ float ECL_L1_Pos_Controller::switch_distance(float wp_radius) } void -ECL_L1_Pos_Controller::navigate_waypoints(const Vector2d &vector_A, const Vector2d &vector_B, - const Vector2d &vector_curr_position, const Vector2f &ground_speed_vector) +ECL_L1_Pos_Controller::navigate_waypoints(const Vector2f &vector_A, const Vector2f &vector_B, + const Vector2f &vector_curr_position, const Vector2f &ground_speed_vector) { _has_guidance_updated = true; @@ -82,8 +82,9 @@ ECL_L1_Pos_Controller::navigate_waypoints(const Vector2d &vector_A, const Vector float eta = 0.0f; /* 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), - vector_B(1)); + Vector2f vector_B_to_P = vector_B - vector_curr_position; + 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 */ 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; /* 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, * skip A and directly continue to B */ 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(); /* 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) */ _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; /* 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) */ // 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 */ 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 -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) { _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_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 */ 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; /* 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; @@ -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; } + /* 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 */ diff --git a/src/lib/l1/ECL_L1_Pos_Controller.hpp b/src/lib/l1/ECL_L1_Pos_Controller.hpp index a2d4d085e3..a4ec91f2b3 100644 --- a/src/lib/l1/ECL_L1_Pos_Controller.hpp +++ b/src/lib/l1/ECL_L1_Pos_Controller.hpp @@ -143,9 +143,8 @@ public: * * @return sets _lateral_accel setpoint */ - void navigate_waypoints(const matrix::Vector2d &vector_A, const matrix::Vector2d &vector_B, - const matrix::Vector2d &vector_curr_position, const matrix::Vector2f &ground_speed); - + void navigate_waypoints(const matrix::Vector2f &vector_A, const matrix::Vector2f &vector_B, + const matrix::Vector2f &vector_curr_position, const matrix::Vector2f &ground_speed); /** * Navigate on an orbit around a loiter waypoint. * @@ -154,7 +153,7 @@ public: * * @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); /** diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index 3c3621de78..e0fc1c2427 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -1208,7 +1208,10 @@ FixedwingPositionControl::control_auto_position(const hrt_abstime &now, const fl target_airspeed = _npfg.getAirspeedRef() / _eas2tas; } 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(); } @@ -1377,7 +1380,10 @@ FixedwingPositionControl::control_auto_loiter(const hrt_abstime &now, const floa target_airspeed = _npfg.getAirspeedRef() / _eas2tas; } 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(); } @@ -1480,7 +1486,11 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo target_airspeed = _npfg.getAirspeedRef() / _eas2tas; } 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()); } @@ -1550,7 +1560,11 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo target_airspeed = _npfg.getAirspeedRef() / _eas2tas; } 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(); } @@ -1807,7 +1821,11 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const flo } else { // 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(); @@ -1922,7 +1940,11 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const flo } else { // 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(); @@ -2086,7 +2108,11 @@ FixedwingPositionControl::control_manual_position(const hrt_abstime &now, const } else { /* 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(); } @@ -2220,7 +2246,8 @@ FixedwingPositionControl::Run() // Convert Local setpoints to global setpoints 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, _local_pos.ref_timestamp); diff --git a/src/modules/rover_pos_control/RoverPositionControl.cpp b/src/modules/rover_pos_control/RoverPositionControl.cpp index b48337f3d6..bae9009296 100644 --- a/src/modules/rover_pos_control/RoverPositionControl.cpp +++ b/src/modules/rover_pos_control/RoverPositionControl.cpp @@ -281,7 +281,11 @@ RoverPositionControl::control_position(const matrix::Vector2d ¤t_position, _pos_ctrl_state = STOPPING; // We are closer than loiter radius to waypoint, stop. } 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;