Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-11 17:13:56 -03:00
Code Issues Packages Projects Releases Wiki Activity

Tracker: clamp output angles to relevant ranges

Browse Source
This commit is contained in:
IamPete1 2019-03-10 10:49:28 +00:00 committed by Peter Barker
parent 020aa6bd49
commit 2d14d79a65
1 changed files with 5 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
AntennaTracker/servos.cpp
View File

@ -130,12 +130,9 @@ void Tracker::update_pitch_onoff_servo(float pitch)
*/ */
void Tracker::update_pitch_cr_servo(float pitch) void Tracker::update_pitch_cr_servo(float pitch)
{ {
int32_t pitch_min_cd = g.pitch_min*100; g.pidPitch2Srv.set_input_filter_all(nav_status.angle_error_pitch);
int32_t pitch_max_cd = g.pitch_max*100; 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);
if ((pitch>pitch_min_cd) && (pitch<pitch_max_cd)) { SRV_Channels::set_output_scaled(SRV_Channel::k_tracker_pitch, pitch_out);
g.pidPitch2Srv.set_input_filter_all(nav_status.angle_error_pitch);
SRV_Channels::set_output_scaled(SRV_Channel::k_tracker_pitch, g.pidPitch2Srv.get_pid());
}
} }
/** /**
@ -253,5 +250,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); g.pidYaw2Srv.set_input_filter_all(nav_status.angle_error_yaw);
SRV_Channels::set_output_scaled(SRV_Channel::k_tracker_yaw, -g.pidYaw2Srv.get_pid()); 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);
} }