Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-23 08:13:56 -04:00
Code Issues Packages Projects Releases Wiki Activity

WPNav: add yaw control for straight line wp nav

Browse Source
This commit is contained in:
Randy Mackay 2014-03-23 20:33:27 +09:00
parent 3753550a72
commit 24eb195aa3
2 changed files with 9 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
libraries/AC_WPNav/AC_WPNav.cpp
View File

@ -87,7 +87,7 @@ AC_WPNav::AC_WPNav(const AP_InertialNav* inav, const AP_AHRS* ahrs, AC_PosContro
_spline_time(0.0), _spline_time(0.0),
_spline_vel_scaler(0.0), _spline_vel_scaler(0.0),
_spline_slow_down_dist(0.0), _spline_slow_down_dist(0.0),
_spline_yaw(0.0) _yaw(0.0)
{ {
AP_Param::setup_object_defaults(this, var_info); AP_Param::setup_object_defaults(this, var_info);
} }
@ -291,6 +291,9 @@ void AC_WPNav::set_wp_origin_and_destination(const Vector3f& origin, const Vecto
// calculate leash lengths // calculate leash lengths
calculate_wp_leash_length(); calculate_wp_leash_length();
// initialise yaw heading
_yaw = get_bearing_cd(_origin, _destination);
// initialise intermediate point to the origin // initialise intermediate point to the origin
_pos_control.set_pos_target(origin); _pos_control.set_pos_target(origin);
_track_desired = 0; // target is at beginning of track _track_desired = 0; // target is at beginning of track
@ -595,7 +598,7 @@ void AC_WPNav::set_spline_origin_and_destination(const Vector3f& origin, const V
} }
// initialise yaw heading to current heading // initialise yaw heading to current heading
_spline_yaw = _ahrs->yaw_sensor; _yaw = _ahrs->yaw_sensor;
// To-Do: handle case where this is a straight segment? // To-Do: handle case where this is a straight segment?
// if this is a straight segment, origin velocity is distance vector from origin to destination // if this is a straight segment, origin velocity is distance vector from origin to destination
@ -701,7 +704,7 @@ void AC_WPNav::advance_spline_target_along_track(float dt)
_pos_control.set_pos_target(target_pos); _pos_control.set_pos_target(target_pos);
// update the yaw // update the yaw
_spline_yaw = RadiansToCentiDegrees(atan2(target_vel.y,target_vel.x)); _yaw = RadiansToCentiDegrees(atan2(target_vel.y,target_vel.x));
// advance spline time to next step // advance spline time to next step
_spline_time += spline_time_scale*0.1f; _spline_time += spline_time_scale*0.1f;

6
libraries/AC_WPNav/AC_WPNav.h
View File

@ -146,8 +146,8 @@ public:
// straight-fast - next segment is straight, vehicle's destination velocity should be directly along track from this segment's destination to next segment's destination // straight-fast - next segment is straight, vehicle's destination velocity should be directly along track from this segment's destination to next segment's destination
// spline-fast - next segment is spline, vehicle's destination velocity should be parallel to position difference vector from previous segment's origin to this segment's destination // spline-fast - next segment is spline, vehicle's destination velocity should be parallel to position difference vector from previous segment's origin to this segment's destination
// get_spline_yaw - returns target yaw in centi-degrees // get_yaw - returns target yaw in centi-degrees (used for wp and spline navigation)
float get_spline_yaw() { return _spline_yaw; } float get_yaw() { return _yaw; }
/// set_spline_destination waypoint using position vector (distance from home in cm) /// set_spline_destination waypoint using position vector (distance from home in cm)
/// stopped_at_start should be set to true if vehicle is stopped at the origin /// stopped_at_start should be set to true if vehicle is stopped at the origin
@ -261,6 +261,6 @@ protected:
float _spline_vel_scaler; // float _spline_vel_scaler; //
float _spline_slow_down_dist; // vehicle should begin to slow down once it is within this distance from the destination float _spline_slow_down_dist; // vehicle should begin to slow down once it is within this distance from the destination
// To-Do: this should be used for straight segments as well // To-Do: this should be used for straight segments as well
float _spline_yaw; // heading according to yaw float _yaw; // heading according to yaw
}; };
#endif // AC_WPNAV_H #endif // AC_WPNAV_H