Hi ABG,
I am using TinyShield BLE and TinyZero Accelerometer currently and i have modified from the example program given. Here's the Arduino code that I have modified:
#include <Wire.h>
#include "RTIMUSettings.h"
#include "RTIMU.h"
#include "RTFusionRTQF.h"
#include <SPI.h>
#include <STBLE.h>
#ifndef ARDUINO_ARCH_SAMD
#include <EEPROM.h>
#endif
//Debug output adds extra flash and memory requirements!
#ifndef BLE_DEBUG
#define BLE_DEBUG true
#endif
RTIMU *imu; // the IMU object
RTFusionRTQF fusion; // the fusion object
RTIMUSettings settings; // the settings object
// DISPLAY_INTERVAL sets the rate at which results are displayed
#define DISPLAY_INTERVAL 300 // interval between pose displays
// SERIAL_PORT_SPEED defines the speed to use for the debug serial port
#define SERIAL_PORT_SPEED 115200
#if defined(ARDUINO_ARCH_SAMD)
#define SerialMonitor SerialUSB
#else
#define SerialMonitor Serial
#endif
unsigned long lastDisplay;
unsigned long lastRate;
int sampleCount;
String data ="";
uint8_t sendLength = 0;
char sendBuffer[30] = " ";
uint8_t ble_rx_buffer[21];
uint8_t ble_rx_buffer_len = 0;
uint8_t ble_connection_state = false;
#define PIPE_UART_OVER_BTLE_UART_TX_TX 0
void setup()
{
int errcode;
SerialMonitor.begin(SERIAL_PORT_SPEED);
BLEsetup();
Wire.begin();
imu = RTIMU::createIMU(&settings); // create the imu object
SerialMonitor.print("ArduinoIMU starting using device "); SerialMonitor.println(imu->IMUName());
if ((errcode = imu->IMUInit()) < 0) {
SerialMonitor.print("Failed to init IMU: "); SerialMonitor.println(errcode);
}
if (imu->getCalibrationValid())
SerialMonitor.println("Using compass calibration");
else
SerialMonitor.println("No valid compass calibration data");
lastDisplay = lastRate = millis();
sampleCount = 0;
// Slerp power controls the fusion and can be between 0 and 1
// 0 means that only gyros are used, 1 means that only accels/compass are used
// In-between gives the fusion mix.
fusion.setSlerpPower(0.02);
// use of sensors in the fusion algorithm can be controlled here
// change any of these to false to disable that sensor
fusion.setGyroEnable(true);
fusion.setAccelEnable(true);
fusion.setCompassEnable(true);
}
void loop()
{
unsigned long now = millis();
unsigned long delta;
if (imu->IMURead()) { // get the latest data if ready yet
fusion.newIMUData(imu->getGyro(), imu->getAccel(), imu->getCompass(), imu->getTimestamp());
sampleCount++;
if ((delta = now - lastRate) >= 1000) {
SerialMonitor.print("Sample rate: "); SerialMonitor.print(sampleCount);
if (imu->IMUGyroBiasValid())
SerialMonitor.println(", gyro bias valid");
else
SerialMonitor.println(", calculating gyro bias - don't move IMU!!");
sampleCount = 0;
lastRate = now;
}
if ((now - lastDisplay) >= DISPLAY_INTERVAL) {
lastDisplay = now;
aci_loop();
RTVector3 accelData=imu->getAccel();
RTVector3 gyroData=imu->getGyro();
RTVector3 compassData=imu->getCompass();
RTVector3 fusionData=fusion.getFusionPose();
//displayAxis("Accel:", accelData.x(), accelData.y(), accelData.z()); // accel data
//displayAxis("Gyro:", gyroData.x(), gyroData.y(), gyroData.z()); // gyro data
//displayAxis("Mag:", compassData.x(), compassData.y(), compassData.z()); // compass data
//displayDegrees("Pose:", fusionData.x(), fusionData.y(), fusionData.z()); // fused output
int x =(fusionData.x() * RTMATH_RAD_TO_DEGREE);
int y = (fusionData.y() * RTMATH_RAD_TO_DEGREE);
int z = (fusionData.z() * RTMATH_RAD_TO_DEGREE);
if (x<0) {
x = (x+360);
}
if (y<0) {
y = (y+360);
}
if (z<0) {
z = (z+360);
}
//SerialMonitor.print("x:"); SerialMonitor.print((x));
//SerialMonitor.print("y:"); SerialMonitor.print((y));
//SerialMonitor.print("z:"); SerialMonitor.print((z));
data =(String) x + "," + (String) y + "," + (String) z;
SerialMonitor.println (data);
sendLength = sprintf(sendBuffer, "%s", data.c_str());
// sendLength = sprintf(sendBuffer, "%d", data);
lib_aci_send_data(PIPE_UART_OVER_BTLE_UART_TX_TX, (uint8_t*)sendBuffer, sendLength);
}
}
}
void displayAxis(const char *label, float x, float y, float z)
{
//SerialMonitor.print(label);
//SerialMonitor.print("x:"); SerialMonitor.print(x);
//SerialMonitor.print("y:"); SerialMonitor.print(y);
//SerialMonitor.print("z:"); SerialMonitor.print(z);
}