void setup() {
hal.console->println("Compass library test");
if (!compass.init()) {
hal.console->println("compass initialisation failed!");
while (1) ;
}
hal.console->printf("init done - %u compasses detected\n", compass.get_count());
compass.set_and_save_offsets(0,0,0,0); // set offsets to account for surrounding interference
compass.set_declination(ToRad(0.0f)); // set local difference between magnetic north and true north
hal.scheduler->delay(1000);
timer = AP_HAL::micros();
}
// to be called only once on boot for initializing objects
static void setup()
{
hal.console->printf("Compass library test\n");
board_config.init();
vehicle.ahrs.init();
compass.init();
hal.console->printf("init done - %u compasses detected\n", compass.get_count());
// set offsets to account for surrounding interference
compass.set_and_save_offsets(0, Vector3f(0, 0, 0));
// set local difference between magnetic north and true north
compass.set_declination(ToRad(0.0f));
hal.scheduler->delay(1000);
timer = AP_HAL::micros();
}
void GCS_MAVLINK::send_raw_imu(const AP_InertialSensor &ins, const Compass &compass)
{
const Vector3f &accel = ins.get_accel(0);
const Vector3f &gyro = ins.get_gyro(0);
const Vector3f &mag = compass.get_field(0);
mavlink_msg_raw_imu_send(
chan,
hal.scheduler->micros(),
accel.x * 1000.0f / GRAVITY_MSS,
accel.y * 1000.0f / GRAVITY_MSS,
accel.z * 1000.0f / GRAVITY_MSS,
gyro.x * 1000.0f,
gyro.y * 1000.0f,
gyro.z * 1000.0f,
mag.x,
mag.y,
mag.z);
#if INS_MAX_INSTANCES > 1
if (ins.get_gyro_count() <= 1 &&
ins.get_accel_count() <= 1 &&
compass.get_count() <= 1) {
return;
}
const Vector3f &accel2 = ins.get_accel(1);
const Vector3f &gyro2 = ins.get_gyro(1);
const Vector3f &mag2 = compass.get_field(1);
mavlink_msg_scaled_imu2_send(
chan,
hal.scheduler->millis(),
accel2.x * 1000.0f / GRAVITY_MSS,
accel2.y * 1000.0f / GRAVITY_MSS,
accel2.z * 1000.0f / GRAVITY_MSS,
gyro2.x * 1000.0f,
gyro2.y * 1000.0f,
gyro2.z * 1000.0f,
mag2.x,
mag2.y,
mag2.z);
#endif
}
static void loop()
{
static const uint8_t compass_count = compass.get_count();
static float min[COMPASS_MAX_INSTANCES][3];
static float max[COMPASS_MAX_INSTANCES][3];
static float offset[COMPASS_MAX_INSTANCES][3];
compass.accumulate();
if ((AP_HAL::micros() - timer) > 100000L) {
timer = AP_HAL::micros();
compass.read();
unsigned long read_time = AP_HAL::micros() - timer;
for (uint8_t i = 0; i < compass_count; i++) {
float heading;
hal.console->printf("Compass #%u: ", i);
if (!compass.healthy()) {
hal.console->println("not healthy");
continue;
}
Matrix3f dcm_matrix;
// use roll = 0, pitch = 0 for this example
dcm_matrix.from_euler(0, 0, 0);
heading = compass.calculate_heading(dcm_matrix, i);
compass.learn_offsets();
const Vector3f &mag = compass.get_field(i);
// capture min
min[i][0] = MIN(mag.x, min[i][0]);
min[i][1] = MIN(mag.y, min[i][1]);
min[i][2] = MIN(mag.z, min[i][2]);
// capture max
max[i][0] = MAX(mag.x, max[i][0]);
max[i][1] = MAX(mag.y, max[i][1]);
max[i][2] = MAX(mag.z, max[i][2]);
// calculate offsets
offset[i][0] = -(max[i][0] + min[i][0]) / 2;
offset[i][1] = -(max[i][1] + min[i][1]) / 2;
offset[i][2] = -(max[i][2] + min[i][2]) / 2;
// display all to user
hal.console->printf("Heading: %.2f (%3d,%3d,%3d)",
ToDeg(heading),
(int)mag.x,
(int)mag.y,
(int)mag.z);
// display offsets
hal.console->printf(" offsets(%.2f, %.2f, %.2f)",
offset[i][0], offset[i][1], offset[i][2]);
hal.console->printf(" t=%u", (unsigned)read_time);
hal.console->println();
}
} else {
hal.scheduler->delay(1);
}
}
void GCS_MAVLINK::send_raw_imu(const AP_InertialSensor &ins, const Compass &compass)
{
const Vector3f &accel = ins.get_accel(0);
const Vector3f &gyro = ins.get_gyro(0);
Vector3f mag;
if (compass.get_count() >= 1) {
mag = compass.get_field(0);
} else {
mag.zero();
}
mavlink_msg_raw_imu_send(
chan,
AP_HAL::micros(),
accel.x * 1000.0f / GRAVITY_MSS,
accel.y * 1000.0f / GRAVITY_MSS,
accel.z * 1000.0f / GRAVITY_MSS,
gyro.x * 1000.0f,
gyro.y * 1000.0f,
gyro.z * 1000.0f,
mag.x,
mag.y,
mag.z);
if (ins.get_gyro_count() <= 1 &&
ins.get_accel_count() <= 1 &&
compass.get_count() <= 1) {
return;
}
const Vector3f &accel2 = ins.get_accel(1);
const Vector3f &gyro2 = ins.get_gyro(1);
if (compass.get_count() >= 2) {
mag = compass.get_field(1);
} else {
mag.zero();
}
mavlink_msg_scaled_imu2_send(
chan,
AP_HAL::millis(),
accel2.x * 1000.0f / GRAVITY_MSS,
accel2.y * 1000.0f / GRAVITY_MSS,
accel2.z * 1000.0f / GRAVITY_MSS,
gyro2.x * 1000.0f,
gyro2.y * 1000.0f,
gyro2.z * 1000.0f,
mag.x,
mag.y,
mag.z);
if (ins.get_gyro_count() <= 2 &&
ins.get_accel_count() <= 2 &&
compass.get_count() <= 2) {
return;
}
const Vector3f &accel3 = ins.get_accel(2);
const Vector3f &gyro3 = ins.get_gyro(2);
if (compass.get_count() >= 3) {
mag = compass.get_field(2);
} else {
mag.zero();
}
mavlink_msg_scaled_imu3_send(
chan,
AP_HAL::millis(),
accel3.x * 1000.0f / GRAVITY_MSS,
accel3.y * 1000.0f / GRAVITY_MSS,
accel3.z * 1000.0f / GRAVITY_MSS,
gyro3.x * 1000.0f,
gyro3.y * 1000.0f,
gyro3.z * 1000.0f,
mag.x,
mag.y,
mag.z);
}
// loop
static void loop()
{
static const uint8_t compass_count = compass.get_count();
static float min[COMPASS_MAX_INSTANCES][3];
static float max[COMPASS_MAX_INSTANCES][3];
static float offset[COMPASS_MAX_INSTANCES][3];
// run read() at 10Hz
if ((AP_HAL::micros() - timer) > 100000L) {
timer = AP_HAL::micros();
compass.read();
const uint32_t read_time = AP_HAL::micros() - timer;
for (uint8_t i = 0; i < compass_count; i++) {
float heading;
hal.console->printf("Compass #%u: ", i);
if (!compass.healthy()) {
hal.console->printf("not healthy\n");
continue;
}
Matrix3f dcm_matrix;
// use roll = 0, pitch = 0 for this example
dcm_matrix.from_euler(0, 0, 0);
heading = compass.calculate_heading(dcm_matrix, i);
const Vector3f &mag = compass.get_field(i);
// capture min
min[i][0] = MIN(mag.x, min[i][0]);
min[i][1] = MIN(mag.y, min[i][1]);
min[i][2] = MIN(mag.z, min[i][2]);
// capture max
max[i][0] = MAX(mag.x, max[i][0]);
max[i][1] = MAX(mag.y, max[i][1]);
max[i][2] = MAX(mag.z, max[i][2]);
// calculate offsets
offset[i][0] = -(max[i][0] + min[i][0]) / 2;
offset[i][1] = -(max[i][1] + min[i][1]) / 2;
offset[i][2] = -(max[i][2] + min[i][2]) / 2;
// display all to user
hal.console->printf("Heading: %.2f (%3d, %3d, %3d)",
(double)ToDeg(heading),
(int)mag.x,
(int)mag.y,
(int)mag.z);
// display offsets
hal.console->printf(" offsets(%.2f, %.2f, %.2f)",
(double)offset[i][0],
(double)offset[i][1],
(double)offset[i][2]);
hal.console->printf(" t=%u", (unsigned)read_time);
hal.console->printf("\n");
}
} else {
// if stipulated time has not passed between two distinct readings, delay the program for a millisecond
hal.scheduler->delay(1);
}
}