mirror of https://github.com/ArduPilot/ardupilot
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
parent
cd89258294
commit
b55db247ca
|
|
@ -78,7 +78,7 @@ public:
|
|||
k_param_heli_servo_averaging,
|
||||
k_param_heli_servo_manual,
|
||||
k_param_heli_phase_angle,
|
||||
k_param_heli_coll_yaw_effect, // 97
|
||||
k_param_heli_collective_yaw_effect, // 97
|
||||
#endif
|
||||
|
||||
// 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
|
||||
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_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_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_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_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
|
||||
|
||||
// RC channels
|
||||
|
|
@ -377,24 +377,20 @@ public:
|
|||
auto_slew_rate (AUTO_SLEW_RATE),
|
||||
|
||||
#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_servo2_pos (60),
|
||||
heli_servo3_pos (180),
|
||||
heli_roll_max (4500),
|
||||
heli_pitch_max (4500),
|
||||
heli_coll_min (1250),
|
||||
heli_coll_max (1750),
|
||||
heli_coll_mid (1500),
|
||||
heli_collective_min (1250),
|
||||
heli_collective_max (1750),
|
||||
heli_collective_mid (1500),
|
||||
heli_ext_gyro_enabled (0),
|
||||
heli_ext_gyro_gain (1350),
|
||||
heli_servo_averaging (0),
|
||||
heli_servo_manual (0),
|
||||
heli_phase_angle (0),
|
||||
heli_coll_yaw_effect (0),
|
||||
heli_collective_yaw_effect (0),
|
||||
#endif
|
||||
|
||||
camera_pitch_gain (CAM_PITCH_GAIN),
|
||||
|
|
|
|||
|
|
@ -72,24 +72,24 @@ static const AP_Param::Info var_info[] PROGMEM = {
|
|||
GSCALAR(auto_slew_rate, "AUTO_SLEW"),
|
||||
|
||||
#if FRAME_CONFIG == HELI_FRAME
|
||||
GSCALAR(heli_servo_1, "HS1_"),
|
||||
GSCALAR(heli_servo_2, "HS2_"),
|
||||
GSCALAR(heli_servo_3, "HS3_"),
|
||||
GSCALAR(heli_servo_4, "HS4_"),
|
||||
GGROUP(heli_servo_1, "HS1_", RC_Channel),
|
||||
GGROUP(heli_servo_2, "HS2_", RC_Channel),
|
||||
GGROUP(heli_servo_3, "HS3_", RC_Channel),
|
||||
GGROUP(heli_servo_4, "HS4_", RC_Channel),
|
||||
GSCALAR(heli_servo1_pos, "SV1_POS_"),
|
||||
GSCALAR(heli_servo2_pos, "SV2_POS_"),
|
||||
GSCALAR(heli_servo3_pos, "SV3_POS_"),
|
||||
GSCALAR(heli_roll_max, "ROL_MAX_"),
|
||||
GSCALAR(heli_pitch_max, "PIT_MAX_"),
|
||||
GSCALAR(heli_coll_min, "COL_MIN_"),
|
||||
GSCALAR(heli_coll_max, "COL_MAX_"),
|
||||
GSCALAR(heli_coll_mid, "COL_MID_"),
|
||||
GSCALAR(heli_collective_min, "COL_MIN_"),
|
||||
GSCALAR(heli_collective_max, "COL_MAX_"),
|
||||
GSCALAR(heli_collective_mid, "COL_MID_"),
|
||||
GSCALAR(heli_ext_gyro_enabled, "GYR_ENABLE_"),
|
||||
GSCALAR(heli_ext_gyro_gain, "GYR_GAIN_"),
|
||||
GSCALAR(heli_servo_averaging, "SV_AVG"),
|
||||
GSCALAR(heli_servo_manual, "HSV_MAN"),
|
||||
GSCALAR(heli_phase_angle, "H_PHANG"),
|
||||
GSCALAR(heli_coll_yaw_effect, "H_COLYAW"),
|
||||
GSCALAR(heli_collective_yaw_effect, "H_COLYAW"),
|
||||
#endif
|
||||
|
||||
// RC channel
|
||||
|
|
|
|||
|
|
@ -7,7 +7,7 @@
|
|||
|
||||
static bool heli_swash_initialised = false;
|
||||
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:
|
||||
// 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));
|
||||
|
||||
// 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
|
||||
heli_swash_initialised = false;
|
||||
|
|
@ -57,17 +57,17 @@ static void heli_init_swash()
|
|||
g.heli_servo_4.set_angle(4500);
|
||||
|
||||
// ensure g.heli_coll values are reasonable
|
||||
if( g.heli_coll_min >= g.heli_coll_max ) {
|
||||
g.heli_coll_min = 1000;
|
||||
g.heli_coll_max = 2000;
|
||||
if( g.heli_collective_min >= g.heli_collective_max ) {
|
||||
g.heli_collective_min = 1000;
|
||||
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
|
||||
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
|
||||
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
|
||||
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 ) {
|
||||
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
|
||||
|
||||
// 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);
|
||||
pitch_out = constrain(pitch_out, (int)-g.heli_pitch_max, (int)g.heli_pitch_max);
|
||||
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
|
||||
#if HIL_MODE == HIL_MODE_DISABLED // don't do rudder feed forward in simulator
|
||||
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
|
||||
}
|
||||
|
|
|
|||
|
|
@ -457,7 +457,7 @@ setup_heli(uint8_t argc, const Menu::arg *argv)
|
|||
int value = 0;
|
||||
int temp;
|
||||
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
|
||||
heli_init_swash();
|
||||
|
|
@ -497,10 +497,10 @@ setup_heli(uint8_t argc, const Menu::arg *argv)
|
|||
max_roll = abs(g.rc_1.control_in);
|
||||
if( abs(g.rc_2.control_in) > max_pitch )
|
||||
max_pitch = abs(g.rc_2.control_in);
|
||||
if( g.rc_3.radio_out < min_coll )
|
||||
min_coll = g.rc_3.radio_out;
|
||||
if( g.rc_3.radio_out > max_coll )
|
||||
max_coll = g.rc_3.radio_out;
|
||||
if( g.rc_3.radio_out < min_collective )
|
||||
min_collective = g.rc_3.radio_out;
|
||||
if( g.rc_3.radio_out > max_collective )
|
||||
max_collective = g.rc_3.radio_out;
|
||||
min_tail = min(g.rc_4.radio_out, min_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':
|
||||
if( g.rc_3.radio_out >= 900 && g.rc_3.radio_out <= 2100 ) {
|
||||
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);
|
||||
g.heli_collective_mid = g.rc_3.radio_out;
|
||||
Serial.printf_P(PSTR("Collective when blade pitch at zero: %d\n"),(int)g.heli_collective_mid);
|
||||
}
|
||||
break;
|
||||
case 'd':
|
||||
|
|
@ -546,27 +546,27 @@ setup_heli(uint8_t argc, const Menu::arg *argv)
|
|||
|
||||
// reset servo ranges
|
||||
g.heli_roll_max = g.heli_pitch_max = 4500;
|
||||
g.heli_coll_min = 1000;
|
||||
g.heli_coll_max = 2000;
|
||||
g.heli_collective_min = 1000;
|
||||
g.heli_collective_max = 2000;
|
||||
g.heli_servo_4.radio_min = 1000;
|
||||
g.heli_servo_4.radio_max = 2000;
|
||||
|
||||
// set sensible values in temp variables
|
||||
max_roll = abs(g.rc_1.control_in);
|
||||
max_pitch = abs(g.rc_2.control_in);
|
||||
min_coll = 2000;
|
||||
max_coll = 1000;
|
||||
min_collective = 2000;
|
||||
max_collective = 1000;
|
||||
min_tail = max_tail = abs(g.rc_4.radio_out);
|
||||
}else{
|
||||
state = 0; // switch back to normal mode
|
||||
// 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 )
|
||||
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);
|
||||
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_collective,max_collective,min_tail,max_tail);
|
||||
else{
|
||||
g.heli_roll_max = max_roll;
|
||||
g.heli_pitch_max = max_pitch;
|
||||
g.heli_coll_min = min_coll;
|
||||
g.heli_coll_max = max_coll;
|
||||
g.heli_collective_min = min_collective;
|
||||
g.heli_collective_max = max_collective;
|
||||
g.heli_servo_4.radio_min = min_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_roll_max.save();
|
||||
g.heli_pitch_max.save();
|
||||
g.heli_coll_min.save();
|
||||
g.heli_coll_max.save();
|
||||
g.heli_coll_mid.save();
|
||||
g.heli_collective_min.save();
|
||||
g.heli_collective_max.save();
|
||||
g.heli_collective_mid.save();
|
||||
g.heli_servo_averaging.save();
|
||||
|
||||
// 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("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
|
||||
if( g.heli_servo_averaging <= 1 ) {
|
||||
|
|
|
|||
Loading…
Reference in New Issue