From 209ad965be6d75981058d0879bd9d7f51c365ecf Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Fri, 5 Nov 2021 16:11:09 +0000 Subject: [PATCH] AP_Compass: remove custom rotations --- libraries/AP_Compass/AP_Compass.cpp | 14 +++++------ libraries/AP_Compass/AP_Compass.h | 14 +---------- libraries/AP_Compass/AP_Compass_Backend.cpp | 25 +------------------ .../AP_Compass/AP_Compass_Calibration.cpp | 2 +- libraries/AP_Compass/AP_Compass_SITL.cpp | 7 +----- 5 files changed, 10 insertions(+), 52 deletions(-) diff --git a/libraries/AP_Compass/AP_Compass.cpp b/libraries/AP_Compass/AP_Compass.cpp index f746177508..9a087c70a7 100644 --- a/libraries/AP_Compass/AP_Compass.cpp +++ b/libraries/AP_Compass/AP_Compass.cpp @@ -161,7 +161,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. The label for each option is specified in the order of rotations for that orientation. - // @Values: 0:None,1:Yaw45,2:Yaw90,3:Yaw135,4:Yaw180,5:Yaw225,6:Yaw270,7:Yaw315,8:Roll180,9:Yaw45Roll180,10:Yaw90Roll180,11:Yaw135Roll180,12:Pitch180,13:Yaw225Roll180,14:Yaw270Roll180,15:Yaw315Roll180,16:Roll90,17:Yaw45Roll90,18:Yaw90Roll90,19:Yaw135Roll90,20:Roll270,21:Yaw45Roll270,22:Yaw90Roll270,23:Yaw135Roll270,24:Pitch90,25:Pitch270,26:Yaw90Pitch180,27:Yaw270Pitch180,28:Pitch90Roll90,29:Pitch90Roll180,30:Pitch90Roll270,31:Pitch180Roll90,32:Pitch180Roll270,33:Pitch270Roll90,34:Pitch270Roll180,35:Pitch270Roll270,36:Yaw90Pitch180Roll90,37:Yaw270Roll90,38:Yaw293Pitch68Roll180,39:Pitch315,40:Pitch315Roll90,42:Roll45,43:Roll315,100:Custom + // @Values: 0:None,1:Yaw45,2:Yaw90,3:Yaw135,4:Yaw180,5:Yaw225,6:Yaw270,7:Yaw315,8:Roll180,9:Yaw45Roll180,10:Yaw90Roll180,11:Yaw135Roll180,12:Pitch180,13:Yaw225Roll180,14:Yaw270Roll180,15:Yaw315Roll180,16:Roll90,17:Yaw45Roll90,18:Yaw90Roll90,19:Yaw135Roll90,20:Roll270,21:Yaw45Roll270,22:Yaw90Roll270,23:Yaw135Roll270,24:Pitch90,25:Pitch270,26:Yaw90Pitch180,27:Yaw270Pitch180,28:Pitch90Roll90,29:Pitch90Roll180,30:Pitch90Roll270,31:Pitch180Roll90,32:Pitch180Roll270,33:Pitch270Roll90,34:Pitch270Roll180,35:Pitch270Roll270,36:Yaw90Pitch180Roll90,37:Yaw270Roll90,38:Yaw293Pitch68Roll180,39:Pitch315,40:Pitch315Roll90,42:Roll45,43:Roll315,100:Custom 4.1 and older,101:Custom 1,102:Custom 2 // @User: Advanced AP_GROUPINFO("ORIENT", 8, Compass, _state._priv_instance[0].orientation, ROTATION_NONE), @@ -321,7 +321,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,42:Roll45,43:Roll315,100:Custom + // @Values: 0:None,1:Yaw45,2:Yaw90,3:Yaw135,4:Yaw180,5:Yaw225,6:Yaw270,7:Yaw315,8:Roll180,9:Yaw45Roll180,10:Yaw90Roll180,11:Yaw135Roll180,12:Pitch180,13:Yaw225Roll180,14:Yaw270Roll180,15:Yaw315Roll180,16:Roll90,17:Yaw45Roll90,18:Yaw90Roll90,19:Yaw135Roll90,20:Roll270,21:Yaw45Roll270,22:Yaw90Roll270,23:Yaw135Roll270,24:Pitch90,25:Pitch270,26:Yaw90Pitch180,27:Yaw270Pitch180,28:Pitch90Roll90,29:Pitch90Roll180,30:Pitch90Roll270,31:Pitch180Roll90,32:Pitch180Roll270,33:Pitch270Roll90,34:Pitch270Roll180,35:Pitch270Roll270,36:Yaw90Pitch180Roll90,37:Yaw270Roll90,38:Yaw293Pitch68Roll180,39:Pitch315,40:Pitch315Roll90,42:Roll45,43:Roll315,100:Custom 4.1 and older,101:Custom 1,102:Custom 2 // @User: Advanced AP_GROUPINFO("ORIENT2", 19, Compass, _state._priv_instance[1].orientation, ROTATION_NONE), @@ -344,7 +344,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,42:Roll45,43:Roll315,100:Custom + // @Values: 0:None,1:Yaw45,2:Yaw90,3:Yaw135,4:Yaw180,5:Yaw225,6:Yaw270,7:Yaw315,8:Roll180,9:Yaw45Roll180,10:Yaw90Roll180,11:Yaw135Roll180,12:Pitch180,13:Yaw225Roll180,14:Yaw270Roll180,15:Yaw315Roll180,16:Roll90,17:Yaw45Roll90,18:Yaw90Roll90,19:Yaw135Roll90,20:Roll270,21:Yaw45Roll270,22:Yaw90Roll270,23:Yaw135Roll270,24:Pitch90,25:Pitch270,26:Yaw90Pitch180,27:Yaw270Pitch180,28:Pitch90Roll90,29:Pitch90Roll180,30:Pitch90Roll270,31:Pitch180Roll90,32:Pitch180Roll270,33:Pitch270Roll90,34:Pitch270Roll180,35:Pitch270Roll270,36:Yaw90Pitch180Roll90,37:Yaw270Roll90,38:Yaw293Pitch68Roll180,39:Pitch315,40:Pitch315Roll90,42:Roll45,43:Roll315,100:Custom 4.1 and older,101:Custom 1,102:Custom 2 // @User: Advanced AP_GROUPINFO("ORIENT3", 22, Compass, _state._priv_instance[2].orientation, ROTATION_NONE), @@ -632,7 +632,6 @@ 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. @@ -641,7 +640,7 @@ const AP_Param::GroupInfo Compass::var_info[] = { // @Increment: 1 // @RebootRequired: True // @User: Advanced - AP_GROUPINFO("CUS_ROLL", 49, Compass, _custom_roll, 0), + // index 49 // @Param: CUS_PIT // @DisplayName: Custom orientation pitch offset @@ -651,7 +650,7 @@ const AP_Param::GroupInfo Compass::var_info[] = { // @Increment: 1 // @RebootRequired: True // @User: Advanced - AP_GROUPINFO("CUS_PIT", 50, Compass, _custom_pitch, 0), + // index 50 // @Param: CUS_YAW // @DisplayName: Custom orientation yaw offset @@ -661,8 +660,7 @@ const AP_Param::GroupInfo Compass::var_info[] = { // @Increment: 1 // @RebootRequired: True // @User: Advanced - AP_GROUPINFO("CUS_YAW", 51, Compass, _custom_yaw, 0), -#endif + // index 51 AP_GROUPEND }; diff --git a/libraries/AP_Compass/AP_Compass.h b/libraries/AP_Compass/AP_Compass.h index 994ea71832..0acfc7e9e3 100644 --- a/libraries/AP_Compass/AP_Compass.h +++ b/libraries/AP_Compass/AP_Compass.h @@ -237,9 +237,8 @@ public: bool auto_declination_enabled() const { return _auto_declination != 0; } // set overall board orientation - void set_board_orientation(enum Rotation orientation, Matrix3f* custom_rotation = nullptr) { + void set_board_orientation(enum Rotation orientation) { _board_orientation = orientation; - _custom_rotation = custom_rotation; } /// Set the motor compensation type @@ -456,12 +455,6 @@ private: // board orientation from AHRS enum Rotation _board_orientation = ROTATION_NONE; - // custom board rotation matrix - Matrix3f* _custom_rotation; - - // custom external compass rotation matrix - Matrix3f* _custom_external_rotation; - // declination in radians AP_Float _declination; @@ -478,11 +471,6 @@ 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 ae41108221..71bb464bba 100644 --- a/libraries/AP_Compass/AP_Compass_Backend.cpp +++ b/libraries/AP_Compass/AP_Compass_Backend.cpp @@ -22,24 +22,10 @@ void AP_Compass_Backend::rotate_field(Vector3f &mag, uint8_t instance) mag.rotate(state.rotation); if (!state.external) { - // and add in AHRS_ORIENTATION setting if not an external compass - if (_compass._board_orientation == ROTATION_CUSTOM && _compass._custom_rotation) { - mag = *_compass._custom_rotation * mag; - } else { - mag.rotate(_compass._board_orientation); - } + mag.rotate(_compass._board_orientation); } 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_external_rotation) { - mag = *_compass._custom_external_rotation * mag; - } else { - mag.rotate(rotation); - } -#else mag.rotate((enum Rotation)state.orientation.get()); -#endif } } @@ -233,15 +219,6 @@ 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_external_rotation && Rotation(_compass._state[Compass::StateIndex(instance)].orientation.get()) == ROTATION_CUSTOM) { - _compass._custom_external_rotation = new Matrix3f(); - if (_compass._custom_external_rotation) { - _compass._custom_external_rotation->from_euler(radians(_compass._custom_roll), radians(_compass._custom_pitch), radians(_compass._custom_yaw)); - } - } -#endif } static constexpr float FILTER_KOEF = 0.1f; diff --git a/libraries/AP_Compass/AP_Compass_Calibration.cpp b/libraries/AP_Compass/AP_Compass_Calibration.cpp index eb7e2909ca..95d432896c 100644 --- a/libraries/AP_Compass/AP_Compass_Calibration.cpp +++ b/libraries/AP_Compass/AP_Compass_Calibration.cpp @@ -82,7 +82,7 @@ bool Compass::_start_calibration(uint8_t i, bool retry, float delay) if (_rotate_auto) { enum Rotation r = _get_state(prio).external?(enum Rotation)_get_state(prio).orientation.get():ROTATION_NONE; - if (r != ROTATION_CUSTOM) { + if (r < ROTATION_MAX) { _calibrator[prio]->set_orientation(r, _get_state(prio).external, _rotate_auto>=2, _rotate_auto>=3); } } diff --git a/libraries/AP_Compass/AP_Compass_SITL.cpp b/libraries/AP_Compass/AP_Compass_SITL.cpp index 2740163720..7f04ab534e 100644 --- a/libraries/AP_Compass/AP_Compass_SITL.cpp +++ b/libraries/AP_Compass/AP_Compass_SITL.cpp @@ -128,12 +128,7 @@ void AP_Compass_SITL::_timer() Vector3f f = (_eliptical_corr * new_mag_data) - _sitl->mag_ofs[i].get(); // rotate compass f.rotate_inverse((enum Rotation)_sitl->mag_orient[i].get()); - // and add in AHRS_ORIENTATION setting if not an external compass - if (get_board_orientation() == ROTATION_CUSTOM) { - f = _sitl->ahrs_rotation * f; - } else { - f.rotate(get_board_orientation()); - } + f.rotate(get_board_orientation()); // scale the compass to simulate sensor scale factor errors f *= _sitl->mag_scaling[i];