Mirrors_Framework/ardupilot
1
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot.git synced 2026-10-19 08:54:04 +08:00
Code Issues Packages Projects Releases Wiki Activity

ArduPlane: add and use HAL_QUADPLANE_ENABLED

Browse Source
This commit is contained in:
Peter Barker
2021-09-14 11:23:18 +10:00
committed by Peter Barker
parent 305a8ad48a
commit 3d34e061fe
42 changed files with 755 additions and 204 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ArduPlane/AP_Arming.cpp
+21 -2
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
ArduPlane/ArduPlane.cpp
+88 -30
View File
File diff suppressed because it is too large Load Diff
ArduPlane/Attitude.cpp
+75 -38
View File
File diff suppressed because it is too large Load Diff
ArduPlane/GCS_Mavlink.cpp
+57 -20
View File
File diff suppressed because it is too large Load Diff
ArduPlane/GCS_Plane.cpp
+8
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;
ArduPlane/Log.cpp
+4
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();
ArduPlane/Parameters.cpp
+9 -3
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);
ArduPlane/Plane.h
+24
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,
ArduPlane/RC_Channel.cpp
+12
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
ArduPlane/RC_Channel.h
+2
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);
ArduPlane/afs_plane.cpp
+7 -1
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
}
/*
ArduPlane/altitude.cpp
+18 -4
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
ArduPlane/avoidance_adsb.cpp
+14 -4
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;
ArduPlane/commands_logic.cpp
+27 -4
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;
ArduPlane/control_modes.cpp
+7
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;
ArduPlane/ekf_check.cpp
+10 -2
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);
}
}
ArduPlane/events.cpp
+54 -12
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
ArduPlane/is_flying.cpp
+14 -6
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);
}
ArduPlane/mode.cpp
+6 -2
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;
}
ArduPlane/mode.h
+12 -2
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
{
ArduPlane/mode_auto.cpp
+17 -4
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();
ArduPlane/mode_guided.cpp
+9 -4
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()
ArduPlane/mode_qacro.cpp
+4
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
ArduPlane/mode_qautotune.cpp
+5
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
ArduPlane/mode_qhover.cpp
+4
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
ArduPlane/mode_qland.cpp
+4
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
ArduPlane/mode_qloiter.cpp
+3
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
ArduPlane/mode_qrtl.cpp
+3
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
ArduPlane/mode_qstabilize.cpp
+4
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
ArduPlane/mode_rtl.cpp
+10 -2
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
ArduPlane/motor_test.cpp
+3
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
ArduPlane/navigation.cpp
+79 -47
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();
}
ArduPlane/qautotune.h
+3 -1
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
ArduPlane/quadplane.cpp
+19 -4
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
ArduPlane/quadplane.h
+10
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
ArduPlane/radio.cpp
+13 -1
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;
ArduPlane/servos.cpp
+45 -7
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;
ArduPlane/system.cpp
+24 -1
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);
}
ArduPlane/tailsitter.cpp
+4
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
ArduPlane/takeoff.cpp
+7 -2
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)) {
ArduPlane/tiltrotor.cpp
+6
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
ArduPlane/tuning.cpp
+10 -1
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
}