From 6cf1d6df4ebf0a9a5aeaa59be5d284f356fcdd1b Mon Sep 17 00:00:00 2001 From: graheeth Date: Tue, 24 Oct 2023 10:18:21 +0800 Subject: [PATCH 1/7] altitude control using throttle input in wp_run() --- ArduCopter/mode_auto.cpp | 10 ++- .../AC_AttitudeControl/AC_PosControl.cpp | 71 +++++++++++++++++++ libraries/AC_AttitudeControl/AC_PosControl.h | 1 + 3 files changed, 79 insertions(+), 3 deletions(-) diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index f60a512e81355..012c111f8d00e 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -972,7 +972,7 @@ void ModeAuto::takeoff_run() // 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,13 @@ 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); // call attitude controller with auto yaw attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.get_heading()); diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index 06eed5ddba461..1261d2a54002b 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp @@ -1001,6 +1001,77 @@ 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(); + + + // Use throttle input to adjust desired altitude (2500 is max throttle input which translates to 2.5m) + float throttle_adjustment = _pos_target.z * throttle_input * abs(throttle_input) / 125000; + float pos_target_zf = _pos_target.z + throttle_adjustment; + + // 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 diff --git a/libraries/AC_AttitudeControl/AC_PosControl.h b/libraries/AC_AttitudeControl/AC_PosControl.h index 8863125ac4c7b..6f706ec9216dc 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.h +++ b/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); /// From a95ade15aade1ca468dda24488f02089a07fca54 Mon Sep 17 00:00:00 2001 From: graheeth Date: Tue, 24 Oct 2023 10:38:52 +0800 Subject: [PATCH 2/7] read.md update --- README.md | 170 +++++------------------------------------------------- 1 file changed, 16 insertions(+), 154 deletions(-) diff --git a/README.md b/README.md index 7b603fc163242..7985501bf62cb 100644 --- a/README.md +++ b/README.md @@ -1,161 +1,23 @@ -# ArduPilot Project +# Stick Mixing in Auto Mode -Discord +Hello everyone! 👋 -[![Test Copter](https://github.com/ArduPilot/ardupilot/workflows/test%20copter/badge.svg?branch=master)](https://github.com/ArduPilot/ardupilot/actions/workflows/test_sitl_copter.yml) [![Test Plane](https://github.com/ArduPilot/ardupilot/workflows/test%20plane/badge.svg?branch=master)](https://github.com/ArduPilot/ardupilot/actions/workflows/test_sitl_plane.yml) [![Test Rover](https://github.com/ArduPilot/ardupilot/workflows/test%20rover/badge.svg?branch=master)](https://github.com/ArduPilot/ardupilot/actions/workflows/test_sitl_rover.yml) [![Test Sub](https://github.com/ArduPilot/ardupilot/workflows/test%20sub/badge.svg?branch=master)](https://github.com/ArduPilot/ardupilot/actions/workflows/test_sitl_sub.yml) [![Test Tracker](https://github.com/ArduPilot/ardupilot/workflows/test%20tracker/badge.svg?branch=master)](https://github.com/ArduPilot/ardupilot/actions/workflows/test_sitl_tracker.yml) +## Update Overview +Exciting times! I've rolled out a new feature: **Stick Mixing in Auto Mode**. This lets you control the altitude of your waypoints directly through the throttle input. Imagine the flexibility and dynamic control you'll now have during your flights! -[![Test AP_Periph](https://github.com/ArduPilot/ardupilot/workflows/test%20ap_periph/badge.svg?branch=master)](https://github.com/ArduPilot/ardupilot/actions/workflows/test_sitl_periph.yml) [![Test Chibios](https://github.com/ArduPilot/ardupilot/workflows/test%20chibios/badge.svg?branch=master)](https://github.com/ArduPilot/ardupilot/actions/workflows/test_chibios.yml) [![Test Linux SBC](https://github.com/ArduPilot/ardupilot/workflows/test%20Linux%20SBC/badge.svg?branch=master)](https://github.com/ArduPilot/ardupilot/actions/workflows/test_linux_sbc.yml) [![Test Replay](https://github.com/ArduPilot/ardupilot/workflows/test%20replay/badge.svg?branch=master)](https://github.com/ArduPilot/ardupilot/actions/workflows/test_replay.yml) +## How it works +1. Engage the auto mode. +2. While your drone navigates its waypoint path, manipulate the throttle input. +3. The upcoming waypoints will adjust their altitudes based on your throttle input, offering a real-time and intuitive altitude mixing experience. -[![Test Unit Tests](https://github.com/ArduPilot/ardupilot/workflows/test%20unit%20tests/badge.svg?branch=master)](https://github.com/ArduPilot/ardupilot/actions/workflows/test_unit_tests.yml) [![test size](https://github.com/ArduPilot/ardupilot/actions/workflows/test_size.yml/badge.svg)](https://github.com/ArduPilot/ardupilot/actions/workflows/test_size.yml) +## Testing & Verification +Before introducing this, I made sure it's rock solid: +- 🖥️ Tested extensively in the SITL (Software In The Loop) simulation. +- 🚁 Conducted hands-on tests with a real drone. -[![Test Environment Setup](https://github.com/ArduPilot/ardupilot/actions/workflows/test_environment.yml/badge.svg?branch=master)](https://github.com/ArduPilot/ardupilot/actions/workflows/test_environment.yml) +Both tests confirmed that the feature works seamlessly and as expected. -[![Cygwin Build](https://github.com/ArduPilot/ardupilot/actions/workflows/cygwin_build.yml/badge.svg)](https://github.com/ArduPilot/ardupilot/actions/workflows/cygwin_build.yml) [![Macos Build](https://github.com/ArduPilot/ardupilot/actions/workflows/macos_build.yml/badge.svg)](https://github.com/ArduPilot/ardupilot/actions/workflows/macos_build.yml) +## Feedback +Your thoughts and feedback drive us forward! If you spot any quirks or have ideas for improvement, please raise an issue right here in the repo. -[![Coverity Scan Build Status](https://scan.coverity.com/projects/5331/badge.svg)](https://scan.coverity.com/projects/ardupilot-ardupilot) - -[![Test Coverage](https://github.com/ArduPilot/ardupilot/actions/workflows/test_coverage.yml/badge.svg?branch=master)](https://github.com/ArduPilot/ardupilot/actions/workflows/test_coverage.yml) - -[![Autotest Status](https://autotest.ardupilot.org/autotest-badge.svg)](https://autotest.ardupilot.org/) - -ArduPilot is the most advanced, full-featured, and reliable open source autopilot software available. -It has been under development since 2010 by a diverse team of professional engineers, computer scientists, and community contributors. -Our autopilot software is capable of controlling almost any vehicle system imaginable, from conventional airplanes, quad planes, multi-rotors, and helicopters to rovers, boats, balance bots, and even submarines. -It is continually being expanded to provide support for new emerging vehicle types. - -## The ArduPilot project is made up of: ## - -- ArduCopter: [code](https://github.com/ArduPilot/ardupilot/tree/master/ArduCopter), [wiki](https://ardupilot.org/copter/index.html) - -- ArduPlane: [code](https://github.com/ArduPilot/ardupilot/tree/master/ArduPlane), [wiki](https://ardupilot.org/plane/index.html) - -- Rover: [code](https://github.com/ArduPilot/ardupilot/tree/master/Rover), [wiki](https://ardupilot.org/rover/index.html) - -- ArduSub : [code](https://github.com/ArduPilot/ardupilot/tree/master/ArduSub), [wiki](http://ardusub.com/) - -- Antenna Tracker : [code](https://github.com/ArduPilot/ardupilot/tree/master/AntennaTracker), [wiki](https://ardupilot.org/antennatracker/index.html) - -## User Support & Discussion Forums ## - -- Support Forum: - -- Community Site: - -## Developer Information ## - -- Github repository: - -- Main developer wiki: - -- Developer discussion: - -- Developer chat: - -## Top Contributors ## - -- [Flight code contributors](https://github.com/ArduPilot/ardupilot/graphs/contributors) -- [Wiki contributors](https://github.com/ArduPilot/ardupilot_wiki/graphs/contributors) -- [Most active support forum users](https://discuss.ardupilot.org/u?order=post_count&period=quarterly) -- [Partners who contribute financially](https://ardupilot.org/about/Partners) - -## How To Get Involved ## - -- The ArduPilot project is open source and we encourage participation and code contributions: [guidelines for contributors to the ardupilot codebase](https://ardupilot.org/dev/docs/contributing.html) - -- We have an active group of Beta Testers to help us improve our code: [release procedures](https://ardupilot.org/dev/docs/release-procedures.html) - -- Desired Enhancements and Bugs can be posted to the [issues list](https://github.com/ArduPilot/ardupilot/issues). - -- Help other users with log analysis in the [support forums](https://discuss.ardupilot.org/) - -- Improve the wiki and chat with other [wiki editors on Discord #documentation](https://discord.com/channels/ardupilot) - -- Contact the developers on one of the [communication channels](https://ardupilot.org/copter/docs/common-contact-us.html) - -## License ## - -The ArduPilot project is licensed under the GNU General Public -License, version 3. - -- [Overview of license](https://ardupilot.org/dev/docs/license-gplv3.html) - -- [Full Text](https://github.com/ArduPilot/ardupilot/blob/master/COPYING.txt) - -## Maintainers ## - -ArduPilot is comprised of several parts, vehicles and boards. The list below -contains the people that regularly contribute to the project and are responsible -for reviewing patches on their specific area. - -- [Andrew Tridgell](https://github.com/tridge): - - ***Vehicle***: Plane, AntennaTracker - - ***Board***: Pixhawk, Pixhawk2, PixRacer -- [Francisco Ferreira](https://github.com/oxinarf): - - ***Bug Master*** -- [Grant Morphett](https://github.com/gmorph): - - ***Vehicle***: Rover -- [Willian Galvani](https://github.com/williangalvani): - - ***Vehicle***: Sub -- [Lucas De Marchi](https://github.com/lucasdemarchi): - - ***Subsystem***: Linux -- [Michael du Breuil](https://github.com/WickedShell): - - ***Subsystem***: Batteries - - ***Subsystem***: GPS - - ***Subsystem***: Scripting -- [Peter Barker](https://github.com/peterbarker): - - ***Subsystem***: DataFlash, Tools -- [Randy Mackay](https://github.com/rmackay9): - - ***Vehicle***: Copter, Rover, AntennaTracker -- [Siddharth Purohit](https://github.com/bugobliterator): - - ***Subsystem***: CAN, Compass - - ***Board***: Cube* -- [Tom Pittenger](https://github.com/magicrub): - - ***Vehicle***: Plane -- [Bill Geyer](https://github.com/bnsgeyer): - - ***Vehicle***: TradHeli -- [Emile Castelnuovo](https://github.com/emilecastelnuovo): - - ***Board***: VRBrain -- [Georgii Staroselskii](https://github.com/staroselskii): - - ***Board***: NavIO -- [Gustavo José de Sousa](https://github.com/guludo): - - ***Subsystem***: Build system -- [Julien Beraud](https://github.com/jberaud): - - ***Board***: Bebop & Bebop 2 -- [Leonard Hall](https://github.com/lthall): - - ***Subsystem***: Copter attitude control and navigation -- [Matt Lawrence](https://github.com/Pedals2Paddles): - - ***Vehicle***: 3DR Solo & Solo based vehicles -- [Matthias Badaire](https://github.com/badzz): - - ***Subsystem***: FRSky -- [Mirko Denecke](https://github.com/mirkix): - - ***Board***: BBBmini, BeagleBone Blue, PocketPilot -- [Paul Riseborough](https://github.com/priseborough): - - ***Subsystem***: AP_NavEKF2 - - ***Subsystem***: AP_NavEKF3 -- [Víctor Mayoral Vilches](https://github.com/vmayoral): - - ***Board***: PXF, Erle-Brain 2, PXFmini -- [Amilcar Lucas](https://github.com/amilcarlucas): - - ***Subsystem***: Marvelmind -- [Samuel Tabor](https://github.com/samuelctabor): - - ***Subsystem***: Soaring/Gliding -- [Henry Wurzburg](https://github.com/Hwurzburg): - - ***Subsystem***: OSD - - ***Site***: Wiki -- [Peter Hall](https://github.com/IamPete1): - - ***Vehicle***: Tailsitters - - ***Vehicle***: Sailboat - - ***Subsystem***: Scripting -- [Andy Piper](https://github.com/andyp1per): - - ***Subsystem***: Crossfire - - ***Subsystem***: ESC - - ***Subsystem***: OSD - - ***Subsystem***: SmartAudio -- [Alessandro Apostoli ](https://github.com/yaapu): - - ***Subsystem***: Telemetry - - ***Subsystem***: OSD -- [Rishabh Singh ](https://github.com/rishabsingh3003): - - ***Subsystem***: Avoidance/Proximity -- [David Bussenschutt ](https://github.com/davidbuzz): - - ***Subsystem***: ESP32,AP_HAL_ESP32 -- [Charles Villard ](https://github.com/Silvanosky): - - ***Subsystem***: ESP32,AP_HAL_ESP32 +Happy flying and mixing! 🎮🚀 From 44218ff08e152d5815728cc190d30a67722f1ff3 Mon Sep 17 00:00:00 2001 From: graheeth Date: Tue, 24 Oct 2023 10:42:24 +0800 Subject: [PATCH 3/7] read.md update --- README.md | 34 +++++++++++++++++++--------------- 1 file changed, 19 insertions(+), 15 deletions(-) diff --git a/README.md b/README.md index 7985501bf62cb..82a19713141e0 100644 --- a/README.md +++ b/README.md @@ -1,23 +1,27 @@ -# Stick Mixing in Auto Mode +# Altitude Control for Waypoints Using Throttle Input -Hello everyone! 👋 +Hello esteemed users, -## Update Overview -Exciting times! I've rolled out a new feature: **Stick Mixing in Auto Mode**. This lets you control the altitude of your waypoints directly through the throttle input. Imagine the flexibility and dynamic control you'll now have during your flights! +## What's New? + +We are pleased to announce the implementation of "Altitude Control for Waypoints." This feature empowers users to dynamically adjust the altitude of waypoints through throttle input, providing enhanced control during flights. + +## How Does It Work? -## How it works 1. Engage the auto mode. -2. While your drone navigates its waypoint path, manipulate the throttle input. -3. The upcoming waypoints will adjust their altitudes based on your throttle input, offering a real-time and intuitive altitude mixing experience. +2. As your drone follows its designated flight path, adjust the throttle input. +3. The system will modify the upcoming waypoints' altitudes based on your input, offering real-time responsiveness. + +## Validation & Testing + +Ensuring the safety and efficiency of our features is paramount: +- Rigorously tested in the SITL (Software In The Loop) environment. +- Further validated through field tests with a real drone. -## Testing & Verification -Before introducing this, I made sure it's rock solid: -- 🖥️ Tested extensively in the SITL (Software In The Loop) simulation. -- 🚁 Conducted hands-on tests with a real drone. +Both testing methods have confirmed the robustness and reliability of this feature. -Both tests confirmed that the feature works seamlessly and as expected. +## Feedback and Suggestions -## Feedback -Your thoughts and feedback drive us forward! If you spot any quirks or have ideas for improvement, please raise an issue right here in the repo. +Your feedback is invaluable to our continuous improvement. Should you encounter any issues or have any suggestions, please do not hesitate to raise an issue in this repository. -Happy flying and mixing! 🎮🚀 +Thank you for your trust and collaboration. Safe flights and we hope you appreciate the enhanced control. From 806bdd17e1965b33d6995843427264788056b097 Mon Sep 17 00:00:00 2001 From: graheeth <49522804+graheeth@users.noreply.github.com> Date: Tue, 24 Oct 2023 10:43:02 +0800 Subject: [PATCH 4/7] Update README.md --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 82a19713141e0..a11341d429e33 100644 --- a/README.md +++ b/README.md @@ -4,7 +4,7 @@ Hello esteemed users, ## What's New? -We are pleased to announce the implementation of "Altitude Control for Waypoints." This feature empowers users to dynamically adjust the altitude of waypoints through throttle input, providing enhanced control during flights. +I am pleased to announce the implementation of "Altitude Control for Waypoints." This feature empowers users to dynamically adjust the altitude of waypoints through throttle input, providing enhanced control during flights. ## How Does It Work? From 0ac510eda6fe2e0544aad2d903d11397ec4730e4 Mon Sep 17 00:00:00 2001 From: graheeth Date: Tue, 7 Nov 2023 10:33:20 +0800 Subject: [PATCH 5/7] changed the function back to original name and changed readme --- ArduCopter/mode_auto.cpp | 2 +- README.md | 166 ++++++++++++++++-- .../AC_AttitudeControl/AC_PosControl.cpp | 82 +-------- libraries/AC_AttitudeControl/AC_PosControl.h | 4 +- 4 files changed, 157 insertions(+), 97 deletions(-) diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index 012c111f8d00e..10d9c44fb02cb 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -990,7 +990,7 @@ void ModeAuto::wp_run() 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_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()); diff --git a/README.md b/README.md index 82a19713141e0..88ef765e1ea19 100644 --- a/README.md +++ b/README.md @@ -1,27 +1,161 @@ -# Altitude Control for Waypoints Using Throttle Input +# ArduPilot Project -Hello esteemed users, +Discord -## What's New? +[![Test Copter](https://github.com/ArduPilot/ardupilot/workflows/test%20copter/badge.svg?branch=master)](https://github.com/ArduPilot/ardupilot/actions/workflows/test_sitl_copter.yml) [![Test Plane](https://github.com/ArduPilot/ardupilot/workflows/test%20plane/badge.svg?branch=master)](https://github.com/ArduPilot/ardupilot/actions/workflows/test_sitl_plane.yml) [![Test Rover](https://github.com/ArduPilot/ardupilot/workflows/test%20rover/badge.svg?branch=master)](https://github.com/ArduPilot/ardupilot/actions/workflows/test_sitl_rover.yml) [![Test Sub](https://github.com/ArduPilot/ardupilot/workflows/test%20sub/badge.svg?branch=master)](https://github.com/ArduPilot/ardupilot/actions/workflows/test_sitl_sub.yml) [![Test Tracker](https://github.com/ArduPilot/ardupilot/workflows/test%20tracker/badge.svg?branch=master)](https://github.com/ArduPilot/ardupilot/actions/workflows/test_sitl_tracker.yml) -We are pleased to announce the implementation of "Altitude Control for Waypoints." This feature empowers users to dynamically adjust the altitude of waypoints through throttle input, providing enhanced control during flights. +[![Test AP_Periph](https://github.com/ArduPilot/ardupilot/workflows/test%20ap_periph/badge.svg?branch=master)](https://github.com/ArduPilot/ardupilot/actions/workflows/test_sitl_periph.yml) [![Test Chibios](https://github.com/ArduPilot/ardupilot/workflows/test%20chibios/badge.svg?branch=master)](https://github.com/ArduPilot/ardupilot/actions/workflows/test_chibios.yml) [![Test Linux SBC](https://github.com/ArduPilot/ardupilot/workflows/test%20Linux%20SBC/badge.svg?branch=master)](https://github.com/ArduPilot/ardupilot/actions/workflows/test_linux_sbc.yml) [![Test Replay](https://github.com/ArduPilot/ardupilot/workflows/test%20replay/badge.svg?branch=master)](https://github.com/ArduPilot/ardupilot/actions/workflows/test_replay.yml) -## How Does It Work? +[![Test Unit Tests](https://github.com/ArduPilot/ardupilot/workflows/test%20unit%20tests/badge.svg?branch=master)](https://github.com/ArduPilot/ardupilot/actions/workflows/test_unit_tests.yml) [![test size](https://github.com/ArduPilot/ardupilot/actions/workflows/test_size.yml/badge.svg)](https://github.com/ArduPilot/ardupilot/actions/workflows/test_size.yml) -1. Engage the auto mode. -2. As your drone follows its designated flight path, adjust the throttle input. -3. The system will modify the upcoming waypoints' altitudes based on your input, offering real-time responsiveness. +[![Test Environment Setup](https://github.com/ArduPilot/ardupilot/actions/workflows/test_environment.yml/badge.svg?branch=master)](https://github.com/ArduPilot/ardupilot/actions/workflows/test_environment.yml) -## Validation & Testing +[![Cygwin Build](https://github.com/ArduPilot/ardupilot/actions/workflows/cygwin_build.yml/badge.svg)](https://github.com/ArduPilot/ardupilot/actions/workflows/cygwin_build.yml) [![Macos Build](https://github.com/ArduPilot/ardupilot/actions/workflows/macos_build.yml/badge.svg)](https://github.com/ArduPilot/ardupilot/actions/workflows/macos_build.yml) -Ensuring the safety and efficiency of our features is paramount: -- Rigorously tested in the SITL (Software In The Loop) environment. -- Further validated through field tests with a real drone. +[![Coverity Scan Build Status](https://scan.coverity.com/projects/5331/badge.svg)](https://scan.coverity.com/projects/ardupilot-ardupilot) -Both testing methods have confirmed the robustness and reliability of this feature. +[![Test Coverage](https://github.com/ArduPilot/ardupilot/actions/workflows/test_coverage.yml/badge.svg?branch=master)](https://github.com/ArduPilot/ardupilot/actions/workflows/test_coverage.yml) -## Feedback and Suggestions +[![Autotest Status](https://autotest.ardupilot.org/autotest-badge.svg)](https://autotest.ardupilot.org/) -Your feedback is invaluable to our continuous improvement. Should you encounter any issues or have any suggestions, please do not hesitate to raise an issue in this repository. +ArduPilot is the most advanced, full-featured, and reliable open source autopilot software available. +It has been under development since 2010 by a diverse team of professional engineers, computer scientists, and community contributors. +Our autopilot software is capable of controlling almost any vehicle system imaginable, from conventional airplanes, quad planes, multi-rotors, and helicopters to rovers, boats, balance bots, and even submarines. +It is continually being expanded to provide support for new emerging vehicle types. -Thank you for your trust and collaboration. Safe flights and we hope you appreciate the enhanced control. +## The ArduPilot project is made up of: ## + +- ArduCopter: [code](https://github.com/ArduPilot/ardupilot/tree/master/ArduCopter), [wiki](https://ardupilot.org/copter/index.html) + +- ArduPlane: [code](https://github.com/ArduPilot/ardupilot/tree/master/ArduPlane), [wiki](https://ardupilot.org/plane/index.html) + +- Rover: [code](https://github.com/ArduPilot/ardupilot/tree/master/Rover), [wiki](https://ardupilot.org/rover/index.html) + +- ArduSub : [code](https://github.com/ArduPilot/ardupilot/tree/master/ArduSub), [wiki](http://ardusub.com/) + +- Antenna Tracker : [code](https://github.com/ArduPilot/ardupilot/tree/master/AntennaTracker), [wiki](https://ardupilot.org/antennatracker/index.html) + +## User Support & Discussion Forums ## + +- Support Forum: + +- Community Site: + +## Developer Information ## + +- Github repository: + +- Main developer wiki: + +- Developer discussion: + +- Developer chat: + +## Top Contributors ## + +- [Flight code contributors](https://github.com/ArduPilot/ardupilot/graphs/contributors) +- [Wiki contributors](https://github.com/ArduPilot/ardupilot_wiki/graphs/contributors) +- [Most active support forum users](https://discuss.ardupilot.org/u?order=post_count&period=quarterly) +- [Partners who contribute financially](https://ardupilot.org/about/Partners) + +## How To Get Involved ## + +- The ArduPilot project is open source and we encourage participation and code contributions: [guidelines for contributors to the ardupilot codebase](https://ardupilot.org/dev/docs/contributing.html) + +- We have an active group of Beta Testers to help us improve our code: [release procedures](https://ardupilot.org/dev/docs/release-procedures.html) + +- Desired Enhancements and Bugs can be posted to the [issues list](https://github.com/ArduPilot/ardupilot/issues). + +- Help other users with log analysis in the [support forums](https://discuss.ardupilot.org/) + +- Improve the wiki and chat with other [wiki editors on Discord #documentation](https://discord.com/channels/ardupilot) + +- Contact the developers on one of the [communication channels](https://ardupilot.org/copter/docs/common-contact-us.html) + +## License ## + +The ArduPilot project is licensed under the GNU General Public +License, version 3. + +- [Overview of license](https://ardupilot.org/dev/docs/license-gplv3.html) + +- [Full Text](https://github.com/ArduPilot/ardupilot/blob/master/COPYING.txt) + +## Maintainers ## + +ArduPilot is comprised of several parts, vehicles and boards. The list below +contains the people that regularly contribute to the project and are responsible +for reviewing patches on their specific area. + +- [Andrew Tridgell](https://github.com/tridge): + - ***Vehicle***: Plane, AntennaTracker + - ***Board***: Pixhawk, Pixhawk2, PixRacer +- [Francisco Ferreira](https://github.com/oxinarf): + - ***Bug Master*** +- [Grant Morphett](https://github.com/gmorph): + - ***Vehicle***: Rover +- [Willian Galvani](https://github.com/williangalvani): + - ***Vehicle***: Sub +- [Lucas De Marchi](https://github.com/lucasdemarchi): + - ***Subsystem***: Linux +- [Michael du Breuil](https://github.com/WickedShell): + - ***Subsystem***: Batteries + - ***Subsystem***: GPS + - ***Subsystem***: Scripting +- [Peter Barker](https://github.com/peterbarker): + - ***Subsystem***: DataFlash, Tools +- [Randy Mackay](https://github.com/rmackay9): + - ***Vehicle***: Copter, Rover, AntennaTracker +- [Siddharth Purohit](https://github.com/bugobliterator): + - ***Subsystem***: CAN, Compass + - ***Board***: Cube* +- [Tom Pittenger](https://github.com/magicrub): + - ***Vehicle***: Plane +- [Bill Geyer](https://github.com/bnsgeyer): + - ***Vehicle***: TradHeli +- [Emile Castelnuovo](https://github.com/emilecastelnuovo): + - ***Board***: VRBrain +- [Georgii Staroselskii](https://github.com/staroselskii): + - ***Board***: NavIO +- [Gustavo José de Sousa](https://github.com/guludo): + - ***Subsystem***: Build system +- [Julien Beraud](https://github.com/jberaud): + - ***Board***: Bebop & Bebop 2 +- [Leonard Hall](https://github.com/lthall): + - ***Subsystem***: Copter attitude control and navigation +- [Matt Lawrence](https://github.com/Pedals2Paddles): + - ***Vehicle***: 3DR Solo & Solo based vehicles +- [Matthias Badaire](https://github.com/badzz): + - ***Subsystem***: FRSky +- [Mirko Denecke](https://github.com/mirkix): + - ***Board***: BBBmini, BeagleBone Blue, PocketPilot +- [Paul Riseborough](https://github.com/priseborough): + - ***Subsystem***: AP_NavEKF2 + - ***Subsystem***: AP_NavEKF3 +- [Víctor Mayoral Vilches](https://github.com/vmayoral): + - ***Board***: PXF, Erle-Brain 2, PXFmini +- [Amilcar Lucas](https://github.com/amilcarlucas): + - ***Subsystem***: Marvelmind +- [Samuel Tabor](https://github.com/samuelctabor): + - ***Subsystem***: Soaring/Gliding +- [Henry Wurzburg](https://github.com/Hwurzburg): + - ***Subsystem***: OSD + - ***Site***: Wiki +- [Peter Hall](https://github.com/IamPete1): + - ***Vehicle***: Tailsitters + - ***Vehicle***: Sailboat + - ***Subsystem***: Scripting +- [Andy Piper](https://github.com/andyp1per): + - ***Subsystem***: Crossfire + - ***Subsystem***: ESC + - ***Subsystem***: OSD + - ***Subsystem***: SmartAudio +- [Alessandro Apostoli ](https://github.com/yaapu): + - ***Subsystem***: Telemetry + - ***Subsystem***: OSD +- [Rishabh Singh ](https://github.com/rishabsingh3003): + - ***Subsystem***: Avoidance/Proximity +- [David Bussenschutt ](https://github.com/davidbuzz): + - ***Subsystem***: ESP32,AP_HAL_ESP32 +- [Charles Villard ](https://github.com/Silvanosky): + - ***Subsystem***: ESP32,AP_HAL_ESP32 \ No newline at end of file diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index 1261d2a54002b..790d21a2b328d 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp @@ -926,82 +926,8 @@ 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 - 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(); - - // calculate the target velocity correction - float pos_target_zf = _pos_target.z; - _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 - - // Calculate vertical acceleration - 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 - - // send throttle to attitude controller with angle boost - _attitude_control.set_throttle_out(thr_out, true, POSCONTROL_THROTTLE_CUTOFF_FREQ_HZ); - - // Check for vertical controller health - - // _speed_down_cms is checked to be non-zero when set - 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; - } -} - -void AC_PosControl::update_z_controller_edited(float throttle_input) { +void AC_PosControl::update_z_controller(float throttle_input) { // check for ekf z-axis position reset handle_ekf_z_reset(); @@ -1016,8 +942,10 @@ void AC_PosControl::update_z_controller_edited(float throttle_input) { } _last_update_z_ticks = AP::scheduler().ticks32(); - - // Use throttle input to adjust desired altitude (2500 is max throttle input which translates to 2.5m) + // Calculate throttle adjustment to modify the desired altitude (_pos_target.z). + // The throttle input, which ranges up to a maximum of 2500, is squared (to ensure a positive value and increase adjustment sensitivity at higher throttle inputs) + // and then divided by 125000 to scale _pos_target.z appropriately. + // This adjustment is then added to the current target altitude to get the new desired altitude. float throttle_adjustment = _pos_target.z * throttle_input * abs(throttle_input) / 125000; float pos_target_zf = _pos_target.z + throttle_adjustment; diff --git a/libraries/AC_AttitudeControl/AC_PosControl.h b/libraries/AC_AttitudeControl/AC_PosControl.h index 6f706ec9216dc..1bfe3b838263b 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.h +++ b/libraries/AC_AttitudeControl/AC_PosControl.h @@ -233,9 +233,7 @@ class AC_PosControl /// 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 update_z_controller(); - - void update_z_controller_edited(float throttle_input); + void update_z_controller(float throttle_input); /// From 927c9f7f86331bdb34229ee7dc288b76592d8c18 Mon Sep 17 00:00:00 2001 From: graheeth Date: Tue, 7 Nov 2023 10:50:19 +0800 Subject: [PATCH 6/7] changed the function back to original name and changed readme again and again --- README.md | 4 ---- 1 file changed, 4 deletions(-) diff --git a/README.md b/README.md index ec75d85d68a40..88ef765e1ea19 100644 --- a/README.md +++ b/README.md @@ -4,11 +4,7 @@ [![Test Copter](https://github.com/ArduPilot/ardupilot/workflows/test%20copter/badge.svg?branch=master)](https://github.com/ArduPilot/ardupilot/actions/workflows/test_sitl_copter.yml) [![Test Plane](https://github.com/ArduPilot/ardupilot/workflows/test%20plane/badge.svg?branch=master)](https://github.com/ArduPilot/ardupilot/actions/workflows/test_sitl_plane.yml) [![Test Rover](https://github.com/ArduPilot/ardupilot/workflows/test%20rover/badge.svg?branch=master)](https://github.com/ArduPilot/ardupilot/actions/workflows/test_sitl_rover.yml) [![Test Sub](https://github.com/ArduPilot/ardupilot/workflows/test%20sub/badge.svg?branch=master)](https://github.com/ArduPilot/ardupilot/actions/workflows/test_sitl_sub.yml) [![Test Tracker](https://github.com/ArduPilot/ardupilot/workflows/test%20tracker/badge.svg?branch=master)](https://github.com/ArduPilot/ardupilot/actions/workflows/test_sitl_tracker.yml) -<<<<<<< HEAD [![Test AP_Periph](https://github.com/ArduPilot/ardupilot/workflows/test%20ap_periph/badge.svg?branch=master)](https://github.com/ArduPilot/ardupilot/actions/workflows/test_sitl_periph.yml) [![Test Chibios](https://github.com/ArduPilot/ardupilot/workflows/test%20chibios/badge.svg?branch=master)](https://github.com/ArduPilot/ardupilot/actions/workflows/test_chibios.yml) [![Test Linux SBC](https://github.com/ArduPilot/ardupilot/workflows/test%20Linux%20SBC/badge.svg?branch=master)](https://github.com/ArduPilot/ardupilot/actions/workflows/test_linux_sbc.yml) [![Test Replay](https://github.com/ArduPilot/ardupilot/workflows/test%20replay/badge.svg?branch=master)](https://github.com/ArduPilot/ardupilot/actions/workflows/test_replay.yml) -======= -I am pleased to announce the implementation of "Altitude Control for Waypoints." This feature empowers users to dynamically adjust the altitude of waypoints through throttle input, providing enhanced control during flights. ->>>>>>> origin/Jank_Stick_Mixing [![Test Unit Tests](https://github.com/ArduPilot/ardupilot/workflows/test%20unit%20tests/badge.svg?branch=master)](https://github.com/ArduPilot/ardupilot/actions/workflows/test_unit_tests.yml) [![test size](https://github.com/ArduPilot/ardupilot/actions/workflows/test_size.yml/badge.svg)](https://github.com/ArduPilot/ardupilot/actions/workflows/test_size.yml) From c4619a20dc3dd254e74f0cfaf842886619c8741d Mon Sep 17 00:00:00 2001 From: graheeth Date: Tue, 28 Nov 2023 12:46:17 +0800 Subject: [PATCH 7/7] Changing Altitude Continously, needed to add a new file for this. --- ArduCopter/mode_auto.cpp | 16 ++-- ArduCopter/shared_throttle.cpp | 3 + ArduCopter/shared_throttle.h | 10 +++ .../AC_AttitudeControl/AC_PosControl.cpp | 78 +++++++++++++++++++ libraries/AC_AttitudeControl/AC_PosControl.h | 1 + libraries/GCS_MAVLink/GCS.h | 1 + libraries/GCS_MAVLink/GCS_Common.cpp | 3 + 7 files changed, 106 insertions(+), 6 deletions(-) create mode 100644 ArduCopter/shared_throttle.cpp create mode 100644 ArduCopter/shared_throttle.h diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index cdf41a4f7aa47..84b30e9b0f85f 100644 --- a/ArduCopter/mode_auto.cpp +++ b/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,13 @@ 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); // call attitude controller with auto yaw attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.get_heading()); diff --git a/ArduCopter/shared_throttle.cpp b/ArduCopter/shared_throttle.cpp new file mode 100644 index 0000000000000..5a4d57c9e955f --- /dev/null +++ b/ArduCopter/shared_throttle.cpp @@ -0,0 +1,3 @@ +#include "shared_throttle.h" + +SharedData sharedData; // Define the variable diff --git a/ArduCopter/shared_throttle.h b/ArduCopter/shared_throttle.h new file mode 100644 index 0000000000000..4006041534f3b --- /dev/null +++ b/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 diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index 83bc5cbd4d21c..473a24a5a8a25 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp @@ -5,6 +5,7 @@ #include // motors library #include #include +#include "/home/solid/Downloads/Extra_Work/Phan/ardupilot/ardupilot/ArduCopter/shared_throttle.h" // Include the header extern const AP_HAL::HAL& hal; @@ -988,6 +989,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 /// diff --git a/libraries/AC_AttitudeControl/AC_PosControl.h b/libraries/AC_AttitudeControl/AC_PosControl.h index 5284f3f36fabc..7982795e64f58 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.h +++ b/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); /// diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index 519459f45c9b3..eb30ea306d5fa 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/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; diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 43a8a46f127a7..a34f147fa2f13 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -4443,6 +4443,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