Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-11 02:18:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

TradHeli - new parameter HSV_MAN to allow better set-up from APMissionPlanner. Also changed swash movement to use radio_out which fixes some setup bugs

Browse Source
This commit is contained in:
unknown 2011-11-12 23:23:07 +09:00
parent 4b89bc174f
commit 4fcf11cf6e
5 changed files with 46 additions and 49 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
ArduCopter/Parameters.h
View File

@ -75,7 +75,8 @@ public:
k_param_heli_collective_mid, k_param_heli_collective_mid,
k_param_heli_ext_gyro_enabled, k_param_heli_ext_gyro_enabled,
k_param_heli_ext_gyro_gain, k_param_heli_ext_gyro_gain,
k_param_heli_servo_averaging, // 94 k_param_heli_servo_averaging,
k_param_heli_servo_manual, // 95
#endif #endif
// 110: Telemetry control // 110: Telemetry control
@ -253,6 +254,7 @@ public:
AP_Int8 heli_ext_gyro_enabled; // 0 = no external tail gyro, 1 = external tail gyro AP_Int8 heli_ext_gyro_enabled; // 0 = no external tail gyro, 1 = external tail gyro
AP_Int16 heli_ext_gyro_gain; // radio output 1000~2000 (value output on CH_7) AP_Int16 heli_ext_gyro_gain; // radio output 1000~2000 (value output on CH_7)
AP_Int8 heli_servo_averaging; // 0 or 1 = no averaging (250hz) for **digital servos**, 2=average of two samples (125hz), 3=three samples (83.3hz) **analog servos**, 4=four samples (62.5hz), 5=5 samples(50hz) AP_Int8 heli_servo_averaging; // 0 or 1 = no averaging (250hz) for **digital servos**, 2=average of two samples (125hz), 3=three samples (83.3hz) **analog servos**, 4=four samples (62.5hz), 5=5 samples(50hz)
AP_Int8 heli_servo_manual; // 0 = normal mode, 1 = radio inputs directly control swash. required for swash set-up
#endif #endif
// RC channels // RC channels
@ -361,6 +363,7 @@ public:
heli_ext_gyro_enabled (0, k_param_heli_ext_gyro_enabled, PSTR("GYR_ENABLE_")), heli_ext_gyro_enabled (0, k_param_heli_ext_gyro_enabled, PSTR("GYR_ENABLE_")),
heli_ext_gyro_gain (1000, k_param_heli_ext_gyro_gain, PSTR("GYR_GAIN_")), heli_ext_gyro_gain (1000, k_param_heli_ext_gyro_gain, PSTR("GYR_GAIN_")),
heli_servo_averaging (0, k_param_heli_servo_averaging, PSTR("SV_AVG")), heli_servo_averaging (0, k_param_heli_servo_averaging, PSTR("SV_AVG")),
heli_servo_manual (0, k_param_heli_servo_manual, PSTR("HSV_MAN")),
#endif #endif
// RC channel group key name // RC channel group key name

54
ArduCopter/heli.pde
View File

@ -5,7 +5,6 @@
#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 int heli_manual_override = false;
static float heli_throttle_scaler = 0; static float heli_throttle_scaler = 0;
// heli_servo_averaging: // heli_servo_averaging:
@ -81,25 +80,18 @@ static void heli_move_servos_to_mid()
// 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)
{ {
// ensure values are acceptable: // ensure values are acceptable:
roll_out = constrain(roll_out, (int)-g.heli_roll_max, (int)g.heli_roll_max); if( g.heli_servo_manual != 1) {
pitch_out = constrain(pitch_out, (int)-g.heli_pitch_max, (int)g.heli_pitch_max); roll_out = constrain(roll_out, (int)-g.heli_roll_max, (int)g.heli_roll_max);
coll_out = constrain(coll_out, (int)g.heli_coll_min, (int)g.heli_coll_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);
}
// 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 + (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 - 1000 + (g.heli_servo_1.radio_trim-1500);
if( g.heli_servo_1.get_reverse() ) 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_1.servo_out = 3000 - g.heli_servo_1.servo_out; 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_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);
if( g.heli_servo_2.get_reverse() )
g.heli_servo_2.servo_out = 3000 - g.heli_servo_2.servo_out;
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);
if( g.heli_servo_3.get_reverse() )
g.heli_servo_3.servo_out = 3000 - g.heli_servo_3.servo_out;
g.heli_servo_4.servo_out = yaw_out; g.heli_servo_4.servo_out = yaw_out;
// use servo_out to calculate pwm_out and radio_out // use servo_out to calculate pwm_out and radio_out
@ -109,9 +101,9 @@ static void heli_move_swash(int roll_out, int pitch_out, int coll_out, int yaw_o
g.heli_servo_4.calc_pwm(); g.heli_servo_4.calc_pwm();
// add the servo values to the averaging // add the servo values to the averaging
heli_servo_out[0] += g.heli_servo_1.servo_out; heli_servo_out[0] += g.heli_servo_1.radio_out;
heli_servo_out[1] += g.heli_servo_2.servo_out; heli_servo_out[1] += g.heli_servo_2.radio_out;
heli_servo_out[2] += g.heli_servo_3.servo_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++;
@ -125,13 +117,13 @@ static void heli_move_swash(int roll_out, int pitch_out, int coll_out, int yaw_o
heli_servo_out[2] /= g.heli_servo_averaging; heli_servo_out[2] /= g.heli_servo_averaging;
heli_servo_out[3] /= g.heli_servo_averaging; heli_servo_out[3] /= g.heli_servo_averaging;
} }
// actually move the servos // actually move the servos
APM_RC.OutputCh(CH_1, heli_servo_out[0]); APM_RC.OutputCh(CH_1, heli_servo_out[0]);
APM_RC.OutputCh(CH_2, heli_servo_out[1]); APM_RC.OutputCh(CH_2, heli_servo_out[1]);
APM_RC.OutputCh(CH_3, heli_servo_out[2]); APM_RC.OutputCh(CH_3, heli_servo_out[2]);
APM_RC.OutputCh(CH_4, heli_servo_out[3]); APM_RC.OutputCh(CH_4, heli_servo_out[3]);
// output gyro value // output gyro value
if( g.heli_ext_gyro_enabled ) { if( g.heli_ext_gyro_enabled ) {
APM_RC.OutputCh(CH_7, g.heli_ext_gyro_gain); APM_RC.OutputCh(CH_7, g.heli_ext_gyro_gain);
@ -164,19 +156,21 @@ static void init_motors_out()
// these are not really motors, they're servos but we don't rename the function because it fits with the rest of the code better // these are not really motors, they're servos but we don't rename the function because it fits with the rest of the code better
static void output_motors_armed() static void output_motors_armed()
{ {
// if manual override (i.e. when setting up swash), pass pilot commands straight through to swash
if( g.heli_servo_manual == 1 ) {
g.rc_1.servo_out = g.rc_1.control_in;
g.rc_2.servo_out = g.rc_2.control_in;
g.rc_3.servo_out = g.rc_3.control_in;
g.rc_4.servo_out = g.rc_4.control_in;
}
//static int counter = 0; //static int counter = 0;
g.rc_1.calc_pwm(); g.rc_1.calc_pwm();
g.rc_2.calc_pwm(); g.rc_2.calc_pwm();
g.rc_3.calc_pwm(); g.rc_3.calc_pwm();
g.rc_4.calc_pwm(); g.rc_4.calc_pwm();
if( heli_manual_override ) { heli_move_swash( g.rc_1.servo_out, g.rc_2.servo_out, g.rc_3.radio_out, g.rc_4.servo_out );
// straight pass through from radio inputs to swash plate
heli_move_swash( g.rc_1.control_in, g.rc_2.control_in, g.rc_3.radio_in, g.rc_4.control_in );
}else{
// source inputs from attitude controller
heli_move_swash( g.rc_1.servo_out, g.rc_2.servo_out, g.rc_3.radio_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
@ -200,7 +194,7 @@ static void output_motor_test()
static int heli_get_scaled_throttle(int throttle) static int heli_get_scaled_throttle(int throttle)
{ {
float scaled_throttle = (g.heli_coll_min - 1000) + throttle * heli_throttle_scaler; float scaled_throttle = (g.heli_coll_min - 1000) + throttle * heli_throttle_scaler;
return g.heli_coll_min - 1000 + (throttle * heli_throttle_scaler); return scaled_throttle;
} }
// heli_angle_boost - takes servo_out position // heli_angle_boost - takes servo_out position

4
ArduCopter/radio.pde
View File

@ -9,8 +9,8 @@ static void default_dead_zones()
g.rc_1.set_dead_zone(60); g.rc_1.set_dead_zone(60);
g.rc_2.set_dead_zone(60); g.rc_2.set_dead_zone(60);
#if FRAME_CONFIG == HELI_FRAME #if FRAME_CONFIG == HELI_FRAME
g.rc_3.set_dead_zone(0); g.rc_3.set_dead_zone(20);
g.rc_4.set_dead_zone(60); g.rc_4.set_dead_zone(30);
#else #else
g.rc_3.set_dead_zone(60); g.rc_3.set_dead_zone(60);
g.rc_4.set_dead_zone(200); g.rc_4.set_dead_zone(200);

31
ArduCopter/setup.pde
View File

@ -469,8 +469,8 @@ setup_heli(uint8_t argc, const Menu::arg *argv)
// initialise swash plate // initialise swash plate
heli_init_swash(); heli_init_swash();
// source swash plate movements directly from // source swash plate movements directly from radio
heli_manual_override = true; g.heli_servo_manual = true;
// display initial settings // display initial settings
report_heli(); report_heli();
@ -494,6 +494,9 @@ setup_heli(uint8_t argc, const Menu::arg *argv)
// read radio although we don't use it yet // read radio although we don't use it yet
read_radio(); read_radio();
// allow swash plate to move
output_motors_armed();
// record min/max // record min/max
if( state == 1 ) { if( state == 1 ) {
@ -501,13 +504,12 @@ setup_heli(uint8_t argc, const Menu::arg *argv)
max_roll = abs(g.rc_1.control_in); max_roll = abs(g.rc_1.control_in);
if( abs(g.rc_2.control_in) > max_pitch ) if( abs(g.rc_2.control_in) > max_pitch )
max_pitch = abs(g.rc_2.control_in); max_pitch = abs(g.rc_2.control_in);
if( g.rc_3.radio_in < min_coll ) if( g.rc_3.radio_out < min_coll )
min_coll = g.rc_3.radio_in; min_coll = g.rc_3.radio_out;
if( g.rc_3.radio_in > max_coll ) if( g.rc_3.radio_out > max_coll )
max_coll = g.rc_3.radio_in; max_coll = g.rc_3.radio_out;
min_tail = min(g.rc_4.radio_in, min_tail); min_tail = min(g.rc_4.radio_out, min_tail);
max_tail = max(g.rc_4.radio_in, max_tail); max_tail = max(g.rc_4.radio_out, max_tail);
//Serial.printf_P(PSTR("4: ri:%d \tro:%d \tso:%d \n"), (int)g.rc_4.radio_in, (int)g.rc_4.radio_out, (int)g.rc_4.servo_out);
} }
if( Serial.available() ) { if( Serial.available() ) {
@ -533,8 +535,8 @@ setup_heli(uint8_t argc, const Menu::arg *argv)
break; break;
case 'c': case 'c':
case 'C': case 'C':
if( g.rc_3.radio_in >= 900 && g.rc_3.radio_in <= 2100 ) { if( g.rc_3.radio_out >= 900 && g.rc_3.radio_out <= 2100 ) {
g.heli_coll_mid = g.rc_3.radio_in; g.heli_coll_mid = g.rc_3.radio_out;
Serial.printf_P(PSTR("Collective when blade pitch at zero: %d\n"),(int)g.heli_coll_mid); Serial.printf_P(PSTR("Collective when blade pitch at zero: %d\n"),(int)g.heli_coll_mid);
} }
break; break;
@ -561,7 +563,7 @@ setup_heli(uint8_t argc, const Menu::arg *argv)
max_pitch = abs(g.rc_2.control_in); max_pitch = abs(g.rc_2.control_in);
min_coll = 2000; min_coll = 2000;
max_coll = 1000; max_coll = 1000;
min_tail = max_tail = abs(g.rc_4.radio_in); min_tail = max_tail = abs(g.rc_4.radio_out);
}else{ }else{
state = 0; // switch back to normal mode state = 0; // switch back to normal mode
// double check values aren't totally terrible // double check values aren't totally terrible
@ -639,9 +641,6 @@ setup_heli(uint8_t argc, const Menu::arg *argv)
} }
} }
// allow swash plate to move
output_motors_armed();
delay(20); delay(20);
} }
@ -664,7 +663,7 @@ setup_heli(uint8_t argc, const Menu::arg *argv)
g.heli_servo_averaging.save(); g.heli_servo_averaging.save();
// return swash plate movements to attitude controller // return swash plate movements to attitude controller
heli_manual_override = false; g.heli_servo_manual = false;
return(0); return(0);
} }

1
ArduCopter/system.pde
View File

@ -178,6 +178,7 @@ static void init_ardupilot()
#endif #endif
#if FRAME_CONFIG == HELI_FRAME #if FRAME_CONFIG == HELI_FRAME
g.heli_servo_manual = false;
heli_init_swash(); // heli initialisation heli_init_swash(); // heli initialisation
#endif #endif