Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-22 16:48:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

TradHeli: formatting and param description changes

Browse Source
This commit is contained in:
Randy Mackay 2013-11-06 14:45:58 +09:00
parent 46118b59d7
commit 34fb70cfc7
2 changed files with 22 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
ArduCopter/Parameters.pde
View File

@ -434,6 +434,7 @@ const AP_Param::Info var_info[] PROGMEM = {
// @DisplayName: Rate Pitch Feed Forward
// @Description: Rate Pitch Feed Forward (for TradHeli Only)
// @Range: 0 10
// @Increment: 0.01
// @User: Standard
GSCALAR(heli_pitch_ff, "RATE_PIT_FF", HELI_PITCH_FF),
@ -441,6 +442,7 @@ const AP_Param::Info var_info[] PROGMEM = {
// @DisplayName: Rate Roll Feed Forward
// @Description: Rate Roll Feed Forward (for TradHeli Only)
// @Range: 0 10
// @Increment: 0.01
// @User: Standard
GSCALAR(heli_roll_ff, "RATE_RLL_FF", HELI_ROLL_FF),
@ -448,6 +450,7 @@ const AP_Param::Info var_info[] PROGMEM = {
// @DisplayName: Rate Yaw Feed Forward
// @Description: Rate Yaw Feed Forward (for TradHeli Only)
// @Range: 0 10
// @Increment: 0.01
// @User: Standard
GSCALAR(heli_yaw_ff, "RATE_YAW_FF", HELI_YAW_FF),
#endif

35
ArduCopter/heli.pde
View File

@ -68,6 +68,9 @@ void init_rate_controllers()
// rate_pitch_filter.set_cutoff_frequency(0.01f, 0.1f);
}
// heli_integrated_swash_controller - convert desired roll and pitch rate to roll and pitch swash angles
// should be called at 100hz
// output placed directly into g.rc_1.servo_out and g.rc_2.servo_out
static void heli_integrated_swash_controller(int32_t target_roll_rate, int32_t target_pitch_rate)
{
int32_t roll_p, roll_i, roll_d, roll_ff; // used to capture pid values for logging
@ -76,13 +79,13 @@ static void heli_integrated_swash_controller(int32_t target_roll_rate, int32_t t
int32_t roll_rate_error, pitch_rate_error; // simply target_rate - current_rate
int32_t roll_output, pitch_output; // output from pid controller
static bool roll_pid_saturated, pitch_pid_saturated; // tracker from last loop if the PID was saturated
current_roll_rate = (omega.x * DEGX100); // get current roll rate
current_pitch_rate = (omega.y * DEGX100); // get current pitch rate
roll_rate_error = target_roll_rate - current_roll_rate;
pitch_rate_error = target_pitch_rate - current_pitch_rate;
roll_p = g.pid_rate_roll.get_p(roll_rate_error);
pitch_p = g.pid_rate_pitch.get_p(pitch_rate_error);
@ -103,7 +106,7 @@ static void heli_integrated_swash_controller(int32_t target_roll_rate, int32_t t
}
}
}
if (pitch_pid_saturated){
pitch_i = g.pid_rate_pitch.get_integrator(); // Locked Integrator due to PID saturation on previous cycle
} else {
@ -121,11 +124,11 @@ static void heli_integrated_swash_controller(int32_t target_roll_rate, int32_t t
}
}
}
roll_d = g.pid_rate_roll.get_d(target_roll_rate, G_Dt);
pitch_d = g.pid_rate_pitch.get_d(target_pitch_rate, G_Dt);
roll_ff = g.heli_roll_ff * target_roll_rate;
roll_d = g.pid_rate_roll.get_d(target_roll_rate, G_Dt);
pitch_d = g.pid_rate_pitch.get_d(target_pitch_rate, G_Dt);
roll_ff = g.heli_roll_ff * target_roll_rate;
pitch_ff = g.heli_pitch_ff * target_pitch_rate;
// Joly, I think your PC and CC code goes here
@ -148,26 +151,26 @@ static void heli_integrated_swash_controller(int32_t target_roll_rate, int32_t t
}
g.rc_1.servo_out = roll_output;
g.rc_2.servo_out = pitch_output;
g.rc_2.servo_out = pitch_output;
}
static int16_t
get_heli_rate_yaw(int32_t target_rate)
{
int32_t p,i,d,ff; // used to capture pid values for logging
int32_t current_rate; // this iteration's rate
int32_t current_rate; // this iteration's rate
int32_t rate_error;
int32_t output;
static bool pid_saturated; // tracker from last loop if the PID was saturated
current_rate = (omega.z * DEGX100); // get current rate
// rate control
rate_error = target_rate - current_rate;
// separately calculate p, i, d values for logging
p = g.pid_rate_yaw.get_p(rate_error);
if (pid_saturated){
i = g.pid_rate_yaw.get_integrator(); // Locked Integrator due to PID saturation on previous cycle
} else {
@ -175,11 +178,11 @@ get_heli_rate_yaw(int32_t target_rate)
}
d = g.pid_rate_yaw.get_d(rate_error, G_Dt);
ff = g.heli_yaw_ff*target_rate;
ff = g.heli_yaw_ff*target_rate;
output = p + i + d + ff;
if (labs(output) > 4500){
output = constrain_int32(output, -4500, 4500); // constrain output
pid_saturated = true; // freeze integrator next cycle