diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp index 68178bf733..256edb3f8b 100644 --- a/ArduPlane/Attitude.cpp +++ b/ArduPlane/Attitude.cpp @@ -998,8 +998,8 @@ void Plane::set_servos(void) // manual pass through of throttle while in GUIDED channel_throttle->radio_out = channel_throttle->radio_in; } else if (quadplane.in_vtol_mode()) { - // no forward throttle for now - channel_throttle->servo_out = 0; + // ask quadplane code for forward throttle + channel_throttle->servo_out = quadplane.forward_throttle_pct(); channel_throttle->calc_pwm(); } else { // normal throttle calculation based on servo_out @@ -1072,7 +1072,8 @@ void Plane::set_servos(void) RC_Channel_aux::set_servo_out(RC_Channel_aux::k_flap, manual_flap_percent); if (control_mode >= FLY_BY_WIRE_B || - quadplane.in_assisted_flight()) { + quadplane.in_assisted_flight() || + quadplane.in_vtol_mode()) { /* only do throttle slew limiting in modes where throttle * control is automatic */ throttle_slew_limit(last_throttle); diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index b2494b5938..340928fc71 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -230,7 +230,15 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = { // @Values: 0:Plus, 1:X, 2:V, 3:H, 4:V-Tail, 5:A-Tail, 10:Y6B // @User: Standard AP_GROUPINFO("FRAME_TYPE", 31, QuadPlane, frame_type, 1), - + + // @Param: VFWD_GAIN + // @DisplayName: Forward velocity hold gain + // @Description: Controls use of forward motor in vtol modes. If this is zero then the forward motor will not be used for position control in VTOL modes. A value of 0.1 is a good place to start if you want to use the forward motor for position control. No forward motor will be used in QSTABILIZE or QHOVER modes. Use QLOITER for position hold with the forward motor. + // @Range: 0 0.5 + // @Increment: 0.01 + // @User: Standard + AP_GROUPINFO("VFWD_GAIN", 32, QuadPlane, vel_forward.gain, 0), + AP_GROUPEND }; @@ -1342,3 +1350,75 @@ void QuadPlane::Log_Write_QControl_Tuning() }; plane.DataFlash.WriteBlock(&pkt, sizeof(pkt)); } + + +/* + calculate the forward throttle percentage. The forward throttle can + be used to assist with position hold and with landing approach. It + reduces the need for down pitch which reduces load on the vertical + lift motors. + */ +int8_t QuadPlane::forward_throttle_pct(void) +{ + /* + in non-VTOL modes or modes without a velocity controller. We + don't use it in QHOVER or QSTABILIZE as they are the primary + recovery modes for a quadplane and need to be as simple as + possible. They will drift with the wind + */ + if (!in_vtol_mode() || + !motors->armed() || + vel_forward.gain <= 0 || + plane.control_mode == QSTABILIZE || + plane.control_mode == QHOVER) { + return 0; + } + + float deltat = (AP_HAL::millis() - vel_forward.lastt_ms) * 0.001f; + if (deltat > 1 || deltat < 0) { + vel_forward.integrator = 0; + deltat = 0.1; + } + if (deltat < 0.1) { + // run at 10Hz + return vel_forward.last_pct; + } + vel_forward.lastt_ms = AP_HAL::millis(); + + // work out the desired speed in forward direction + const Vector3f &desired_velocity_cms = pos_control->get_desired_velocity(); + Vector3f vel_ned; + if (!plane.ahrs.get_velocity_NED(vel_ned)) { + // we don't know our velocity? EKF must be pretty sick + vel_forward.last_pct = 0; + return 0; + } + Vector3f vel_error_body = ahrs.get_rotation_body_to_ned().transposed() * ((desired_velocity_cms*0.01f) - vel_ned); + + // find component of velocity error in fwd body frame direction + float fwd_vel_error = vel_error_body * Vector3f(1,0,0); + + // scale forward velocity error by maximum airspeed + fwd_vel_error /= MAX(plane.aparm.airspeed_max, 5); + + // add in a component from our current pitch demand. This tends to + // move us to zero pitch. Assume that LIM_PITCH would give us the + // WP nav speed. + fwd_vel_error -= (wp_nav->get_speed_xy() * 0.01f) * plane.nav_pitch_cd / (float)plane.aparm.pitch_limit_max_cd; + + if (should_relax() && vel_ned.length() < 1) { + // we may be landed + fwd_vel_error = 0; + vel_forward.integrator *= 0.95f; + } + + // integrator as throttle percentage (-100 to 100) + vel_forward.integrator += fwd_vel_error * deltat * vel_forward.gain * 100; + + // constrain to throttle range. This allows for reverse throttle if configured + vel_forward.integrator = constrain_float(vel_forward.integrator, plane.aparm.throttle_min, plane.aparm.throttle_max); + + vel_forward.last_pct = vel_forward.integrator; + + return vel_forward.last_pct; +} diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index 1acbe164bc..c0833b42c2 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -59,6 +59,9 @@ public: return last_throttle * 0.1f; } + // return desired forward throttle percentage + int8_t forward_throttle_pct(void); + struct PACKED log_QControl_Tuning { LOG_PACKET_HEADER; uint64_t time_us; @@ -170,6 +173,13 @@ private: AP_Int8 enable; AP_Int8 transition_pitch_max; + + struct { + AP_Float gain; + float integrator; + uint32_t lastt_ms; + int8_t last_pct; + } vel_forward; bool initialised;