mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-02 14:08:45 -04:00
AC_WPNav: optimise to reduce memory copies
save a few cycles Pair-Programmed-With: Randy Mackay <rmackay9@yahoo.com>
This commit is contained in:
Andrew Tridgell
parent
5f1bd1a452
commit
ad40ba57be
@ -66,17 +66,17 @@ AC_WPNav::AC_WPNav(AP_InertialNav* inav, APM_PI* pid_pos_lat, APM_PI* pid_pos_lo
|
||||
///
|
||||
|
||||
/// project_stopping_point - returns vector to stopping point based on a horizontal position and velocity
|
||||
Vector3f AC_WPNav::project_stopping_point(const Vector3f& position, const Vector3f& velocity)
|
||||
void AC_WPNav::project_stopping_point(const Vector3f& position, const Vector3f& velocity, Vector3f &target)
|
||||
{
|
||||
float linear_distance; // half the distace we swap between linear and sqrt and the distace we offset sqrt.
|
||||
float linear_velocity; // the velocity we swap between linear and sqrt.
|
||||
float vel_total;
|
||||
float target_dist;
|
||||
Vector3f target;
|
||||
|
||||
// avoid divide by zero
|
||||
if( _pid_pos_lat->kP() <= 0.1 ) {
|
||||
return(position);
|
||||
target = position;
|
||||
return;
|
||||
}
|
||||
|
||||
// calculate point at which velocity switches from linear to sqrt
|
||||
@ -97,13 +97,13 @@ Vector3f AC_WPNav::project_stopping_point(const Vector3f& position, const Vector
|
||||
target.x = position.x + (target_dist * velocity.x / vel_total);
|
||||
target.y = position.y + (target_dist * velocity.y / vel_total);
|
||||
target.z = position.z;
|
||||
return target;
|
||||
}
|
||||
|
||||
/// set_loiter_target - set initial loiter target based on current position and velocity
|
||||
void AC_WPNav::set_loiter_target(const Vector3f& position, const Vector3f& velocity)
|
||||
{
|
||||
Vector3f target = project_stopping_point(position, velocity);
|
||||
Vector3f target;
|
||||
project_stopping_point(position, velocity, target);
|
||||
_target.x = target.x;
|
||||
_target.y = target.y;
|
||||
}
|
||||
@ -167,13 +167,13 @@ void AC_WPNav::translate_loiter_target_movements(float nav_dt)
|
||||
}
|
||||
|
||||
/// get_distance_to_target - get horizontal distance to loiter target in cm
|
||||
float AC_WPNav::get_distance_to_target()
|
||||
float AC_WPNav::get_distance_to_target() const
|
||||
{
|
||||
return _distance_to_target;
|
||||
}
|
||||
|
||||
/// get_bearing_to_target - get bearing to loiter target in centi-degrees
|
||||
int32_t AC_WPNav::get_bearing_to_target()
|
||||
int32_t AC_WPNav::get_bearing_to_target() const
|
||||
{
|
||||
return get_bearing_cd(_inav->get_position(), _target);
|
||||
}
|
||||
@ -205,18 +205,16 @@ void AC_WPNav::update_loiter()
|
||||
/// set_destination - set destination using cm from home
|
||||
void AC_WPNav::set_destination(const Vector3f& destination)
|
||||
{
|
||||
Vector3f origin;
|
||||
|
||||
// if waypoint controlls is active and copter has reached the previous waypoint use it for the origin
|
||||
if( _reached_destination && ((hal.scheduler->millis() - _wpnav_last_update) < 1000) ) {
|
||||
origin = _destination;
|
||||
_origin = _destination;
|
||||
}else{
|
||||
// otherwise calculate origin from the current position and velocity
|
||||
origin = project_stopping_point(_inav->get_position(), _inav->get_velocity());
|
||||
project_stopping_point(_inav->get_position(), _inav->get_velocity(), _origin);
|
||||
}
|
||||
|
||||
// set origin and destination
|
||||
set_origin_and_destination(origin, destination);
|
||||
set_origin_and_destination(_origin, destination);
|
||||
}
|
||||
|
||||
/// set_origin_and_destination - set origin and destination using lat/lon coordinates
|
||||
@ -430,7 +428,7 @@ void AC_WPNav::get_loiter_acceleration_to_lean_angles(float accel_lat, float acc
|
||||
|
||||
// get_bearing_cd - return bearing in centi-degrees between two positions
|
||||
// To-Do: move this to math library
|
||||
float AC_WPNav::get_bearing_cd(const Vector3f origin, const Vector3f destination)
|
||||
float AC_WPNav::get_bearing_cd(const Vector3f &origin, const Vector3f &destination) const
|
||||
{
|
||||
float bearing = 9000 + atan2f(-(destination.x-origin.x), destination.y-origin.y) * 5729.57795f;
|
||||
if (bearing < 0) {
|
||||
@ -498,4 +496,4 @@ void AC_WPNav::calculate_leash_length(bool climb)
|
||||
|
||||
// calculate vertical track scale used to give altitude equal weighting to horizontal position
|
||||
_vert_track_scale = _leash_xy / leash_z;
|
||||
}
|
||||
}
|
||||
|
||||
@ -48,7 +48,7 @@ public:
|
||||
///
|
||||
|
||||
/// get_loiter_target - get loiter target as position vector (from home in cm)
|
||||
Vector3f get_loiter_target() { return _target; }
|
||||
const Vector3f &get_loiter_target() const { return _target; }
|
||||
|
||||
/// set_loiter_target in cm from home
|
||||
void set_loiter_target(const Vector3f& position) { _target = position; }
|
||||
@ -60,10 +60,10 @@ public:
|
||||
void move_loiter_target(float control_roll, float control_pitch, float dt);
|
||||
|
||||
/// get_distance_to_target - get horizontal distance to loiter target in cm
|
||||
float get_distance_to_target();
|
||||
float get_distance_to_target() const;
|
||||
|
||||
/// get_bearing_to_target - get bearing to loiter target in centi-degrees
|
||||
int32_t get_bearing_to_target();
|
||||
int32_t get_bearing_to_target() const;
|
||||
|
||||
/// update_loiter - run the loiter controller - should be called at 10hz
|
||||
void update_loiter();
|
||||
@ -75,14 +75,14 @@ public:
|
||||
void clear_angle_limit() { _lean_angle_max = MAX_LEAN_ANGLE; }
|
||||
|
||||
/// get_angle_limit - retrieve maximum angle in centi-degrees the copter will lean
|
||||
int32_t get_angle_limit() { return _lean_angle_max; }
|
||||
int32_t get_angle_limit() const { return _lean_angle_max; }
|
||||
|
||||
///
|
||||
/// waypoint controller
|
||||
///
|
||||
|
||||
/// get_destination waypoint using position vector (distance from home in cm)
|
||||
Vector3f get_destination() { return _destination; }
|
||||
const Vector3f &get_destination() const { return _destination; }
|
||||
|
||||
/// set_destination waypoint using position vector (distance from home in cm)
|
||||
void set_destination(const Vector3f& destination);
|
||||
@ -100,7 +100,7 @@ public:
|
||||
int32_t get_bearing_to_destination();
|
||||
|
||||
/// reached_destination - true when we have come within RADIUS cm of the waypoint
|
||||
bool reached_destination() { return _reached_destination; }
|
||||
bool reached_destination() const { return _reached_destination; }
|
||||
|
||||
/// update_wp - update waypoint controller
|
||||
void update_wpnav();
|
||||
@ -110,11 +110,11 @@ public:
|
||||
///
|
||||
|
||||
/// get desired roll, pitch which should be fed into stabilize controllers
|
||||
int32_t get_desired_roll() { return _desired_roll; };
|
||||
int32_t get_desired_pitch() { return _desired_pitch; };
|
||||
int32_t get_desired_roll() const { return _desired_roll; };
|
||||
int32_t get_desired_pitch() const { return _desired_pitch; };
|
||||
|
||||
/// get_desired_alt - get desired altitude (in cm above home) from loiter or wp controller which should be fed into throttle controller
|
||||
float get_desired_alt() { return _target.z; }
|
||||
float get_desired_alt() const { return _target.z; }
|
||||
|
||||
/// set_desired_alt - set desired altitude (in cm above home)
|
||||
void set_desired_alt(float desired_alt) { _target.z = desired_alt; }
|
||||
@ -130,20 +130,20 @@ public:
|
||||
void set_horizontal_velocity(float velocity_cms) { _speed_xy_cms = velocity_cms; };
|
||||
|
||||
/// get_climb_velocity - returns target climb speed in cm/s during missions
|
||||
float get_climb_velocity() { return _speed_up_cms; };
|
||||
float get_climb_velocity() const { return _speed_up_cms; };
|
||||
|
||||
/// get_descent_velocity - returns target descent speed in cm/s during missions. Note: always positive
|
||||
float get_descent_velocity() { return _speed_down_cms; };
|
||||
float get_descent_velocity() const { return _speed_down_cms; };
|
||||
|
||||
/// get_waypoint_radius - access for waypoint radius in cm
|
||||
float get_waypoint_radius() { return _wp_radius_cm; }
|
||||
float get_waypoint_radius() const { return _wp_radius_cm; }
|
||||
|
||||
static const struct AP_Param::GroupInfo var_info[];
|
||||
|
||||
protected:
|
||||
|
||||
/// project_stopping_point - returns vector to stopping point based on a horizontal position and velocity
|
||||
Vector3f project_stopping_point(const Vector3f& position, const Vector3f& velocity);
|
||||
void project_stopping_point(const Vector3f& position, const Vector3f& velocity, Vector3f &target);
|
||||
|
||||
/// translate_loiter_target_movements - consumes adjustments created by move_loiter_target
|
||||
void translate_loiter_target_movements(float nav_dt);
|
||||
@ -161,7 +161,7 @@ protected:
|
||||
void get_loiter_acceleration_to_lean_angles(float accel_lat_cmss, float accel_lon_cmss);
|
||||
|
||||
/// get_bearing_cd - return bearing in centi-degrees between two positions
|
||||
float get_bearing_cd(const Vector3f origin, const Vector3f destination);
|
||||
float get_bearing_cd(const Vector3f &origin, const Vector3f &destination) const;
|
||||
|
||||
/// reset_I - clears I terms from loiter PID controller
|
||||
void reset_I();
|
||||
|
||||
Loading…
Reference in New Issue
Block a user