Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Add support for custom compass rotation via COMPASS_CUS_{ROLL,PIT,YAW} #13586

Merged
merged 1 commit into from May 4, 2020
Merged

Add support for custom compass rotation via COMPASS_CUS_{ROLL,PIT,YAW} #13586

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
41 changes: 38 additions & 3 deletions libraries/AP_Compass/AP_Compass.cpp
View file
Open in desktop
Expand Up @@ -5,6 +5,7 @@
#include <AP_Vehicle/AP_Vehicle.h>
#include <AP_BoardConfig/AP_BoardConfig.h>
#include <AP_Logger/AP_Logger.h>
#include <AP_Vehicle/AP_Vehicle_Type.h>

#include "AP_Compass_SITL.h"
#include "AP_Compass_AK8963.h"
Expand Down Expand Up @@ -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),

Expand Down Expand Up @@ -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),

Expand All @@ -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),

Expand Down Expand Up @@ -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
Expand All @@ -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
Expand Down Expand Up @@ -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
};

Expand Down
6 changes: 6 additions & 0 deletions libraries/AP_Compass/AP_Compass.h
View file
Open in desktop
Expand Up @@ -433,6 +433,7 @@ friend class AP_Compass_Backend;

// board orientation from AHRS
enum Rotation _board_orientation = ROTATION_NONE;
// custom rotation matrix
Matrix3f* _custom_rotation;

// declination in radians
Expand All @@ -453,6 +454,11 @@ friend class AP_Compass_Backend;

// 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;
Expand Down
17 changes: 17 additions & 0 deletions libraries/AP_Compass/AP_Compass_Backend.cpp
View file
Open in desktop
Expand Up @@ -4,6 +4,7 @@
#include "AP_Compass_Backend.h"

#include <AP_BattMonitor/AP_BattMonitor.h>
#include <AP_Vehicle/AP_Vehicle_Type.h>

extern const AP_HAL::HAL& hal;

Expand All @@ -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
}
}

Expand Down Expand Up @@ -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;
Expand Down