aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions 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_vel_scaler(0.0),
_spline_slow_down_dist(0.0),
_spline_yaw(0.0)
_yaw(0.0)
{
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_wp_leash_length();
// initialise yaw heading
_yaw = get_bearing_cd(_origin, _destination);
// initialise intermediate point to the origin
_pos_control.set_pos_target(origin);
_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
_spline_yaw = _ahrs->yaw_sensor;
_yaw = _ahrs->yaw_sensor;
// 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
@ -701,7 +704,7 @@ void AC_WPNav::advance_spline_target_along_track(float dt)
_pos_control.set_pos_target(target_pos);
// 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
_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
// 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
float get_spline_yaw() { return _spline_yaw; }
// get_yaw - returns target yaw in centi-degrees (used for wp and spline navigation)
float get_yaw() { return _yaw; }
/// 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
@ -261,6 +261,6 @@ protected:
float _spline_vel_scaler; //
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
float _spline_yaw; // heading according to yaw
float _yaw; // heading according to yaw
};
#endif // AC_WPNAV_H