Copter: use position controller for landing reposition

ArduCopter/mode.cpp

@@ -431,6 +431,39 @@ void Mode::get_pilot_desired_lean_angles(float &roll_out, float &pitch_out, floa
     // roll_out and pitch_out are returned
 }
 
+// transform pilot's roll or pitch input into a desired velocity
+Vector2f Mode::get_pilot_desired_velocity(float vel_max) const
+{
+    Vector2f vel;
+
+    // throttle failsafe check
+    if (copter.failsafe.radio || !copter.ap.rc_receiver_present) {
+        return vel;
+    }
+    // fetch roll and pitch inputs
+    float roll_out = channel_roll->get_control_in();
+    float pitch_out = channel_pitch->get_control_in();
+
+    // convert roll and pitch inputs to -1 to +1 range
+    float scaler = 1.0 / (float)ROLL_PITCH_YAW_INPUT_MAX;
+    roll_out *= scaler;
+    pitch_out *= scaler;
+
+    // convert roll and pitch inputs into velocity in NE frame
+    vel = Vector2f(-pitch_out, roll_out);
+    if (vel.is_zero()) {
+        return vel;
+    }
+    copter.rotate_body_frame_to_NE(vel.x, vel.y);
+
+    // Transform square input range to circular output
+    // vel_scaler is the vector to the edge of the +- 1.0 square in the direction of the current input
+    Vector2f vel_scaler = vel / MAX(fabsf(vel.x), fabsf(vel.y));
+    // We scale the output by the ratio of the distance to the square to the unit circle and multiply by vel_max
+    vel *= vel_max / vel_scaler.length();
+    return vel;
+}
+
 bool Mode::_TakeOff::triggered(const float target_climb_rate) const
 {
     if (!copter.ap.land_complete) {
@@ -598,13 +631,12 @@ void Mode::land_run_vertical_control(bool pause_descent)
 
 void Mode::land_run_horizontal_control()
 {
-    float target_roll = 0.0f;
+    Vector2f vel_correction;
-    float target_pitch = 0.0f;
     float target_yaw_rate = 0;
 
     // relax loiter target if we might be landed
     if (copter.ap.land_complete_maybe) {
-        loiter_nav->soften_for_landing();
+        pos_control->soften_for_landing_xy();
     }
 
     // process pilot inputs
@@ -621,11 +653,14 @@ void Mode::land_run_horizontal_control()
             // apply SIMPLE mode transform to pilot inputs
             update_simple_mode();
 
-            // convert pilot input to lean angles
+            // convert pilot input to reposition velocity
-            get_pilot_desired_lean_angles(target_roll, target_pitch, loiter_nav->get_angle_max_cd(), attitude_control->get_althold_lean_angle_max_cd());
+            // use half maximum acceleration as the maximum velocity to ensure aircraft will
+            // stop from full reposition speed in less than 1 second.
+            const float max_pilot_vel = wp_nav->get_wp_acceleration() * 0.5;
+            vel_correction = get_pilot_desired_velocity(max_pilot_vel);
 
             // record if pilot has overridden roll or pitch
-            if (!is_zero(target_roll) || !is_zero(target_pitch)) {
+            if (!vel_correction.is_zero()) {
                 if (!copter.ap.land_repo_active) {
                     AP::logger().Write_Event(LogEvent::LAND_REPO_ACTIVE);
                 }
@@ -641,11 +676,11 @@ void Mode::land_run_horizontal_control()
     }
 
     // this variable will be updated if prec land target is in sight and pilot isn't trying to reposition the vehicle
-    bool doing_precision_landing = false;
+    copter.ap.prec_land_active = false;
 #if PRECISION_LANDING == ENABLED
-    doing_precision_landing = !copter.ap.land_repo_active && copter.precland.target_acquired();
+    copter.ap.prec_land_active = !copter.ap.land_repo_active && copter.precland.target_acquired();
     // run precision landing
-    if (doing_precision_landing) {
+    if (copter.ap.prec_land_active) {
         Vector2f target_pos, target_vel;
         if (!copter.precland.get_target_position_cm(target_pos)) {
             target_pos = inertial_nav.get_position_xy_cm();
@@ -657,12 +692,10 @@ void Mode::land_run_horizontal_control()
         Vector2p landing_pos = target_pos.topostype();
         // target vel will remain zero if landing target is stationary
         pos_control->input_pos_vel_accel_xy(landing_pos, target_vel, zero);
-        // run pos controller
-        pos_control->update_xy_controller();
     }
 #endif
 
-    if (!doing_precision_landing) {
+    if (!copter.ap.prec_land_active) {
         if (copter.ap.prec_land_active) {
             // precland isn't active anymore but was active a while back
             // lets run an init again
@@ -671,15 +704,12 @@ void Mode::land_run_horizontal_control()
             loiter_nav->get_stopping_point_xy(stopping_point);
             loiter_nav->init_target(stopping_point);
         }
-        // process roll, pitch inputs
+        Vector2f accel;
-        loiter_nav->set_pilot_desired_acceleration(target_roll, target_pitch);
+        pos_control->input_vel_accel_xy(vel_correction, accel);
-
-        // run loiter controller
-        loiter_nav->update();
     }
 
-    copter.ap.prec_land_active = doing_precision_landing;
+    // run pos controller
-
+    pos_control->update_xy_controller();
     Vector3f thrust_vector = pos_control->get_thrust_vector();
 
     if (g2.wp_navalt_min > 0) {

ArduCopter/mode.h

@@ -90,6 +90,7 @@ public:
 
     // pilot input processing
     void get_pilot_desired_lean_angles(float &roll_out, float &pitch_out, float angle_max, float angle_limit) const;
+    Vector2f get_pilot_desired_velocity(float vel_max) const;
     float get_pilot_desired_yaw_rate(float yaw_in);
     float get_pilot_desired_throttle() const;
 

ArduCopter/mode_brake.cpp

@@ -47,7 +47,7 @@ void ModeBrake::run()
 
     // relax stop target if we might be landed
     if (copter.ap.land_complete_maybe) {
-        loiter_nav->soften_for_landing();
+        pos_control->soften_for_landing_xy();
     }
 
     // use position controller to stop

ArduCopter/mode_rtl.cpp

@@ -297,8 +297,7 @@ void ModeRTL::descent_start()
 //      called by rtl_run at 100hz or more
 void ModeRTL::descent_run()
 {
-    float target_roll = 0.0f;
+    Vector2f vel_correction;
-    float target_pitch = 0.0f;
     float target_yaw_rate = 0.0f;
 
     // if not armed set throttle to zero and exit immediately
@@ -321,11 +320,11 @@ void ModeRTL::descent_run()
             // apply SIMPLE mode transform to pilot inputs
             update_simple_mode();
 
-            // convert pilot input to lean angles
+            // convert pilot input to reposition velocity
-            get_pilot_desired_lean_angles(target_roll, target_pitch, loiter_nav->get_angle_max_cd(), attitude_control->get_althold_lean_angle_max_cd());
+            vel_correction = get_pilot_desired_velocity(wp_nav->get_wp_acceleration() * 0.5);
 
             // record if pilot has overridden roll or pitch
-            if (!is_zero(target_roll) || !is_zero(target_pitch)) {
+            if (!vel_correction.is_zero()) {
                 if (!copter.ap.land_repo_active) {
                     AP::logger().Write_Event(LogEvent::LAND_REPO_ACTIVE);
                 }
@@ -342,11 +341,9 @@ void ModeRTL::descent_run()
     // set motors to full range
     motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
 
-    // process roll, pitch inputs
+    Vector2f accel;
-    loiter_nav->set_pilot_desired_acceleration(target_roll, target_pitch);
+    pos_control->input_vel_accel_xy(vel_correction, accel);
-
+    pos_control->update_xy_controller();
-    // run loiter controller
-    loiter_nav->update();
 
     // WP_Nav has set the vertical position control targets
     // run the vertical position controller and set output throttle
@@ -354,7 +351,7 @@ void ModeRTL::descent_run()
     pos_control->update_z_controller();
 
     // roll & pitch from waypoint controller, yaw rate from pilot
-    attitude_control->input_thrust_vector_rate_heading(loiter_nav->get_thrust_vector(), target_yaw_rate);
+    attitude_control->input_thrust_vector_rate_heading(pos_control->get_thrust_vector(), target_yaw_rate);
 
     // check if we've reached within 20cm of final altitude
     _state_complete = labs(rtl_path.descent_target.alt - copter.current_loc.alt) < 20;