diff --git a/libraries/AC_WPNav/AC_Circle.cpp b/libraries/AC_WPNav/AC_Circle.cpp index 714a83b4724..935dbd1c8c1 100644 --- a/libraries/AC_WPNav/AC_Circle.cpp +++ b/libraries/AC_WPNav/AC_Circle.cpp @@ -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. diff --git a/libraries/AC_WPNav/AC_Circle.h b/libraries/AC_WPNav/AC_Circle.h index cc52c3605c9..1258bc849e6 100644 --- a/libraries/AC_WPNav/AC_Circle.h +++ b/libraries/AC_WPNav/AC_Circle.h @@ -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. diff --git a/libraries/AC_WPNav/AC_Loiter.cpp b/libraries/AC_WPNav/AC_Loiter.cpp index ee663eb95bd..d6509c974a7 100644 --- a/libraries/AC_WPNav/AC_Loiter.cpp +++ b/libraries/AC_WPNav/AC_Loiter.cpp @@ -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) diff --git a/libraries/AC_WPNav/AC_Loiter.h b/libraries/AC_WPNav/AC_Loiter.h index dc55f74b639..8b566d24a31 100644 --- a/libraries/AC_WPNav/AC_Loiter.h +++ b/libraries/AC_WPNav/AC_Loiter.h @@ -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(); } diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp index ee8f45f0e5f..c5be207a0fa 100644 --- a/libraries/AC_WPNav/AC_WPNav.cpp +++ b/libraries/AC_WPNav/AC_WPNav.cpp @@ -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. diff --git a/libraries/AC_WPNav/AC_WPNav.h b/libraries/AC_WPNav/AC_WPNav.h index c84af1ddf4e..08dcae3689f 100644 --- a/libraries/AC_WPNav/AC_WPNav.h +++ b/libraries/AC_WPNav/AC_WPNav.h @@ -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).