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

Cosmetic

This commit is contained in:
Jason Short 2012-01-03 10:24:18 -08:00
parent 801b3a8e8f
commit c629795050
1 changed files with 20 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

40
ArduCopter/heli.pde
View File

@ -41,7 +41,7 @@ static void heli_reset_swash()
g.heli_servo_3.radio_min = g.rc_3.radio_min + (g.heli_servo_3.radio_trim-1500); 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); g.heli_servo_3.radio_max = g.rc_3.radio_max + (g.heli_servo_3.radio_trim-1500);
} }
// 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));
heli_pitchFactor[CH_2] = cos(radians(g.heli_servo2_pos - g.heli_phase_angle)); heli_pitchFactor[CH_2] = cos(radians(g.heli_servo2_pos - g.heli_phase_angle));
@ -51,12 +51,12 @@ static void heli_reset_swash()
heli_rollFactor[CH_1] = cos(radians(g.heli_servo1_pos + 90 - g.heli_phase_angle)); 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_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));
// 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;
} }
// initialise the swash // initialise the swash
static void heli_init_swash() static void heli_init_swash()
{ {
int i; int i;
@ -67,18 +67,18 @@ static void heli_init_swash()
g.heli_servo_2.set_range(0,1000); g.heli_servo_2.set_range(0,1000);
g.heli_servo_3.set_range(0,1000); g.heli_servo_3.set_range(0,1000);
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_coll_min >= g.heli_coll_max ) {
g.heli_coll_min = 1000; g.heli_coll_min = 1000;
g.heli_coll_max = 2000; g.heli_coll_max = 2000;
} }
g.heli_coll_mid = constrain(g.heli_coll_mid, g.heli_coll_min, g.heli_coll_max); 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 // calculate compensation for collective range on roll & pitch range
if( g.heli_coll_max - g.heli_coll_min > 100 ) if( g.heli_coll_max - g.heli_coll_min > 100 )
coll_range_comp = 1000 / (g.heli_coll_max - g.heli_coll_min); coll_range_comp = 1000 / (g.heli_coll_max - g.heli_coll_min);
// calculate throttle mid point // 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)); heli_throttle_mid = (g.heli_coll_mid-g.heli_coll_min)*(1000.0/(g.heli_coll_max-g.heli_coll_min));
@ -91,7 +91,7 @@ static void heli_init_swash()
heli_rollFactor[CH_1] = cos(radians(g.heli_servo1_pos + 90 - g.heli_phase_angle)) * coll_range_comp; 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_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; heli_rollFactor[CH_3] = cos(radians(g.heli_servo3_pos + 90 - g.heli_phase_angle)) * coll_range_comp;
// servo min/max values // servo min/max values
if( g.heli_servo_1.get_reverse() ) { 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); g.heli_servo_1.radio_min = 3000 - g.heli_coll_max + (g.heli_servo_1.radio_trim-1500);
@ -114,7 +114,7 @@ static void heli_init_swash()
g.heli_servo_3.radio_min = g.heli_coll_min + (g.heli_servo_3.radio_trim-1500); 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); g.heli_servo_3.radio_max = g.heli_coll_max + (g.heli_servo_3.radio_trim-1500);
} }
// reset the servo averaging // reset the servo averaging
for( i=0; i<=3; i++ ) for( i=0; i<=3; i++ )
heli_servo_out[i] = 0; heli_servo_out[i] = 0;
@ -124,7 +124,7 @@ static void heli_init_swash()
g.heli_servo_averaging = 0; g.heli_servo_averaging = 0;
g.heli_servo_averaging.save(); g.heli_servo_averaging.save();
} }
// mark swash as initialised // mark swash as initialised
heli_swash_initialised = true; heli_swash_initialised = true;
} }
@ -147,26 +147,26 @@ 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)
{ {
int yaw_offset = 0; int yaw_offset = 0;
if( g.heli_servo_manual == 1 ) { // are we in manual servo mode? (i.e. swash set-up mode)? if( g.heli_servo_manual == 1 ) { // are we in manual servo mode? (i.e. swash set-up mode)?
// check if we need to freeup the swash // check if we need to freeup the swash
if( heli_swash_initialised ) { if( heli_swash_initialised ) {
heli_reset_swash(); heli_reset_swash();
} }
}else{ // regular flight mode }else{ // regular flight mode
// check if we need to reinitialise the swash // check if we need to reinitialise the swash
if( !heli_swash_initialised ) { if( !heli_swash_initialised ) {
heli_init_swash(); heli_init_swash();
} }
// 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, 0, 1000); 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
if( !g.heli_ext_gyro_enabled ) { if( !g.heli_ext_gyro_enabled ) {
@ -193,7 +193,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 ) {
@ -204,13 +204,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);
@ -248,7 +248,7 @@ static void output_motors_armed()
g.rc_3.servo_out = g.rc_3.control_in; g.rc_3.servo_out = g.rc_3.control_in;
g.rc_4.servo_out = g.rc_4.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();
@ -282,7 +282,7 @@ static int16_t heli_get_angle_boost(int throttle)
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_mid = max(throttle - heli_throttle_mid,0); int throttle_above_mid = max(throttle - heli_throttle_mid,0);
return throttle + throttle_above_mid*angle_boost_factor; return throttle + throttle_above_mid*angle_boost_factor;
} }
#endif // HELI_FRAME #endif // HELI_FRAME