From a13639d9d8b72fe658e818e250df710ca889a4ea 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] 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 35353a4126..eb16147737 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 e7601c5552..527af304f6 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 @@ public: 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 a16286a6a4..38aabd23db 100644 --- a/ArduCopter/Copter.cpp +++ b/ArduCopter/Copter.cpp @@ -272,6 +272,22 @@ void Copter::get_scheduler_tasks(const AP_Scheduler::Task *&tasks, constexpr int8_t Copter::_failsafe_priorities[7]; + +#if AP_SCRIPTING_ENABLED || AP_EXTERNAL_CONTROL_ENABLED +#if MODE_GUIDED_ENABLED == ENABLED +// 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; + } + + return mode_guided.set_destination(target_loc); +} +#endif //MODE_GUIDED_ENABLED == ENABLED +#endif //AP_SCRIPTING_ENABLED || AP_EXTERNAL_CONTROL_ENABLED + #if AP_SCRIPTING_ENABLED #if MODE_GUIDED_ENABLED == ENABLED // start takeoff to given altitude (for use by scripting) @@ -289,17 +305,6 @@ bool Copter::start_takeoff(float alt) return false; } -// set target location (for use by 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; - } - - return mode_guided.set_destination(target_loc); -} - // set target position (for use by scripting) bool Copter::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) { diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 3285f26797..a872dc67a2 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -666,10 +666,15 @@ private: 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;