1234567891011121314151617181920212223242526272829303132333435363738394041424344454647484950515253545556575859606162636465666768697071727374757677 |
- #include <Arduino.h>
- #include <BMP280_DEV.h>
- #include <MPU9250.h>
- #include "ultrasonic.hpp"
- #define LOOP_INTERVAL_US (12 * 1000) // 12 * 1000us
- MPU9250 mpu;
- void blinkLed() {
- digitalWrite(LED_BUILTIN, (millis()>>7) & 1);
- }
- void setup() {
- Serial.begin(1000000);
- Serial.println(F("boot"));
- pinMode(LED_BUILTIN, OUTPUT);
-
- //initialize acustic sensor
- us_init();
- //initialize I2C MPU
- Wire.begin();
- delay(2000);
- Serial.println(F("setup mpu"));
- mpu.setup(0x68);
- delay(1000);
- Serial.println(F("calibrate accel/gyro"));
- mpu.calibrateAccelGyro();
- Serial.println(F("FIELDS:\tUS_0\tUS_1\tMAG_X\tMAG_Y\tMAG_Z\tACCEL_X\tACCEL_Y\tACCEL_Z\tGYRO_X\tGYRO_Y\tGYRO_Z\tTEMP\tEXEC_TIME"));
- }
- unsigned long loopStart = 0;
- char outputBuffer[256];
- void loop() {
- if(micros() >= loopStart + LOOP_INTERVAL_US) {
- loopStart = micros();
- //emit ultrasound pulse
- us_transmit();
- //get mpu values
- mpu.update();
- snprintf(outputBuffer, sizeof(outputBuffer), "DATA:\t%ld\t%ld\t%ld\t%ld\t%ld\t%ld\t%ld\t%ld\t%ld\t%ld\t%ld\t%ld\t%ld\r\n",
- //acustic RTT
- us_get_duration(0), us_get_duration(1),
- //magnetic field
- (long)(mpu.getMagX()*1000),
- (long)(mpu.getMagY()*1000),
- (long)(mpu.getMagZ()*1000),
- //accelerometer
- (long)(mpu.getAccX()*1000),
- (long)(mpu.getAccY()*1000),
- (long)(mpu.getAccZ()*1000),
- //gyroscope
- (long)(mpu.getGyroX()*1000),
- (long)(mpu.getGyroY()*1000),
- (long)(mpu.getGyroZ()*1000),
- //temperature
- (long)(mpu.getTemperature()*1000),
- //execution time in microseconds
- micros() - loopStart
- );
- Serial.write(outputBuffer);
- blinkLed();
- }
- }
|