From 06138fddda7d206d2da6e24abda62625b5812e93 Mon Sep 17 00:00:00 2001 From: haarshitgarg <59084710+haarshitgarg@users.noreply.github.com> Date: Wed, 1 May 2024 13:43:49 -0600 Subject: [PATCH 1/3] ArduCopter: enabled sending waypoints from a companion computer to ardupilot for copter and rover Signed-off-by: Ryan Friedman --- ArduCopter/AP_ExternalControl_Copter.cpp | 9 ++++++++ ArduCopter/AP_ExternalControl_Copter.h | 8 ++++++- ArduCopter/Copter.cpp | 27 ++++++++++++++---------- ArduCopter/Copter.h | 7 +++++- 4 files changed, 38 insertions(+), 13 deletions(-) diff --git a/ArduCopter/AP_ExternalControl_Copter.cpp b/ArduCopter/AP_ExternalControl_Copter.cpp index 35353a412618f..eb16147737fd6 100644 --- a/ArduCopter/AP_ExternalControl_Copter.cpp +++ b/ArduCopter/AP_ExternalControl_Copter.cpp @@ -29,6 +29,15 @@ bool AP_ExternalControl_Copter::set_linear_velocity_and_yaw_rate(const Vector3f return true; } +bool AP_ExternalControl_Copter::set_global_position(const Location& loc) +{ + // Check if copter is ready for external control and returns false if it is not. + if (!ready_for_external_control()) { + return false; + } + return copter.set_target_location(loc); +} + bool AP_ExternalControl_Copter::ready_for_external_control() { return copter.flightmode->in_guided_mode() && copter.motors->armed(); diff --git a/ArduCopter/AP_ExternalControl_Copter.h b/ArduCopter/AP_ExternalControl_Copter.h index e7601c5552fc6..527af304f64a7 100644 --- a/ArduCopter/AP_ExternalControl_Copter.h +++ b/ArduCopter/AP_ExternalControl_Copter.h @@ -7,7 +7,8 @@ #if AP_EXTERNAL_CONTROL_ENABLED -class AP_ExternalControl_Copter : public AP_ExternalControl { +class AP_ExternalControl_Copter : public AP_ExternalControl +{ public: /* Set linear velocity and yaw rate. Pass NaN for yaw_rate_rads to not control yaw. @@ -15,6 +16,11 @@ class AP_ExternalControl_Copter : public AP_ExternalControl { Yaw is in earth frame, NED [rad/s]. */ bool set_linear_velocity_and_yaw_rate(const Vector3f &linear_velocity, float yaw_rate_rads) override WARN_IF_UNUSED; + + /* + Sets the target global position for a loiter point. + */ + bool set_global_position(const Location& loc) override WARN_IF_UNUSED; private: /* Return true if Copter is ready to handle external control data. diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp index a16286a6a4a93..38aabd23dbf15 100644 --- a/ArduCopter/Copter.cpp +++ b/ArduCopter/Copter.cpp @@ -272,32 +272,37 @@ void Copter::get_scheduler_tasks(const AP_Scheduler::Task *&tasks, constexpr int8_t Copter::_failsafe_priorities[7]; -#if AP_SCRIPTING_ENABLED + +#if AP_SCRIPTING_ENABLED || AP_EXTERNAL_CONTROL_ENABLED #if MODE_GUIDED_ENABLED == ENABLED -// start takeoff to given altitude (for use by scripting) -bool Copter::start_takeoff(float alt) +// set target location (for use by external control and scripting) +bool Copter::set_target_location(const Location& target_loc) { // exit if vehicle is not in Guided mode or Auto-Guided mode if (!flightmode->in_guided_mode()) { return false; } - if (mode_guided.do_user_takeoff_start(alt * 100.0f)) { - copter.set_auto_armed(true); - return true; - } - return false; + return mode_guided.set_destination(target_loc); } +#endif //MODE_GUIDED_ENABLED == ENABLED +#endif //AP_SCRIPTING_ENABLED || AP_EXTERNAL_CONTROL_ENABLED -// set target location (for use by scripting) -bool Copter::set_target_location(const Location& target_loc) +#if AP_SCRIPTING_ENABLED +#if MODE_GUIDED_ENABLED == ENABLED +// start takeoff to given altitude (for use by scripting) +bool Copter::start_takeoff(float alt) { // exit if vehicle is not in Guided mode or Auto-Guided mode if (!flightmode->in_guided_mode()) { return false; } - return mode_guided.set_destination(target_loc); + if (mode_guided.do_user_takeoff_start(alt * 100.0f)) { + copter.set_auto_armed(true); + return true; + } + return false; } // set target position (for use by scripting) diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 3285f267976e5..a872dc67a2888 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -666,10 +666,15 @@ class Copter : public AP_Vehicle { void get_scheduler_tasks(const AP_Scheduler::Task *&tasks, uint8_t &task_count, uint32_t &log_bit) override; +#if AP_SCRIPTING_ENABLED || AP_EXTERNAL_CONTROL_ENABLED +#if MODE_GUIDED_ENABLED == ENABLED + bool set_target_location(const Location& target_loc) override; +#endif // MODE_GUIDED_ENABLED == ENABLED +#endif // AP_SCRIPTING_ENABLED || AP_EXTERNAL_CONTROL_ENABLED + #if AP_SCRIPTING_ENABLED #if MODE_GUIDED_ENABLED == ENABLED bool start_takeoff(float alt) override; - bool set_target_location(const Location& target_loc) override; bool set_target_pos_NED(const Vector3f& target_pos, bool use_yaw, float yaw_deg, bool use_yaw_rate, float yaw_rate_degs, bool yaw_relative, bool terrain_alt) override; bool set_target_posvel_NED(const Vector3f& target_pos, const Vector3f& target_vel) override; bool set_target_posvelaccel_NED(const Vector3f& target_pos, const Vector3f& target_vel, const Vector3f& target_accel, bool use_yaw, float yaw_deg, bool use_yaw_rate, float yaw_rate_degs, bool yaw_relative) override; From 53f4867a419fab257be851549fdad009617f307f Mon Sep 17 00:00:00 2001 From: haarshitgarg <59084710+haarshitgarg@users.noreply.github.com> Date: Wed, 1 May 2024 13:43:49 -0600 Subject: [PATCH 2/3] Rover: enabled sending waypoints from a companion computer to ardupilot for copter and rover Signed-off-by: Ryan Friedman --- Rover/AP_ExternalControl_Rover.cpp | 6 ++++++ Rover/AP_ExternalControl_Rover.h | 9 ++++++++- Rover/Rover.cpp | 6 ++++-- Rover/Rover.h | 5 ++++- 4 files changed, 22 insertions(+), 4 deletions(-) diff --git a/Rover/AP_ExternalControl_Rover.cpp b/Rover/AP_ExternalControl_Rover.cpp index 1c08f92a9b5fd..d7511f6f0f832 100644 --- a/Rover/AP_ExternalControl_Rover.cpp +++ b/Rover/AP_ExternalControl_Rover.cpp @@ -29,6 +29,12 @@ bool AP_ExternalControl_Rover::set_linear_velocity_and_yaw_rate(const Vector3f & return true; } +bool AP_ExternalControl_Rover::set_global_position(const Location& loc) +{ + // set_target_location only checks if the rover is in guided mode or not + return rover.set_target_location(loc); +} + bool AP_ExternalControl_Rover::ready_for_external_control() { return rover.control_mode->in_guided_mode() && rover.arming.is_armed(); diff --git a/Rover/AP_ExternalControl_Rover.h b/Rover/AP_ExternalControl_Rover.h index 434972833e45d..e7350ebbe2c62 100644 --- a/Rover/AP_ExternalControl_Rover.h +++ b/Rover/AP_ExternalControl_Rover.h @@ -7,7 +7,8 @@ #if AP_EXTERNAL_CONTROL_ENABLED -class AP_ExternalControl_Rover : public AP_ExternalControl { +class AP_ExternalControl_Rover : public AP_ExternalControl +{ public: /* Set linear velocity and yaw rate. Pass NaN for yaw_rate_rads to not control yaw. @@ -15,6 +16,12 @@ class AP_ExternalControl_Rover : public AP_ExternalControl { Yaw is in earth frame, NED [rad/s]. */ bool set_linear_velocity_and_yaw_rate(const Vector3f &linear_velocity, float yaw_rate_rads)override WARN_IF_UNUSED; + + /* + Sets the global position for loiter point + */ + bool set_global_position(const Location& loc) override WARN_IF_UNUSED; + private: /* Return true if Rover is ready to handle external control data. diff --git a/Rover/Rover.cpp b/Rover/Rover.cpp index f1a47b452545d..1278977b59a91 100644 --- a/Rover/Rover.cpp +++ b/Rover/Rover.cpp @@ -157,8 +157,8 @@ Rover::Rover(void) : { } -#if AP_SCRIPTING_ENABLED -// set target location (for use by scripting) +#if AP_SCRIPTING_ENABLED || AP_EXTERNAL_CONTROL_ENABLED +// set target location (for use by external control and scripting) bool Rover::set_target_location(const Location& target_loc) { // exit if vehicle is not in Guided mode or Auto-Guided mode @@ -168,7 +168,9 @@ bool Rover::set_target_location(const Location& target_loc) return mode_guided.set_desired_location(target_loc); } +#endif //AP_SCRIPTING_ENABLED || AP_EXTERNAL_CONTROL_ENABLED +#if AP_SCRIPTING_ENABLED // set target velocity (for use by scripting) bool Rover::set_target_velocity_NED(const Vector3f& vel_ned) { diff --git a/Rover/Rover.h b/Rover/Rover.h index 6b2493d4fc7a5..0db95d23b3997 100644 --- a/Rover/Rover.h +++ b/Rover/Rover.h @@ -269,8 +269,11 @@ class Rover : public AP_Vehicle { cruise_learn_t cruise_learn; // Rover.cpp -#if AP_SCRIPTING_ENABLED +#if AP_SCRIPTING_ENABLED || AP_EXTERNAL_CONTROL_ENABLED bool set_target_location(const Location& target_loc) override; +#endif + +#if AP_SCRIPTING_ENABLED bool set_target_velocity_NED(const Vector3f& vel_ned) override; bool set_steering_and_throttle(float steering, float throttle) override; bool get_steering_and_throttle(float& steering, float& throttle) override; From f66e85d152e4b8853270cfaaebe931d94ce85b07 Mon Sep 17 00:00:00 2001 From: haarshitgarg <59084710+haarshitgarg@users.noreply.github.com> Date: Wed, 1 May 2024 13:43:49 -0600 Subject: [PATCH 3/3] Tools: enabled sending waypoints from a companion computer to ardupilot for copter and rover Signed-off-by: Ryan Friedman --- Tools/scripts/run_astyle.py | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/Tools/scripts/run_astyle.py b/Tools/scripts/run_astyle.py index 4e495994735b5..c92fc9079f1d6 100755 --- a/Tools/scripts/run_astyle.py +++ b/Tools/scripts/run_astyle.py @@ -26,8 +26,12 @@ def __init__(self, *, dry_run=DRY_RUN_DEFAULT): ] self.files_to_check = [ pathlib.Path(s) for s in [ + 'ArduCopter/AP_ExternalControl_Copter.cpp', + 'ArduCopter/AP_ExternalControl_Copter.h', 'libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain7.cpp', 'libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain7.h', + 'Rover/AP_ExternalControl_Rover.cpp', + 'Rover/AP_ExternalControl_Rover.h', ] ] self.dry_run = dry_run