AC_AttitudeControl_Heli: Add passthrough_to_motor_roll_pitch function.

This commit is contained in:
Robert Lefebvre 2014-07-03 15:49:48 -04:00 committed by Randy Mackay
parent 82061bc65d
commit 9b1d9e3cf0
2 changed files with 25 additions and 3 deletions

View File

@ -71,7 +71,11 @@ void AC_AttitudeControl_Heli::rate_controller_run()
{
// call rate controllers and send output to motors object
// To-Do: should the outputs from get_rate_roll, pitch, yaw be int16_t which is the input to the motors library?
rate_bf_to_motor_roll_pitch(_rate_bf_target.x, _rate_bf_target.y);
if (_flags_heli.flybar_passthrough){
passthrough_to_motor_roll_pitch();
}else{
rate_bf_to_motor_roll_pitch(_rate_bf_target.x, _rate_bf_target.y);
}
_motors.set_yaw(rate_bf_to_motor_yaw(_rate_bf_target.z));
}
@ -241,6 +245,14 @@ static LowPassFilterFloat rate_dynamics_filter; // Rate Dynamics filter
*/
}
// passthrough_to_motor_roll_pitch - passthrough the pilots roll and pitch inputs directly to swashplate for flybar acro mode
void AC_AttitudeControl_Heli::passthrough_to_motor_roll_pitch()
{
// output to motors
_motors.set_roll(_rc_roll.control_in);
_motors.set_pitch(_rc_pitch.control_in);
}
// rate_bf_to_motor_yaw - ask the rate controller to calculate the motor outputs to achieve the target rate in centi-degrees / second
float AC_AttitudeControl_Heli::rate_bf_to_motor_yaw(float rate_target_cds)
{

View File

@ -22,11 +22,14 @@ public:
const AP_Vehicle::MultiCopter &aparm,
AP_MotorsHeli& motors,
AC_P& p_angle_roll, AC_P& p_angle_pitch, AC_P& p_angle_yaw,
AC_HELI_PID& pid_rate_roll, AC_HELI_PID& pid_rate_pitch, AC_HELI_PID& pid_rate_yaw
AC_HELI_PID& pid_rate_roll, AC_HELI_PID& pid_rate_pitch, AC_HELI_PID& pid_rate_yaw,
RC_Channel& rc_roll, RC_Channel& rc_pitch
) :
AC_AttitudeControl(ahrs, aparm, motors,
p_angle_roll, p_angle_pitch, p_angle_yaw,
pid_rate_roll, pid_rate_pitch, pid_rate_yaw)
pid_rate_roll, pid_rate_pitch, pid_rate_yaw),
_rc_roll(rc_roll),
_rc_pitch(rc_pitch)
{
AP_Param::setup_object_defaults(this, var_info);
}
@ -53,6 +56,10 @@ private:
uint8_t leaky_i : 1; // 1 if we should use leaky i term for body-frame rate to motor stage
uint8_t flybar_passthrough : 1; // 1 if we should pass through pilots input directly to swash-plate
} _flags_heli;
// references to external libraries
RC_Channel& _rc_roll;
RC_Channel& _rc_pitch;
//
// body-frame rate controller
@ -69,6 +76,9 @@ private:
// get_angle_boost - calculate total body frame throttle required to produce the given earth frame throttle
virtual int16_t get_angle_boost(int16_t throttle_pwm);
// roll and pitch inputs are sent directly to motor outputs (servos) direct flybar control.
void passthrough_to_motor_roll_pitch();
// LPF filters to act on Rate Feedforward terms to linearize output.
// Due to complicated aerodynamic effects, feedforwards acting too fast can lead
// to jerks on rate change requests.