Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AC_WPNav: add distance and bearing to target methods

This commit is contained in:
Randy Mackay 2013-03-20 22:28:05 +09:00
parent b92c4097d2
commit 39bc3800c9
2 changed files with 122 additions and 39 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

100
libraries/AC_WPNav/AC_WPNav.cpp
View File

@ -35,14 +35,6 @@ AC_WPNav::AC_WPNav(AP_InertialNav* inav, APM_PI* pid_pos_lat, APM_PI* pid_pos_lo
/// simple loiter controller
///
/// set_loiter_target in cm from home
void AC_WPNav::set_loiter_target(const Vector3f& position)
{
_state = WPNAV_STATE_LOITER;
_target = position;
}
/// move_loiter_target - move loiter target by velocity provided in front/right directions in cm/s
void AC_WPNav::move_loiter_target(int16_t vel_forward_cms, int16_t vel_right_cms, float dt)
{
@ -65,6 +57,35 @@ void AC_WPNav::move_loiter_target(int16_t vel_forward_cms, int16_t vel_right_cms
_target.y += vel_lon * dt;
}
/// get_distance_to_target - get horizontal distance to loiter target in cm
float AC_WPNav::get_distance_to_target()
{
return _distance_to_target;
}
/// get_bearing_to_target - get bearing to loiter target in centi-degrees
int32_t AC_WPNav::get_bearing_to_target()
{
return get_bearing_cd(_inav->get_position(), _target);
}
/// update_loiter - run the loiter controller - should be called at 10hz
void AC_WPNav::update_loiter()
{
uint32_t now = hal.scheduler->millis();
float dt = now - _last_update;
_last_update = now;
// catch if we've just been started
if( dt >= 1.0 ) {
dt = 0.0;
reset_I();
}
// run loiter position controller
get_loiter_pos_lat_lon(_target.x, _target.y, dt);
}
///
/// waypoint navigation
///
@ -78,7 +99,6 @@ void AC_WPNav::set_destination(const Vector3f& destination)
/// set_origin_and_destination - set origin and destination using lat/lon coordinates
void AC_WPNav::set_origin_and_destination(const Vector3f& origin, const Vector3f& destination)
{
_state = WPNAV_STATE_WPNAV;
_origin = origin;
_destination = destination;
_pos_delta = _destination - _origin;
@ -148,20 +168,33 @@ void AC_WPNav::advance_target_along_track(float velocity_cms, float dt)
_target.y = _origin.y + _pos_delta.y * track_length_pct;
}
///
/// shared methods
///
/// update - run the loiter and wpnav controllers - should be called at 10hz
void AC_WPNav::update()
/// get_distance_to_destination - get horizontal distance to destination in cm
float AC_WPNav::get_distance_to_destination()
{
float dt = hal.scheduler->millis() - _last_update;
// get current location
Vector3f curr = _inav->get_position();
return pythagorous2(_destination.x-curr.x,_destination.y-curr.y);
}
// prevent run up in dt value
dt = min(dt, 1.0f);
/// get_bearing_to_destination - get bearing to next waypoint in centi-degrees
int32_t AC_WPNav::get_bearing_to_destination()
{
return get_bearing_cd(_inav->get_position(), _destination);
}
// advance the target if necessary
if( _state == WPNAV_STATE_WPNAV ) {
/// update_wpnav - run the wp controller - should be called at 10hz
void AC_WPNav::update_wpnav()
{
uint32_t now = hal.scheduler->millis();
float dt = now - _last_update;
_last_update = now;
// catch if we've just been started
if( dt >= 1.0 ) {
dt = 0.0;
reset_I();
}else{
// advance the target if necessary
advance_target_along_track(_speed_cms, dt);
}
@ -169,6 +202,10 @@ void AC_WPNav::update()
get_loiter_pos_lat_lon(_target.x, _target.y, dt);
}
///
/// shared methods
///
// get_loiter_pos_lat_lon - loiter position controller
// converts desired position provided as distance from home in lat/lon directions to desired velocity
void AC_WPNav::get_loiter_pos_lat_lon(int32_t target_lat_from_home, int32_t target_lon_from_home, float dt)
@ -191,6 +228,7 @@ void AC_WPNav::get_loiter_pos_lat_lon(int32_t target_lat_from_home, int32_t targ
dist_error_lon = target_lon_from_home - _inav->get_longitude_diff();
linear_distance = MAX_LOITER_POS_ACCEL/(2*_pid_pos_lat->kP()*_pid_pos_lat->kP());
_distance_to_target = linear_distance; // for reporting purposes
dist_error_total = safe_sqrt(dist_error_lat*dist_error_lat + dist_error_lon*dist_error_lon);
if( dist_error_total > 2*linear_distance ) {
@ -261,6 +299,8 @@ void AC_WPNav::get_loiter_accel_lat_lon(int16_t accel_lat, int16_t accel_lon)
float accel_forward;
float accel_right;
// To-Do: add 1hz filter to accel_lat, accel_lon
// rotate accelerations into body forward-right frame
accel_forward = accel_lat*_cos_yaw + accel_lon*_sin_yaw;
accel_right = -accel_lat*_sin_yaw + accel_lon*_cos_yaw;
@ -269,3 +309,23 @@ void AC_WPNav::get_loiter_accel_lat_lon(int16_t accel_lat, int16_t accel_lon)
_desired_roll = constrain((accel_right/(-z_accel_meas))*(18000/M_PI), -4500, 4500);
_desired_pitch = constrain((-accel_forward/(-z_accel_meas*_cos_roll))*(18000/M_PI), -4500, 4500);
}
// 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 bearing = 9000 + atan2f(-(destination.x-origin.x), destination.y-origin.y) * 5729.57795f;
if (bearing < 0) {
bearing += 36000;
}
return bearing;
}
/// reset_I - clears I terms from loiter PID controller
void AC_WPNav::reset_I()
{
_pid_pos_lon->reset_I();
_pid_pos_lat->reset_I();
_pid_rate_lon->reset_I();
_pid_rate_lat->reset_I();
}

61
libraries/AC_WPNav/AC_WPNav.h
View File

@ -20,11 +20,6 @@
#define WPINAV_MAX_POS_ERROR 2000.0f // maximum distance (in cm) that the desired track can stray from our current location.
#define WP_SPEED 500 // default horizontal speed betwen waypoints in cm/s
// possible states
#define WPNAV_STATE_INACTIVE 0
#define WPNAV_STATE_LOITER 1
#define WPNAV_STATE_WPNAV 2
class AC_WPNav
{
public:
@ -36,32 +31,50 @@ public:
/// simple loiter controller
///
/// get_loiter_target - get loiter target as position vector (from home in cm)
Vector3f get_loiter_target() { return _target; }
/// set_loiter_target in cm from home
void set_loiter_target(const Vector3f& position);
void set_loiter_target(const Vector3f& position) { _target = position; }
/// move_loiter_target - move destination using forward and right velocities in cm/s
void move_loiter_target(int16_t vel_forward_cms, int16_t vel_right_cms, float dt);
/// get_distance_to_target - get horizontal distance to loiter target in cm
float get_distance_to_target();
/// get_bearing_to_target - get bearing to loiter target in centi-degrees
int32_t get_bearing_to_target();
/// update_loiter - run the loiter controller - should be called at 10hz
void update_loiter();
///
/// waypoint navigation
///
/// set_destination with distance from home in cm
/// set_destination waypoint using position vector (distance from home in cm)
void set_destination(const Vector3f& destination);
/// set_origin_and_destination - set origin and destination using lat/lon coordinates
/// set_origin_and_destination - set origin and destination waypoints using position vectors (distance from home in cm)
void set_origin_and_destination(const Vector3f& origin, const Vector3f& destination);
/// advance_target_along_track - move target location along track from origin to destination
void advance_target_along_track(float velocity_cms, float dt);
/// get_distance_to_destination - get horizontal distance to destination in cm
float get_distance_to_destination();
/// get_bearing_to_destination - get bearing to next waypoint in centi-degrees
int32_t get_bearing_to_destination();
/// update_wp - update waypoint controller
void update_wpnav();
///
/// shared methods
///
/// update - run the loiter and wpnav controllers - should be called at 10hz
void update(void);
/// get desired roll, pitch and altitude which should be fed into stabilize controllers
int32_t get_desired_roll() { return _desired_roll; };
int32_t get_desired_pitch() { return _desired_pitch; };
@ -92,9 +105,11 @@ protected:
/// converts desired accelerations provided in lat/lon frame to roll/pitch angles
void get_loiter_accel_lat_lon(int16_t accel_lat, int16_t accel_lon);
/// waypoint controller
/// get_wpinav_pos - wpinav position controller with desired position held in wpinav_destination
void get_wpinav_pos(float dt);
/// get_bearing_cd - return bearing in centi-degrees between two positions
float get_bearing_cd(const Vector3f origin, const Vector3f destination);
/// reset_I - clears I terms from loiter PID controller
void reset_I();
// pointers to inertial nav library
AP_InertialNav* _inav;
@ -107,23 +122,31 @@ protected:
// parameters
AP_Float _speed_cms; // default horizontal speed in cm/s
uint8_t _state; // records whether we are loitering or navigating towards a waypoint
uint32_t _last_update; // time of last update call
float _cos_yaw; // short-cut to save on calcs required to convert roll-pitch frame to lat-lon frame
float _sin_yaw;
float _cos_roll;
// output from controller
int32_t _desired_roll;
int32_t _desired_pitch;
int32_t _desired_altitude;
int32_t _desired_roll; // fed to stabilize controllers at 50hz
int32_t _desired_pitch; // fed to stabilize controllers at 50hz
int32_t _desired_altitude; // fed to alt hold controller at 50hz
// internal variables
Vector3f _target; // loiter's target location in cm from home
Vector3f _origin; // starting point of trip to next waypoint in cm from home (equivalent to next_WP)
Vector3f _destination; // target destination in cm from home (equivalent to next_WP)
Vector3f _wpinav_target; // the intermediate target location in cm from home
Vector3f _pos_delta; // position difference between origin and destination
float _track_length; // distance in cm between origin and destination
float _track_desired; // our desired distance along the track in cm
float _distance_to_target; // distance to loiter target
// To-Do: add split of fast (100hz for accel->angle) and slow (10hz for navigation)
//float _desired_accel_fwd; // updated from loiter controller at 10hz, consumed by accel->angle controller at 50hz
//float _desired_accel_rgt;
/// update - run the loiter and wpnav controllers - should be called at 100hz
//void update_100hz(void);
/// update - run the loiter and wpnav controllers - should be called at 10hz
//void update_10hz(void);
};
#endif // AC_WPNAV_H