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.

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

fail to calibrate QMC5883L module #1

Open
mkeyno opened this issue May 24, 2018 · 0 comments
Open

fail to calibrate QMC5883L module #1

mkeyno opened this issue May 24, 2018 · 0 comments

Comments

@mkeyno
Copy link

mkeyno commented May 24, 2018

hi buddy , sorry to pen ticket for my question , but actually I’ve been trying to install Chinese compass QMC5883L (instead of HMC5843 ) to my Arducopter , I ran compos test sketch in the library and notice it didn’t initialize. After tracking the issue I’ve notice init() function do not work correctly because it’s return unsuccessful flag for rest of operation, then I’ve notice there are following parameters which is base on well known HMC5843 compass whereas its not compatible with this Chinese compass

uint16_t expected_x = 715; 
uint16_t expected_yz = 715; 
float gain_multiple = 1.0; 
float calibration[3];

and this is ini() function in the driver lib

bool AP_Compass_QMC5883L::init()
{
    
	int numAttempts = 0, good_count = 0;
    bool success = false;
    
    uint16_t expected_x = 715;
    uint16_t expected_yz = 715;
    float gain_multiple = 1.0;
	
  if (!write_register(ConfigRegB,0x80)) return false; //softReset  must reset first         
  hal.scheduler->delay(10);
       write_register(0x0B, 0x01);//SET/RESET Period
  
    _i2c_sem = hal.i2c->get_semaphore();
    if (!_i2c_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER))  hal.scheduler->panic(PSTR("Failed to get QMC5883L semaphore")); 
    // determine if we are using 5843 or 5883L
    _base_config = BaseReg;
    if (  !write_register(ConfigRegA, Mode_Continuous | ODR_200Hz | Mode_Continuous| RNG_8G | OSR_512 ) 
	//	|| !read_register(ConfigRegA, &_base_config)  
	) 
	{
        _healthy[0] = false;
        _i2c_sem->give();
        return false;
    }
	
    
        product_id = AP_COMPASS_TYPE_QMC5883L;
       
        /*
          note that the HMC5883 datasheet gives the x and y expected
          values as 766 and the z as 713. Experiments have shown the x
          axis is around 766, and the y and z closer to 713.
         */
        expected_x = 766;
        expected_yz  = 713;
        gain_multiple = 660.0 / 1090;  // adjustment for runtime vs calibration gain
    

    calibration[0] = 0;
    calibration[1] = 0;
    calibration[2] = 0;

    while ( success == 0 && numAttempts < 25 && good_count < 5)
    {
        // record number of attempts at initialisation
        numAttempts++;
        // read values from the compass
        hal.scheduler->delay(50);
        if (!read_raw())  continue; // we didn't read valid values     Fill _mag_x _mag_y _mag_z       

        hal.scheduler->delay(10);

        float cal[3];

        //   hal.console->printf_P(PSTR("%d) mag: %d - %d - %d \n"),numAttempts, _mag_x, _mag_y, _mag_z);
        cal[0] = fabsf(expected_x / (float)_mag_x);
        cal[1] = fabsf(expected_yz / (float)_mag_y);
        cal[2] = fabsf(expected_yz / (float)_mag_z);

        //  hal.console->printf_P(PSTR(" cal=%.2f   %.2f   %.2f\n"),numAttempts, cal[0], cal[1], cal[2]);

        // we throw away the first two samples as the compass may still be changing its state from the application of the
        // strap excitation. After that we accept values in a reasonable range
        if (numAttempts > 2 &&
            cal[0] > 0.7f && cal[0] < 1.35f &&
            cal[1] > 0.7f && cal[1] < 1.35f &&
            cal[2] > 0.7f && cal[2] < 1.35f) 
		{
            //  hal.console->printf_P(PSTR("  good\n") );
            good_count++;
            calibration[0] += cal[0];
            calibration[1] += cal[1];
            calibration[2] += cal[2];
        }

#if 0
        /* useful for debugging */
        hal.console->printf_P(PSTR("MagX: %d MagY: %d MagZ: %d\n"), (int)_mag_x, (int)_mag_y, (int)_mag_z);
        hal.console->printf_P(PSTR("CalX: %.2f CalY: %.2f CalZ: %.2f\n"), cal[0], cal[1], cal[2]);
#endif
    }

	//hal.console->printf_P(PSTR("good_count: %d  \n"),good_count);
    if (good_count >= 5)
						{
							/*
							  The use of gain_multiple below is incorrect, as the gain
							  difference between 2.5Ga mode and 1Ga mode is already taken
							  into account by the expected_x and expected_yz values.  We
							  are not going to fix it however as it would mean all
							  APM1/APM2 users redoing their compass calibration. The
							  impact is that the values we report on APM1/APM2 are lower
							  than they should be (by a multiple of about 0.6). This
							  doesn't have any impact other than the learned compass
							  offsets
							 */
							calibration[0] = calibration[0] * gain_multiple / good_count;
							calibration[1] = calibration[1] * gain_multiple / good_count;
							calibration[2] = calibration[2] * gain_multiple / good_count;
							success = true;
						} 
	else
			{
				/* best guess */
				calibration[0] = 1.0;
				calibration[1] = 1.0;
				calibration[2] = 1.0;
			}

    // leave test mode
    if (!re_initialise()) 
	{
        _i2c_sem->give();
        return false;
    }

    _i2c_sem->give();
    _initialised = true;

	// perform an initial read
	_healthy[0] = true;
	read();

#if 0
    hal.console->printf_P(PSTR("CalX: %.2f CalY: %.2f CalZ: %.2f\n"), calibration[0], calibration[1], calibration[2]);
#endif

    return true;//success;
}

so I’ve bypassed it in the way that init() function always return successful flag & calibration[3] array is always equal to 1, then compile and upload the firmware into hardware, but in the end I received compass error that says my compass too bios or fail , I repeated calibration procedure couple of times but the drone didn’t arm so I’m guessing I kinda need to consider calibration into account which is fundamentally need to know correct initial value, unfortunately data sheet didn't be helpful and I don't know how to guess the initial value of expected_x, expected_yz & gain_multiple, so I really appreciated to have your comments about that
full driver lib located here
https://github.com/mkeyno/AP_Compass_QMC5883L/blob/master/AP_Compass_QMC5883L.cpp#L150-L282

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

No branches or pull requests

1 participant