From 14fc6c544642934fe881e9b2d7b39b02f2d3e55b Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Thu, 28 Nov 2019 08:24:00 -0700 Subject: [PATCH] Plane: add logging for tailsitter gainscaling and clean up tailsitter body-frame roll input handling fix metadata for tailsitter param GSCMSK rework roll/yaw scaling for body-frame roll options add constraints on body-frame roll and yaw inputs move speed_scaler logging into QTUN message --- ArduPlane/Log.cpp | 2 +- ArduPlane/quadplane.cpp | 72 ++++++++++++++++++++++++---------------- ArduPlane/quadplane.h | 3 +- ArduPlane/tailsitter.cpp | 4 +-- 4 files changed, 49 insertions(+), 32 deletions(-) diff --git a/ArduPlane/Log.cpp b/ArduPlane/Log.cpp index 087b86d4bb..0e3c9c1fd3 100644 --- a/ArduPlane/Log.cpp +++ b/ArduPlane/Log.cpp @@ -263,7 +263,7 @@ const struct LogStructure Plane::log_structure[] = { { LOG_STATUS_MSG, sizeof(log_Status), "STAT", "QBfBBBBBB", "TimeUS,isFlying,isFlyProb,Armed,Safety,Crash,Still,Stage,Hit", "s--------", "F--------" }, { LOG_QTUN_MSG, sizeof(QuadPlane::log_QControl_Tuning), - "QTUN", "Qffffffeccf", "TimeUS,ThI,ABst,ThO,ThH,DAlt,Alt,BAlt,DCRt,CRt,TMix", "s----mmmnn-", "F----00000-" }, + "QTUN", "Qffffffeccff", "TimeUS,ThI,ABst,ThO,ThH,DAlt,Alt,BAlt,DCRt,CRt,TMix,Sscl", "s----mmmnn--", "F----00000-0" }, { LOG_AOA_SSA_MSG, sizeof(log_AOA_SSA), "AOA", "Qff", "TimeUS,AOA,SSA", "sdd", "F00" }, { LOG_PIQR_MSG, sizeof(log_PID), \ diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 9567f78f88..e03ba010f3 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -474,7 +474,7 @@ const AP_Param::GroupInfo QuadPlane::var_info2[] = { // @DisplayName: Tailsitter gain scaling mask // @Description: Bitmask of gain scaling methods to be applied: BOOST: boost gain at low throttle, ATT_THR: reduce gain at high throttle/tilt, INTERP: interpolate between fixed-wing and copter controls // @User: Standard - // @Bitmask: 1:BOOST,2:ATT_THR,4:INTERP + // @Bitmask: 0:BOOST,1:ATT_THR,2:INTERP AP_GROUPINFO("TAILSIT_GSCMSK", 17, QuadPlane, tailsitter.gain_scaling_mask, TAILSITTER_GSCL_BOOST), // @Param: TAILSIT_GSCMIN @@ -824,42 +824,50 @@ void QuadPlane::multicopter_attitude_rate_update(float yaw_rate_cds) { check_attitude_relax(); - // tailsitter-only bodyframe roll control options + // tailsitter-only body-frame roll control options // Angle mode attitude control for pitch and body-frame roll, rate control for euler yaw. - if (is_tailsitter()) { - const float euler_pitch = plane.nav_pitch_cd * .01f; + if (is_tailsitter() && + ((tailsitter.input_type == TAILSITTER_INPUT_BF_ROLL_M) || + (tailsitter.input_type == TAILSITTER_INPUT_BF_ROLL_P))) { - int16_t roll_limit = MIN(plane.roll_limit_cd, plane.quadplane.aparm.angle_max); + // Since function input_euler_rate_yaw_euler_angle_pitch_bf_roll() + // needs body-frame roll and yaw inputs, it is necessary to rescale + // the plane inputs nav_roll_cd and yaw_rate_cds to represent + // yaw rate and roll angle, respectively. + + // Get the roll and yaw ranges from parameters + int16_t roll_limit = plane.quadplane.aparm.angle_max; // separate limit for tailsitter roll, if set if (plane.quadplane.tailsitter.max_roll_angle > 0) { roll_limit = plane.quadplane.tailsitter.max_roll_angle * 100.0f; } - float roll_rate_limit_cds = plane.quadplane.yaw_rate_max * 100.0f; - float bf_yaw_cds = constrain_float(plane.nav_roll_cd, -roll_rate_limit_cds, roll_rate_limit_cds); - float bf_roll_cd = constrain_float(yaw_rate_cds, -roll_limit, roll_limit); + // Prevent a divide by zero + float yaw_rate_limit = ((yaw_rate_max < 1.0f) ? 1 : yaw_rate_max) * 100.0f; + float yaw2roll_scale = roll_limit / yaw_rate_limit; + if (tailsitter.input_type == TAILSITTER_INPUT_BF_ROLL_M) { - // If pitch is small (nose vertical) use roll rate limit for MC yaw rate and roll limit for MC roll angle - if (fabsf(euler_pitch) < 30.0f) { - float yaw_input_scale = roll_rate_limit_cds / roll_limit; - bf_yaw_cds = constrain_float(yaw_input_scale * plane.nav_roll_cd, -roll_rate_limit_cds, roll_rate_limit_cds); - bf_roll_cd = constrain_float(yaw_rate_cds, -roll_limit, roll_limit); - } - // multicopter style: rudder stick controls bodyframe roll when hovering - attitude_control->input_euler_rate_yaw_euler_angle_pitch_bf_roll_m(bf_yaw_cds, - plane.nav_pitch_cd, - bf_roll_cd); + // In multicopter input mode, the roll and yaw stick axes are independent of pitch + float m_roll_angle = yaw2roll_scale * yaw_rate_cds; + float m_yaw_rate = plane.nav_roll_cd / yaw2roll_scale; + attitude_control->input_euler_rate_yaw_euler_angle_pitch_bf_roll(false, + m_yaw_rate, + plane.nav_pitch_cd, + m_roll_angle); return; } else if (tailsitter.input_type == TAILSITTER_INPUT_BF_ROLL_P) { - // If pitch is small (nose vertical) use roll rate limit for roll rate and roll limit for bf yaw angle - if (fabsf(euler_pitch) < 30.0f) { - bf_yaw_cds = constrain_float(plane.nav_roll_cd, -roll_limit, roll_limit); - bf_roll_cd = constrain_float(yaw_rate_cds, -roll_rate_limit_cds, roll_rate_limit_cds); - } - // plane style: rudder stick controls bodyframe yaw when hovering - attitude_control->input_euler_rate_yaw_euler_angle_pitch_bf_roll_p(bf_yaw_cds, - plane.nav_pitch_cd, - bf_roll_cd); + // In plane input mode, the roll and yaw stick axes rotate as pitch goes from zero to 90 + // so it is necessary to also rotate their scaling. + float euler_pitch = radians(.01f * plane.nav_pitch_cd); + float spitch = fabsf(sinf(euler_pitch)); + float y2r_scale = linear_interpolate(1, yaw2roll_scale, spitch, 0, 1); + float p_yaw_rate = plane.nav_roll_cd / y2r_scale; + float p_roll_angle = y2r_scale * yaw_rate_cds; + + attitude_control->input_euler_rate_yaw_euler_angle_pitch_bf_roll(true, + p_yaw_rate, + plane.nav_pitch_cd, + p_roll_angle); return; } } @@ -1304,7 +1312,14 @@ float QuadPlane::get_pilot_input_yaw_rate_cds(void) const } // add in rudder input - return plane.channel_rudder->get_control_in() * yaw_rate_max / 45; + float max_rate = yaw_rate_max; + if (is_tailsitter() && + ((tailsitter.input_type == TAILSITTER_INPUT_BF_ROLL_M) || + (tailsitter.input_type == TAILSITTER_INPUT_BF_ROLL_P))) { + // constrain max yaw rate + max_rate = (yaw_rate_max < 1.0f) ? 1 : yaw_rate_max; + } + return plane.channel_rudder->get_control_in() * max_rate / 45; } /* @@ -2799,6 +2814,7 @@ void QuadPlane::Log_Write_QControl_Tuning() target_climb_rate : target_climb_rate_cms, climb_rate : int16_t(inertial_nav.get_velocity_z()), throttle_mix : attitude_control->get_throttle_mix(), + speed_scaler : last_spd_scaler, }; plane.logger.WriteBlock(&pkt, sizeof(pkt)); diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index 3d3a85ca49..1765353546 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -149,6 +149,7 @@ public: int16_t target_climb_rate; int16_t climb_rate; float throttle_mix; + float speed_scaler; }; MAV_TYPE get_mav_type(void) const; @@ -477,7 +478,7 @@ private: AP_Int16 motor_mask; AP_Float scaling_speed_min; AP_Float scaling_speed_max; - AP_Int8 gain_scaling_mask; + AP_Int16 gain_scaling_mask; } tailsitter; // tailsitter speed scaler diff --git a/ArduPlane/tailsitter.cpp b/ArduPlane/tailsitter.cpp index ebece0f971..ed6f2f152f 100644 --- a/ArduPlane/tailsitter.cpp +++ b/ArduPlane/tailsitter.cpp @@ -217,8 +217,8 @@ bool QuadPlane::tailsitter_transition_vtol_complete(void) const void QuadPlane::tailsitter_check_input(void) { if (tailsitter_active() && - (tailsitter.input_type == TAILSITTER_INPUT_BF_ROLL_M || - tailsitter.input_type == TAILSITTER_INPUT_BF_ROLL_P || + (tailsitter.input_type == TAILSITTER_INPUT_BF_ROLL_P || + tailsitter.input_type == TAILSITTER_INPUT_BF_ROLL_M || tailsitter.input_type == TAILSITTER_INPUT_PLANE)) { // the user has asked for body frame controls when tailsitter // is active. We switch around the control_in value for the