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

plane: Qacro: move functions to ModeQAcro

Browse Source
This commit is contained in:
Iampete1 2021-08-14 19:52:04 +01:00 committed by Andrew Tridgell
parent bf7474c940
commit e293866245
2 changed files with 51 additions and 51 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

51
ArduPlane/mode_qacro.cpp
View File

@ -6,6 +6,16 @@ bool ModeQAcro::_enter()
return plane.mode_qstabilize._enter();
}
/*
init QACRO mode
*/
void ModeQAcro::init()
{
quadplane.throttle_wait = false;
quadplane.transition_state = QuadPlane::TRANSITION_DONE;
attitude_control->relax_attitude_controllers();
}
void ModeQAcro::update()
{
// get nav_roll and nav_pitch from multicopter attitude controller
@ -15,3 +25,44 @@ void ModeQAcro::update()
return;
}
/*
control QACRO mode
*/
void ModeQAcro::run()
{
if (quadplane.throttle_wait) {
quadplane.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
attitude_control->set_throttle_out(0, true, 0);
quadplane.relax_attitude_control();
} else {
quadplane.check_attitude_relax();
quadplane.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
// convert the input to the desired body frame rate
float target_roll = 0;
float target_pitch = plane.channel_pitch->norm_input() * quadplane.acro_pitch_rate * 100.0f;
float target_yaw = 0;
if (quadplane.tailsitter.enabled()) {
// Note that the 90 degree Y rotation for copter mode swaps body-frame roll and yaw
target_roll = plane.channel_rudder->norm_input() * quadplane.acro_yaw_rate * 100.0f;
target_yaw = -plane.channel_roll->norm_input() * quadplane.acro_roll_rate * 100.0f;
} else {
target_roll = plane.channel_roll->norm_input() * quadplane.acro_roll_rate * 100.0f;
target_yaw = plane.channel_rudder->norm_input() * quadplane.acro_yaw_rate * 100.0;
}
float throttle_out = quadplane.get_pilot_throttle();
// run attitude controller
if (plane.g.acro_locking) {
attitude_control->input_rate_bf_roll_pitch_yaw_3(target_roll, target_pitch, target_yaw);
} else {
attitude_control->input_rate_bf_roll_pitch_yaw_2(target_roll, target_pitch, target_yaw);
}
// output pilot's throttle without angle boost
attitude_control->set_throttle_out(throttle_out, false, 10.0f);
}
}

51
ArduPlane/quadplane.cpp
View File

@ -1053,16 +1053,6 @@ void QuadPlane::check_attitude_relax(void)
last_att_control_ms = now;
}
/*
init QACRO mode
*/
void ModeQAcro::init()
{
quadplane.throttle_wait = false;
quadplane.transition_state = QuadPlane::TRANSITION_DONE;
attitude_control->relax_attitude_controllers();
}
// init quadplane hover mode
void ModeQHover::init()
{
@ -1191,47 +1181,6 @@ float QuadPlane::get_pilot_land_throttle(void) const
return constrain_float(throttle_in, 0, 1);
}
/*
control QACRO mode
*/
void ModeQAcro::run()
{
if (quadplane.throttle_wait) {
quadplane.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
attitude_control->set_throttle_out(0, true, 0);
quadplane.relax_attitude_control();
} else {
quadplane.check_attitude_relax();
quadplane.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
// convert the input to the desired body frame rate
float target_roll = 0;
float target_pitch = plane.channel_pitch->norm_input() * quadplane.acro_pitch_rate * 100.0f;
float target_yaw = 0;
if (quadplane.tailsitter.enabled()) {
// Note that the 90 degree Y rotation for copter mode swaps body-frame roll and yaw
target_roll = plane.channel_rudder->norm_input() * quadplane.acro_yaw_rate * 100.0f;
target_yaw = -plane.channel_roll->norm_input() * quadplane.acro_roll_rate * 100.0f;
} else {
target_roll = plane.channel_roll->norm_input() * quadplane.acro_roll_rate * 100.0f;
target_yaw = plane.channel_rudder->norm_input() * quadplane.acro_yaw_rate * 100.0;
}
float throttle_out = quadplane.get_pilot_throttle();
// run attitude controller
if (plane.g.acro_locking) {
attitude_control->input_rate_bf_roll_pitch_yaw_3(target_roll, target_pitch, target_yaw);
} else {
attitude_control->input_rate_bf_roll_pitch_yaw_2(target_roll, target_pitch, target_yaw);
}
// output pilot's throttle without angle boost
attitude_control->set_throttle_out(throttle_out, false, 10.0f);
}
}
/*
control QHOVER mode
*/