Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

ArduPlane: add and use HAL_QUADPLANE_ENABLED

This commit is contained in:
Peter Barker 2021-09-10 16:28:21 +10:00 committed by Peter Barker
parent 305a8ad48a
commit 3d34e061fe
42 changed files with 755 additions and 204 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
ArduPlane/AP_Arming.cpp
View File

@ -4,6 +4,8 @@
#include "AP_Arming.h"
#include "Plane.h"
#include "qautotune.h"
constexpr uint32_t AP_ARMING_DELAY_MS = 2000; // delay from arming to start of motor spoolup
const AP_Param::GroupInfo AP_Arming_Plane::var_info[] = {
@ -72,6 +74,7 @@ bool AP_Arming_Plane::pre_arm_checks(bool display_failure)
ret = false;
}
#if HAL_QUADPLANE_ENABLED
if (plane.quadplane.enabled() && !plane.quadplane.available()) {
check_failed(display_failure, "Quadplane enabled but not running");
ret = false;
@ -99,6 +102,7 @@ bool AP_Arming_Plane::pre_arm_checks(bool display_failure)
return false;
}
}
#endif
if (plane.control_mode == &plane.mode_auto && plane.mission.num_commands() <= 1) {
check_failed(display_failure, "No mission loaded");
@ -116,11 +120,13 @@ bool AP_Arming_Plane::pre_arm_checks(bool display_failure)
ret = false;
}
#if HAL_QUADPLANE_ENABLED
if (plane.quadplane.enabled() && ((plane.quadplane.options & QuadPlane::OPTION_ONLY_ARM_IN_QMODE_OR_AUTO) != 0) &&
!plane.control_mode->is_vtol_mode() && (plane.control_mode != &plane.mode_auto)) {
check_failed(display_failure,"not in Q mode");
ret = false;
}
#endif
if (plane.g2.flight_options & FlightOptions::CENTER_THROTTLE_TRIM){
int16_t trim = plane.channel_throttle->get_radio_trim();
@ -203,7 +209,9 @@ bool AP_Arming_Plane::arm_checks(AP_Arming::Method method)
void AP_Arming_Plane::change_arm_state(void)
{
update_soft_armed();
#if HAL_QUADPLANE_ENABLED
plane.quadplane.set_armed(hal.util->get_soft_armed());
#endif
}
bool AP_Arming_Plane::arm(const AP_Arming::Method method, const bool do_arming_checks)
@ -212,12 +220,14 @@ bool AP_Arming_Plane::arm(const AP_Arming::Method method, const bool do_arming_c
return false;
}
#if HAL_QUADPLANE_ENABLED
if ((method == Method::AUXSWITCH) && (plane.quadplane.options & QuadPlane::OPTION_AIRMODE)) {
// if no airmode switch assigned, honour the QuadPlane option bit:
if (rc().find_channel_for_option(RC_Channel::AUX_FUNC::AIRMODE) == nullptr) {
plane.quadplane.air_mode = AirMode::ON;
}
}
#endif
change_arm_state();
@ -260,9 +270,11 @@ bool AP_Arming_Plane::disarm(const AP_Arming::Method method, bool do_disarm_chec
plane.throttle_suppressed = plane.control_mode->does_auto_throttle();
// if no airmode switch assigned, ensure airmode is off:
#if HAL_QUADPLANE_ENABLED
if ((plane.quadplane.air_mode == AirMode::ON) && (rc().find_channel_for_option(RC_Channel::AUX_FUNC::AIRMODE) == nullptr)) {
plane.quadplane.air_mode = AirMode::OFF;
}
#endif
//only log if disarming was successful
change_arm_state();
@ -283,8 +295,15 @@ bool AP_Arming_Plane::disarm(const AP_Arming::Method method, bool do_disarm_chec
void AP_Arming_Plane::update_soft_armed()
{
hal.util->set_soft_armed((plane.quadplane.motor_test.running || is_armed()) &&
hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED);
bool _armed = is_armed();
#if HAL_QUADPLANE_ENABLED
if (plane.quadplane.motor_test.running){
_armed = true;
}
#endif
_armed = _armed && hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED;
hal.util->set_soft_armed(_armed);
AP::logger().set_vehicle_armed(hal.util->get_soft_armed());
// update delay_arming oneshot

118
ArduPlane/ArduPlane.cpp
View File

@ -128,7 +128,11 @@ void Plane::get_scheduler_tasks(const AP_Scheduler::Task *&tasks,
log_bit = MASK_LOG_PM;
}
#if HAL_QUADPLANE_ENABLED
constexpr int8_t Plane::_failsafe_priorities[7];
#else
constexpr int8_t Plane::_failsafe_priorities[6];
#endif
// update AHRS system
void Plane::ahrs_update()
@ -145,7 +149,13 @@ void Plane::ahrs_update()
roll_limit_cd = aparm.roll_limit_cd;
pitch_limit_min_cd = aparm.pitch_limit_min_cd;
if (!quadplane.tailsitter.active()) {
bool rotate_limits = true;
#if HAL_QUADPLANE_ENABLED
if (quadplane.tailsitter.active()) {
rotate_limits = false;
}
#endif
if (rotate_limits) {
roll_limit_cd *= ahrs.cos_pitch();
pitch_limit_min_cd *= fabsf(ahrs.cos_roll());
}
@ -156,11 +166,13 @@ void Plane::ahrs_update()
steer_state.locked_course_err += ahrs.get_yaw_rate_earth() * G_Dt;
steer_state.locked_course_err = wrap_PI(steer_state.locked_course_err);
#if HAL_QUADPLANE_ENABLED
// check if we have had a yaw reset from the EKF
quadplane.check_yaw_reset();
// update inertial_nav for quadplane
quadplane.inertial_nav.update();
#endif
}
/*
@ -175,10 +187,12 @@ void Plane::update_speed_height(void)
SpdHgt_Controller->update_50hz();
}
#if HAL_QUADPLANE_ENABLED
if (quadplane.in_vtol_mode() ||
quadplane.in_assisted_flight()) {
quadplane.update_throttle_mix();
}
#endif
}
@ -424,22 +438,37 @@ void Plane::update_control_mode(void)
steer_state.hold_course_cd = -1;
}
update_fly_forward();
control_mode->update();
}
void Plane::update_fly_forward(void)
{
// ensure we are fly-forward when we are flying as a pure fixed
// wing aircraft. This helps the EKF produce better state
// estimates as it can make stronger assumptions
#if HAL_QUADPLANE_ENABLED
if (quadplane.available() &&
quadplane.tailsitter.is_in_fw_flight()) {
ahrs.set_fly_forward(true);
} else if (quadplane.in_vtol_mode() ||
quadplane.in_assisted_flight()) {
ahrs.set_fly_forward(false);
} else if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND) {
ahrs.set_fly_forward(landing.is_flying_forward());
} else {
ahrs.set_fly_forward(true);
return;
}
control_mode->update();
if (quadplane.in_vtol_mode() ||
quadplane.in_assisted_flight()) {
ahrs.set_fly_forward(false);
return;
}
#endif
if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND) {
ahrs.set_fly_forward(landing.is_flying_forward());
return;
}
ahrs.set_fly_forward(true);
}
/*
@ -466,9 +495,11 @@ void Plane::update_alt()
{
barometer.update();
#if HAL_QUADPLANE_ENABLED
if (quadplane.available()) {
quadplane.motors->set_air_density_ratio(barometer.get_air_density_ratio());
}
#endif
// calculate the sink rate.
float sink_rate;
@ -524,9 +555,13 @@ void Plane::update_flight_stage(void)
// Update the speed & height controller states
if (control_mode->does_auto_throttle() && !throttle_suppressed) {
if (control_mode == &mode_auto) {
#if HAL_QUADPLANE_ENABLED
if (quadplane.in_vtol_auto()) {
set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_VTOL);
} else if (auto_state.takeoff_complete == false) {
return;
}
#endif
if (auto_state.takeoff_complete == false) {
set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_TAKEOFF);
} else if (mission.get_current_nav_cmd().id == MAV_CMD_NAV_LAND) {
if (landing.is_commanded_go_around() || flight_stage == AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND) {
@ -539,23 +574,31 @@ void Plane::update_flight_stage(void)
} else {
set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_LAND);
}
} else if (quadplane.in_assisted_flight()) {
set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_VTOL);
} else {
set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_NORMAL);
return;
}
#if HAL_QUADPLANE_ENABLED
if (quadplane.in_assisted_flight()) {
set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_VTOL);
return;
}
#endif
set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_NORMAL);
} else if (control_mode != &mode_takeoff) {
// If not in AUTO then assume normal operation for normal TECS operation.
// This prevents TECS from being stuck in the wrong stage if you switch from
// AUTO to, say, FBWB during a landing, an aborted landing or takeoff.
set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_NORMAL);
}
} else if (quadplane.in_vtol_mode() ||
quadplane.in_assisted_flight()) {
set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_VTOL);
} else {
set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_NORMAL);
return;
}
#if HAL_QUADPLANE_ENABLED
if (quadplane.in_vtol_mode() ||
quadplane.in_assisted_flight()) {
set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_VTOL);
return;
}
#endif
set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_NORMAL);
}
@ -613,11 +656,13 @@ bool Plane::get_wp_distance_m(float &distance) const
if (control_mode == &mode_manual) {
return false;
}
#if HAL_QUADPLANE_ENABLED
if (quadplane.in_vtol_mode()) {
distance = quadplane.using_wp_nav() ? quadplane.wp_nav->get_wp_distance_to_destination() : 0;
} else {
distance = auto_state.wp_distance;
return true;
}
#endif
distance = auto_state.wp_distance;
return true;
}
@ -627,11 +672,13 @@ bool Plane::get_wp_bearing_deg(float &bearing) const
if (control_mode == &mode_manual) {
return false;
}
#if HAL_QUADPLANE_ENABLED
if (quadplane.in_vtol_mode()) {
bearing = quadplane.using_wp_nav() ? quadplane.wp_nav->get_wp_bearing_to_destination() : 0;
} else {
bearing = nav_controller->target_bearing_cd() * 0.01;
return true;
}
#endif
bearing = nav_controller->target_bearing_cd() * 0.01;
return true;
}
@ -641,11 +688,13 @@ bool Plane::get_wp_crosstrack_error_m(float &xtrack_error) const
if (control_mode == &mode_manual) {
return false;
}
#if HAL_QUADPLANE_ENABLED
if (quadplane.in_vtol_mode()) {
xtrack_error = quadplane.using_wp_nav() ? quadplane.wp_nav->crosstrack_error() : 0;
} else {
xtrack_error = nav_controller->crosstrack_error();
return true;
}
#endif
xtrack_error = nav_controller->crosstrack_error();
return true;
}
@ -676,9 +725,11 @@ bool Plane::get_target_location(Location& target_loc)
case Mode::Number::GUIDED:
case Mode::Number::AUTO:
case Mode::Number::LOITER:
#if HAL_QUADPLANE_ENABLED
case Mode::Number::QLOITER:
case Mode::Number::QLAND:
case Mode::Number::QRTL:
#endif
target_loc = next_WP_loc;
return true;
break;
@ -695,12 +746,19 @@ void Plane::get_osd_roll_pitch_rad(float &roll, float &pitch) const
{
pitch = ahrs.pitch;
roll = ahrs.roll;
if (!quadplane.show_vtol_view() && !(g2.flight_options & FlightOptions::OSD_REMOVE_TRIM_PITCH_CD)) { // correct for TRIM_PITCH_CD
pitch -= g.pitch_trim_cd * 0.01 * DEG_TO_RAD;
} else if (!quadplane.show_vtol_view()) {
pitch = quadplane.ahrs_view->pitch;
roll = quadplane.ahrs_view->roll;
#if HAL_QUADPLANE_ENABLED
if (quadplane.show_vtol_view()) {
return;
}
#endif
if (!(g2.flight_options & FlightOptions::OSD_REMOVE_TRIM_PITCH_CD)) { // correct for TRIM_PITCH_CD
pitch -= g.pitch_trim_cd * 0.01 * DEG_TO_RAD;
return;
}
#if HAL_QUADPLANE_ENABLED
pitch = quadplane.ahrs_view->pitch;
roll = quadplane.ahrs_view->roll;
#endif
}
#endif

113
ArduPlane/Attitude.cpp
View File

@ -22,6 +22,7 @@ float Plane::get_speed_scaler(void)
float scale_max = MAX(2.0, (1.5 * aparm.airspeed_max) / g.scaling_speed);
speed_scaler = constrain_float(speed_scaler, scale_min, scale_max);
#if HAL_QUADPLANE_ENABLED
if (quadplane.in_vtol_mode() && hal.util->get_soft_armed()) {
// when in VTOL modes limit surface movement at low speed to prevent instability
float threshold = aparm.airspeed_min * 0.5;
@ -36,6 +37,7 @@ float Plane::get_speed_scaler(void)
yawController.decay_I();
}
}
#endif
} else if (hal.util->get_soft_armed()) {
// scale assumed surface movement using throttle output
float throttle_out = MAX(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle), 1);
@ -64,6 +66,7 @@ bool Plane::stick_mixing_enabled(void)
#else
const bool stickmixing = true;
#endif
#if HAL_QUADPLANE_ENABLED
if (control_mode == &mode_qrtl &&
quadplane.poscontrol.get_state() >= QuadPlane::QPOS_POSITION1) {
// user may be repositioning
@ -73,6 +76,7 @@ bool Plane::stick_mixing_enabled(void)
// user may be repositioning
return false;
}
#endif
if (control_mode->does_auto_throttle() && plane.control_mode->does_auto_navigation()) {
// we're in an auto mode. Check the stick mixing flag
if (g.stick_mixing != STICK_MIXING_DISABLED &&
@ -114,23 +118,30 @@ void Plane::stabilize_roll(float speed_scaler)
if (ahrs.roll_sensor < 0) nav_roll_cd -= 36000;
}
bool disable_integrator = false;
if (control_mode == &mode_stabilize && channel_roll->get_control_in() != 0) {
disable_integrator = true;
}
int32_t roll_out;
const int32_t roll_out = stabilize_roll_get_roll_out(speed_scaler);
SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, roll_out);
}
int32_t Plane::stabilize_roll_get_roll_out(float speed_scaler)
{
#if HAL_QUADPLANE_ENABLED
if (!quadplane.use_fw_attitude_controllers()) {
// use the VTOL rate for control, to ensure consistency
const auto &pid_info = quadplane.attitude_control->get_rate_roll_pid().get_pid_info();
roll_out = rollController.get_rate_out(degrees(pid_info.target), speed_scaler);
const int32_t roll_out = rollController.get_rate_out(degrees(pid_info.target), speed_scaler);
/* when slaving fixed wing control to VTOL control we need to decay the integrator to prevent
opposing integrators balancing between the two controllers
*/
rollController.decay_I();
} else {
roll_out = rollController.get_servo_out(nav_roll_cd - ahrs.roll_sensor, speed_scaler, disable_integrator);
return roll_out;
}
SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, roll_out);
#endif
bool disable_integrator = false;
if (control_mode == &mode_stabilize && channel_roll->get_control_in() != 0) {
disable_integrator = true;
}
return rollController.get_servo_out(nav_roll_cd - ahrs.roll_sensor, speed_scaler, disable_integrator);
}
/*
@ -147,34 +158,46 @@ void Plane::stabilize_pitch(float speed_scaler)
SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, 45*force_elevator);
return;
}
const int32_t pitch_out = stabilize_pitch_get_pitch_out(speed_scaler);
SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, pitch_out);
}
int32_t Plane::stabilize_pitch_get_pitch_out(float speed_scaler)
{
#if HAL_QUADPLANE_ENABLED
if (!quadplane.use_fw_attitude_controllers()) {
// use the VTOL rate for control, to ensure consistency
const auto &pid_info = quadplane.attitude_control->get_rate_pitch_pid().get_pid_info();
const int32_t pitch_out = pitchController.get_rate_out(degrees(pid_info.target), speed_scaler);
/* when slaving fixed wing control to VTOL control we need to decay the integrator to prevent
opposing integrators balancing between the two controllers
*/
pitchController.decay_I();
return pitch_out;
}
#endif
// if LANDING_FLARE RCx_OPTION switch is set and in FW mode, manual throttle,throttle idle then set pitch to LAND_PITCH_CD if flight option FORCE_FLARE_ATTITUDE is set
#if HAL_QUADPLANE_ENABLED
const bool quadplane_in_transition = quadplane.in_transition();
#else
const bool quadplane_in_transition = false;
#endif
int32_t demanded_pitch = nav_pitch_cd + g.pitch_trim_cd + SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) * g.kff_throttle_to_pitch;
bool disable_integrator = false;
if (control_mode == &mode_stabilize && channel_pitch->get_control_in() != 0) {
disable_integrator = true;
}
int32_t pitch_out;
if (!quadplane.use_fw_attitude_controllers()) {
// use the VTOL rate for control, to ensure consistency
const auto &pid_info = quadplane.attitude_control->get_rate_pitch_pid().get_pid_info();
pitch_out = pitchController.get_rate_out(degrees(pid_info.target), speed_scaler);
/* when slaving fixed wing control to VTOL control we need to decay the integrator to prevent
opposing integrators balancing between the two controllers
*/
pitchController.decay_I();
} else {
// if LANDING_FLARE RCx_OPTION switch is set and in FW mode, manual throttle,throttle idle then set pitch to LAND_PITCH_CD if flight option FORCE_FLARE_ATTITUDE is set
if (!quadplane.in_transition() &&
!control_mode->is_vtol_mode() &&
channel_throttle->in_trim_dz() &&
!control_mode->does_auto_throttle() &&
flare_mode == FlareMode::ENABLED_PITCH_TARGET) {
demanded_pitch = landing.get_pitch_cd();
}
pitch_out = pitchController.get_servo_out(demanded_pitch - ahrs.pitch_sensor, speed_scaler, disable_integrator);
if (!quadplane_in_transition &&
!control_mode->is_vtol_mode() &&
channel_throttle->in_trim_dz() &&
!control_mode->does_auto_throttle() &&
flare_mode == FlareMode::ENABLED_PITCH_TARGET) {
demanded_pitch = landing.get_pitch_cd();
}
SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, pitch_out);
return pitchController.get_servo_out(demanded_pitch - ahrs.pitch_sensor, speed_scaler, disable_integrator);
}
/*
@ -188,14 +211,18 @@ void Plane::stabilize_stick_mixing_direct()
control_mode == &mode_autotune ||
control_mode == &mode_fbwb ||
control_mode == &mode_cruise ||
#if HAL_QUADPLANE_ENABLED
control_mode == &mode_qstabilize ||
control_mode == &mode_qhover ||
control_mode == &mode_qloiter ||
control_mode == &mode_qland ||
control_mode == &mode_qrtl ||
control_mode == &mode_qacro ||
control_mode == &mode_training ||
control_mode == &mode_qautotune) {
#if QAUTOTUNE_ENABLED
control_mode == &mode_qautotune ||
#endif
#endif
control_mode == &mode_training) {
return;
}
int16_t aileron = SRV_Channels::get_output_scaled(SRV_Channel::k_aileron);
@ -219,14 +246,18 @@ void Plane::stabilize_stick_mixing_fbw()
control_mode == &mode_autotune ||
control_mode == &mode_fbwb ||
control_mode == &mode_cruise ||
#if HAL_QUADPLANE_ENABLED
control_mode == &mode_qstabilize ||
control_mode == &mode_qhover ||
control_mode == &mode_qloiter ||
control_mode == &mode_qland ||
control_mode == &mode_qrtl ||
control_mode == &mode_qacro ||
control_mode == &mode_training ||
control_mode == &mode_qautotune) {
#if QAUTOTUNE_ENABLED
control_mode == &mode_qautotune ||
#endif
#endif // HAL_QUADPLANE_ENABLED
control_mode == &mode_training) {
return;
}
// do FBW style stick mixing. We don't treat it linearly
@ -431,6 +462,7 @@ void Plane::stabilize()
float speed_scaler = get_speed_scaler();
uint32_t now = AP_HAL::millis();
#if HAL_QUADPLANE_ENABLED
if (quadplane.tailsitter.in_vtol_transition(now)) {
/*
during transition to vtol in a tailsitter try to raise the
@ -441,6 +473,7 @@ void Plane::stabilize()
nav_pitch_cd = constrain_float(quadplane.transition_initial_pitch + (quadplane.tailsitter.transition_rate_vtol * dt) * 0.1f, -8500, 8500);
nav_roll_cd = 0;
}
#endif
if (now - last_stabilize_ms > 2000) {
// if we haven't run the rate controllers for 2 seconds then
@ -459,6 +492,7 @@ void Plane::stabilize()
stabilize_training(speed_scaler);
} else if (control_mode == &mode_acro) {
stabilize_acro(speed_scaler);
#if HAL_QUADPLANE_ENABLED
} else if (control_mode->is_vtol_mode() && !quadplane.tailsitter.in_vtol_transition(now)) {
// run controlers specific to this mode
plane.control_mode->run();
@ -470,7 +504,7 @@ void Plane::stabilize()
plane.stabilize_roll(speed_scaler);
plane.stabilize_pitch(speed_scaler);
}
#endif
} else {
if (g.stick_mixing == STICK_MIXING_FBW && control_mode != &mode_stabilize) {
stabilize_stick_mixing_fbw();
@ -718,6 +752,7 @@ void Plane::update_load_factor(void)
}
aerodynamic_load_factor = 1.0f / safe_sqrt(cosf(radians(demanded_roll)));
#if HAL_QUADPLANE_ENABLED
if (quadplane.in_transition() &&
(quadplane.options & QuadPlane::OPTION_LEVEL_TRANSITION)) {
// the user wants transitions to be kept level to within LEVEL_ROLL_LIMIT
@ -725,7 +760,8 @@ void Plane::update_load_factor(void)
nav_roll_cd = constrain_int32(nav_roll_cd, -roll_limit_cd, roll_limit_cd);
return;
}
#endif
if (!aparm.stall_prevention) {
// stall prevention is disabled
return;
@ -734,11 +770,12 @@ void Plane::update_load_factor(void)
// no roll limits when inverted
return;
}
#if HAL_QUADPLANE_ENABLED
if (quadplane.tailsitter.active()) {
// no limits while hovering
return;
}
#endif
float max_load_factor = smoothed_airspeed / MAX(aparm.airspeed_min, 1);
if (max_load_factor <= 1) {

77
ArduPlane/GCS_Mavlink.cpp
View File

@ -4,7 +4,11 @@
MAV_TYPE GCS_Plane::frame_type() const
{
#if HAL_QUADPLANE_ENABLED
return plane.quadplane.get_mav_type();
#else
return MAV_TYPE_FIXED_WING;
#endif
}
MAV_MODE GCS_MAVLINK_Plane::base_mode() const
@ -23,19 +27,25 @@ MAV_MODE GCS_MAVLINK_Plane::base_mode() const
case Mode::Number::MANUAL:
case Mode::Number::TRAINING:
case Mode::Number::ACRO:
#if HAL_QUADPLANE_ENABLED
case Mode::Number::QACRO:
_base_mode = MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
break;
#endif
case Mode::Number::STABILIZE:
case Mode::Number::FLY_BY_WIRE_A:
case Mode::Number::AUTOTUNE:
case Mode::Number::FLY_BY_WIRE_B:
#if HAL_QUADPLANE_ENABLED
case Mode::Number::QSTABILIZE:
case Mode::Number::QHOVER:
case Mode::Number::QLOITER:
case Mode::Number::QLAND:
case Mode::Number::CRUISE:
#if QAUTOTUNE_ENABLED
case Mode::Number::QAUTOTUNE:
#endif
#endif // HAL_QUADPLANE_ENABLED
case Mode::Number::CRUISE:
_base_mode = MAV_MODE_FLAG_STABILIZE_ENABLED;
break;
case Mode::Number::AUTO:
@ -46,7 +56,9 @@ MAV_MODE GCS_MAVLINK_Plane::base_mode() const
case Mode::Number::GUIDED:
case Mode::Number::CIRCLE:
case Mode::Number::TAKEOFF:
#if HAL_QUADPLANE_ENABLED
case Mode::Number::QRTL:
#endif
_base_mode = MAV_MODE_FLAG_GUIDED_ENABLED |
MAV_MODE_FLAG_STABILIZE_ENABLED;
// note that MAV_MODE_FLAG_AUTO_ENABLED does not match what
@ -120,13 +132,15 @@ void GCS_MAVLINK_Plane::send_attitude() const
if (!(plane.g2.flight_options & FlightOptions::GCS_REMOVE_TRIM_PITCH_CD)) {
p -= radians(plane.g.pitch_trim_cd * 0.01f);
}
#if HAL_QUADPLANE_ENABLED
if (plane.quadplane.show_vtol_view()) {
r = plane.quadplane.ahrs_view->roll;
p = plane.quadplane.ahrs_view->pitch;
y = plane.quadplane.ahrs_view->yaw;
}
#endif
const Vector3f &omega = ahrs.get_gyro();
mavlink_msg_attitude_send(
chan,
@ -155,6 +169,7 @@ void GCS_MAVLINK_Plane::send_nav_controller_output() const
if (plane.control_mode == &plane.mode_manual) {
return;
}
#if HAL_QUADPLANE_ENABLED
const QuadPlane &quadplane = plane.quadplane;
if (quadplane.show_vtol_view()) {
const Vector3f &targets = quadplane.attitude_control->get_att_target_euler_cd();
@ -170,7 +185,10 @@ void GCS_MAVLINK_Plane::send_nav_controller_output() const
(plane.control_mode != &plane.mode_qstabilize) ? quadplane.pos_control->get_pos_error_z_cm() * 1.0e-2f : 0,
plane.airspeed_error * 100,
wp_nav_valid ? quadplane.wp_nav->crosstrack_error() : 0);
} else {
return;
}
#endif
{
const AP_Navigation *nav_controller = plane.nav_controller;
mavlink_msg_nav_controller_output_send(
chan,
@ -295,27 +313,30 @@ void GCS_MAVLINK_Plane::send_pid_tuning()
const AP_Logger::PID_Info *pid_info;
if (g.gcs_pid_mask & TUNING_BITS_ROLL) {
pid_info = &plane.rollController.get_pid_info();
#if HAL_QUADPLANE_ENABLED
if (plane.quadplane.in_vtol_mode()) {
pid_info = &plane.quadplane.attitude_control->get_rate_roll_pid().get_pid_info();
} else {
pid_info = &plane.rollController.get_pid_info();
}
#endif
send_pid_info(pid_info, PID_TUNING_ROLL, pid_info->actual);
}
if (g.gcs_pid_mask & TUNING_BITS_PITCH) {
pid_info = &plane.pitchController.get_pid_info();
#if HAL_QUADPLANE_ENABLED
if (plane.quadplane.in_vtol_mode()) {
pid_info = &plane.quadplane.attitude_control->get_rate_pitch_pid().get_pid_info();
} else {
pid_info = &plane.pitchController.get_pid_info();
}
#endif
send_pid_info(pid_info, PID_TUNING_PITCH, pid_info->actual);
}
if (g.gcs_pid_mask & TUNING_BITS_YAW) {
pid_info = &plane.yawController.get_pid_info();
#if HAL_QUADPLANE_ENABLED
if (plane.quadplane.in_vtol_mode()) {
pid_info = &plane.quadplane.attitude_control->get_rate_yaw_pid().get_pid_info();
} else {
pid_info = &plane.yawController.get_pid_info();
}
#endif
send_pid_info(pid_info, PID_TUNING_YAW, pid_info->actual);
}
if (g.gcs_pid_mask & TUNING_BITS_STEER) {
@ -327,10 +348,12 @@ void GCS_MAVLINK_Plane::send_pid_tuning()
const Vector3f &gyro = ahrs.get_gyro();
send_pid_info(plane.landing.get_pid_info(), PID_TUNING_LANDING, degrees(gyro.z));
}
#if HAL_QUADPLANE_ENABLED
if (g.gcs_pid_mask & TUNING_BITS_ACCZ && plane.quadplane.in_vtol_mode()) {
pid_info = &plane.quadplane.pos_control->get_accel_z_pid().get_pid_info();
send_pid_info(pid_info, PID_TUNING_ACCZ, pid_info->actual);
}
#endif
}
uint8_t GCS_MAVLINK_Plane::sysid_my_gcs() const
@ -917,6 +940,7 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_long_packet(const mavlink_command_l
plane.set_mode(plane.mode_rtl, ModeReason::GCS_COMMAND);
return MAV_RESULT_ACCEPTED;
#if HAL_QUADPLANE_ENABLED
case MAV_CMD_NAV_TAKEOFF: {
// user takeoff only works with quadplane code for now
// param7 : altitude [metres]
@ -926,6 +950,7 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_long_packet(const mavlink_command_l
}
return MAV_RESULT_FAILED;
}
#endif // HAL_QUADPLANE_ENABLED
case MAV_CMD_MISSION_START:
plane.set_mode(plane.mode_auto, ModeReason::GCS_COMMAND);
@ -953,9 +978,15 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_long_packet(const mavlink_command_l
// only fly a fixed wing abort if we aren't doing quadplane stuff, or potentially
// shooting a quadplane approach
if ((!plane.quadplane.available()) ||
#if HAL_QUADPLANE_ENABLED
const bool attempt_go_around =
(!plane.quadplane.available()) ||
((!plane.quadplane.in_vtol_auto()) &&
(!(plane.quadplane.options & QuadPlane::OPTION_MISSION_LAND_FW_APPROACH)))) {
(!(plane.quadplane.options & QuadPlane::OPTION_MISSION_LAND_FW_APPROACH)));
#else
const bool attempt_go_around = true;
#endif
if (attempt_go_around) {
// Initiate an aborted landing. This will trigger a pitch-up and
// climb-out to a safe altitude holding heading then one of the
// following actions will occur, check for in this order:
@ -1039,6 +1070,7 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_long_packet(const mavlink_command_l
return MAV_RESULT_FAILED;
#endif
#if HAL_QUADPLANE_ENABLED
case MAV_CMD_DO_MOTOR_TEST:
// param1 : motor sequence number (a number from 1 to max number of motors on the vehicle)
// param2 : throttle type (0=throttle percentage, 1=PWM, 2=pilot throttle channel pass-through. See MOTOR_TEST_THROTTLE_TYPE enum)
@ -1057,6 +1089,7 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_long_packet(const mavlink_command_l
return MAV_RESULT_FAILED;
}
return MAV_RESULT_ACCEPTED;
#endif
case MAV_CMD_DO_ENGINE_CONTROL:
if (!plane.g2.ice_control.engine_control(packet.param1, packet.param2, packet.param3)) {
@ -1337,39 +1370,43 @@ int16_t GCS_MAVLINK_Plane::high_latency_target_altitude() const
struct Location global_position_current;
UNUSED_RESULT(ahrs.get_position(global_position_current));
#if HAL_QUADPLANE_ENABLED
const QuadPlane &quadplane = plane.quadplane;
//return units are m
if (quadplane.show_vtol_view()) {
return (plane.control_mode != &plane.mode_qstabilize) ? 0.01 * (global_position_current.alt + quadplane.pos_control->get_pos_error_z_cm()) : 0;
} else {
return 0.01 * (global_position_current.alt + plane.altitude_error_cm);
}
#endif
return 0.01 * (global_position_current.alt + plane.altitude_error_cm);
}
uint8_t GCS_MAVLINK_Plane::high_latency_tgt_heading() const
{
// return units are deg/2
#if HAL_QUADPLANE_ENABLED
const QuadPlane &quadplane = plane.quadplane;
if (quadplane.show_vtol_view()) {
const Vector3f &targets = quadplane.attitude_control->get_att_target_euler_cd();
return ((uint16_t)(targets.z * 0.01)) / 2;
} else {
}
#endif
const AP_Navigation *nav_controller = plane.nav_controller;
// need to convert -18000->18000 to 0->360/2
return wrap_360_cd(nav_controller->target_bearing_cd() ) / 200;
}
}
// return units are dm
uint16_t GCS_MAVLINK_Plane::high_latency_tgt_dist() const
{
// return units are dm
#if HAL_QUADPLANE_ENABLED
const QuadPlane &quadplane = plane.quadplane;
if (quadplane.show_vtol_view()) {
bool wp_nav_valid = quadplane.using_wp_nav();
return (wp_nav_valid ? MIN(quadplane.wp_nav->get_wp_distance_to_destination(), UINT16_MAX) : 0) / 10;
} else {
return MIN(plane.auto_state.wp_distance, UINT16_MAX) / 10;
}
#endif
return MIN(plane.auto_state.wp_distance, UINT16_MAX) / 10;
}
uint8_t GCS_MAVLINK_Plane::high_latency_tgt_airspeed() const

8
ArduPlane/GCS_Plane.cpp
View File

@ -44,18 +44,24 @@ void GCS_Plane::update_vehicle_sensor_status_flags(void)
break;
case Mode::Number::ACRO:
#if HAL_QUADPLANE_ENABLED
case Mode::Number::QACRO:
#endif
rate_controlled = true;
break;
case Mode::Number::STABILIZE:
case Mode::Number::FLY_BY_WIRE_A:
case Mode::Number::AUTOTUNE:
#if HAL_QUADPLANE_ENABLED
case Mode::Number::QSTABILIZE:
case Mode::Number::QHOVER:
case Mode::Number::QLAND:
case Mode::Number::QLOITER:
#if QAUTOTUNE_ENABLED
case Mode::Number::QAUTOTUNE:
#endif
#endif // HAL_QUADPLANE_ENABLED
case Mode::Number::FLY_BY_WIRE_B:
case Mode::Number::CRUISE:
rate_controlled = true;
@ -76,7 +82,9 @@ void GCS_Plane::update_vehicle_sensor_status_flags(void)
case Mode::Number::GUIDED:
case Mode::Number::CIRCLE:
case Mode::Number::TAKEOFF:
#if HAL_QUADPLANE_ENABLED
case Mode::Number::QRTL:
#endif
case Mode::Number::THERMAL:
rate_controlled = true;
attitude_stabilized = true;

4
ArduPlane/Log.cpp
View File

@ -10,6 +10,7 @@ void Plane::Log_Write_Attitude(void)
targets.y = nav_pitch_cd;
targets.z = 0; //Plane does not have the concept of navyaw. This is a placeholder.
#if HAL_QUADPLANE_ENABLED
if (quadplane.show_vtol_view()) {
// we need the attitude targets from the AC_AttitudeControl controller, as they
// account for the acceleration limits.
@ -29,6 +30,7 @@ void Plane::Log_Write_Attitude(void)
logger.Write_PID(LOG_PIQY_MSG, quadplane.attitude_control->get_rate_yaw_pid().get_pid_info());
logger.Write_PID(LOG_PIQA_MSG, quadplane.pos_control->get_accel_z_pid().get_pid_info() );
}
#endif
logger.Write_PID(LOG_PIDR_MSG, rollController.get_pid_info());
logger.Write_PID(LOG_PIDP_MSG, pitchController.get_pid_info());
@ -521,10 +523,12 @@ void Plane::Log_Write_Vehicle_Startup_Messages()
{
// only 200(?) bytes are guaranteed by AP_Logger
Log_Write_Startup(TYPE_GROUNDSTART_MSG);
#if HAL_QUADPLANE_ENABLED
if (quadplane.initialised) {
logger.Write_MessageF("QuadPlane Frame: %s/%s", quadplane.motors->get_frame_string(),
quadplane.motors->get_type_string());
}
#endif
logger.Write_Mode(control_mode->mode_number(), control_mode_reason);
ahrs.Log_Write_Home_And_Origin();
gps.Write_AP_Logger_Log_Startup_messages();

12
ArduPlane/Parameters.cpp
View File

@ -792,20 +792,24 @@ const AP_Param::Info Plane::var_info[] = {
GOBJECT(avoidance_adsb, "AVD_", AP_Avoidance_Plane),
#endif
#if HAL_QUADPLANE_ENABLED
// @Group: Q_
// @Path: quadplane.cpp
GOBJECT(quadplane, "Q_", QuadPlane),
#endif
// @Group: TUNE_
// @Path: tuning.cpp,../libraries/AP_Tuning/AP_Tuning.cpp
GOBJECT(tuning, "TUNE_", AP_Tuning_Plane),
#if HAL_QUADPLANE_ENABLED
// @Group: Q_A_
// @Path: ../libraries/AC_AttitudeControl/AC_AttitudeControl.cpp,../libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp
{ AP_PARAM_GROUP, "Q_A_", Parameters::k_param_q_attitude_control,
(const void *)&plane.quadplane.attitude_control,
{group_info : AC_AttitudeControl_Multi::var_info}, AP_PARAM_FLAG_POINTER },
#endif
// @Group: RLL
// @Path: ../libraries/APM_Control/AP_RollController.cpp
GOBJECT(rollController, "RLL", AP_RollController),
@ -1359,11 +1363,13 @@ void Plane::load_parameters(void)
// possibly convert elevon and vtail mixers
convert_mixers();
#if HAL_QUADPLANE_ENABLED
if (quadplane.enable) {
// quadplanes needs a higher loop rate
AP_Param::set_default_by_name("SCHED_LOOP_RATE", 300);
}
#endif
AP_Param::set_frame_type_flags(AP_PARAM_FRAME_PLANE);

24
ArduPlane/Plane.h
View File

@ -283,13 +283,17 @@ private:
ModeGuided mode_guided;
ModeInitializing mode_initializing;
ModeManual mode_manual;
#if HAL_QUADPLANE_ENABLED
ModeQStabilize mode_qstabilize;
ModeQHover mode_qhover;
ModeQLoiter mode_qloiter;
ModeQLand mode_qland;
ModeQRTL mode_qrtl;
ModeQAcro mode_qacro;
#if QAUTOTUNE_ENABLED
ModeQAutotune mode_qautotune;
#endif // QAUTOTUNE_ENABLED
#endif // HAL_QUADPLANE_ENABLED
ModeTakeoff mode_takeoff;
#if HAL_SOARING_ENABLED
ModeThermal mode_thermal;
@ -354,11 +358,13 @@ private:
VTOL_LANDING,
};
#if HAL_QUADPLANE_ENABLED
// Landing
struct {
enum Landing_ApproachStage approach_stage;
float approach_direction_deg;
} vtol_approach_s;
#endif
bool any_failsafe_triggered() {
return failsafe.state != FAILSAFE_NONE || battery.has_failsafed() || failsafe.adsb;
@ -770,8 +776,10 @@ private:
// time that rudder arming has been running
uint32_t rudder_arm_timer;
#if HAL_QUADPLANE_ENABLED
// support for quadcopter-plane
QuadPlane quadplane{ahrs};
#endif
// support for transmitter tuning
AP_Tuning_Plane tuning;
@ -845,7 +853,9 @@ private:
float get_speed_scaler(void);
bool stick_mixing_enabled(void);
void stabilize_roll(float speed_scaler);
int32_t stabilize_roll_get_roll_out(float speed_scaler);
void stabilize_pitch(float speed_scaler);
int32_t stabilize_pitch_get_pitch_out(float speed_scaler);
void stabilize_stick_mixing_direct();
void stabilize_stick_mixing_fbw();
void stabilize_yaw(float speed_scaler);
@ -897,7 +907,9 @@ private:
void do_takeoff(const AP_Mission::Mission_Command& cmd);
void do_nav_wp(const AP_Mission::Mission_Command& cmd);
void do_land(const AP_Mission::Mission_Command& cmd);
#if HAL_QUADPLANE_ENABLED
void do_landing_vtol_approach(const AP_Mission::Mission_Command& cmd);
#endif
void loiter_set_direction_wp(const AP_Mission::Mission_Command& cmd);
void do_loiter_unlimited(const AP_Mission::Mission_Command& cmd);
void do_loiter_turns(const AP_Mission::Mission_Command& cmd);
@ -908,7 +920,9 @@ private:
void do_vtol_takeoff(const AP_Mission::Mission_Command& cmd);
void do_vtol_land(const AP_Mission::Mission_Command& cmd);
bool verify_nav_wp(const AP_Mission::Mission_Command& cmd);
#if HAL_QUADPLANE_ENABLED
bool verify_landing_vtol_approach(const AP_Mission::Mission_Command& cmd);
#endif
void do_wait_delay(const AP_Mission::Mission_Command& cmd);
void do_within_distance(const AP_Mission::Mission_Command& cmd);
bool do_change_speed(const AP_Mission::Mission_Command& cmd);
@ -923,8 +937,10 @@ private:
bool set_home_persistently(const Location &loc) WARN_IF_UNUSED;
// quadplane.cpp
#if HAL_QUADPLANE_ENABLED
bool verify_vtol_takeoff(const AP_Mission::Mission_Command &cmd);
bool verify_vtol_land(const AP_Mission::Mission_Command &cmd);
#endif
// control_modes.cpp
void read_control_switch();
@ -943,6 +959,7 @@ private:
void failsafe_short_off_event(ModeReason reason);
void failsafe_long_off_event(ModeReason reason);
void handle_battery_failsafe(const char* type_str, const int8_t action);
bool failsafe_in_landing_sequence() const; // returns true if the vehicle is in landing sequence. Intended only for use in failsafe code.
#if AC_FENCE == ENABLED
// fence.cpp
@ -978,6 +995,7 @@ private:
void update_logging1(void);
void update_logging2(void);
void update_control_mode(void);
void update_fly_forward(void);
void update_flight_stage();
void set_flight_stage(AP_Vehicle::FixedWing::FlightStage fs);
@ -986,8 +1004,10 @@ private:
void loiter_angle_update(void);
void navigate();
void calc_airspeed_errors();
float mode_auto_target_airspeed_cm();
void calc_gndspeed_undershoot();
void update_loiter(uint16_t radius);
void update_loiter_update_nav(uint16_t radius);
void update_cruise();
void update_fbwb_speed_height(void);
void setup_turn_angle(void);
@ -1099,7 +1119,9 @@ private:
Failsafe_Action_RTL = 1,
Failsafe_Action_Land = 2,
Failsafe_Action_Terminate = 3,
#if HAL_QUADPLANE_ENABLED
Failsafe_Action_QLand = 4,
#endif
Failsafe_Action_Parachute = 5
};
@ -1107,7 +1129,9 @@ private:
static constexpr int8_t _failsafe_priorities[] = {
Failsafe_Action_Terminate,
Failsafe_Action_Parachute,
#if HAL_QUADPLANE_ENABLED
Failsafe_Action_QLand,
#endif
Failsafe_Action_Land,
Failsafe_Action_RTL,
Failsafe_Action_None,

12
ArduPlane/RC_Channel.cpp
View File

@ -51,6 +51,7 @@ void RC_Channel_Plane::do_aux_function_change_mode(const Mode::Number number,
}
}
#if HAL_QUADPLANE_ENABLED
void RC_Channel_Plane::do_aux_function_q_assist_state(AuxSwitchPos ch_flag)
{
switch(ch_flag) {
@ -70,6 +71,7 @@ void RC_Channel_Plane::do_aux_function_q_assist_state(AuxSwitchPos ch_flag)
break;
}
}
#endif // HAL_QUADPLANE_ENABLED
void RC_Channel_Plane::do_aux_function_crow_mode(AuxSwitchPos ch_flag)
{
@ -115,14 +117,20 @@ void RC_Channel_Plane::do_aux_function_flare(AuxSwitchPos ch_flag)
switch(ch_flag) {
case AuxSwitchPos::HIGH:
plane.flare_mode = Plane::FlareMode::ENABLED_PITCH_TARGET;
#if HAL_QUADPLANE_ENABLED
plane.quadplane.set_q_assist_state(plane.quadplane.Q_ASSIST_STATE_ENUM::Q_ASSIST_DISABLED);
#endif
break;
case AuxSwitchPos::MIDDLE:
plane.flare_mode = Plane::FlareMode::ENABLED_NO_PITCH_TARGET;
#if HAL_QUADPLANE_ENABLED
plane.quadplane.set_q_assist_state(plane.quadplane.Q_ASSIST_STATE_ENUM::Q_ASSIST_DISABLED);
#endif
break;
case AuxSwitchPos::LOW:
#if HAL_QUADPLANE_ENABLED
plane.quadplane.set_q_assist_state(plane.quadplane.Q_ASSIST_STATE_ENUM::Q_ASSIST_ENABLED);
#endif
plane.flare_mode = Plane::FlareMode::FLARE_DISABLED;
break;
}
@ -244,9 +252,11 @@ bool RC_Channel_Plane::do_aux_function(const aux_func_t ch_option, const AuxSwit
case AUX_FUNC::FBWA_TAILDRAGGER:
break; // input labels, nothing to do
#if HAL_QUADPLANE_ENABLED
case AUX_FUNC::Q_ASSIST:
do_aux_function_q_assist_state(ch_flag);
break;
#endif
case AUX_FUNC::FWD_THR:
break; // VTOL forward throttle input label, nothing to do
@ -275,6 +285,7 @@ bool RC_Channel_Plane::do_aux_function(const aux_func_t ch_option, const AuxSwit
do_aux_function_crow_mode(ch_flag);
break;
#if HAL_QUADPLANE_ENABLED
case AUX_FUNC::AIRMODE:
switch (ch_flag) {
case AuxSwitchPos::HIGH:
@ -287,6 +298,7 @@ bool RC_Channel_Plane::do_aux_function(const aux_func_t ch_option, const AuxSwit
break;
}
break;
#endif
case AUX_FUNC::ARSPD_CALIBRATE:
#if AP_AIRSPEED_AUTOCAL_ENABLE

2
ArduPlane/RC_Channel.h
View File

@ -18,7 +18,9 @@ private:
void do_aux_function_change_mode(Mode::Number number,
AuxSwitchPos ch_flag);
#if HAL_QUADPLANE_ENABLED
void do_aux_function_q_assist_state(AuxSwitchPos ch_flag);
#endif
void do_aux_function_crow_mode(AuxSwitchPos ch_flag);

8
ArduPlane/afs_plane.cpp
View File

@ -11,11 +11,13 @@
*/
void AP_AdvancedFailsafe_Plane::terminate_vehicle(void)
{
#if HAL_QUADPLANE_ENABLED
if (plane.quadplane.available() && _terminate_action == TERMINATE_ACTION_LAND) {
// perform a VTOL landing
plane.set_mode(plane.mode_qland, ModeReason::FENCE_BREACHED);
return;
}
#endif
plane.g2.servo_channels.disable_passthrough(true);
@ -45,8 +47,10 @@ void AP_AdvancedFailsafe_Plane::terminate_vehicle(void)
plane.servos_output();
#if HAL_QUADPLANE_ENABLED
plane.quadplane.afs_terminate();
#endif
// also disarm to ensure that ignition is cut
plane.arming.disarm(AP_Arming::Method::AFS);
}
@ -73,11 +77,13 @@ void AP_AdvancedFailsafe_Plane::setup_IO_failsafe(void)
SRV_Channels::set_failsafe_limit(SRV_Channel::k_manual, SRV_Channel::Limit::TRIM);
SRV_Channels::set_failsafe_limit(SRV_Channel::k_none, SRV_Channel::Limit::TRIM);
#if HAL_QUADPLANE_ENABLED
if (plane.quadplane.available()) {
// setup AP_Motors outputs for failsafe
uint16_t mask = plane.quadplane.motors->get_motor_mask();
hal.rcout->set_failsafe_pwm(mask, plane.quadplane.thr_min_pwm);
}
#endif
}
/*

22
ArduPlane/altitude.cpp
View File

@ -157,12 +157,14 @@ float Plane::relative_ground_altitude(bool use_rangefinder_if_available)
return rangefinder_state.height_estimate;
}
#if HAL_QUADPLANE_ENABLED
if (use_rangefinder_if_available && quadplane.in_vtol_land_final() &&
rangefinder.status_orient(ROTATION_PITCH_270) == RangeFinder::Status::OutOfRangeLow) {
// a special case for quadplane landing when rangefinder goes
// below minimum. Consider our height above ground to be zero
return 0;
}
#endif
#if AP_TERRAIN_AVAILABLE
float altitude;
@ -173,6 +175,7 @@ float Plane::relative_ground_altitude(bool use_rangefinder_if_available)
}
#endif
#if HAL_QUADPLANE_ENABLED
if (quadplane.in_vtol_land_descent() &&
!(quadplane.options & QuadPlane::OPTION_MISSION_LAND_FW_APPROACH)) {
// when doing a VTOL landing we can use the waypoint height as
@ -181,6 +184,7 @@ float Plane::relative_ground_altitude(bool use_rangefinder_if_available)
// height
return height_above_target();
}
#endif
return relative_altitude;
}
@ -668,11 +672,19 @@ void Plane::rangefinder_height_update(void)
}
} else {
rangefinder_state.in_range = true;
bool flightstage_good_for_rangefinder_landing = false;
if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND) {
flightstage_good_for_rangefinder_landing = true;
}
#if HAL_QUADPLANE_ENABLED
if (control_mode == &mode_qland ||
control_mode == &mode_qrtl ||
(control_mode == &mode_auto && quadplane.is_vtol_land(plane.mission.get_current_nav_cmd().id))) {
flightstage_good_for_rangefinder_landing = true;
}
#endif
if (!rangefinder_state.in_use &&
(flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND ||
control_mode == &mode_qland ||
control_mode == &mode_qrtl ||
(control_mode == &mode_auto && quadplane.is_vtol_land(plane.mission.get_current_nav_cmd().id))) &&
flightstage_good_for_rangefinder_landing &&
g.rangefinder_landing) {
rangefinder_state.in_use = true;
gcs().send_text(MAV_SEVERITY_INFO, "Rangefinder engaged at %.2fm", (double)rangefinder_state.height_estimate);
@ -745,9 +757,11 @@ const Plane::TerrainLookupTable Plane::Terrain_lookup[] = {
{Mode::Number::GUIDED, terrain_bitmask::GUIDED},
{Mode::Number::LOITER, terrain_bitmask::LOITER},
{Mode::Number::CIRCLE, terrain_bitmask::CIRCLE},
#if HAL_QUADPLANE_ENABLED
{Mode::Number::QRTL, terrain_bitmask::QRTL},
{Mode::Number::QLAND, terrain_bitmask::QLAND},
{Mode::Number::QLOITER, terrain_bitmask::QLOITER},
#endif
};
bool Plane::terrain_enabled_in_current_mode() const

18
ArduPlane/avoidance_adsb.cpp
View File

@ -24,11 +24,19 @@ MAV_COLLISION_ACTION AP_Avoidance_Plane::handle_avoidance(const AP_Avoidance::Ob
}
// take no action in some flight modes
bool flightmode_prohibits_action = false;
if (plane.control_mode == &plane.mode_manual ||
(plane.control_mode == &plane.mode_auto && !plane.auto_state.takeoff_complete) ||
(plane.flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND) || // TODO: consider allowing action during approach
plane.control_mode == &plane.mode_autotune ||
plane.control_mode == &plane.mode_qland) {
plane.control_mode == &plane.mode_autotune) {
flightmode_prohibits_action = true;
}
#if HAL_QUADPLANE_ENABLED
if (plane.control_mode == &plane.mode_qland) {
flightmode_prohibits_action = true;
}
#endif
if (flightmode_prohibits_action) {
actual_action = MAV_COLLISION_ACTION_NONE;
}
@ -43,11 +51,13 @@ MAV_COLLISION_ACTION AP_Avoidance_Plane::handle_avoidance(const AP_Avoidance::Ob
case MAV_COLLISION_ACTION_HOVER:
if (failsafe_state_change) {
#if HAL_QUADPLANE_ENABLED
if (plane.quadplane.is_flying()) {
plane.set_mode(plane.mode_qloiter, ModeReason::AVOIDANCE);
} else {
plane.set_mode(plane.mode_loiter, ModeReason::AVOIDANCE);
break;
}
#endif
plane.set_mode(plane.mode_loiter, ModeReason::AVOIDANCE);
}
break;

31
ArduPlane/commands_logic.cpp
View File

@ -29,17 +29,23 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd)
AP_Mission::Mission_Command next_nav_cmd;
const uint16_t next_index = mission.get_current_nav_index() + 1;
auto_state.wp_is_land_approach = mission.get_next_nav_cmd(next_index, next_nav_cmd) && (next_nav_cmd.id == MAV_CMD_NAV_LAND) &&
!quadplane.is_vtol_land(next_nav_cmd.id);
auto_state.wp_is_land_approach = mission.get_next_nav_cmd(next_index, next_nav_cmd) && (next_nav_cmd.id == MAV_CMD_NAV_LAND);
#if HAL_QUADPLANE_ENABLED
if (quadplane.is_vtol_land(next_nav_cmd.id)) {
auto_state.wp_is_land_approach = false;
}
#endif
}
switch(cmd.id) {
case MAV_CMD_NAV_TAKEOFF:
crash_state.is_crashed = false;
#if HAL_QUADPLANE_ENABLED
if (quadplane.is_vtol_takeoff(cmd.id)) {
return quadplane.do_vtol_takeoff(cmd);
}
#endif
do_takeoff(cmd);
break;
@ -48,10 +54,12 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd)
break;
case MAV_CMD_NAV_LAND: // LAND to Waypoint
#if HAL_QUADPLANE_ENABLED
if (quadplane.is_vtol_land(cmd.id)) {
crash_state.is_crashed = false;
return quadplane.do_vtol_land(cmd);
}
#endif
do_land(cmd);
break;
@ -83,6 +91,7 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd)
do_altitude_wait(cmd);
break;
#if HAL_QUADPLANE_ENABLED
case MAV_CMD_NAV_VTOL_TAKEOFF:
crash_state.is_crashed = false;
return quadplane.do_vtol_takeoff(cmd);
@ -99,7 +108,8 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd)
} else {
return quadplane.do_vtol_land(cmd);
}
#endif
// Conditional commands
case MAV_CMD_CONDITION_DELAY:
@ -175,9 +185,11 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd)
break;
#endif
#if HAL_QUADPLANE_ENABLED
case MAV_CMD_DO_VTOL_TRANSITION:
plane.quadplane.handle_do_vtol_transition((enum MAV_VTOL_STATE)cmd.content.do_vtol_transition.target_state);
break;
#endif
case MAV_CMD_DO_ENGINE_CONTROL:
plane.g2.ice_control.engine_control(cmd.content.do_engine_control.start_control,
@ -207,18 +219,22 @@ bool Plane::verify_command(const AP_Mission::Mission_Command& cmd) // Ret
switch(cmd.id) {
case MAV_CMD_NAV_TAKEOFF:
#if HAL_QUADPLANE_ENABLED
if (quadplane.is_vtol_takeoff(cmd.id)) {
return quadplane.verify_vtol_takeoff(cmd);
}
#endif
return verify_takeoff();
case MAV_CMD_NAV_WAYPOINT:
return verify_nav_wp(cmd);
case MAV_CMD_NAV_LAND:
#if HAL_QUADPLANE_ENABLED
if (quadplane.is_vtol_land(cmd.id)) {
return quadplane.verify_vtol_land();
}
#endif
if (flight_stage == AP_Vehicle::FixedWing::FlightStage::FLIGHT_ABORT_LAND) {
return landing.verify_abort_landing(prev_WP_loc, next_WP_loc, current_loc, auto_state.takeoff_altitude_rel_cm, throttle_suppressed);
@ -255,9 +271,9 @@ bool Plane::verify_command(const AP_Mission::Mission_Command& cmd) // Ret
case MAV_CMD_NAV_ALTITUDE_WAIT:
return verify_altitude_wait(cmd);
#if HAL_QUADPLANE_ENABLED
case MAV_CMD_NAV_VTOL_TAKEOFF:
return quadplane.verify_vtol_takeoff(cmd);
case MAV_CMD_NAV_VTOL_LAND:
if ((quadplane.options & QuadPlane::OPTION_MISSION_LAND_FW_APPROACH) && !verify_landing_vtol_approach(cmd)) {
// verify_landing_vtol_approach will return true once we have completed the approach,
@ -266,6 +282,7 @@ bool Plane::verify_command(const AP_Mission::Mission_Command& cmd) // Ret
} else {
return quadplane.verify_vtol_land();
}
#endif // HAL_QUADPLANE_ENABLED
// Conditional commands
@ -387,6 +404,7 @@ void Plane::do_land(const AP_Mission::Mission_Command& cmd)
#endif
}
#if HAL_QUADPLANE_ENABLED
void Plane::do_landing_vtol_approach(const AP_Mission::Mission_Command& cmd)
{
//set target alt
@ -405,6 +423,7 @@ void Plane::do_landing_vtol_approach(const AP_Mission::Mission_Command& cmd)
vtol_approach_s.approach_stage = LOITER_TO_ALT;
}
#endif
void Plane::loiter_set_direction_wp(const AP_Mission::Mission_Command& cmd)
{
@ -950,6 +969,7 @@ void Plane::exit_mission_callback()
}
}
#if HAL_QUADPLANE_ENABLED
bool Plane::verify_landing_vtol_approach(const AP_Mission::Mission_Command &cmd)
{
switch (vtol_approach_s.approach_stage) {
@ -1053,13 +1073,16 @@ bool Plane::verify_landing_vtol_approach(const AP_Mission::Mission_Command &cmd)
return false;
}
#endif // HAL_QUADPLANE_ENABLED
bool Plane::verify_loiter_heading(bool init)
{
#if HAL_QUADPLANE_ENABLED
if (quadplane.in_vtol_auto()) {
// skip heading verify if in VTOL auto
return true;
}
#endif
//Get the lat/lon of next Nav waypoint after this one:
AP_Mission::Mission_Command next_nav_cmd;

7
ArduPlane/control_modes.cpp
View File

@ -1,5 +1,8 @@
#include "Plane.h"
#include "quadplane.h"
#include "qautotune.h"
Mode *Plane::mode_from_mode_num(const enum Mode::Number num)
{
Mode *ret = nullptr;
@ -52,6 +55,7 @@ Mode *Plane::mode_from_mode_num(const enum Mode::Number num)
case Mode::Number::INITIALISING:
ret = &mode_initializing;
break;
#if HAL_QUADPLANE_ENABLED
case Mode::Number::QSTABILIZE:
ret = &mode_qstabilize;
break;
@ -70,9 +74,12 @@ Mode *Plane::mode_from_mode_num(const enum Mode::Number num)
case Mode::Number::QACRO:
ret = &mode_qacro;
break;
#if QAUTOTUNE_ENABLED
case Mode::Number::QAUTOTUNE:
ret = &mode_qautotune;
break;
#endif
#endif // HAL_QUADPLANE_ENABLED
case Mode::Number::TAKEOFF:
ret = &mode_takeoff;
break;

12
ArduPlane/ekf_check.cpp
View File

@ -39,7 +39,13 @@ void Plane::ekf_check()
}
// return immediately if motors are not armed, or ekf check is disabled
if (!plane.arming.is_armed() || !quadplane.in_vtol_posvel_mode() || (g2.fs_ekf_thresh <= 0.0f)) {
bool ekf_check_disabled = !plane.arming.is_armed() || (g2.fs_ekf_thresh <= 0.0f);
#if HAL_QUADPLANE_ENABLED
if (!quadplane.in_vtol_posvel_mode()) {
ekf_check_disabled = true;
}
#endif
if (ekf_check_disabled) {
ekf_check_state.fail_count = 0;
ekf_check_state.bad_variance = false;
AP_Notify::flags.ekf_bad = ekf_check_state.bad_variance;
@ -152,6 +158,7 @@ void Plane::failsafe_ekf_event()
AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_EKFINAV, LogErrorCode::FAILSAFE_OCCURRED);
// if not in a VTOL mode requring position, then nothing needs to be done
#if HAL_QUADPLANE_ENABLED
if (!quadplane.in_vtol_posvel_mode()) {
return;
}
@ -163,6 +170,7 @@ void Plane::failsafe_ekf_event()
// the pilot is controlling via sticks so fallbacl to QHOVER
plane.set_mode(mode_qhover, ModeReason::EKF_FAILSAFE);
}
#endif
}
// failsafe_ekf_off_event - actions to take when EKF failsafe is cleared
@ -175,4 +183,4 @@ void Plane::failsafe_ekf_off_event(void)
ekf_check_state.failsafe_on = false;
AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_EKFINAV, LogErrorCode::FAILSAFE_RESOLVED);
}
}

66
ArduPlane/events.cpp
View File

@ -1,5 +1,20 @@
#include "Plane.h"
// returns true if the vehicle is in landing sequence. Intended only
// for use in failsafe code.
bool Plane::failsafe_in_landing_sequence() const
{
if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND) {
return true;
}
#if HAL_QUADPLANE_ENABLED
if (quadplane.in_vtol_land_sequence()) {
return true;
}
#endif
return false;
}
void Plane::failsafe_short_on_event(enum failsafe_state fstype, ModeReason reason)
{
// This is how to handle a short loss of control signal failsafe.
@ -25,10 +40,13 @@ void Plane::failsafe_short_on_event(enum failsafe_state fstype, ModeReason reaso
}
break;
#if HAL_QUADPLANE_ENABLED
case Mode::Number::QSTABILIZE:
case Mode::Number::QLOITER:
case Mode::Number::QHOVER:
#if QAUTOTUNE_ENABLED
case Mode::Number::QAUTOTUNE:
#endif
case Mode::Number::QACRO:
failsafe.saved_mode_number = control_mode->mode_number();
failsafe.saved_mode_set = true;
@ -38,15 +56,15 @@ void Plane::failsafe_short_on_event(enum failsafe_state fstype, ModeReason reaso
set_mode(mode_qland, reason);
}
break;
case Mode::Number::AUTO:
if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND ||
quadplane.in_vtol_land_sequence()) {
#endif // HAL_QUADPLANE_ENABLED
case Mode::Number::AUTO: {
if (failsafe_in_landing_sequence()) {
// don't failsafe in a landing sequence
break;
}
FALLTHROUGH;
}
case Mode::Number::AVOID_ADSB:
case Mode::Number::GUIDED:
case Mode::Number::LOITER:
@ -65,8 +83,10 @@ void Plane::failsafe_short_on_event(enum failsafe_state fstype, ModeReason reaso
case Mode::Number::CIRCLE:
case Mode::Number::TAKEOFF:
case Mode::Number::RTL:
#if HAL_QUADPLANE_ENABLED
case Mode::Number::QLAND:
case Mode::Number::QRTL:
#endif
case Mode::Number::INITIALISING:
break;
}
@ -104,21 +124,24 @@ void Plane::failsafe_long_on_event(enum failsafe_state fstype, ModeReason reason
}
break;
#if HAL_QUADPLANE_ENABLED
case Mode::Number::QSTABILIZE:
case Mode::Number::QHOVER:
case Mode::Number::QLOITER:
case Mode::Number::QACRO:
#if QAUTOTUNE_ENABLED
case Mode::Number::QAUTOTUNE:
#endif
if (quadplane.options & QuadPlane::OPTION_FS_QRTL) {
set_mode(mode_qrtl, reason);
} else {
set_mode(mode_qland, reason);
}
break;
#endif // HAL_QUADPLANE_ENABLED
case Mode::Number::AUTO:
if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND ||
quadplane.in_vtol_land_sequence()) {
if (failsafe_in_landing_sequence()) {
// don't failsafe in a landing sequence
break;
}
@ -138,8 +161,10 @@ void Plane::failsafe_long_on_event(enum failsafe_state fstype, ModeReason reason
break;
case Mode::Number::RTL:
#if HAL_QUADPLANE_ENABLED
case Mode::Number::QLAND:
case Mode::Number::QRTL:
#endif
case Mode::Number::TAKEOFF:
case Mode::Number::INITIALISING:
break;
@ -171,14 +196,22 @@ void Plane::failsafe_long_off_event(ModeReason reason)
void Plane::handle_battery_failsafe(const char *type_str, const int8_t action)
{
switch ((Failsafe_Action)action) {
#if HAL_QUADPLANE_ENABLED
case Failsafe_Action_QLand:
if (quadplane.available()) {
plane.set_mode(mode_qland, ModeReason::BATTERY_FAILSAFE);
break;
}
FALLTHROUGH;
case Failsafe_Action_Land:
if (flight_stage != AP_Vehicle::FixedWing::FLIGHT_LAND && control_mode != &mode_qland) {
#endif
case Failsafe_Action_Land: {
bool already_landing = flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND;
#if HAL_QUADPLANE_ENABLED
if (control_mode == &mode_qland) {
already_landing = true;
}
#endif
if (!already_landing) {
// never stop a landing if we were already committed
if (plane.mission.is_best_land_sequence()) {
// continue mission as it will reach a landing in less distance
@ -191,8 +224,16 @@ void Plane::handle_battery_failsafe(const char *type_str, const int8_t action)
}
}
FALLTHROUGH;
case Failsafe_Action_RTL:
if (flight_stage != AP_Vehicle::FixedWing::FLIGHT_LAND && control_mode != &mode_qland && !quadplane.in_vtol_land_sequence()) {
}
case Failsafe_Action_RTL: {
bool already_landing = flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND;
#if HAL_QUADPLANE_ENABLED
if (control_mode == &mode_qland ||
quadplane.in_vtol_land_sequence()) {
already_landing = true;
}
#endif
if (!already_landing) {
// never stop a landing if we were already committed
if (g.rtl_autoland == 2 && plane.mission.is_best_land_sequence()) {
// continue mission as it will reach a landing in less distance
@ -203,6 +244,7 @@ void Plane::handle_battery_failsafe(const char *type_str, const int8_t action)
aparm.throttle_cruise.load();
}
break;
}
case Failsafe_Action_Terminate:
#if ADVANCED_FAILSAFE == ENABLED

20
ArduPlane/is_flying.cpp
View File

@ -15,7 +15,7 @@
void Plane::update_is_flying_5Hz(void)
{
float aspeed=0;
bool is_flying_bool;
bool is_flying_bool = false;
uint32_t now_ms = AP_HAL::millis();
uint32_t ground_speed_thresh_cm = (aparm.min_gndspeed_cm > 0) ? ((uint32_t)(aparm.min_gndspeed_cm*0.9f)) : GPS_IS_FLYING_SPEED_CMS;
@ -34,9 +34,11 @@ void Plane::update_is_flying_5Hz(void)
airspeed_movement = aspeed >= airspeed_threshold;
}
if (quadplane.is_flying()) {
is_flying_bool = true;
#if HAL_QUADPLANE_ENABLED
is_flying_bool = quadplane.is_flying();
#endif
if (is_flying_bool) {
// no need to look further
} else if(arming.is_armed()) {
// when armed assuming flying and we need overwhelming evidence that we ARE NOT flying
// short drop-outs of GPS are common during flight due to banking which points the antenna in different directions
@ -182,9 +184,11 @@ void Plane::update_is_flying_5Hz(void)
bool Plane::is_flying(void)
{
if (hal.util->get_soft_armed()) {
#if HAL_QUADPLANE_ENABLED
if (quadplane.is_flying_vtol()) {
return true;
}
#endif
// when armed, assume we're flying unless we probably aren't
return (isFlyingProbability >= 0.1f);
}
@ -317,9 +321,13 @@ bool Plane::in_preLaunch_flight_stage(void)
if (control_mode == &mode_takeoff && throttle_suppressed) {
return true;
}
#if HAL_QUADPLANE_ENABLED
if (quadplane.is_vtol_takeoff(mission.get_current_nav_cmd().id)) {
return false;
}
#endif
return (control_mode == &mode_auto &&
throttle_suppressed &&
flight_stage == AP_Vehicle::FixedWing::FLIGHT_NORMAL &&
mission.get_current_nav_cmd().id == MAV_CMD_NAV_TAKEOFF &&
!quadplane.is_vtol_takeoff(mission.get_current_nav_cmd().id));
mission.get_current_nav_cmd().id == MAV_CMD_NAV_TAKEOFF);
}

8
ArduPlane/mode.cpp
View File

@ -1,11 +1,13 @@
#include "Plane.h"
Mode::Mode() :
quadplane(plane.quadplane),
Mode::Mode()
#if HAL_QUADPLANE_ENABLED
: quadplane(plane.quadplane),
pos_control(plane.quadplane.pos_control),
attitude_control(plane.quadplane.attitude_control),
loiter_nav(plane.quadplane.loiter_nav),
poscontrol(plane.quadplane.poscontrol)
#endif
{
}
@ -96,6 +98,7 @@ bool Mode::enter()
bool Mode::is_vtol_man_throttle() const
{
#if HAL_QUADPLANE_ENABLED
if (plane.quadplane.tailsitter.is_in_fw_flight() &&
plane.quadplane.assisted_flight) {
// We are a tailsitter that has fully transitioned to Q-assisted forward flight.
@ -104,5 +107,6 @@ bool Mode::is_vtol_man_throttle() const
// forward throttle uses 'does_auto_throttle' whereas vertical uses 'is_vtol_man_throttle'.
return !does_auto_throttle();
}
#endif
return false;
}

14
ArduPlane/mode.h
View File

@ -39,13 +39,17 @@ public:
AVOID_ADSB = 14,
GUIDED = 15,
INITIALISING = 16,
#if HAL_QUADPLANE_ENABLED
QSTABILIZE = 17,
QHOVER = 18,
QLOITER = 19,
QLAND = 20,
QRTL = 21,
#if QAUTOTUNE_ENABLED
QAUTOTUNE = 22,
#endif
QACRO = 23,
#endif
THERMAL = 24,
};
@ -84,6 +88,7 @@ public:
virtual bool is_vtol_mode() const { return false; }
virtual bool is_vtol_man_throttle() const;
virtual bool is_vtol_man_mode() const { return false; }
// guided or adsb mode
virtual bool is_guided_mode() const { return false; }
@ -119,13 +124,14 @@ protected:
// subclasses override this to perform any required cleanup when exiting the mode
virtual void _exit() { return; }
#if HAL_QUADPLANE_ENABLED
// References for convenience, used by QModes
QuadPlane& quadplane;
AC_PosControl*& pos_control;
AC_AttitudeControl_Multi*& attitude_control;
AC_Loiter*& loiter_nav;
QuadPlane& quadplane;
QuadPlane::PosControlState &poscontrol;
#endif
};
@ -442,6 +448,7 @@ protected:
};
#endif
#if HAL_QUADPLANE_ENABLED
class ModeQStabilize : public Mode
{
public:
@ -585,6 +592,7 @@ protected:
bool _enter() override;
};
#if QAUTOTUNE_ENABLED
class ModeQAutotune : public Mode
{
public:
@ -606,7 +614,9 @@ protected:
bool _enter() override;
void _exit() override;
};
#endif // QAUTOTUNE_ENABLED
#endif // HAL_QUADPLANE_ENABLED
class ModeTakeoff: public Mode
{

21
ArduPlane/mode_auto.cpp
View File

@ -3,11 +3,15 @@
bool ModeAuto::_enter()
{
#if HAL_QUADPLANE_ENABLED
if (plane.quadplane.available() && plane.quadplane.enable == 2) {
plane.auto_state.vtol_mode = true;
} else {
plane.auto_state.vtol_mode = false;
}
#else
plane.auto_state.vtol_mode = false;
#endif
plane.next_WP_loc = plane.prev_WP_loc = plane.current_loc;
// start or resume the mission, based on MIS_AUTORESET
plane.mission.start_or_resume();
@ -32,9 +36,13 @@ void ModeAuto::_exit()
if (plane.mission.state() == AP_Mission::MISSION_RUNNING) {
plane.mission.stop();
if (plane.mission.get_current_nav_cmd().id == MAV_CMD_NAV_LAND &&
!plane.quadplane.is_vtol_land(plane.mission.get_current_nav_cmd().id))
{
bool restart = plane.mission.get_current_nav_cmd().id == MAV_CMD_NAV_LAND;
#if HAL_QUADPLANE_ENABLED
if (plane.quadplane.is_vtol_land(plane.mission.get_current_nav_cmd().id)) {
restart = false;
}
#endif
if (restart) {
plane.landing.restart_landing_sequence();
}
}
@ -53,9 +61,14 @@ void ModeAuto::update()
uint16_t nav_cmd_id = plane.mission.get_current_nav_cmd().id;
#if HAL_QUADPLANE_ENABLED
if (plane.quadplane.in_vtol_auto()) {
plane.quadplane.control_auto();
} else if (nav_cmd_id == MAV_CMD_NAV_TAKEOFF ||
return;
}
#endif
if (nav_cmd_id == MAV_CMD_NAV_TAKEOFF ||
(nav_cmd_id == MAV_CMD_NAV_LAND && plane.flight_stage == AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND)) {
plane.takeoff_calc_roll();
plane.takeoff_calc_pitch();

13
ArduPlane/mode_guided.cpp
View File

@ -10,6 +10,7 @@ bool ModeGuided::_enter()
*/
plane.guided_WP_loc = plane.current_loc;
#if HAL_QUADPLANE_ENABLED
if (plane.quadplane.guided_mode_enabled()) {
/*
if using Q_GUIDED_MODE then project forward by the stopping distance
@ -17,19 +18,23 @@ bool ModeGuided::_enter()
plane.guided_WP_loc.offset_bearing(degrees(plane.ahrs.groundspeed_vector().angle()),
plane.quadplane.stopping_distance());
}
#endif
plane.set_guided_WP();
return true;
}
void ModeGuided::update()
{
#if HAL_QUADPLANE_ENABLED
if (plane.auto_state.vtol_loiter && plane.quadplane.available()) {
plane.quadplane.guided_update();
} else {
plane.calc_nav_roll();
plane.calc_nav_pitch();
plane.calc_throttle();
return;
}
#endif
plane.calc_nav_roll();
plane.calc_nav_pitch();
plane.calc_throttle();
}
void ModeGuided::navigate()

4
ArduPlane/mode_qacro.cpp
View File

@ -1,6 +1,8 @@
#include "mode.h"
#include "Plane.h"
#if HAL_QUADPLANE_ENABLED
bool ModeQAcro::_enter()
{
quadplane.throttle_wait = false;
@ -59,3 +61,5 @@ void ModeQAcro::run()
attitude_control->set_throttle_out(throttle_out, false, 10.0f);
}
}
#endif

5
ArduPlane/mode_qautotune.cpp
View File

@ -1,6 +1,10 @@
#include "mode.h"
#include "Plane.h"
#include "qautotune.h"
#if QAUTOTUNE_ENABLED
bool ModeQAutotune::_enter()
{
#if QAUTOTUNE_ENABLED
@ -29,3 +33,4 @@ void ModeQAutotune::_exit()
#endif
}
#endif

4
ArduPlane/mode_qhover.cpp
View File

@ -1,6 +1,8 @@
#include "mode.h"
#include "Plane.h"
#if HAL_QUADPLANE_ENABLED
bool ModeQHover::_enter()
{
// set vertical speed and acceleration limits
@ -31,3 +33,5 @@ void ModeQHover::run()
quadplane.hold_hover(quadplane.get_pilot_desired_climb_rate_cms());
}
}
#endif

4
ArduPlane/mode_qland.cpp
View File

@ -1,6 +1,8 @@
#include "mode.h"
#include "Plane.h"
#if HAL_QUADPLANE_ENABLED
bool ModeQLand::_enter()
{
plane.mode_qloiter._enter();
@ -29,3 +31,5 @@ void ModeQLand::run()
{
plane.mode_qloiter.run();
}
#endif

3
ArduPlane/mode_qloiter.cpp
View File

@ -1,6 +1,8 @@
#include "mode.h"
#include "Plane.h"
#if HAL_QUADPLANE_ENABLED
bool ModeQLoiter::_enter()
{
// initialise loiter
@ -120,3 +122,4 @@ void ModeQLoiter::run()
quadplane.run_z_controller();
}
#endif

3
ArduPlane/mode_qrtl.cpp
View File

@ -1,6 +1,8 @@
#include "mode.h"
#include "Plane.h"
#if HAL_QUADPLANE_ENABLED
bool ModeQRTL::_enter()
{
// use do_RTL() to setup next_WP_loc
@ -78,3 +80,4 @@ bool ModeQRTL::update_target_altitude()
return true;
}
#endif

4
ArduPlane/mode_qstabilize.cpp
View File

@ -1,6 +1,8 @@
#include "mode.h"
#include "Plane.h"
#if HAL_QUADPLANE_ENABLED
bool ModeQStabilize::_enter()
{
quadplane.throttle_wait = false;
@ -76,3 +78,5 @@ void ModeQStabilize::set_limited_roll_pitch(const float roll_input, const float
plane.nav_pitch_cd = pitch_input * MIN(-plane.pitch_limit_min_cd, plane.quadplane.aparm.angle_max);
}
}
#endif

12
ArduPlane/mode_rtl.cpp
View File

@ -6,10 +6,14 @@ bool ModeRTL::_enter()
plane.prev_WP_loc = plane.current_loc;
plane.do_RTL(plane.get_RTL_altitude_cm());
plane.rtl.done_climb = false;
#if HAL_QUADPLANE_ENABLED
plane.vtol_approach_s.approach_stage = Plane::Landing_ApproachStage::RTL;
#endif
// do not check if we have reached the loiter target if switching from loiter this will trigger as the nav controller has not yet proceeded the new destination
#if HAL_QUADPLANE_ENABLED
switch_QRTL(false);
#endif
return true;
}
@ -50,6 +54,7 @@ void ModeRTL::update()
void ModeRTL::navigate()
{
#if HAL_QUADPLANE_ENABLED
if (plane.control_mode->mode_number() != QRTL) {
// QRTL shares this navigate function with RTL
@ -68,6 +73,7 @@ void ModeRTL::navigate()
return;
}
}
#endif
if (plane.g.rtl_autoland == 1 &&
!plane.auto_state.checked_for_autoland &&
@ -105,10 +111,10 @@ void ModeRTL::navigate()
plane.update_loiter(radius);
}
#if HAL_QUADPLANE_ENABLED
// Switch to QRTL if enabled and within radius
bool ModeRTL::switch_QRTL(bool check_loiter_target)
{
{
if (!plane.quadplane.available() || ((plane.quadplane.rtl_mode != QuadPlane::RTL_MODE::SWITCH_QRTL) && (plane.quadplane.rtl_mode != QuadPlane::RTL_MODE::QRTL_ALWAYS))) {
return false;
}
@ -138,3 +144,5 @@ bool ModeRTL::switch_QRTL(bool check_loiter_target)
return false;
}
#endif // HAL_QUADPLANE_ENABLED

3
ArduPlane/motor_test.cpp
View File

@ -12,6 +12,7 @@
#define MOTOR_TEST_TIMEOUT_MS_MAX 30000 // max timeout is 30 seconds
// motor_test_output - checks for timeout and sends updates to motors objects
#if HAL_QUADPLANE_ENABLED
void QuadPlane::motor_test_output()
{
// exit immediately if the motor test is not running
@ -132,3 +133,5 @@ void QuadPlane::motor_test_stop()
// turn off notify leds
AP_Notify::flags.esc_calibration = false;
}
#endif // HAL_QUADPLANE_ENABLED

126
ArduPlane/navigation.cpp
View File

@ -109,6 +109,35 @@ void Plane::navigate()
control_mode->navigate();
}
// method intended for use in calc_airspeed_errors only
float Plane::mode_auto_target_airspeed_cm()
{
#if HAL_QUADPLANE_ENABLED
if ((quadplane.options & QuadPlane::OPTION_MISSION_LAND_FW_APPROACH) &&
((vtol_approach_s.approach_stage == Landing_ApproachStage::APPROACH_LINE) ||
(vtol_approach_s.approach_stage == Landing_ApproachStage::VTOL_LANDING))) {
const float land_airspeed = SpdHgt_Controller->get_land_airspeed();
if (is_positive(land_airspeed)) {
return land_airspeed * 100;
}
// fallover to normal airspeed
return aparm.airspeed_cruise_cm;
}
if (quadplane.in_vtol_land_approach()) {
return quadplane.get_land_airspeed() * 100;
}
#endif
// normal AUTO mode and new_airspeed variable was set by
// DO_CHANGE_SPEED command while in AUTO mode
if (new_airspeed_cm > 0) {
return new_airspeed_cm;
}
// fallover to normal airspeed
return aparm.airspeed_cruise_cm;
}
void Plane::calc_airspeed_errors()
{
float airspeed_measured = 0;
@ -171,29 +200,11 @@ void Plane::calc_airspeed_errors()
} else if (control_mode == &mode_guided && new_airspeed_cm > 0) { //DO_CHANGE_SPEED overrides onboard guided speed commands, user would have re-enter guided mode to revert
target_airspeed_cm = new_airspeed_cm;
} else if (control_mode == &mode_auto) {
if ((quadplane.options & QuadPlane::OPTION_MISSION_LAND_FW_APPROACH) &&
((vtol_approach_s.approach_stage == Landing_ApproachStage::APPROACH_LINE) ||
(vtol_approach_s.approach_stage == Landing_ApproachStage::VTOL_LANDING))) {
const float land_airspeed = SpdHgt_Controller->get_land_airspeed();
if (is_positive(land_airspeed)) {
target_airspeed_cm = land_airspeed * 100;
} else {
// fallover to normal airspeed
target_airspeed_cm = aparm.airspeed_cruise_cm;
}
} else if (quadplane.in_vtol_land_approach()) {
target_airspeed_cm = quadplane.get_land_airspeed() * 100;
} else {
// normal AUTO mode and new_airspeed variable was set by DO_CHANGE_SPEED command while in AUTO mode
if (new_airspeed_cm > 0) {
target_airspeed_cm = new_airspeed_cm;
} else {
// fallover to normal airspeed
target_airspeed_cm = aparm.airspeed_cruise_cm;
}
}
target_airspeed_cm = mode_auto_target_airspeed_cm();
#if HAL_QUADPLANE_ENABLED
} else if (control_mode == &mode_qrtl && quadplane.in_vtol_land_approach()) {
target_airspeed_cm = quadplane.get_land_airspeed() * 100;
#endif
} else {
// Normal airspeed target for all other cases
target_airspeed_cm = aparm.airspeed_cruise_cm;
@ -249,6 +260,48 @@ void Plane::calc_gndspeed_undershoot()
}
}
// method intended to be used by update_loiter
void Plane::update_loiter_update_nav(uint16_t radius)
{
#if HAL_QUADPLANE_ENABLED
if (loiter.start_time_ms != 0 &&
quadplane.guided_mode_enabled()) {
if (!auto_state.vtol_loiter) {
auto_state.vtol_loiter = true;
// reset loiter start time, so we don't consider the point
// reached till we get much closer
loiter.start_time_ms = 0;
quadplane.guided_start();
}
return;
}
#endif
#if HAL_QUADPLANE_ENABLED
const bool quadplane_qrtl_switch = (control_mode == &mode_rtl && quadplane.available() && quadplane.rtl_mode == QuadPlane::RTL_MODE::SWITCH_QRTL);
#else
const bool quadplane_qrtl_switch = false;
#endif
if ((loiter.start_time_ms == 0 &&
(control_mode == &mode_auto || control_mode == &mode_guided) &&
auto_state.crosstrack &&
current_loc.get_distance(next_WP_loc) > radius*3) ||
quadplane_qrtl_switch) {
/*
if never reached loiter point and using crosstrack and somewhat far away from loiter point
navigate to it like in auto-mode for normal crosstrack behavior
we also use direct waypoint navigation if we are a quadplane
that is going to be switching to QRTL when it gets within
RTL_RADIUS
*/
nav_controller->update_waypoint(prev_WP_loc, next_WP_loc);
return;
}
nav_controller->update_loiter(next_WP_loc, radius, loiter.direction);
}
void Plane::update_loiter(uint16_t radius)
{
if (radius <= 1) {
@ -261,32 +314,7 @@ void Plane::update_loiter(uint16_t radius)
}
}
if (loiter.start_time_ms != 0 &&
quadplane.guided_mode_enabled()) {
if (!auto_state.vtol_loiter) {
auto_state.vtol_loiter = true;
// reset loiter start time, so we don't consider the point
// reached till we get much closer
loiter.start_time_ms = 0;
quadplane.guided_start();
}
} else if ((loiter.start_time_ms == 0 &&
(control_mode == &mode_auto || control_mode == &mode_guided) &&
auto_state.crosstrack &&
current_loc.get_distance(next_WP_loc) > radius*3) ||
(control_mode == &mode_rtl && quadplane.available() && quadplane.rtl_mode == QuadPlane::RTL_MODE::SWITCH_QRTL)) {
/*
if never reached loiter point and using crosstrack and somewhat far away from loiter point
navigate to it like in auto-mode for normal crosstrack behavior
we also use direct waypoint navigation if we are a quadplane
that is going to be switching to QRTL when it gets within
RTL_RADIUS
*/
nav_controller->update_waypoint(prev_WP_loc, next_WP_loc);
} else {
nav_controller->update_loiter(next_WP_loc, radius, loiter.direction);
}
update_loiter_update_nav(radius);
if (loiter.start_time_ms == 0) {
if (reached_loiter_target() ||
@ -297,9 +325,11 @@ void Plane::update_loiter(uint16_t radius)
// starting a loiter in GUIDED means we just reached the target point
gcs().send_mission_item_reached_message(0);
}
#if HAL_QUADPLANE_ENABLED
if (quadplane.guided_mode_enabled()) {
quadplane.guided_start();
}
#endif
}
}
}
@ -382,8 +412,10 @@ void Plane::setup_turn_angle(void)
*/
bool Plane::reached_loiter_target(void)
{
#if HAL_QUADPLANE_ENABLED
if (quadplane.in_vtol_auto()) {
return auto_state.wp_distance < 3;
}
#endif
return nav_controller->reached_loiter_target();
}

4
ArduPlane/qautotune.h
View File

@ -6,7 +6,9 @@
#include <AP_HAL/AP_HAL.h>
#define QAUTOTUNE_ENABLED !HAL_MINIMIZE_FEATURES
#include "quadplane.h"
#define QAUTOTUNE_ENABLED HAL_QUADPLANE_ENABLED && !HAL_MINIMIZE_FEATURES
#if QAUTOTUNE_ENABLED

23
ArduPlane/quadplane.cpp
View File

@ -1,4 +1,7 @@
#include "Plane.h"
#if HAL_QUADPLANE_ENABLED
#include "AC_AttitudeControl/AC_AttitudeControl_TS.h"
const AP_Param::GroupInfo QuadPlane::var_info[] = {
@ -2120,7 +2123,9 @@ bool QuadPlane::in_vtol_posvel_mode(void) const
return (plane.control_mode == &plane.mode_qloiter ||
plane.control_mode == &plane.mode_qland ||
plane.control_mode == &plane.mode_qrtl ||
#if QAUTOTUNE_ENABLED
plane.control_mode == &plane.mode_qautotune ||
#endif
(plane.control_mode->is_guided_mode() &&
plane.auto_state.vtol_loiter &&
poscontrol.get_state() > QPOS_APPROACH) ||
@ -3181,8 +3186,13 @@ int8_t QuadPlane::forward_throttle_pct()
/*
in qautotune mode or modes without a velocity controller
*/
if (vel_forward.gain <= 0 ||
plane.control_mode == &plane.mode_qautotune) {
bool use_forward_gain = (vel_forward.gain > 0);
#if QAUTOTUNE_ENABLED
if (plane.control_mode == &plane.mode_qautotune) {
use_forward_gain = false;
}
#endif
if (!use_forward_gain) {
return 0;
}
@ -3281,8 +3291,11 @@ float QuadPlane::get_weathervane_yaw_rate_cds(void)
!motors->armed() ||
weathervane.gain <= 0 ||
plane.control_mode == &plane.mode_qstabilize ||
plane.control_mode == &plane.mode_qhover ||
plane.control_mode == &plane.mode_qautotune) {
#if QAUTOTUNE_ENABLED
plane.control_mode == &plane.mode_qautotune ||
#endif
plane.control_mode == &plane.mode_qhover
) {
weathervane.last_output = 0;
return 0;
}
@ -3741,3 +3754,5 @@ bool QuadPlane::air_mode_active() const
}
QuadPlane *QuadPlane::_singleton = nullptr;
#endif // HAL_QUADPLANE_ENABLED

10
ArduPlane/quadplane.h
View File

@ -1,5 +1,13 @@
#pragma once
#include <AP_HAL/AP_HAL_Boards.h>
#ifndef HAL_QUADPLANE_ENABLED
#define HAL_QUADPLANE_ENABLED 1
#endif
#if HAL_QUADPLANE_ENABLED
#include <AP_Motors/AP_Motors.h>
#include <AC_PID/AC_PID.h>
#include <AC_AttitudeControl/AC_AttitudeControl_Multi.h> // Attitude control library
@ -651,3 +659,5 @@ private:
static QuadPlane *_singleton;
};
#endif // HAL_QUADPLANE_ENABLED

14
ArduPlane/radio.cpp
View File

@ -44,10 +44,16 @@ void Plane::set_control_channels(void)
channel_flap = rc().find_channel_for_option(RC_Channel::AUX_FUNC::FLAP);
channel_airbrake = rc().find_channel_for_option(RC_Channel::AUX_FUNC::AIRBRAKE);
#if HAL_QUADPLANE_ENABLED
// update manual forward throttle channel assignment
quadplane.rc_fwd_thr_ch = rc().find_channel_for_option(RC_Channel::AUX_FUNC::FWD_THR);
#endif
if (!quadplane.enable) {
bool set_throttle_esc_scaling = true;
#if HAL_QUADPLANE_ENABLED
set_throttle_esc_scaling = !quadplane.enable;
#endif
if (set_throttle_esc_scaling) {
// setup correct scaling for ESCs like the UAVCAN ESCs which
// take a proportion of speed. For quadplanes we use AP_Motors
// scaling
@ -188,8 +194,10 @@ void Plane::read_radio()
rudder_arm_disarm_check();
#if HAL_QUADPLANE_ENABLED
// potentially swap inputs for tailsitters
quadplane.tailsitter.check_input();
#endif
// check for transmitter tuning changes
tuning.check_input(control_mode->mode_number());
@ -236,19 +244,23 @@ void Plane::control_failsafe()
throttle_nudge = 0;
switch (control_mode->mode_number()) {
#if HAL_QUADPLANE_ENABLED
case Mode::Number::QSTABILIZE:
case Mode::Number::QHOVER:
case Mode::Number::QLOITER:
case Mode::Number::QLAND: // throttle is ignored, but reset anyways
case Mode::Number::QRTL: // throttle is ignored, but reset anyways
case Mode::Number::QACRO:
#if QAUTOTUNE_ENABLED
case Mode::Number::QAUTOTUNE:
#endif
if (quadplane.available() && quadplane.motors->get_desired_spool_state() > AP_Motors::DesiredSpoolState::GROUND_IDLE) {
// set half throttle to avoid descending at maximum rate, still has a slight descent due to throttle deadzone
channel_throttle->set_control_in(channel_throttle->get_range() / 2);
break;
}
FALLTHROUGH;
#endif
default:
channel_throttle->set_control_in(0);
break;

52
ArduPlane/servos.cpp
View File

@ -24,7 +24,13 @@
*****************************************/
void Plane::throttle_slew_limit(SRV_Channel::Aux_servo_function_t func)
{
if (!(control_mode->does_auto_throttle() || quadplane.in_assisted_flight() || quadplane.in_vtol_mode())) {
#if HAL_QUADPLANE_ENABLED
const bool do_throttle_slew = (control_mode->does_auto_throttle() || quadplane.in_assisted_flight() || quadplane.in_vtol_mode());
#else
const bool do_throttle_slew = control_mode->does_auto_throttle();
#endif
if (!do_throttle_slew) {
// only do throttle slew limiting in modes where throttle control is automatic
return;
}
@ -125,10 +131,12 @@ bool Plane::suppress_throttle(void)
}
}
#if HAL_QUADPLANE_ENABLED
if (quadplane.is_flying()) {
throttle_suppressed = false;
return false;
}
#endif
// throttle remains suppressed
return true;
@ -363,6 +371,7 @@ void Plane::set_servos_manual_passthrough(void)
int8_t throttle = get_throttle_input(true);
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, throttle);
#if HAL_QUADPLANE_ENABLED
if (quadplane.available() && (quadplane.options & QuadPlane::OPTION_IDLE_GOV_MANUAL)) {
// for quadplanes it can be useful to run the idle governor in MANUAL mode
// as it prevents the VTOL motors from running
@ -373,6 +382,7 @@ void Plane::set_servos_manual_passthrough(void)
throttle = MAX(throttle, min_throttle);
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, throttle);
}
#endif
}
/*
@ -480,11 +490,19 @@ void Plane::set_servos_controlled(void)
// reverse thrust is available but inhibited.
min_throttle = 0;
}
if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_TAKEOFF ||
flight_stage == AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND ||
quadplane.in_transition()) {
bool flight_stage_determines_max_throttle = false;
if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_TAKEOFF ||
flight_stage == AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND
) {
flight_stage_determines_max_throttle = true;
}
#if HAL_QUADPLANE_ENABLED
if (quadplane.in_transition()) {
flight_stage_determines_max_throttle = true;
}
#endif
if (flight_stage_determines_max_throttle) {
if (aparm.takeoff_throttle_max != 0) {
max_throttle = aparm.takeoff_throttle_max;
} else {
@ -544,6 +562,7 @@ void Plane::set_servos_controlled(void)
guided_throttle_passthru) {
// manual pass through of throttle while in GUIDED
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, get_throttle_input(true));
#if HAL_QUADPLANE_ENABLED
} else if (quadplane.in_vtol_mode()) {
int16_t fwd_thr = 0;
// if armed and not spooled down ask quadplane code for forward throttle
@ -553,6 +572,7 @@ void Plane::set_servos_controlled(void)
fwd_thr = constrain_int16(quadplane.forward_throttle_pct(), min_throttle, max_throttle);
}
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, fwd_thr);
#endif // HAL_QUADPLANE_ENABLED
}
// let EKF know to start GSF yaw estimator before takeoff movement starts so that yaw angle is better estimated
@ -732,14 +752,24 @@ void Plane::servos_twin_engine_mix(void)
*/
void Plane::force_flare(void)
{
if (!quadplane.in_transition() && !control_mode->is_vtol_mode() && !control_mode->does_auto_throttle() && channel_throttle->in_trim_dz() && flare_mode != FlareMode::FLARE_DISABLED) {
#if HAL_QUADPLANE_ENABLED
if (quadplane.in_transition()) {
return;
}
if (control_mode->is_vtol_mode()) {
return;
}
#endif
if (!control_mode->does_auto_throttle() && channel_throttle->in_trim_dz() && flare_mode != FlareMode::FLARE_DISABLED) {
int32_t tilt = -SERVO_MAX; //this is tilts up for a normal tiltrotor
#if HAL_QUADPLANE_ENABLED
if (quadplane.tilt.tilt_type == QuadPlane::TILT_TYPE_BICOPTER) {
tilt = 0; // this is tilts up for a Bicopter
}
if (quadplane.tailsitter.enabled()) {
tilt = SERVO_MAX; //this is tilts up for a tailsitter
}
#endif
SRV_Channels::set_output_scaled(SRV_Channel::k_motor_tilt, tilt);
SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorLeft, tilt);
SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRight, tilt);
@ -786,7 +816,9 @@ void Plane::set_servos(void)
#endif
// do any transition updates for quadplane
quadplane.update();
#if HAL_QUADPLANE_ENABLED
quadplane.update();
#endif
if (control_mode == &mode_auto && auto_state.idle_mode) {
// special handling for balloon launch
@ -905,9 +937,11 @@ void Plane::servos_output(void)
// support twin-engine aircraft
servos_twin_engine_mix();
#if HAL_QUADPLANE_ENABLED
// cope with tailsitters and bicopters
quadplane.tailsitter.output();
quadplane.tiltrotor_bicopter();
#endif
// support forced flare option
force_flare();
@ -933,7 +967,9 @@ void Plane::servos_output(void)
void Plane::update_throttle_hover() {
// update hover throttle at 100Hz
#if HAL_QUADPLANE_ENABLED
quadplane.update_throttle_hover();
#endif
}
/*
@ -953,10 +989,12 @@ void Plane::servos_auto_trim(void)
if (!is_flying()) {
return;
}
#if HAL_QUADPLANE_ENABLED
if (quadplane.in_assisted_flight() || quadplane.in_vtol_mode()) {
// can't auto-trim with quadplane motors running
return;
}
#endif
if (abs(nav_roll_cd) > 700 || abs(nav_pitch_cd) > 700) {
// only when close to level
return;

25
ArduPlane/system.cpp
View File

@ -1,5 +1,7 @@
#include "Plane.h"
#include "qautotune.h"
/*****************************************************************************
* The init_ardupilot function processes everything we need for an in - air restart
* We will determine later if we are actually on the ground and process a
@ -111,7 +113,9 @@ void Plane::init_ardupilot()
*/
hal.scheduler->register_timer_failsafe(failsafe_check_static, 1000);
#if HAL_QUADPLANE_ENABLED
quadplane.setup();
#endif
AP_Param::reload_defaults_file(true);
@ -212,6 +216,7 @@ bool Plane::set_mode(Mode &new_mode, const ModeReason reason)
return true;
}
#if HAL_QUADPLANE_ENABLED
if (new_mode.is_vtol_mode() && !plane.quadplane.available()) {
// dont try and switch to a Q mode if quadplane is not enabled and initalized
gcs().send_text(MAV_SEVERITY_INFO,"Q_ENABLE 0");
@ -232,7 +237,19 @@ bool Plane::set_mode(Mode &new_mode, const ModeReason reason)
}
return false;
}
#endif
#endif // !QAUTOTUNE_ENABLED
#else
if (new_mode.is_vtol_mode()) {
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
gcs().send_text(MAV_SEVERITY_INFO,"HAL_QUADPLANE_ENABLED=0");
// make sad noise
if (reason != ModeReason::INITIALISED) {
AP_Notify::events.user_mode_change_failed = 1;
}
return false;
}
#endif // HAL_QUADPLANE_ENABLED
// backup current control_mode and previous_mode
Mode &old_previous_mode = *previous_mode;
@ -424,9 +441,11 @@ bool Plane::should_log(uint32_t mask)
*/
int8_t Plane::throttle_percentage(void)
{
#if HAL_QUADPLANE_ENABLED
if (quadplane.in_vtol_mode() && !quadplane.tailsitter.in_vtol_transition()) {
return quadplane.throttle_percentage();
}
#endif
float throttle = SRV_Channels::get_output_scaled(SRV_Channel::k_throttle);
if (!have_reverse_thrust()) {
return constrain_int16(throttle, 0, 100);
@ -451,9 +470,11 @@ void Plane::update_dynamic_notch()
switch (ins.get_gyro_harmonic_notch_tracking_mode()) {
case HarmonicNotchDynamicMode::UpdateThrottle: // throttle based tracking
// set the harmonic notch filter frequency approximately scaled on motor rpm implied by throttle
#if HAL_QUADPLANE_ENABLED
if (quadplane.available()) {
ins.update_harmonic_notch_freq_hz(ref_freq * MAX(1.0f, sqrtf(quadplane.motors->get_throttle_out() / ref)));
}
#endif
break;
case HarmonicNotchDynamicMode::UpdateRPM: // rpm sensor based tracking
@ -477,8 +498,10 @@ void Plane::update_dynamic_notch()
}
if (num_notches > 0) {
ins.update_harmonic_notch_frequencies_hz(num_notches, notches);
#if HAL_QUADPLANE_ENABLED
} else if (quadplane.available()) { // throttle fallback
ins.update_harmonic_notch_freq_hz(ref_freq * MAX(1.0f, sqrtf(quadplane.motors->get_throttle_out() / ref)));
#endif
} else {
ins.update_harmonic_notch_freq_hz(ref_freq);
}

4
ArduPlane/tailsitter.cpp
View File

@ -20,6 +20,8 @@
#include "tailsitter.h"
#include "Plane.h"
#if HAL_QUADPLANE_ENABLED
const AP_Param::GroupInfo Tailsitter::var_info[] = {
// @Param: ENABLE
@ -633,3 +635,5 @@ void Tailsitter::speed_scaling(void)
SRV_Channels::set_output_scaled(functions[i], v);
}
}
#endif // HAL_QUADPLANE_ENABLED

9
ArduPlane/takeoff.cpp
View File

@ -36,6 +36,12 @@ bool Plane::auto_takeoff_check(void)
return false;
}
bool do_takeoff_attitude_check = !(g2.flight_options & FlightOptions::DISABLE_TOFF_ATTITUDE_CHK);
#if HAL_QUADPLANE_ENABLED
// disable attitude check on tailsitters
do_takeoff_attitude_check = !quadplane.tailsitter.enabled();
#endif
if (!takeoff_state.launchTimerStarted && !is_zero(g.takeoff_throttle_min_accel)) {
// we are requiring an X acceleration event to launch
float xaccel = SpdHgt_Controller->get_VXdot();
@ -80,8 +86,7 @@ bool Plane::auto_takeoff_check(void)
goto no_launch;
}
if (!quadplane.tailsitter.enabled() &&
!(g2.flight_options & FlightOptions::DISABLE_TOFF_ATTITUDE_CHK)) {
if (do_takeoff_attitude_check) {
// Check aircraft attitude for bad launch
if (ahrs.pitch_sensor <= -3000 || ahrs.pitch_sensor >= 4500 ||
(!fly_inverted() && labs(ahrs.roll_sensor) > 3000)) {

6
ArduPlane/tiltrotor.cpp
View File

@ -1,5 +1,7 @@
#include "Plane.h"
#if HAL_QUADPLANE_ENABLED
/*
control code for tiltrotors and tiltwings. Enabled by setting
Q_TILT_MASK to a non-zero value
@ -113,10 +115,12 @@ void QuadPlane::tiltrotor_continuous_update(void)
to forward flight and should put the rotors all the way forward
*/
#if QAUTOTUNE_ENABLED
if (plane.control_mode == &plane.mode_qautotune) {
tiltrotor_slew(0);
return;
}
#endif
// if not in assisted flight and in QACRO, QSTABILIZE or QHOVER mode
if (!assisted_flight &&
@ -468,3 +472,5 @@ void QuadPlane::tiltrotor_bicopter(void)
SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorLeft, tilt_left);
SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRight, tilt_right);
}
#endif // HAL_QUADPLANE_ENABLED

11
ArduPlane/tuning.cpp
View File

@ -92,13 +92,16 @@ const AP_Tuning_Plane::tuning_name AP_Tuning_Plane::tuning_names[] = {
*/
AP_Float *AP_Tuning_Plane::get_param_pointer(uint8_t parm)
{
#if HAL_QUADPLANE_ENABLED
if (parm < TUNING_FIXED_WING_BASE && !plane.quadplane.available()) {
// quadplane tuning options not available
return nullptr;
}
#endif
switch(parm) {
#if HAL_QUADPLANE_ENABLED
case TUNING_RATE_ROLL_PI:
// use P for initial value when tuning PI
return &plane.quadplane.attitude_control->get_rate_roll_pid().kP();
@ -177,6 +180,7 @@ AP_Float *AP_Tuning_Plane::get_param_pointer(uint8_t parm)
case TUNING_RATE_YAW_FF:
return &plane.quadplane.attitude_control->get_rate_yaw_pid().ff();
#endif // HAL_QUADPLANE_ENABLED
// fixed wing tuning parameters
case TUNING_RLL_P:
@ -300,6 +304,7 @@ void AP_Tuning_Plane::reload_value(uint8_t parm)
*/
float AP_Tuning_Plane::controller_error(uint8_t parm)
{
#if HAL_QUADPLANE_ENABLED
if (!plane.quadplane.available()) {
return 0;
}
@ -360,4 +365,8 @@ float AP_Tuning_Plane::controller_error(uint8_t parm)
// no special handler
return 0;
}
#else
// no special handler
return 0;
#endif // HAL_QUADPLANE_ENABLED
}