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

Added second PI loop for alt hold. 

Upped gains for default JDrones Frame
added gains for second PI loop for alt
removed some tests for 1280 space constraints
This commit is contained in:
Jason Short 2011-10-02 11:36:23 -07:00
parent c46fe580c2
commit 7e81756ccd
8 changed files with 76 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduCopter/APM_Config.h
View File

@ -40,7 +40,7 @@
CH7_ADC_FILTER (experimental) CH7_ADC_FILTER (experimental)
*/ */
#define ACCEL_ALT_HOLD 0 #define ACCEL_ALT_HOLD 0 // disabled by default, work in progress
#define ACCEL_ALT_HOLD_GAIN 12.0 #define ACCEL_ALT_HOLD_GAIN 12.0
// ACCEL_ALT_HOLD 1 to enable experimental alt_hold_mode // ACCEL_ALT_HOLD 1 to enable experimental alt_hold_mode

29
ArduCopter/ArduCopter.pde
View File

@ -260,16 +260,20 @@ static const char* flight_mode_strings[] = {
*/ */
// test // test
#if ACCEL_ALT_HOLD == 1
Vector3f accels_rot; Vector3f accels_rot;
//float accel_gain = 12; static int accels_rot_count;
static float accels_rot_sum;
static float alt_hold_gain = ACCEL_ALT_HOLD_GAIN;
#endif
// temp // temp
int y_actual_speed; static int y_actual_speed;
int y_rate_error; static int y_rate_error;
// calc the // calc the
int x_actual_speed; static int x_actual_speed;
int x_rate_error; static int x_rate_error;
// Radio // Radio
// ----- // -----
@ -585,7 +589,7 @@ static void fast_loop()
set_servos_4(); set_servos_4();
//if(motor_armed) //if(motor_armed)
// Log_Write_Attitude(); //Log_Write_Attitude();
} }
static void medium_loop() static void medium_loop()
@ -1078,7 +1082,7 @@ void update_throttle_mode(void)
altitude_error = get_altitude_error(); altitude_error = get_altitude_error();
// get the AP throttle // get the AP throttle
nav_throttle = get_nav_throttle(altitude_error, 250); //150 = target speed of 1.5m/s nav_throttle = get_nav_throttle(altitude_error);//, 250); //150 = target speed of 1.5m/s
//Serial.printf("in:%d, cr:%d, NT:%d, I:%1.4f\n", g.rc_3.control_in,altitude_error, nav_throttle, g.pi_throttle.get_integrator()); //Serial.printf("in:%d, cr:%d, NT:%d, I:%1.4f\n", g.rc_3.control_in,altitude_error, nav_throttle, g.pi_throttle.get_integrator());
// clear the new data flag // clear the new data flag
@ -1201,8 +1205,12 @@ static void update_trig(void){
// 270 = cos_yaw: -1.00, sin_yaw: 0.00, // 270 = cos_yaw: -1.00, sin_yaw: 0.00,
#if ACCEL_ALT_HOLD == 1
Vector3f accel_filt = imu.get_accel_filtered(); Vector3f accel_filt = imu.get_accel_filtered();
accels_rot = dcm.get_dcm_matrix() * imu.get_accel_filtered(); accels_rot = dcm.get_dcm_matrix() * imu.get_accel_filtered();
accels_rot_sum += accels_rot.z;
accels_rot_count++;
#endif
} }
// updated at 10hz // updated at 10hz
@ -1266,6 +1274,13 @@ static void tuning(){
tuning_value = (float)g.rc_6.control_in / 1000.0; tuning_value = (float)g.rc_6.control_in / 1000.0;
switch(g.radio_tuning){ switch(g.radio_tuning){
/*case CH6_STABILIZE_KP:
g.rc_6.set_range(0,2000); // 0 to 8
tuning_value = (float)g.rc_6.control_in / 100.0;
alt_hold_gain = tuning_value;
break;*/
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.pi_stabilize_roll.kP(tuning_value); g.pi_stabilize_roll.kP(tuning_value);

36
ArduCopter/Attitude.pde
View File

@ -83,19 +83,18 @@ get_stabilize_yaw(long target_angle)
return (int)constrain(rate, -2500, 2500); return (int)constrain(rate, -2500, 2500);
} }
#define ALT_ERROR_MAX 300 #define ALT_ERROR_MAX 400
static int static int
get_nav_throttle(long z_error, int target_speed) get_nav_throttle(long z_error)//, //int target_speed)
{ {
int rate_error;
float scaler = (float)target_speed/(float)ALT_ERROR_MAX;
// limit error to prevent I term run up // limit error to prevent I term run up
z_error = constrain(z_error, -ALT_ERROR_MAX, ALT_ERROR_MAX); z_error = constrain(z_error, -ALT_ERROR_MAX, ALT_ERROR_MAX);
target_speed = z_error * scaler; int rate_error = g.pi_alt_hold.get_pi(z_error, .1); //_p = .85
rate_error = target_speed - altitude_rate;
rate_error = constrain(rate_error, -110, 110);
rate_error = rate_error - altitude_rate;
// limit the rate
rate_error = constrain(rate_error, -100, 120);
return (int)g.pi_throttle.get_pi(rate_error, .1); return (int)g.pi_throttle.get_pi(rate_error, .1);
} }
@ -187,20 +186,35 @@ get_nav_yaw_offset(int yaw_input, int reset)
} }
} }
///*
static int alt_hold_velocity() static int alt_hold_velocity()
{ {
#if ACCEL_ALT_HOLD == 1 #if ACCEL_ALT_HOLD == 1
// subtract filtered Accel // subtract filtered Accel
float error = abs(next_WP.alt - current_loc.alt); float error = abs(next_WP.alt - current_loc.alt);
error -= 100;
error = min(error, 200.0); error = min(error, 200.0);
error = max(error, 0.0);
error = 1 - (error/ 200.0); error = 1 - (error/ 200.0);
return (accels_rot.z + 9.81) * ACCEL_ALT_HOLD_GAIN * error; float sum = accels_rot_sum / (float)accels_rot_count;
accels_rot_sum = 0;
accels_rot_count = 0;
int output = (sum + 9.81) * alt_hold_gain * error;
// fast rise
//s: -17.6241, g:0.0000, e:1.0000, o:0
//s: -18.4990, g:0.0000, e:1.0000, o:0
//s: -19.3193, g:0.0000, e:1.0000, o:0
//s: -13.1310, g:47.8700, e:1.0000, o:-158
//Serial.printf("s: %1.4f, g:%1.4f, e:%1.4f, o:%d\n",sum, alt_hold_gain, error, output);
return output;
#else #else
return 0; return 0;
#endif #endif
} }
//*/
static int get_angle_boost() static int get_angle_boost()
{ {

5
ArduCopter/Parameters.h
View File

@ -164,6 +164,7 @@ public:
k_param_pi_loiter_lon, k_param_pi_loiter_lon,
k_param_pi_nav_lat, k_param_pi_nav_lat,
k_param_pi_nav_lon, k_param_pi_nav_lon,
k_param_pi_alt_hold,
k_param_pi_throttle, k_param_pi_throttle,
k_param_pi_crosstrack, k_param_pi_crosstrack,
@ -277,6 +278,7 @@ public:
APM_PI pi_nav_lat; APM_PI pi_nav_lat;
APM_PI pi_nav_lon; APM_PI pi_nav_lon;
APM_PI pi_alt_hold;
APM_PI pi_throttle; APM_PI pi_throttle;
APM_PI pi_crosstrack; APM_PI pi_crosstrack;
@ -381,7 +383,8 @@ public:
pi_nav_lat (k_param_pi_nav_lat, PSTR("NAV_LAT_"), NAV_P, NAV_I, NAV_IMAX * 100), pi_nav_lat (k_param_pi_nav_lat, PSTR("NAV_LAT_"), NAV_P, NAV_I, NAV_IMAX * 100),
pi_nav_lon (k_param_pi_nav_lon, PSTR("NAV_LON_"), NAV_P, NAV_I, NAV_IMAX * 100), pi_nav_lon (k_param_pi_nav_lon, PSTR("NAV_LON_"), NAV_P, NAV_I, NAV_IMAX * 100),
pi_throttle (k_param_pi_throttle, PSTR("THR_HOLD_"), THROTTLE_P, THROTTLE_I, THROTTLE_IMAX), pi_alt_hold (k_param_pi_alt_hold, PSTR("THR_ALT_"), THR_HOLD_P, THR_HOLD_I, THR_HOLD_IMAX),
pi_throttle (k_param_pi_throttle, PSTR("THR_RATE_"), THROTTLE_P, THROTTLE_I, THROTTLE_IMAX),
pi_crosstrack (k_param_pi_crosstrack, PSTR("XTRACK_"), XTRACK_P, XTRACK_I, XTRACK_IMAX), pi_crosstrack (k_param_pi_crosstrack, PSTR("XTRACK_"), XTRACK_P, XTRACK_I, XTRACK_IMAX),
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

2
ArduCopter/commands_logic.pde
View File

@ -482,7 +482,7 @@ static void do_wait_delay()
//Serial.print("dwd "); //Serial.print("dwd ");
condition_start = millis(); condition_start = millis();
condition_value = next_command.lat * 1000; // convert to milliseconds condition_value = next_command.lat * 1000; // convert to milliseconds
Serial.println(condition_value,DEC); //Serial.println(condition_value,DEC);
} }
static void do_change_alt() static void do_change_alt()

20
ArduCopter/config.h
View File

@ -388,7 +388,7 @@
// Attitude Control // Attitude Control
// //
#ifndef STABILIZE_ROLL_P #ifndef STABILIZE_ROLL_P
# define STABILIZE_ROLL_P 4.0 # define STABILIZE_ROLL_P 4.6
#endif #endif
#ifndef STABILIZE_ROLL_I #ifndef STABILIZE_ROLL_I
# define STABILIZE_ROLL_I 0.001 # define STABILIZE_ROLL_I 0.001
@ -398,7 +398,7 @@
#endif #endif
#ifndef STABILIZE_PITCH_P #ifndef STABILIZE_PITCH_P
# define STABILIZE_PITCH_P 4.0 # define STABILIZE_PITCH_P 4.6
#endif #endif
#ifndef STABILIZE_PITCH_I #ifndef STABILIZE_PITCH_I
# define STABILIZE_PITCH_I 0.001 # define STABILIZE_PITCH_I 0.001
@ -411,7 +411,7 @@
// Rate Control // Rate Control
// //
#ifndef RATE_ROLL_P #ifndef RATE_ROLL_P
# define RATE_ROLL_P .13 # define RATE_ROLL_P 0.145
#endif #endif
#ifndef RATE_ROLL_I #ifndef RATE_ROLL_I
# define RATE_ROLL_I 0.0 # define RATE_ROLL_I 0.0
@ -421,7 +421,7 @@
#endif #endif
#ifndef RATE_PITCH_P #ifndef RATE_PITCH_P
# define RATE_PITCH_P 0.13 # define RATE_PITCH_P 0.145
#endif #endif
#ifndef RATE_PITCH_I #ifndef RATE_PITCH_I
# define RATE_PITCH_I 0 //0.18 # define RATE_PITCH_I 0 //0.18
@ -500,6 +500,16 @@
# define THROTTLE_CRUISE 350 // # define THROTTLE_CRUISE 350 //
#endif #endif
#ifndef THR_HOLD_P
# define THR_HOLD_P 0.80 //
#endif
#ifndef THR_HOLD_I
# define THR_HOLD_I 0.00 // with 4m error, 12.5s windup
#endif
#ifndef THR_HOLD_IMAX
# define THR_HOLD_IMAX 00
#endif
#ifndef THROTTLE_P #ifndef THROTTLE_P
# define THROTTLE_P 0.6 // # define THROTTLE_P 0.6 //
#endif #endif
@ -507,7 +517,7 @@
# define THROTTLE_I 0.10 // with 4m error, 12.5s windup # define THROTTLE_I 0.10 // with 4m error, 12.5s windup
#endif #endif
#ifndef THROTTLE_IMAX #ifndef THROTTLE_IMAX
# define THROTTLE_IMAX 300 # define THROTTLE_IMAX 300
#endif #endif

2
ArduCopter/control_modes.pde
View File

@ -120,7 +120,7 @@ static void auto_trim()
led_mode = NORMAL_LEDS; led_mode = NORMAL_LEDS;
clear_leds(); clear_leds();
imu.save(); imu.save();
Serial.println("Done"); //Serial.println("Done");
auto_level_counter = 0; auto_level_counter = 0;
} }
} }

11
ArduCopter/test.pde
View File

@ -24,7 +24,7 @@ static int8_t test_current(uint8_t argc, const Menu::arg *argv);
static int8_t test_relay(uint8_t argc, const Menu::arg *argv); static int8_t test_relay(uint8_t argc, const Menu::arg *argv);
static int8_t test_wp(uint8_t argc, const Menu::arg *argv); static int8_t test_wp(uint8_t argc, const Menu::arg *argv);
static int8_t test_baro(uint8_t argc, const Menu::arg *argv); static int8_t test_baro(uint8_t argc, const Menu::arg *argv);
static int8_t test_mag(uint8_t argc, const Menu::arg *argv); //static int8_t test_mag(uint8_t argc, const Menu::arg *argv);
static int8_t test_sonar(uint8_t argc, const Menu::arg *argv); static int8_t test_sonar(uint8_t argc, const Menu::arg *argv);
#ifdef OPTFLOW_ENABLED #ifdef OPTFLOW_ENABLED
static int8_t test_optflow(uint8_t argc, const Menu::arg *argv); static int8_t test_optflow(uint8_t argc, const Menu::arg *argv);
@ -76,11 +76,11 @@ const struct Menu::command test_menu_commands[] PROGMEM = {
{"altitude", test_baro}, {"altitude", test_baro},
#endif #endif
{"sonar", test_sonar}, {"sonar", test_sonar},
{"compass", test_mag}, //{"compass", test_mag},
#ifdef OPTFLOW_ENABLED #ifdef OPTFLOW_ENABLED
{"optflow", test_optflow}, {"optflow", test_optflow},
#endif #endif
{"xbee", test_xbee}, //{"xbee", test_xbee},
{"eedump", test_eedump}, {"eedump", test_eedump},
{"rawgps", test_rawgps}, {"rawgps", test_rawgps},
// {"mission", test_mission}, // {"mission", test_mission},
@ -769,7 +769,7 @@ static int8_t test_rawgps(uint8_t argc, const Menu::arg *argv) {
} }
} }
static int8_t /*static int8_t
test_xbee(uint8_t argc, const Menu::arg *argv) test_xbee(uint8_t argc, const Menu::arg *argv)
{ {
print_hit_enter(); print_hit_enter();
@ -785,6 +785,7 @@ test_xbee(uint8_t argc, const Menu::arg *argv)
} }
} }
} }
*/
#if HIL_MODE != HIL_MODE_ATTITUDE #if HIL_MODE != HIL_MODE_ATTITUDE
static int8_t static int8_t
@ -805,6 +806,7 @@ test_baro(uint8_t argc, const Menu::arg *argv)
} }
#endif #endif
/*
static int8_t static int8_t
test_mag(uint8_t argc, const Menu::arg *argv) test_mag(uint8_t argc, const Menu::arg *argv)
{ {
@ -834,6 +836,7 @@ test_mag(uint8_t argc, const Menu::arg *argv)
return (0); return (0);
} }
} }
*/
/* /*
static int8_t static int8_t
test_reverse(uint8_t argc, const Menu::arg *argv) test_reverse(uint8_t argc, const Menu::arg *argv)