Copter: refactor landing to reduce duplication, use vertical vel ff, slow down precision landing descent based on position error

.gitmodules

@@ -1,6 +1,6 @@
 [submodule "modules/PX4Firmware"]
 	path = modules/PX4Firmware
-	url = git://github.com/ArduPilot/PX4Firmware.git
+	url = git://github.com/jschall/PX4Firmware.git
 [submodule "modules/PX4NuttX"]
 	path = modules/PX4NuttX
 	url = git://github.com/ArduPilot/PX4NuttX.git

ArduCopter/ArduCopter.cpp

@@ -102,7 +102,7 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = {
     SCHED_TASK(compass_accumulate,   100,    100),
     SCHED_TASK(barometer_accumulate,  50,     90),
 #if PRECISION_LANDING == ENABLED
-    SCHED_TASK(update_precland,       50,     50),
+    SCHED_TASK(update_precland,      400,     50),
 #endif
 #if FRAME_CONFIG == HELI_FRAME
     SCHED_TASK(check_dynamic_flight,  50,     75),
@@ -428,6 +428,11 @@ void Copter::twentyfive_hz_logging()
         DataFlash.Log_Write_IMU(ins);
     }
 #endif
+
+#if PRECISION_LANDING == ENABLED
+    // log output
+    Log_Write_Precland();
+#endif
 }
 
 void Copter::dataflash_periodic(void)

ArduCopter/Copter.h

@@ -813,7 +813,8 @@ class Copter : public AP_HAL::HAL::Callbacks {
     void land_run();
     void land_gps_run();
     void land_nogps_run();
-    float get_land_descent_speed();
+    void land_run_vertical_control(bool pause_descent = false);
+    void land_run_horizontal_control();
     void land_do_not_use_GPS();
     void set_mode_land_with_pause(mode_reason_t reason);
     bool landing_with_GPS();

ArduCopter/control_auto.cpp

@@ -360,8 +360,9 @@ void Copter::auto_land_start(const Vector3f& destination)
     // initialise loiter target destination
     wp_nav.init_loiter_target(destination);
 
-    // initialise altitude target to stopping point
-    pos_control.set_target_to_stopping_point_z();
+    // initialise position and desired velocity
+    pos_control.set_alt_target(inertial_nav.get_altitude());
+    pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z());
 
     // initialise yaw
     set_auto_yaw_mode(AUTO_YAW_HOLD);
@@ -371,11 +372,8 @@ void Copter::auto_land_start(const Vector3f& destination)
 //      called by auto_run at 100hz or more
 void Copter::auto_land_run()
 {
-    int16_t roll_control = 0, pitch_control = 0;
-    float target_yaw_rate = 0;
-
-    // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
-    if (!motors.armed() || !ap.auto_armed || ap.land_complete) {
+    // if not auto armed or landed or motor interlock not enabled set throttle to zero and exit immediately
+    if (!motors.armed() || !ap.auto_armed || ap.land_complete || !motors.get_interlock()) {
 #if FRAME_CONFIG == HELI_FRAME  // Helicopters always stabilize roll/pitch/yaw
         // call attitude controller
         attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
@@ -390,64 +388,11 @@ void Copter::auto_land_run()
         return;
     }
 
-    // relax loiter targets if we might be landed
-    if (ap.land_complete_maybe) {
-        wp_nav.loiter_soften_for_landing();
-    }
-
-    // process pilot's input
-    if (!failsafe.radio) {
-        if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && rc_throttle_control_in_filter.get() > LAND_CANCEL_TRIGGER_THR){
-            Log_Write_Event(DATA_LAND_CANCELLED_BY_PILOT);
-            // exit land if throttle is high
-            if (!set_mode(LOITER, MODE_REASON_THROTTLE_LAND_ESCAPE)) {
-                set_mode(ALT_HOLD, MODE_REASON_THROTTLE_LAND_ESCAPE);
-            }
-        }
-
-        if (g.land_repositioning) {
-            // apply SIMPLE mode transform to pilot inputs
-            update_simple_mode();
-
-            // process pilot's roll and pitch input
-            roll_control = channel_roll->get_control_in();
-            pitch_control = channel_pitch->get_control_in();
-        }
-
-        // get pilot's desired yaw rate
-        target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
-    }
-
     // set motors to full range
     motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
-
-    // process roll, pitch inputs
-    wp_nav.set_pilot_desired_acceleration(roll_control, pitch_control);
-
-#if PRECISION_LANDING == ENABLED
-    // run precision landing
-    if (!ap.land_repo_active && precland.target_acquired() && precland_last_update_ms != precland.last_update_ms()) {
-        Vector3f target_pos;
-        precland.get_target_position(target_pos);
-        pos_control.set_xy_target(target_pos.x, target_pos.y);
-        pos_control.freeze_ff_xy();
-        precland_last_update_ms = precland.last_update_ms();
-    }
-#endif
-
-    // run loiter controller
-    wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
-
-    // call z-axis position controller
-    float cmb_rate = get_land_descent_speed();
-    pos_control.set_alt_target_from_climb_rate(cmb_rate, G_Dt, true);
-    pos_control.update_z_controller();
-
-    // record desired climb rate for logging
-    desired_climb_rate = cmb_rate;
-
-    // roll & pitch from waypoint controller, yaw rate from pilot
-    attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain());
+
+    land_run_horizontal_control();
+    land_run_vertical_control();
 }
 
 // auto_rtl_start - initialises RTL in AUTO flight mode

ArduCopter/control_land.cpp

@@ -23,9 +23,10 @@ bool Copter::land_init(bool ignore_checks)
     pos_control.set_speed_z(wp_nav.get_speed_down(), wp_nav.get_speed_up());
     pos_control.set_accel_z(wp_nav.get_accel_z());
 
-    // initialise altitude target to stopping point
-    pos_control.set_target_to_stopping_point_z();
-
+    // initialise position and desired velocity
+    pos_control.set_alt_target(inertial_nav.get_altitude());
+    pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z());
+
     land_start_time = millis();
 
     land_pause = false;
@@ -52,9 +53,6 @@ void Copter::land_run()
 //      should be called at 100hz or more
 void Copter::land_gps_run()
 {
-    int16_t roll_control = 0, pitch_control = 0;
-    float target_yaw_rate = 0;
-
     // if not auto armed or landed or motor interlock not enabled set throttle to zero and exit immediately
     if (!motors.armed() || !ap.auto_armed || ap.land_complete || !motors.get_interlock()) {
 #if FRAME_CONFIG == HELI_FRAME  // Helicopters always stabilize roll/pitch/yaw
@@ -81,78 +79,17 @@ void Copter::land_gps_run()
 #endif
         return;
     }
-
-    // relax loiter target if we might be landed
-    if (ap.land_complete_maybe) {
-        wp_nav.loiter_soften_for_landing();
-    }
-
-    // process pilot inputs
-    if (!failsafe.radio) {
-        if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && rc_throttle_control_in_filter.get() > LAND_CANCEL_TRIGGER_THR){
-            Log_Write_Event(DATA_LAND_CANCELLED_BY_PILOT);
-            // exit land if throttle is high
-            if (!set_mode(LOITER, MODE_REASON_THROTTLE_LAND_ESCAPE)) {
-                set_mode(ALT_HOLD, MODE_REASON_THROTTLE_LAND_ESCAPE);
-            }
-        }
-
-        if (g.land_repositioning) {
-            // apply SIMPLE mode transform to pilot inputs
-            update_simple_mode();
-
-            // process pilot's roll and pitch input
-            roll_control = channel_roll->get_control_in();
-            pitch_control = channel_pitch->get_control_in();
-
-            // record if pilot has overriden roll or pitch
-            if (roll_control != 0 || pitch_control != 0) {
-                ap.land_repo_active = true;
-            }
-        }
-
-        // get pilot's desired yaw rate
-        target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
-    }
-
+
     // set motors to full range
     motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
-
-    // process roll, pitch inputs
-    wp_nav.set_pilot_desired_acceleration(roll_control, pitch_control);
-
-#if PRECISION_LANDING == ENABLED
-    // run precision landing
-    if (!ap.land_repo_active && precland.target_acquired() && precland_last_update_ms != precland.last_update_ms()) {
-        Vector3f target_pos;
-        precland.get_target_position(target_pos);
-        pos_control.set_xy_target(target_pos.x, target_pos.y);
-        pos_control.freeze_ff_xy();
-        precland_last_update_ms = precland.last_update_ms();
-    }
-#endif
-
-    // run loiter controller
-    wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
-
-    // call attitude controller
-    attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain());
-
-    // pause 4 seconds before beginning land descent
-    float cmb_rate;
-    if(land_pause && millis()-land_start_time < 4000) {
-        cmb_rate = 0;
-    } else {
+
+    // pause before beginning land descent
+    if(land_pause && millis()-land_start_time >= LAND_WITH_DELAY_MS) {
         land_pause = false;
-        cmb_rate = get_land_descent_speed();
     }
-
-    // record desired climb rate for logging
-    desired_climb_rate = cmb_rate;
-
-    // update altitude target and call position controller
-    pos_control.set_alt_target_from_climb_rate(cmb_rate, G_Dt, true);
-    pos_control.update_z_controller();
+
+    land_run_horizontal_control();
+    land_run_vertical_control(land_pause);
 }
 
 // land_nogps_run - runs the land controller
@@ -215,56 +152,115 @@ void Copter::land_nogps_run()
     // call attitude controller
     attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
 
-    // pause 4 seconds before beginning land descent
-    float cmb_rate;
-    if(land_pause && millis()-land_start_time < LAND_WITH_DELAY_MS) {
-        cmb_rate = 0;
-    } else {
+    // pause before beginning land descent
+    if(land_pause && millis()-land_start_time >= LAND_WITH_DELAY_MS) {
         land_pause = false;
-        cmb_rate = get_land_descent_speed();
+    }
+
+    land_run_vertical_control(land_pause);
+}
+
+void Copter::land_run_vertical_control(bool pause_descent)
+{
+    bool navigating = pos_control.is_active_xy();
+
+#if PRECISION_LANDING == ENABLED
+    bool doing_precision_landing = !ap.land_repo_active && precland.target_acquired() && navigating;
+#else
+    bool doing_precision_landing = false;
+#endif
+
+    // compute desired velocity
+    const float precland_acceptable_error = 25.0f;
+    const float precland_min_descent_speed = 10.0f;
+    int32_t alt_above_ground;
+    if (rangefinder_alt_ok()) {
+        alt_above_ground = rangefinder_state.alt_cm_filt.get();
+    } else {
+        if (!navigating || !current_loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, alt_above_ground)) {
+            current_loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_HOME, alt_above_ground);
+        }
+    }
+
+    float cmb_rate = 0;
+    if (!pause_descent) {
+        cmb_rate = AC_AttitudeControl::sqrt_controller(LAND_START_ALT-alt_above_ground, g.p_alt_hold.kP(), pos_control.get_accel_z());
+        cmb_rate = constrain_float(cmb_rate, pos_control.get_speed_down(), -abs(g.land_speed));
+
+        if (doing_precision_landing && alt_above_ground < 300.0f) {
+            float land_slowdown = MAX(0.0f, pos_control.get_horizontal_error()*(abs(g.land_speed)/precland_acceptable_error));
+            cmb_rate = MIN(-precland_min_descent_speed, cmb_rate+land_slowdown);
+        }
     }
 
     // record desired climb rate for logging
     desired_climb_rate = cmb_rate;
 
-    // call position controller
-    pos_control.set_alt_target_from_climb_rate(cmb_rate, G_Dt, true);
+    // update altitude target and call position controller
+    pos_control.set_alt_target_from_climb_rate_ff(cmb_rate, G_Dt, true);
     pos_control.update_z_controller();
 }
 
-// get_land_descent_speed - high level landing logic
-//      returns climb rate (in cm/s) which should be passed to the position controller
-//      should be called at 100hz or higher
-float Copter::get_land_descent_speed()
+void Copter::land_run_horizontal_control()
 {
-#if RANGEFINDER_ENABLED == ENABLED
-    bool rangefinder_ok = rangefinder_state.enabled && rangefinder_state.alt_healthy;
-#else
-    bool rangefinder_ok = false;
-#endif
+    int16_t roll_control = 0, pitch_control = 0;
+    float target_yaw_rate = 0;
+
+    // relax loiter target if we might be landed
+    if (ap.land_complete_maybe) {
+        wp_nav.loiter_soften_for_landing();
+    }
+
+    // process pilot inputs
+    if (!failsafe.radio) {
+        if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && rc_throttle_control_in_filter.get() > LAND_CANCEL_TRIGGER_THR){
+            Log_Write_Event(DATA_LAND_CANCELLED_BY_PILOT);
+            // exit land if throttle is high
+            if (!set_mode(LOITER, MODE_REASON_THROTTLE_LAND_ESCAPE)) {
+                set_mode(ALT_HOLD, MODE_REASON_THROTTLE_LAND_ESCAPE);
+            }
+        }
 
-    // get position controller's target altitude above terrain
-    Location_Class target_loc = pos_control.get_pos_target();
-    int32_t target_alt_cm;
+        if (g.land_repositioning) {
+            // apply SIMPLE mode transform to pilot inputs
+            update_simple_mode();
 
-    // get altitude target above home by default
-    target_loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_HOME, target_alt_cm);
+            // process pilot's roll and pitch input
+            roll_control = channel_roll->get_control_in();
+            pitch_control = channel_pitch->get_control_in();
 
-    // try to use terrain if enabled
-    if (terrain_use() && !target_loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, target_alt_cm)) {
-        Log_Write_Error(ERROR_SUBSYSTEM_TERRAIN, ERROR_CODE_MISSING_TERRAIN_DATA);
+            // record if pilot has overriden roll or pitch
+            if (roll_control != 0 || pitch_control != 0) {
+                ap.land_repo_active = true;
+            }
+        }
+
+        // get pilot's desired yaw rate
+        target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
     }
 
-    // if we are above 10m and the rangefinder does not sense anything perform regular alt hold descent
-    if ((target_alt_cm >= LAND_START_ALT) && !rangefinder_ok) {
-        if (g.land_speed_high > 0) {
-            // user has asked for a different landing speed than normal descent rate
-            return -abs(g.land_speed_high);
-        }
-        return pos_control.get_speed_down();
-    }else{
-        return -abs(g.land_speed);
+#if PRECISION_LANDING == ENABLED
+    bool doing_precision_landing = !ap.land_repo_active && precland.target_acquired();
+    // run precision landing
+    if (doing_precision_landing && precland_last_update_ms != precland.last_update_ms()) {
+        Vector3f target_pos;
+        precland.get_target_position(target_pos);
+        pos_control.set_xy_target(target_pos.x, target_pos.y);
+        pos_control.freeze_ff_xy();
+        precland_last_update_ms = precland.last_update_ms();
     }
+#else
+    bool doing_precision_landing = false;
+#endif
+
+    // process roll, pitch inputs
+    wp_nav.set_pilot_desired_acceleration(roll_control, pitch_control);
+
+    // run loiter controller
+    wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
+
+    // call attitude controller
+    attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain());
 }
 
 // land_do_not_use_GPS - forces land-mode to not use the GPS but instead rely on pilot input for roll and pitch