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

Plane: quadplane: add assisted flight only airmode

Browse Source
This commit is contained in:
Iampete1 2021-08-28 18:51:41 +01:00 committed by Andrew Tridgell
parent 4efa9866fa
commit f1477a6c29
3 changed files with 19 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ArduPlane/defines.h
View File

@ -182,6 +182,7 @@ enum guided_heading_type_t {
enum class AirMode {
OFF,
ON,
ASSISTED_FLIGHT_ONLY,
};
enum class FenceAutoEnable : uint8_t {

20
ArduPlane/quadplane.cpp
View File

@ -936,7 +936,7 @@ void QuadPlane::hold_stabilize(float throttle_in)
// call attitude controller
multicopter_attitude_rate_update(get_desired_yaw_rate_cds());
if ((throttle_in <= 0) && (air_mode == AirMode::OFF)) {
if ((throttle_in <= 0) && !air_mode_active()) {
set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
attitude_control->set_throttle_out(0, true, 0);
relax_attitude_control();
@ -1164,7 +1164,7 @@ bool QuadPlane::is_flying_vtol(void) const
// if we are demanding more than 1% throttle then don't consider aircraft landed
return true;
}
if (plane.control_mode->is_vtol_man_throttle() && air_mode == AirMode::ON) {
if (plane.control_mode->is_vtol_man_throttle() && air_mode_active()) {
// in manual throttle modes with airmode on, don't consider aircraft landed
return true;
}
@ -1233,7 +1233,7 @@ float QuadPlane::landing_descent_rate_cms(float height_above_ground)
*/
float QuadPlane::get_pilot_input_yaw_rate_cds(void) const
{
bool manual_air_mode = plane.control_mode->is_vtol_man_throttle() && (air_mode == AirMode::ON);
bool manual_air_mode = plane.control_mode->is_vtol_man_throttle() && air_mode_active();
if (!manual_air_mode &&
plane.get_throttle_input() <= 0 && !plane.control_mode->does_auto_throttle() &&
plane.arming.get_rudder_arming_type() != AP_Arming::RudderArming::IS_DISABLED && !(inertial_nav.get_velocity_z() < -0.5 * get_pilot_velocity_z_max_dn())) {
@ -1268,7 +1268,7 @@ float QuadPlane::get_desired_yaw_rate_cds(void)
// use bank angle to get desired yaw rate
yaw_cds += desired_auto_yaw_rate_cds();
}
bool manual_air_mode = plane.control_mode->is_vtol_man_throttle() && (air_mode == AirMode::ON);
bool manual_air_mode = plane.control_mode->is_vtol_man_throttle() && air_mode_active();
if (plane.get_throttle_input() <= 0 && !plane.control_mode->does_auto_throttle() && !manual_air_mode && !(inertial_nav.get_velocity_z() < -0.5 * get_pilot_velocity_z_max_dn())) {
// the user may be trying to disarm
return 0;
@ -1865,7 +1865,7 @@ void QuadPlane::update_throttle_suppression(void)
}
// if in a VTOL manual throttle mode and air_mode is on then allow motors to run
if (plane.control_mode->is_vtol_man_throttle() && air_mode == AirMode::ON) {
if (plane.control_mode->is_vtol_man_throttle() && air_mode_active()) {
return;
}
@ -3582,7 +3582,7 @@ void QuadPlane::update_throttle_mix(void)
if (plane.control_mode->is_vtol_man_throttle()) {
// manual throttle
if ((plane.get_throttle_input() <= 0) && (air_mode == AirMode::OFF)) {
if ((plane.get_throttle_input() <= 0) && !air_mode_active()) {
attitude_control->set_throttle_mix_min();
} else {
attitude_control->set_throttle_mix_man();
@ -3795,4 +3795,12 @@ void QuadPlane::set_desired_spool_state(AP_Motors::DesiredSpoolState state)
}
}
bool QuadPlane::air_mode_active() const
{
if ((air_mode == AirMode::ON) || ((air_mode == AirMode::ASSISTED_FLIGHT_ONLY) && assisted_flight)) {
return true;
}
return false;
}
QuadPlane *QuadPlane::_singleton = nullptr;

5
ArduPlane/quadplane.h
View File

@ -180,9 +180,12 @@ private:
// vertical acceleration the pilot may request
AP_Int16 pilot_accel_z;
// air mode state: OFF, ON
// air mode state: OFF, ON, ASSISTED_FLIGHT_ONLY
AirMode air_mode;
// return true if airmode should be active
bool air_mode_active() const;
// check for quadplane assistance needed
bool should_assist(float aspeed, bool have_airspeed);