#include <Wire.h>
#include <math.h>
#define SERIAL_CMD true
uint8_t MAX_BUFF_LENGTH = 32;
uint8_t requestBytes = 4;
#define HEADER_LENGTH 4
#define SHTP_REPORT_COMMAND_RESPONSE 0xF1
#define SHTP_REPORT_COMMAND_REQUEST 0xF2
#define SHTP_REPORT_FRS_READ_RESPONSE 0xF3
#define SHTP_REPORT_FRS_READ_REQUEST 0xF4
#define SHTP_REPORT_PRODUCT_ID_RESPONSE 0xF8
#define SHTP_REPORT_PRODUCT_ID_REQUEST 0xF9
#define SHTP_REPORT_BASE_TIMESTAMP 0xFB
#define SHTP_REPORT_SET_FEATURE_COMMAND 0xFD
//All the different sensors and features we can get reports from
//These are used when enabling a given sensor
#define SENSOR_REPORTID_ACCELEROMETER 0x01
#define SENSOR_REPORTID_GYROSCOPE 0x02
#define SENSOR_REPORTID_MAGNETIC_FIELD 0x03
#define SENSOR_REPORTID_LINEAR_ACCELERATION 0x04
#define SENSOR_REPORTID_ROTATION_VECTOR 0x05
#define SENSOR_REPORTID_GRAVITY 0x06
#define SENSOR_REPORTID_GAME_ROTATION_VECTOR 0x08
#define SENSOR_REPORTID_GEOMAGNETIC_ROTATION_VECTOR 0x09
#define SENSOR_REPORTID_GYRO_INTEGRATED_ROTATION_VECTOR 0x2A
#define SENSOR_REPORTID_TAP_DETECTOR 0x10
#define SENSOR_REPORTID_STEP_COUNTER 0x11
#define SENSOR_REPORTID_STABILITY_CLASSIFIER 0x13
#define SENSOR_REPORTID_RAW_ACCELEROMETER 0x14
#define SENSOR_REPORTID_RAW_GYROSCOPE 0x15
#define SENSOR_REPORTID_RAW_MAGNETOMETER 0x16
#define SENSOR_REPORTID_PERSONAL_ACTIVITY_CLASSIFIER 0x1E
#define SENSOR_REPORTID_AR_VR_STABILIZED_ROTATION_VECTOR 0x28
//#define SENSOR_REPORTID_AR_VR_STABILIZED_GAME_ROTATION_VECTOR*0x29
#define DEBUG_I2C false
const byte CHANNEL_COMMAND = 0;
const byte CHANNEL_EXECUTABLE = 1;
const byte CHANNEL_CONTROL = 2;
const byte CHANNEL_REPORTS = 3;
const byte CHANNEL_WAKE_REPORTS = 4;
const byte CHANNEL_GYRO = 5;
uint8_t deviceAddress = 0x4B;
uint8_t shtpDataRead[128];
uint8_t shtpDataWrite[128];
uint8_t shtpDataSend[128];
uint8_t sequenceNumber[6] = { 0, 0, 0, 0, 0, 0 };
uint8_t syncCom = 0;
uint16_t remainingBytes = 0;
uint8_t flagStartup = 0;
uint32_t timerToSend = 1000;
uint32_t timeToSend = 0;
uint32_t timerToSendConf = 2000;
uint32_t timeToSendConf = 0;
uint32_t timer_head_i2c = 70;
uint32_t time_head_i2c = 0;
int pinInterrupt = 8;
float quatI, quatJ, quatK, quatReal, quatRadianAccuracy;
uint8_t quatAccuracy;
void setup() {
// put your setup code here, to run once:
while(!Serial && millis() < 3000) ;
//if (CrashReport) {
// Serial.print(CrashReport);
// while (Serial.read() == -1);
// while (Serial.read() != -1);
//}
pinMode(pinInterrupt, INPUT_PULLUP);
pinMode(4, OUTPUT);
pinMode(5, OUTPUT);
Serial.begin(115200);
Wire.begin();
Wire.setClock(400000);
softResetI2C();
delay(250);
Serial.println("END SETUP");
Serial.println("AFTER STARTUP ADVERTISE MSG READ:");
Serial.println("- Send '1' to request ProductID msg");
Serial.println("- Send '2' to enable Rotation Vector Output");
}
void loop() {
if (SERIAL_CMD) {
if (Serial.available() > 0) {
char cmd = Serial.read();
if (cmd == '1') {
timeToSend = millis();
productIdReq();
delay(100);
flagStartup = 2;
Serial.println("productIdReq");
}
if (cmd == '2') {
timeToSendConf = millis();
enableRotationVectorI2C(10);
flagStartup = 3;
Serial.println("enableRotationVector");
Serial.println(F("Output in form i, j, k, real, accuracy"));
}
if (cmd == '3') {
timeToSendConf = millis();
softResetI2C();
Serial.println("softReset");
}
}
} else {
if (((millis() - timeToSendConf) > timerToSendConf) && (flagStartup == 1)) {
timeToSend = millis();
productIdReq();
flagStartup = 2;
Serial.println("productIdReq");
}
if (((millis() - timeToSend) > timerToSend) && (flagStartup == 2)) {
timeToSend = millis();
enableRotationVectorI2C(10);
flagStartup = 3;
Serial.println("enableRotationVector");
Serial.println(F("Output in form i, j, k, real, accuracy"));
}
}
if (interruptOn()) {
timeToSend = millis();
if ((micros() - time_head_i2c) > timer_head_i2c) {
receivePackets();
time_head_i2c = micros();
if (shtpDataRead[2] == CHANNEL_REPORTS && shtpDataRead[4] == SHTP_REPORT_BASE_TIMESTAMP) {
getQuat(quatI, quatJ, quatK, quatReal, quatRadianAccuracy, quatAccuracy);
Serial.print(quatI, 2);
Serial.print(F(","));
Serial.print(quatJ, 2);
Serial.print(F(","));
Serial.print(quatK, 2);
Serial.print(F(","));
Serial.print(quatReal, 2);
Serial.print(F(","));
Serial.print(quatRadianAccuracy, 2);
Serial.print(F(","));
Serial.println();
}
}
}
}
void printSyncCom() {
Serial.print("syncCom:");
Serial.println(syncCom);
}
bool receivePackets() {
Wire.requestFrom((uint8_t)deviceAddress, requestBytes);
if (waitForI2C() == false)
return (false); //Error
return readI2C();
}
uint8_t byteToread = 0;
uint8_t readI2C() {
//Serial.printf("readI2C: %u ", syncCom); Serial.flush();
switch (syncCom) {
case 0:
byteToread = Wire.available();
//Serial.printf(" avail: %u\n", byteToread); Serial.flush();
if (byteToread >= requestBytes) {
//Serial.println(requestBytes);
for (uint8_t k = 0; k < byteToread; k++) {
shtpDataRead[k] = Wire.read();
}
printPacketI2C(requestBytes);
remainingBytes = getRemainingBytes(shtpDataRead[1], shtpDataRead[0]);
//Serial.println(remainingBytes);
if (remainingBytes < (MAX_BUFF_LENGTH - HEADER_LENGTH)) {
requestBytes = remainingBytes;
syncCom = 2;
} else {
requestBytes = MAX_BUFF_LENGTH;
syncCom = 1;
}
}
if( syncCom == 0) return true;
break;
case 1:
byteToread = Wire.available();
//Serial.printf(" avail: %u\n", byteToread); Serial.flush();
if (byteToread >= requestBytes) {
for (uint8_t k = 0; k < byteToread; k++) {
shtpDataRead[k] = Wire.read();
}
printPacketI2C(requestBytes);
remainingBytes -= (MAX_BUFF_LENGTH - HEADER_LENGTH);
if (remainingBytes < (MAX_BUFF_LENGTH - HEADER_LENGTH)) {
requestBytes = remainingBytes;
syncCom = 2;
}
}
if( syncCom == 1) return true;
break;
case 2:
byteToread = Wire.available();
//Serial.printf(" avail: %u\n", byteToread); Serial.flush();
if (Wire.available() >= byteToread) {
for (uint8_t k = 0; k < byteToread; k++) {
shtpDataRead[k] = Wire.read();
}
syncCom = 0;
printPacketI2C(requestBytes);
requestBytes = HEADER_LENGTH;
if (flagStartup == 0) {
flagStartup = 1;
Serial.println("flagStartup = 1");
}
}
return true;
break;
}
return false;
}
uint8_t packetContinue(uint8_t MSB) {
uint8_t temp_bit = (MSB >> 7);
return temp_bit;
}
uint16_t getRemainingBytes(uint8_t MSB, uint8_t LSB) {
uint16_t dataLengtha = (((uint16_t)MSB) << 8) | ((uint16_t)LSB);
dataLengtha &= ~((uint16_t)1 << 15); //Clear the MSbit.
return dataLengtha;
}
uint8_t interruptOn() {
if (digitalRead(pinInterrupt) == LOW) {
return true;
} else {
return false;
}
}
void setFeatureCommandI2C(uint8_t reportID, uint16_t timeBetweenReports, uint32_t specificConfig) {
long microsBetweenReports = (long)timeBetweenReports * 1000L;
shtpDataWrite[0] = SHTP_REPORT_SET_FEATURE_COMMAND; //Set feature command. Reference page 55
shtpDataWrite[1] = reportID; //Feature Report ID. 0x01 = Accelerometer, 0x05 = Rotation vector
shtpDataWrite[2] = 0; //Feature flags
shtpDataWrite[3] = 0; //Change sensitivity (LSB)
shtpDataWrite[4] = 0; //Change sensitivity (MSB)
shtpDataWrite[5] = (microsBetweenReports >> 0) & 0xFF; //Report interval (LSB) in microseconds. 0x7A120 = 500ms
shtpDataWrite[6] = (microsBetweenReports >> 8) & 0xFF; //Report interval
shtpDataWrite[7] = (microsBetweenReports >> 16) & 0xFF; //Report interval
shtpDataWrite[8] = (microsBetweenReports >> 24) & 0xFF; //Report interval (MSB)
shtpDataWrite[9] = 0; //Batch Interval (LSB)
shtpDataWrite[10] = 0; //Batch Interval
shtpDataWrite[11] = 0; //Batch Interval
shtpDataWrite[12] = 0; //Batch Interval (MSB)
shtpDataWrite[13] = (specificConfig >> 0) & 0xFF; //Sensor-specific config (LSB)
shtpDataWrite[14] = (specificConfig >> 8) & 0xFF; //Sensor-specific config
shtpDataWrite[15] = (specificConfig >> 16) & 0xFF; //Sensor-specific config
shtpDataWrite[16] = (specificConfig >> 24) & 0xFF; //Sensor-specific config (MSB)
//Transmit packet on channel 2, 17 bytes
sendPacketI2C(CHANNEL_CONTROL, 17);
}
uint8_t sendPacketI2C(uint8_t channelNumber, uint8_t dataLength) {
digitalWrite(4, HIGH);
uint8_t packetLength = dataLength + 4; //Add four bytes for the header
Wire.beginTransmission(deviceAddress);
shtpDataSend[0] = (packetLength & 0xFF);
shtpDataSend[1] = (packetLength >> 8);
shtpDataSend[2] = (channelNumber);
shtpDataSend[3] = (sequenceNumber[channelNumber]); //Send the sequence number, increments with each packet sent, different counter for each channel
for (uint8_t i = 0; i < (dataLength); i++) {
shtpDataSend[i + 4] = shtpDataWrite[i];
}
for (uint8_t i = 0; i < (dataLength + 4); i++) {
Wire.write(shtpDataSend[i]);
}
sequenceNumber[channelNumber]++;
Wire.endTransmission();
digitalWrite(4, LOW);
return true;
}
void productIdReq() {
shtpDataWrite[0] = SHTP_REPORT_PRODUCT_ID_REQUEST; //Set feature command. Reference page 55
shtpDataWrite[1] = 0;
sendPacketI2C(CHANNEL_CONTROL, 2);
}
void softResetI2C() {
shtpDataWrite[0] = 1;
sendPacketI2C(CHANNEL_EXECUTABLE, 1);
//Read all incoming data and flush it
delay(50);
while (receivePackets() == true)
; //delay(1);
delay(50);
while (receivePackets() == true)
; //delay(1);
}
void enableRotationVectorI2C(uint16_t timeBetweenReports) {
setFeatureCommandI2C(SENSOR_REPORTID_ROTATION_VECTOR, timeBetweenReports, 0);
}
void enableAccelerometerI2C(uint16_t timeBetweenReports) {
setFeatureCommandI2C(SENSOR_REPORTID_LINEAR_ACCELERATION, timeBetweenReports, 0);
}
void printPacketI2C(uint8_t printBytes) {
if (DEBUG_I2C) {
//Serial.printf("printPacketI2C(%u)\n", printBytes); Serial.flush();
for (uint8_t j = 0; j < printBytes; j++) {
Serial.print(shtpDataRead[j], HEX);
Serial.print(",");
}
Serial.println();
}
}
boolean waitForI2C() {
for (uint8_t counter = 0; counter < 255; counter++) //Don't got more than 255
{
if (Wire.available() > 0)
return (true);
delay(1);
}
//if (_printDebug == true)
Serial.println(F("I2C timeout"));
return (false);
}
//Gets the full quaternion
//i,j,k,real output floats
void getQuat(float &i, float &j, float &k, float &real, float &radAccuracy, uint8_t &accuracy)
{
uint16_t rawQuatI, rawQuatJ, rawQuatK, rawQuatReal, rawQuatRadianAccuracy, quatAccuracy;
int16_t rotationVector_Q1 = 14;
int16_t rotationVectorAccuracy_Q1 = 12; //Heading accuracy estimate in radians. The Q point is 12.
//Calculate the number of data bytes in this packet
int16_t dataLength = ((uint16_t)shtpDataRead[1] << 8 | shtpDataRead[0]);
dataLength &= ~(1 << 15); //Clear the MSbit. This bit indicates if this package is a continuation of the last.
//Ignore it for now. TODO catch this as an error and exit
dataLength -= 4; //Remove the header bytes from the data count
uint32_t timeStamp = ((uint32_t)shtpDataRead[HEADER_LENGTH + 4] << (8 * 3)) | ((uint32_t)shtpDataRead[HEADER_LENGTH + 3] << (8 * 2)) | ((uint32_t)shtpDataRead[HEADER_LENGTH + 2] << (8 * 1)) | ((uint32_t)shtpDataRead[HEADER_LENGTH + 1] << (8 * 0));
uint8_t status = shtpDataRead[HEADER_LENGTH + 5 + 2] & 0x03; //Get status bits
uint16_t data1 = (uint16_t)shtpDataRead[HEADER_LENGTH + 5 + 5] << 8 | shtpDataRead[5 + 4];
uint16_t data2 = (uint16_t)shtpDataRead[HEADER_LENGTH + 5 + 7] << 8 | shtpDataRead[5 + 6];
uint16_t data3 = (uint16_t)shtpDataRead[HEADER_LENGTH + 5 + 9] << 8 | shtpDataRead[5 + 8];
uint16_t data4 = 0;
uint16_t data5 = 0; //We would need to change this to uin32_t to capture time stamp value on Raw Accel/Gyro/Mag reports
if (dataLength - 5 > 9)
{
data4 = (uint16_t)shtpDataRead[HEADER_LENGTH + 5 + 11] << 8 | shtpDataRead[HEADER_LENGTH + 5 + 10];
}
if (dataLength - 5 > 11)
{
data5 = (uint16_t)shtpDataRead[HEADER_LENGTH + 5 + 13] << 8 | shtpDataRead[HEADER_LENGTH + 5 + 12];
}
quatAccuracy = status;
rawQuatI = data1;
rawQuatJ = data2;
rawQuatK = data3;
rawQuatReal = data4;
//Only available on rotation vector and ar/vr stabilized rotation vector,
// not game rot vector and not ar/vr stabilized rotation vector
rawQuatRadianAccuracy = data5;
i = qToFloat(rawQuatI, rotationVector_Q1);
j = qToFloat(rawQuatJ, rotationVector_Q1);
k = qToFloat(rawQuatK, rotationVector_Q1);
real = qToFloat(rawQuatReal, rotationVector_Q1);
radAccuracy = qToFloat(rawQuatRadianAccuracy, rotationVector_Q1);
accuracy = quatAccuracy;
}
//Given a register value and a Q point, convert to float
//See https://en.wikipedia.org/wiki/Q_(number_format)
float qToFloat(int16_t fixedPointValue, uint8_t qPoint)
{
float qFloat = fixedPointValue;
qFloat *= pow(2, qPoint * -1);
return (qFloat);
}