Mirrors_Framework/ardupilot
1
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot.git synced 2026-10-06 19:00:27 +08:00
Code Issues Packages Projects Releases Wiki Activity

AC_WPNav: Remove unused functions

Browse Source
This commit is contained in:
Leonard Hall
2025-09-26 17:54:32 +09:00
committed by Randy Mackay
parent 4b436becb7
commit bf0ffe499a
6 changed files with 7 additions and 168 deletions
Download Patch File Download Diff File Expand all files Collapse all files
libraries/AC_WPNav/AC_Circle.cpp
-13
View File
@@ -410,19 +410,6 @@ AC_Circle::TerrainSource AC_Circle::get_terrain_source() const
#endif
}
// Returns terrain offset in centimeters above the EKF origin at the current position.
// See get_terrain_offset_m() for full details.
bool AC_Circle::get_terrain_offset_cm(float& offset_cm)
{
// Convert input value to meters for internal processing
float offset_m = offset_cm * 0.01;
// Retrieve terrain offset in meters
bool terrain_valid = get_terrain_offset_m(offset_m);
// Convert result back to centimeters
offset_cm = offset_m * 100.0;
return terrain_valid;
}
// Returns terrain offset in meters above the EKF origin at the current position.
// Positive values indicate terrain is above the EKF origin altitude.
// Terrain source may be rangefinder or terrain database.
libraries/AC_WPNav/AC_Circle.h
+2 -14
View File
@@ -40,10 +40,6 @@ public:
// If conversion fails, defaults to current position and logs a navigation error.
void set_center(const Location& center);
// Sets the circle center using a NEU position vector in centimeters from the EKF origin.
// See set_center_NEU_m() for full details.
void set_center_NEU_cm(const Vector3f& center_neu_cm, bool is_terrain_alt) { _center_neu_m = center_neu_cm.topostype() * 0.01; _is_terrain_alt = is_terrain_alt; }
// Sets the circle center using a NEU position vector in meters from the EKF origin.
// If `is_terrain_alt` is true, the Z component is treated as altitude above terrain; otherwise, above EKF origin.
void set_center_NEU_m(const Vector3p& center_neu_m, bool is_terrain_alt) { _center_neu_m = center_neu_m; _is_terrain_alt = is_terrain_alt; }
@@ -100,11 +96,11 @@ public:
// Returns the desired roll angle in centidegrees from the position controller.
// Should be passed to the attitude controller as a stabilization input.
float get_roll_cd() const { return _pos_control.get_roll_cd(); }
float get_roll_cd() const { return rad_to_cd(_pos_control.get_roll_rad()); }
// Returns the desired pitch angle in centidegrees from the position controller.
// Should be passed to the attitude controller as a stabilization input.
float get_pitch_cd() const { return _pos_control.get_pitch_cd(); }
float get_pitch_cd() const { return rad_to_cd(_pos_control.get_pitch_rad()); }
// Returns the desired yaw angle in centidegrees computed by the circle controller.
// May be oriented toward the circle center or along the path depending on configuration.
@@ -133,10 +129,6 @@ public:
// If the vehicle is at the center, the point directly behind the vehicle (based on yaw) is returned.
void get_closest_point_on_circle_NEU_m(Vector3p& result_NEU_m, float& dist_m) const;
// Returns the horizontal distance to the circle target in centimeters.
// See get_distance_to_target_m() for full details.
float get_distance_to_target_cm() const { return get_distance_to_target_m() * 100.0; }
// Returns the horizontal distance to the circle target in meters.
// Calculated using the position controller’s NE position error norm.
float get_distance_to_target_m() const { return _pos_control.get_pos_error_NE_m(); }
@@ -188,10 +180,6 @@ private:
};
AC_Circle::TerrainSource get_terrain_source() const;
// Returns terrain offset in centimeters above the EKF origin at the current position.
// See get_terrain_offset_m() for full details.
bool get_terrain_offset_cm(float& offset_cm);
// Returns terrain offset in meters above the EKF origin at the current position.
// Positive values indicate terrain is above the EKF origin altitude.
// Terrain source may be rangefinder or terrain database.
libraries/AC_WPNav/AC_Loiter.cpp
-26
View File
@@ -94,13 +94,6 @@ AC_Loiter::AC_Loiter(const AP_AHRS_View& ahrs, AC_PosControl& pos_control, const
AP_Param::setup_object_defaults(this, var_info);
}
// Sets the initial loiter target position in centimeters from the EKF origin.
// See init_target_m() for full details.
void AC_Loiter::init_target_cm(const Vector2f& position_ne_cm)
{
init_target_m(position_ne_cm.topostype() * 0.01);
}
// Sets the initial loiter target position in meters from the EKF origin.
// - position_neu_m: horizontal position in the NE frame, in meters.
// - Initializes internal control state including acceleration targets and feed-forward planning.
@@ -198,18 +191,6 @@ void AC_Loiter::set_pilot_desired_acceleration_rad(float euler_roll_angle_rad, f
}
}
// Calculates the expected stopping point based on current velocity and position in the NE frame.
// Result is returned in centimeters.
// See get_stopping_point_NE_m() for full details.
void AC_Loiter::get_stopping_point_NE_cm(Vector2f& stopping_point_ne_cm) const
{
Vector2f stop_ne_m;
// Retrieve stopping point in meters
get_stopping_point_NE_m(stop_ne_m);
// Convert to centimeters
stopping_point_ne_cm = stop_ne_m * 100.0;
}
// Calculates the expected stopping point based on current velocity and position in the NE frame.
// Result is returned in meters.
// Uses the position controller’s deceleration model.
@@ -255,13 +236,6 @@ void AC_Loiter::update(bool avoidance_on)
_pos_control.update_NE_controller();
}
// Sets the maximum allowed horizontal loiter speed in cm/s.
// See set_speed_max_NE_ms() for full details.
void AC_Loiter::set_speed_max_NE_cms(float speed_max_ne_cms)
{
set_speed_max_NE_ms(speed_max_ne_cms * 0.01);
}
// Sets the maximum allowed horizontal loiter speed in m/s.
// Internally converts to cm/s and clamps to a minimum of LOITER_SPEED_MIN_CMS.
void AC_Loiter::set_speed_max_NE_ms(float speed_max_ne_ms)
libraries/AC_WPNav/AC_Loiter.h
+2 -23
View File
@@ -15,10 +15,6 @@ public:
/// Constructor
AC_Loiter(const AP_AHRS_View& ahrs, AC_PosControl& pos_control, const AC_AttitudeControl& attitude_control);
// Sets the initial loiter target position in centimeters from the EKF origin.
// See init_target_m() for full details.
void init_target_cm(const Vector2f& position_neu_cm);
// Sets the initial loiter target position in meters from the EKF origin.
// - position_neu_m: horizontal position in the NE frame, in meters.
// - Initializes internal control state including acceleration targets and feed-forward planning.
@@ -42,10 +38,6 @@ public:
// - Applies internal shaping using the current attitude controller dt.
void set_pilot_desired_acceleration_rad(float euler_roll_angle_rad, float euler_pitch_angle_rad);
// Returns pilot-requested horizontal acceleration in the NE frame in cm/s².
// See get_pilot_desired_acceleration_NE_mss() for full details.
Vector2f get_pilot_desired_acceleration_NE_cmss() const { return get_pilot_desired_acceleration_NE_mss() * 100.0; }
// Returns pilot-requested horizontal acceleration in the NE frame in m/s².
// This is the internally computed and smoothed acceleration vector applied by the loiter controller.
const Vector2f& get_pilot_desired_acceleration_NE_mss() const { return _desired_accel_ne_mss; }
@@ -53,20 +45,11 @@ public:
// Clears any pilot-requested acceleration by setting roll and pitch inputs to zero.
void clear_pilot_desired_acceleration() { set_pilot_desired_acceleration_rad(0.0, 0.0); }
// Calculates the expected stopping point based on current velocity and position in the NE frame.
// Result is returned in centimeters.
// See get_stopping_point_NE_m() for full details.
void get_stopping_point_NE_cm(Vector2f& stopping_point_ne_cm) const;
// Calculates the expected stopping point based on current velocity and position in the NE frame.
// Result is returned in meters.
// Uses the position controller’s deceleration model.
void get_stopping_point_NE_m(Vector2f& stopping_point_ne_m) const;
// Returns the horizontal distance to the loiter target in centimeters.
// See get_distance_to_target_m() for full details.
float get_distance_to_target_cm() const { return get_distance_to_target_m() * 100.0; }
// Returns the horizontal distance to the loiter target in meters.
// Computed using the NE position error from the position controller.
float get_distance_to_target_m() const { return _pos_control.get_pos_error_NE_m(); }
@@ -88,19 +71,15 @@ public:
// If `avoidance_on` is true, velocity is adjusted using avoidance logic before being applied.
void update(bool avoidance_on = true);
// Sets the maximum allowed horizontal loiter speed in cm/s.
// See set_speed_max_NE_ms() for full details.
void set_speed_max_NE_cms(float speed_max_NE_cms);
// Sets the maximum allowed horizontal loiter speed in m/s.
// Internally converts to cm/s and clamps to a minimum of LOITER_SPEED_MIN_CMS.
void set_speed_max_NE_ms(float speed_max_NE_ms);
// Returns the desired roll angle in centidegrees from the loiter controller.
float get_roll_cd() const { return _pos_control.get_roll_cd(); }
float get_roll_cd() const { return rad_to_cd(get_roll_rad()); }
// Returns the desired pitch angle in centidegrees from the loiter controller.
float get_pitch_cd() const { return _pos_control.get_pitch_cd(); }
float get_pitch_cd() const { return rad_to_cd(get_pitch_rad()); }
// Returns the desired roll angle in radians from the loiter controller.
float get_roll_rad() const { return _pos_control.get_roll_rad(); }
libraries/AC_WPNav/AC_WPNav.cpp
-53
View File
@@ -150,14 +150,6 @@ AC_WPNav::TerrainSource AC_WPNav::get_terrain_source() const
/// waypoint navigation
///
// Initializes waypoint and spline controllers using inputs in cm.
// See wp_and_spline_init_m() for full details.
void AC_WPNav::wp_and_spline_init_cm(float speed_cms, Vector3f stopping_point_neu_cm)
{
// convert inputs from centimeters to meters and call the main initializer
wp_and_spline_init_m(speed_cms * 0.01, stopping_point_neu_cm.topostype() * 0.01);
}
// Initializes waypoint and spline navigation using inputs in meters.
// Sets speed and acceleration limits, calculates jerk constraints,
// and initializes spline or S-curve leg with a defined starting point.
@@ -243,13 +235,6 @@ void AC_WPNav::set_speed_NE_ms(float speed_ms)
}
}
// Sets the climb speed for waypoint navigation in cm/s.
// See set_speed_up_ms() for full details.
void AC_WPNav::set_speed_up_cms(float speed_up_cms)
{
set_speed_up_ms(speed_up_cms * 0.01);
}
// Sets the climb speed for waypoint navigation in m/s.
// Updates the vertical controller with the new ascent rate limit.
void AC_WPNav::set_speed_up_ms(float speed_up_ms)
@@ -261,14 +246,6 @@ void AC_WPNav::set_speed_up_ms(float speed_up_ms)
update_track_with_speed_accel_limits();
}
// Sets the descent speed for waypoint navigation in cm/s.
// See set_speed_down_ms() for full details.
void AC_WPNav::set_speed_down_cms(float speed_down_cms)
{
// convert cm/s to m/s and apply to descent limit
set_speed_down_ms(speed_down_cms * 0.01);
}
// Sets the descent speed for waypoint navigation in m/s.
// Updates the vertical controller with the new descent rate limit.
void AC_WPNav::set_speed_down_ms(float speed_down_ms)
@@ -410,13 +387,6 @@ bool AC_WPNav::set_wp_destination_NEU_m(const Vector3p& destination_neu_m, bool
return true;
}
// Sets the next waypoint destination using a NEU position vector in centimeters.
// See set_wp_destination_next_NEU_m() for full details.
bool AC_WPNav::set_wp_destination_next_NEU_cm(const Vector3f& destination_neu_cm, bool is_terrain_alt)
{
return set_wp_destination_next_NEU_m(destination_neu_cm.topostype() * 0.01, is_terrain_alt);
}
// Sets the next waypoint destination using a NEU position vector in meters.
// Only updates if terrain frame matches current leg.
// Calculates trajectory preview for smoother transition into next segment.
@@ -735,17 +705,6 @@ bool AC_WPNav::force_stop_at_next_wp()
return true;
}
// Returns terrain offset in cm above the EKF origin at the current position.
// See get_terrain_offset_m() for full details.
bool AC_WPNav::get_terrain_offset_cm(float& offset_cm)
{
// call meter-version and convert to centimeters
float offset_m = offset_cm;
const bool ret = get_terrain_offset_m(offset_m);
offset_cm = offset_m * 100.0;
return ret;
}
// Returns terrain offset in meters above the EKF origin at the current position.
// Positive values mean terrain lies above the EKF origin altitude.
// Source may be rangefinder or terrain database depending on availability.
@@ -965,18 +924,6 @@ bool AC_WPNav::set_spline_destination_next_NEU_m(const Vector3p& next_destinatio
return true;
}
// Converts a Location to a NEU position vector in cm from the EKF origin.
// See get_vector_NEU_m() for full details.
bool AC_WPNav::get_vector_NEU_cm(const Location &loc, Vector3f &pos_from_origin_neu_cm, bool &is_terrain_alt)
{
// convert input to meters for processing
Vector3p pos_from_origin_neu_m = pos_from_origin_neu_cm.topostype() * 0.01;
// perform vector conversion using meters interface
const bool ret = get_vector_NEU_m(loc, pos_from_origin_neu_m, is_terrain_alt);
pos_from_origin_neu_cm = pos_from_origin_neu_m.tofloat() * 100.0;
return ret;
}
// Converts a Location to a NEU position vector in meters from the EKF origin.
// Sets `is_terrain_alt` to true if the resulting Z position is relative to terrain.
// Returns false if terrain data is unavailable or conversion fails.
libraries/AC_WPNav/AC_WPNav.h
+3 -39
View File
@@ -44,10 +44,6 @@ public:
};
AC_WPNav::TerrainSource get_terrain_source() const;
// Returns terrain offset in cm above the EKF origin at the current position.
// See get_terrain_offset_m() for full details.
bool get_terrain_offset_cm(float& offset_cm);
// Returns terrain offset in meters above the EKF origin at the current position.
// Positive values mean terrain lies above the EKF origin altitude.
// Source may be rangefinder or terrain database depending on availability.
@@ -57,10 +53,6 @@ public:
// Vehicle will stop if distance from target altitude exceeds this margin.
float get_terrain_margin_m() const { return MAX(_terrain_margin_m, 0.1); }
// Converts a Location to a NEU position vector in cm from the EKF origin.
// See get_vector_NEU_m() for full details.
bool get_vector_NEU_cm(const Location &loc, Vector3f &pos_from_origin_NEU_cm, bool &is_terrain_alt);
// Converts a Location to a NEU position vector in meters from the EKF origin.
// Sets `is_terrain_alt` to true if the resulting Z position is relative to terrain.
// Returns false if terrain data is unavailable or conversion fails.
@@ -70,10 +62,6 @@ public:
/// waypoint controller
///
// Initializes waypoint and spline controllers using inputs in cm.
// See wp_and_spline_init_m() for full details.
void wp_and_spline_init_cm(float speed_cms = 0.0f, Vector3f stopping_point_neu_cm = Vector3f{});
// Initializes waypoint and spline navigation using inputs in meters.
// Sets speed and acceleration limits, calculates jerk constraints,
// and initializes spline or S-curve leg with a defined starting point.
@@ -96,18 +84,10 @@ public:
// Returns true if waypoint navigation is currently paused via set_pause().
bool paused() { return _paused; }
// Sets the climb speed for waypoint navigation in cm/s.
// See set_speed_up_ms() for full details.
void set_speed_up_cms(float speed_up_cms);
// Sets the climb speed for waypoint navigation in m/s.
// Updates the vertical controller with the new ascent rate limit.
void set_speed_up_ms(float speed_up_ms);
// Sets the descent speed for waypoint navigation in cm/s.
// See set_speed_down_ms() for full details.
void set_speed_down_cms(float speed_down_cms);
// Sets the descent speed for waypoint navigation in m/s.
// Updates the vertical controller with the new descent rate limit.
void set_speed_down_ms(float speed_down_ms);
@@ -206,10 +186,6 @@ public:
// Returns false if terrain offset cannot be determined when required.
virtual bool set_wp_destination_NEU_m(const Vector3p& destination_neu_m, bool is_terrain_alt = false);
// Sets the next waypoint destination using a NEU position vector in centimeters.
// See set_wp_destination_next_NEU_m() for full details.
bool set_wp_destination_next_NEU_cm(const Vector3f& destination_neu_cm, bool is_terrain_alt = false);
// Sets the next waypoint destination using a NEU position vector in meters.
// Only updates if terrain frame matches current leg.
// Calculates trajectory preview for smoother transition into next segment.
@@ -265,10 +241,6 @@ public:
return get_wp_distance_to_destination_m() < _wp_radius_cm * 0.01;
}
// Returns the waypoint acceptance radius in centimeters.
// See get_wp_radius_m() for full details.
float get_wp_radius_cm() const { return get_wp_radius_m() * 100.0; }
// Returns the waypoint acceptance radius in meters.
// This radius defines the distance from the target waypoint within which the vehicle is considered to have arrived.
float get_wp_radius_m() const { return _wp_radius_cm * 0.01; }
@@ -302,20 +274,12 @@ public:
// Returns false if any conversion from location to vector fails.
bool set_spline_destination_next_loc(const Location& next_destination, const Location& next_next_destination, bool next_next_is_spline);
// Sets the current spline segment using position vectors in centimeters.
// See set_spline_destination_NEU_m() for full details.
bool set_spline_destination_NEU_cm(const Vector3f& destination_neu_cm, bool is_terrain_alt, const Vector3f& next_destination_neu_cm, bool next_terrain_alt, bool next_is_spline);
// Sets the current spline waypoint using NEU position vectors in meters.
// Initializes a spline path from `destination_neu_m` to `next_destination_neu_m`, respecting terrain altitude framing.
// Both waypoints must use the same altitude frame (either above terrain or above origin).
// Returns false if terrain altitude cannot be determined when required.
bool set_spline_destination_NEU_m(const Vector3p& destination_neu_m, bool is_terrain_alt, const Vector3p& next_destination_neu_m, bool next_terrain_alt, bool next_is_spline);
// Sets the next spline segment using NEU position vectors in centimeters.
// See set_spline_destination_next_NEU_m() for full details.
bool set_spline_destination_next_NEU_cm(const Vector3f& next_destination_neu_cm, bool next_is_terrain_alt, const Vector3f& next_next_destination_neu_cm, bool next_next_is_terrain_alt, bool next_next_is_spline);
// Sets the next spline segment using NEU position vectors in meters.
// Creates a spline path from the current destination to `next_destination_neu_m`, and prepares transition toward `next_next_destination_neu_m`.
// All waypoints must use the same altitude frame (above terrain or origin).
@@ -344,15 +308,15 @@ public:
// Returns the desired roll angle in centidegrees from the position controller.
// See get_roll_rad() for full details.
float get_roll() const { return _pos_control.get_roll_cd(); }
float get_roll() const { return rad_to_cd(get_roll_rad()); }
// Returns the desired pitch angle in centidegrees from the position controller.
// See get_pitch_rad() for full details.
float get_pitch() const { return _pos_control.get_pitch_cd(); }
float get_pitch() const { return rad_to_cd(get_pitch_rad()); }
// Returns the desired yaw angle in centidegrees from the position controller.
// See get_yaw_rad() for full details.
float get_yaw() const { return _pos_control.get_yaw_cd(); }
float get_yaw() const { return rad_to_cd(get_yaw_rad()); }
// Advances the target location along the current path segment.
// Updates target position, velocity, and acceleration based on jerk-limited profile (or spline).