diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index b14b18c700..00a213d764 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -398,7 +398,12 @@ void Plane::send_pid_tuning(mavlink_channel_t chan) if ((g.gcs_pid_mask & TUNING_BITS_LAND) && (flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND)) { send_pid_info(chan, landing.get_pid_info(), PID_TUNING_LANDING, degrees(gyro.z)); } -} + if (g.gcs_pid_mask & TUNING_BITS_ACCZ && quadplane.in_vtol_mode()) { + const Vector3f &accel = ahrs.get_accel_ef(); + pid_info = &quadplane.pos_control->get_accel_z_pid().get_pid_info(); + send_pid_info(chan, pid_info, PID_TUNING_ACCZ, -accel.z); + } + } uint8_t GCS_MAVLINK_Plane::sysid_my_gcs() const { diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h index d4374990fd..bc8138b291 100644 --- a/ArduPlane/defines.h +++ b/ArduPlane/defines.h @@ -123,6 +123,7 @@ enum tuning_pid_bits { TUNING_BITS_YAW = (1 << 2), TUNING_BITS_STEER = (1 << 3), TUNING_BITS_LAND = (1 << 4), + TUNING_BITS_ACCZ = (1 << 5), TUNING_BITS_END // dummy just used for static checking };