Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AC_AttControlHeli: add passthrough_bf_roll_pitch_rate_yaw

This commit is contained in:
Randy Mackay 2014-08-20 22:14:25 +09:00
parent 968f1d64ae
commit 691a3d8126
2 changed files with 72 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

75
libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp
View File

@ -61,6 +61,64 @@ const AP_Param::GroupInfo AC_AttitudeControl_Heli::var_info[] PROGMEM = {
AP_GROUPEND
};
// passthrough_bf_roll_pitch_rate_yaw - passthrough the pilots roll and pitch inputs directly to swashplate for flybar acro mode
void AC_AttitudeControl_Heli::passthrough_bf_roll_pitch_rate_yaw(float roll_passthrough, float pitch_passthrough, float yaw_rate_bf)
{
// store roll and pitch passthroughs
_passthrough_roll = roll_passthrough;
_passthrough_pitch = pitch_passthrough;
// set rate controller to use pass through
_flags_heli.flybar_passthrough = true;
// set bf rate targets to current body frame rates (i.e. relax and be ready for vehicle to switch out of acro)
_rate_bf_desired.x = _ahrs.get_gyro().x * AC_ATTITUDE_CONTROL_DEGX100;
_rate_bf_desired.y = _ahrs.get_gyro().y * AC_ATTITUDE_CONTROL_DEGX100;
// accel limit desired yaw rate
if (_accel_y_max > 0.0f) {
float rate_change_limit = _accel_y_max * _dt;
float rate_change = yaw_rate_bf - _rate_bf_desired.z;
rate_change = constrain_float(rate_change, -rate_change_limit, rate_change_limit);
_rate_bf_desired.z += rate_change;
} else {
_rate_bf_desired.z = yaw_rate_bf;
}
integrate_bf_rate_error_to_angle_errors();
_angle_bf_error.x = 0;
_angle_bf_error.y = 0;
// update our earth-frame angle targets
Vector3f angle_ef_error;
frame_conversion_bf_to_ef(_angle_bf_error, angle_ef_error);
_angle_ef_target.x = wrap_180_cd_float(angle_ef_error.x + _ahrs.roll_sensor);
_angle_ef_target.y = wrap_180_cd_float(angle_ef_error.y + _ahrs.pitch_sensor);
_angle_ef_target.z = wrap_360_cd_float(angle_ef_error.z + _ahrs.yaw_sensor);
// handle flipping over pitch axis
if (_angle_ef_target.y > 9000.0f) {
_angle_ef_target.x = wrap_180_cd_float(_angle_ef_target.x + 18000.0f);
_angle_ef_target.y = wrap_180_cd_float(18000.0f - _angle_ef_target.x);
_angle_ef_target.z = wrap_360_cd_float(_angle_ef_target.z + 18000.0f);
}
if (_angle_ef_target.y < -9000.0f) {
_angle_ef_target.x = wrap_180_cd_float(_angle_ef_target.x + 18000.0f);
_angle_ef_target.y = wrap_180_cd_float(-18000.0f - _angle_ef_target.x);
_angle_ef_target.z = wrap_360_cd_float(_angle_ef_target.z + 18000.0f);
}
// convert body-frame angle errors to body-frame rate targets
update_rate_bf_targets();
// set body-frame roll/pitch rate target to current desired rates which are the vehicle's actual rates
_rate_bf_target.x = _rate_bf_desired.x;
_rate_bf_target.y = _rate_bf_desired.y;
// add desired target to yaw
_rate_bf_target.z += _rate_bf_desired.z;
}
//
// rate controller (body-frame) methods
//
@ -70,10 +128,11 @@ const AP_Param::GroupInfo AC_AttitudeControl_Heli::var_info[] PROGMEM = {
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?
if (_flags_heli.flybar_passthrough){
passthrough_to_motor_roll_pitch();
}else{
// if using a flybar passthrough roll and pitch directly to motors
if (_flags_heli.flybar_passthrough) {
_motors.set_roll(_passthrough_roll);
_motors.set_pitch(_passthrough_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));
@ -245,14 +304,6 @@ 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)
{

20
libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h
View File

@ -22,18 +22,19 @@ 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,
RC_Channel& rc_roll, RC_Channel& rc_pitch
AC_HELI_PID& pid_rate_roll, AC_HELI_PID& pid_rate_pitch, AC_HELI_PID& pid_rate_yaw
) :
AC_AttitudeControl(ahrs, aparm, motors,
p_angle_roll, p_angle_pitch, p_angle_yaw,
pid_rate_roll, pid_rate_pitch, pid_rate_yaw),
_rc_roll(rc_roll),
_rc_pitch(rc_pitch)
_passthrough_roll(0), _passthrough_pitch(0)
{
AP_Param::setup_object_defaults(this, var_info);
}
// passthrough_bf_roll_pitch_rate_yaw - roll and pitch are passed through directly, body-frame rate target for yaw
void passthrough_bf_roll_pitch_rate_yaw(float roll_passthrough, float pitch_passthrough, float yaw_rate_bf);
// rate_controller_run - run lowest level body-frame rate controller and send outputs to the motors
// should be called at 100hz or more
virtual void rate_controller_run();
@ -57,13 +58,9 @@ private:
uint8_t limit_pitch : 1; // 1 if we have requested larger pitch angle than swash can physically move
uint8_t limit_yaw : 1; // 1 if we have requested larger yaw angle than tail servo can physically move
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
uint8_t flybar_passthrough : 1; // 1 if we should pass through pilots roll & pitch input directly to swash-plate
} _flags_heli;
// references to external libraries
RC_Channel& _rc_roll;
RC_Channel& _rc_pitch;
//
// body-frame rate controller
//
@ -79,8 +76,6 @@ 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
@ -89,6 +84,9 @@ private:
LowPassFilterInt32 roll_feedforward_filter;
LowPassFilterInt32 yaw_feedforward_filter;
// pass through for roll and pitch
int16_t _passthrough_roll;
int16_t _passthrough_pitch;
};
#endif //AC_ATTITUDECONTROL_HELI_H