diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp index 7cecc7e3f0..bb11e95020 100644 --- a/libraries/AC_WPNav/AC_WPNav.cpp +++ b/libraries/AC_WPNav/AC_WPNav.cpp @@ -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(); +} \ No newline at end of file diff --git a/libraries/AC_WPNav/AC_WPNav.h b/libraries/AC_WPNav/AC_WPNav.h index 7bbc341eba..3647cd8e9e 100644 --- a/libraries/AC_WPNav/AC_WPNav.h +++ b/libraries/AC_WPNav/AC_WPNav.h @@ -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