Skip to content

Instantly share code, notes, and snippets.

@donghee

donghee/OTOS_raw_data.ino

Last active August 4, 2024 00:09
Show Gist options
  • Save donghee/dcea336e4d8802c5659692fd7f9ac96b to your computer and use it in GitHub Desktop.
Save donghee/dcea336e4d8802c5659692fd7f9ac96b to your computer and use it in GitHub Desktop.
Download ZIP
OTOS raw data
Raw
OTOS_raw_data.ino
#include "SparkFun_Qwiic_OTOS_Arduino_Library.h"
#include "Wire.h"
QwiicOTOS myOtos;
byte buffer[24]; // lsm + paa registers
//byte buffer[96]; // all registers
byte lsmData[12];
byte paaData[12];
void setup() {
// Start serial
Serial.begin(115200);
Serial.println("Qwiic OTOS Example 1 - Basic Readings");
Wire.begin();
// Attempt to begin the sensor
while (myOtos.begin() == false) {
Serial.println("OTOS not connected, check your wiring and I2C address!");
delay(1000);
}
Serial.println("OTOS connected!");
Serial.println("Ensure the OTOS is flat and stationary, then enter any key to calibrate the IMU");
// Clear the serial buffer
while (Serial.available())
Serial.read();
// Wait for user input
while (!Serial.available())
;
Serial.println("Calibrating IMU...");
// Calibrate the IMU, which removes the accelerometer and gyroscope offsets
myOtos.calibrateImu();
// Reset the tracking algorithm - this resets the position to the origin,
// but can also be used to recover from some rare tracking errors
myOtos.resetTracking();
delay(1000);
sfe_otos_pose2d_t myPosition;
myOtos.getPosition(myPosition);
// Print measurement
Serial.println();
Serial.println("Position:");
Serial.print("X (Inches): ");
Serial.println(myPosition.x);
Serial.print("Y (Inches): ");
Serial.println(myPosition.y);
Serial.print("Heading (Degrees): ");
Serial.println(myPosition.h);
}
void loop() {
int i = 0;
Wire.beginTransmission(0x17); // 0x17 is OTOS i2c address
Wire.write(0x44); // from lsm gyro: https://github.com/sparkfun/SparkFun_Optical_Tracking_Odometry_Sensor/blob/737950b14e4e960c40c73ddc157a2b9d9fe7224e/Firmware/SFE/Inc/registers.h#L71C15-L71C31
// Wire.write(0x00); // from product id
Wire.endTransmission();
Wire.requestFrom(0x17, 24); // request 24 bytes of raw sensor values
// Wire.requestFrom(0x17, 96); // request 96 bytes of whole registers
while (Wire.available()) { // slave may send less than requested
char c = Wire.read(); // receive a byte as character
buffer[i] = c;
i++;
}
// print OTOS registers
// for (int i = 0; i < sizeof(buffer); i++) {
// if (buffer[i] < 0x10) { // 한 자리 수인 경우
// Serial.print("0"); // 앞에 '0' 추가
// }
// Serial.print(buffer[i], HEX);
// Serial.print(" ");
// }
memcpy(lsmData, buffer, 12);
memcpy(paaData, buffer + 12, 12);
// ref: https://github.com/sparkfun/SparkFun_Optical_Tracking_Odometry_Sensor/blob/737950b14e4e960c40c73ddc157a2b9d9fe7224e/Firmware/SFE/Src/main.cpp#L283
int16_t deltaRRaw = (lsmData[1] << 8) | lsmData[0];
int16_t deltaPRaw = (lsmData[3] << 8) | lsmData[2];
int16_t deltaHRaw = (lsmData[5] << 8) | lsmData[4];
int16_t accelXRaw = (lsmData[7] << 8) | lsmData[6];
int16_t accelYRaw = (lsmData[9] << 8) | lsmData[8];
int16_t accelZRaw = (lsmData[11] << 8) | lsmData[10];
int16_t deltaXRaw = (paaData[3] << 8) | paaData[2];
int16_t deltaYRaw = (paaData[5] << 8) | paaData[4];
// LSM6DSO gyro
Serial.print(deltaRRaw);
Serial.print(", ");
Serial.print(deltaPRaw);
Serial.print(", ");
Serial.print(deltaHRaw);
Serial.print(" ");
// LSM6DSO accel
Serial.print(accelXRaw);
Serial.print(", ");
Serial.print(accelYRaw);
Serial.print(", ");
Serial.print(accelZRaw);
Serial.print(" ");
// paa5160E1 optical flow
Serial.print(deltaXRaw);
Serial.print(", ");
Serial.print(deltaYRaw);
Serial.println();
// TODO: need to convert unit, ref: https://github.com/sparkfun/Qwiic_OTOS_Py/blob/master/qwiic_otos.py
delay(2);
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment