Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-12 09:33:59 -03:00
Code Issues Packages Projects Releases Wiki Activity

AC_WPNav: optimise to reduce memory copies

Browse Source 
save a few cycles

Pair-Programmed-With: Randy Mackay <rmackay9@yahoo.com>
This commit is contained in:
Andrew Tridgell 2013-04-21 20:54:31 +10:00
parent 5f1bd1a452
commit ad40ba57be
2 changed files with 26 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
libraries/AC_WPNav/AC_WPNav.cpp
View File

@ -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 /// 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_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 linear_velocity; // the velocity we swap between linear and sqrt.
float vel_total; float vel_total;
float target_dist; float target_dist;
Vector3f target;
// avoid divide by zero // avoid divide by zero
if( _pid_pos_lat->kP() <= 0.1 ) { if( _pid_pos_lat->kP() <= 0.1 ) {
return(position); target = position;
return;
} }
// calculate point at which velocity switches from linear to sqrt // 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.x = position.x + (target_dist * velocity.x / vel_total);
target.y = position.y + (target_dist * velocity.y / vel_total); target.y = position.y + (target_dist * velocity.y / vel_total);
target.z = position.z; target.z = position.z;
return target;
} }
/// set_loiter_target - set initial loiter target based on current position and velocity /// 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) 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.x = target.x;
_target.y = target.y; _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 /// 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; return _distance_to_target;
} }
/// get_bearing_to_target - get bearing to loiter target in centi-degrees /// 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); 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 /// set_destination - set destination using cm from home
void AC_WPNav::set_destination(const Vector3f& destination) 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 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) ) { if( _reached_destination && ((hal.scheduler->millis() - _wpnav_last_update) < 1000) ) {
origin = _destination; _origin = _destination;
}else{ }else{
// otherwise calculate origin from the current position and velocity // 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
set_origin_and_destination(origin, destination); set_origin_and_destination(_origin, destination);
} }
/// set_origin_and_destination - set origin and destination using lat/lon coordinates /// 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 // get_bearing_cd - return bearing in centi-degrees between two positions
// To-Do: move this to math library // 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; float bearing = 9000 + atan2f(-(destination.x-origin.x), destination.y-origin.y) * 5729.57795f;
if (bearing < 0) { if (bearing < 0) {

28
libraries/AC_WPNav/AC_WPNav.h
View File

@ -48,7 +48,7 @@ public:
/// ///
/// get_loiter_target - get loiter target as position vector (from home in cm) /// 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 /// set_loiter_target in cm from home
void set_loiter_target(const Vector3f& position) { _target = position; } 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); void move_loiter_target(float control_roll, float control_pitch, float dt);
/// get_distance_to_target - get horizontal distance to loiter target in cm /// 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 /// 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 /// update_loiter - run the loiter controller - should be called at 10hz
void update_loiter(); void update_loiter();
@ -75,14 +75,14 @@ public:
void clear_angle_limit() { _lean_angle_max = MAX_LEAN_ANGLE; } void clear_angle_limit() { _lean_angle_max = MAX_LEAN_ANGLE; }
/// get_angle_limit - retrieve maximum angle in centi-degrees the copter will lean /// 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 /// waypoint controller
/// ///
/// get_destination waypoint using position vector (distance from home in cm) /// 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) /// set_destination waypoint using position vector (distance from home in cm)
void set_destination(const Vector3f& destination); void set_destination(const Vector3f& destination);
@ -100,7 +100,7 @@ public:
int32_t get_bearing_to_destination(); int32_t get_bearing_to_destination();
/// reached_destination - true when we have come within RADIUS cm of the waypoint /// 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 /// update_wp - update waypoint controller
void update_wpnav(); void update_wpnav();
@ -110,11 +110,11 @@ public:
/// ///
/// get desired roll, pitch which should be fed into stabilize controllers /// get desired roll, pitch which should be fed into stabilize controllers
int32_t get_desired_roll() { return _desired_roll; }; int32_t get_desired_roll() const { return _desired_roll; };
int32_t get_desired_pitch() { return _desired_pitch; }; 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 /// 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) /// set_desired_alt - set desired altitude (in cm above home)
void set_desired_alt(float desired_alt) { _target.z = desired_alt; } 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; }; void set_horizontal_velocity(float velocity_cms) { _speed_xy_cms = velocity_cms; };
/// get_climb_velocity - returns target climb speed in cm/s during missions /// 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 /// 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 /// 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[]; static const struct AP_Param::GroupInfo var_info[];
protected: protected:
/// project_stopping_point - returns vector to stopping point based on a horizontal position and velocity /// 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 /// translate_loiter_target_movements - consumes adjustments created by move_loiter_target
void translate_loiter_target_movements(float nav_dt); 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); 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 /// 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 /// reset_I - clears I terms from loiter PID controller
void reset_I(); void reset_I();