diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp index e02681a9e3..ed675beeb0 100644 --- a/libraries/AC_WPNav/AC_WPNav.cpp +++ b/libraries/AC_WPNav/AC_WPNav.cpp @@ -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; -} \ No newline at end of file +} diff --git a/libraries/AC_WPNav/AC_WPNav.h b/libraries/AC_WPNav/AC_WPNav.h index aea26ac2b7..6f5ada0d28 100644 --- a/libraries/AC_WPNav/AC_WPNav.h +++ b/libraries/AC_WPNav/AC_WPNav.h @@ -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();