diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index fba7e28997..398a575998 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -388,6 +388,21 @@ void Plane::send_pid_tuning(mavlink_channel_t chan) return; } } + if ((g.gcs_pid_mask & 0x10) && (flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND)) { + pid_info = landing.get_pid_info(); + if (pid_info != nullptr) { + mavlink_msg_pid_tuning_send(chan, PID_TUNING_LANDING, + pid_info->desired, + gyro.z, + pid_info->FF, + pid_info->P, + pid_info->I, + pid_info->D); + } + if (!HAVE_PAYLOAD_SPACE(chan, PID_TUNING)) { + return; + } + } } void Plane::send_rangefinder(mavlink_channel_t chan) diff --git a/ArduPlane/Log.cpp b/ArduPlane/Log.cpp index 6a9094aa68..1469013c8e 100644 --- a/ArduPlane/Log.cpp +++ b/ArduPlane/Log.cpp @@ -169,10 +169,17 @@ void Plane::Log_Write_Attitude(void) DataFlash.Log_Write_PID(LOG_PIDY_MSG, quadplane.attitude_control->get_rate_yaw_pid().get_pid_info()); DataFlash.Log_Write_PID(LOG_PIDA_MSG, quadplane.pid_accel_z.get_pid_info() ); } else { + const DataFlash_Class::PID_Info *landing_info; DataFlash.Log_Write_PID(LOG_PIDR_MSG, rollController.get_pid_info()); DataFlash.Log_Write_PID(LOG_PIDP_MSG, pitchController.get_pid_info()); DataFlash.Log_Write_PID(LOG_PIDY_MSG, yawController.get_pid_info()); DataFlash.Log_Write_PID(LOG_PIDS_MSG, steerController.get_pid_info()); + if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND) { + landing_info = landing.get_pid_info(); + if (landing_info != nullptr) { // only log LANDING PID's while in landing + DataFlash.Log_Write_PID(LOG_PIDL_MSG, *landing_info); + } + } } #if AP_AHRS_NAVEKF_AVAILABLE diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index c58bed8b04..9424026c84 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -73,7 +73,7 @@ const AP_Param::Info Plane::var_info[] = { // @DisplayName: GCS PID tuning mask // @Description: bitmask of PIDs to send MAVLink PID_TUNING messages for // @User: Advanced - // @Bitmask: 0:Roll,1:Pitch,2:Yaw + // @Bitmask: 0:Roll,1:Pitch,2:Yaw,3:Steering,4:Landing GSCALAR(gcs_pid_mask, "GCS_PID_MASK", 0), // @Param: KFF_RDDRMIX diff --git a/ArduPlane/servos.cpp b/ArduPlane/servos.cpp index afc175c170..570587fdd0 100644 --- a/ArduPlane/servos.cpp +++ b/ArduPlane/servos.cpp @@ -408,10 +408,17 @@ void Plane::set_servos_controlled(void) set_servos_old_elevons(); } else { // both types of secondary aileron are slaved to the roll servo out - SRV_Channels::set_output_scaled(SRV_Channel::k_aileron_with_input, SRV_Channels::get_output_scaled(SRV_Channel::k_aileron)); - + SRV_Channels::set_output_scaled(SRV_Channel::k_aileron_with_input, + SRV_Channels::get_output_scaled(SRV_Channel::k_aileron)); + // both types of secondary elevator are slaved to the pitch servo out - SRV_Channels::set_output_scaled(SRV_Channel::k_elevator_with_input, SRV_Channels::get_output_scaled(SRV_Channel::k_elevator)); + SRV_Channels::set_output_scaled(SRV_Channel::k_elevator_with_input, + SRV_Channels::get_output_scaled(SRV_Channel::k_elevator)); + } + + if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND) { + // allow landing to override servos if it would like to + landing.override_servos(); } // convert 0 to 100% (or -100 to +100) into PWM