From f504e2ec677a6aa1b781427fc5672299d4939a6d Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 20 Dec 2012 15:16:43 +1100 Subject: [PATCH] InertialSensor: removed sample rate in example not needed any more --- libraries/AP_InertialSensor/examples/MPU6000/MPU6000.pde | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/libraries/AP_InertialSensor/examples/MPU6000/MPU6000.pde b/libraries/AP_InertialSensor/examples/MPU6000/MPU6000.pde index db93a28ad2..7fd1f5e2a4 100644 --- a/libraries/AP_InertialSensor/examples/MPU6000/MPU6000.pde +++ b/libraries/AP_InertialSensor/examples/MPU6000/MPU6000.pde @@ -17,12 +17,9 @@ #include #if CONFIG_HAL_BOARD == HAL_BOARD_APM2 -#define SAMPLE_UNIT 1 #define A_LED_PIN 27 #define C_LED_PIN 25 #else -// we need 5x as many samples on the oilpan -#define SAMPLE_UNIT 5 #define A_LED_PIN 37 #define C_LED_PIN 35 #endif @@ -182,7 +179,7 @@ void run_test() while( !hal.console->available() ) { // wait until we have 8 samples - while( ins.num_samples_available() < 8 * SAMPLE_UNIT ) { + while (ins.num_samples_available() == 0) { hal.scheduler->delay(1); } @@ -192,7 +189,7 @@ void run_test() gyro = ins.get_gyro(); temperature = ins.temperature(); - length = sqrt(accel.x*accel.x + accel.y*accel.y + accel.z*accel.z); + length = accel.length(); // 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 \t Temp:%4.2f\n"),