From 6b871fba550706ceb34fc18cc8a4776fcef9c672 Mon Sep 17 00:00:00 2001 From: Josh Henderson Date: Sat, 11 Sep 2021 04:07:42 -0400 Subject: [PATCH] ArduCopter: inav funcs use _xy() --- ArduCopter/autoyaw.cpp | 2 +- ArduCopter/mode.cpp | 8 +++----- ArduCopter/mode_auto.cpp | 4 +--- ArduCopter/mode_guided.cpp | 4 ++-- ArduCopter/mode_loiter.cpp | 6 ++---- ArduCopter/mode_poshold.cpp | 2 +- ArduCopter/mode_zigzag.cpp | 18 +++++++----------- 7 files changed, 17 insertions(+), 27 deletions(-) diff --git a/ArduCopter/autoyaw.cpp b/ArduCopter/autoyaw.cpp index 96aa59da06..c06b944d82 100644 --- a/ArduCopter/autoyaw.cpp +++ b/ArduCopter/autoyaw.cpp @@ -5,7 +5,7 @@ Mode::AutoYaw Mode::auto_yaw; // roi_yaw - returns heading towards location held in roi float Mode::AutoYaw::roi_yaw() const { - return get_bearing_cd(copter.inertial_nav.get_position().xy(), roi.xy()); + return get_bearing_cd(copter.inertial_nav.get_position_xy(), roi.xy()); } float Mode::AutoYaw::look_ahead_yaw() diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index f1afd7c541..45a25951db 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -569,7 +569,7 @@ void Mode::land_run_vertical_control(bool pause_descent) Vector2f target_pos; float target_error_cm = 0.0f; if (copter.precland.get_target_position_cm(target_pos)) { - const Vector2f current_pos = inertial_nav.get_position().xy(); + const Vector2f current_pos = inertial_nav.get_position_xy(); // target is this many cm away from the vehicle target_error_cm = (target_pos - current_pos).length(); } @@ -647,12 +647,10 @@ void Mode::land_run_horizontal_control() if (doing_precision_landing) { Vector2f target_pos, target_vel_rel; if (!copter.precland.get_target_position_cm(target_pos)) { - target_pos.x = inertial_nav.get_position().x; - target_pos.y = inertial_nav.get_position().y; + target_pos = inertial_nav.get_position_xy(); } if (!copter.precland.get_target_velocity_relative_cms(target_vel_rel)) { - target_vel_rel.x = -inertial_nav.get_velocity().x; - target_vel_rel.y = -inertial_nav.get_velocity().y; + target_vel_rel = -inertial_nav.get_velocity_xy(); } pos_control->set_pos_target_xy_cm(target_pos.x, target_pos.y); pos_control->override_vehicle_velocity_xy(-target_vel_rel); diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index 6b8c2a2562..7933863c2e 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -375,9 +375,7 @@ void ModeAuto::circle_movetoedge_start(const Location &circle_center, float radi } // if we are outside the circle, point at the edge, otherwise hold yaw - const Vector3p &circle_center_neu = copter.circle_nav->get_center(); - const Vector3f &curr_pos = inertial_nav.get_position(); - float dist_to_center = norm(circle_center_neu.x - curr_pos.x, circle_center_neu.y - curr_pos.y); + const float dist_to_center = get_horizontal_distance_cm(inertial_nav.get_position_xy().topostype(), copter.circle_nav->get_center().xy()); // initialise yaw // To-Do: reset the yaw only when the previous navigation command is not a WP. this would allow removing the special check for ROI if (auto_yaw.mode() != AUTO_YAW_ROI) { diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp index e93a2b7962..6a558bf2cc 100644 --- a/ArduCopter/mode_guided.cpp +++ b/ArduCopter/mode_guided.cpp @@ -1118,7 +1118,7 @@ uint32_t ModeGuided::wp_distance() const case SubMode::WP: return wp_nav->get_wp_distance_to_destination(); case SubMode::Pos: - return get_horizontal_distance_cm(inertial_nav.get_position().xy(), guided_pos_target_cm.tofloat().xy()); + return get_horizontal_distance_cm(inertial_nav.get_position_xy(), guided_pos_target_cm.tofloat().xy()); case SubMode::PosVelAccel: return pos_control->get_pos_error_xy_cm(); break; @@ -1133,7 +1133,7 @@ int32_t ModeGuided::wp_bearing() const case SubMode::WP: return wp_nav->get_wp_bearing_to_destination(); case SubMode::Pos: - return get_bearing_cd(inertial_nav.get_position().xy(), guided_pos_target_cm.tofloat().xy()); + return get_bearing_cd(inertial_nav.get_position_xy(), guided_pos_target_cm.tofloat().xy()); case SubMode::PosVelAccel: return pos_control->get_bearing_to_target_cd(); break; diff --git a/ArduCopter/mode_loiter.cpp b/ArduCopter/mode_loiter.cpp index 06ae647bf3..0dae994bd9 100644 --- a/ArduCopter/mode_loiter.cpp +++ b/ArduCopter/mode_loiter.cpp @@ -61,12 +61,10 @@ void ModeLoiter::precision_loiter_xy() loiter_nav->clear_pilot_desired_acceleration(); Vector2f target_pos, target_vel_rel; if (!copter.precland.get_target_position_cm(target_pos)) { - target_pos.x = inertial_nav.get_position().x; - target_pos.y = inertial_nav.get_position().y; + target_pos = inertial_nav.get_position_xy(); } if (!copter.precland.get_target_velocity_relative_cms(target_vel_rel)) { - target_vel_rel.x = -inertial_nav.get_velocity().x; - target_vel_rel.y = -inertial_nav.get_velocity().y; + target_vel_rel = -inertial_nav.get_velocity_xy(); } pos_control->set_pos_target_xy_cm(target_pos.x, target_pos.y); pos_control->override_vehicle_velocity_xy(-target_vel_rel); diff --git a/ArduCopter/mode_poshold.cpp b/ArduCopter/mode_poshold.cpp index 6534c3de70..ee06d7e9bb 100644 --- a/ArduCopter/mode_poshold.cpp +++ b/ArduCopter/mode_poshold.cpp @@ -378,7 +378,7 @@ void ModePosHold::run() pitch_mode = RPMode::BRAKE_TO_LOITER; brake.to_loiter_timer = POSHOLD_BRAKE_TO_LOITER_TIMER; // init loiter controller - loiter_nav->init_target(inertial_nav.get_position().xy()); + loiter_nav->init_target(inertial_nav.get_position_xy()); // set delay to start of wind compensation estimate updates wind_comp_start_timer = POSHOLD_WIND_COMP_START_TIMER; } diff --git a/ArduCopter/mode_zigzag.cpp b/ArduCopter/mode_zigzag.cpp index be972ec8e4..0833358ef5 100644 --- a/ArduCopter/mode_zigzag.cpp +++ b/ArduCopter/mode_zigzag.cpp @@ -161,7 +161,7 @@ void ModeZigZag::run() void ModeZigZag::save_or_move_to_destination(Destination ab_dest) { // get current position as an offset from EKF origin - const Vector3f curr_pos = inertial_nav.get_position(); + const Vector2f curr_pos {inertial_nav.get_position_xy()}; // handle state machine changes switch (stage) { @@ -169,14 +169,12 @@ void ModeZigZag::save_or_move_to_destination(Destination ab_dest) case STORING_POINTS: if (ab_dest == Destination::A) { // store point A - dest_A.x = curr_pos.x; - dest_A.y = curr_pos.y; + dest_A = curr_pos; gcs().send_text(MAV_SEVERITY_INFO, "ZigZag: point A stored"); AP::logger().Write_Event(LogEvent::ZIGZAG_STORE_A); } else { // store point B - dest_B.x = curr_pos.x; - dest_B.y = curr_pos.y; + dest_B = curr_pos; gcs().send_text(MAV_SEVERITY_INFO, "ZigZag: point B stored"); AP::logger().Write_Event(LogEvent::ZIGZAG_STORE_B); } @@ -429,8 +427,7 @@ bool ModeZigZag::calculate_next_dest(Destination ab_dest, bool use_wpnav_alt, Ve } // get distance from vehicle to start_pos - const Vector3f curr_pos = inertial_nav.get_position(); - const Vector2f curr_pos2d = Vector2f(curr_pos.x, curr_pos.y); + const Vector2f curr_pos2d {inertial_nav.get_position_xy()}; Vector2f veh_to_start_pos = curr_pos2d - start_pos; // lengthen AB_diff so that it is at least as long as vehicle is from start point @@ -461,7 +458,7 @@ bool ModeZigZag::calculate_next_dest(Destination ab_dest, bool use_wpnav_alt, Ve next_dest.z = copter.rangefinder_state.alt_cm_filt.get(); } } else { - next_dest.z = pos_control->is_active_z() ? pos_control->get_pos_target_z_cm() : curr_pos.z; + next_dest.z = pos_control->is_active_z() ? pos_control->get_pos_target_z_cm() : inertial_nav.get_altitude(); } } @@ -502,8 +499,7 @@ bool ModeZigZag::calculate_side_dest(Vector3f& next_dest, bool& terrain_alt) con float scalar = constrain_float(_side_dist, 0.1f, 100.0f) * 100 / safe_sqrt(AB_side.length_squared()); // get distance from vehicle to start_pos - const Vector3f curr_pos = inertial_nav.get_position(); - const Vector2f curr_pos2d = Vector2f(curr_pos.x, curr_pos.y); + const Vector2f curr_pos2d {inertial_nav.get_position_xy()}; next_dest.x = curr_pos2d.x + (AB_side.x * scalar); next_dest.y = curr_pos2d.y + (AB_side.y * scalar); @@ -514,7 +510,7 @@ bool ModeZigZag::calculate_side_dest(Vector3f& next_dest, bool& terrain_alt) con next_dest.z = copter.rangefinder_state.alt_cm_filt.get(); } } else { - next_dest.z = pos_control->is_active_z() ? pos_control->get_pos_target_z_cm() : curr_pos.z; + next_dest.z = pos_control->is_active_z() ? pos_control->get_pos_target_z_cm() : inertial_nav.get_altitude(); } return true;