diff --git a/AntennaTracker/Parameters.cpp b/AntennaTracker/Parameters.cpp index 816c429cb7..ea25f3e5d1 100644 --- a/AntennaTracker/Parameters.cpp +++ b/AntennaTracker/Parameters.cpp @@ -302,16 +302,12 @@ const AP_Param::Info Tracker::var_info[] = { // @Path: ../libraries/AP_Notify/AP_Notify.cpp GOBJECT(notify, "NTF_", AP_Notify), - // RC channel - //----------- - // @Group: RC1_ // @Path: ../libraries/RC_Channel/RC_Channel.cpp - GOBJECT(channel_yaw, "RC1_", RC_Channel), - - // @Group: RC2_ - // @Path: ../libraries/RC_Channel/RC_Channel.cpp - GOBJECT(channel_pitch, "RC2_", RC_Channel), + GOBJECT(rc_channels, "RC", RC_Channels), + // @Path: ../libraries/SRV_Channel/SRV_Channel.cpp + GOBJECT(servo_channels, "SERVO", SRV_Channels), + // @Group: SERIAL // @Path: ../libraries/AP_SerialManager/AP_SerialManager.cpp GOBJECT(serial_manager, "SERIAL", AP_SerialManager), diff --git a/AntennaTracker/Parameters.h b/AntennaTracker/Parameters.h index 1ebae782ed..5bc388ba67 100644 --- a/AntennaTracker/Parameters.h +++ b/AntennaTracker/Parameters.h @@ -106,10 +106,12 @@ public: // // 200 : Radio settings // - k_param_channel_yaw = 200, - k_param_channel_pitch, + k_param_channel_yaw_old = 200, + k_param_channel_pitch_old, k_param_pidPitch2Srv, k_param_pidYaw2Srv, + k_param_rc_channels, + k_param_servo_channels, // // 220: Waypoint data diff --git a/AntennaTracker/Tracker.h b/AntennaTracker/Tracker.h index a2b5430d6e..f16319aa01 100644 --- a/AntennaTracker/Tracker.h +++ b/AntennaTracker/Tracker.h @@ -128,8 +128,8 @@ private: /** antenna control channels */ - RC_Channel channel_yaw{CH_YAW}; - RC_Channel channel_pitch{CH_PITCH}; + RC_Channels rc_channels; + SRV_Channels servo_channels; LowPassFilterFloat yaw_servo_out_filt; LowPassFilterFloat pitch_servo_out_filt; diff --git a/AntennaTracker/control_manual.cpp b/AntennaTracker/control_manual.cpp index 3322bdc15e..00335d9c39 100644 --- a/AntennaTracker/control_manual.cpp +++ b/AntennaTracker/control_manual.cpp @@ -11,10 +11,12 @@ void Tracker::update_manual(void) { // copy yaw and pitch input to output - 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(); - channel_pitch.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_elevator, RC_Channels::rc_channel(CH_PITCH)->get_radio_in()); + SRV_Channels::constrain_pwm(SRV_Channel::k_elevator); + + SRV_Channels::calc_pwm(); + SRV_Channels::output_ch_all(); } diff --git a/AntennaTracker/control_servo_test.cpp b/AntennaTracker/control_servo_test.cpp index 4aba496565..a0508b67bf 100644 --- a/AntennaTracker/control_servo_test.cpp +++ b/AntennaTracker/control_servo_test.cpp @@ -25,16 +25,19 @@ 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.set_radio_out(constrain_int16(pwm, channel_yaw.get_radio_min(), channel_yaw.get_radio_max())); - channel_yaw.output(); + SRV_Channels::set_output_pwm(SRV_Channel::k_steering, pwm); + SRV_Channels::constrain_pwm(SRV_Channel::k_steering); } // set pitch servo pwm and send output to servo if (servo_num == CH_PITCH) { - channel_pitch.set_radio_out(constrain_int16(pwm, channel_pitch.get_radio_min(), channel_pitch.get_radio_max())); - channel_pitch.output(); + SRV_Channels::set_output_pwm(SRV_Channel::k_elevator, pwm); + SRV_Channels::constrain_pwm(SRV_Channel::k_elevator); } + SRV_Channels::calc_pwm(); + SRV_Channels::output_ch_all(); + // return success return true; } diff --git a/AntennaTracker/radio.cpp b/AntennaTracker/radio.cpp index 65bd5691c8..8c85befef1 100644 --- a/AntennaTracker/radio.cpp +++ b/AntennaTracker/radio.cpp @@ -5,7 +5,6 @@ void Tracker::read_radio() { if (hal.rcin->new_input()) { - channel_yaw.set_pwm(hal.rcin->read(CH_YAW)); - channel_pitch.set_pwm(hal.rcin->read(CH_PITCH)); + RC_Channels::set_pwm_all(); } } diff --git a/AntennaTracker/servos.cpp b/AntennaTracker/servos.cpp index 89d5871616..4d850f3d50 100644 --- a/AntennaTracker/servos.cpp +++ b/AntennaTracker/servos.cpp @@ -7,18 +7,19 @@ // init_servos - initialises the servos void Tracker::init_servos() { - // setup antenna control PWM channels - channel_yaw.set_angle(g.yaw_range * 100/2); // yaw range is +/- (YAW_RANGE parameter/2) converted to centi-degrees - channel_pitch.set_angle((-g.pitch_min+g.pitch_max) * 100/2); // pitch range is +/- (PITCH_MIN/MAX parameters/2) converted to centi-degrees + SRV_Channels::set_default_function(CH_YAW, SRV_Channel::k_steering); + SRV_Channels::set_default_function(CH_PITCH, SRV_Channel::k_elevator); - // move servos to mid position - channel_yaw.output_trim(); - channel_pitch.output_trim(); + // yaw range is +/- (YAW_RANGE parameter/2) converted to centi-degrees + SRV_Channels::set_angle(SRV_Channel::k_steering, g.yaw_range * 100/2); - // initialise output to servos - channel_yaw.calc_pwm(); - channel_pitch.calc_pwm(); + // 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::output_trim_all(); + SRV_Channels::calc_pwm(); + SRV_Channels::output_ch_all(); + yaw_servo_out_filt.set_cutoff_frequency(SERVO_OUT_FILT_HZ); pitch_servo_out_filt.set_cutoff_frequency(SERVO_OUT_FILT_HZ); } @@ -45,8 +46,8 @@ void Tracker::update_pitch_servo(float pitch) } // convert servo_out to radio_out and send to servo - channel_pitch.calc_pwm(); - channel_pitch.output(); + SRV_Channels::calc_pwm(); + SRV_Channels::output_ch_all(); } /** @@ -76,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 = channel_pitch.get_servo_out() + g.pidPitch2Srv.get_pid(); + int32_t new_servo_out = SRV_Channels::get_output_scaled(SRV_Channel::k_elevator) + g.pidPitch2Srv.get_pid(); // position limit pitch servo if (new_servo_out <= pitch_min_cd) { @@ -88,7 +89,7 @@ void Tracker::update_pitch_position_servo() g.pidPitch2Srv.reset_I(); } // rate limit pitch servo - channel_pitch.set_servo_out(new_servo_out); + SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, new_servo_out); if (pitch_servo_out_filt_init) { pitch_servo_out_filt.apply(new_servo_out, G_Dt); @@ -110,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) { - channel_pitch.set_servo_out(0); + SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, 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 - channel_pitch.set_servo_out(-9000); + SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, -9000); } else if (pitch*100pitch_min_cd) && (pitch 0) { // positive error means we are counter-clockwise of the target, so // move clockwise - channel_yaw.set_servo_out(18000); + SRV_Channels::set_output_scaled(SRV_Channel::k_steering, 18000); } else { // negative error means we are clockwise of the target, so // move counter-clockwise - channel_yaw.set_servo_out(-18000); + SRV_Channels::set_output_scaled(SRV_Channel::k_steering, -18000); } } @@ -250,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); - channel_yaw.set_servo_out(-g.pidYaw2Srv.get_pid()); + SRV_Channels::set_output_scaled(SRV_Channel::k_steering, -g.pidYaw2Srv.get_pid()); } diff --git a/AntennaTracker/system.cpp b/AntennaTracker/system.cpp index 96aef6eefe..27bef68514 100644 --- a/AntennaTracker/system.cpp +++ b/AntennaTracker/system.cpp @@ -162,15 +162,13 @@ void Tracker::set_home(struct Location temp) } void Tracker::arm_servos() -{ - channel_yaw.enable_out(); - channel_pitch.enable_out(); +{ + hal.util->set_soft_armed(true); } void Tracker::disarm_servos() { - channel_yaw.disable_out(); - channel_pitch.disable_out(); + hal.util->set_soft_armed(false); } /* @@ -179,10 +177,10 @@ void Tracker::disarm_servos() void Tracker::prepare_servos() { start_time_ms = AP_HAL::millis(); - 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(); + 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::calc_pwm(); + SRV_Channels::output_ch_all(); } void Tracker::set_mode(enum ControlMode mode)