From 662cefd0860eea3990b34cdbdac4da5386850f43 Mon Sep 17 00:00:00 2001 From: skyscraper Date: Sun, 8 May 2016 09:51:39 +0100 Subject: [PATCH] AntennaTracker: Fix up after refactoring RC_Channel class Further to refactor of RC_Channel class which included adding get_xx set_xx methods, change reads and writes to the public members to calls to get and set functionsss old public member(int16_t) get function -> int16_t set function (int16_t) (expression where c is an object of type RC_Channel) c.radio_in c.get_radio_in() c.set_radio_in(v) c.control_in c.get_control_in() c.set_control_in(v) c.servo_out c.get_servo_out() c.set_servo_out(v) c.pwm_out c.get_pwm_out() // use existing c.radio_out c.get_radio_out() c.set_radio_out(v) c.radio_max c.get_radio_max() c.set_radio_max(v) c.radio_min c.get_radio_min() c.set_radio_min(v) c.radio_trim c.get_radio_trim() c.set_radio_trim(v); c.min_max_configured() // return true if min and max are configured Because data members of RC_Channels are now private and so cannot be written directly some overloads are provided in the Plane classes to provide the old functionality new overload Plane::stick_mix_channel(RC_Channel *channel) which forwards to the previously existing void stick_mix_channel(RC_Channel *channel, int16_t &servo_out); new overload Plane::channel_output_mixer(Rc_Channel* , RC_Channel*)const which forwards to (uint8_t mixing_type, int16_t & chan1, int16_t & chan2)const; Rename functions RC_Channel_aux::set_radio_trim(Aux_servo_function_t function) to RC_Channel_aux::set_trim_to_radio_in_for(Aux_servo_function_t function) RC_Channel_aux::set_servo_out(Aux_servo_function_t function, int16_t value) to RC_Channel_aux::set_servo_out_for(Aux_servo_function_t function, int16_t value) Rationale: RC_Channel is a complicated class, which combines several functionalities dealing with stick inputs in pwm and logical units, logical and actual actuator outputs, unit conversion etc, etc The intent of this PR is to clarify existing use of the class. At the basic level it should now be possible to grep all places where private variable is set by searching for the set_xx function. (The wider purpose is to provide a more generic and logically simpler method of output mixing. This is a small step) --- AntennaTracker/control_manual.cpp | 4 +- AntennaTracker/control_servo_test.cpp | 4 +- AntennaTracker/servos.cpp | 64 +++++++++++++-------------- AntennaTracker/system.cpp | 4 +- 4 files changed, 38 insertions(+), 38 deletions(-) diff --git a/AntennaTracker/control_manual.cpp b/AntennaTracker/control_manual.cpp index 4ce72a3861..40b7536f61 100644 --- a/AntennaTracker/control_manual.cpp +++ b/AntennaTracker/control_manual.cpp @@ -13,8 +13,8 @@ void Tracker::update_manual(void) { // copy yaw and pitch input to output - channel_yaw.radio_out = constrain_int16(channel_yaw.radio_in, channel_yaw.radio_min, channel_yaw.radio_max); - channel_pitch.radio_out = constrain_int16(channel_pitch.radio_in, channel_pitch.radio_min, channel_pitch.radio_max); + channel_yaw.set_radio_out(constrain_int16(channel_yaw.get_radio_in(), channel_yaw.get_radio_min(), channel_yaw.get_radio_max())); + channel_pitch.set_radio_out(constrain_int16(channel_pitch.get_radio_in(), channel_pitch.get_radio_min(), channel_pitch.get_radio_max())); // send output to servos channel_yaw.output(); diff --git a/AntennaTracker/control_servo_test.cpp b/AntennaTracker/control_servo_test.cpp index cbee7088e5..26bed0d86b 100644 --- a/AntennaTracker/control_servo_test.cpp +++ b/AntennaTracker/control_servo_test.cpp @@ -27,13 +27,13 @@ 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) { - channel_yaw.radio_out = constrain_int16(pwm, channel_yaw.radio_min, channel_yaw.radio_max); + channel_yaw.set_radio_out(constrain_int16(pwm, channel_yaw.get_radio_min(), channel_yaw.get_radio_max())); channel_yaw.output(); } // set pitch servo pwm and send output to servo if (servo_num == CH_PITCH) { - channel_pitch.radio_out = constrain_int16(pwm, channel_pitch.radio_min, channel_pitch.radio_max); + channel_pitch.set_radio_out(constrain_int16(pwm, channel_pitch.get_radio_min(), channel_pitch.get_radio_max())); channel_pitch.output(); } diff --git a/AntennaTracker/servos.cpp b/AntennaTracker/servos.cpp index 0397d7f1f1..cf8bfedc55 100644 --- a/AntennaTracker/servos.cpp +++ b/AntennaTracker/servos.cpp @@ -78,7 +78,7 @@ void Tracker::update_pitch_position_servo(float pitch) // PITCH2SRV_IMAX 4000.000000 // calculate new servo position - int32_t new_servo_out = channel_pitch.servo_out + g.pidPitch2Srv.get_pid(angle_err); + int32_t new_servo_out = channel_pitch.get_servo_out() + g.pidPitch2Srv.get_pid(angle_err); // initialise limit flags servo_limit.pitch_lower = false; @@ -90,24 +90,24 @@ void Tracker::update_pitch_position_servo(float pitch) if (max_change < 1) { max_change = 1; } - if (new_servo_out <= channel_pitch.servo_out - max_change) { - new_servo_out = channel_pitch.servo_out - max_change; + 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.servo_out + max_change) { - new_servo_out = channel_pitch.servo_out + max_change; + 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.servo_out = new_servo_out; + channel_pitch.set_servo_out(new_servo_out); // position limit pitch servo - if (channel_pitch.servo_out <= -pitch_limit_cd) { - channel_pitch.servo_out = -pitch_limit_cd; + if (channel_pitch.get_servo_out() <= -pitch_limit_cd) { + channel_pitch.set_servo_out(-pitch_limit_cd); servo_limit.pitch_lower = true; } - if (channel_pitch.servo_out >= pitch_limit_cd) { - channel_pitch.servo_out = pitch_limit_cd; + if (channel_pitch.get_servo_out() >= pitch_limit_cd) { + channel_pitch.set_servo_out(pitch_limit_cd); servo_limit.pitch_upper = true; } } @@ -121,21 +121,21 @@ void Tracker::update_pitch_onoff_servo(float pitch) { // degrees(ahrs.pitch) is -90 to 90, where 0 is horizontal // pitch argument is -90 to 90, where 0 is horizontal - // servo_out is in 100ths of a degree + // get_servo_out() is in 100ths of a degree float ahrs_pitch = degrees(ahrs.pitch); float err = ahrs_pitch - pitch; float acceptable_error = g.onoff_pitch_rate * g.onoff_pitch_mintime; if (fabsf(err) < acceptable_error) { - channel_pitch.servo_out = 0; + channel_pitch.set_servo_out(0); } else if (err > 0) { // positive error means we are pointing too high, so push the // servo down - channel_pitch.servo_out = -9000; + channel_pitch.set_servo_out(-9000); } else { // negative error means we are pointing too low, so push the // servo up - channel_pitch.servo_out = 9000; + channel_pitch.set_servo_out(9000); } } @@ -146,7 +146,7 @@ void Tracker::update_pitch_cr_servo(float pitch) { float ahrs_pitch = degrees(ahrs.pitch); float err_cd = (pitch - ahrs_pitch) * 100.0f; - channel_pitch.servo_out = g.pidPitch2Srv.get_pid(err_cd); + channel_pitch.set_servo_out(g.pidPitch2Srv.get_pid(err_cd)); } /** @@ -229,13 +229,13 @@ void Tracker::update_yaw_position_servo(float yaw) } // handle hitting servo limits - if (abs(channel_yaw.servo_out) == 18000 && + if (abs(channel_yaw.get_servo_out()) == 18000 && labs(angle_err) > margin && making_progress && AP_HAL::millis() - slew_start_ms > g.min_reverse_time*1000) { // we are at the limit of the servo and are not moving in the // right direction, so slew the other way - new_slew_dir = -channel_yaw.servo_out / 18000; + new_slew_dir = -channel_yaw.get_servo_out() / 18000; slew_start_ms = AP_HAL::millis(); } @@ -251,7 +251,7 @@ void Tracker::update_yaw_position_servo(float yaw) tracker.gcs_send_text_fmt(MAV_SEVERITY_WARNING, "Slew: %d/%d err=%ld servo=%ld ez=%.3f", (int)slew_dir, (int)new_slew_dir, (long)angle_err, - (long)channel_yaw.servo_out, + (long)channel_yaw.get_servo_out(), (double)degrees(ahrs.get_gyro().z)); } @@ -268,7 +268,7 @@ void Tracker::update_yaw_position_servo(float yaw) } else { float servo_change = g.pidYaw2Srv.get_pid(angle_err); servo_change = constrain_float(servo_change, -18000, 18000); - new_servo_out = constrain_float(channel_yaw.servo_out - servo_change, -18000, 18000); + new_servo_out = constrain_float(channel_yaw.get_servo_out() - servo_change, -18000, 18000); } // rate limit yaw servo @@ -277,24 +277,24 @@ void Tracker::update_yaw_position_servo(float yaw) if (max_change < 1) { max_change = 1; } - if (new_servo_out <= channel_yaw.servo_out - max_change) { - new_servo_out = channel_yaw.servo_out - max_change; + 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.servo_out + max_change) { - new_servo_out = channel_yaw.servo_out + max_change; + 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.servo_out = new_servo_out; + channel_yaw.set_servo_out(new_servo_out); // position limit pitch servo - if (channel_yaw.servo_out <= -yaw_limit_cd) { - channel_yaw.servo_out = -yaw_limit_cd; + if (channel_yaw.get_servo_out() <= -yaw_limit_cd) { + channel_yaw.set_servo_out(-yaw_limit_cd); servo_limit.yaw_lower = true; } - if (channel_yaw.servo_out >= yaw_limit_cd) { - channel_yaw.servo_out = yaw_limit_cd; + if (channel_yaw.get_servo_out() >= yaw_limit_cd) { + channel_yaw.set_servo_out(yaw_limit_cd); servo_limit.yaw_upper = true; } } @@ -314,15 +314,15 @@ void Tracker::update_yaw_onoff_servo(float yaw) float acceptable_error = g.onoff_yaw_rate * g.onoff_yaw_mintime; if (fabsf(err) < acceptable_error) { - channel_yaw.servo_out = 0; + channel_yaw.set_servo_out(0); } else if (err > 0) { // positive error means we are clockwise of the target, so // move anti-clockwise - channel_yaw.servo_out = -18000; + channel_yaw.set_servo_out(-18000); } else { // negative error means we are anti-clockwise of the target, so // move clockwise - channel_yaw.servo_out = 18000; + channel_yaw.set_servo_out(18000); } } @@ -335,5 +335,5 @@ void Tracker::update_yaw_cr_servo(float yaw) float yaw_cd = wrap_180_cd_float(yaw*100.0f); float err_cd = wrap_180_cd(yaw_cd - (float)ahrs_yaw_cd); - channel_yaw.servo_out = g.pidYaw2Srv.get_pid(err_cd); + channel_yaw.set_servo_out(g.pidYaw2Srv.get_pid(err_cd)); } diff --git a/AntennaTracker/system.cpp b/AntennaTracker/system.cpp index 2d2ee65d16..b25b76ba91 100644 --- a/AntennaTracker/system.cpp +++ b/AntennaTracker/system.cpp @@ -189,8 +189,8 @@ void Tracker::disarm_servos() void Tracker::prepare_servos() { start_time_ms = AP_HAL::millis(); - channel_yaw.radio_out = channel_yaw.radio_trim; - channel_pitch.radio_out = channel_pitch.radio_trim; + channel_yaw.set_radio_out(channel_yaw.get_radio_trim()); + channel_pitch.set_radio_out(channel_pitch.get_radio_trim()); channel_yaw.output(); channel_pitch.output(); }