mirror of https://github.com/ArduPilot/ardupilot
refinements to nav
git-svn-id: https://arducopter.googlecode.com/svn/trunk@1922 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
parent
6e6db9864e
commit
2ba41bb374
|
@ -17,6 +17,7 @@
|
||||||
#define STABILIZE_PITCH_P 0.75
|
#define STABILIZE_PITCH_P 0.75
|
||||||
//#define STABILIZE_DAMPENER 0.1
|
//#define STABILIZE_DAMPENER 0.1
|
||||||
|
|
||||||
|
#define NAV_TEST 0 // 0 = traditional, 1 = rate controlled
|
||||||
|
|
||||||
//#define CHANNEL_6_TUNING CH6_NONE
|
//#define CHANNEL_6_TUNING CH6_NONE
|
||||||
#define CHANNEL_6_TUNING CH6_PMAX
|
#define CHANNEL_6_TUNING CH6_PMAX
|
||||||
|
|
|
@ -899,9 +899,6 @@ void update_current_flight_mode(void)
|
||||||
nav_pitch = 0;
|
nav_pitch = 0;
|
||||||
nav_roll = 0;
|
nav_roll = 0;
|
||||||
|
|
||||||
// Output Pitch, Roll, Yaw and Throttle
|
|
||||||
// ------------------------------------
|
|
||||||
|
|
||||||
// Yaw control
|
// Yaw control
|
||||||
output_manual_yaw();
|
output_manual_yaw();
|
||||||
|
|
||||||
|
@ -1171,7 +1168,6 @@ void update_alt()
|
||||||
// read barometer
|
// read barometer
|
||||||
baro_alt = read_barometer();
|
baro_alt = read_barometer();
|
||||||
|
|
||||||
|
|
||||||
// XXX temp removed fr debugging
|
// XXX temp removed fr debugging
|
||||||
//filter out bad sonar reads
|
//filter out bad sonar reads
|
||||||
//int temp = sonar.read();
|
//int temp = sonar.read();
|
||||||
|
|
|
@ -108,11 +108,9 @@ void increment_WP_index()
|
||||||
{
|
{
|
||||||
if (g.waypoint_index < g.waypoint_total) {
|
if (g.waypoint_index < g.waypoint_total) {
|
||||||
g.waypoint_index.set_and_save(g.waypoint_index + 1);
|
g.waypoint_index.set_and_save(g.waypoint_index + 1);
|
||||||
SendDebug("MSG <increment_WP_index> WP index is incremented to ");
|
//SendDebug("MSG <increment_WP_index> WP index is incremented to ");
|
||||||
}else{
|
|
||||||
//SendDebug("MSG <increment_WP_index> Failed to increment WP index of ");
|
|
||||||
// This message is used excessively at the end of a mission
|
|
||||||
}
|
}
|
||||||
|
|
||||||
SendDebugln(g.waypoint_index,DEC);
|
SendDebugln(g.waypoint_index,DEC);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -106,11 +106,7 @@ void handle_no_commands()
|
||||||
{
|
{
|
||||||
// we don't want to RTL. Maybe this will change in the future. RTL is kinda dangerous.
|
// we don't want to RTL. Maybe this will change in the future. RTL is kinda dangerous.
|
||||||
// use landing commands
|
// use landing commands
|
||||||
return;
|
|
||||||
/*
|
/*
|
||||||
if (command_must_ID)
|
|
||||||
return;
|
|
||||||
|
|
||||||
switch (control_mode){
|
switch (control_mode){
|
||||||
default:
|
default:
|
||||||
//set_mode(RTL);
|
//set_mode(RTL);
|
||||||
|
@ -226,6 +222,7 @@ void do_takeoff()
|
||||||
{
|
{
|
||||||
wp_control = LOITER_MODE;
|
wp_control = LOITER_MODE;
|
||||||
|
|
||||||
|
// Start with current location
|
||||||
Location temp = current_loc;
|
Location temp = current_loc;
|
||||||
|
|
||||||
// next_command.alt is a relative altitude!!!
|
// next_command.alt is a relative altitude!!!
|
||||||
|
@ -233,9 +230,7 @@ void do_takeoff()
|
||||||
|
|
||||||
takeoff_complete = false; // set flag to use g_gps ground course during TO. IMU will be doing yaw drift correction
|
takeoff_complete = false; // set flag to use g_gps ground course during TO. IMU will be doing yaw drift correction
|
||||||
|
|
||||||
//Serial.print("dt ");
|
// Set our waypoint
|
||||||
//Serial.println(temp.alt, DEC);
|
|
||||||
|
|
||||||
set_next_WP(&temp);
|
set_next_WP(&temp);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -18,13 +18,16 @@ void update_commands(void)
|
||||||
next_command = get_command_with_index(g.waypoint_index + 1);
|
next_command = get_command_with_index(g.waypoint_index + 1);
|
||||||
|
|
||||||
if(next_command.id == NO_COMMAND){
|
if(next_command.id == NO_COMMAND){
|
||||||
|
|
||||||
// if no commands were available from EEPROM
|
// if no commands were available from EEPROM
|
||||||
// --------------------------------------------
|
// --------------------------------------------
|
||||||
|
if (command_must_ID == NO_COMMAND)
|
||||||
|
handle_no_commands();
|
||||||
|
|
||||||
handle_no_commands();
|
|
||||||
gcs.send_text_P(SEVERITY_LOW,PSTR("out of commands!"));
|
gcs.send_text_P(SEVERITY_LOW,PSTR("out of commands!"));
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
|
|
||||||
// A command was loaded from EEPROM
|
// A command was loaded from EEPROM
|
||||||
// --------------------------------------------
|
// --------------------------------------------
|
||||||
|
|
||||||
|
|
|
@ -368,33 +368,31 @@
|
||||||
// Navigation control gains
|
// Navigation control gains
|
||||||
//
|
//
|
||||||
#ifndef NAV_LOITER_P
|
#ifndef NAV_LOITER_P
|
||||||
# define NAV_LOITER_P 2.0
|
# define NAV_LOITER_P 2.5 // upped to be a bit more aggressive
|
||||||
#endif
|
#endif
|
||||||
#ifndef NAV_LOITER_I
|
#ifndef NAV_LOITER_I
|
||||||
# define NAV_LOITER_I 0.1
|
# define NAV_LOITER_I 0.15 // upped a bit to deal with wind faster
|
||||||
#endif
|
#endif
|
||||||
#ifndef NAV_LOITER_D
|
#ifndef NAV_LOITER_D
|
||||||
# define NAV_LOITER_D 0.00
|
# define NAV_LOITER_D 0.00
|
||||||
#endif
|
#endif
|
||||||
#ifndef NAV_LOITER_IMAX
|
#ifndef NAV_LOITER_IMAX
|
||||||
# define NAV_LOITER_IMAX 250 // 250 Lat and Longtitude
|
# define NAV_LOITER_IMAX 20 // 20°
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
|
||||||
#ifndef NAV_WP_P
|
#ifndef NAV_WP_P
|
||||||
# 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
|
# define NAV_WP_I 0.0 // leave 0
|
||||||
#endif
|
#endif
|
||||||
#ifndef NAV_WP_D
|
#ifndef NAV_WP_D
|
||||||
# define NAV_WP_D 15
|
# define NAV_WP_D 15 // not sure about at all
|
||||||
#endif
|
#endif
|
||||||
#ifndef NAV_WP_IMAX
|
#ifndef NAV_WP_IMAX
|
||||||
# define NAV_WP_IMAX 20 // 20 degrees
|
# define NAV_WP_IMAX 20 // 20 degrees
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
|
||||||
//////////////////////////////////////////////////////////////////////////////
|
//////////////////////////////////////////////////////////////////////////////
|
||||||
// Throttle control gains
|
// Throttle control gains
|
||||||
//
|
//
|
||||||
|
|
|
@ -112,6 +112,20 @@ void calc_simple_nav()
|
||||||
nav_lat *= g.pid_nav_lat.kP(); // 1800 * 2 = 3600 or 36°
|
nav_lat *= g.pid_nav_lat.kP(); // 1800 * 2 = 3600 or 36°
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void calc_nav_output()
|
||||||
|
{
|
||||||
|
// get the sin and cos of the bearing error - rotated 90°
|
||||||
|
sin_nav_y = sin(radians((float)(9000 - bearing_error) / 100));
|
||||||
|
cos_nav_x = cos(radians((float)(bearing_error - 9000) / 100));
|
||||||
|
|
||||||
|
// rotate the vector
|
||||||
|
nav_roll = (float)nav_lat * cos_nav_x;
|
||||||
|
nav_pitch = -(float)nav_lat * sin_nav_y;
|
||||||
|
}
|
||||||
|
#define WAYPOINT_SPEED 450
|
||||||
|
|
||||||
|
#if NAV_TEST == 0
|
||||||
|
|
||||||
void calc_rate_nav()
|
void calc_rate_nav()
|
||||||
{
|
{
|
||||||
// calc distance error
|
// calc distance error
|
||||||
|
@ -133,22 +147,10 @@ void calc_rate_nav()
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void calc_nav_output()
|
#else
|
||||||
{
|
|
||||||
// get the sin and cos of the bearing error - rotated 90°
|
|
||||||
sin_nav_y = sin(radians((float)(9000 - bearing_error) / 100));
|
|
||||||
cos_nav_x = cos(radians((float)(bearing_error - 9000) / 100));
|
|
||||||
|
|
||||||
// rotate the vector
|
|
||||||
nav_roll = (float)nav_lat * cos_nav_x;
|
|
||||||
nav_pitch = -(float)nav_lat * sin_nav_y;
|
|
||||||
}
|
|
||||||
|
|
||||||
#define WAYPOINT_SPEED 450
|
|
||||||
|
|
||||||
|
|
||||||
// called after we get GPS read
|
// called after we get GPS read
|
||||||
void calc_rate_nav2()
|
void calc_rate_nav()
|
||||||
{
|
{
|
||||||
// change to rate error
|
// change to rate error
|
||||||
// we want to be going 450cm/s
|
// we want to be going 450cm/s
|
||||||
|
@ -175,6 +177,7 @@ void calc_rate_nav2()
|
||||||
nav_lat = constrain(nav_lat, -3800, 3800); // +- 20m max error
|
nav_lat = constrain(nav_lat, -3800, 3800); // +- 20m max error
|
||||||
}
|
}
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
||||||
void calc_bearing_error()
|
void calc_bearing_error()
|
||||||
{
|
{
|
||||||
|
@ -220,7 +223,7 @@ void update_crosstrack(void)
|
||||||
{
|
{
|
||||||
// Crosstrack Error
|
// Crosstrack Error
|
||||||
// ----------------
|
// ----------------
|
||||||
if (cross_track_test() < 9000) { // If we are too far off or too close we don't do track following
|
if (cross_track_test() < 5000) { // If we are too far off or too close we don't do track following
|
||||||
crosstrack_error = sin(radians((target_bearing - crosstrack_bearing) / 100)) * wp_distance; // Meters we are off track line
|
crosstrack_error = sin(radians((target_bearing - crosstrack_bearing) / 100)) * wp_distance; // Meters we are off track line
|
||||||
nav_bearing += constrain(crosstrack_error * g.crosstrack_gain, -g.crosstrack_entry_angle.get(), g.crosstrack_entry_angle.get());
|
nav_bearing += constrain(crosstrack_error * g.crosstrack_gain, -g.crosstrack_entry_angle.get(), g.crosstrack_entry_angle.get());
|
||||||
nav_bearing = wrap_360(nav_bearing);
|
nav_bearing = wrap_360(nav_bearing);
|
||||||
|
|
Loading…
Reference in New Issue