diff --git a/ArduPlane/AP_Arming.cpp b/ArduPlane/AP_Arming.cpp index 5167e404c0..93d6d2bb69 100644 --- a/ArduPlane/AP_Arming.cpp +++ b/ArduPlane/AP_Arming.cpp @@ -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 diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index 66e3e6fe74..74c2308a6a 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -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 diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp index cfa0515323..967eebe25e 100644 --- a/ArduPlane/Attitude.cpp +++ b/ArduPlane/Attitude.cpp @@ -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) { diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index b27add78df..4105b38f88 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -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 diff --git a/ArduPlane/GCS_Plane.cpp b/ArduPlane/GCS_Plane.cpp index deca2aa804..c018ed1dd1 100644 --- a/ArduPlane/GCS_Plane.cpp +++ b/ArduPlane/GCS_Plane.cpp @@ -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; diff --git a/ArduPlane/Log.cpp b/ArduPlane/Log.cpp index f12233eeac..4265b07bcd 100644 --- a/ArduPlane/Log.cpp +++ b/ArduPlane/Log.cpp @@ -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(); diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index bf8cbd6bed..30e3b918c8 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -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); diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 5fbc82b0fa..0090e4edbe 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -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, diff --git a/ArduPlane/RC_Channel.cpp b/ArduPlane/RC_Channel.cpp index 90acc3860d..24e9e95f18 100644 --- a/ArduPlane/RC_Channel.cpp +++ b/ArduPlane/RC_Channel.cpp @@ -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 diff --git a/ArduPlane/RC_Channel.h b/ArduPlane/RC_Channel.h index 9b14f5efe2..35663554a5 100644 --- a/ArduPlane/RC_Channel.h +++ b/ArduPlane/RC_Channel.h @@ -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); diff --git a/ArduPlane/afs_plane.cpp b/ArduPlane/afs_plane.cpp index 438dccc3bc..b0872d0d40 100644 --- a/ArduPlane/afs_plane.cpp +++ b/ArduPlane/afs_plane.cpp @@ -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 } /* diff --git a/ArduPlane/altitude.cpp b/ArduPlane/altitude.cpp index 7131c0e777..6b2e9f0379 100644 --- a/ArduPlane/altitude.cpp +++ b/ArduPlane/altitude.cpp @@ -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 diff --git a/ArduPlane/avoidance_adsb.cpp b/ArduPlane/avoidance_adsb.cpp index ae102afbd5..b44ff75b96 100644 --- a/ArduPlane/avoidance_adsb.cpp +++ b/ArduPlane/avoidance_adsb.cpp @@ -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; diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index fe06ba3a07..45d2124495 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -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; diff --git a/ArduPlane/control_modes.cpp b/ArduPlane/control_modes.cpp index 05268dec92..f2a7dfab58 100644 --- a/ArduPlane/control_modes.cpp +++ b/ArduPlane/control_modes.cpp @@ -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; diff --git a/ArduPlane/ekf_check.cpp b/ArduPlane/ekf_check.cpp index 709935babe..86b972225c 100644 --- a/ArduPlane/ekf_check.cpp +++ b/ArduPlane/ekf_check.cpp @@ -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); -} \ No newline at end of file +} diff --git a/ArduPlane/events.cpp b/ArduPlane/events.cpp index fa5a2be54b..3513ff49d7 100644 --- a/ArduPlane/events.cpp +++ b/ArduPlane/events.cpp @@ -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 diff --git a/ArduPlane/is_flying.cpp b/ArduPlane/is_flying.cpp index 2ddad79c5d..1b37e66b5e 100644 --- a/ArduPlane/is_flying.cpp +++ b/ArduPlane/is_flying.cpp @@ -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); } diff --git a/ArduPlane/mode.cpp b/ArduPlane/mode.cpp index 97a67c4c9c..f407709c27 100644 --- a/ArduPlane/mode.cpp +++ b/ArduPlane/mode.cpp @@ -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; } diff --git a/ArduPlane/mode.h b/ArduPlane/mode.h index f2bd6cf06b..52c928e9f6 100644 --- a/ArduPlane/mode.h +++ b/ArduPlane/mode.h @@ -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 { diff --git a/ArduPlane/mode_auto.cpp b/ArduPlane/mode_auto.cpp index 48cbe22791..b050a02005 100644 --- a/ArduPlane/mode_auto.cpp +++ b/ArduPlane/mode_auto.cpp @@ -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(); diff --git a/ArduPlane/mode_guided.cpp b/ArduPlane/mode_guided.cpp index 72277fc56c..5ffee4b5b5 100644 --- a/ArduPlane/mode_guided.cpp +++ b/ArduPlane/mode_guided.cpp @@ -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() diff --git a/ArduPlane/mode_qacro.cpp b/ArduPlane/mode_qacro.cpp index d53befbb3c..32cc739e13 100644 --- a/ArduPlane/mode_qacro.cpp +++ b/ArduPlane/mode_qacro.cpp @@ -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 diff --git a/ArduPlane/mode_qautotune.cpp b/ArduPlane/mode_qautotune.cpp index 6b84b27858..d74cf792b5 100644 --- a/ArduPlane/mode_qautotune.cpp +++ b/ArduPlane/mode_qautotune.cpp @@ -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 diff --git a/ArduPlane/mode_qhover.cpp b/ArduPlane/mode_qhover.cpp index 43328087ed..fe616292a7 100644 --- a/ArduPlane/mode_qhover.cpp +++ b/ArduPlane/mode_qhover.cpp @@ -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 diff --git a/ArduPlane/mode_qland.cpp b/ArduPlane/mode_qland.cpp index 3dae6b3b33..dfc89ebfd9 100644 --- a/ArduPlane/mode_qland.cpp +++ b/ArduPlane/mode_qland.cpp @@ -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 diff --git a/ArduPlane/mode_qloiter.cpp b/ArduPlane/mode_qloiter.cpp index 25cfcf55d5..71a08e52e2 100644 --- a/ArduPlane/mode_qloiter.cpp +++ b/ArduPlane/mode_qloiter.cpp @@ -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 diff --git a/ArduPlane/mode_qrtl.cpp b/ArduPlane/mode_qrtl.cpp index 07a25f97ca..99ef904896 100644 --- a/ArduPlane/mode_qrtl.cpp +++ b/ArduPlane/mode_qrtl.cpp @@ -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 diff --git a/ArduPlane/mode_qstabilize.cpp b/ArduPlane/mode_qstabilize.cpp index dae092a015..73a7ec9acb 100644 --- a/ArduPlane/mode_qstabilize.cpp +++ b/ArduPlane/mode_qstabilize.cpp @@ -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 diff --git a/ArduPlane/mode_rtl.cpp b/ArduPlane/mode_rtl.cpp index f471acbb26..42f38d560e 100644 --- a/ArduPlane/mode_rtl.cpp +++ b/ArduPlane/mode_rtl.cpp @@ -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 diff --git a/ArduPlane/motor_test.cpp b/ArduPlane/motor_test.cpp index 55aca13089..850e136211 100644 --- a/ArduPlane/motor_test.cpp +++ b/ArduPlane/motor_test.cpp @@ -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 diff --git a/ArduPlane/navigation.cpp b/ArduPlane/navigation.cpp index e6d073aec7..471a45f518 100644 --- a/ArduPlane/navigation.cpp +++ b/ArduPlane/navigation.cpp @@ -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(); } diff --git a/ArduPlane/qautotune.h b/ArduPlane/qautotune.h index 0dd6b6412d..4c22fad69e 100644 --- a/ArduPlane/qautotune.h +++ b/ArduPlane/qautotune.h @@ -6,7 +6,9 @@ #include -#define QAUTOTUNE_ENABLED !HAL_MINIMIZE_FEATURES +#include "quadplane.h" + +#define QAUTOTUNE_ENABLED HAL_QUADPLANE_ENABLED && !HAL_MINIMIZE_FEATURES #if QAUTOTUNE_ENABLED diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 8d36c90810..83a3d8d7ec 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -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 diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index a5e555f9b1..3d1c65b3f8 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -1,5 +1,13 @@ #pragma once +#include + +#ifndef HAL_QUADPLANE_ENABLED +#define HAL_QUADPLANE_ENABLED 1 +#endif + +#if HAL_QUADPLANE_ENABLED + #include #include #include // Attitude control library @@ -651,3 +659,5 @@ private: static QuadPlane *_singleton; }; + +#endif // HAL_QUADPLANE_ENABLED diff --git a/ArduPlane/radio.cpp b/ArduPlane/radio.cpp index e21d136100..48ce2c7da1 100644 --- a/ArduPlane/radio.cpp +++ b/ArduPlane/radio.cpp @@ -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; diff --git a/ArduPlane/servos.cpp b/ArduPlane/servos.cpp index 5aa093290b..9a0df10f8f 100644 --- a/ArduPlane/servos.cpp +++ b/ArduPlane/servos.cpp @@ -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; diff --git a/ArduPlane/system.cpp b/ArduPlane/system.cpp index 35ce1ff2d5..5ff2eb1afd 100644 --- a/ArduPlane/system.cpp +++ b/ArduPlane/system.cpp @@ -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); } diff --git a/ArduPlane/tailsitter.cpp b/ArduPlane/tailsitter.cpp index b5426cd2ef..050ffa5698 100644 --- a/ArduPlane/tailsitter.cpp +++ b/ArduPlane/tailsitter.cpp @@ -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 diff --git a/ArduPlane/takeoff.cpp b/ArduPlane/takeoff.cpp index a3f66441c8..7b994f0af9 100644 --- a/ArduPlane/takeoff.cpp +++ b/ArduPlane/takeoff.cpp @@ -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)) { diff --git a/ArduPlane/tiltrotor.cpp b/ArduPlane/tiltrotor.cpp index 4a2d04c580..f566e038c9 100644 --- a/ArduPlane/tiltrotor.cpp +++ b/ArduPlane/tiltrotor.cpp @@ -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 diff --git a/ArduPlane/tuning.cpp b/ArduPlane/tuning.cpp index 5d24785e0c..68664676b1 100644 --- a/ArduPlane/tuning.cpp +++ b/ArduPlane/tuning.cpp @@ -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 }