Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'master' of https://code.google.com/p/ardupilot-mega

This commit is contained in:
Chris Anderson 2012-01-31 07:13:34 -08:00
parent c1c820de72 4402a92e85
commit 6dcba35c45
8 changed files with 78 additions and 65 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
ArduCopter/ArduCopter.pde
View File

@ -30,6 +30,8 @@ Jack Dunkle :Alpha testing
Christof Schmid :Alpha testing
Oliver :Piezo support
Guntars :Arming safety suggestion
Igor van Airde :Control Law optimization
Jean-Louis Naudin :Auto Landing
And much more so PLEASE PM me on DIYDRONES to add your contribution to the List
@ -1919,8 +1921,8 @@ static void tuning(){
case CH6_STABILIZE_KP:
g.rc_6.set_range(0,8000); // 0 to 8
g.pid_rate_roll.kP(tuning_value);
g.pid_rate_pitch.kP(tuning_value);
g.pi_stabilize_roll.kP(tuning_value);
g.pi_stabilize_pitch.kP(tuning_value);
break;
case CH6_STABILIZE_KI:
@ -1980,11 +1982,17 @@ static void tuning(){
break;
case CH6_NAV_P:
g.rc_6.set_range(0,6000);
g.rc_6.set_range(0,4000);
g.pid_nav_lat.kP(tuning_value);
g.pid_nav_lon.kP(tuning_value);
break;
case CH6_NAV_I:
g.rc_6.set_range(0,500);
g.pid_nav_lat.kI(tuning_value);
g.pid_nav_lon.kI(tuning_value);
break;
#if FRAME_CONFIG == HELI_FRAME
case CH6_HELI_EXTERNAL_GYRO:
g.rc_6.set_range(1000,2000);

10
ArduCopter/Attitude.pde
View File

@ -130,7 +130,6 @@ get_rate_yaw(int32_t target_rate)
return constrain(target_rate, -yaw_limit, yaw_limit);
}
#define ALT_ERROR_MAX 400
static int16_t
get_nav_throttle(int32_t z_error)
{
@ -138,11 +137,12 @@ get_nav_throttle(int32_t z_error)
int16_t rate_error = 0;
int16_t output = 0;
// limit error to prevent I term run up
z_error = constrain(z_error, -ALT_ERROR_MAX, ALT_ERROR_MAX);
// convert to desired Rate:
rate_error = g.pi_alt_hold.get_p(z_error); //_p = .85
rate_error = g.pi_alt_hold.get_p(z_error);
rate_error = constrain(rate_error, -100, 100);
// limit error to prevent I term wind up
z_error = constrain(z_error, -400, 400);
// compensates throttle setpoint error for hovering
int16_t iterm = g.pi_alt_hold.get_i(z_error, .1);

46
ArduCopter/Log.pde
View File

@ -535,8 +535,8 @@ static void Log_Write_Nav_Tuning()
DataFlash.WriteInt(nav_lat); // 6
DataFlash.WriteInt(x_actual_speed); // 7
DataFlash.WriteInt(y_actual_speed); // 8
DataFlash.WriteInt(g.pi_loiter_lon.get_integrator()); // 9
DataFlash.WriteInt(g.pi_loiter_lat.get_integrator()); // 10
DataFlash.WriteInt(g.pid_nav_lon.get_integrator()); // 9
DataFlash.WriteInt(g.pid_nav_lat.get_integrator()); // 10
/*DataFlash.WriteInt(wp_distance); // 1
DataFlash.WriteInt(nav_bearing/100); // 2
@ -577,20 +577,17 @@ static void Log_Write_Control_Tuning()
DataFlash.WriteByte(HEAD_BYTE2);
DataFlash.WriteByte(LOG_CONTROL_TUNING_MSG);
DataFlash.WriteInt(g.rc_1.control_in); // 0
DataFlash.WriteInt(g.rc_2.control_in); // 1
DataFlash.WriteInt(g.rc_3.control_in); // 2
DataFlash.WriteInt(g.rc_4.control_in); // 3
DataFlash.WriteInt(sonar_alt); // 4
DataFlash.WriteInt(baro_alt); // 5
DataFlash.WriteInt(next_WP.alt); // 6
DataFlash.WriteInt(nav_throttle); // 7
DataFlash.WriteInt(angle_boost); // 8
DataFlash.WriteInt(manual_boost); // 9
DataFlash.WriteInt(climb_rate); // 10
DataFlash.WriteInt(g.rc_3.servo_out); // 11
DataFlash.WriteInt(g.pi_alt_hold.get_integrator()); // 12
DataFlash.WriteInt(g.pid_throttle.get_integrator()); // 13
DataFlash.WriteInt(g.rc_3.control_in); // 1
DataFlash.WriteInt(sonar_alt); // 2
DataFlash.WriteInt(baro_alt); // 3
DataFlash.WriteInt(next_WP.alt); // 4
DataFlash.WriteInt(nav_throttle); // 5
DataFlash.WriteInt(angle_boost); // 6
DataFlash.WriteInt(manual_boost); // 7
DataFlash.WriteInt(climb_rate); // 8
DataFlash.WriteInt(g.rc_3.servo_out); // 9
DataFlash.WriteInt(g.pi_alt_hold.get_integrator()); // 10
DataFlash.WriteInt(g.pid_throttle.get_integrator());// 11
DataFlash.WriteByte(END_BYTE);
}
@ -602,7 +599,7 @@ static void Log_Read_Control_Tuning()
Serial.printf_P(PSTR("CTUN, "));
for(int8_t i = 0; i < 13; i++ ){
for(int8_t i = 0; i < 11; i++ ){
temp = DataFlash.ReadInt();
Serial.printf("%d, ", temp);
}
@ -696,13 +693,12 @@ static void Log_Write_Attitude()
DataFlash.WriteByte(HEAD_BYTE2);
DataFlash.WriteByte(LOG_ATTITUDE_MSG);
DataFlash.WriteInt(g.rc_1.control_in); // 0
DataFlash.WriteInt((int)dcm.roll_sensor); // 1
DataFlash.WriteInt((int)dcm.pitch_sensor); // 2
DataFlash.WriteInt((uint16_t)dcm.yaw_sensor); // 3
DataFlash.WriteInt((int)g.rc_1.servo_out); // 4
DataFlash.WriteInt((int)g.rc_2.servo_out); // 5
DataFlash.WriteInt((int)g.rc_4.servo_out); // 6
DataFlash.WriteInt(g.rc_2.control_in); // 2
DataFlash.WriteInt((int)dcm.pitch_sensor); // 3
DataFlash.WriteInt(g.rc_4.control_in); // 4
DataFlash.WriteInt((uint16_t)dcm.yaw_sensor); // 5
DataFlash.WriteByte(END_BYTE);
}
@ -712,10 +708,10 @@ static void Log_Read_Attitude()
{
int16_t temp1 = DataFlash.ReadInt();
int16_t temp2 = DataFlash.ReadInt();
uint16_t temp3 = DataFlash.ReadInt();
int16_t temp3 = DataFlash.ReadInt();
int16_t temp4 = DataFlash.ReadInt();
int16_t temp5 = DataFlash.ReadInt();
int16_t temp6 = DataFlash.ReadInt();
uint16_t temp6 = DataFlash.ReadInt();
// 1 2 3 4 5 6
Serial.printf_P(PSTR("ATT, %d, %d, %u, %d, %d, %d\n"),

2
ArduCopter/Parameters.h
View File

@ -413,7 +413,6 @@ public:
pid_optflow_roll (k_param_pid_optflow_roll, PSTR("OF_RLL_"), OPTFLOW_ROLL_P, OPTFLOW_ROLL_I, OPTFLOW_ROLL_D, OPTFLOW_IMAX * 100),
pid_optflow_pitch (k_param_pid_optflow_pitch, PSTR("OF_PIT_"), OPTFLOW_PITCH_P, OPTFLOW_PITCH_I, OPTFLOW_PITCH_D,OPTFLOW_IMAX * 100),
// PI controller group key name initial P initial I initial imax
//--------------------------------------------------------------------------------------------------------------------------------------------------------------------
pi_stabilize_roll (k_param_pi_stabilize_roll, PSTR("STB_RLL_"), STABILIZE_ROLL_P, STABILIZE_ROLL_I, STABILIZE_ROLL_IMAX * 100),
@ -424,7 +423,6 @@ public:
pi_loiter_lat (k_param_pi_loiter_lat, PSTR("HLD_LAT_"), LOITER_P, LOITER_I, LOITER_IMAX * 100),
pi_loiter_lon (k_param_pi_loiter_lon, PSTR("HLD_LON_"), LOITER_P, LOITER_I, LOITER_IMAX * 100),
junk(0) // XXX just so that we can add things without worrying about the trailing comma
{
}

32
ArduCopter/config.h
View File

@ -523,21 +523,21 @@
// Extra motor values that are changed from time to time by jani @ jDrones as software
// and charachteristics changes.
#ifdef MOTORS_JD880
# define STABILIZE_ROLL_P 3.6
# define STABILIZE_ROLL_P 3.7
# define STABILIZE_ROLL_I 0.0
# define STABILIZE_ROLL_IMAX 40.0 // degrees
# define STABILIZE_PITCH_P 3.6
# define STABILIZE_ROLL_IMAX 40.0 // degrees
# define STABILIZE_PITCH_P 3.7
# define STABILIZE_PITCH_I 0.0
# define STABILIZE_PITCH_IMAX 40.0 // degrees
# define STABILIZE_PITCH_IMAX 40.0 // degrees
#endif
#ifdef MOTORS_JD850
# define STABILIZE_ROLL_P 4.0
# define STABILIZE_ROLL_P 4.2
# define STABILIZE_ROLL_I 0.0
# define STABILIZE_ROLL_IMAX 40.0 // degrees
# define STABILIZE_PITCH_P 4.0
# define STABILIZE_ROLL_IMAX 40.0 // degrees
# define STABILIZE_PITCH_P 4.2
# define STABILIZE_PITCH_I 0.0
# define STABILIZE_PITCH_IMAX 40.0 // degrees
# define STABILIZE_PITCH_IMAX 40.0 // degrees
#endif
@ -587,7 +587,7 @@
# define RATE_ROLL_I 0.0
#endif
#ifndef RATE_ROLL_D
# define RATE_ROLL_D 0
# define RATE_ROLL_D 0.0
#endif
#ifndef RATE_ROLL_IMAX
# define RATE_ROLL_IMAX 15 // degrees
@ -597,10 +597,10 @@
# define RATE_PITCH_P 0.14
#endif
#ifndef RATE_PITCH_I
# define RATE_PITCH_I 0 // 0.18
# define RATE_PITCH_I 0.0 // 0.18
#endif
#ifndef RATE_PITCH_D
# define RATE_PITCH_D 0 // 0.002
# define RATE_PITCH_D 0.0 // 0.002
#endif
#ifndef RATE_PITCH_IMAX
# define RATE_PITCH_IMAX 15 // degrees
@ -624,10 +624,10 @@
// Loiter control gains
//
#ifndef LOITER_P
# define LOITER_P .4 // was .25 in previous
# define LOITER_P .2 // was .25 in previous
#endif
#ifndef LOITER_I
# define LOITER_I 0.2 // Wind control
# define LOITER_I 0.0
#endif
#ifndef LOITER_IMAX
# define LOITER_IMAX 30 // degrees
@ -640,10 +640,10 @@
# define NAV_P 2.3 // 3 was causing rapid oscillations in Loiter
#endif
#ifndef NAV_I
# define NAV_I 0 //
# define NAV_I 0.4 // Wind control
#endif
#ifndef NAV_D
# define NAV_D 0.015 //
# define NAV_D 0.00 //
#endif
#ifndef NAV_IMAX
# define NAV_IMAX 30 // degrees
@ -767,7 +767,7 @@
#endif
// guess!
#ifndef LOG_OPTFLOW
# define LOG_OPTFLOW DISABLED
# define LOG_OPTFLOW DISABLED
#endif
// calculate the default log_bitmask

2
ArduCopter/control_modes.pde
View File

@ -77,7 +77,7 @@ static void read_trim_switch()
adc.filter_result = false;
}
#elif CH7_OPTION == CH7_AUTO_TRIM
#elif CH7_OPTION == CH7_AUTO_TRIM
if (g.rc_7.control_in > 800){
auto_level_counter = 10;
}

2
ArduCopter/defines.h
View File

@ -166,6 +166,8 @@
#define CH6_OPTFLOW_KI 18
#define CH6_OPTFLOW_KD 19
#define CH6_NAV_I 20
// nav byte mask
// -------------

35
ArduCopter/navigation.pde
View File

@ -44,8 +44,8 @@ static void calc_XY_velocity(){
static int32_t last_longitude = 0;
static int32_t last_latitude = 0;
//static int16_t x_speed_old = 0;
//static int16_t y_speed_old = 0;
static int16_t x_speed_old = 0;
static int16_t y_speed_old = 0;
// y_GPS_speed positve = Up
// x_GPS_speed positve = Right
@ -55,20 +55,22 @@ static void calc_XY_velocity(){
// straightforward approach:
///*
int16_t x_estimate = (float)(g_gps->longitude - last_longitude) * tmp;
int16_t y_estimate = (float)(g_gps->latitude - last_latitude) * tmp;
x_actual_speed = x_speed_old + (float)(g_gps->longitude - last_longitude) * tmp;
y_actual_speed = y_speed_old + (float)(g_gps->latitude - last_latitude) * tmp;
// slight averaging filter
x_actual_speed = (x_actual_speed + x_estimate) >> 1;
y_actual_speed = (y_actual_speed + y_estimate) >> 1;
x_actual_speed = x_actual_speed >> 1;
y_actual_speed = y_actual_speed >> 1;
x_speed_old = x_actual_speed;
y_speed_old = y_actual_speed;
/*
// Ryan Beall's forward estimator:
int16_t x_speed_new = (float)(g_gps->longitude - last_longitude) * tmp;
int16_t y_speed_new = (float)(g_gps->latitude - last_latitude) * tmp;
x_GPS_speed = x_speed_new + (x_speed_new - x_speed_old);
y_GPS_speed = y_speed_new + (y_speed_new - y_speed_old);
x_actual_speed = x_speed_new + (x_speed_new - x_speed_old);
y_actual_speed = y_speed_new + (y_speed_new - y_speed_old);
x_speed_old = x_speed_new;
y_speed_old = y_speed_new;
@ -96,18 +98,22 @@ static void calc_location_error(struct Location *next_loc)
lat_error = next_loc->lat - current_loc.lat; // 500 - 0 = 500 Go North
}
#define NAV_ERR_MAX 800
#define NAV_ERR_MAX 600
static void calc_loiter(int x_error, int y_error)
{
// East/West
x_error = constrain(x_error, -NAV_ERR_MAX, NAV_ERR_MAX); //800
int16_t x_target_speed = g.pi_loiter_lon.get_p(x_error);
x_target_speed = constrain(x_error, -150, 150);
// limit windup
x_error = constrain(x_error, -NAV_ERR_MAX, NAV_ERR_MAX);
int16_t x_iterm = g.pi_loiter_lon.get_i(x_error, dTnav);
x_rate_error = x_target_speed - x_actual_speed;
// North/South
y_error = constrain(y_error, -NAV_ERR_MAX, NAV_ERR_MAX);
int16_t y_target_speed = g.pi_loiter_lat.get_p(y_error);
y_target_speed = constrain(y_error, -150, 150);
// limit windup
y_error = constrain(y_error, -NAV_ERR_MAX, NAV_ERR_MAX);
int16_t y_iterm = g.pi_loiter_lat.get_i(y_error, dTnav);
y_rate_error = y_target_speed - y_actual_speed;
@ -212,7 +218,7 @@ static void calc_nav_lon(int rate)
static void calc_nav_lat(int rate)
{
nav_lat = g.pid_nav_lon.get_pid(rate, dTnav);
nav_lat = g.pid_nav_lat.get_pid(rate, dTnav);
nav_lat = get_corrected_angle(rate, nav_lat);
nav_lat = constrain(nav_lat, -3000, 3000);
}
@ -396,6 +402,9 @@ static int32_t get_new_altitude()
}
}
// we use the elapsed time as our altitude offset
// 1000 = 1 sec
// 1000 >> 4 = 64cm/s descent by default
int32_t change = (millis() - alt_change_timer) >> _scale;
if(alt_change_flag == ASCENDING){