mirror of https://github.com/ArduPilot/ardupilot
uncrustify libraries/AP_IMU/examples/IMU_MPU6000_test/IMU_MPU6000_test.pde
This commit is contained in:
parent
f597b7101a
commit
0fce87c934
|
@ -16,7 +16,7 @@
|
|||
FastSerialPort(Serial, 0);
|
||||
|
||||
Arduino_Mega_ISR_Registry isr_registry;
|
||||
AP_TimerProcess mpu_scheduler;
|
||||
AP_TimerProcess mpu_scheduler;
|
||||
|
||||
AP_InertialSensor_MPU6000 mpu6000( 53 ); /* chip select is pin 53 */
|
||||
AP_IMU_INS imu(&mpu6000, 0); /* second arg is for Parameters. Can we leave it null?*/
|
||||
|
@ -28,34 +28,34 @@ AP_IMU_INS imu(&mpu6000, 0); /* second arg is for Parameters. Can we leave it nu
|
|||
|
||||
static void flash_leds(bool on)
|
||||
{
|
||||
digitalWrite(A_LED_PIN, on?LED_OFF:LED_ON);
|
||||
digitalWrite(C_LED_PIN, on?LED_ON:LED_OFF);
|
||||
digitalWrite(A_LED_PIN, on ? LED_OFF : LED_ON);
|
||||
digitalWrite(C_LED_PIN, on ? LED_ON : LED_OFF);
|
||||
}
|
||||
|
||||
void setup(void)
|
||||
{
|
||||
pinMode(53, OUTPUT);
|
||||
digitalWrite(53, HIGH);
|
||||
pinMode(53, OUTPUT);
|
||||
digitalWrite(53, HIGH);
|
||||
|
||||
Serial.begin(115200);
|
||||
Serial.println("Doing IMU startup...");
|
||||
Serial.begin(115200);
|
||||
Serial.println("Doing IMU startup...");
|
||||
|
||||
isr_registry.init();
|
||||
mpu_scheduler.init(&isr_registry);
|
||||
|
||||
imu.init(IMU::COLD_START, delay, flash_leds, &mpu_scheduler);
|
||||
imu.init(IMU::COLD_START, delay, flash_leds, &mpu_scheduler);
|
||||
}
|
||||
|
||||
void loop(void)
|
||||
{
|
||||
Vector3f accel;
|
||||
Vector3f gyro;
|
||||
Vector3f accel;
|
||||
Vector3f gyro;
|
||||
|
||||
delay(1000);
|
||||
imu.update();
|
||||
accel = imu.get_accel();
|
||||
gyro = imu.get_gyro();
|
||||
delay(1000);
|
||||
imu.update();
|
||||
accel = imu.get_accel();
|
||||
gyro = imu.get_gyro();
|
||||
|
||||
Serial.printf("AX: 0x%4.4f AY: 0x%4.4f AZ: 0x%4.4f GX: 0x%4.4f GY: 0x%4.4f GZ: 0x%4.4f\n",
|
||||
accel.x, accel.y, accel.z, gyro.x, gyro.y, gyro.z);
|
||||
Serial.printf("AX: 0x%4.4f AY: 0x%4.4f AZ: 0x%4.4f GX: 0x%4.4f GY: 0x%4.4f GZ: 0x%4.4f\n",
|
||||
accel.x, accel.y, accel.z, gyro.x, gyro.y, gyro.z);
|
||||
}
|
||||
|
|
Loading…
Reference in New Issue