void loop(void)
{
static uint16_t counter;
static uint32_t last_t, last_print, last_compass;
uint32_t now = hal.scheduler->micros();
float heading = 0;
if (last_t == 0) {
last_t = now;
return;
}
last_t = now;
if (now - last_compass > 100*1000UL &&
compass.read()) {
heading = compass.calculate_heading(ahrs.get_dcm_matrix());
// read compass at 10Hz
last_compass = now;
#if WITH_GPS
g_gps->update();
#endif
}
ahrs.update();
counter++;
if (now - last_print >= 100000 /* 100ms : 10hz */) {
Vector3f drift = ahrs.get_gyro_drift();
hal.console->printf(
"r:%4.1f p:%4.1f y:%4.1f "
"drift=(%5.1f %5.1f %5.1f) hdg=%.1f rate=%.1f\n",
ToDeg(ahrs.roll),
ToDeg(ahrs.pitch),
ToDeg(ahrs.yaw),
ToDeg(drift.x),
ToDeg(drift.y),
ToDeg(drift.z),
compass.use_for_yaw() ? ToDeg(heading) : 0.0f,
(1.0e6f*counter)/(now-last_print));
last_print = now;
counter = 0;
}
}
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 loop()
{
static float min[3], max[3], offset[3];
compass.accumulate();
if((AP_HAL::micros()- timer) > 100000L)
{
timer = AP_HAL::micros();
compass.read();
unsigned long read_time = AP_HAL::micros() - timer;
float heading;
if (!compass.healthy()) {
hal.console->println("not healthy");
return;
}
Matrix3f dcm_matrix;
// use roll = 0, pitch = 0 for this example
dcm_matrix.from_euler(0, 0, 0);
heading = compass.calculate_heading(dcm_matrix);
compass.learn_offsets();
// capture min
const Vector3f &mag = compass.get_field();
if( mag.x < min[0] )
min[0] = mag.x;
if( mag.y < min[1] )
min[1] = mag.y;
if( mag.z < min[2] )
min[2] = mag.z;
// capture max
if( mag.x > max[0] )
max[0] = mag.x;
if( mag.y > max[1] )
max[1] = mag.y;
if( mag.z > max[2] )
max[2] = mag.z;
// calculate offsets
offset[0] = -(max[0]+min[0])/2;
offset[1] = -(max[1]+min[1])/2;
offset[2] = -(max[2]+min[2])/2;
// display all to user
hal.console->printf("Heading: %.2f (%3d,%3d,%3d) i2c error: %u",
ToDeg(heading),
(int)mag.x,
(int)mag.y,
(int)mag.z,
(unsigned)hal.i2c->lockup_count());
// display offsets
hal.console->printf(" offsets(%.2f, %.2f, %.2f)",
offset[0], offset[1], offset[2]);
hal.console->printf(" t=%u", (unsigned)read_time);
hal.console->println();
} else {
hal.scheduler->delay(1);
}
}
// 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);
}
}