TradHeli - automatically reinitialise swash plate if HSV_MAN is switched from 1 to 0.

Turn-off feed forward correction to yaw based on collect when in HIL mode.
This commit is contained in:
Randy Mackay 2011-11-17 20:43:07 +09:00
parent ea4359574f
commit 4baa2fd133

View File

@ -6,6 +6,7 @@
#define HELI_SERVO_AVERAGING_ANALOG 2 // 125Hz #define HELI_SERVO_AVERAGING_ANALOG 2 // 125Hz
static float heli_throttle_scaler = 0; static float heli_throttle_scaler = 0;
static bool heli_swash_initialised = false;
// heli_servo_averaging: // heli_servo_averaging:
// 0 or 1 = no averaging, 250hz // 0 or 1 = no averaging, 250hz
@ -64,6 +65,9 @@ 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
heli_swash_initialised = true;
} }
static void heli_move_servos_to_mid() static void heli_move_servos_to_mid()
@ -83,17 +87,29 @@ static void heli_move_swash(int roll_out, int pitch_out, int coll_out, int yaw_o
{ {
int yaw_offset = 0; int yaw_offset = 0;
// regular flight mode checks if( g.heli_servo_manual == 1 ) { // are we in manual servo mode? (i.e. swash set-up mode)?
if( g.heli_servo_manual != 1) {
// we must be in set-up mode so mark swash as uninitialised
heli_swash_initialised = false;
}else{ // regular flight mode
// check if we need to reinitialise the swash
if( !heli_swash_initialised ) {
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, (int)g.heli_coll_min, (int)g.heli_coll_max); coll_out = constrain(coll_out, (int)g.heli_coll_min, (int)g.heli_coll_max);
// 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( !g.heli_ext_gyro_enabled ) { if( !g.heli_ext_gyro_enabled ) {
yaw_offset = g.heli_coll_yaw_effect * (coll_out - g.heli_coll_mid); yaw_offset = g.heli_coll_yaw_effect * (coll_out - g.heli_coll_mid);
} }
#endif
} }
// swashplate servos // swashplate servos