/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- #include #include "Compass.h" const AP_Param::GroupInfo Compass::var_info[] PROGMEM = { // index 0 was used for the old orientation matrix // @Param: OFS_X // @DisplayName: Compass offsets on the X axis // @Description: Offset to be added to the compass x-axis values to compensate for metal in the frame // @Range: -400 400 // @Increment: 1 // @Param: OFS_Y // @DisplayName: Compass offsets on the Y axis // @Description: Offset to be added to the compass y-axis values to compensate for metal in the frame // @Range: -400 400 // @Increment: 1 // @Param: OFS_Z // @DisplayName: Compass offsets on the Z axis // @Description: Offset to be added to the compass z-axis values to compensate for metal in the frame // @Range: -400 400 // @Increment: 1 AP_GROUPINFO("OFS", 1, Compass, _offset[0], 0), // @Param: DEC // @DisplayName: Compass declination // @Description: An angle to compensate between the true north and magnetic north // @Range: -3.142 3.142 // @Units: Radians // @Increment: 0.01 // @User: Standard AP_GROUPINFO("DEC", 2, Compass, _declination, 0), // @Param: LEARN // @DisplayName: Learn compass offsets automatically // @Description: Enable or disable the automatic learning of compass offsets // @Values: 0:Disabled,1:Enabled // @User: Advanced AP_GROUPINFO("LEARN", 3, Compass, _learn, 1), // true if learning calibration // @Param: USE // @DisplayName: Use compass for yaw // @Description: Enable or disable the use of the compass (instead of the GPS) for determining heading // @Values: 0:Disabled,1:Enabled // @User: Advanced AP_GROUPINFO("USE", 4, Compass, _use_for_yaw, 1), // true if used for DCM yaw #if !defined( __AVR_ATmega1280__ ) // @Param: AUTODEC // @DisplayName: Auto Declination // @Description: Enable or disable the automatic calculation of the declination based on gps location // @Values: 0:Disabled,1:Enabled // @User: Advanced AP_GROUPINFO("AUTODEC",5, Compass, _auto_declination, 1), #endif // @Param: MOTCT // @DisplayName: Motor interference compensation type // @Description: Set motor interference compensation type to disabled, throttle or current. Do not change manually. // @Values: 0:Disabled,1:Use Throttle,2:Use Current // @Increment: 1 AP_GROUPINFO("MOTCT", 6, Compass, _motor_comp_type, AP_COMPASS_MOT_COMP_DISABLED), // @Param: MOT_X // @DisplayName: Motor interference compensation for body frame X axis // @Description: Multiplied by the current throttle and added to the compass's x-axis values to compensate for motor interference // @Range: -1000 1000 // @Units: Offset per Amp or at Full Throttle // @Increment: 1 // @Param: MOT_Y // @DisplayName: Motor interference compensation for body frame Y axis // @Description: Multiplied by the current throttle and added to the compass's y-axis values to compensate for motor interference // @Range: -1000 1000 // @Units: Offset per Amp or at Full Throttle // @Increment: 1 // @Param: MOT_Z // @DisplayName: Motor interference compensation for body frame Z axis // @Description: Multiplied by the current throttle and added to the compass's z-axis values to compensate for motor interference // @Range: -1000 1000 // @Units: Offset per Amp or at Full Throttle // @Increment: 1 AP_GROUPINFO("MOT", 7, Compass, _motor_compensation[0], 0), // @Param: ORIENT // @DisplayName: Compass orientation // @Description: The orientation of the compass relative to the autopilot board. This will default to the right value for each board type, but can be changed if you have an external compass. See the documentation for your external compass for the right value. The correct orientation should give the X axis forward, the Y axis to the right and the Z axis down. So if your aircraft is pointing west it should show a positive value for the Y axis, and a value close to zero for the X axis. NOTE: This orientation is combined with any AHRS_ORIENTATION setting. // @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:Roll270Yaw136,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 AP_GROUPINFO("ORIENT", 8, Compass, _orientation, ROTATION_NONE), // @Param: EXTERNAL // @DisplayName: Compass is attached via an external cable // @Description: Configure compass so it is attached externally. This is auto-detected on PX4, but must be set correctly on an APM2. Set to 1 if the compass is externally connected. When externally connected the COMPASS_ORIENT option operates independently of the AHRS_ORIENTATION board orientation option // @Values: 0:Internal,1:External // @User: Advanced AP_GROUPINFO("EXTERNAL", 9, Compass, _external, 0), #if COMPASS_MAX_INSTANCES > 1 AP_GROUPINFO("OFS2", 10, Compass, _offset[1], 0), AP_GROUPINFO("MOT2", 11, Compass, _motor_compensation[1], 0), #endif AP_GROUPEND }; // Default constructor. // Note that the Vector/Matrix constructors already implicitly zero // their values. // Compass::Compass(void) : product_id(AP_COMPASS_TYPE_UNKNOWN), _null_init_done(false) { AP_Param::setup_object_defaults(this, var_info); } // Default init method, just returns success. // bool Compass::init() { return true; } void Compass::set_offsets(const Vector3f &offsets) { _offset[0].set(offsets); } void Compass::save_offsets() { for (uint8_t k=0; k 0.0f ) { heading = heading + _declination; if (heading > PI) // Angle normalization (-180 deg, 180 deg) heading -= (2.0f * PI); else if (heading < -PI) heading += (2.0f * PI); } return heading; } /* * this offset nulling algorithm is inspired by this paper from Bill Premerlani * * http://gentlenav.googlecode.com/files/MagnetometerOffsetNullingRevisited.pdf * * The base algorithm works well, but is quite sensitive to * noise. After long discussions with Bill, the following changes were * made: * * 1) we keep a history buffer that effectively divides the mag * vectors into a set of N streams. The algorithm is run on the * streams separately * * 2) within each stream we only calculate a change when the mag * vector has changed by a significant amount. * * This gives us the property that we learn quickly if there is no * noise, but still learn correctly (and slowly) in the face of lots of * noise. */ void Compass::null_offsets(void) { if (_learn == 0) { // auto-calibration is disabled return; } // this gain is set so we converge on the offsets in about 5 // minutes with a 10Hz compass const float gain = 0.01; const float max_change = 10.0; const float min_diff = 50.0; if (!_null_init_done) { // first time through _null_init_done = true; for (uint8_t k=0; k max_change) { diff *= max_change / length; } // set the new offsets _offset[k].set(_offset[k].get() - diff); } }