Copter: Initialize position controller

bnsgeyer · Oct 28, 2023 · 01d324a · 01d324a
1 parent 600e781
commit 01d324a
Showing 1 changed file with 39 additions and 8 deletions.
diff --git a/ArduCopter/mode_systemid.cpp b/ArduCopter/mode_systemid.cpp
@@ -85,10 +85,44 @@ bool ModeSystemId::init(bool ignore_checks)
         return false;
     }
 
-    if (copter.flightmode->mode_number() != Mode::Number::LOITER) {
+    if ((AxisType)axis.get() != AxisType::DISTURB_POS_LAT && (AxisType)axis.get() != AxisType::DISTURB_POS_LONG 
+         && (AxisType)axis.get() != AxisType::DISTURB_VEL_LAT && (AxisType)axis.get() != AxisType::DISTURB_VEL_LONG
+         && (AxisType)axis.get() != AxisType::INPUT_LOITER_LAT && (AxisType)axis.get() != AxisType::INPUT_LOITER_LONG) {
+
 #if FRAME_CONFIG == HELI_FRAME
-    copter.input_manager.set_use_stab_col(true);
+        copter.input_manager.set_use_stab_col(true);
 #endif
+
+    } else {
+
+        if (!copter.failsafe.radio) {
+            float target_roll, target_pitch;
+            // apply SIMPLE mode transform to pilot inputs
+            update_simple_mode();
+
+            // convert pilot input to lean angles
+            get_pilot_desired_lean_angles(target_roll, target_pitch, loiter_nav->get_angle_max_cd(), attitude_control->get_althold_lean_angle_max_cd());
+
+            // process pilot's roll and pitch input
+            loiter_nav->set_pilot_desired_acceleration(target_roll, target_pitch);
+        } else {
+            // clear out pilot desired acceleration in case radio failsafe event occurs and we do not switch to RTL for some reason
+            loiter_nav->clear_pilot_desired_acceleration();
+        }
+        loiter_nav->init_target();
+
+        // initialise the vertical position controller
+        if (!pos_control->is_active_z()) {
+            pos_control->init_z_controller();
+        }
+
+        // set vertical speed and acceleration limits
+        pos_control->set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);
+        pos_control->set_correction_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);
+
+// #if AC_PRECLAND_ENABLED
+//        _precision_loiter_active = false;
+// #endif
     }
 
     att_bf_feedforward = attitude_control->get_bf_feedforward();
@@ -200,8 +234,6 @@ void ModeSystemId::run()
             // apply SIMPLE mode transform to pilot inputs
             update_simple_mode();
 
-            // convert pilot input to lean angles
-//            get_pilot_desired_lean_angles(target_roll, target_pitch, pos_control->get_lean_angle_max_cd(), attitude_control->get_althold_lean_angle_max_cd());
             pilot_vel = get_pilot_desired_velocity(250.0f);
 
         }
@@ -345,9 +377,8 @@ void ModeSystemId::run()
         // this was split into two parts to allow pilot input to accomplish closed loop sweeps
         if (!copter.failsafe.radio) {
 
-            // set target accels in position controller
-//            Vector3f target_attitude_rad = Vector3f(radians(target_roll*0.01), radians(target_pitch*0.01), attitude_control->get_att_target_euler_rad().z);
-//            target_accel = pos_control->lean_angles_to_accel(target_attitude_rad);
+            // Set pilot's roll and pitch input to zero
+            loiter_nav->set_pilot_desired_acceleration(0.0f, 0.0f);
 
             // get pilot's desired yaw rate
             target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->norm_input_dz());
@@ -415,7 +446,7 @@ void ModeSystemId::run()
 
             input_vel += pilot_vel;
             Vector2f accel = Vector2f((input_vel.x - target_vel.x) / G_Dt, (input_vel.y - target_vel.y) / G_Dt);
-//            Vector2f accel;
+
             target_vel = input_vel;
             pos_control->input_vel_accel_xy(target_vel, accel);
             pos_control->update_xy_controller();