mirror of https://github.com/ArduPilot/ardupilot
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
parent
c46fe580c2
commit
7e81756ccd
|
|
@ -40,7 +40,7 @@
|
|||
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
|
||||
// ACCEL_ALT_HOLD 1 to enable experimental alt_hold_mode
|
||||
|
||||
|
|
|
|||
|
|
@ -260,16 +260,20 @@ static const char* flight_mode_strings[] = {
|
|||
*/
|
||||
|
||||
// test
|
||||
#if ACCEL_ALT_HOLD == 1
|
||||
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
|
||||
int y_actual_speed;
|
||||
int y_rate_error;
|
||||
static int y_actual_speed;
|
||||
static int y_rate_error;
|
||||
|
||||
// calc the
|
||||
int x_actual_speed;
|
||||
int x_rate_error;
|
||||
static int x_actual_speed;
|
||||
static int x_rate_error;
|
||||
|
||||
// Radio
|
||||
// -----
|
||||
|
|
@ -585,7 +589,7 @@ static void fast_loop()
|
|||
set_servos_4();
|
||||
|
||||
//if(motor_armed)
|
||||
// Log_Write_Attitude();
|
||||
//Log_Write_Attitude();
|
||||
}
|
||||
|
||||
static void medium_loop()
|
||||
|
|
@ -1078,7 +1082,7 @@ void update_throttle_mode(void)
|
|||
altitude_error = get_altitude_error();
|
||||
|
||||
// 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());
|
||||
|
||||
// clear the new data flag
|
||||
|
|
@ -1201,8 +1205,12 @@ static void update_trig(void){
|
|||
// 270 = cos_yaw: -1.00, sin_yaw: 0.00,
|
||||
|
||||
|
||||
#if ACCEL_ALT_HOLD == 1
|
||||
Vector3f accel_filt = 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
|
||||
|
|
@ -1266,6 +1274,13 @@ static void tuning(){
|
|||
tuning_value = (float)g.rc_6.control_in / 1000.0;
|
||||
|
||||
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:
|
||||
g.rc_6.set_range(0,8000); // 0 to 8
|
||||
g.pi_stabilize_roll.kP(tuning_value);
|
||||
|
|
|
|||
|
|
@ -83,19 +83,18 @@ get_stabilize_yaw(long target_angle)
|
|||
return (int)constrain(rate, -2500, 2500);
|
||||
}
|
||||
|
||||
#define ALT_ERROR_MAX 300
|
||||
#define ALT_ERROR_MAX 400
|
||||
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
|
||||
z_error = constrain(z_error, -ALT_ERROR_MAX, ALT_ERROR_MAX);
|
||||
target_speed = z_error * scaler;
|
||||
rate_error = target_speed - altitude_rate;
|
||||
rate_error = constrain(rate_error, -110, 110);
|
||||
int rate_error = g.pi_alt_hold.get_pi(z_error, .1); //_p = .85
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
|
|
@ -187,20 +186,35 @@ get_nav_yaw_offset(int yaw_input, int reset)
|
|||
}
|
||||
}
|
||||
|
||||
///*
|
||||
static int alt_hold_velocity()
|
||||
{
|
||||
#if ACCEL_ALT_HOLD == 1
|
||||
// subtract filtered Accel
|
||||
float error = abs(next_WP.alt - current_loc.alt);
|
||||
|
||||
error -= 100;
|
||||
error = min(error, 200.0);
|
||||
error = max(error, 0.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
|
||||
return 0;
|
||||
#endif
|
||||
}
|
||||
//*/
|
||||
|
||||
static int get_angle_boost()
|
||||
{
|
||||
|
|
|
|||
|
|
@ -164,6 +164,7 @@ public:
|
|||
k_param_pi_loiter_lon,
|
||||
k_param_pi_nav_lat,
|
||||
k_param_pi_nav_lon,
|
||||
k_param_pi_alt_hold,
|
||||
k_param_pi_throttle,
|
||||
k_param_pi_crosstrack,
|
||||
|
||||
|
|
@ -277,6 +278,7 @@ public:
|
|||
APM_PI pi_nav_lat;
|
||||
APM_PI pi_nav_lon;
|
||||
|
||||
APM_PI pi_alt_hold;
|
||||
APM_PI pi_throttle;
|
||||
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_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),
|
||||
|
||||
junk(0) // XXX just so that we can add things without worrying about the trailing comma
|
||||
|
|
|
|||
|
|
@ -482,7 +482,7 @@ static void do_wait_delay()
|
|||
//Serial.print("dwd ");
|
||||
condition_start = millis();
|
||||
condition_value = next_command.lat * 1000; // convert to milliseconds
|
||||
Serial.println(condition_value,DEC);
|
||||
//Serial.println(condition_value,DEC);
|
||||
}
|
||||
|
||||
static void do_change_alt()
|
||||
|
|
|
|||
|
|
@ -388,7 +388,7 @@
|
|||
// Attitude Control
|
||||
//
|
||||
#ifndef STABILIZE_ROLL_P
|
||||
# define STABILIZE_ROLL_P 4.0
|
||||
# define STABILIZE_ROLL_P 4.6
|
||||
#endif
|
||||
#ifndef STABILIZE_ROLL_I
|
||||
# define STABILIZE_ROLL_I 0.001
|
||||
|
|
@ -398,7 +398,7 @@
|
|||
#endif
|
||||
|
||||
#ifndef STABILIZE_PITCH_P
|
||||
# define STABILIZE_PITCH_P 4.0
|
||||
# define STABILIZE_PITCH_P 4.6
|
||||
#endif
|
||||
#ifndef STABILIZE_PITCH_I
|
||||
# define STABILIZE_PITCH_I 0.001
|
||||
|
|
@ -411,7 +411,7 @@
|
|||
// Rate Control
|
||||
//
|
||||
#ifndef RATE_ROLL_P
|
||||
# define RATE_ROLL_P .13
|
||||
# define RATE_ROLL_P 0.145
|
||||
#endif
|
||||
#ifndef RATE_ROLL_I
|
||||
# define RATE_ROLL_I 0.0
|
||||
|
|
@ -421,7 +421,7 @@
|
|||
#endif
|
||||
|
||||
#ifndef RATE_PITCH_P
|
||||
# define RATE_PITCH_P 0.13
|
||||
# define RATE_PITCH_P 0.145
|
||||
#endif
|
||||
#ifndef RATE_PITCH_I
|
||||
# define RATE_PITCH_I 0 //0.18
|
||||
|
|
@ -500,6 +500,16 @@
|
|||
# define THROTTLE_CRUISE 350 //
|
||||
#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
|
||||
# define THROTTLE_P 0.6 //
|
||||
#endif
|
||||
|
|
@ -507,7 +517,7 @@
|
|||
# define THROTTLE_I 0.10 // with 4m error, 12.5s windup
|
||||
#endif
|
||||
#ifndef THROTTLE_IMAX
|
||||
# define THROTTLE_IMAX 300
|
||||
# define THROTTLE_IMAX 300
|
||||
#endif
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -120,7 +120,7 @@ static void auto_trim()
|
|||
led_mode = NORMAL_LEDS;
|
||||
clear_leds();
|
||||
imu.save();
|
||||
Serial.println("Done");
|
||||
//Serial.println("Done");
|
||||
auto_level_counter = 0;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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_wp(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);
|
||||
#ifdef OPTFLOW_ENABLED
|
||||
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},
|
||||
#endif
|
||||
{"sonar", test_sonar},
|
||||
{"compass", test_mag},
|
||||
//{"compass", test_mag},
|
||||
#ifdef OPTFLOW_ENABLED
|
||||
{"optflow", test_optflow},
|
||||
#endif
|
||||
{"xbee", test_xbee},
|
||||
//{"xbee", test_xbee},
|
||||
{"eedump", test_eedump},
|
||||
{"rawgps", test_rawgps},
|
||||
// {"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)
|
||||
{
|
||||
print_hit_enter();
|
||||
|
|
@ -785,6 +785,7 @@ test_xbee(uint8_t argc, const Menu::arg *argv)
|
|||
}
|
||||
}
|
||||
}
|
||||
*/
|
||||
|
||||
#if HIL_MODE != HIL_MODE_ATTITUDE
|
||||
static int8_t
|
||||
|
|
@ -805,6 +806,7 @@ test_baro(uint8_t argc, const Menu::arg *argv)
|
|||
}
|
||||
#endif
|
||||
|
||||
/*
|
||||
static int8_t
|
||||
test_mag(uint8_t argc, const Menu::arg *argv)
|
||||
{
|
||||
|
|
@ -834,6 +836,7 @@ test_mag(uint8_t argc, const Menu::arg *argv)
|
|||
return (0);
|
||||
}
|
||||
}
|
||||
*/
|
||||
/*
|
||||
static int8_t
|
||||
test_reverse(uint8_t argc, const Menu::arg *argv)
|
||||
|
|
|
|||
Loading…
Reference in New Issue