mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 14:38:30 -04:00
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
This commit is contained in:
Andy Piper
Andrew Tridgell
parent
167e1d12d7
commit
b7b2d9a6ce
@ -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"
|
||||
@ -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
|
||||
};
|
||||
|
||||
|
||||
@ -433,6 +433,7 @@ private:
|
||||
|
||||
// board orientation from AHRS
|
||||
enum Rotation _board_orientation = ROTATION_NONE;
|
||||
// custom rotation matrix
|
||||
Matrix3f* _custom_rotation;
|
||||
|
||||
// declination in radians
|
||||
@ -454,6 +455,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;
|
||||
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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;
|
||||
|
||||
Loading…
Reference in New Issue
Block a user