diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index d28efbfb58..46111f0fbd 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -684,6 +684,21 @@ void Plane::update_flight_mode(void) steering_control.steering = steering_control.rudder = channel_rudder->pwm_to_angle(); break; //roll: -13788.000, pitch: -13698.000, thr: 0.000, rud: -13742.000 + + + case HOVER: { + // set nav_roll and nav_pitch using sticks + nav_roll_cd = channel_roll->norm_input() * roll_limit_cd; + nav_roll_cd = constrain_int32(nav_roll_cd, -roll_limit_cd, roll_limit_cd); + float pitch_input = channel_pitch->norm_input(); + if (pitch_input > 0) { + nav_pitch_cd = pitch_input * aparm.pitch_limit_max_cd; + } else { + nav_pitch_cd = -(pitch_input * pitch_limit_min_cd); + } + nav_pitch_cd = constrain_int32(nav_pitch_cd, pitch_limit_min_cd, aparm.pitch_limit_max_cd.get()); + break; + } case INITIALISING: // handled elsewhere @@ -749,6 +764,7 @@ void Plane::update_navigation() case AUTOTUNE: case FLY_BY_WIRE_B: case CIRCLE: + case HOVER: // nothing to do break; } diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp index 1098ea6b08..6396ab55c0 100644 --- a/ArduPlane/Attitude.cpp +++ b/ArduPlane/Attitude.cpp @@ -140,6 +140,7 @@ void Plane::stabilize_stick_mixing_direct() control_mode == AUTOTUNE || control_mode == FLY_BY_WIRE_B || control_mode == CRUISE || + control_mode == HOVER || control_mode == TRAINING) { return; } @@ -159,6 +160,7 @@ void Plane::stabilize_stick_mixing_fbw() control_mode == AUTOTUNE || control_mode == FLY_BY_WIRE_B || control_mode == CRUISE || + control_mode == HOVER || control_mode == TRAINING || (control_mode == AUTO && g.auto_fbw_steer)) { return; @@ -354,6 +356,8 @@ void Plane::stabilize() stabilize_training(speed_scaler); } else if (control_mode == ACRO) { stabilize_acro(speed_scaler); + } else if (control_mode == HOVER) { + quadplane.stabilize_hover(); } else { if (g.stick_mixing == STICK_MIXING_FBW && control_mode != STABILIZE) { stabilize_stick_mixing_fbw(); @@ -760,6 +764,9 @@ void Plane::set_servos(void) { int16_t last_throttle = channel_throttle->radio_out; + // do any transition updates for quadplane + quadplane.update(); + if (control_mode == AUTO && auto_state.idle_mode) { // special handling for balloon launch set_servos_idle(); @@ -915,6 +922,10 @@ void Plane::set_servos(void) guided_throttle_passthru) { // manual pass through of throttle while in GUIDED channel_throttle->radio_out = channel_throttle->radio_in; + } else if (control_mode == HOVER) { + // no forward throttle for now + channel_throttle->servo_out = 0; + channel_throttle->calc_pwm(); } else { // normal throttle calculation based on servo_out channel_throttle->calc_pwm(); diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index e0af13a13d..c0daf2dec7 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -39,6 +39,7 @@ void Plane::send_heartbeat(mavlink_channel_t chan) case FLY_BY_WIRE_A: case AUTOTUNE: case FLY_BY_WIRE_B: + case HOVER: case CRUISE: base_mode = MAV_MODE_FLAG_STABILIZE_ENABLED; break; @@ -169,6 +170,7 @@ void Plane::send_extended_status1(mavlink_channel_t chan) case STABILIZE: case FLY_BY_WIRE_A: case AUTOTUNE: + case HOVER: control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL; // 3D angular rate control control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION; // attitude stabilisation break; diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index 215fa8d5b2..2bfd622fce 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -1034,6 +1034,10 @@ const AP_Param::Info Plane::var_info[] = { // @Path: ../libraries/AP_ADSB/AP_ADSB.cpp GOBJECT(adsb, "ADSB_", AP_ADSB), + // @Group: Q_ + // @Path: quadplane.cpp + GOBJECT(quadplane, "Q_", QuadPlane), + // RC channel //----------- // @Group: RC1_ diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index 6be939aed4..8f8ae66e0c 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -276,6 +276,7 @@ public: k_param_kff_pitch_to_throttle, // unused k_param_kff_throttle_to_pitch, k_param_scaling_speed, + k_param_quadplane, // // 210: flight modes diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 303a5e84ee..c61d9f268b 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -96,6 +96,8 @@ #include #include +#include "quadplane.h" + // Configuration #include "config.h" @@ -135,6 +137,7 @@ public: friend class GCS_MAVLINK; friend class Parameters; friend class AP_Arming_Plane; + friend class QuadPlane; Plane(void); @@ -712,7 +715,9 @@ private: // time that rudder arming has been running uint32_t rudder_arm_timer; - + // support for quadcopter-plane + QuadPlane quadplane{ahrs}; + void demo_servos(uint8_t i); void adjust_nav_pitch_throttle(void); void update_load_factor(void); diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h index 438a8f886b..8be362c5ae 100644 --- a/ArduPlane/defines.h +++ b/ArduPlane/defines.h @@ -63,7 +63,8 @@ enum FlightMode { RTL = 11, LOITER = 12, GUIDED = 15, - INITIALISING = 16 + INITIALISING = 16, + HOVER = 17 }; // type of stick mixing enabled diff --git a/ArduPlane/events.cpp b/ArduPlane/events.cpp index c0989bf390..39217e9f7e 100644 --- a/ArduPlane/events.cpp +++ b/ArduPlane/events.cpp @@ -13,6 +13,7 @@ void Plane::failsafe_short_on_event(enum failsafe_state fstype) case MANUAL: case STABILIZE: case ACRO: + case HOVER: case FLY_BY_WIRE_A: case AUTOTUNE: case FLY_BY_WIRE_B: @@ -61,6 +62,7 @@ void Plane::failsafe_long_on_event(enum failsafe_state fstype) case MANUAL: case STABILIZE: case ACRO: + case HOVER: case FLY_BY_WIRE_A: case AUTOTUNE: case FLY_BY_WIRE_B: diff --git a/ArduPlane/make.inc b/ArduPlane/make.inc index 8dbc87585d..6e73b694aa 100644 --- a/ArduPlane/make.inc +++ b/ArduPlane/make.inc @@ -49,4 +49,7 @@ LIBRARIES += AP_RSSI LIBRARIES += AP_RPM LIBRARIES += AP_Parachute LIBRARIES += AP_ADSB - +LIBRARIES += AP_Motors +LIBRARIES += AC_AttitudeControl +LIBRARIES += AC_PID +LIBRARIES += AP_InertialNav diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp new file mode 100644 index 0000000000..2d3891bda3 --- /dev/null +++ b/ArduPlane/quadplane.cpp @@ -0,0 +1,307 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +#include "Plane.h" + +const AP_Param::GroupInfo QuadPlane::var_info[] = { + + // @Group: MOT_ + // @Path: ../libraries/AP_Motors/AP_MotorsMulticopter.cpp + AP_SUBGROUPINFO(motors, "M_", 1, QuadPlane, AP_MotorsQuad), + + // @Param: RT_RLL_P + // @DisplayName: Roll axis rate controller P gain + // @Description: Roll axis rate controller P gain. Converts the difference between desired roll rate and actual roll rate into a motor speed output + // @Range: 0.08 0.30 + // @Increment: 0.005 + // @User: Standard + + // @Param: RT_RLL_I + // @DisplayName: Roll axis rate controller I gain + // @Description: Roll axis rate controller I gain. Corrects long-term difference in desired roll rate vs actual roll rate + // @Range: 0.01 0.5 + // @Increment: 0.01 + // @User: Standard + + // @Param: RT_RLL_IMAX + // @DisplayName: Roll axis rate controller I gain maximum + // @Description: Roll axis rate controller I gain maximum. Constrains the maximum motor output that the I gain will output + // @Range: 0 4500 + // @Increment: 10 + // @Units: Percent*10 + // @User: Standard + + // @Param: RT_RLL_D + // @DisplayName: Roll axis rate controller D gain + // @Description: Roll axis rate controller D gain. Compensates for short-term change in desired roll rate vs actual roll rate + // @Range: 0.001 0.02 + // @Increment: 0.001 + // @User: Standard + AP_SUBGROUPINFO(pid_rate_roll, "RT_RLL_", 2, QuadPlane, AC_PID), + + // @Param: RT_PIT_P + // @DisplayName: Pitch axis rate controller P gain + // @Description: Pitch axis rate controller P gain. Converts the difference between desired pitch rate and actual pitch rate into a motor speed output + // @Range: 0.08 0.30 + // @Increment: 0.005 + // @User: Standard + + // @Param: RT_PIT_I + // @DisplayName: Pitch axis rate controller I gain + // @Description: Pitch axis rate controller I gain. Corrects long-term difference in desired pitch rate vs actual pitch rate + // @Range: 0.01 0.5 + // @Increment: 0.01 + // @User: Standard + + // @Param: RT_PIT_IMAX + // @DisplayName: Pitch axis rate controller I gain maximum + // @Description: Pitch axis rate controller I gain maximum. Constrains the maximum motor output that the I gain will output + // @Range: 0 4500 + // @Increment: 10 + // @Units: Percent*10 + // @User: Standard + + // @Param: RT_PIT_D + // @DisplayName: Pitch axis rate controller D gain + // @Description: Pitch axis rate controller D gain. Compensates for short-term change in desired pitch rate vs actual pitch rate + // @Range: 0.001 0.02 + // @Increment: 0.001 + // @User: Standard + AP_SUBGROUPINFO(pid_rate_pitch, "RT_PIT_", 3, QuadPlane, AC_PID), + + // @Param: RT_YAW_P + // @DisplayName: Yaw axis rate controller P gain + // @Description: Yaw axis rate controller P gain. Converts the difference between desired yaw rate and actual yaw rate into a motor speed output + // @Range: 0.150 0.50 + // @Increment: 0.005 + // @User: Standard + + // @Param: RT_YAW_I + // @DisplayName: Yaw axis rate controller I gain + // @Description: Yaw axis rate controller I gain. Corrects long-term difference in desired yaw rate vs actual yaw rate + // @Range: 0.010 0.05 + // @Increment: 0.01 + // @User: Standard + + // @Param: RT_YAW_IMAX + // @DisplayName: Yaw axis rate controller I gain maximum + // @Description: Yaw axis rate controller I gain maximum. Constrains the maximum motor output that the I gain will output + // @Range: 0 4500 + // @Increment: 10 + // @Units: Percent*10 + // @User: Standard + + // @Param: RT_YAW_D + // @DisplayName: Yaw axis rate controller D gain + // @Description: Yaw axis rate controller D gain. Compensates for short-term change in desired yaw rate vs actual yaw rate + // @Range: 0.000 0.02 + // @Increment: 0.001 + // @User: Standard + AP_SUBGROUPINFO(pid_rate_yaw, "RT_YAW_", 4, QuadPlane, AC_PID), + + // P controllers + //-------------- + // @Param: STB_RLL_P + // @DisplayName: Roll axis stabilize controller P gain + // @Description: Roll axis stabilize (i.e. angle) controller P gain. Converts the error between the desired roll angle and actual angle to a desired roll rate + // @Range: 3.000 12.000 + // @User: Standard + AP_SUBGROUPINFO(p_stabilize_roll, "STB_R_", 5, QuadPlane, AC_P), + + // @Param: STB_PIT_P + // @DisplayName: Pitch axis stabilize controller P gain + // @Description: Pitch axis stabilize (i.e. angle) controller P gain. Converts the error between the desired pitch angle and actual angle to a desired pitch rate + // @Range: 3.000 12.000 + // @User: Standard + AP_SUBGROUPINFO(p_stabilize_pitch, "STB_P_", 6, QuadPlane, AC_P), + + // @Param: STB_YAW_P + // @DisplayName: Yaw axis stabilize controller P gain + // @Description: Yaw axis stabilize (i.e. angle) controller P gain. Converts the error between the desired yaw angle and actual angle to a desired yaw rate + // @Range: 3.000 6.000 + // @User: Standard + AP_SUBGROUPINFO(p_stabilize_yaw, "STB_Y_", 7, QuadPlane, AC_P), + + // @Group: ATC_ + // @Path: ../libraries/AC_AttitudeControl/AC_AttitudeControl.cpp + AP_SUBGROUPINFO(attitude_control, "A_", 8, QuadPlane, AC_AttitudeControl), + + // @Param: ANGLE_MAX + // @DisplayName: Angle Max + // @Description: Maximum lean angle in all flight modes + // @Units: Centi-degrees + // @Range: 1000 8000 + // @User: Advanced + AP_GROUPINFO("ANGLE_MAX", 9, QuadPlane, aparm.angle_max, 4500), + + // @Param: TRANSITION_MS + // @DisplayName: Transition time + // @Description: Transition time in milliseconds + // @Units: milli-seconds + // @Range: 0 30000 + // @User: Advanced + AP_GROUPINFO("TRANSITION_MS", 10, QuadPlane, transition_time_ms, 10000), + + // @Param: POS_Z_P + // @DisplayName: Position (vertical) controller P gain + // @Description: Position (vertical) controller P gain. Converts the difference between the desired altitude and actual altitude into a climb or descent rate which is passed to the throttle rate controller + // @Range: 1.000 3.000 + // @User: Standard + AP_SUBGROUPINFO(p_alt_hold, "PZ_", 11, QuadPlane, AC_P), + + // @Param: POS_XY_P + // @DisplayName: Position (horizonal) controller P gain + // @Description: Loiter position controller P gain. Converts the distance (in the latitude direction) to the target location into a desired speed which is then passed to the loiter latitude rate controller + // @Range: 0.500 2.000 + // @User: Standard + AP_SUBGROUPINFO(p_pos_xy, "PXY_", 12, QuadPlane, AC_P), + + // @Param: VEL_XY_P + // @DisplayName: Velocity (horizontal) P gain + // @Description: Velocity (horizontal) P gain. Converts the difference between desired velocity to a target acceleration + // @Range: 0.1 6.0 + // @Increment: 0.1 + // @User: Advanced + + // @Param: VEL_XY_I + // @DisplayName: Velocity (horizontal) I gain + // @Description: Velocity (horizontal) I gain. Corrects long-term difference in desired velocity to a target acceleration + // @Range: 0.02 1.00 + // @Increment: 0.01 + // @User: Advanced + + // @Param: VEL_XY_IMAX + // @DisplayName: Velocity (horizontal) integrator maximum + // @Description: Velocity (horizontal) integrator maximum. Constrains the target acceleration that the I gain will output + // @Range: 0 4500 + // @Increment: 10 + // @Units: cm/s/s + // @User: Advanced + AP_SUBGROUPINFO(pi_vel_xy, "VXY_", 13, QuadPlane, AC_PI_2D), + + // @Param: VEL_Z_P + // @DisplayName: Velocity (vertical) P gain + // @Description: Velocity (vertical) P gain. Converts the difference between desired vertical speed and actual speed into a desired acceleration that is passed to the throttle acceleration controller + // @Range: 1.000 8.000 + // @User: Standard + AP_SUBGROUPINFO(p_vel_z, "VZ_", 14, QuadPlane, AC_P), + + // @Param: ACCEL_Z_P + // @DisplayName: Throttle acceleration controller P gain + // @Description: Throttle acceleration controller P gain. Converts the difference between desired vertical acceleration and actual acceleration into a motor output + // @Range: 0.500 1.500 + // @User: Standard + + // @Param: ACCEL_Z_I + // @DisplayName: Throttle acceleration controller I gain + // @Description: Throttle acceleration controller I gain. Corrects long-term difference in desired vertical acceleration and actual acceleration + // @Range: 0.000 3.000 + // @User: Standard + + // @Param: ACCEL_Z_IMAX + // @DisplayName: Throttle acceleration controller I gain maximum + // @Description: Throttle acceleration controller I gain maximum. Constrains the maximum pwm that the I term will generate + // @Range: 0 1000 + // @Units: Percent*10 + // @User: Standard + + // @Param: ACCEL_Z_D + // @DisplayName: Throttle acceleration controller D gain + // @Description: Throttle acceleration controller D gain. Compensates for short-term change in desired vertical acceleration vs actual acceleration + // @Range: 0.000 0.400 + // @User: Standard + + // @Param: ACCEL_Z_FILT_HZ + // @DisplayName: Throttle acceleration filter + // @Description: Filter applied to acceleration to reduce noise. Lower values reduce noise but add delay. + // @Range: 1.000 100.000 + // @Units: Hz + // @User: Standard + AP_SUBGROUPINFO(pid_accel_z, "AZ_", 15, QuadPlane, AC_PID), + + // @Group: POSCON_ + // @Path: ../libraries/AC_AttitudeControl/AC_PosControl.cpp + AP_SUBGROUPINFO(pos_control, "P", 16, QuadPlane, AC_PosControl), + + AP_GROUPEND +}; + +QuadPlane::QuadPlane(AP_AHRS_NavEKF &_ahrs) : + ahrs(_ahrs) +{ + AP_Param::setup_object_defaults(this, var_info); +} + +void QuadPlane::setup(void) +{ + motors.set_frame_orientation(AP_MOTORS_QUADPLANE); + motors.set_throttle_range(0, 1000, 2000); + motors.set_hover_throttle(500); + motors.set_update_rate(490); + motors.set_interlock(true); + motors.set_loop_rate(plane.ins.get_sample_rate()); + attitude_control.set_dt(plane.ins.get_loop_delta_t()); + pid_rate_roll.set_dt(plane.ins.get_loop_delta_t()); + pid_rate_pitch.set_dt(plane.ins.get_loop_delta_t()); + pid_rate_yaw.set_dt(plane.ins.get_loop_delta_t()); + pid_accel_z.set_dt(plane.ins.get_loop_delta_t()); +} + +// stabilize in hover mode +void QuadPlane::stabilize_hover(void) +{ + // max 100 degrees/sec for now + const float yaw_rate_max_dps = 100; + float yaw_rate_ef_cds = plane.channel_rudder->norm_input() * 100 * yaw_rate_max_dps; + + // call attitude controller + attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(plane.nav_roll_cd, plane.nav_pitch_cd, yaw_rate_ef_cds, smoothing_gain); + + // output pilot's throttle + int16_t pilot_throttle_scaled = plane.channel_throttle->control_in * 10; + attitude_control.set_throttle_out(pilot_throttle_scaled, true, 0); + + // run low level rate controllers that only require IMU data + attitude_control.rate_controller_run(); + + last_run_ms = millis(); + last_throttle = pilot_throttle_scaled; +} + +// set motor arming +void QuadPlane::set_armed(bool armed) +{ + motors.armed(armed); + if (armed) { + motors.enable(); + } +} + + +/* + update for transition from quadplane + */ +void QuadPlane::update_transition(void) +{ + if (millis() - last_run_ms < (unsigned)transition_time_ms && plane.control_mode != MANUAL) { + // we are transitioning. Call attitude controller + attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(plane.nav_roll_cd, plane.nav_pitch_cd, 0, smoothing_gain); + // and degrade throttle + int16_t throttle_scaled = last_throttle * (transition_time_ms - (millis() - last_run_ms)) / (float)transition_time_ms; + attitude_control.set_throttle_out(throttle_scaled, true, 0); + motors.output(); + } else { + motors.output_min(); + } +} + +/* + update motor output for quadplane + */ +void QuadPlane::update(void) +{ + if (plane.control_mode != HOVER) { + update_transition(); + } else { + motors.output(); + } +} diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h new file mode 100644 index 0000000000..1abe69f064 --- /dev/null +++ b/ArduPlane/quadplane.h @@ -0,0 +1,72 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +#include +#include +#include // Attitude control library +#include +#include + +/* + QuadPlane specific functionality + */ +class QuadPlane +{ +public: + friend class Plane; + QuadPlane(AP_AHRS_NavEKF &_ahrs); + + // var_info for holding Parameter information + static const struct AP_Param::GroupInfo var_info[]; + + // setup quadplane + void setup(void); + + // stabilize in hover mode + void stabilize_hover(void); + + // update transition handling + void update(void); + + // set motor arming + void set_armed(bool armed); + +private: + AP_AHRS_NavEKF &ahrs; + AP_Vehicle::MultiCopter aparm; + AP_MotorsQuad motors{50}; + AC_PID pid_rate_roll {0.15, 0.1, 0.004, 2000, 20, 0.02}; + AC_PID pid_rate_pitch{0.15, 0.1, 0.004, 2000, 20, 0.02}; + AC_PID pid_rate_yaw {0.15, 0.1, 0.004, 2000, 20, 0.02}; + AC_P p_stabilize_roll{4.5}; + AC_P p_stabilize_pitch{4.5}; + AC_P p_stabilize_yaw{4.5}; + + AC_AttitudeControl_Multi attitude_control{ahrs, aparm, motors, + p_stabilize_roll, p_stabilize_pitch, p_stabilize_yaw, + pid_rate_roll, pid_rate_pitch, pid_rate_yaw}; + + AP_InertialNav_NavEKF inertial_nav{ahrs}; + + AC_P p_pos_xy{1}; + AC_P p_alt_hold{1}; + AC_P p_vel_z{5}; + AC_PID pid_accel_z{0.5, 1, 0, 800, 20, 0.02}; + AC_PI_2D pi_vel_xy{1.0, 0.5, 1000, 5, 0.02}; + + AC_PosControl pos_control{ahrs, inertial_nav, motors, attitude_control, + p_alt_hold, p_vel_z, pid_accel_z, + p_pos_xy, pi_vel_xy}; + + // update transition handling + void update_transition(void); + + AP_Int16 transition_time_ms; + + // last time quadplane was active, used for transition + uint32_t last_run_ms; + + // last throttle value when active + int16_t last_throttle; + + const float smoothing_gain = 6; +}; diff --git a/ArduPlane/system.cpp b/ArduPlane/system.cpp index 657ac37a20..57ec05fa80 100644 --- a/ArduPlane/system.cpp +++ b/ArduPlane/system.cpp @@ -233,6 +233,8 @@ void Plane::init_ardupilot() startup_ground(); + quadplane.setup(); + // choose the nav controller set_nav_controller(); @@ -396,7 +398,7 @@ void Plane::set_mode(enum FlightMode mode) acro_state.locked_roll = false; acro_state.locked_pitch = false; break; - + case CRUISE: auto_throttle_mode = true; cruise_state.locked_heading = false; @@ -443,6 +445,10 @@ void Plane::set_mode(enum FlightMode mode) guided_WP_loc = current_loc; set_guided_WP(); break; + + case HOVER: + auto_throttle_mode = false; + break; } // start with throttle suppressed in auto_throttle modes @@ -477,6 +483,7 @@ bool Plane::mavlink_set_mode(uint8_t mode) case AUTO: case RTL: case LOITER: + case HOVER: set_mode((enum FlightMode)mode); return true; } @@ -743,6 +750,7 @@ void Plane::change_arm_state(void) Log_Arm_Disarm(); hal.util->set_soft_armed(arming.is_armed() && hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED); + quadplane.set_armed(hal.util->get_soft_armed()); } /*