Plane: Add support for landing PID's and servo control

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)

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

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

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