From 27d96eb64e13bcb66b0b570974af8f59c4e7d8b5 Mon Sep 17 00:00:00 2001 From: Asif Khan Date: Sun, 7 May 2023 13:11:48 +0530 Subject: [PATCH] AP_Mount: Support for pointing mount to circle center --- ArduCopter/mode_circle.cpp | 13 +++++++++++++ libraries/AC_WPNav/AC_Circle.h | 4 ++++ 2 files changed, 17 insertions(+) diff --git a/ArduCopter/mode_circle.cpp b/ArduCopter/mode_circle.cpp index c86de8856d..1c73b807dd 100644 --- a/ArduCopter/mode_circle.cpp +++ b/ArduCopter/mode_circle.cpp @@ -1,4 +1,5 @@ #include "Copter.h" +#include #if MODE_CIRCLE_ENABLED == ENABLED @@ -20,6 +21,18 @@ bool ModeCircle::init(bool ignore_checks) // initialise circle controller including setting the circle center based on vehicle speed copter.circle_nav->init(); +#if HAL_MOUNT_ENABLED + AP_Mount *s = AP_Mount::get_singleton(); + + // Check if the CIRCLE_OPTIONS parameter have roi_at_center + if (copter.circle_nav->roi_at_center()) { + Vector3p loc = copter.circle_nav->get_center(); + loc.z = 0; + Location circle_center(loc, Location::AltFrame::ABOVE_TERRAIN); + s->set_roi_target(circle_center); + } +#endif + // set auto yaw circle mode auto_yaw.set_mode(AutoYaw::Mode::CIRCLE); diff --git a/libraries/AC_WPNav/AC_Circle.h b/libraries/AC_WPNav/AC_Circle.h index 88dd03c02c..498241e025 100644 --- a/libraries/AC_WPNav/AC_Circle.h +++ b/libraries/AC_WPNav/AC_Circle.h @@ -89,6 +89,9 @@ public: /// true if pilot control of radius and turn rate is enabled bool pilot_control_enabled() const { return (_options.get() & CircleOptions::MANUAL_CONTROL) != 0; } + /// true if mount roi is at circle center + bool roi_at_center() const { return (_options.get() & CircleOptions::ROI_AT_CENTER) != 0; } + /// provide rangefinder based terrain offset /// terrain offset is the terrain's height above the EKF origin void set_rangefinder_terrain_offset(bool use, bool healthy, float terrain_offset_cm) { _rangefinder_available = use; _rangefinder_healthy = healthy; _rangefinder_terrain_offset_cm = terrain_offset_cm;} @@ -136,6 +139,7 @@ private: MANUAL_CONTROL = 1U << 0, FACE_DIRECTION_OF_TRAVEL = 1U << 1, INIT_AT_CENTER = 1U << 2, // true then the circle center will be the current location, false and the center will be 1 radius ahead + ROI_AT_CENTER = 1U << 3, // true when the mount roi is at circle center }; // parameters