Where are Quadplane flight modes implemented in the code?

Hi all, apologies if this is a stupid question but I am wondering where in the Plane code the Quadplane-specific flight modes (QSTABILIZE, QHOVER, etc) are implemented. I see that over in the Copter code, there exist files for each mode, “control_stabilize.cpp”, “control_rtl.cpp” etc, but there don’t appear to be any equivalents on the Plane side. Are the Quadplane modes drawing on the Copter implementations somehow? Any guidance appreciated!

I believe everything you’ll want is in the ArduPlane directory.

In defines.h, you’ll see the enum FlightMode which defines the available modes. Then, you can text-search on the name of whichever mode you want to find code… for example, I see QSTABILIZE code in:
tiltrotor.cpp (L 112)
system.cpp (L 429 and 638)
events.cpp (L28 and 90)
sensors.cpp, Attitude.cpp, (and others, I didn’t want to type them all)

Does that help?

Thanks! Yes, that does help. I was basically trying to figure out if the stabilization control loop for quadplanes and multicopters was the same, and at looks like it is. If I follow the function calls from where QSTABILIZE is used, it looks like the quadplane code is using the same function (AC_AttitudeControl::input_euler_angle_roll_pitch_euler_rate_yaw(…)) as the Copter stabilize code:

  /*
  ask the multicopter attitude control to match the roll and pitch rates being demanded by the
  fixed wing controller if not in a pure VTOL mode
 */
void QuadPlane::multicopter_attitude_rate_update(float yaw_rate_cds, float smooth_gain)
{
    if (in_vtol_mode() || is_tailsitter()) {
        // use euler angle attitude control
        attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(plane.nav_roll_cd,
                                                                      plane.nav_pitch_cd,
                                                                      yaw_rate_cds,
                                                                      smooth_gain);
    } else {
        // use the fixed wing desired rates
        float roll_rate = plane.rollController.get_pid_info().desired;
        float pitch_rate = plane.pitchController.get_pid_info().desired;
        attitude_control->input_euler_rate_roll_pitch_yaw(roll_rate*100, pitch_rate*100, yaw_rate_cds);
    }
}
1 Like

This is so close to being on the same topic that I feel bad starting a new one – can anyone show me where in the code the quadplane PID tuning parameters (Q_A_RAT_RLL/PIT/YAW_P/I/D) are used? The only instance of them I can find is in quadplane.cpp where the defaults are defined on line 405. Theres gotta be more than that, right?

/*
  defaults for all quadplanes
 */
static const struct defaults_struct defaults_table[] = {
{ "Q_A_RAT_RLL_P",    0.25 },
{ "Q_A_RAT_RLL_I",    0.25 },
{ "Q_A_RAT_RLL_FILT", 10.0 },
{ "Q_A_RAT_PIT_P",    0.25 },
{ "Q_A_RAT_PIT_I",    0.25 },
{ "Q_A_RAT_PIT_FILT", 10.0 },
{ "Q_M_SPOOL_TIME",   0.25 },
};

Also, what exactly does Q_A_RAT_[AXIS]_FILT do? The docs say it is the “axis rate controller input frequency in Hz”, so is this actually changing how ofter the rate PID loop runs?

I think you’ll need to understand a bit how AP parameters work to interpret them… let us know if you dig into that and get stuck?

I’m not very familiar with it, but many of those look like parameters found in libraries/AC_PID, with prefixes indicating what the PID is controlling. Check there?

For the prefixes, you might checkout ArduPlane/Parameters.cpp to see what the parameter prefixes Q_, Q_M_, and Q_A_ mean…