Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

Tracker: support for upgrade to PID object

Browse Source
This commit is contained in:
Leonard Hall 2019-06-27 19:06:31 +09:30 committed by Randy Mackay
parent 6f14673f08
commit d61aa7a4ce
3 changed files with 8 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
AntennaTracker/Parameters.h
View File

@ -170,8 +170,8 @@ public:
AC_PID pidYaw2Srv; AC_PID pidYaw2Srv;
Parameters() : Parameters() :
pidPitch2Srv(0.2, 0, 0.05f, 4000.0f, 0.1, 0.02f), pidPitch2Srv(0.2, 0.0f, 0.05f, 0.02f, 4000.0f, 0.0f, 0.0f, 0.0f, 0.1f),
pidYaw2Srv (0.2, 0, 0.05f, 4000.0f, 0.1, 0.02f) pidYaw2Srv (0.2, 0.0f, 0.05f, 0.02f, 4000.0f, 0.0f, 0.0f, 0.0f, 0.1f)
{} {}
}; };

4
AntennaTracker/control_auto.cpp
View File

@ -57,9 +57,9 @@ void Tracker::calc_angle_error(float pitch, float yaw, bool direction_reversed)
nav_status.angle_error_yaw = bf_yaw_err; nav_status.angle_error_yaw = bf_yaw_err;
// set actual and desired for logging, note we are using angles not rates // set actual and desired for logging, note we are using angles not rates
g.pidPitch2Srv.set_desired_rate(pitch * 0.01); g.pidPitch2Srv.set_target_rate(pitch * 0.01);
g.pidPitch2Srv.set_actual_rate(ahrs_pitch * 0.01); g.pidPitch2Srv.set_actual_rate(ahrs_pitch * 0.01);
g.pidYaw2Srv.set_desired_rate(yaw * 0.01); g.pidYaw2Srv.set_target_rate(yaw * 0.01);
g.pidYaw2Srv.set_actual_rate(ahrs_yaw_cd * 0.01); g.pidYaw2Srv.set_actual_rate(ahrs_yaw_cd * 0.01);
} }

12
AntennaTracker/servos.cpp
View File

@ -74,8 +74,7 @@ void Tracker::update_pitch_position_servo()
// PITCH2SRV_IMAX 4000.000000 // PITCH2SRV_IMAX 4000.000000
// calculate new servo position // calculate new servo position
g.pidPitch2Srv.set_input_filter_all(nav_status.angle_error_pitch); int32_t new_servo_out = SRV_Channels::get_output_scaled(SRV_Channel::k_tracker_pitch) + g.pidPitch2Srv.update_error(nav_status.angle_error_pitch);
int32_t new_servo_out = SRV_Channels::get_output_scaled(SRV_Channel::k_tracker_pitch) + g.pidPitch2Srv.get_pid();
// position limit pitch servo // position limit pitch servo
if (new_servo_out <= pitch_min_cd) { if (new_servo_out <= pitch_min_cd) {
@ -126,8 +125,7 @@ void Tracker::update_pitch_onoff_servo(float pitch)
*/ */
void Tracker::update_pitch_cr_servo(float pitch) void Tracker::update_pitch_cr_servo(float pitch)
{ {
g.pidPitch2Srv.set_input_filter_all(nav_status.angle_error_pitch); const float pitch_out = constrain_float(g.pidPitch2Srv.update_error(nav_status.angle_error_pitch), -(-g.pitch_min+g.pitch_max) * 100/2, (-g.pitch_min+g.pitch_max) * 100/2);
const float pitch_out = constrain_float(g.pidPitch2Srv.get_pid(), -(-g.pitch_min+g.pitch_max) * 100/2, (-g.pitch_min+g.pitch_max) * 100/2);
SRV_Channels::set_output_scaled(SRV_Channel::k_tracker_pitch, pitch_out); SRV_Channels::set_output_scaled(SRV_Channel::k_tracker_pitch, pitch_out);
} }
@ -189,8 +187,7 @@ void Tracker::update_yaw_position_servo()
right direction right direction
*/ */
g.pidYaw2Srv.set_input_filter_all(nav_status.angle_error_yaw); float servo_change = g.pidYaw2Srv.update_error(nav_status.angle_error_yaw);
float servo_change = g.pidYaw2Srv.get_pid();
servo_change = constrain_float(servo_change, -18000, 18000); servo_change = constrain_float(servo_change, -18000, 18000);
float new_servo_out = constrain_float(SRV_Channels::get_output_scaled(SRV_Channel::k_tracker_yaw) + servo_change, -18000, 18000); float new_servo_out = constrain_float(SRV_Channels::get_output_scaled(SRV_Channel::k_tracker_yaw) + servo_change, -18000, 18000);
@ -241,7 +238,6 @@ void Tracker::update_yaw_onoff_servo(float yaw)
*/ */
void Tracker::update_yaw_cr_servo(float yaw) void Tracker::update_yaw_cr_servo(float yaw)
{ {
g.pidYaw2Srv.set_input_filter_all(nav_status.angle_error_yaw); const float yaw_out = constrain_float(-g.pidYaw2Srv.update_error(nav_status.angle_error_yaw), -g.yaw_range * 100/2, g.yaw_range * 100/2);
const float yaw_out = constrain_float(-g.pidYaw2Srv.get_pid(), -g.yaw_range * 100/2, g.yaw_range * 100/2);
SRV_Channels::set_output_scaled(SRV_Channel::k_tracker_yaw, yaw_out); SRV_Channels::set_output_scaled(SRV_Channel::k_tracker_yaw, yaw_out);
} }