From 1981519aad6c342ff1e869b1bcf356c131bf46dd Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Thu, 11 Feb 2021 19:57:31 -0500 Subject: [PATCH] L1 position controller: use double precision floating point for all lat/lon --- src/lib/l1/ECL_L1_Pos_Controller.cpp | 53 +++++++------ src/lib/l1/ECL_L1_Pos_Controller.hpp | 8 +- src/lib/matrix | 2 +- .../FixedwingPositionControl.cpp | 74 +++++++++++-------- .../FixedwingPositionControl.hpp | 23 ++---- .../runway_takeoff/RunwayTakeoff.cpp | 17 +---- .../runway_takeoff/RunwayTakeoff.h | 4 +- .../RoverPositionControl.cpp | 12 +-- .../RoverPositionControl.hpp | 4 +- 9 files changed, 102 insertions(+), 95 deletions(-) diff --git a/src/lib/l1/ECL_L1_Pos_Controller.cpp b/src/lib/l1/ECL_L1_Pos_Controller.cpp index e46c7bac62..c5e81c4f85 100644 --- a/src/lib/l1/ECL_L1_Pos_Controller.cpp +++ b/src/lib/l1/ECL_L1_Pos_Controller.cpp @@ -46,6 +46,7 @@ #include +using matrix::Vector2d; using matrix::Vector2f; using matrix::wrap_pi; @@ -72,17 +73,15 @@ float ECL_L1_Pos_Controller::switch_distance(float wp_radius) } void -ECL_L1_Pos_Controller::navigate_waypoints(const Vector2f &vector_A, const Vector2f &vector_B, - const Vector2f &vector_curr_position, const Vector2f &ground_speed_vector) +ECL_L1_Pos_Controller::navigate_waypoints(const Vector2d &vector_A, const Vector2d &vector_B, + const Vector2d &vector_curr_position, const Vector2f &ground_speed_vector) { /* this follows the logic presented in [1] */ float eta = 0.0f; - float xtrack_vel = 0.0f; - float ltrack_vel = 0.0f; /* get the direction between the last (visited) and next waypoint */ - _target_bearing = get_bearing_to_next_waypoint((double)vector_curr_position(0), (double)vector_curr_position(1), - (double)vector_B(0), (double)vector_B(1)); + _target_bearing = get_bearing_to_next_waypoint(vector_curr_position(0), vector_curr_position(1), vector_B(0), + vector_B(1)); /* enforce a minimum ground speed of 0.1 m/s to avoid singularities */ float ground_speed = math::max(ground_speed_vector.length(), 0.1f); @@ -132,11 +131,14 @@ ECL_L1_Pos_Controller::navigate_waypoints(const Vector2f &vector_A, const Vector /* unit vector from waypoint A to current position */ Vector2f vector_A_to_airplane_unit = vector_A_to_airplane.normalized(); + /* velocity across / orthogonal to line */ - xtrack_vel = ground_speed_vector % (-vector_A_to_airplane_unit); + float xtrack_vel = ground_speed_vector % (-vector_A_to_airplane_unit); + /* velocity along line */ - ltrack_vel = ground_speed_vector * (-vector_A_to_airplane_unit); + float ltrack_vel = ground_speed_vector * (-vector_A_to_airplane_unit); eta = atan2f(xtrack_vel, ltrack_vel); + /* bearing from current position to L1 point */ _nav_bearing = atan2f(-vector_A_to_airplane_unit(1), -vector_A_to_airplane_unit(0)); @@ -158,33 +160,38 @@ ECL_L1_Pos_Controller::navigate_waypoints(const Vector2f &vector_A, const Vector /* calculate eta to fly to waypoint B */ /* velocity across / orthogonal to line */ - xtrack_vel = ground_speed_vector % (-vector_B_to_P_unit); + float xtrack_vel = ground_speed_vector % (-vector_B_to_P_unit); + /* velocity along line */ - ltrack_vel = ground_speed_vector * (-vector_B_to_P_unit); + float ltrack_vel = ground_speed_vector * (-vector_B_to_P_unit); eta = atan2f(xtrack_vel, ltrack_vel); + /* bearing from current position to L1 point */ _nav_bearing = atan2f(-vector_B_to_P_unit(1), -vector_B_to_P_unit(0)); } else { - /* calculate eta to fly along the line between A and B */ /* velocity across / orthogonal to line */ - xtrack_vel = ground_speed_vector % vector_AB; + float xtrack_vel = ground_speed_vector % vector_AB; + /* velocity along line */ - ltrack_vel = ground_speed_vector * vector_AB; + float ltrack_vel = ground_speed_vector * vector_AB; + /* calculate eta2 (angle of velocity vector relative to line) */ float eta2 = atan2f(xtrack_vel, ltrack_vel); + /* calculate eta1 (angle to L1 point) */ float xtrackErr = vector_A_to_airplane % vector_AB; float sine_eta1 = xtrackErr / math::max(_L1_distance, 0.1f); + /* limit output to 45 degrees */ sine_eta1 = math::constrain(sine_eta1, -0.7071f, 0.7071f); //sin(pi/4) = 0.7071 float eta1 = asinf(sine_eta1); eta = eta1 + eta2; + /* bearing from current position to L1 point */ _nav_bearing = atan2f(vector_AB(1), vector_AB(0)) + eta1; - } /* limit angle to +-90 degrees */ @@ -201,7 +208,7 @@ ECL_L1_Pos_Controller::navigate_waypoints(const Vector2f &vector_A, const Vector } void -ECL_L1_Pos_Controller::navigate_loiter(const Vector2f &vector_A, const Vector2f &vector_curr_position, float radius, +ECL_L1_Pos_Controller::navigate_loiter(const Vector2d &vector_A, const Vector2d &vector_curr_position, float radius, int8_t loiter_direction, const Vector2f &ground_speed_vector) { /* the complete guidance logic in this section was proposed by [2] */ @@ -212,8 +219,8 @@ ECL_L1_Pos_Controller::navigate_loiter(const Vector2f &vector_A, const Vector2f float K_velocity = 2.0f * _L1_damping * omega; /* update bearing to next waypoint */ - _target_bearing = get_bearing_to_next_waypoint((double)vector_curr_position(0), (double)vector_curr_position(1), - (double)vector_A(0), (double)vector_A(1)); + _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); @@ -356,13 +363,17 @@ void ECL_L1_Pos_Controller::navigate_level_flight(float current_heading) update_roll_setpoint(); } -Vector2f ECL_L1_Pos_Controller::get_local_planar_vector(const Vector2f &origin, const Vector2f &target) const +Vector2f ECL_L1_Pos_Controller::get_local_planar_vector(const Vector2d &origin, const Vector2d &target) const { /* this is an approximation for small angles, proposed by [2] */ - Vector2f out(math::radians((target(0) - origin(0))), - math::radians((target(1) - origin(1))*cosf(math::radians(origin(0))))); + const double x_angle = math::radians(target(0) - origin(0)); + const double y_angle = math::radians(target(1) - origin(1)); + const double x_origin_cos = cos(math::radians(origin(0))); - return out * static_cast(CONSTANTS_RADIUS_OF_EARTH); + return Vector2f{ + static_cast(x_angle * CONSTANTS_RADIUS_OF_EARTH), + static_cast(y_angle *x_origin_cos * CONSTANTS_RADIUS_OF_EARTH), + }; } void ECL_L1_Pos_Controller::set_l1_period(float period) diff --git a/src/lib/l1/ECL_L1_Pos_Controller.hpp b/src/lib/l1/ECL_L1_Pos_Controller.hpp index 1cdaa3f06c..4c56239cc0 100644 --- a/src/lib/l1/ECL_L1_Pos_Controller.hpp +++ b/src/lib/l1/ECL_L1_Pos_Controller.hpp @@ -143,8 +143,8 @@ public: * * @return sets _lateral_accel setpoint */ - void navigate_waypoints(const matrix::Vector2f &vector_A, const matrix::Vector2f &vector_B, - const matrix::Vector2f &vector_curr_position, const matrix::Vector2f &ground_speed); + void navigate_waypoints(const matrix::Vector2d &vector_A, const matrix::Vector2d &vector_B, + const matrix::Vector2d &vector_curr_position, const matrix::Vector2f &ground_speed); /** * Navigate on an orbit around a loiter waypoint. @@ -154,7 +154,7 @@ public: * * @return sets _lateral_accel setpoint */ - void navigate_loiter(const matrix::Vector2f &vector_A, const matrix::Vector2f &vector_curr_position, float radius, + void navigate_loiter(const matrix::Vector2d &vector_A, const matrix::Vector2d &vector_curr_position, float radius, int8_t loiter_direction, const matrix::Vector2f &ground_speed_vector); /** @@ -235,7 +235,7 @@ private: * @param wp The point to convert to into the local coordinates, in WGS84 coordinates * @return The vector in meters pointing from the reference position to the coordinates */ - matrix::Vector2f get_local_planar_vector(const matrix::Vector2f &origin, const matrix::Vector2f &target) const; + matrix::Vector2f get_local_planar_vector(const matrix::Vector2d &origin, const matrix::Vector2d &target) const; /** * Update roll angle setpoint. This will also apply slew rate limits if set. diff --git a/src/lib/matrix b/src/lib/matrix index 977cf52322..3679f7fd51 160000 --- a/src/lib/matrix +++ b/src/lib/matrix @@ -1 +1 @@ -Subproject commit 977cf52322ab3ca93125423a0f9e9b3eca0516d7 +Subproject commit 3679f7fd5160c7d2a8271b1134ca1e601f1a4254 diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index 6a78a4cc65..2eed783cbc 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -35,6 +35,19 @@ #include +using math::constrain; +using math::max; +using math::min; +using math::radians; + +using matrix::Dcmf; +using matrix::Eulerf; +using matrix::Quatf; +using matrix::Vector2f; +using matrix::Vector2d; +using matrix::Vector3f; +using matrix::wrap_pi; + FixedwingPositionControl::FixedwingPositionControl(bool vtol) : ModuleParams(nullptr), WorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers), @@ -587,7 +600,7 @@ FixedwingPositionControl::do_takeoff_help(float *hold_altitude, float *pitch_lim } bool -FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2f &curr_pos, +FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2d &curr_pos, const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr, const position_setpoint_s &pos_sp_next) { @@ -662,8 +675,8 @@ FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2 _tecs.set_speed_weight(_param_fw_t_spdweight.get()); _tecs.set_height_error_time_constant(_param_fw_t_h_error_tc.get()); - Vector2f curr_wp{0.0f, 0.0f}; - Vector2f prev_wp{0.0f, 0.0f}; + Vector2d curr_wp{0, 0}; + Vector2d prev_wp{0, 0}; if (_vehicle_status.in_transition_to_fw) { @@ -674,8 +687,8 @@ FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2 waypoint_from_heading_and_distance(_current_latitude, _current_longitude, _yaw, HDG_HOLD_DIST_NEXT, &lat_transition, &lon_transition); - _transition_waypoint(0) = (float)lat_transition; - _transition_waypoint(1) = (float)lon_transition; + _transition_waypoint(0) = lat_transition; + _transition_waypoint(1) = lon_transition; } @@ -683,24 +696,25 @@ FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2 } else { /* current waypoint (the one currently heading for) */ - curr_wp = Vector2f((float)pos_sp_curr.lat, (float)pos_sp_curr.lon); + curr_wp = Vector2d(pos_sp_curr.lat, pos_sp_curr.lon); if (pos_sp_prev.valid) { - prev_wp(0) = (float)pos_sp_prev.lat; - prev_wp(1) = (float)pos_sp_prev.lon; + prev_wp(0) = pos_sp_prev.lat; + prev_wp(1) = pos_sp_prev.lon; } else { /* * No valid previous waypoint, go for the current wp. * This is automatically handled by the L1 library. */ - prev_wp(0) = (float)pos_sp_curr.lat; - prev_wp(1) = (float)pos_sp_curr.lon; + prev_wp(0) = pos_sp_curr.lat; + prev_wp(1) = pos_sp_curr.lon; } /* reset transition waypoint, will be set upon entering front transition */ - _transition_waypoint.setNaN(); + _transition_waypoint(0) = static_cast(NAN); + _transition_waypoint(1) = static_cast(NAN); } /* Initialize attitude controller integrator reset flags to 0 */ @@ -1007,8 +1021,8 @@ FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2 get_waypoint_heading_distance(_hdg_hold_yaw, _hdg_hold_prev_wp, _hdg_hold_curr_wp, false); } - Vector2f prev_wp{(float)_hdg_hold_prev_wp.lat, (float)_hdg_hold_prev_wp.lon}; - Vector2f curr_wp{(float)_hdg_hold_curr_wp.lat, (float)_hdg_hold_curr_wp.lon}; + Vector2d prev_wp{_hdg_hold_prev_wp.lat, _hdg_hold_prev_wp.lon}; + Vector2d curr_wp{_hdg_hold_curr_wp.lat, _hdg_hold_curr_wp.lon}; /* populate l1 control setpoint */ _l1_control.navigate_waypoints(prev_wp, curr_wp, curr_pos, ground_speed); @@ -1157,24 +1171,24 @@ FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2 } void -FixedwingPositionControl::control_takeoff(const hrt_abstime &now, const Vector2f &curr_pos, +FixedwingPositionControl::control_takeoff(const hrt_abstime &now, const Vector2d &curr_pos, const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr) { /* current waypoint (the one currently heading for) */ - Vector2f curr_wp((float)pos_sp_curr.lat, (float)pos_sp_curr.lon); - Vector2f prev_wp{0.0f, 0.0f}; /* previous waypoint */ + Vector2d curr_wp(pos_sp_curr.lat, pos_sp_curr.lon); + Vector2d prev_wp{0, 0}; /* previous waypoint */ if (pos_sp_prev.valid) { - prev_wp(0) = (float)pos_sp_prev.lat; - prev_wp(1) = (float)pos_sp_prev.lon; + prev_wp(0) = pos_sp_prev.lat; + prev_wp(1) = pos_sp_prev.lon; } else { /* * No valid previous waypoint, go for the current wp. * This is automatically handled by the L1 library. */ - prev_wp(0) = (float)pos_sp_curr.lat; - prev_wp(1) = (float)pos_sp_curr.lon; + prev_wp(0) = pos_sp_curr.lat; + prev_wp(1) = pos_sp_curr.lon; } // apply flaps for takeoff according to the corresponding scale factor set @@ -1326,24 +1340,24 @@ FixedwingPositionControl::control_takeoff(const hrt_abstime &now, const Vector2f } void -FixedwingPositionControl::control_landing(const hrt_abstime &now, const Vector2f &curr_pos, +FixedwingPositionControl::control_landing(const hrt_abstime &now, const Vector2d &curr_pos, const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr) { /* current waypoint (the one currently heading for) */ - Vector2f curr_wp((float)pos_sp_curr.lat, (float)pos_sp_curr.lon); - Vector2f prev_wp{0.0f, 0.0f}; /* previous waypoint */ + Vector2d curr_wp(pos_sp_curr.lat, pos_sp_curr.lon); + Vector2d prev_wp{0, 0}; /* previous waypoint */ if (pos_sp_prev.valid) { - prev_wp(0) = (float)pos_sp_prev.lat; - prev_wp(1) = (float)pos_sp_prev.lon; + prev_wp(0) = pos_sp_prev.lat; + prev_wp(1) = pos_sp_prev.lon; } else { /* * No valid previous waypoint, go for the current wp. * This is automatically handled by the L1 library. */ - prev_wp(0) = (float)pos_sp_curr.lat; - prev_wp(1) = (float)pos_sp_curr.lon; + prev_wp(0) = pos_sp_curr.lat; + prev_wp(1) = pos_sp_curr.lon; } // apply full flaps for landings. this flag will also trigger the use of flaperons @@ -1392,8 +1406,8 @@ FixedwingPositionControl::control_landing(const hrt_abstime &now, const Vector2f create_waypoint_from_line_and_dist(pos_sp_curr.lat, pos_sp_curr.lon, pos_sp_prev.lat, pos_sp_prev.lon, -1000.0f, &lat, &lon); - curr_wp(0) = (float)lat; - curr_wp(1) = (float)lon; + curr_wp(0) = lat; + curr_wp(1) = lon; } // we want the plane to keep tracking the desired flight path until we start flaring @@ -1693,7 +1707,7 @@ FixedwingPositionControl::Run() _vehicle_status_sub.update(&_vehicle_status); - Vector2f curr_pos((float)_current_latitude, (float)_current_longitude); + Vector2d curr_pos(_current_latitude, _current_longitude); Vector2f ground_speed(_local_pos.vx, _local_pos.vy); //Convert Local setpoints to global setpoints diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp index 1c17fe1c3c..7eaf88776b 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp @@ -89,22 +89,13 @@ #include #include -using math::constrain; -using math::max; -using math::min; -using math::radians; - -using matrix::Dcmf; -using matrix::Eulerf; -using matrix::Quatf; -using matrix::Vector2f; -using matrix::Vector3f; -using matrix::wrap_pi; - using namespace launchdetection; using namespace runwaytakeoff; using namespace time_literals; +using matrix::Vector2d; +using matrix::Vector2f; + static constexpr float HDG_HOLD_DIST_NEXT = 3000.0f; // initial distance of waypoint in front of plane in heading hold mode static constexpr float HDG_HOLD_REACHED_DIST = @@ -248,7 +239,7 @@ private: bool _vtol_tailsitter{false}; - Vector2f _transition_waypoint{NAN, NAN}; + matrix::Vector2d _transition_waypoint{(double)NAN, (double)NAN}; // estimator reset counters uint8_t _pos_reset_counter{0}; ///< captures the number of times the estimator has reset the horizontal position @@ -324,13 +315,13 @@ private: */ void update_desired_altitude(float dt); - bool control_position(const hrt_abstime &now, const Vector2f &curr_pos, const Vector2f &ground_speed, + bool control_position(const hrt_abstime &now, const Vector2d &curr_pos, const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr, const position_setpoint_s &pos_sp_next); - void control_takeoff(const hrt_abstime &now, const Vector2f &curr_pos, const Vector2f &ground_speed, + void control_takeoff(const hrt_abstime &now, const Vector2d &curr_pos, const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr); - void control_landing(const hrt_abstime &now, const Vector2f &curr_pos, const Vector2f &ground_speed, + void control_landing(const hrt_abstime &now, const Vector2d &curr_pos, const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr); diff --git a/src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.cpp b/src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.cpp index fc85cc2f90..c9b9bfd437 100644 --- a/src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.cpp +++ b/src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.cpp @@ -46,7 +46,6 @@ #include #include -using matrix::Vector2f; using namespace time_literals; namespace runwaytakeoff @@ -69,8 +68,8 @@ void RunwayTakeoff::init(const hrt_abstime &now, float yaw, double current_lat, _state = RunwayTakeoffState::THROTTLE_RAMP; _initialized_time = now; _climbout = true; // this is true until climbout is finished - _start_wp(0) = (float)current_lat; - _start_wp(1) = (float)current_lon; + _start_wp(0) = current_lat; + _start_wp(1) = current_lon; } void RunwayTakeoff::update(const hrt_abstime &now, float airspeed, float alt_agl, @@ -103,8 +102,8 @@ void RunwayTakeoff::update(const hrt_abstime &now, float airspeed, float alt_agl * The navigator will take this as starting point to navigate towards the takeoff WP. */ if (_param_rwto_hdg.get() == 0) { - _start_wp(0) = (float)current_lat; - _start_wp(1) = (float)current_lon; + _start_wp(0) = current_lat; + _start_wp(1) = current_lon; } mavlink_log_info(mavlink_log_pub, "#Climbout"); @@ -248,14 +247,6 @@ float RunwayTakeoff::getMaxPitch(float max) } } -/* - * Returns the "previous" (start) WP for navigation. - */ -Vector2f RunwayTakeoff::getStartWP() -{ - return _start_wp; -} - void RunwayTakeoff::reset() { _initialized = false; diff --git a/src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.h b/src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.h index b42ced184b..796967d682 100644 --- a/src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.h +++ b/src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.h @@ -88,7 +88,7 @@ public: bool resetIntegrators(); float getMinPitch(float climbout_min, float min); float getMaxPitch(float max); - matrix::Vector2f getStartWP(); + const matrix::Vector2d &getStartWP() const { return _start_wp; }; void reset(); @@ -99,7 +99,7 @@ private: hrt_abstime _initialized_time{0}; float _init_yaw{0.f}; bool _climbout{false}; - matrix::Vector2f _start_wp; + matrix::Vector2d _start_wp; DEFINE_PARAMETERS( (ParamBool) _param_rwto_tkoff, diff --git a/src/modules/rover_pos_control/RoverPositionControl.cpp b/src/modules/rover_pos_control/RoverPositionControl.cpp index a19d9b1989..2b592da5fc 100644 --- a/src/modules/rover_pos_control/RoverPositionControl.cpp +++ b/src/modules/rover_pos_control/RoverPositionControl.cpp @@ -149,7 +149,7 @@ RoverPositionControl::vehicle_attitude_poll() } bool -RoverPositionControl::control_position(const matrix::Vector2f ¤t_position, +RoverPositionControl::control_position(const matrix::Vector2d ¤t_position, const matrix::Vector3f &ground_speed, const position_setpoint_triplet_s &pos_sp_triplet) { float dt = 0.01; // Using non zero value to a avoid division by zero @@ -172,14 +172,14 @@ RoverPositionControl::control_position(const matrix::Vector2f ¤t_position, //bool was_circle_mode = _gnd_control.circle_mode(); /* current waypoint (the one currently heading for) */ - matrix::Vector2f curr_wp((float)pos_sp_triplet.current.lat, (float)pos_sp_triplet.current.lon); + matrix::Vector2d curr_wp(pos_sp_triplet.current.lat, pos_sp_triplet.current.lon); /* previous waypoint */ - matrix::Vector2f prev_wp = curr_wp; + matrix::Vector2d prev_wp = curr_wp; if (pos_sp_triplet.previous.valid) { - prev_wp(0) = (float)pos_sp_triplet.previous.lat; - prev_wp(1) = (float)pos_sp_triplet.previous.lon; + prev_wp(0) = pos_sp_triplet.previous.lat; + prev_wp(1) = pos_sp_triplet.previous.lon; } matrix::Vector2f ground_speed_2d(ground_speed); @@ -433,7 +433,7 @@ RoverPositionControl::run() _pos_reset_counter = _global_pos.lat_lon_reset_counter; matrix::Vector3f ground_speed(_local_pos.vx, _local_pos.vy, _local_pos.vz); - matrix::Vector2f current_position((float)_global_pos.lat, (float)_global_pos.lon); + matrix::Vector2d current_position(_global_pos.lat, _global_pos.lon); matrix::Vector3f current_velocity(_local_pos.vx, _local_pos.vy, _local_pos.vz); if (!manual_mode && _control_mode.flag_control_position_enabled) { diff --git a/src/modules/rover_pos_control/RoverPositionControl.hpp b/src/modules/rover_pos_control/RoverPositionControl.hpp index 2795205bee..f503d367f2 100644 --- a/src/modules/rover_pos_control/RoverPositionControl.hpp +++ b/src/modules/rover_pos_control/RoverPositionControl.hpp @@ -150,7 +150,7 @@ private: } _pos_ctrl_state {STOPPING}; /// Position control state machine /* previous waypoint */ - matrix::Vector2f _prev_wp{0.0f, 0.0f}; + matrix::Vector2d _prev_wp{0, 0}; DEFINE_PARAMETERS( (ParamFloat) _param_l1_period, @@ -190,7 +190,7 @@ private: /** * Control position. */ - bool control_position(const matrix::Vector2f &global_pos, const matrix::Vector3f &ground_speed, + bool control_position(const matrix::Vector2d &global_pos, const matrix::Vector3f &ground_speed, const position_setpoint_triplet_s &_pos_sp_triplet); void control_velocity(const matrix::Vector3f ¤t_velocity, const position_setpoint_triplet_s &pos_sp_triplet); void control_attitude(const vehicle_attitude_s &att, const vehicle_attitude_setpoint_s &att_sp);