From bd5e966e5a7c5a32b9b50f401c99c4a4e866f332 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Henrik=20Forst=C3=A9n?= Date: Mon, 28 Oct 2024 18:45:28 +0200 Subject: [PATCH] Copter: Configurable yaw offset for DO_SET_ROI --- ArduCopter/Parameters.cpp | 8 ++++++++ ArduCopter/Parameters.h | 2 ++ ArduCopter/autoyaw.cpp | 4 +++- 3 files changed, 13 insertions(+), 1 deletion(-) diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index 8f5425482143a..f0b78980a6b3f 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -1232,6 +1232,14 @@ const AP_Param::GroupInfo ParametersG2::var_info2[] = { // @User: Advanced AP_GROUPINFO("FS_EKF_FILT", 8, ParametersG2, fs_ekf_filt_hz, FS_EKF_FILT_DEFAULT), + // @Param: ROI_YAW_OFFSET + // @DisplayName: ROI yaw offset + // @Description: Offset applied to yaw when pointing to region of interest. Zero offset points nose at ROI. + // @Range: -18000 18000 + // @Units: cdeg + // @User: Advanced + AP_GROUPINFO("ROI_YAW_OFFSET", 9, ParametersG2, roi_yaw_offset, 0), + // ID 62 is reserved for the AP_SUBGROUPEXTENSION AP_GROUPEND diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index 0eee4058331d7..7752c8c2193ee 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -676,6 +676,8 @@ class ParametersG2 { // EKF variance filter cutoff AP_Float fs_ekf_filt_hz; + AP_Float roi_yaw_offset; + #if WEATHERVANE_ENABLED AC_WeatherVane weathervane; #endif diff --git a/ArduCopter/autoyaw.cpp b/ArduCopter/autoyaw.cpp index 9ce780a9a002f..4571da6cd9395 100644 --- a/ArduCopter/autoyaw.cpp +++ b/ArduCopter/autoyaw.cpp @@ -5,7 +5,9 @@ Mode::AutoYaw Mode::auto_yaw; // roi_yaw - returns heading towards location held in roi float Mode::AutoYaw::roi_yaw() const { - return get_bearing_cd(copter.inertial_nav.get_position_xy_cm(), roi.xy()); + float yaw = get_bearing_cd(copter.inertial_nav.get_position_xy_cm(), roi.xy()); + yaw += copter.g2.roi_yaw_offset; + return wrap_360_cd(yaw); } // returns a yaw in degrees, direction of vehicle travel: