aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Plane: refactor stick_mixing to library

Browse Source
This commit is contained in:
Tom Pittenger 2019-05-01 13:43:50 -07:00 committed by Tom Pittenger
parent fbe43dba5c
commit 6970a66cad
2 changed files with 3 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
ArduPlane/Attitude.cpp
View File

@ -121,24 +121,6 @@ void Plane::stabilize_pitch(float speed_scaler)
disable_integrator)); 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 this gives the user control of the aircraft in stabilization modes
*/ */
@ -161,11 +143,11 @@ void Plane::stabilize_stick_mixing_direct()
return; return;
} }
int16_t aileron = SRV_Channels::get_output_scaled(SRV_Channel::k_aileron); 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); SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, aileron);
int16_t elevator = SRV_Channels::get_output_scaled(SRV_Channel::k_elevator); 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); 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(); int32_t bearing_error_cd = nav_controller->bearing_error_cd();
steering_control.steering = steerController.get_steering_out_angle_error(bearing_error_cd); steering_control.steering = steerController.get_steering_out_angle_error(bearing_error_cd);
if (stick_mixing_enabled()) { 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); steering_control.steering = constrain_int16(steering_control.steering, -4500, 4500);
} }

1
ArduPlane/Plane.h
View File

@ -1003,7 +1003,6 @@ private:
bool stick_mixing_enabled(void); bool stick_mixing_enabled(void);
void stabilize_roll(float speed_scaler); void stabilize_roll(float speed_scaler);
void stabilize_pitch(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_direct();
void stabilize_stick_mixing_fbw(); void stabilize_stick_mixing_fbw();
void stabilize_yaw(float speed_scaler); void stabilize_yaw(float speed_scaler);