diff --git a/ArduCopter/Parameters.pde b/ArduCopter/Parameters.pde index 4dcb0378fe..ba9fce3a3b 100644 --- a/ArduCopter/Parameters.pde +++ b/ArduCopter/Parameters.pde @@ -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 diff --git a/ArduCopter/heli.pde b/ArduCopter/heli.pde index 022ca2818d..65805810c3 100644 --- a/ArduCopter/heli.pde +++ b/ArduCopter/heli.pde @@ -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