From 487ad7a90f27583389d079b1410a593cf35a7661 Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Sun, 17 Feb 2019 11:42:05 -0700 Subject: [PATCH] Plane: add QACRO mode for quadplanes use acro_r/p/y_rate params in qacro port ACRO throttle handling from copter handle non-tailsitter controls --- ArduPlane/ArduPlane.cpp | 3 +- ArduPlane/Attitude.cpp | 3 ++ ArduPlane/GCS_Mavlink.cpp | 1 + ArduPlane/GCS_Plane.cpp | 1 + ArduPlane/defines.h | 3 +- ArduPlane/quadplane.cpp | 80 +++++++++++++++++++++++++++++++++++++++ ArduPlane/quadplane.h | 2 + ArduPlane/system.cpp | 1 + 8 files changed, 92 insertions(+), 2 deletions(-) diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index 4a327464b4..d2fd53ca6f 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -679,7 +679,7 @@ void Plane::update_flight_mode(void) break; } - + case QACRO: case INITIALISING: // handled elsewhere break; @@ -775,6 +775,7 @@ void Plane::update_navigation() case QLAND: case QRTL: case QAUTOTUNE: + case QACRO: // nothing to do break; } diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp index 4f724ff56c..f3771ab68f 100644 --- a/ArduPlane/Attitude.cpp +++ b/ArduPlane/Attitude.cpp @@ -155,6 +155,7 @@ void Plane::stabilize_stick_mixing_direct() control_mode == QLOITER || control_mode == QLAND || control_mode == QRTL || + control_mode == QACRO || control_mode == TRAINING || control_mode == QAUTOTUNE) { return; @@ -185,6 +186,7 @@ void Plane::stabilize_stick_mixing_fbw() control_mode == QLOITER || control_mode == QLAND || control_mode == QRTL || + control_mode == QACRO || control_mode == TRAINING || control_mode == QAUTOTUNE || (control_mode == AUTO && g.auto_fbw_steer == 42)) { @@ -396,6 +398,7 @@ void Plane::stabilize() control_mode == QLOITER || control_mode == QLAND || control_mode == QRTL || + control_mode == QACRO || control_mode == QAUTOTUNE) && !quadplane.in_tailsitter_vtol_transition()) { quadplane.control_run(); diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index cbac1df086..b4ff191827 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -23,6 +23,7 @@ MAV_MODE GCS_MAVLINK_Plane::base_mode() const case MANUAL: case TRAINING: case ACRO: + case QACRO: _base_mode = MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; break; case STABILIZE: diff --git a/ArduPlane/GCS_Plane.cpp b/ArduPlane/GCS_Plane.cpp index ded708e21f..e93c68eb8d 100644 --- a/ArduPlane/GCS_Plane.cpp +++ b/ArduPlane/GCS_Plane.cpp @@ -77,6 +77,7 @@ void GCS_Plane::update_sensor_status_flags(void) break; case ACRO: + case QACRO: control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL; // 3D angular rate control break; diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h index 31cf56adef..546d63331c 100644 --- a/ArduPlane/defines.h +++ b/ArduPlane/defines.h @@ -66,7 +66,8 @@ enum FlightMode { QLOITER = 19, QLAND = 20, QRTL = 21, - QAUTOTUNE = 22 + QAUTOTUNE = 22, + QACRO = 23, }; enum mode_reason_t { diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 5a529e32ea..790894db82 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -807,6 +807,16 @@ void QuadPlane::check_attitude_relax(void) last_att_control_ms = now; } +/* + init QACRO mode + */ +void QuadPlane::init_qacro(void) +{ + throttle_wait = false; + transition_state = TRANSITION_DONE; + attitude_control->relax_attitude_controllers(); +} + // init quadplane hover mode void QuadPlane::init_hover(void) { @@ -858,6 +868,66 @@ void QuadPlane::hold_hover(float target_climb_rate) run_z_controller(); } +/* + control QACRO mode + */ +void QuadPlane::control_qacro(void) +{ + if (throttle_wait) { + motors->set_desired_spool_state(AP_Motors::DESIRED_GROUND_IDLE); + attitude_control->set_throttle_out_unstabilized(0, true, 0); + } else { + check_attitude_relax(); + + motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); + + // convert the input to the desired body frame rate + float target_roll = 0; + float target_pitch = plane.channel_pitch->norm_input() * plane.g.acro_pitch_rate * 100.0f; + float target_yaw = 0; + if (is_tailsitter()) { + // Note that the 90 degree Y rotation for copter mode swaps body-frame roll and yaw + // acro_roll_rate param applies to yaw in copter frame + // no parameter for acro yaw rate; just use the normal one since the default is 100 deg/sec + target_roll = plane.channel_rudder->norm_input() * yaw_rate_max * 100.0f; + target_yaw = -plane.channel_roll->norm_input() * plane.g.acro_roll_rate * 100.0f; + } else { + target_roll = plane.channel_roll->norm_input() * plane.g.acro_roll_rate * 100.0f; + target_yaw = plane.channel_rudder->norm_input() * yaw_rate_max * 100.0; + } + + // get pilot's desired throttle + int16_t mid_stick = plane.channel_throttle->get_control_mid(); + // protect against unlikely divide by zero + if (mid_stick <= 0) { + mid_stick = 50; + } + float thr_mid = motors->get_throttle_hover(); + int16_t throttle_control = plane.channel_throttle->get_control_in(); + float throttle_in; + if (throttle_control < mid_stick) { + // below the deadband + throttle_in = ((float)throttle_control) * 0.5f / (float)mid_stick; + } else if (throttle_control > mid_stick) { + // above the deadband + throttle_in = 0.5f + ((float)(throttle_control - mid_stick)) * 0.5f / (float)(100 - mid_stick); + } else { + // must be in the deadband + throttle_in = 0.5f; + } + + float expo = constrain_float(-(thr_mid - 0.5) / 0.375, -0.5f, 1.0f); + // calculate the output throttle using the given expo function + float throttle_out = throttle_in*(1.0f-expo) + expo*throttle_in*throttle_in*throttle_in; + + // run attitude controller + attitude_control->input_rate_bf_roll_pitch_yaw_3(target_roll, target_pitch, target_yaw); + + // output pilot's throttle without angle boost + attitude_control->set_throttle_out(throttle_out, false, 10.0f); + } +} + /* control QHOVER mode */ @@ -954,6 +1024,9 @@ bool QuadPlane::is_flying_vtol(void) const // if we are demanding more than 1% throttle then don't consider aircraft landed return true; } + if (plane.control_mode == QACRO) { + return true; + } if (plane.control_mode == GUIDED && guided_takeoff) { return true; } @@ -1703,6 +1776,9 @@ void QuadPlane::control_run(void) } switch (plane.control_mode) { + case QACRO: + control_qacro(); + break; case QSTABILIZE: control_stabilize(); break; @@ -1768,6 +1844,9 @@ bool QuadPlane::init_mode(void) case QAUTOTUNE: return qautotune.init(); #endif + case QACRO: + init_qacro(); + break; default: break; } @@ -1857,6 +1936,7 @@ bool QuadPlane::in_vtol_mode(void) const plane.control_mode == QLOITER || plane.control_mode == QLAND || plane.control_mode == QRTL || + plane.control_mode == QACRO || plane.control_mode == QAUTOTUNE || ((plane.control_mode == GUIDED || plane.control_mode == AVOID_ADSB) && plane.auto_state.vtol_loiter) || in_vtol_auto()); diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index 07411142cc..c66f248501 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -196,6 +196,8 @@ private: void control_stabilize(void); void check_attitude_relax(void); + void init_qacro(void); + void control_qacro(void); void init_hover(void); void control_hover(void); diff --git a/ArduPlane/system.cpp b/ArduPlane/system.cpp index 2046fb689a..957e109046 100644 --- a/ArduPlane/system.cpp +++ b/ArduPlane/system.cpp @@ -473,6 +473,7 @@ void Plane::set_mode(enum FlightMode mode, mode_reason_t reason) case QLAND: case QRTL: case QAUTOTUNE: + case QACRO: throttle_allows_nudging = true; auto_navigation_mode = false; if (!quadplane.init_mode()) {