aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

TradHeli: leaky-I-term based on dynamic_flight_detector

Browse Source
This commit is contained in:
Robert Lefebvre 2013-07-17 21:47:39 -04:00 committed by Randy Mackay
parent 19716e21b5
commit c60ff46cc5
2 changed files with 43 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
ArduCopter/ArduCopter.pde
View File

@ -670,6 +670,11 @@ static uint8_t throttle_mode;
static int16_t angle_boost;
// counter to verify landings
static uint16_t land_detector;
// counter to control dynamic flight profile
#if FRAME_CONFIG == HELI_FRAME
static uint8_t dynamic_flight_counter;
static bool dynamic_flight;
#endif // HELI_FRAME
////////////////////////////////////////////////////////////////////////////////
@ -1873,7 +1878,7 @@ void update_throttle_mode(void)
} else {
motors.stab_throttle = false;
}
check_dynamic_flight();
// allow swash collective to move if we are in manual throttle modes, even if disarmed
if( !motors.armed() ) {
if ( !(throttle_mode == THROTTLE_MANUAL) && !(throttle_mode == THROTTLE_MANUAL_TILT_COMPENSATED)){
@ -2014,6 +2019,33 @@ void update_throttle_mode(void)
}
}
#if FRAME_CONFIG == HELI_FRAME
static void check_dynamic_flight(void){
if (!motors.armed() || throttle_mode == THROTTLE_LAND || motors.motor_runup_complete == false){
dynamic_flight_counter=0;
dynamic_flight=false;
return;
}
if (dynamic_flight_counter < 255){ // check if we're in dynamic flight mode
if (g.rc_3.servo_out > 800 || (labs(ahrs.pitch_sensor) > 2000)) {
dynamic_flight_counter++;
}
if (dynamic_flight_counter > 254){ // we must be in the air by now
dynamic_flight = true;
}
}
if (dynamic_flight_counter > 0){
if ((labs(ahrs.roll_sensor) < 1500) && (labs(ahrs.pitch_sensor) < 1500)) {
dynamic_flight_counter--;
}
if (dynamic_flight_counter < 1){
dynamic_flight = false;
}
}
}
#endif // HELI_FRAME
// set_target_alt_for_reporting - set target altitude in cm for reporting purposes (logs and gcs)
static void set_target_alt_for_reporting(float alt_cm)
{

12
ArduCopter/Attitude.pde
View File

@ -498,7 +498,11 @@ static void heli_integrated_swash_controller(int32_t target_roll_rate, int32_t t
roll_i = g.pid_rate_roll.get_integrator();
}
} else {
roll_i = g.pid_rate_roll.get_leaky_i(roll_rate_error, G_Dt, RATE_INTEGRATOR_LEAK_RATE); // Flybarless Helis get huge I-terms. I-term controls much of the rate
if (dynamic_flight){
roll_i = g.pid_rate_roll.get_i(roll_rate_error, G_Dt);
} else {
roll_i = g.pid_rate_roll.get_leaky_i(roll_rate_error, G_Dt, RATE_INTEGRATOR_LEAK_RATE);
}
}
}
@ -512,7 +516,11 @@ static void heli_integrated_swash_controller(int32_t target_roll_rate, int32_t t
pitch_i = g.pid_rate_pitch.get_integrator();
}
} else {
pitch_i = g.pid_rate_pitch.get_leaky_i(pitch_rate_error, G_Dt, RATE_INTEGRATOR_LEAK_RATE); // Flybarless Helis get huge I-terms. I-term controls much of the rate
if (dynamic_flight){
pitch_i = g.pid_rate_pitch.get_i(pitch_rate_error, G_Dt);
} else {
pitch_i = g.pid_rate_pitch.get_leaky_i(pitch_rate_error, G_Dt, RATE_INTEGRATOR_LEAK_RATE);
}
}
}