mirror of https://github.com/ArduPilot/ardupilot
Cosmetic
This commit is contained in:
Jason Short
parent
2fc6a645b4
commit
37cda3c364
|
|
@ -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_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));
|
||||
|
|
@ -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_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;
|
||||
heli_swash_initialised = false;
|
||||
}
|
||||
|
||||
// initialise the swash
|
||||
// initialise the swash
|
||||
static void heli_init_swash()
|
||||
{
|
||||
int i;
|
||||
|
|
@ -67,18 +67,18 @@ static void heli_init_swash()
|
|||
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;
|
||||
g.heli_coll_max = 2000;
|
||||
}
|
||||
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));
|
||||
|
||||
|
|
@ -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_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);
|
||||
|
|
@ -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_max = g.heli_coll_max + (g.heli_servo_3.radio_trim-1500);
|
||||
}
|
||||
|
||||
|
||||
// reset the servo averaging
|
||||
for( i=0; i<=3; i++ )
|
||||
heli_servo_out[i] = 0;
|
||||
|
|
@ -124,7 +124,7 @@ static void heli_init_swash()
|
|||
g.heli_servo_averaging = 0;
|
||||
g.heli_servo_averaging.save();
|
||||
}
|
||||
|
||||
|
||||
// mark swash as initialised
|
||||
heli_swash_initialised = true;
|
||||
}
|
||||
|
|
@ -147,26 +147,26 @@ static void heli_move_servos_to_mid()
|
|||
// yaw: -4500 ~ 4500
|
||||
//
|
||||
static void heli_move_swash(int roll_out, int pitch_out, int coll_out, int yaw_out)
|
||||
{
|
||||
{
|
||||
int yaw_offset = 0;
|
||||
|
||||
|
||||
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
|
||||
if( heli_swash_initialised ) {
|
||||
heli_reset_swash();
|
||||
}
|
||||
}else{ // regular flight mode
|
||||
|
||||
|
||||
// check if we need to reinitialise the swash
|
||||
if( !heli_swash_initialised ) {
|
||||
heli_init_swash();
|
||||
}
|
||||
|
||||
|
||||
// ensure values are acceptable:
|
||||
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);
|
||||
|
||||
|
||||
// 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 ) {
|
||||
|
|
@ -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[3] += g.heli_servo_4.radio_out;
|
||||
heli_servo_out_count++;
|
||||
|
||||
|
||||
// is it time to move the servos?
|
||||
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[3] /= g.heli_servo_averaging;
|
||||
}
|
||||
|
||||
|
||||
// actually move the servos
|
||||
APM_RC.OutputCh(CH_1, heli_servo_out[0]);
|
||||
APM_RC.OutputCh(CH_2, heli_servo_out[1]);
|
||||
APM_RC.OutputCh(CH_3, heli_servo_out[2]);
|
||||
APM_RC.OutputCh(CH_4, heli_servo_out[3]);
|
||||
|
||||
|
||||
// output gyro value
|
||||
if( g.heli_ext_gyro_enabled ) {
|
||||
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_4.servo_out = g.rc_4.control_in;
|
||||
}
|
||||
|
||||
|
||||
//static int counter = 0;
|
||||
g.rc_1.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);
|
||||
int throttle_above_mid = max(throttle - heli_throttle_mid,0);
|
||||
return throttle + throttle_above_mid*angle_boost_factor;
|
||||
|
||||
|
||||
}
|
||||
|
||||
#endif // HELI_FRAME
|
||||
|
|
|
|||
Loading…
Reference in New Issue