Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AntennaTracker: Support changing update period

This commit is contained in:
Leonard Hall 2022-11-30 15:57:41 +10:30 committed by Randy Mackay
parent f8d4f6afc9
commit 749ac19295
1 changed files with 4 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
AntennaTracker/servos.cpp
View File

@ -74,7 +74,7 @@ void Tracker::update_pitch_position_servo()
// PITCH2SRV_IMAX 4000.000000 // PITCH2SRV_IMAX 4000.000000
// calculate new servo position // calculate new servo position
float new_servo_out = SRV_Channels::get_output_scaled(SRV_Channel::k_tracker_pitch) + g.pidPitch2Srv.update_error(nav_status.angle_error_pitch); float new_servo_out = SRV_Channels::get_output_scaled(SRV_Channel::k_tracker_pitch) + g.pidPitch2Srv.update_error(nav_status.angle_error_pitch, G_Dt);
// position limit pitch servo // position limit pitch servo
if (new_servo_out <= pitch_min_cd) { if (new_servo_out <= pitch_min_cd) {
@ -125,7 +125,7 @@ void Tracker::update_pitch_onoff_servo(float pitch) const
*/ */
void Tracker::update_pitch_cr_servo(float pitch) void Tracker::update_pitch_cr_servo(float 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.update_error(nav_status.angle_error_pitch, G_Dt), -(-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);
} }
@ -187,7 +187,7 @@ void Tracker::update_yaw_position_servo()
right direction right direction
*/ */
float servo_change = g.pidYaw2Srv.update_error(nav_status.angle_error_yaw); float servo_change = g.pidYaw2Srv.update_error(nav_status.angle_error_yaw, G_Dt);
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);
@ -238,6 +238,6 @@ void Tracker::update_yaw_onoff_servo(float yaw) const
*/ */
void Tracker::update_yaw_cr_servo(float yaw) void Tracker::update_yaw_cr_servo(float 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.update_error(nav_status.angle_error_yaw, G_Dt), -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);
} }