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 Christof Schmid :Alpha testing
Oliver :Piezo support Oliver :Piezo support
Guntars :Arming safety suggestion 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 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: case CH6_STABILIZE_KP:
g.rc_6.set_range(0,8000); // 0 to 8 g.rc_6.set_range(0,8000); // 0 to 8
g.pid_rate_roll.kP(tuning_value); g.pi_stabilize_roll.kP(tuning_value);
g.pid_rate_pitch.kP(tuning_value); g.pi_stabilize_pitch.kP(tuning_value);
break; break;
case CH6_STABILIZE_KI: case CH6_STABILIZE_KI:
@ -1980,11 +1982,17 @@ static void tuning(){
break; break;
case CH6_NAV_P: 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_lat.kP(tuning_value);
g.pid_nav_lon.kP(tuning_value); g.pid_nav_lon.kP(tuning_value);
break; 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 #if FRAME_CONFIG == HELI_FRAME
case CH6_HELI_EXTERNAL_GYRO: case CH6_HELI_EXTERNAL_GYRO:
g.rc_6.set_range(1000,2000); 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); return constrain(target_rate, -yaw_limit, yaw_limit);
} }
#define ALT_ERROR_MAX 400
static int16_t static int16_t
get_nav_throttle(int32_t z_error) 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 rate_error = 0;
int16_t output = 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: // 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 // compensates throttle setpoint error for hovering
int16_t iterm = g.pi_alt_hold.get_i(z_error, .1); 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(nav_lat); // 6
DataFlash.WriteInt(x_actual_speed); // 7 DataFlash.WriteInt(x_actual_speed); // 7
DataFlash.WriteInt(y_actual_speed); // 8 DataFlash.WriteInt(y_actual_speed); // 8
DataFlash.WriteInt(g.pi_loiter_lon.get_integrator()); // 9 DataFlash.WriteInt(g.pid_nav_lon.get_integrator()); // 9
DataFlash.WriteInt(g.pi_loiter_lat.get_integrator()); // 10 DataFlash.WriteInt(g.pid_nav_lat.get_integrator()); // 10
/*DataFlash.WriteInt(wp_distance); // 1 /*DataFlash.WriteInt(wp_distance); // 1
DataFlash.WriteInt(nav_bearing/100); // 2 DataFlash.WriteInt(nav_bearing/100); // 2
@ -577,20 +577,17 @@ static void Log_Write_Control_Tuning()
DataFlash.WriteByte(HEAD_BYTE2); DataFlash.WriteByte(HEAD_BYTE2);
DataFlash.WriteByte(LOG_CONTROL_TUNING_MSG); DataFlash.WriteByte(LOG_CONTROL_TUNING_MSG);
DataFlash.WriteInt(g.rc_1.control_in); // 0 DataFlash.WriteInt(g.rc_3.control_in); // 1
DataFlash.WriteInt(g.rc_2.control_in); // 1 DataFlash.WriteInt(sonar_alt); // 2
DataFlash.WriteInt(g.rc_3.control_in); // 2 DataFlash.WriteInt(baro_alt); // 3
DataFlash.WriteInt(g.rc_4.control_in); // 3 DataFlash.WriteInt(next_WP.alt); // 4
DataFlash.WriteInt(sonar_alt); // 4 DataFlash.WriteInt(nav_throttle); // 5
DataFlash.WriteInt(baro_alt); // 5 DataFlash.WriteInt(angle_boost); // 6
DataFlash.WriteInt(next_WP.alt); // 6 DataFlash.WriteInt(manual_boost); // 7
DataFlash.WriteInt(nav_throttle); // 7 DataFlash.WriteInt(climb_rate); // 8
DataFlash.WriteInt(angle_boost); // 8 DataFlash.WriteInt(g.rc_3.servo_out); // 9
DataFlash.WriteInt(manual_boost); // 9 DataFlash.WriteInt(g.pi_alt_hold.get_integrator()); // 10
DataFlash.WriteInt(climb_rate); // 10 DataFlash.WriteInt(g.pid_throttle.get_integrator());// 11
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.WriteByte(END_BYTE); DataFlash.WriteByte(END_BYTE);
} }
@ -602,7 +599,7 @@ static void Log_Read_Control_Tuning()
Serial.printf_P(PSTR("CTUN, ")); Serial.printf_P(PSTR("CTUN, "));
for(int8_t i = 0; i < 13; i++ ){ for(int8_t i = 0; i < 11; i++ ){
temp = DataFlash.ReadInt(); temp = DataFlash.ReadInt();
Serial.printf("%d, ", temp); Serial.printf("%d, ", temp);
} }
@ -696,13 +693,12 @@ static void Log_Write_Attitude()
DataFlash.WriteByte(HEAD_BYTE2); DataFlash.WriteByte(HEAD_BYTE2);
DataFlash.WriteByte(LOG_ATTITUDE_MSG); DataFlash.WriteByte(LOG_ATTITUDE_MSG);
DataFlash.WriteInt(g.rc_1.control_in); // 0
DataFlash.WriteInt((int)dcm.roll_sensor); // 1 DataFlash.WriteInt((int)dcm.roll_sensor); // 1
DataFlash.WriteInt((int)dcm.pitch_sensor); // 2 DataFlash.WriteInt(g.rc_2.control_in); // 2
DataFlash.WriteInt((uint16_t)dcm.yaw_sensor); // 3 DataFlash.WriteInt((int)dcm.pitch_sensor); // 3
DataFlash.WriteInt(g.rc_4.control_in); // 4
DataFlash.WriteInt((int)g.rc_1.servo_out); // 4 DataFlash.WriteInt((uint16_t)dcm.yaw_sensor); // 5
DataFlash.WriteInt((int)g.rc_2.servo_out); // 5
DataFlash.WriteInt((int)g.rc_4.servo_out); // 6
DataFlash.WriteByte(END_BYTE); DataFlash.WriteByte(END_BYTE);
} }
@ -712,10 +708,10 @@ static void Log_Read_Attitude()
{ {
int16_t temp1 = DataFlash.ReadInt(); int16_t temp1 = DataFlash.ReadInt();
int16_t temp2 = DataFlash.ReadInt(); int16_t temp2 = DataFlash.ReadInt();
uint16_t temp3 = DataFlash.ReadInt(); int16_t temp3 = DataFlash.ReadInt();
int16_t temp4 = DataFlash.ReadInt(); int16_t temp4 = DataFlash.ReadInt();
int16_t temp5 = DataFlash.ReadInt(); int16_t temp5 = DataFlash.ReadInt();
int16_t temp6 = DataFlash.ReadInt(); uint16_t temp6 = DataFlash.ReadInt();
// 1 2 3 4 5 6 // 1 2 3 4 5 6
Serial.printf_P(PSTR("ATT, %d, %d, %u, %d, %d, %d\n"), 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_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), 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 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), 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_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), 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 junk(0) // XXX just so that we can add things without worrying about the trailing comma
{ {
} }

22
ArduCopter/config.h
View File

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

2
ArduCopter/defines.h
View File

@ -166,6 +166,8 @@
#define CH6_OPTFLOW_KI 18 #define CH6_OPTFLOW_KI 18
#define CH6_OPTFLOW_KD 19 #define CH6_OPTFLOW_KD 19
#define CH6_NAV_I 20
// nav byte mask // 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_longitude = 0;
static int32_t last_latitude = 0; static int32_t last_latitude = 0;
//static int16_t x_speed_old = 0; static int16_t x_speed_old = 0;
//static int16_t y_speed_old = 0; static int16_t y_speed_old = 0;
// y_GPS_speed positve = Up // y_GPS_speed positve = Up
// x_GPS_speed positve = Right // x_GPS_speed positve = Right
@ -55,20 +55,22 @@ static void calc_XY_velocity(){
// straightforward approach: // straightforward approach:
///* ///*
int16_t x_estimate = (float)(g_gps->longitude - last_longitude) * tmp; x_actual_speed = x_speed_old + (float)(g_gps->longitude - last_longitude) * tmp;
int16_t y_estimate = (float)(g_gps->latitude - last_latitude) * tmp; y_actual_speed = y_speed_old + (float)(g_gps->latitude - last_latitude) * tmp;
// slight averaging filter x_actual_speed = x_actual_speed >> 1;
x_actual_speed = (x_actual_speed + x_estimate) >> 1; y_actual_speed = y_actual_speed >> 1;
y_actual_speed = (y_actual_speed + y_estimate) >> 1;
x_speed_old = x_actual_speed;
y_speed_old = y_actual_speed;
/* /*
// Ryan Beall's forward estimator: // Ryan Beall's forward estimator:
int16_t x_speed_new = (float)(g_gps->longitude - last_longitude) * tmp; int16_t x_speed_new = (float)(g_gps->longitude - last_longitude) * tmp;
int16_t y_speed_new = (float)(g_gps->latitude - last_latitude) * 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); x_actual_speed = x_speed_new + (x_speed_new - x_speed_old);
y_GPS_speed = y_speed_new + (y_speed_new - y_speed_old); y_actual_speed = y_speed_new + (y_speed_new - y_speed_old);
x_speed_old = x_speed_new; x_speed_old = x_speed_new;
y_speed_old = y_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 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) static void calc_loiter(int x_error, int y_error)
{ {
// East/West // 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); 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); int16_t x_iterm = g.pi_loiter_lon.get_i(x_error, dTnav);
x_rate_error = x_target_speed - x_actual_speed; x_rate_error = x_target_speed - x_actual_speed;
// North/South // 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); 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); int16_t y_iterm = g.pi_loiter_lat.get_i(y_error, dTnav);
y_rate_error = y_target_speed - y_actual_speed; 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) 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 = get_corrected_angle(rate, nav_lat);
nav_lat = constrain(nav_lat, -3000, 3000); 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; int32_t change = (millis() - alt_change_timer) >> _scale;
if(alt_change_flag == ASCENDING){ if(alt_change_flag == ASCENDING){