altitude control using throttle input in wp_run()

ArduCopter/mode_auto.cpp

@@ -1,7 +1,8 @@
 #include "Copter.h"
-
+#include "shared_throttle.h"
 #if MODE_AUTO_ENABLED == ENABLED
 
+
 /*
  * Init and run calls for auto flight mode
  *
@@ -22,6 +23,7 @@
 // auto_init - initialise auto controller
 bool ModeAuto::init(bool ignore_checks)
 {
+    sharedData.throttle_input_total = 0.0; // Reset when entering auto mode
     auto_RTL = false;
     if (mission.num_commands() > 1 || ignore_checks) {
         // reject switching to auto mode if landed with motors armed but first command is not a takeoff (reduce chance of flips)
@@ -968,11 +970,9 @@ void ModeAuto::takeoff_run()
     auto_takeoff.run();
 }
 
-// auto_wp_run - runs the auto waypoint controller
-//      called by auto_run at 100hz or more
 void ModeAuto::wp_run()
 {
-    // if not armed set throttle to zero and exit immediately
+    // safety checks
     if (is_disarmed_or_landed()) {
         make_safe_ground_handling();
         return;
@@ -984,9 +984,14 @@ void ModeAuto::wp_run()
     // run waypoint controller
     copter.failsafe_terrain_set_status(wp_nav->update_wpnav());
 
-    // WP_Nav has set the vertical position control targets
+    // Get pilot's desired climb rate from throttle stick input (like in AltHold)
+    float pilot_throttle_input = channel_throttle->get_control_in();
+    float pilot_desired_climb_rate = get_pilot_desired_climb_rate(pilot_throttle_input);
+    float g_pilot_desired_climb_rate = constrain_float(pilot_desired_climb_rate, -get_pilot_speed_dn(), g.pilot_speed_up);
+
     // run the vertical position controller and set output throttle
-    pos_control->update_z_controller();
+    pos_control->update_z_controller_edited(g_pilot_desired_climb_rate);
+    // pos_control->update_z_controller(g_pilot_desired_climb_rate);
 
     // call attitude controller with auto yaw
     attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.get_heading());

ArduCopter/shared_throttle.cpp

@@ -0,0 +1,3 @@
+#include "shared_throttle.h"
+
+SharedData sharedData; // Define the variable

ArduCopter/shared_throttle.h

@@ -0,0 +1,10 @@
+#ifndef SHARED_DATA_H
+#define SHARED_DATA_H
+
+struct SharedData {
+    float throttle_input_total = 0.0;
+};
+
+extern SharedData sharedData; // Extern keyword for global declaration
+
+#endif // SHARED_DATA_H

README.md

@@ -158,4 +158,4 @@ for reviewing patches on their specific area.
 - [David Bussenschutt ](https://github.com/davidbuzz):
   - ***Subsystem***: ESP32,AP_HAL_ESP32
 - [Charles Villard ](https://github.com/Silvanosky):
-  - ***Subsystem***: ESP32,AP_HAL_ESP32
+  - ***Subsystem***: ESP32,AP_HAL_ESP32

libraries/AC_AttitudeControl/AC_PosControl.cpp

@@ -5,6 +5,7 @@
 #include <AP_Motors/AP_Motors.h>    // motors library
 #include <AP_Vehicle/AP_Vehicle_Type.h>
 #include <AP_Scheduler/AP_Scheduler.h>
+#include "/home/solid/Downloads/Extra_Work/Phan/ardupilot/ardupilot/ArduCopter/shared_throttle.h" // Include the header
 
 extern const AP_HAL::HAL& hal;
 
@@ -926,6 +927,7 @@ bool AC_PosControl::is_active_z() const
 ///     Position and velocity errors are converted to velocity and acceleration targets using PID objects
 ///     Desired velocity and accelerations are added to these corrections as they are calculated
 ///     Kinematically consistent target position and desired velocity and accelerations should be provided before calling this function
+
 void AC_PosControl::update_z_controller()
 {
     // check for ekf z-axis position reset
@@ -1002,6 +1004,83 @@ void AC_PosControl::update_z_controller()
 }
 
 
+void AC_PosControl::update_z_controller_edited(float throttle_input) {
+
+    // check for ekf z-axis position reset
+    handle_ekf_z_reset();
+
+    // Check for z_controller time out
+    if (!is_active_z()) {
+        init_z_controller();
+        if (has_good_timing()) {
+            // call internal error because initialisation has not been done
+            INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
+        }
+    }
+    _last_update_z_ticks = AP::scheduler().ticks32();
+
+
+    sharedData.throttle_input_total += (throttle_input * abs(throttle_input) * 0.0002  * 0.02);  // Adjust scaling factor as needed
+
+    // Update the position target based on cumulative throttle input
+    float final_sum_thing = _pos_target.z + sharedData.throttle_input_total;
+
+    // The new position target should be final_sum_thing
+    float pos_target_zf = final_sum_thing;
+
+
+    // Calculate the target velocity correction based on throttle input
+    _vel_target.z = _p_pos_z.update_all(pos_target_zf, _inav.get_position_z_up_cm());
+    _vel_target.z *= AP::ahrs().getControlScaleZ();
+
+
+    _pos_target.z = pos_target_zf;
+
+    // add feed forward component
+    _vel_target.z += _vel_desired.z;
+
+    // Velocity Controller
+    const float curr_vel_z = _inav.get_velocity_z_up_cms();
+    _accel_target.z = _pid_vel_z.update_all(_vel_target.z, curr_vel_z, _dt, _motors.limit.throttle_lower, _motors.limit.throttle_upper);
+    _accel_target.z *= AP::ahrs().getControlScaleZ();
+
+    // add feed forward component
+    _accel_target.z += _accel_desired.z;
+
+    // Acceleration Controller
+    const float z_accel_meas = get_z_accel_cmss();
+
+    // ensure imax is always large enough to overpower hover throttle
+    if (_motors.get_throttle_hover() * 1000.0f > _pid_accel_z.imax()) {
+        _pid_accel_z.imax(_motors.get_throttle_hover() * 1000.0f);
+    }
+    float thr_out;
+    if (_vibe_comp_enabled) {
+        thr_out = get_throttle_with_vibration_override();
+    } else {
+        thr_out = _pid_accel_z.update_all(_accel_target.z, z_accel_meas, _dt, (_motors.limit.throttle_lower || _motors.limit.throttle_upper)) * 0.001f;
+        thr_out += _pid_accel_z.get_ff() * 0.001f;
+    }
+    thr_out += _motors.get_throttle_hover();
+
+    // Actuator commands
+    _attitude_control.set_throttle_out(thr_out, true, POSCONTROL_THROTTLE_CUTOFF_FREQ_HZ);
+
+    // Check for vertical controller health
+    float error_ratio = _pid_vel_z.get_error() / _vel_max_down_cms;
+    _vel_z_control_ratio += _dt * 0.1f * (0.5 - error_ratio);
+    _vel_z_control_ratio = constrain_float(_vel_z_control_ratio, 0.0f, 2.0f);
+
+    // set vertical component of the limit vector
+    if (_motors.limit.throttle_upper) {
+        _limit_vector.z = 1.0f;
+    } else if (_motors.limit.throttle_lower) {
+        _limit_vector.z = -1.0f;
+    } else {
+        _limit_vector.z = 0.0f;
+    }
+}
+
 ///
 /// Accessors
 ///

libraries/AC_AttitudeControl/AC_PosControl.h

@@ -235,6 +235,7 @@ class AC_PosControl
     ///     Kinematically consistent target position and desired velocity and accelerations should be provided before calling this function
     void update_z_controller();
 
+    void update_z_controller_edited(float throttle_input);
 
 
     ///

libraries/GCS_MAVLink/GCS.h

@@ -324,6 +324,7 @@ class GCS_MAVLINK
     virtual MISSION_STATE mission_state(const class AP_Mission &mission) const;
     // send a mission_current message for the supplied waypoint
     void send_mission_current(const class AP_Mission &mission, uint16_t seq);
+    void send_mission_item_int(const AP_Mission &mission, uint16_t seq);
 
     // common send functions
     void send_heartbeat(void) const;

libraries/GCS_MAVLink/GCS_Common.cpp

@@ -4454,6 +4454,9 @@ MAV_RESULT GCS_MAVLINK::handle_command_run_prearm_checks(const mavlink_command_l
     return MAV_RESULT_ACCEPTED;
 }
 
+//Testing Graheeth
+
+
 // changes the current waypoint; at time of writing GCS
 // implementations use the mavlink message MISSION_SET_CURRENT to set
 // the current waypoint, rather than this DO command.  It is hoped we