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

TradHeli - fixed small parameter bug affecting helis. The servo parameters were declared as GSCALAR instead of GGROUP in Parameters.pde. 

also renamed heli parameters from _coll_ to the more descriptive _collective_
This commit is contained in:
Randy Mackay 2012-02-18 20:57:51 +09:00
parent cd89258294
commit b55db247ca
4 changed files with 45 additions and 49 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
ArduCopter/Parameters.h
View File

@ -78,7 +78,7 @@ public:
k_param_heli_servo_averaging, k_param_heli_servo_averaging,
k_param_heli_servo_manual, k_param_heli_servo_manual,
k_param_heli_phase_angle, k_param_heli_phase_angle,
k_param_heli_coll_yaw_effect, // 97 k_param_heli_collective_yaw_effect, // 97
#endif #endif
// 110: Telemetry control // 110: Telemetry control
@ -268,13 +268,13 @@ public:
RC_Channel heli_servo_1, heli_servo_2, heli_servo_3, heli_servo_4; // servos for swash plate and tail RC_Channel heli_servo_1, heli_servo_2, heli_servo_3, heli_servo_4; // servos for swash plate and tail
AP_Int16 heli_servo1_pos, heli_servo2_pos, heli_servo3_pos; // servo positions (3 because we don't need pos for tail servo) AP_Int16 heli_servo1_pos, heli_servo2_pos, heli_servo3_pos; // servo positions (3 because we don't need pos for tail servo)
AP_Int16 heli_roll_max, heli_pitch_max; // maximum allowed roll and pitch of swashplate AP_Int16 heli_roll_max, heli_pitch_max; // maximum allowed roll and pitch of swashplate
AP_Int16 heli_coll_min, heli_coll_max, heli_coll_mid; // min and max collective. mid = main blades at zero pitch AP_Int16 heli_collective_min, heli_collective_max, heli_collective_mid; // min and max collective. mid = main blades at zero pitch
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 AP_Int8 heli_servo_manual; // 0 = normal mode, 1 = radio inputs directly control swash. required for swash set-up
AP_Int16 heli_phase_angle; // 0 to 360 degrees. specifies mixing between roll and pitch for helis AP_Int16 heli_phase_angle; // 0 to 360 degrees. specifies mixing between roll and pitch for helis
AP_Float heli_coll_yaw_effect; // -5.0 ~ 5.0. Feed forward control from collective to yaw. 1.0 = move rudder right 1% for every 1% of collective above the mid point AP_Float heli_collective_yaw_effect; // -5.0 ~ 5.0. Feed forward control from collective to yaw. 1.0 = move rudder right 1% for every 1% of collective above the mid point
#endif #endif
// RC channels // RC channels
@ -377,24 +377,20 @@ public:
auto_slew_rate (AUTO_SLEW_RATE), auto_slew_rate (AUTO_SLEW_RATE),
#if FRAME_CONFIG == HELI_FRAME #if FRAME_CONFIG == HELI_FRAME
heli_servo_1 (k_param_heli_servo_1),
heli_servo_2 (k_param_heli_servo_2),
heli_servo_3 (k_param_heli_servo_3),
heli_servo_4 (k_param_heli_servo_4),
heli_servo1_pos (-60), heli_servo1_pos (-60),
heli_servo2_pos (60), heli_servo2_pos (60),
heli_servo3_pos (180), heli_servo3_pos (180),
heli_roll_max (4500), heli_roll_max (4500),
heli_pitch_max (4500), heli_pitch_max (4500),
heli_coll_min (1250), heli_collective_min (1250),
heli_coll_max (1750), heli_collective_max (1750),
heli_coll_mid (1500), heli_collective_mid (1500),
heli_ext_gyro_enabled (0), heli_ext_gyro_enabled (0),
heli_ext_gyro_gain (1350), heli_ext_gyro_gain (1350),
heli_servo_averaging (0), heli_servo_averaging (0),
heli_servo_manual (0), heli_servo_manual (0),
heli_phase_angle (0), heli_phase_angle (0),
heli_coll_yaw_effect (0), heli_collective_yaw_effect (0),
#endif #endif
camera_pitch_gain (CAM_PITCH_GAIN), camera_pitch_gain (CAM_PITCH_GAIN),

16
ArduCopter/Parameters.pde
View File

@ -72,24 +72,24 @@ static const AP_Param::Info var_info[] PROGMEM = {
GSCALAR(auto_slew_rate, "AUTO_SLEW"), GSCALAR(auto_slew_rate, "AUTO_SLEW"),
#if FRAME_CONFIG == HELI_FRAME #if FRAME_CONFIG == HELI_FRAME
GSCALAR(heli_servo_1, "HS1_"), GGROUP(heli_servo_1, "HS1_", RC_Channel),
GSCALAR(heli_servo_2, "HS2_"), GGROUP(heli_servo_2, "HS2_", RC_Channel),
GSCALAR(heli_servo_3, "HS3_"), GGROUP(heli_servo_3, "HS3_", RC_Channel),
GSCALAR(heli_servo_4, "HS4_"), GGROUP(heli_servo_4, "HS4_", RC_Channel),
GSCALAR(heli_servo1_pos, "SV1_POS_"), GSCALAR(heli_servo1_pos, "SV1_POS_"),
GSCALAR(heli_servo2_pos, "SV2_POS_"), GSCALAR(heli_servo2_pos, "SV2_POS_"),
GSCALAR(heli_servo3_pos, "SV3_POS_"), GSCALAR(heli_servo3_pos, "SV3_POS_"),
GSCALAR(heli_roll_max, "ROL_MAX_"), GSCALAR(heli_roll_max, "ROL_MAX_"),
GSCALAR(heli_pitch_max, "PIT_MAX_"), GSCALAR(heli_pitch_max, "PIT_MAX_"),
GSCALAR(heli_coll_min, "COL_MIN_"), GSCALAR(heli_collective_min, "COL_MIN_"),
GSCALAR(heli_coll_max, "COL_MAX_"), GSCALAR(heli_collective_max, "COL_MAX_"),
GSCALAR(heli_coll_mid, "COL_MID_"), GSCALAR(heli_collective_mid, "COL_MID_"),
GSCALAR(heli_ext_gyro_enabled, "GYR_ENABLE_"), GSCALAR(heli_ext_gyro_enabled, "GYR_ENABLE_"),
GSCALAR(heli_ext_gyro_gain, "GYR_GAIN_"), GSCALAR(heli_ext_gyro_gain, "GYR_GAIN_"),
GSCALAR(heli_servo_averaging, "SV_AVG"), GSCALAR(heli_servo_averaging, "SV_AVG"),
GSCALAR(heli_servo_manual, "HSV_MAN"), GSCALAR(heli_servo_manual, "HSV_MAN"),
GSCALAR(heli_phase_angle, "H_PHANG"), GSCALAR(heli_phase_angle, "H_PHANG"),
GSCALAR(heli_coll_yaw_effect, "H_COLYAW"), GSCALAR(heli_collective_yaw_effect, "H_COLYAW"),
#endif #endif
// RC channel // RC channel

22
ArduCopter/heli.pde
View File

@ -7,7 +7,7 @@
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) static int heli_throttle_mid = 0; // throttle mid point in pwm form (i.e. 0 ~ 1000)
static float heli_coll_scalar = 1; // throttle scalar to convert pwm form (i.e. 0 ~ 1000) passed in to actual servo range (i.e 1250~1750 would be 500) static float heli_collective_scalar = 1; // throttle scalar to convert pwm form (i.e. 0 ~ 1000) passed in to actual servo range (i.e 1250~1750 would be 500)
// heli_servo_averaging: // heli_servo_averaging:
// 0 or 1 = no averaging, 250hz // 0 or 1 = no averaging, 250hz
@ -39,7 +39,7 @@ static void heli_reset_swash()
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));
// set throttle scaling // set throttle scaling
heli_coll_scalar = ((float)(g.rc_3.radio_max - g.rc_3.radio_min))/1000.0; heli_collective_scalar = ((float)(g.rc_3.radio_max - g.rc_3.radio_min))/1000.0;
// 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;
@ -57,17 +57,17 @@ static void heli_init_swash()
g.heli_servo_4.set_angle(4500); g.heli_servo_4.set_angle(4500);
// ensure g.heli_coll values are reasonable // ensure g.heli_coll values are reasonable
if( g.heli_coll_min >= g.heli_coll_max ) { if( g.heli_collective_min >= g.heli_collective_max ) {
g.heli_coll_min = 1000; g.heli_collective_min = 1000;
g.heli_coll_max = 2000; g.heli_collective_max = 2000;
} }
g.heli_coll_mid = constrain(g.heli_coll_mid, g.heli_coll_min, g.heli_coll_max); g.heli_collective_mid = constrain(g.heli_collective_mid, g.heli_collective_min, g.heli_collective_max);
// calculate throttle mid point // calculate throttle mid point
heli_throttle_mid = ((float)(g.heli_coll_mid-g.heli_coll_min))/((float)(g.heli_coll_max-g.heli_coll_min))*1000.0; heli_throttle_mid = ((float)(g.heli_collective_mid-g.heli_collective_min))/((float)(g.heli_collective_max-g.heli_collective_min))*1000.0;
// determine scalar throttle input // determine scalar throttle input
heli_coll_scalar = ((float)(g.heli_coll_max-g.heli_coll_min))/1000.0; heli_collective_scalar = ((float)(g.heli_collective_max-g.heli_collective_min))/1000.0;
// pitch factors // pitch factors
heli_pitchFactor[CH_1] = cos(radians(g.heli_servo1_pos - g.heli_phase_angle)); heli_pitchFactor[CH_1] = cos(radians(g.heli_servo1_pos - g.heli_phase_angle));
@ -128,7 +128,7 @@ static void heli_move_swash(int roll_out, int pitch_out, int coll_out, int yaw_o
if( heli_swash_initialised ) { if( heli_swash_initialised ) {
heli_reset_swash(); heli_reset_swash();
} }
coll_out_scaled = coll_out * heli_coll_scalar + g.rc_3.radio_min - 1000; coll_out_scaled = coll_out * heli_collective_scalar + g.rc_3.radio_min - 1000;
}else{ // regular flight mode }else{ // regular flight mode
// check if we need to reinitialise the swash // check if we need to reinitialise the swash
@ -140,12 +140,12 @@ static void heli_move_swash(int roll_out, int pitch_out, int coll_out, int yaw_o
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, 0, 1000); coll_out = constrain(coll_out, 0, 1000);
coll_out_scaled = coll_out * heli_coll_scalar + g.heli_coll_min - 1000; coll_out_scaled = coll_out * heli_collective_scalar + g.heli_collective_min - 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
if( !g.heli_ext_gyro_enabled ) { if( !g.heli_ext_gyro_enabled ) {
yaw_offset = g.heli_coll_yaw_effect * abs(coll_out_scaled - g.heli_coll_mid); yaw_offset = g.heli_collective_yaw_effect * abs(coll_out_scaled - g.heli_collective_mid);
} }
#endif #endif
} }

38
ArduCopter/setup.pde
View File

@ -457,7 +457,7 @@ setup_heli(uint8_t argc, const Menu::arg *argv)
int value = 0; int value = 0;
int temp; int temp;
int state = 0; // 0 = set rev+pos, 1 = capture min/max int state = 0; // 0 = set rev+pos, 1 = capture min/max
int max_roll, max_pitch, min_coll, max_coll, min_tail, max_tail; int max_roll, max_pitch, min_collective, max_collective, min_tail, max_tail;
// initialise swash plate // initialise swash plate
heli_init_swash(); heli_init_swash();
@ -497,10 +497,10 @@ 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_out < min_coll ) if( g.rc_3.radio_out < min_collective )
min_coll = g.rc_3.radio_out; min_collective = g.rc_3.radio_out;
if( g.rc_3.radio_out > max_coll ) if( g.rc_3.radio_out > max_collective )
max_coll = g.rc_3.radio_out; max_collective = g.rc_3.radio_out;
min_tail = min(g.rc_4.radio_out, min_tail); min_tail = min(g.rc_4.radio_out, min_tail);
max_tail = max(g.rc_4.radio_out, max_tail); max_tail = max(g.rc_4.radio_out, max_tail);
} }
@ -529,8 +529,8 @@ setup_heli(uint8_t argc, const Menu::arg *argv)
case 'c': case 'c':
case 'C': case 'C':
if( g.rc_3.radio_out >= 900 && g.rc_3.radio_out <= 2100 ) { if( g.rc_3.radio_out >= 900 && g.rc_3.radio_out <= 2100 ) {
g.heli_coll_mid = g.rc_3.radio_out; g.heli_collective_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_collective_mid);
} }
break; break;
case 'd': case 'd':
@ -546,27 +546,27 @@ setup_heli(uint8_t argc, const Menu::arg *argv)
// reset servo ranges // reset servo ranges
g.heli_roll_max = g.heli_pitch_max = 4500; g.heli_roll_max = g.heli_pitch_max = 4500;
g.heli_coll_min = 1000; g.heli_collective_min = 1000;
g.heli_coll_max = 2000; g.heli_collective_max = 2000;
g.heli_servo_4.radio_min = 1000; g.heli_servo_4.radio_min = 1000;
g.heli_servo_4.radio_max = 2000; g.heli_servo_4.radio_max = 2000;
// set sensible values in temp variables // set sensible values in temp variables
max_roll = abs(g.rc_1.control_in); max_roll = abs(g.rc_1.control_in);
max_pitch = abs(g.rc_2.control_in); max_pitch = abs(g.rc_2.control_in);
min_coll = 2000; min_collective = 2000;
max_coll = 1000; max_collective = 1000;
min_tail = max_tail = abs(g.rc_4.radio_out); 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
if( max_roll <= 1000 || max_pitch <= 1000 || (max_coll - min_coll < 200) || (max_tail - min_tail < 200) || min_tail < 1000 || max_tail > 2000 ) if( max_roll <= 1000 || max_pitch <= 1000 || (max_collective - min_collective < 200) || (max_tail - min_tail < 200) || min_tail < 1000 || max_tail > 2000 )
Serial.printf_P(PSTR("Invalid min/max captured roll:%d, pitch:%d, collective min: %d max: %d, tail min:%d max:%d\n"),max_roll,max_pitch,min_coll,max_coll,min_tail,max_tail); Serial.printf_P(PSTR("Invalid min/max captured roll:%d, pitch:%d, collective min: %d max: %d, tail min:%d max:%d\n"),max_roll,max_pitch,min_collective,max_collective,min_tail,max_tail);
else{ else{
g.heli_roll_max = max_roll; g.heli_roll_max = max_roll;
g.heli_pitch_max = max_pitch; g.heli_pitch_max = max_pitch;
g.heli_coll_min = min_coll; g.heli_collective_min = min_collective;
g.heli_coll_max = max_coll; g.heli_collective_max = max_collective;
g.heli_servo_4.radio_min = min_tail; g.heli_servo_4.radio_min = min_tail;
g.heli_servo_4.radio_max = max_tail; g.heli_servo_4.radio_max = max_tail;
@ -650,9 +650,9 @@ setup_heli(uint8_t argc, const Menu::arg *argv)
g.heli_servo3_pos.save(); g.heli_servo3_pos.save();
g.heli_roll_max.save(); g.heli_roll_max.save();
g.heli_pitch_max.save(); g.heli_pitch_max.save();
g.heli_coll_min.save(); g.heli_collective_min.save();
g.heli_coll_max.save(); g.heli_collective_max.save();
g.heli_coll_mid.save(); g.heli_collective_mid.save();
g.heli_servo_averaging.save(); g.heli_servo_averaging.save();
// return swash plate movements to attitude controller // return swash plate movements to attitude controller
@ -942,7 +942,7 @@ static void report_heli()
Serial.printf_P(PSTR("roll max: \t%d\n"), (int)g.heli_roll_max); Serial.printf_P(PSTR("roll max: \t%d\n"), (int)g.heli_roll_max);
Serial.printf_P(PSTR("pitch max: \t%d\n"), (int)g.heli_pitch_max); Serial.printf_P(PSTR("pitch max: \t%d\n"), (int)g.heli_pitch_max);
Serial.printf_P(PSTR("coll min:\t%d\t mid:%d\t max:%d\n"),(int)g.heli_coll_min, (int)g.heli_coll_mid, (int)g.heli_coll_max); Serial.printf_P(PSTR("coll min:\t%d\t mid:%d\t max:%d\n"),(int)g.heli_collective_min, (int)g.heli_collective_mid, (int)g.heli_collective_max);
// calculate and print servo rate // calculate and print servo rate
if( g.heli_servo_averaging <= 1 ) { if( g.heli_servo_averaging <= 1 ) {