diff --git a/AntennaTracker/Parameters.cpp b/AntennaTracker/Parameters.cpp index de28506b77..6f3e2e101b 100644 --- a/AntennaTracker/Parameters.cpp +++ b/AntennaTracker/Parameters.cpp @@ -337,7 +337,7 @@ const AP_Param::Info Tracker::var_info[] = { // @Range: 0.001 0.1 // @Increment: 0.001 // @User: Standard - GGROUP(pidPitch2Srv, "PITCH2SRV_", PID), + GGROUP(pidPitch2Srv, "PITCH2SRV_", AC_PID), // @Param: YAW2SRV_P // @DisplayName: Yaw axis controller P gain @@ -367,7 +367,7 @@ const AP_Param::Info Tracker::var_info[] = { // @Range: 0.001 0.1 // @Increment: 0.001 // @User: Standard - GGROUP(pidYaw2Srv, "YAW2SRV_", PID), + GGROUP(pidYaw2Srv, "YAW2SRV_", AC_PID), // @Param: CMD_TOTAL // @DisplayName: Number of loaded mission items diff --git a/AntennaTracker/Parameters.h b/AntennaTracker/Parameters.h index 4c30ec6c2f..c81adad566 100644 --- a/AntennaTracker/Parameters.h +++ b/AntennaTracker/Parameters.h @@ -63,8 +63,8 @@ public: k_param_scheduler, k_param_ins, k_param_sitl, - k_param_pidPitch2Srv, - k_param_pidYaw2Srv, + k_param_pidPitch_old, // deprecated + k_param_pidYaw_old, // deprecated k_param_gcs2, // stream rates for uartD k_param_serial2_baud, // deprecated @@ -107,6 +107,8 @@ public: // k_param_channel_yaw = 200, k_param_channel_pitch, + k_param_pidPitch2Srv, + k_param_pidYaw2Srv, // // 220: Waypoint data @@ -156,13 +158,13 @@ public: AP_Int32 log_bitmask; - // PID controllers - PID pidPitch2Srv; - PID pidYaw2Srv; + // AC_PID controllers + AC_PID pidPitch2Srv; + AC_PID pidYaw2Srv; Parameters() : - pidPitch2Srv(0.2, 0, 0.05f, 4000.0f), - pidYaw2Srv (0.2, 0, 0.05f, 4000.0f) + pidPitch2Srv(0.2, 0, 0.05f, 4000.0f, 0.1, 0.02f), + pidYaw2Srv (0.2, 0, 0.05f, 4000.0f, 0.1, 0.02f) {} }; diff --git a/AntennaTracker/Tracker.h b/AntennaTracker/Tracker.h index 03ca3031d5..1c3d2dee81 100644 --- a/AntennaTracker/Tracker.h +++ b/AntennaTracker/Tracker.h @@ -46,7 +46,7 @@ #include // Serial manager library #include // ArduPilot Mega Declination Helper Library #include -#include +#include #include // main loop scheduler #include #include diff --git a/AntennaTracker/make.inc b/AntennaTracker/make.inc index 8436c32658..bfb21a51b9 100644 --- a/AntennaTracker/make.inc +++ b/AntennaTracker/make.inc @@ -15,7 +15,7 @@ LIBRARIES += GCS_MAVLink LIBRARIES += AP_SerialManager LIBRARIES += AP_Declination LIBRARIES += DataFlash -LIBRARIES += PID +LIBRARIES += AC_PID LIBRARIES += AP_Scheduler LIBRARIES += AP_NavEKF LIBRARIES += AP_NavEKF2 diff --git a/AntennaTracker/servos.cpp b/AntennaTracker/servos.cpp index a8ae952cd4..03dbbc5eae 100644 --- a/AntennaTracker/servos.cpp +++ b/AntennaTracker/servos.cpp @@ -78,11 +78,8 @@ void Tracker::update_pitch_position_servo(float pitch) // PITCH2SRV_IMAX 4000.000000 // calculate new servo position - int32_t new_servo_out = channel_pitch.get_servo_out() + g.pidPitch2Srv.get_pid(angle_err); - - // initialise limit flags - servo_limit.pitch_lower = false; - servo_limit.pitch_upper = false; + g.pidPitch2Srv.set_input_filter_all(angle_err); + int32_t new_servo_out = channel_pitch.get_servo_out() + g.pidPitch2Srv.get_pid(); // rate limit pitch servo if (g.pitch_slew_time > 0.02f) { @@ -92,11 +89,9 @@ void Tracker::update_pitch_position_servo(float pitch) } if (new_servo_out <= channel_pitch.get_servo_out() - max_change) { new_servo_out = channel_pitch.get_servo_out() - max_change; - servo_limit.pitch_lower = true; } if (new_servo_out >= channel_pitch.get_servo_out() + max_change) { new_servo_out = channel_pitch.get_servo_out() + max_change; - servo_limit.pitch_upper = true; } } channel_pitch.set_servo_out(new_servo_out); @@ -104,11 +99,11 @@ void Tracker::update_pitch_position_servo(float pitch) // position limit pitch servo if (channel_pitch.get_servo_out() <= -pitch_limit_cd) { channel_pitch.set_servo_out(-pitch_limit_cd); - servo_limit.pitch_lower = true; + g.pidPitch2Srv.reset_I(); } if (channel_pitch.get_servo_out() >= pitch_limit_cd) { channel_pitch.set_servo_out(pitch_limit_cd); - servo_limit.pitch_upper = true; + g.pidPitch2Srv.reset_I(); } } @@ -146,7 +141,8 @@ void Tracker::update_pitch_cr_servo(float pitch) { float ahrs_pitch = degrees(ahrs.pitch); float err_cd = (pitch - ahrs_pitch) * 100.0f; - channel_pitch.set_servo_out(g.pidPitch2Srv.get_pid(err_cd)); + g.pidPitch2Srv.set_input_filter_all(err_cd); + channel_pitch.set_servo_out(g.pidPitch2Srv.get_pid()); } /** @@ -256,17 +252,13 @@ void Tracker::update_yaw_position_servo(float yaw) } slew_dir = new_slew_dir; - - // initialise limit flags - servo_limit.yaw_lower = false; - servo_limit.yaw_upper = false; - int16_t new_servo_out; if (slew_dir != 0) { new_servo_out = slew_dir * 18000; g.pidYaw2Srv.reset_I(); } else { - float servo_change = g.pidYaw2Srv.get_pid(angle_err); + g.pidYaw2Srv.set_input_filter_all(angle_err); + float servo_change = g.pidYaw2Srv.get_pid(); servo_change = constrain_float(servo_change, -18000, 18000); new_servo_out = constrain_float(channel_yaw.get_servo_out() - servo_change, -18000, 18000); } @@ -279,23 +271,21 @@ void Tracker::update_yaw_position_servo(float yaw) } if (new_servo_out <= channel_yaw.get_servo_out() - max_change) { new_servo_out = channel_yaw.get_servo_out() - max_change; - servo_limit.yaw_lower = true; } if (new_servo_out >= channel_yaw.get_servo_out() + max_change) { new_servo_out = channel_yaw.get_servo_out() + max_change; - servo_limit.yaw_upper = true; } } channel_yaw.set_servo_out(new_servo_out); - // position limit pitch servo + // position limit yaw servo if (channel_yaw.get_servo_out() <= -yaw_limit_cd) { channel_yaw.set_servo_out(-yaw_limit_cd); - servo_limit.yaw_lower = true; + g.pidYaw2Srv.reset_I(); } if (channel_yaw.get_servo_out() >= yaw_limit_cd) { channel_yaw.set_servo_out(yaw_limit_cd); - servo_limit.yaw_upper = true; + g.pidYaw2Srv.reset_I(); } } @@ -335,5 +325,6 @@ void Tracker::update_yaw_cr_servo(float yaw) float yaw_cd = wrap_180_cd(yaw*100.0f); float err_cd = wrap_180_cd(yaw_cd - (float)ahrs_yaw_cd); - channel_yaw.set_servo_out(g.pidYaw2Srv.get_pid(err_cd)); + g.pidYaw2Srv.set_input_filter_all(err_cd); + channel_yaw.set_servo_out(g.pidYaw2Srv.get_pid()); } diff --git a/AntennaTracker/wscript b/AntennaTracker/wscript index 522790df87..1db0b23acc 100644 --- a/AntennaTracker/wscript +++ b/AntennaTracker/wscript @@ -7,7 +7,7 @@ def build(bld): name=vehicle + '_libs', vehicle=vehicle, libraries=bld.ap_common_vehicle_libraries() + [ - 'PID', + 'AC_PID', ], use='mavlink', )