// Update raw magnetometer values from HIL data
//
void Compass::setHIL(uint8_t instance, float roll, float pitch, float yaw)
{
Matrix3f R;
// create a rotation matrix for the given attitude
R.from_euler(roll, pitch, yaw);
if (!is_equal(_hil.last_declination,get_declination())) {
_setup_earth_field();
_hil.last_declination = get_declination();
}
// convert the earth frame magnetic vector to body frame, and
// apply the offsets
_hil.field[instance] = R.mul_transpose(_hil.Bearth);
// apply default board orientation for this compass type. This is
// a noop on most boards
_hil.field[instance].rotate(MAG_BOARD_ORIENTATION);
// add user selectable orientation
_hil.field[instance].rotate((enum Rotation)_state[0].orientation.get());
if (!_state[0].external) {
// and add in AHRS_ORIENTATION setting if not an external compass
_hil.field[instance].rotate(_board_orientation);
}
_hil.healthy[instance] = true;
}
void setup(void)
{
float declination, declination_test;
uint16_t pass = 0, fail = 0;
uint32_t total_time=0;
hal.console->print("Beginning Test. Please wait...\n");
for(int16_t i = -90; i <= 90; i+=5)
{
for(int16_t j = -180; j <= 180; j+=5)
{
uint32_t t1 = hal.scheduler->micros();
declination = AP_Declination::get_declination(i, j);
total_time += hal.scheduler->micros() - t1;
declination_test = get_declination(i, j);
if(declination == declination_test)
{
//Serial.printf("Pass: %i, %i : %f, %f\n", i, j, declination, declination_test);
pass++;
}
else
{
hal.console->printf("FAIL: %i, %i : %f, %f\n", i, j, declination, declination_test);
fail++;
}
}
}
hal.console->print("Ending Test.\n\n");
hal.console->printf("Total Pass: %i\n", pass);
hal.console->printf("Total Fail: %i\n", fail);
hal.console->printf("Average time per call: %.1f usec\n",
total_time/(float)(pass+fail));
}
// setup _Bearth
void Compass::_setup_earth_field(void)
{
// assume a earth field strength of 400
_hil.Bearth(400, 0, 0);
// rotate _Bearth for inclination and declination. -66 degrees
// is the inclination in Canberra, Australia
Matrix3f R;
R.from_euler(0, ToRad(66), get_declination());
_hil.Bearth = R * _hil.Bearth;
}
void interface_hmc5883l_i2c_set_initial_location(s32 latitude, s32 longitude)
{
// Set the declination based on the lat/lng from GPS
hmc5883_declination = radians(get_declination((float)latitude / 10000000,(float)longitude / 10000000));
}