Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-05 07:28:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

ArduCopter: inav funcs use _xy()

Browse Source
This commit is contained in:
Josh Henderson 2021-09-11 04:07:42 -04:00 committed by Andrew Tridgell
parent 6243532e69
commit 6b871fba55
7 changed files with 17 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduCopter/autoyaw.cpp
View File

@ -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()

8
ArduCopter/mode.cpp
View File

@ -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);

4
ArduCopter/mode_auto.cpp
View File

@ -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) {

4
ArduCopter/mode_guided.cpp
View File

@ -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;

6
ArduCopter/mode_loiter.cpp
View File

@ -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);

2
ArduCopter/mode_poshold.cpp
View File

@ -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;
}

18
ArduCopter/mode_zigzag.cpp
View File

@ -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;