zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

ArduCopter Alt Hold - Change to not run get_nav_throttle if manually adjusting altitude with manual_boost - removes possibility of I term running up 

Tuning - added ability to modify altitude (position, not rate) controller
TradHeli - Fairly large change to how throttle is scaled and collective moves.
master
Randy Mackay 14 years ago
parent
bf6a675a82
commit
f397bae793
3 changed files with 104 additions and 63 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 47
      ArduCopter/ArduCopter.pde
  2. 5
      ArduCopter/defines.h
  3. 115
      ArduCopter/heli.pde

47
ArduCopter/ArduCopter.pde
Unescape View File

@ -1057,7 +1057,7 @@ void update_throttle_mode(void)
case THROTTLE_MANUAL: case THROTTLE_MANUAL:
if (g.rc_3.control_in > 0){ if (g.rc_3.control_in > 0){
#if FRAME_CONFIG == HELI_FRAME #if FRAME_CONFIG == HELI_FRAME
g.rc_3.servo_out = heli_get_angle_boost(heli_get_scaled_throttle(g.rc_3.control_in)); g.rc_3.servo_out = heli_get_angle_boost(g.rc_3.control_in);
#else #else
angle_boost = get_angle_boost(g.rc_3.control_in); angle_boost = get_angle_boost(g.rc_3.control_in);
g.rc_3.servo_out = g.rc_3.control_in + angle_boost; g.rc_3.servo_out = g.rc_3.control_in + angle_boost;
@ -1077,34 +1077,40 @@ void update_throttle_mode(void)
// fall through // fall through
case THROTTLE_AUTO: case THROTTLE_AUTO:
// 10hz, don't run up i term
if(invalid_throttle && motor_auto_armed == true){
// how far off are we
altitude_error = get_altitude_error();
// get the AP throttle
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 // calculate angle boost
invalid_throttle = false;
}
angle_boost = get_angle_boost(g.throttle_cruise); angle_boost = get_angle_boost(g.throttle_cruise);
// manual command up or down?
if(manual_boost != 0){ if(manual_boost != 0){
//remove alt_hold_velocity when implemented //remove alt_hold_velocity when implemented
#if FRAME_CONFIG == HELI_FRAME #if FRAME_CONFIG == HELI_FRAME
g.rc_3.servo_out = heli_get_angle_boost(heli_get_scaled_throttle(g.throttle_cruise + nav_throttle + manual_boost + get_z_damping())); g.rc_3.servo_out = heli_get_angle_boost(g.throttle_cruise + manual_boost + get_z_damping());
#else #else
g.rc_3.servo_out = g.throttle_cruise + angle_boost + manual_boost + get_z_damping(); g.rc_3.servo_out = g.throttle_cruise + angle_boost + manual_boost + get_z_damping();
#endif #endif
// reset next_WP.alt // reset next_WP.alt
next_WP.alt = max(current_loc.alt, 100); next_WP.alt = max(current_loc.alt, 100);
}else{ }else{
// 10hz, don't run up i term
if(invalid_throttle && motor_auto_armed == true){
// how far off are we
altitude_error = get_altitude_error();
// get the AP throttle
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
invalid_throttle = false;
}
#if FRAME_CONFIG == HELI_FRAME #if FRAME_CONFIG == HELI_FRAME
//g.rc_3.servo_out = heli_get_angle_boost(g.throttle_cruise + nav_throttle + get_z_damping()); g.rc_3.servo_out = heli_get_angle_boost(g.throttle_cruise + nav_throttle + get_z_damping());
g.rc_3.servo_out = heli_get_angle_boost(heli_get_scaled_throttle(g.throttle_cruise + nav_throttle + get_z_damping()));
#else #else
g.rc_3.servo_out = g.throttle_cruise + nav_throttle + angle_boost + get_z_damping(); g.rc_3.servo_out = g.throttle_cruise + nav_throttle + angle_boost + get_z_damping();
#endif #endif
@ -1399,7 +1405,7 @@ static void tuning(){
break; break;
case CH6_THROTTLE_KP: case CH6_THROTTLE_KP:
g.rc_6.set_range(0,1000); g.rc_6.set_range(0,1000); // 0 to 1
g.pi_throttle.kP(tuning_value); g.pi_throttle.kP(tuning_value);
break; break;
@ -1437,6 +1443,11 @@ static void tuning(){
g.heli_ext_gyro_gain = tuning_value * 1000; g.heli_ext_gyro_gain = tuning_value * 1000;
break; break;
#endif #endif
case CH6_THR_HOLD_KP:
g.rc_6.set_range(0,1000); // 0 to 1
g.pi_alt_hold.kP(tuning_value);
break;
} }
} }

5
ArduCopter/defines.h
Unescape View File

@ -140,7 +140,7 @@
#define CH6_RATE_KP 4 #define CH6_RATE_KP 4
#define CH6_RATE_KI 5 #define CH6_RATE_KI 5
#define CH6_YAW_RATE_KP 6 #define CH6_YAW_RATE_KP 6
// Altitude // Altitude rate controller
#define CH6_THROTTLE_KP 7 #define CH6_THROTTLE_KP 7
// Extras // Extras
#define CH6_TOP_BOTTOM_RATIO 8 #define CH6_TOP_BOTTOM_RATIO 8
@ -151,6 +151,9 @@
#define CH6_LOITER_P 12 #define CH6_LOITER_P 12
#define CH6_HELI_EXTERNAL_GYRO 13 #define CH6_HELI_EXTERNAL_GYRO 13
// altitude controller
#define CH6_THR_HOLD_KP 14
// nav byte mask // nav byte mask
// ------------- // -------------
#define NAV_LOCATION 1 #define NAV_LOCATION 1

115
ArduCopter/heli.pde
Unescape View File

@ -5,8 +5,8 @@
#define HELI_SERVO_AVERAGING_DIGITAL 0 // 250Hz #define HELI_SERVO_AVERAGING_DIGITAL 0 // 250Hz
#define HELI_SERVO_AVERAGING_ANALOG 2 // 125Hz #define HELI_SERVO_AVERAGING_ANALOG 2 // 125Hz
static float heli_throttle_scaler = 0;
static bool heli_swash_initialised = false; static bool heli_swash_initialised = false;
static int heli_throttle_mid = 0; // throttle mid point in pwm form (i.e. 0 ~ 1000)
// heli_servo_averaging: // heli_servo_averaging:
// 0 or 1 = no averaging, 250hz // 0 or 1 = no averaging, 250hz
@ -19,8 +19,6 @@ static bool heli_swash_initialised = false;
static void heli_init_swash() static void heli_init_swash()
{ {
int i; int i;
int tilt_max[CH_3+1];
int total_tilt_max = 0;
// swash servo initialisation // swash servo initialisation
g.heli_servo_1.set_range(0,1000); g.heli_servo_1.set_range(0,1000);
@ -38,23 +36,38 @@ static void heli_init_swash()
heli_rollFactor[CH_2] = cos(radians(g.heli_servo2_pos + 90 - g.heli_phase_angle)); heli_rollFactor[CH_2] = cos(radians(g.heli_servo2_pos + 90 - g.heli_phase_angle));
heli_rollFactor[CH_3] = cos(radians(g.heli_servo3_pos + 90 - g.heli_phase_angle)); heli_rollFactor[CH_3] = cos(radians(g.heli_servo3_pos + 90 - g.heli_phase_angle));
// collective min / max // ensure g.heli_coll values are reasonable
total_tilt_max = 0; if( g.heli_coll_min >= g.heli_coll_max ) {
for( i=CH_1; i<=CH_3; i++ ) { g.heli_coll_min = 1000;
tilt_max[i] = max(abs(heli_rollFactor[i]*g.heli_roll_max), abs(heli_pitchFactor[i]*g.heli_pitch_max))/100; g.heli_coll_max = 2000;
total_tilt_max = max(total_tilt_max,tilt_max[i]);
} }
g.heli_coll_mid = constrain(g.heli_coll_mid, g.heli_coll_min, g.heli_coll_max);
// servo min/max values - or should I use set_range?
g.heli_servo_1.radio_min = g.heli_coll_min - tilt_max[CH_1]; // servo min/max values
g.heli_servo_1.radio_max = g.heli_coll_max + tilt_max[CH_1]; if( g.heli_servo_1.get_reverse() ) {
g.heli_servo_2.radio_min = g.heli_coll_min - tilt_max[CH_2]; g.heli_servo_1.radio_min = 3000 - g.heli_coll_max + (g.heli_servo_1.radio_trim-1500);
g.heli_servo_2.radio_max = g.heli_coll_max + tilt_max[CH_2]; g.heli_servo_1.radio_max = 3000 - g.heli_coll_min + (g.heli_servo_1.radio_trim-1500);
g.heli_servo_3.radio_min = g.heli_coll_min - tilt_max[CH_3]; }else{
g.heli_servo_3.radio_max = g.heli_coll_max + tilt_max[CH_3]; g.heli_servo_1.radio_min = g.heli_coll_min + (g.heli_servo_1.radio_trim-1500);
g.heli_servo_1.radio_max = g.heli_coll_max + (g.heli_servo_1.radio_trim-1500);
// scaler for changing channel 3 radio input into collective range }
heli_throttle_scaler = ((float)(g.heli_coll_max - g.heli_coll_min))/1000; if( g.heli_servo_2.get_reverse() ) {
g.heli_servo_2.radio_min = 3000 - g.heli_coll_max + (g.heli_servo_2.radio_trim-1500);
g.heli_servo_2.radio_max = 3000 - g.heli_coll_min + (g.heli_servo_2.radio_trim-1500);
}else{
g.heli_servo_2.radio_min = g.heli_coll_min + (g.heli_servo_2.radio_trim-1500);
g.heli_servo_2.radio_max = g.heli_coll_max + (g.heli_servo_2.radio_trim-1500);
}
if( g.heli_servo_3.get_reverse() ) {
g.heli_servo_3.radio_min = 3000 - g.heli_coll_max + (g.heli_servo_3.radio_trim-1500);
g.heli_servo_3.radio_max = 3000 - g.heli_coll_min + (g.heli_servo_3.radio_trim-1500);
}else{
g.heli_servo_3.radio_min = g.heli_coll_min + (g.heli_servo_3.radio_trim-1500);
g.heli_servo_3.radio_max = g.heli_coll_max + (g.heli_servo_3.radio_trim-1500);
}
// calculate throttle mid point
heli_throttle_mid = (g.heli_coll_mid-g.heli_coll_min)*(1000.0/(g.heli_coll_max-g.heli_coll_min));
// reset the servo averaging // reset the servo averaging
for( i=0; i<=3; i++ ) for( i=0; i<=3; i++ )
@ -72,7 +85,7 @@ static void heli_init_swash()
static void heli_move_servos_to_mid() static void heli_move_servos_to_mid()
{ {
heli_move_swash(0,0,1500,0); heli_move_swash(0,0,500,0);
} }
// //
@ -80,7 +93,7 @@ static void heli_move_servos_to_mid()
// - expected ranges: // - expected ranges:
// roll : -4500 ~ 4500 // roll : -4500 ~ 4500
// pitch: -4500 ~ 4500 // pitch: -4500 ~ 4500
// collective: 1000 ~ 2000 // collective: 0 ~ 1000
// yaw: -4500 ~ 4500 // yaw: -4500 ~ 4500
// //
static void heli_move_swash(int roll_out, int pitch_out, int coll_out, int yaw_out) static void heli_move_swash(int roll_out, int pitch_out, int coll_out, int yaw_out)
@ -92,6 +105,29 @@ static void heli_move_swash(int roll_out, int pitch_out, int coll_out, int yaw_o
// we must be in set-up mode so mark swash as uninitialised // we must be in set-up mode so mark swash as uninitialised
heli_swash_initialised = false; heli_swash_initialised = false;
// free up servo ranges
if( g.heli_servo_1.get_reverse() ) {
g.heli_servo_1.radio_min = 3000 - g.rc_3.radio_max + (g.heli_servo_1.radio_trim-1500);
g.heli_servo_1.radio_max = 3000 - g.rc_3.radio_min + (g.heli_servo_1.radio_trim-1500);
}else{
g.heli_servo_1.radio_min = g.rc_3.radio_min + (g.heli_servo_1.radio_trim-1500);
g.heli_servo_1.radio_max = g.rc_3.radio_max + (g.heli_servo_1.radio_trim-1500);
}
if( g.heli_servo_2.get_reverse() ) {
g.heli_servo_2.radio_min = 3000 - g.rc_3.radio_max + (g.heli_servo_2.radio_trim-1500);
g.heli_servo_2.radio_max = 3000 - g.rc_3.radio_min + (g.heli_servo_2.radio_trim-1500);
}else{
g.heli_servo_2.radio_min = g.rc_3.radio_min + (g.heli_servo_2.radio_trim-1500);
g.heli_servo_2.radio_max = g.rc_3.radio_max + (g.heli_servo_2.radio_trim-1500);
}
if( g.heli_servo_3.get_reverse() ) {
g.heli_servo_3.radio_min = 3000 - g.rc_3.radio_max + (g.heli_servo_3.radio_trim-1500);
g.heli_servo_3.radio_max = 3000 - g.rc_3.radio_min + (g.heli_servo_3.radio_trim-1500);
}else{
g.heli_servo_3.radio_min = g.rc_3.radio_min + (g.heli_servo_3.radio_trim-1500);
g.heli_servo_3.radio_max = g.rc_3.radio_max + (g.heli_servo_3.radio_trim-1500);
}
}else{ // regular flight mode }else{ // regular flight mode
// check if we need to reinitialise the swash // check if we need to reinitialise the swash
@ -102,7 +138,7 @@ static void heli_move_swash(int roll_out, int pitch_out, int coll_out, int yaw_o
// ensure values are acceptable: // ensure values are acceptable:
roll_out = constrain(roll_out, (int)-g.heli_roll_max, (int)g.heli_roll_max); roll_out = constrain(roll_out, (int)-g.heli_roll_max, (int)g.heli_roll_max);
pitch_out = constrain(pitch_out, (int)-g.heli_pitch_max, (int)g.heli_pitch_max); pitch_out = constrain(pitch_out, (int)-g.heli_pitch_max, (int)g.heli_pitch_max);
coll_out = constrain(coll_out, (int)g.heli_coll_min, (int)g.heli_coll_max); coll_out = constrain(coll_out, 0, 1000);
// rudder feed forward based on collective // rudder feed forward based on collective
#if HIL_MODE == HIL_MODE_DISABLED // don't do rudder feed forward in simulator #if HIL_MODE == HIL_MODE_DISABLED // don't do rudder feed forward in simulator
@ -113,9 +149,9 @@ static void heli_move_swash(int roll_out, int pitch_out, int coll_out, int yaw_o
} }
// swashplate servos // swashplate servos
g.heli_servo_1.servo_out = (heli_rollFactor[CH_1] * roll_out + heli_pitchFactor[CH_1] * pitch_out)/10 + coll_out - 1000 + (g.heli_servo_1.radio_trim-1500); g.heli_servo_1.servo_out = (heli_rollFactor[CH_1] * roll_out + heli_pitchFactor[CH_1] * pitch_out)/10 + coll_out + (g.heli_servo_1.radio_trim-1500);
g.heli_servo_2.servo_out = (heli_rollFactor[CH_2] * roll_out + heli_pitchFactor[CH_2] * pitch_out)/10 + coll_out - 1000 + (g.heli_servo_2.radio_trim-1500); g.heli_servo_2.servo_out = (heli_rollFactor[CH_2] * roll_out + heli_pitchFactor[CH_2] * pitch_out)/10 + coll_out + (g.heli_servo_2.radio_trim-1500);
g.heli_servo_3.servo_out = (heli_rollFactor[CH_3] * roll_out + heli_pitchFactor[CH_3] * pitch_out)/10 + coll_out - 1000 + (g.heli_servo_3.radio_trim-1500); g.heli_servo_3.servo_out = (heli_rollFactor[CH_3] * roll_out + heli_pitchFactor[CH_3] * pitch_out)/10 + coll_out + (g.heli_servo_3.radio_trim-1500);
g.heli_servo_4.servo_out = yaw_out + yaw_offset; g.heli_servo_4.servo_out = yaw_out + yaw_offset;
// use servo_out to calculate pwm_out and radio_out // use servo_out to calculate pwm_out and radio_out
@ -130,7 +166,7 @@ static void heli_move_swash(int roll_out, int pitch_out, int coll_out, int yaw_o
heli_servo_out[2] += g.heli_servo_3.radio_out; heli_servo_out[2] += g.heli_servo_3.radio_out;
heli_servo_out[3] += g.heli_servo_4.radio_out; heli_servo_out[3] += g.heli_servo_4.radio_out;
heli_servo_out_count++; heli_servo_out_count++;
// is it time to move the servos? // is it time to move the servos?
if( heli_servo_out_count >= g.heli_servo_averaging ) { if( heli_servo_out_count >= g.heli_servo_averaging ) {
@ -171,8 +207,8 @@ static void heli_move_swash(int roll_out, int pitch_out, int coll_out, int yaw_o
static void init_motors_out() static void init_motors_out()
{ {
#if INSTANT_PWM == 0 #if INSTANT_PWM == 0
ICR5 = 5000; // 400 hz output CH 1, 2, 9 ICR5 = 8000; // 250 hz output CH 1, 2, 9
ICR1 = 5000; // 400 hz output CH 3, 4, 10 ICR1 = 8000; // 250 hz output CH 3, 4, 10
ICR3 = 40000; // 50 hz output CH 7, 8, 11 ICR3 = 40000; // 50 hz output CH 7, 8, 11
#endif #endif
} }
@ -194,7 +230,7 @@ static void output_motors_armed()
g.rc_3.calc_pwm(); g.rc_3.calc_pwm();
g.rc_4.calc_pwm(); g.rc_4.calc_pwm();
heli_move_swash( g.rc_1.servo_out, g.rc_2.servo_out, g.rc_3.radio_out, g.rc_4.servo_out ); heli_move_swash( g.rc_1.servo_out, g.rc_2.servo_out, g.rc_3.servo_out, g.rc_4.servo_out );
} }
// for helis - armed or disarmed we allow servos to move // for helis - armed or disarmed we allow servos to move
@ -212,25 +248,16 @@ static void output_motor_test()
{ {
} }
// heli_get_scaled_throttle - user's throttle scaled to collective range // heli_angle_boost - adds a boost depending on roll/pitch values
// input is expected to be in the range of 0~1000 (ie. pwm)
// also does equivalent of angle_boost
static int heli_get_scaled_throttle(int throttle)
{
float scaled_throttle = (g.heli_coll_min - 1000) + throttle * heli_throttle_scaler;
return scaled_throttle;
}
// heli_angle_boost - takes servo_out position
// adds a boost depending on roll/pitch values
// equivalent of quad's angle_boost function // equivalent of quad's angle_boost function
// pwm_out value should be 0 ~ 1000 // throttle value should be 0 ~ 1000
static int heli_get_angle_boost(int pwm_out) static int heli_get_angle_boost(int throttle)
{ {
float angle_boost_factor = cos_pitch_x * cos_roll_x; float angle_boost_factor = cos_pitch_x * cos_roll_x;
angle_boost_factor = 1.0 - constrain(angle_boost_factor, .5, 1.0); angle_boost_factor = 1.0 - constrain(angle_boost_factor, .5, 1.0);
int throttle_above_center = max(1000 + pwm_out - g.heli_coll_mid,0); int throttle_above_mid = max(throttle - heli_throttle_mid,0);
return pwm_out + throttle_above_center*angle_boost_factor; return throttle + throttle_above_mid*angle_boost_factor;
} }
#endif // HELI_FRAME #endif // HELI_FRAME

Write Preview
Loading…
Cancel
Save