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

Tracker: use k_tracker_yaw and k_tracker_pitch 

thanks to review by Buzz
This commit is contained in:
Andrew Tridgell 2017-01-09 09:30:37 +11:00
parent 26f9a5569f
commit 2513b27058
4 changed files with 26 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
AntennaTracker/control_manual.cpp
View File

@ -11,11 +11,11 @@
void Tracker::update_manual(void)
{
// copy yaw and pitch input to output
SRV_Channels::set_output_pwm(SRV_Channel::k_steering, RC_Channels::rc_channel(CH_YAW)->get_radio_in());
SRV_Channels::constrain_pwm(SRV_Channel::k_steering);
SRV_Channels::set_output_pwm(SRV_Channel::k_tracker_yaw, RC_Channels::rc_channel(CH_YAW)->get_radio_in());
SRV_Channels::constrain_pwm(SRV_Channel::k_tracker_yaw);
SRV_Channels::set_output_pwm(SRV_Channel::k_elevator, RC_Channels::rc_channel(CH_PITCH)->get_radio_in());
SRV_Channels::constrain_pwm(SRV_Channel::k_elevator);
SRV_Channels::set_output_pwm(SRV_Channel::k_tracker_pitch, RC_Channels::rc_channel(CH_PITCH)->get_radio_in());
SRV_Channels::constrain_pwm(SRV_Channel::k_tracker_pitch);
SRV_Channels::calc_pwm();
SRV_Channels::output_ch_all();

8
AntennaTracker/control_servo_test.cpp
View File

@ -25,14 +25,14 @@ bool Tracker::servo_test_set_servo(uint8_t servo_num, uint16_t pwm)
// set yaw servo pwm and send output to servo
if (servo_num == CH_YAW) {
SRV_Channels::set_output_pwm(SRV_Channel::k_steering, pwm);
SRV_Channels::constrain_pwm(SRV_Channel::k_steering);
SRV_Channels::set_output_pwm(SRV_Channel::k_tracker_yaw, pwm);
SRV_Channels::constrain_pwm(SRV_Channel::k_tracker_yaw);
}
// set pitch servo pwm and send output to servo
if (servo_num == CH_PITCH) {
SRV_Channels::set_output_pwm(SRV_Channel::k_elevator, pwm);
SRV_Channels::constrain_pwm(SRV_Channel::k_elevator);
SRV_Channels::set_output_pwm(SRV_Channel::k_tracker_pitch, pwm);
SRV_Channels::constrain_pwm(SRV_Channel::k_tracker_pitch);
}
SRV_Channels::calc_pwm();

32
AntennaTracker/servos.cpp
View File

@ -7,14 +7,14 @@
// init_servos - initialises the servos
void Tracker::init_servos()
{
SRV_Channels::set_default_function(CH_YAW, SRV_Channel::k_steering);
SRV_Channels::set_default_function(CH_PITCH, SRV_Channel::k_elevator);
SRV_Channels::set_default_function(CH_YAW, SRV_Channel::k_tracker_yaw);
SRV_Channels::set_default_function(CH_PITCH, SRV_Channel::k_tracker_pitch);
// yaw range is +/- (YAW_RANGE parameter/2) converted to centi-degrees
SRV_Channels::set_angle(SRV_Channel::k_steering, g.yaw_range * 100/2);
SRV_Channels::set_angle(SRV_Channel::k_tracker_yaw, g.yaw_range * 100/2);
// pitch range is +/- (PITCH_MIN/MAX parameters/2) converted to centi-degrees
SRV_Channels::set_angle(SRV_Channel::k_elevator, (-g.pitch_min+g.pitch_max) * 100/2);
SRV_Channels::set_angle(SRV_Channel::k_tracker_pitch, (-g.pitch_min+g.pitch_max) * 100/2);
SRV_Channels::output_trim_all();
SRV_Channels::calc_pwm();
@ -77,7 +77,7 @@ void Tracker::update_pitch_position_servo()
// 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_elevator) + g.pidPitch2Srv.get_pid();
int32_t new_servo_out = SRV_Channels::get_output_scaled(SRV_Channel::k_tracker_pitch) + g.pidPitch2Srv.get_pid();
// position limit pitch servo
if (new_servo_out <= pitch_min_cd) {
@ -89,7 +89,7 @@ void Tracker::update_pitch_position_servo()
g.pidPitch2Srv.reset_I();
}
// rate limit pitch servo
SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, new_servo_out);
SRV_Channels::set_output_scaled(SRV_Channel::k_tracker_pitch, new_servo_out);
if (pitch_servo_out_filt_init) {
pitch_servo_out_filt.apply(new_servo_out, G_Dt);
@ -111,15 +111,15 @@ void Tracker::update_pitch_onoff_servo(float pitch)
float acceptable_error = g.onoff_pitch_rate * g.onoff_pitch_mintime;
if (fabsf(nav_status.angle_error_pitch) < acceptable_error) {
SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, 0);
SRV_Channels::set_output_scaled(SRV_Channel::k_tracker_pitch, 0);
} else if ((nav_status.angle_error_pitch > 0) && (pitch*100>pitch_min_cd)) {
// positive error means we are pointing too low, so push the
// servo up
SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, -9000);
SRV_Channels::set_output_scaled(SRV_Channel::k_tracker_pitch, -9000);
} else if (pitch*100<pitch_max_cd) {
// negative error means we are pointing too high, so push the
// servo down
SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, 9000);
SRV_Channels::set_output_scaled(SRV_Channel::k_tracker_pitch, 9000);
}
}
@ -132,7 +132,7 @@ void Tracker::update_pitch_cr_servo(float pitch)
int32_t pitch_max_cd = g.pitch_max*100;
if ((pitch>pitch_min_cd) && (pitch<pitch_max_cd)) {
g.pidPitch2Srv.set_input_filter_all(nav_status.angle_error_pitch);
SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, g.pidPitch2Srv.get_pid());
SRV_Channels::set_output_scaled(SRV_Channel::k_tracker_pitch, g.pidPitch2Srv.get_pid());
}
}
@ -201,7 +201,7 @@ void Tracker::update_yaw_position_servo()
g.pidYaw2Srv.set_input_filter_all(nav_status.angle_error_yaw);
float servo_change = g.pidYaw2Srv.get_pid();
servo_change = constrain_float(servo_change, -18000, 18000);
float new_servo_out = constrain_float(SRV_Channels::get_output_scaled(SRV_Channel::k_steering) + servo_change, -18000, 18000);
float new_servo_out = constrain_float(SRV_Channels::get_output_scaled(SRV_Channel::k_tracker_yaw) + servo_change, -18000, 18000);
// position limit yaw servo
if (new_servo_out <= -yaw_limit_cd) {
@ -213,7 +213,7 @@ void Tracker::update_yaw_position_servo()
g.pidYaw2Srv.reset_I();
}
SRV_Channels::set_output_scaled(SRV_Channel::k_steering, new_servo_out);
SRV_Channels::set_output_scaled(SRV_Channel::k_tracker_yaw, new_servo_out);
if (yaw_servo_out_filt_init) {
yaw_servo_out_filt.apply(new_servo_out, G_Dt);
@ -233,15 +233,15 @@ void Tracker::update_yaw_onoff_servo(float yaw)
{
float acceptable_error = g.onoff_yaw_rate * g.onoff_yaw_mintime;
if (fabsf(nav_status.angle_error_yaw * 0.01f) < acceptable_error) {
SRV_Channels::set_output_scaled(SRV_Channel::k_steering, 0);
SRV_Channels::set_output_scaled(SRV_Channel::k_tracker_yaw, 0);
} else if (nav_status.angle_error_yaw * 0.01f > 0) {
// positive error means we are counter-clockwise of the target, so
// move clockwise
SRV_Channels::set_output_scaled(SRV_Channel::k_steering, 18000);
SRV_Channels::set_output_scaled(SRV_Channel::k_tracker_yaw, 18000);
} else {
// negative error means we are clockwise of the target, so
// move counter-clockwise
SRV_Channels::set_output_scaled(SRV_Channel::k_steering, -18000);
SRV_Channels::set_output_scaled(SRV_Channel::k_tracker_yaw, -18000);
}
}
@ -251,5 +251,5 @@ void Tracker::update_yaw_onoff_servo(float yaw)
void Tracker::update_yaw_cr_servo(float yaw)
{
g.pidYaw2Srv.set_input_filter_all(nav_status.angle_error_yaw);
SRV_Channels::set_output_scaled(SRV_Channel::k_steering, -g.pidYaw2Srv.get_pid());
SRV_Channels::set_output_scaled(SRV_Channel::k_tracker_yaw, -g.pidYaw2Srv.get_pid());
}

4
AntennaTracker/system.cpp
View File

@ -177,8 +177,8 @@ void Tracker::disarm_servos()
void Tracker::prepare_servos()
{
start_time_ms = AP_HAL::millis();
SRV_Channels::set_output_limit(SRV_Channel::k_steering, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM);
SRV_Channels::set_output_limit(SRV_Channel::k_elevator, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM);
SRV_Channels::set_output_limit(SRV_Channel::k_tracker_yaw, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM);
SRV_Channels::set_output_limit(SRV_Channel::k_tracker_pitch, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM);
SRV_Channels::calc_pwm();
SRV_Channels::output_ch_all();
}