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

Plane: QHover: move functions to ModeQHover

Browse Source
This commit is contained in:
Iampete1 2021-08-14 19:54:08 +01:00 committed by Andrew Tridgell
parent a1fa683272
commit 737096f342
2 changed files with 25 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
ArduPlane/mode_qhover.cpp
View File

@ -6,9 +6,33 @@ bool ModeQHover::_enter()
return plane.mode_qstabilize._enter(); return plane.mode_qstabilize._enter();
} }
// init quadplane hover mode
void ModeQHover::init()
{
// set vertical speed and acceleration limits
pos_control->set_max_speed_accel_z(-quadplane.get_pilot_velocity_z_max_dn(), quadplane.pilot_velocity_z_max_up, quadplane.pilot_accel_z);
pos_control->set_correction_speed_accel_z(-quadplane.get_pilot_velocity_z_max_dn(), quadplane.pilot_velocity_z_max_up, quadplane.pilot_accel_z);
quadplane.set_climb_rate_cms(0, false);
quadplane.init_throttle_wait();
}
void ModeQHover::update() void ModeQHover::update()
{ {
plane.mode_qstabilize.update(); plane.mode_qstabilize.update();
} }
/*
control QHOVER mode
*/
void ModeQHover::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();
pos_control->relax_z_controller(0);
} else {
quadplane.hold_hover(quadplane.get_pilot_desired_climb_rate_cms());
}
}

26
ArduPlane/quadplane.cpp
View File

@ -1053,17 +1053,6 @@ void QuadPlane::check_attitude_relax(void)
last_att_control_ms = now; last_att_control_ms = now;
} }
// init quadplane hover mode
void ModeQHover::init()
{
// set vertical speed and acceleration limits
pos_control->set_max_speed_accel_z(-quadplane.get_pilot_velocity_z_max_dn(), quadplane.pilot_velocity_z_max_up, quadplane.pilot_accel_z);
pos_control->set_correction_speed_accel_z(-quadplane.get_pilot_velocity_z_max_dn(), quadplane.pilot_velocity_z_max_up, quadplane.pilot_accel_z);
quadplane.set_climb_rate_cms(0, false);
quadplane.init_throttle_wait();
}
/* /*
check for an EKF yaw reset check for an EKF yaw reset
*/ */
@ -1181,21 +1170,6 @@ float QuadPlane::get_pilot_land_throttle(void) const
return constrain_float(throttle_in, 0, 1); return constrain_float(throttle_in, 0, 1);
} }
/*
control QHOVER mode
*/
void ModeQHover::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();
pos_control->relax_z_controller(0);
} else {
quadplane.hold_hover(quadplane.get_pilot_desired_climb_rate_cms());
}
}
void ModeQLoiter::init() void ModeQLoiter::init()
{ {
// initialise loiter // initialise loiter