From 6970a66cad0bf9d91f1f6c7eb43fd5e9638964e5 Mon Sep 17 00:00:00 2001 From: Tom Pittenger Date: Wed, 1 May 2019 13:43:50 -0700 Subject: [PATCH] Plane: refactor stick_mixing to library --- ArduPlane/Attitude.cpp | 24 +++--------------------- ArduPlane/Plane.h | 1 - 2 files changed, 3 insertions(+), 22 deletions(-) diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp index aefac45cc7..d5fd998c64 100644 --- a/ArduPlane/Attitude.cpp +++ b/ArduPlane/Attitude.cpp @@ -121,24 +121,6 @@ void Plane::stabilize_pitch(float speed_scaler) disable_integrator)); } -/* - perform stick mixing on one channel - This type of stick mixing reduces the influence of the auto - controller as it increases the influence of the users stick input, - allowing the user full deflection if needed - */ -void Plane::stick_mix_channel(RC_Channel *channel, int16_t &servo_out) -{ - float ch_inf; - - ch_inf = (float)channel->get_radio_in() - (float)channel->get_radio_trim(); - ch_inf = fabsf(ch_inf); - ch_inf = MIN(ch_inf, 400.0f); - ch_inf = ((400.0f - ch_inf) / 400.0f); - servo_out *= ch_inf; - servo_out += channel->get_control_in(); -} - /* this gives the user control of the aircraft in stabilization modes */ @@ -161,11 +143,11 @@ void Plane::stabilize_stick_mixing_direct() return; } int16_t aileron = SRV_Channels::get_output_scaled(SRV_Channel::k_aileron); - stick_mix_channel(channel_roll, aileron); + aileron = channel_roll->stick_mixing(aileron); SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, aileron); int16_t elevator = SRV_Channels::get_output_scaled(SRV_Channel::k_elevator); - stick_mix_channel(channel_pitch, elevator); + elevator = channel_pitch->stick_mixing(elevator); SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, elevator); } @@ -502,7 +484,7 @@ void Plane::calc_nav_yaw_course(void) int32_t bearing_error_cd = nav_controller->bearing_error_cd(); steering_control.steering = steerController.get_steering_out_angle_error(bearing_error_cd); if (stick_mixing_enabled()) { - stick_mix_channel(channel_rudder, steering_control.steering); + steering_control.steering = channel_rudder->stick_mixing(steering_control.steering); } steering_control.steering = constrain_int16(steering_control.steering, -4500, 4500); } diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 8169a0118e..948cd64cf5 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -1003,7 +1003,6 @@ private: bool stick_mixing_enabled(void); void stabilize_roll(float speed_scaler); void stabilize_pitch(float speed_scaler); - static void stick_mix_channel(RC_Channel *channel, int16_t &servo_out); void stabilize_stick_mixing_direct(); void stabilize_stick_mixing_fbw(); void stabilize_yaw(float speed_scaler);