Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-10 18:08:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

Fixed Rate navigation

Browse Source 
git-svn-id: https://arducopter.googlecode.com/svn/trunk@2090 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jasonshort 2011-05-05 05:19:45 +00:00
parent e6c4595d3c
commit 3c003f6653
2 changed files with 11 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
ArduCopterMega/config.h
View File

@ -384,13 +384,13 @@
# define NAV_WP_P 4.0
#endif
#ifndef NAV_WP_I
# define NAV_WP_I 0.0 // leave 0
# define NAV_WP_I 0.15 // leave 0
#endif
#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
#ifndef NAV_WP_IMAX
# define NAV_WP_IMAX 30 // 20 degrees
# define NAV_WP_IMAX 20 // 20 degrees
#endif
//////////////////////////////////////////////////////////////////////////////

19
ArduCopterMega/navigation.pde
View File

@ -151,26 +151,23 @@ void calc_rate_nav()
// called after we get GPS read
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?
long target_error = target_bearing - g_gps->ground_course;
target_error = wrap_180(target_error);
// 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
//long nav_lat = g.pid_nav_wp.kP() * error;
//int dampening = g.pid_nav_wp.kD() * (g_gps->ground_speed - last_ground_speed);
long nav_lat = NAV_WP_P * error;
int dampening = NAV_WP_D * (g_gps->ground_speed - last_ground_speed);
long nav_lat = g.pid_nav_wp.kP() * error;
int dampening = g.pid_nav_wp.kD() * (groundspeed - last_ground_speed);
// remember our old speed
last_ground_speed = g_gps->ground_speed;
last_ground_speed = groundspeed;
// dampen our response
nav_lat -= constrain(dampening, -1800, 1800); // +- max error