void SchedTest::loop(void)
{
// wait for an INS sample
ins.wait_for_sample();
// tell the scheduler one tick has passed
scheduler.tick();
// run all tasks that fit in 20ms
scheduler.run(20000);
}
static void run_test()
{
Vector3f accel;
Vector3f gyro;
float length;
uint8_t counter = 0;
// flush any user input
while( hal.console->available() ) {
hal.console->read();
}
// clear out any existing samples from ins
ins.update();
// loop as long as user does not press a key
while( !hal.console->available() ) {
// wait until we have a sample
ins.wait_for_sample();
// read samples from ins
ins.update();
accel = ins.get_accel();
gyro = ins.get_gyro();
length = accel.length();
if (counter++ % 50 == 0) {
// display results
hal.console->printf_P(PSTR("Accel X:%4.2f \t Y:%4.2f \t Z:%4.2f \t len:%4.2f \t Gyro X:%4.2f \t Y:%4.2f \t Z:%4.2f\n"),
accel.x, accel.y, accel.z, length, gyro.x, gyro.y, gyro.z);
}
}
// clear user input
while( hal.console->available() ) {
hal.console->read();
}
}
static void run_test()
{
Vector3f accel;
Vector3f gyro;
uint8_t counter = 0;
static uint8_t accel_count = ins.get_accel_count();
static uint8_t gyro_count = ins.get_gyro_count();
static uint8_t ins_count = MAX(accel_count, gyro_count);
// flush any user input
while (hal.console->available()) {
hal.console->read();
}
// clear out any existing samples from ins
ins.update();
// loop as long as user does not press a key
while (!hal.console->available()) {
// wait until we have a sample
ins.wait_for_sample();
// read samples from ins
ins.update();
// print each accel/gyro result every 50 cycles
if (counter++ % 50 != 0) {
continue;
}
// loop and print each sensor
for (uint8_t ii = 0; ii < ins_count; ii++) {
char state;
if (ii > accel_count - 1) {
// No accel present
state = '-';
} else if (ins.get_accel_health(ii)) {
// Healthy accel
state = 'h';
} else {
// Accel present but not healthy
state = 'u';
}
accel = ins.get_accel(ii);
hal.console->printf("%u - Accel (%c) : X:%6.2f Y:%6.2f Z:%6.2f norm:%5.2f",
ii, state, (double)accel.x, (double)accel.y, (double)accel.z,
(double)accel.length());
gyro = ins.get_gyro(ii);
if (ii > gyro_count - 1) {
// No gyro present
state = '-';
} else if (ins.get_gyro_health(ii)) {
// Healthy gyro
state = 'h';
} else {
// Gyro present but not healthy
state = 'u';
}
hal.console->printf(" Gyro (%c) : X:%6.2f Y:%6.2f Z:%6.2f\n",
state, (double)gyro.x, (double)gyro.y, (double)gyro.z);
auto temp = ins.get_temperature(ii);
hal.console->printf(" t:%6.2f\n", (double)temp);
}
}
// clear user input
while (hal.console->available()) {
hal.console->read();
}
}
