diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 68f620396b..4e20e773c0 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -258,6 +258,7 @@ public: bool init(bool ignore_checks) override; void run() override; + void virtual_flybar( float &roll_out, float &pitch_out, float &yaw_out, float pitch_leak, float roll_leak); protected: private: diff --git a/ArduCopter/mode_acro_heli.cpp b/ArduCopter/mode_acro_heli.cpp index b941c581b9..b921116d50 100644 --- a/ArduCopter/mode_acro_heli.cpp +++ b/ArduCopter/mode_acro_heli.cpp @@ -70,7 +70,13 @@ void Copter::ModeAcro_Heli::run() if (!motors->has_flybar()){ // convert the input to the desired body frame rate get_pilot_desired_angle_rates(channel_roll->get_control_in(), channel_pitch->get_control_in(), channel_yaw->get_control_in(), target_roll, target_pitch, target_yaw); - + if (g.acro_trainer == ACRO_TRAINER_DISABLED) { + if (ap.land_complete) { + virtual_flybar(target_roll, target_pitch, target_yaw, 3.0f, 3.0f) + } else { + virtual_flybar(target_roll, target_pitch, target_yaw, g.acro_balance_pitch, g.acro_balance_roll) + } + } if (motors->supports_yaw_passthrough()) { // if the tail on a flybar heli has an external gyro then // also use no deadzone for the yaw control and @@ -112,5 +118,32 @@ void Copter::ModeAcro_Heli::run() attitude_control->set_throttle_out(pilot_throttle_scaled, false, g.throttle_filt); } + +// virtual_flybar - acts like a flybar by leaking target atttitude back to current attitude +void Copter::ModeAcro_Heli::virtual_flybar( float &roll_out, float &pitch_out, float &yaw_out, float pitch_leak, float roll_leak) +{ + Vector3f rate_ef_level, rate_bf_level; + + // get attitude targets + const Vector3f att_target = attitude_control->get_att_target_euler_cd(); + + // Calculate Heli mode earth frame rate command for roll + rate_ef_level.x = -wrap_180_cd(att_target.x - ahrs.roll_sensor) * roll_leak; + + // Calculate Heli mode earth frame rate command for pitch + rate_ef_level.y = -wrap_180_cd(att_target.y - ahrs.pitch_sensor) * pitch_leak; + + // Calculate earth frame rate command for yaw + rate_ef_level.z = 0; + + // convert earth-frame level rates to body-frame level rates + attitude_control->euler_rate_to_ang_vel(attitude_control->get_att_target_euler_cd()*radians(0.01f), rate_ef_level, rate_bf_level); + + // combine earth frame rate corrections with rate requests + roll_out += rate_bf_level.x; + pitch_out += rate_bf_level.y; + yaw_out += rate_bf_level.z; + +} #endif //HELI_FRAME #endif //MODE_ACRO_ENABLED == ENABLED