| 0,0 → 1,301 |
|---|
| /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- |
| #include "Compass.h" |
| |
| // Default constructor. |
| // Note that the Vector/Matrix constructors already implicitly zero |
| // their values. |
| // |
| Compass::Compass(void) : |
| product_id(AP_COMPASS_TYPE_UNKNOWN), |
| _declination (0.0), |
| _learn(1), |
| _use_for_yaw(1), |
| _null_enable(false), |
| _null_init_done(false), |
| _auto_declination(1), |
| _orientation(ROTATION_NONE) |
| { |
| } |
| |
| // Default init method, just returns success. |
| // |
| bool |
| Compass::init() |
| { |
| return true; |
| } |
| |
| /* set_orientation |
| void |
| Compass::set_orientation(enum Rotation rotation) |
| { |
| _orientation = rotation; |
| } |
| */ |
| |
| /* set_offsets |
| void |
| Compass::set_offsets(const Vector3f &offsets) |
| { |
| _offset.set(offsets); |
| } |
| */ |
| |
| /* save_offsets |
| void |
| Compass::save_offsets() |
| { |
| _offset.save(); |
| } |
| */ |
| |
| /* get_offsets |
| Vector3f & |
| Compass::get_offsets() |
| { |
| return _offset; |
| } |
| */ |
| |
| /* set_initial_location |
| void |
| Compass::set_initial_location(long latitude, long longitude) |
| { |
| // if automatic declination is configured, then compute |
| // the declination based on the initial GPS fix |
| if (_auto_declination) { |
| // Set the declination based on the lat/lng from GPS |
| null_offsets_disable(); |
| _declination.set(radians(AP_Declination::get_declination((float)latitude / 10000000, (float)longitude / 10000000))); |
| null_offsets_enable(); |
| } |
| } |
| */ |
| |
| /* set_declination |
| void |
| Compass::set_declination(float radians) |
| { |
| _declination.set_and_save(radians); |
| } |
| */ |
| |
| /* get_declination |
| float |
| Compass::get_declination() |
| { |
| return _declination.get(); |
| } |
| */ |
| |
| /* calculate |
| void |
| Compass::calculate(float roll, float pitch) |
| { |
| // Note - This function implementation is deprecated |
| // The alternate implementation of this function using the dcm matrix is preferred |
| float headX; |
| float headY; |
| float cos_roll; |
| float sin_roll; |
| float cos_pitch; |
| float sin_pitch; |
| cos_roll = cos(roll); |
| sin_roll = sin(roll); |
| cos_pitch = cos(pitch); |
| sin_pitch = sin(pitch); |
| |
| // Tilt compensated magnetic field X component: |
| headX = mag_x*cos_pitch + mag_y*sin_roll*sin_pitch + mag_z*cos_roll*sin_pitch; |
| // Tilt compensated magnetic field Y component: |
| headY = mag_y*cos_roll - mag_z*sin_roll; |
| // magnetic heading |
| heading = atan2(-headY,headX); |
| |
| // Declination correction (if supplied) |
| if( fabs(_declination) > 0.0 ) |
| { |
| heading = heading + _declination; |
| if (heading > M_PI) // Angle normalization (-180 deg, 180 deg) |
| heading -= (2.0 * M_PI); |
| else if (heading < -M_PI) |
| heading += (2.0 * M_PI); |
| } |
| |
| // Optimization for external DCM use. Calculate normalized components |
| heading_x = cos(heading); |
| heading_y = sin(heading); |
| } |
| */ |
| |
| void |
| Compass::calculate(const Matrix3f &dcm_matrix) |
| { |
| float headX; |
| float headY; |
| float cos_pitch = safe_sqrt(1-(dcm_matrix.c.x*dcm_matrix.c.x)); |
| // sin(pitch) = - dcm_matrix(3,1) |
| // cos(pitch)*sin(roll) = - dcm_matrix(3,2) |
| // cos(pitch)*cos(roll) = - dcm_matrix(3,3) |
| |
| if (cos_pitch == 0.0) { |
| // we are pointing straight up or down so don't update our |
| // heading using the compass. Wait for the next iteration when |
| // we hopefully will have valid values again. |
| return; |
| } |
| |
| // Tilt compensated magnetic field X component: |
| headX = mag_x*cos_pitch - mag_y*dcm_matrix.c.y*dcm_matrix.c.x/cos_pitch - mag_z*dcm_matrix.c.z*dcm_matrix.c.x/cos_pitch; |
| // Tilt compensated magnetic field Y component: |
| headY = mag_y*dcm_matrix.c.z/cos_pitch - mag_z*dcm_matrix.c.y/cos_pitch; |
| // magnetic heading |
| // 6/4/11 - added constrain to keep bad values from ruining DCM Yaw - Jason S. |
| heading = constrain(atan2(-headY,headX), -3.15, 3.15); |
| |
| // Declination correction (if supplied) |
| if( fabs(_declination) > 0.0 ) |
| { |
| heading = heading + _declination; |
| if (heading > M_PI) // Angle normalization (-180 deg, 180 deg) |
| heading -= (2.0 * M_PI); |
| else if (heading < -M_PI) |
| heading += (2.0 * M_PI); |
| } |
| |
| // Optimization for external DCM use. Calculate normalized components |
| heading_x = cos(heading); |
| heading_y = sin(heading); |
| |
| #if 0 |
| if (isnan(heading_x) || isnan(heading_y)) { |
| Serial.printf("COMPASS: c.x %f c.y %f c.z %f cos_pitch %f mag_x %d mag_y %d mag_z %d headX %f headY %f heading %f heading_x %f heading_y %f\n", |
| dcm_matrix.c.x, |
| dcm_matrix.c.y, |
| dcm_matrix.c.x, |
| cos_pitch, |
| (int)mag_x, (int)mag_y, (int)mag_z, |
| headX, headY, |
| heading, |
| heading_x, heading_y); |
| } |
| #endif |
| } |
| |
| |
| /* |
| 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. |
| */ |
| /* null_offsets |
| void |
| Compass::null_offsets(void) |
| { |
| if (_null_enable == false || _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; |
| Vector3f ofs; |
| |
| ofs = _offset.get(); |
| |
| if (!_null_init_done) { |
| // first time through |
| _null_init_done = true; |
| for (uint8_t i=0; i<_mag_history_size; i++) { |
| // fill the history buffer with the current mag vector, |
| // with the offset removed |
| _mag_history[i] = Vector3i((mag_x+0.5) - ofs.x, (mag_y+0.5) - ofs.y, (mag_z+0.5) - ofs.z); |
| } |
| _mag_history_index = 0; |
| return; |
| } |
| |
| Vector3f b1, b2, diff; |
| float length; |
| |
| // get a past element |
| b1 = Vector3f(_mag_history[_mag_history_index].x, |
| _mag_history[_mag_history_index].y, |
| _mag_history[_mag_history_index].z); |
| // the history buffer doesn't have the offsets |
| b1 += ofs; |
| |
| // get the current vector |
| b2 = Vector3f(mag_x, mag_y, mag_z); |
| |
| // calculate the delta for this sample |
| diff = b2 - b1; |
| length = diff.length(); |
| if (length < min_diff) { |
| // the mag vector hasn't changed enough - we don't get |
| // enough information from this vector to use it. |
| // Note that we don't put the current vector into the mag |
| // history here. We want to wait for a larger rotation to |
| // build up before calculating an offset change, as accuracy |
| // of the offset change is highly dependent on the size of the |
| // rotation. |
| _mag_history_index = (_mag_history_index + 1) % _mag_history_size; |
| return; |
| } |
| |
| // put the vector in the history |
| _mag_history[_mag_history_index] = Vector3i((mag_x+0.5) - ofs.x, (mag_y+0.5) - ofs.y, (mag_z+0.5) - ofs.z); |
| _mag_history_index = (_mag_history_index + 1) % _mag_history_size; |
| |
| // equation 6 of Bills paper |
| diff = diff * (gain * (b2.length() - b1.length()) / length); |
| |
| // limit the change from any one reading. This is to prevent |
| // single crazy readings from throwing off the offsets for a long |
| // time |
| length = diff.length(); |
| if (length > max_change) { |
| diff *= max_change / length; |
| } |
| |
| // set the new offsets |
| _offset.set(_offset.get() - diff); |
| } |
| */ |
| |
| // Have no idea why this is necessary: |
| bool Compass::read(void) { |
| return false; |
| } |
| |
| void |
| Compass::null_offsets_enable(void) |
| { |
| _null_init_done = false; |
| _null_enable = true; |
| } |
| |
| void |
| Compass::null_offsets_disable(void) |
| { |
| _null_init_done = false; |
| _null_enable = false; |
| } |