// 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;
}
// Update raw magnetometer values from HIL data
//
void AP_Compass_HIL::setHIL(float roll, float pitch, float yaw)
{
Matrix3f R;
// create a rotation matrix for the given attitude
R.from_euler(roll, pitch, yaw);
if (_last_declination != _declination.get()) {
_setup_earth_field();
_last_declination = _declination.get();
}
// convert the earth frame magnetic vector to body frame, and
// apply the offsets
_hil_mag = R.mul_transpose(_Bearth);
_hil_mag -= Vector3f(MAG_OFS_X, MAG_OFS_Y, MAG_OFS_Z);
// apply default board orientation for this compass type. This is
// a noop on most boards
_hil_mag.rotate(MAG_BOARD_ORIENTATION);
// add user selectable orientation
_hil_mag.rotate((enum Rotation)_orientation.get());
// and add in AHRS_ORIENTATION setting
_hil_mag.rotate(_board_orientation);
healthy = true;
}
// constructor
AP_Compass_HIL::AP_Compass_HIL(AP_Compass &_compass):
AP_Compass_Backend(_compass)
{
compass.product_id = AP_COMPASS_TYPE_HIL;
hal.console->println("HIL");
_setup_earth_field();
}
// constructor
AP_Compass_HIL::AP_Compass_HIL() : Compass()
{
product_id = AP_COMPASS_TYPE_HIL;
_setup_earth_field();
}
