mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-10 18:08:30 -04:00
TradHeli - fixed roll/pitch so it's not scaled when throttle is scaled
This commit is contained in:
Randy Mackay
parent
6dc236b8f2
commit
68e6be5eeb
@ -9,6 +9,17 @@ get_stabilize_roll(int32_t target_angle)
|
||||
// angle error
|
||||
error = wrap_180(target_angle - dcm.roll_sensor);
|
||||
|
||||
#if FRAME_CONFIG == HELI_FRAME
|
||||
// limit the error we're feeding to the PID
|
||||
error = constrain(error, -4500, 4500);
|
||||
|
||||
// convert to desired Rate:
|
||||
rate = g.pi_stabilize_roll.get_pi(error, G_Dt);
|
||||
|
||||
// output control:
|
||||
rate = constrain(rate, -4500, 4500);
|
||||
return (int)rate;
|
||||
#else
|
||||
// limit the error we're feeding to the PID
|
||||
error = constrain(error, -2500, 2500);
|
||||
|
||||
@ -17,15 +28,15 @@ get_stabilize_roll(int32_t target_angle)
|
||||
|
||||
// experiment to pipe iterm directly into the output
|
||||
int16_t iterm = g.pi_stabilize_roll.get_i(error, G_Dt);
|
||||
|
||||
#if FRAME_CONFIG != HELI_FRAME // cannot use rate control for helicopters
|
||||
|
||||
// rate control
|
||||
error = rate - (omega.x * DEGX100);
|
||||
rate = g.pi_rate_roll.get_pi(error, G_Dt);
|
||||
#endif
|
||||
|
||||
// output control:
|
||||
rate = constrain(rate, -2500, 2500);
|
||||
return (int)rate + iterm;
|
||||
#endif
|
||||
}
|
||||
|
||||
static int
|
||||
@ -37,7 +48,18 @@ get_stabilize_pitch(int32_t target_angle)
|
||||
// angle error
|
||||
error = wrap_180(target_angle - dcm.pitch_sensor);
|
||||
|
||||
#if FRAME_CONFIG == HELI_FRAME
|
||||
// limit the error we're feeding to the PID
|
||||
error = constrain(error, -4500, 4500);
|
||||
|
||||
// convert to desired Rate:
|
||||
rate = g.pi_stabilize_pitch.get_pi(error, G_Dt);
|
||||
|
||||
// output control:
|
||||
rate = constrain(rate, -4500, 4500);
|
||||
return (int)rate;
|
||||
#else
|
||||
// angle error
|
||||
error = constrain(error, -2500, 2500);
|
||||
|
||||
// convert to desired Rate:
|
||||
@ -46,14 +68,13 @@ get_stabilize_pitch(int32_t target_angle)
|
||||
// experiment to pipe iterm directly into the output
|
||||
int16_t iterm = g.pi_stabilize_roll.get_i(error, G_Dt);
|
||||
|
||||
#if FRAME_CONFIG != HELI_FRAME // cannot use rate control for helicopters
|
||||
error = rate - (omega.y * DEGX100);
|
||||
rate = g.pi_rate_pitch.get_pi(error, G_Dt);
|
||||
#endif
|
||||
|
||||
// output control:
|
||||
rate = constrain(rate, -2500, 2500);
|
||||
return (int)rate + iterm;
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
@ -70,21 +91,24 @@ get_stabilize_yaw(int32_t target_angle)
|
||||
// limit the error we're feeding to the PID
|
||||
error = constrain(error, -YAW_ERROR_MAX, YAW_ERROR_MAX);
|
||||
|
||||
// convert to desired Rate:
|
||||
rate = g.pi_stabilize_yaw.get_p(error);
|
||||
|
||||
// experiment to pipe iterm directly into the output
|
||||
int16_t iterm = g.pi_stabilize_yaw.get_i(error, G_Dt);
|
||||
|
||||
#if FRAME_CONFIG == HELI_FRAME // cannot use rate control for helicopters
|
||||
// convert to desired Rate:
|
||||
rate = g.pi_stabilize_yaw.get_pi(error, G_Dt);
|
||||
|
||||
if(!g.heli_ext_gyro_enabled ) {
|
||||
error = rate - (degrees(omega.z) * 100.0);
|
||||
rate = g.pi_rate_yaw.get_pi(error, G_Dt);
|
||||
}
|
||||
// output control:
|
||||
rate = constrain(rate, -4500, 4500);
|
||||
return (int)rate + iterm;
|
||||
return (int)rate;
|
||||
#else
|
||||
// convert to desired Rate:
|
||||
rate = g.pi_stabilize_yaw.get_p(error);
|
||||
|
||||
// experiment to pipe iterm directly into the output
|
||||
int16_t iterm = g.pi_stabilize_yaw.get_i(error, G_Dt);
|
||||
|
||||
error = rate - (omega.z * DEGX100);;
|
||||
rate = g.pi_rate_yaw.get_pi(error, G_Dt);
|
||||
|
||||
@ -308,8 +332,4 @@ static int get_z_damping()
|
||||
return 0;
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
|
||||
#endif
|
||||
@ -16,16 +16,32 @@ static int heli_throttle_mid = 0; // throttle mid point in pwm form (i.e. 0 ~ 1
|
||||
// 5 = averaging 5 samples = 50hz
|
||||
// digital = 0 / 250hz, analog = 2 / 83.3
|
||||
|
||||
static void heli_init_swash()
|
||||
// reset swash for maximum movements - used for set-up
|
||||
static void heli_reset_swash()
|
||||
{
|
||||
int i;
|
||||
|
||||
// swash servo initialisation
|
||||
g.heli_servo_1.set_range(0,1000);
|
||||
g.heli_servo_2.set_range(0,1000);
|
||||
g.heli_servo_3.set_range(0,1000);
|
||||
g.heli_servo_4.set_angle(4500);
|
||||
|
||||
// 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);
|
||||
}
|
||||
|
||||
// pitch factors
|
||||
heli_pitchFactor[CH_1] = cos(radians(g.heli_servo1_pos - g.heli_phase_angle));
|
||||
heli_pitchFactor[CH_2] = cos(radians(g.heli_servo2_pos - g.heli_phase_angle));
|
||||
@ -35,7 +51,23 @@ static void heli_init_swash()
|
||||
heli_rollFactor[CH_1] = cos(radians(g.heli_servo1_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));
|
||||
|
||||
// we must be in set-up mode so mark swash as uninitialised
|
||||
heli_swash_initialised = false;
|
||||
}
|
||||
|
||||
// initialise the swash
|
||||
static void heli_init_swash()
|
||||
{
|
||||
int i;
|
||||
float coll_range_comp = 1; // factor to negate collective range's effect on roll & pitch range
|
||||
|
||||
// swash servo initialisation
|
||||
g.heli_servo_1.set_range(0,1000);
|
||||
g.heli_servo_2.set_range(0,1000);
|
||||
g.heli_servo_3.set_range(0,1000);
|
||||
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;
|
||||
@ -43,6 +75,23 @@ static void heli_init_swash()
|
||||
}
|
||||
g.heli_coll_mid = constrain(g.heli_coll_mid, g.heli_coll_min, g.heli_coll_max);
|
||||
|
||||
// calculate compensation for collective range on roll & pitch range
|
||||
if( g.heli_coll_max - g.heli_coll_min > 100 )
|
||||
coll_range_comp = 1000 / (g.heli_coll_max - g.heli_coll_min);
|
||||
|
||||
// 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));
|
||||
|
||||
// pitch factors
|
||||
heli_pitchFactor[CH_1] = cos(radians(g.heli_servo1_pos - g.heli_phase_angle)) * coll_range_comp;
|
||||
heli_pitchFactor[CH_2] = cos(radians(g.heli_servo2_pos - g.heli_phase_angle)) * coll_range_comp;
|
||||
heli_pitchFactor[CH_3] = cos(radians(g.heli_servo3_pos - g.heli_phase_angle)) * coll_range_comp;
|
||||
|
||||
// roll factors
|
||||
heli_rollFactor[CH_1] = cos(radians(g.heli_servo1_pos + 90 - g.heli_phase_angle)) * coll_range_comp;
|
||||
heli_rollFactor[CH_2] = cos(radians(g.heli_servo2_pos + 90 - g.heli_phase_angle)) * coll_range_comp;
|
||||
heli_rollFactor[CH_3] = cos(radians(g.heli_servo3_pos + 90 - g.heli_phase_angle)) * coll_range_comp;
|
||||
|
||||
// servo min/max values
|
||||
if( g.heli_servo_1.get_reverse() ) {
|
||||
g.heli_servo_1.radio_min = 3000 - g.heli_coll_max + (g.heli_servo_1.radio_trim-1500);
|
||||
@ -66,15 +115,12 @@ static void heli_init_swash()
|
||||
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
|
||||
for( i=0; i<=3; i++ )
|
||||
heli_servo_out[i] = 0;
|
||||
|
||||
// double check heli_servo_averaging is reasonable
|
||||
if( g.heli_servo_averaging < 0 || g.heli_servo_averaging < 0 > 5 ) {
|
||||
if( g.heli_servo_averaging < 0 || g.heli_servo_averaging > 5 ) {
|
||||
g.heli_servo_averaging = 0;
|
||||
g.heli_servo_averaging.save();
|
||||
}
|
||||
@ -85,7 +131,11 @@ static void heli_init_swash()
|
||||
|
||||
static void heli_move_servos_to_mid()
|
||||
{
|
||||
heli_move_swash(0,0,500,0);
|
||||
// call multiple times to force through the servo averaging
|
||||
for( int i=0; i<5; i++ ) {
|
||||
heli_move_swash(0,0,500,0);
|
||||
delay(20);
|
||||
}
|
||||
}
|
||||
|
||||
//
|
||||
@ -101,33 +151,10 @@ static void heli_move_swash(int roll_out, int pitch_out, int coll_out, int yaw_o
|
||||
int yaw_offset = 0;
|
||||
|
||||
if( g.heli_servo_manual == 1 ) { // are we in manual servo mode? (i.e. swash set-up mode)?
|
||||
|
||||
// we must be in set-up mode so mark swash as uninitialised
|
||||
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);
|
||||
// check if we need to freeup the swash
|
||||
if( heli_swash_initialised ) {
|
||||
heli_reset_swash();
|
||||
}
|
||||
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
|
||||
|
||||
// check if we need to reinitialise the swash
|
||||
@ -249,7 +276,7 @@ static void output_motor_test()
|
||||
// heli_angle_boost - adds a boost depending on roll/pitch values
|
||||
// equivalent of quad's angle_boost function
|
||||
// throttle value should be 0 ~ 1000
|
||||
static int heli_get_angle_boost(int throttle)
|
||||
static int16_t heli_get_angle_boost(int throttle)
|
||||
{
|
||||
float angle_boost_factor = cos_pitch_x * cos_roll_x;
|
||||
angle_boost_factor = 1.0 - constrain(angle_boost_factor, .5, 1.0);
|
||||
|
||||
Loading…
Reference in New Issue
Block a user