g1smo
/
pifcamp-2021
1
1
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Dodan quaternion, serijski link

kegel
Jurij Podgoršek 2022-11-27 09:47:28 +01:00
parent 9e0a0c5c89
commit b019145c97
1 changed files with 27 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
osc32_9255_rtimulib/osc32_9255_rtimulib.ino
View File

@ -35,6 +35,16 @@ BluetoothSerial SerialBT;
SLIPEncodedBluetoothSerial SLIPBTSerial(SerialBT); SLIPEncodedBluetoothSerial SLIPBTSerial(SerialBT);
#endif #endif
// SERIAL
#ifdef BOARD_HAS_USB_SERIAL
#include <SLIPEncodedUSBSerial.h>
SLIPEncodedUSBSerial SLIPSerial( thisBoardsSerialUSB );
#else
#include <SLIPEncodedSerial.h>
SLIPEncodedSerial SLIPSerial(Serial); // Change to Serial1 or Serial2 etc. for boards with multiple serial ports that don’t have Serial
#endif
// Motion sensor objects // Motion sensor objects
RTIMU *imu; // the IMU object RTIMU *imu; // the IMU object
RTFusionRTQF fusion; // the fusion object RTFusionRTQF fusion; // the fusion object
@ -53,7 +63,8 @@ OSCBundle bundle;
void setup() { void setup() {
int errcode; int errcode;
// Basic(debug) serial init // Basic(debug) serial init
Serial.begin(115200); // set this as high as you can reliably run on your platform SLIPSerial.begin(115200);
//Serial.begin(115200); // set this as high as you can reliably run on your platform
Serial.println("Starting up..."); Serial.println("Starting up...");
// Init EEPROM based on magnet calibration size requirement // Init EEPROM based on magnet calibration size requirement
@ -130,11 +141,13 @@ void loop() {
sampleCount++; sampleCount++;
if ((delta = now - lastRate) >= 1000) { if ((delta = now - lastRate) >= 1000) {
Serial.print("Sample rate: "); Serial.print(sampleCount);
if (!imu->IMUGyroBiasValid()) //Serial.print("Sample rate: "); Serial.print(sampleCount);
Serial.println(", calculating gyro bias"); if (!imu->IMUGyroBiasValid()) {
else // Serial.println(", calculating gyro bias");
Serial.println(); } else {
// Serial.println();
}
sampleCount = 0; sampleCount = 0;
lastRate = now; lastRate = now;
@ -143,9 +156,6 @@ void loop() {
//RTMath::display("Accel:", realAccel); //RTMath::display("Accel:", realAccel);
//Serial.println(); //Serial.println();
// Quaternion - rotacija
//bundle.add("/quaternion").add(fusion.getFusionQPose().scalar()).add(fusion.getFusionQPose().x()).add(fusion.getFusionQPose().y()).add(fusion.getFusionQPose().z()); // W X Y Z
// Euler - rotacija // Euler - rotacija
//eulerVector = eulerFromQuaternion(q); //eulerVector = eulerFromQuaternion(q);
//bundle.add("/euler").add(eulerVector(0)).add(eulerVector(1)).add(eulerVector(2)); // X Y Z //bundle.add("/euler").add(eulerVector(0)).add(eulerVector(1)).add(eulerVector(2)); // X Y Z
@ -162,7 +172,13 @@ void loop() {
// Accelerometer // Accelerometer
bundle.add("/accel").add(realAccel.x()).add(realAccel.y()).add(realAccel.z()); bundle.add("/accel").add(realAccel.x()).add(realAccel.y()).add(realAccel.z());
// Some bug below, it seems // Quaternion - rotacija
bundle.add("/quaternion").add(fusion.getFusionQPose().scalar()).add(fusion.getFusionQPose().x()).add(fusion.getFusionQPose().y()).add(fusion.getFusionQPose().z()); // W X Y Z
SLIPSerial.beginPacket();
bundle.send(SLIPSerial);
SLIPSerial.endPacket();
#ifdef BT_OSC #ifdef BT_OSC
SLIPBTSerial.beginPacket(); SLIPBTSerial.beginPacket();
bundle.send(SLIPBTSerial); bundle.send(SLIPBTSerial);