From b7b2d9a6ced572daf76334bf5e8c0105839ca292 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Tue, 14 Apr 2020 11:38:50 +0100 Subject: [PATCH] AP_Compass: add support for custom compass rotation via COMPASS_CUS_{ROLL,PIT,YAW} add reboot required for parameters that need it disable custom rotation on AP_Periph devices --- libraries/AP_Compass/AP_Compass.cpp | 41 +++++++++++++++++++-- libraries/AP_Compass/AP_Compass.h | 6 +++ libraries/AP_Compass/AP_Compass_Backend.cpp | 17 +++++++++ 3 files changed, 61 insertions(+), 3 deletions(-) diff --git a/libraries/AP_Compass/AP_Compass.cpp b/libraries/AP_Compass/AP_Compass.cpp index b091b41912..95c8ad7f27 100644 --- a/libraries/AP_Compass/AP_Compass.cpp +++ b/libraries/AP_Compass/AP_Compass.cpp @@ -5,6 +5,7 @@ #include #include #include +#include #include "AP_Compass_SITL.h" #include "AP_Compass_AK8963.h" @@ -149,7 +150,7 @@ const AP_Param::GroupInfo Compass::var_info[] = { // @Param: ORIENT // @DisplayName: Compass orientation // @Description: The orientation of the first external compass relative to the vehicle frame. This value will be ignored unless this compass is set as an external compass. When set correctly in the northern hemisphere, pointing the nose and right side down should increase the MagX and MagY values respectively. Rolling the vehicle upside down should decrease the MagZ value. For southern hemisphere, switch increase and decrease. NOTE: For internal compasses, AHRS_ORIENT is used. - // @Values: 0:None,1:Yaw45,2:Yaw90,3:Yaw135,4:Yaw180,5:Yaw225,6:Yaw270,7:Yaw315,8:Roll180,9:Roll180Yaw45,10:Roll180Yaw90,11:Roll180Yaw135,12:Pitch180,13:Roll180Yaw225,14:Roll180Yaw270,15:Roll180Yaw315,16:Roll90,17:Roll90Yaw45,18:Roll90Yaw90,19:Roll90Yaw135,20:Roll270,21:Roll270Yaw45,22:Roll270Yaw90,23:Roll270Yaw135,24:Pitch90,25:Pitch270,26:Pitch180Yaw90,27:Pitch180Yaw270,28:Roll90Pitch90,29:Roll180Pitch90,30:Roll270Pitch90,31:Roll90Pitch180,32:Roll270Pitch180,33:Roll90Pitch270,34:Roll180Pitch270,35:Roll270Pitch270,36:Roll90Pitch180Yaw90,37:Roll90Yaw270,38:Yaw293Pitch68Roll180,39:Pitch315,40:Roll90Pitch315 + // @Values: 0:None,1:Yaw45,2:Yaw90,3:Yaw135,4:Yaw180,5:Yaw225,6:Yaw270,7:Yaw315,8:Roll180,9:Roll180Yaw45,10:Roll180Yaw90,11:Roll180Yaw135,12:Pitch180,13:Roll180Yaw225,14:Roll180Yaw270,15:Roll180Yaw315,16:Roll90,17:Roll90Yaw45,18:Roll90Yaw90,19:Roll90Yaw135,20:Roll270,21:Roll270Yaw45,22:Roll270Yaw90,23:Roll270Yaw135,24:Pitch90,25:Pitch270,26:Pitch180Yaw90,27:Pitch180Yaw270,28:Roll90Pitch90,29:Roll180Pitch90,30:Roll270Pitch90,31:Roll90Pitch180,32:Roll270Pitch180,33:Roll90Pitch270,34:Roll180Pitch270,35:Roll270Pitch270,36:Roll90Pitch180Yaw90,37:Roll90Yaw270,38:Yaw293Pitch68Roll180,39:Pitch315,40:Roll90Pitch315,100:Custom // @User: Advanced AP_GROUPINFO("ORIENT", 8, Compass, _state._priv_instance[0].orientation, ROTATION_NONE), @@ -309,7 +310,7 @@ const AP_Param::GroupInfo Compass::var_info[] = { // @Param: ORIENT2 // @DisplayName: Compass2 orientation // @Description: The orientation of a second external compass relative to the vehicle frame. This value will be ignored unless this compass is set as an external compass. When set correctly in the northern hemisphere, pointing the nose and right side down should increase the MagX and MagY values respectively. Rolling the vehicle upside down should decrease the MagZ value. For southern hemisphere, switch increase and decrease. NOTE: For internal compasses, AHRS_ORIENT is used. - // @Values: 0:None,1:Yaw45,2:Yaw90,3:Yaw135,4:Yaw180,5:Yaw225,6:Yaw270,7:Yaw315,8:Roll180,9:Roll180Yaw45,10:Roll180Yaw90,11:Roll180Yaw135,12:Pitch180,13:Roll180Yaw225,14:Roll180Yaw270,15:Roll180Yaw315,16:Roll90,17:Roll90Yaw45,18:Roll90Yaw90,19:Roll90Yaw135,20:Roll270,21:Roll270Yaw45,22:Roll270Yaw90,23:Roll270Yaw135,24:Pitch90,25:Pitch270,26:Pitch180Yaw90,27:Pitch180Yaw270,28:Roll90Pitch90,29:Roll180Pitch90,30:Roll270Pitch90,31:Roll90Pitch180,32:Roll270Pitch180,33:Roll90Pitch270,34:Roll180Pitch270,35:Roll270Pitch270,36:Roll90Pitch180Yaw90,37:Roll90Yaw270,38:Yaw293Pitch68Roll180,39:Pitch315,40:Roll90Pitch315 + // @Values: 0:None,1:Yaw45,2:Yaw90,3:Yaw135,4:Yaw180,5:Yaw225,6:Yaw270,7:Yaw315,8:Roll180,9:Roll180Yaw45,10:Roll180Yaw90,11:Roll180Yaw135,12:Pitch180,13:Roll180Yaw225,14:Roll180Yaw270,15:Roll180Yaw315,16:Roll90,17:Roll90Yaw45,18:Roll90Yaw90,19:Roll90Yaw135,20:Roll270,21:Roll270Yaw45,22:Roll270Yaw90,23:Roll270Yaw135,24:Pitch90,25:Pitch270,26:Pitch180Yaw90,27:Pitch180Yaw270,28:Roll90Pitch90,29:Roll180Pitch90,30:Roll270Pitch90,31:Roll90Pitch180,32:Roll270Pitch180,33:Roll90Pitch270,34:Roll180Pitch270,35:Roll270Pitch270,36:Roll90Pitch180Yaw90,37:Roll90Yaw270,38:Yaw293Pitch68Roll180,39:Pitch315,40:Roll90Pitch315,100:Custom // @User: Advanced AP_GROUPINFO("ORIENT2", 19, Compass, _state._priv_instance[1].orientation, ROTATION_NONE), @@ -332,7 +333,7 @@ const AP_Param::GroupInfo Compass::var_info[] = { // @Param: ORIENT3 // @DisplayName: Compass3 orientation // @Description: The orientation of a third external compass relative to the vehicle frame. This value will be ignored unless this compass is set as an external compass. When set correctly in the northern hemisphere, pointing the nose and right side down should increase the MagX and MagY values respectively. Rolling the vehicle upside down should decrease the MagZ value. For southern hemisphere, switch increase and decrease. NOTE: For internal compasses, AHRS_ORIENT is used. - // @Values: 0:None,1:Yaw45,2:Yaw90,3:Yaw135,4:Yaw180,5:Yaw225,6:Yaw270,7:Yaw315,8:Roll180,9:Roll180Yaw45,10:Roll180Yaw90,11:Roll180Yaw135,12:Pitch180,13:Roll180Yaw225,14:Roll180Yaw270,15:Roll180Yaw315,16:Roll90,17:Roll90Yaw45,18:Roll90Yaw90,19:Roll90Yaw135,20:Roll270,21:Roll270Yaw45,22:Roll270Yaw90,23:Roll270Yaw135,24:Pitch90,25:Pitch270,26:Pitch180Yaw90,27:Pitch180Yaw270,28:Roll90Pitch90,29:Roll180Pitch90,30:Roll270Pitch90,31:Roll90Pitch180,32:Roll270Pitch180,33:Roll90Pitch270,34:Roll180Pitch270,35:Roll270Pitch270,36:Roll90Pitch180Yaw90,37:Roll90Yaw270,38:Yaw293Pitch68Roll180,39:Pitch315,40:Roll90Pitch315 + // @Values: 0:None,1:Yaw45,2:Yaw90,3:Yaw135,4:Yaw180,5:Yaw225,6:Yaw270,7:Yaw315,8:Roll180,9:Roll180Yaw45,10:Roll180Yaw90,11:Roll180Yaw135,12:Pitch180,13:Roll180Yaw225,14:Roll180Yaw270,15:Roll180Yaw315,16:Roll90,17:Roll90Yaw45,18:Roll90Yaw90,19:Roll90Yaw135,20:Roll270,21:Roll270Yaw45,22:Roll270Yaw90,23:Roll270Yaw135,24:Pitch90,25:Pitch270,26:Pitch180Yaw90,27:Pitch180Yaw270,28:Roll90Pitch90,29:Roll180Pitch90,30:Roll270Pitch90,31:Roll90Pitch180,32:Roll270Pitch180,33:Roll90Pitch270,34:Roll180Pitch270,35:Roll270Pitch270,36:Roll90Pitch180Yaw90,37:Roll90Yaw270,38:Yaw293Pitch68Roll180,39:Pitch315,40:Roll90Pitch315,100:Custom // @User: Advanced AP_GROUPINFO("ORIENT3", 22, Compass, _state._priv_instance[2].orientation, ROTATION_NONE), @@ -508,12 +509,14 @@ const AP_Param::GroupInfo Compass::var_info[] = { // @Param: PRIO1_ID // @DisplayName: Compass device id with 1st order priority // @Description: Compass device id with 1st order priority, set automatically if 0. Reboot required after change. + // @RebootRequired: True // @User: Advanced AP_GROUPINFO("PRIO1_ID", 36, Compass, _priority_did_stored_list._priv_instance[0], 0), // @Param: PRIO2_ID // @DisplayName: Compass device id with 2nd order priority // @Description: Compass device id with 2nd order priority, set automatically if 0. Reboot required after change. + // @RebootRequired: True // @User: Advanced AP_GROUPINFO("PRIO2_ID", 37, Compass, _priority_did_stored_list._priv_instance[1], 0), #endif // COMPASS_MAX_INSTANCES @@ -522,6 +525,7 @@ const AP_Param::GroupInfo Compass::var_info[] = { // @Param: PRIO3_ID // @DisplayName: Compass device id with 3rd order priority // @Description: Compass device id with 3rd order priority, set automatically if 0. Reboot required after change. + // @RebootRequired: True // @User: Advanced AP_GROUPINFO("PRIO3_ID", 38, Compass, _priority_did_stored_list._priv_instance[2], 0), #endif // COMPASS_MAX_INSTANCES @@ -610,6 +614,37 @@ const AP_Param::GroupInfo Compass::var_info[] = { AP_GROUPINFO("DEV_ID8", 48, Compass, extra_dev_id[4], 0), #endif // COMPASS_MAX_UNREG_DEV +#if !APM_BUILD_TYPE(APM_BUILD_AP_Periph) + // @Param: CUS_ROLL + // @DisplayName: Custom orientation roll offset + // @Description: Compass mounting position roll offset. Positive values = roll right, negative values = roll left. This parameter is only used when COMPASS_ORIENT/2/3 is set to CUSTOM. + // @Range: -180 180 + // @Units: deg + // @Increment: 1 + // @RebootRequired: True + // @User: Advanced + AP_GROUPINFO("CUS_ROLL", 49, Compass, _custom_roll, 0), + + // @Param: CUS_PIT + // @DisplayName: Custom orientation pitch offset + // @Description: Compass mounting position pitch offset. Positive values = pitch up, negative values = pitch down. This parameter is only used when COMPASS_ORIENT/2/3 is set to CUSTOM. + // @Range: -180 180 + // @Units: deg + // @Increment: 1 + // @RebootRequired: True + // @User: Advanced + AP_GROUPINFO("CUS_PIT", 50, Compass, _custom_pitch, 0), + + // @Param: CUS_YAW + // @DisplayName: Custom orientation yaw offset + // @Description: Compass mounting position yaw offset. Positive values = yaw right, negative values = yaw left. This parameter is only used when COMPASS_ORIENT/2/3 is set to CUSTOM. + // @Range: -180 180 + // @Units: deg + // @Increment: 1 + // @RebootRequired: True + // @User: Advanced + AP_GROUPINFO("CUS_YAW", 51, Compass, _custom_yaw, 0), +#endif AP_GROUPEND }; diff --git a/libraries/AP_Compass/AP_Compass.h b/libraries/AP_Compass/AP_Compass.h index 93a277c9b8..3b8582f7de 100644 --- a/libraries/AP_Compass/AP_Compass.h +++ b/libraries/AP_Compass/AP_Compass.h @@ -433,6 +433,7 @@ private: // board orientation from AHRS enum Rotation _board_orientation = ROTATION_NONE; + // custom rotation matrix Matrix3f* _custom_rotation; // declination in radians @@ -453,6 +454,11 @@ private: // automatic compass orientation on calibration AP_Int8 _rotate_auto; + + // custom compass rotation + AP_Float _custom_roll; + AP_Float _custom_pitch; + AP_Float _custom_yaw; // throttle expressed as a percentage from 0 ~ 1.0, used for motor compensation float _thr; diff --git a/libraries/AP_Compass/AP_Compass_Backend.cpp b/libraries/AP_Compass/AP_Compass_Backend.cpp index 78e28d4496..48b55bb2ee 100644 --- a/libraries/AP_Compass/AP_Compass_Backend.cpp +++ b/libraries/AP_Compass/AP_Compass_Backend.cpp @@ -4,6 +4,7 @@ #include "AP_Compass_Backend.h" #include +#include extern const AP_HAL::HAL& hal; @@ -29,7 +30,16 @@ void AP_Compass_Backend::rotate_field(Vector3f &mag, uint8_t instance) } } else { // add user selectable orientation +#if !APM_BUILD_TYPE(APM_BUILD_AP_Periph) + Rotation rotation = Rotation(state.orientation.get()); + if (rotation == ROTATION_CUSTOM && _compass._custom_rotation) { + mag = *_compass._custom_rotation * mag; + } else { + mag.rotate(rotation); + } +#else mag.rotate((enum Rotation)state.orientation.get()); +#endif } } @@ -222,6 +232,13 @@ bool AP_Compass_Backend::is_external(uint8_t instance) void AP_Compass_Backend::set_rotation(uint8_t instance, enum Rotation rotation) { _compass._state[Compass::StateIndex(instance)].rotation = rotation; +#if !APM_BUILD_TYPE(APM_BUILD_AP_Periph) + // lazily create the custom rotation matrix + if (!_compass._custom_rotation && Rotation(_compass._state[Compass::StateIndex(instance)].orientation.get()) == ROTATION_CUSTOM) { + _compass._custom_rotation = new Matrix3f(); + _compass._custom_rotation->from_euler(radians(_compass._custom_roll), radians(_compass._custom_pitch), radians(_compass._custom_yaw)); + } +#endif } static constexpr float FILTER_KOEF = 0.1f;