mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-25 10:08:28 -04:00
Fixed Rate navigation
git-svn-id: https://arducopter.googlecode.com/svn/trunk@2090 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
parent
e6c4595d3c
commit
3c003f6653
@ -384,13 +384,13 @@
|
|||||||
# define NAV_WP_P 4.0
|
# define NAV_WP_P 4.0
|
||||||
#endif
|
#endif
|
||||||
#ifndef NAV_WP_I
|
#ifndef NAV_WP_I
|
||||||
# define NAV_WP_I 0.0 // leave 0
|
# define NAV_WP_I 0.15 // leave 0
|
||||||
#endif
|
#endif
|
||||||
#ifndef NAV_WP_D
|
#ifndef NAV_WP_D
|
||||||
# define NAV_WP_D 0 // not sure about at all
|
# define NAV_WP_D 10 // not sure about at all
|
||||||
#endif
|
#endif
|
||||||
#ifndef NAV_WP_IMAX
|
#ifndef NAV_WP_IMAX
|
||||||
# define NAV_WP_IMAX 30 // 20 degrees
|
# define NAV_WP_IMAX 20 // 20 degrees
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
//////////////////////////////////////////////////////////////////////////////
|
//////////////////////////////////////////////////////////////////////////////
|
||||||
|
@ -151,26 +151,23 @@ void calc_rate_nav()
|
|||||||
// called after we get GPS read
|
// called after we get GPS read
|
||||||
void calc_rate_nav()
|
void calc_rate_nav()
|
||||||
{
|
{
|
||||||
// change to rate error
|
|
||||||
// we want to be going 450cm/s
|
|
||||||
int error = WAYPOINT_SPEED - g_gps->ground_speed;
|
|
||||||
//int error = 450
|
|
||||||
|
|
||||||
// which direction are we moving?
|
// which direction are we moving?
|
||||||
long target_error = target_bearing - g_gps->ground_course;
|
long target_error = target_bearing - g_gps->ground_course;
|
||||||
target_error = wrap_180(target_error);
|
target_error = wrap_180(target_error);
|
||||||
|
|
||||||
// calc the cos of the error to tell how fast we are moving towards the target
|
// calc the cos of the error to tell how fast we are moving towards the target
|
||||||
//error = (float)error * cos(radians((float)target_error/100));
|
int groundspeed = (float)g_gps->ground_speed * cos(radians((float)target_error/100));
|
||||||
|
|
||||||
|
// change to rate error
|
||||||
|
// we want to be going 450cm/s
|
||||||
|
int error = WAYPOINT_SPEED - groundspeed;
|
||||||
|
|
||||||
// Scale response by kP
|
// Scale response by kP
|
||||||
//long nav_lat = g.pid_nav_wp.kP() * error;
|
long nav_lat = g.pid_nav_wp.kP() * error;
|
||||||
//int dampening = g.pid_nav_wp.kD() * (g_gps->ground_speed - last_ground_speed);
|
int dampening = g.pid_nav_wp.kD() * (groundspeed - last_ground_speed);
|
||||||
long nav_lat = NAV_WP_P * error;
|
|
||||||
int dampening = NAV_WP_D * (g_gps->ground_speed - last_ground_speed);
|
|
||||||
|
|
||||||
// remember our old speed
|
// remember our old speed
|
||||||
last_ground_speed = g_gps->ground_speed;
|
last_ground_speed = groundspeed;
|
||||||
|
|
||||||
// dampen our response
|
// dampen our response
|
||||||
nav_lat -= constrain(dampening, -1800, 1800); // +- max error
|
nav_lat -= constrain(dampening, -1800, 1800); // +- max error
|
||||||
|
Loading…
Reference in New Issue
Block a user