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

Copter: TradHeli - add virtual flybar to heli acro flight mode

Browse Source
This commit is contained in:
Bill Geyer 2019-03-18 12:08:47 -04:00 committed by Randy Mackay
parent 220176f283
commit 0c64cdc565
2 changed files with 35 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ArduCopter/mode.h
View File

@ -258,6 +258,7 @@ public:
bool init(bool ignore_checks) override; bool init(bool ignore_checks) override;
void run() override; void run() override;
void virtual_flybar( float &roll_out, float &pitch_out, float &yaw_out, float pitch_leak, float roll_leak);
protected: protected:
private: private:

35
ArduCopter/mode_acro_heli.cpp
View File

@ -70,7 +70,13 @@ void Copter::ModeAcro_Heli::run()
if (!motors->has_flybar()){ if (!motors->has_flybar()){
// convert the input to the desired body frame rate // 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); 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 (motors->supports_yaw_passthrough()) {
// if the tail on a flybar heli has an external gyro then // if the tail on a flybar heli has an external gyro then
// also use no deadzone for the yaw control and // 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); 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 //HELI_FRAME
#endif //MODE_ACRO_ENABLED == ENABLED #endif //MODE_ACRO_ENABLED == ENABLED