Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-02 05:58:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

Navigation bug fixes, tuning.

Browse Source 
better handling of sonar at angles and baro mixing. - optimized for flats.
Crosstrack test bug found and fixed.

git-svn-id: https://arducopter.googlecode.com/svn/trunk@1916 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jasonshort 2011-04-21 23:07:31 +00:00
parent 0c61e9dd79
commit 9aaba3fd0f
10 changed files with 84 additions and 45 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
ArduCopterMega/APM_Config.h
View File

@ -11,15 +11,15 @@
//#define GCS_PROTOCOL GCS_PROTOCOL_NONE
//#define GCS_PORT 0
#define SERIAL0_BAUD 38400
//#define SERIAL0_BAUD 38400
#define STABILIZE_ROLL_P 0.75
#define STABILIZE_PITCH_P 0.75
//#define STABILIZE_DAMPENER 0.1
#define CHANNEL_6_TUNING CH6_NONE
//#define CHANNEL_6_TUNING CH6_STABLIZE_KP
//#define CHANNEL_6_TUNING CH6_NONE
#define CHANNEL_6_TUNING CH6_PMAX
/*
CH6_NONE

48
ArduCopterMega/ArduCopterMega.pde
View File

@ -244,7 +244,7 @@ boolean timer_light; // status of the Motor safety
const float t7 = 10000000.0; // used to scale GPS values for EEPROM storage
float scaleLongUp = 1; // used to reverse longtitude scaling
float scaleLongDown = 1; // used to reverse longtitude scaling
byte ground_start_count = 5; // have we achieved first lock and set Home?
byte ground_start_count = 10; // have we achieved first lock and set Home?
// Location & Navigation
// ---------------------
@ -308,8 +308,9 @@ int ground_temperature;
// Altitude Sensor variables
// ----------------------
long sonar_alt;
long baro_alt;
int sonar_alt;
int baro_alt;
int baro_alt_offset;
byte altitude_sensor = BARO; // used to know which sensor is active, BARO or SONAR
// flight mode specific
@ -1108,29 +1109,46 @@ void update_alt()
#if HIL_MODE == HIL_MODE_ATTITUDE
current_loc.alt = g_gps->altitude;
#else
altitude_sensor = BARO;
baro_alt = read_barometer();
//Serial.printf("b_alt: %ld, home: %ld ", baro_alt, home.alt);
if(g.sonar_enabled){
// decide which sensor we're usings
sonar_alt = sonar.read();
// read barometer
baro_alt = read_barometer();
if(baro_alt < 500 && sonar_alt < 600){ // less than 5m or 15 feet
//filter out bad sonar reads
int temp = sonar.read();
if(abs(temp - sonar_alt) < 300){
sonar_alt = temp;
}
// correct alt for angle of the sonar
sonar_alt = (float)sonar_alt * (cos_pitch_x * cos_roll_x);
// output a light to show sonar is working
update_sonar_light(sonar_alt > 100);
// decide which sensor we're usings
if(sonar_alt < 500){ // less than 5m or 15 feet
altitude_sensor = SONAR;
// XXX this is a hack for now. it kills accuracy from GPS reading of altitude and focuses
// on altitude above flat ground.
baro_alt_offset = sonar_alt - baro_alt;
}else{
altitude_sensor = BARO;
}
//altitude_sensor = (target_altitude > (home.alt + 500)) ? BARO : SONAR;
// calculate our altitude
if(altitude_sensor == BARO){
current_loc.alt = baro_alt + home.alt;
current_loc.alt = baro_alt + baro_alt_offset + home.alt;
}else{
current_loc.alt = sonar_alt + home.alt;
}
}else{
altitude_sensor = BARO;
baro_alt = read_barometer();
// no sonar altitude
current_loc.alt = baro_alt + home.alt;
}
@ -1197,5 +1215,9 @@ void tuning(){
#elif CHANNEL_6_TUNING == CH6_Y6_SCALING
Y6_scaling = (float)g.rc_6.control_in / 1000.0;
#elif CHANNEL_6_TUNING == CH6_PMAX
g.pitch_max.set(g.rc_6.control_in * 2); // 0 to 2000
#endif
}

15
ArduCopterMega/Attitude.pde
View File

@ -144,13 +144,13 @@ output_yaw_with_hold(boolean hold)
yaw_error = wrap_180(yaw_error);
// limit the error we're feeding to the PID
yaw_error = constrain(yaw_error, -6000, 6000); // limit error to 60 degees
yaw_error = constrain(yaw_error, -4000, 4000); // limit error to 60 degees
// Apply PID and save the new angle back to RC_Channel
g.rc_4.servo_out = g.pid_yaw.get_pid(yaw_error, delta_ms_fast_loop, 1.0); // .5 * 6000 = 3000
g.rc_4.servo_out = g.pid_yaw.get_pid(yaw_error, delta_ms_fast_loop, 1.0); // .4 * 4000 = 1600
// add in yaw dampener
g.rc_4.servo_out -= constrain(dampener, -2400, 2400);
g.rc_4.servo_out -= constrain(dampener, -1600, 1600);
yaw_debug = YAW_HOLD; //0
}else{
@ -239,15 +239,16 @@ void calc_nav_throttle()
float scaler = 1.0;
if(error < 0){
scaler = (altitude_sensor == BARO) ? .5 : .5;
// try and prevent rapid fall
scaler = (altitude_sensor == BARO) ? .3 : .3;
}
if(altitude_sensor == BARO){
nav_throttle = g.pid_baro_throttle.get_pid(error, delta_ms_fast_loop, scaler); // .25
nav_throttle = g.throttle_cruise + constrain(nav_throttle, -30, 80);
nav_throttle = g.throttle_cruise + constrain(nav_throttle, -30, 120);
}else{
nav_throttle = g.pid_sonar_throttle.get_pid(error, delta_ms_fast_loop, scaler); // .5
nav_throttle = g.throttle_cruise + constrain(nav_throttle, -60, 100);
nav_throttle = g.throttle_cruise + constrain(nav_throttle, -50, 150);
}
nav_throttle = (nav_throttle + nav_throttle_old) >> 1;
@ -259,7 +260,7 @@ void calc_nav_throttle()
float angle_boost()
{
float temp = cos_pitch_x * cos_roll_x;
temp = 2.0 - constrain(temp, .7, 1.0);
temp = 2.0 - constrain(temp, .5, 1.0);
return temp;
}

21
ArduCopterMega/Log.pde
View File

@ -20,6 +20,9 @@ static int8_t select_logs(uint8_t argc, const Menu::arg *argv);
// printf_P is a version of print_f that reads from flash memory
static int8_t help_log(uint8_t argc, const Menu::arg *argv)
{
// log hack
//Serial.begin(115200, 128, 128);
Serial.printf_P(PSTR("\n"
"Commands:\n"
" dump <n>"
@ -380,26 +383,28 @@ void Log_Write_Nav_Tuning()
DataFlash.WriteInt((int)(target_bearing/100)); // 3
DataFlash.WriteInt((int)(nav_bearing/100)); // 4
DataFlash.WriteByte(altitude_sensor); // 5
DataFlash.WriteInt((int)sonar_alt); // 6
DataFlash.WriteInt((int)baro_alt); // 7
DataFlash.WriteInt((int)(g.rc_3.servo_out)); // 5
DataFlash.WriteByte(altitude_sensor); // 6
DataFlash.WriteInt((int)sonar_alt); // 7
DataFlash.WriteInt((int)baro_alt); // 8
DataFlash.WriteInt(home.alt); // 8
DataFlash.WriteInt((int)next_WP.alt); // 9
DataFlash.WriteInt((int)altitude_error); // 10
DataFlash.WriteInt(home.alt); // 9
DataFlash.WriteInt((int)next_WP.alt); // 11
DataFlash.WriteInt((int)altitude_error); // 11
DataFlash.WriteByte(END_BYTE);
}
void Log_Read_Nav_Tuning()
{
// 1 2 3 4 5 6 7 8 9 10
Serial.printf_P(PSTR("NTUN, %d, %d, %d, %d, %d, %d, %d, %d, %d, %d\n"),
// 1 2 3 4 5 6 7 8 9 10 11
Serial.printf_P(PSTR("NTUN, %d, %d, %d, %d, %d, %d, %d, %d, %d, %d, %d\n"),
DataFlash.ReadInt(), //yaw sensor
DataFlash.ReadInt(), //distance
DataFlash.ReadInt(), //target bearing
DataFlash.ReadInt(), //nav bearing
DataFlash.ReadInt(), //throttle
DataFlash.ReadByte(), //Alt sensor
DataFlash.ReadInt(), //Sonar
DataFlash.ReadInt(), //Baro

3
ArduCopterMega/commands.pde
View File

@ -189,6 +189,7 @@ void set_next_WP(struct Location *wp)
wp_totalDistance = get_distance(&current_loc, &next_WP);
wp_distance = wp_totalDistance;
target_bearing = get_bearing(&current_loc, &next_WP);
nav_bearing = target_bearing;
// to check if we have missed the WP
// ----------------------------
@ -196,7 +197,7 @@ void set_next_WP(struct Location *wp)
// set a new crosstrack bearing
// ----------------------------
reset_crosstrack();
crosstrack_bearing = target_bearing; // Used for track following
gcs.print_current_waypoints();
}

20
ArduCopterMega/config.h
View File

@ -343,13 +343,13 @@
// YAW Control
//
#ifndef YAW_P
# define YAW_P 0.5 // increase for more aggressive Yaw Hold, decrease if it's bouncy
# define YAW_P 0.4 // increase for more aggressive Yaw Hold, decrease if it's bouncy
#endif
#ifndef YAW_I
# define YAW_I 0.0 // Always 0
#endif
#ifndef YAW_D
# define YAW_D 0.05 // Trying a lower value to prevent odd behavior
# define YAW_D 0.13 // Trying a lower value to prevent odd behavior
#endif
#ifndef YAW_IMAX
# define YAW_IMAX 0 // Always 0
@ -385,13 +385,13 @@
// Throttle control gains
//
#ifndef THROTTLE_BARO_P
# define THROTTLE_BARO_P 0.25
# define THROTTLE_BARO_P 0.4
#endif
#ifndef THROTTLE_BARO_I
# define THROTTLE_BARO_I 0.04
# define THROTTLE_BARO_I 0.1
#endif
#ifndef THROTTLE_BARO_D
# define THROTTLE_BARO_D 0.0 // lowered to 0 to debug effects
# define THROTTLE_BARO_D 0.03 // lowered to 0 to debug effects
#endif
#ifndef THROTTLE_BARO_IMAX
# define THROTTLE_BARO_IMAX 50
@ -399,13 +399,13 @@
#ifndef THROTTLE_SONAR_P
# define THROTTLE_SONAR_P .65 // upped a hair from .5
# define THROTTLE_SONAR_P .8 // upped a hair from .5
#endif
#ifndef THROTTLE_SONAR_I
# define THROTTLE_SONAR_I 0.1
# define THROTTLE_SONAR_I 0.4
#endif
#ifndef THROTTLE_SONAR_D
# define THROTTLE_SONAR_D 0.03
# define THROTTLE_SONAR_D 0.15
#endif
#ifndef THROTTLE_SONAR_IMAX
# define THROTTLE_SONAR_IMAX 50
@ -416,10 +416,10 @@
// Crosstrack compensation
//
#ifndef XTRACK_GAIN
# define XTRACK_GAIN 1 // deg/m
# define XTRACK_GAIN 5 // deg/m
#endif
#ifndef XTRACK_ENTRY_ANGLE
# define XTRACK_ENTRY_ANGLE 30 // deg
# define XTRACK_ENTRY_ANGLE 40 // deg
#endif

1
ArduCopterMega/defines.h
View File

@ -111,6 +111,7 @@
#define CH6_SONAR_KP 5
#define CH6_SONAR_KD 6
#define CH6_Y6_SCALING 7
#define CH6_PMAX 8
// nav byte mask
// -------------

11
ArduCopterMega/navigation.pde
View File

@ -199,13 +199,22 @@ void update_crosstrack(void)
{
// Crosstrack Error
// ----------------
if (abs(target_bearing - crosstrack_bearing) < 4500) { // If we are too far off or too close we don't do track following
if (cross_track_test() < 9000) { // 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
nav_bearing += constrain(crosstrack_error * g.crosstrack_gain, -g.crosstrack_entry_angle.get(), g.crosstrack_entry_angle.get());
nav_bearing = wrap_360(nav_bearing);
}else{
reset_crosstrack();
}
}
long cross_track_test()
{
long temp = target_bearing - crosstrack_bearing;
temp = wrap_180(temp);
return abs(temp);
}
void reset_crosstrack()
{
crosstrack_bearing = get_bearing(&current_loc, &next_WP); // Used for track following

2
ArduCopterMega/setup.pde
View File

@ -42,7 +42,7 @@ int8_t
setup_mode(uint8_t argc, const Menu::arg *argv)
{
// Give the user some guidance
Serial.printf_P(PSTR("Setup Mode\n"));
Serial.printf_P(PSTR("Setup Mode\n\n\n"));
//"\n"
//"IMPORTANT: if you have not previously set this system up, use the\n"
//"'reset' command to initialize the EEPROM to sensible default values\n"

2
ArduCopterMega/system.pde
View File

@ -419,7 +419,7 @@ void update_motor_light(void)
}
}
void update_timer_light(bool light)
void update_sonar_light(bool light)
{
if(light == true){
digitalWrite(B_LED_PIN, HIGH);