Bluetooth funcitonality, OSC extension, new calibration for proto

eulerConversion
Jurij Podgoršek 2022-03-03 22:37:49 +01:00
parent fac248290d
commit 53d957ea0f
5 changed files with 353 additions and 70 deletions

View File

@ -0,0 +1,53 @@
/*
* Here a simple sketch to test the upload of ESP32 S2 Saola 1MI.
* The addressable RGB LED (WS2812), driven by GPIO18 need
* Adafruit NeoPixel library
* by Mischianti Renzo <https://www.mischianti.org>
*
* https://www.mischianti.org/
*
*/
#include <Adafruit_NeoPixel.h>
#define PIN 18
#define NUMPIXELS 1
Adafruit_NeoPixel pixels(NUMPIXELS, PIN, NEO_GRB + NEO_KHZ800);
enum {NONE, RED, GREEN, BLUE};
int ledColor = NONE;
void setup()
{
pixels.begin();
}
void loop()
{
switch (ledColor) {
case NONE:
pixels.setPixelColor(0, pixels.Color(0, 0, 0));
pixels.show();
break;
case RED:
pixels.setPixelColor(0, pixels.Color(20, 0, 0));
pixels.show();
break;
case GREEN:
pixels.setPixelColor(0, pixels.Color(0, 20, 0));
pixels.show();
break;
case BLUE:
pixels.setPixelColor(0, pixels.Color(0, 0, 20));
pixels.show();
break;
default:
break;
}
ledColor++;
if (ledColor == 4) {
ledColor = NONE;
}
delay(1000);
}

View File

@ -0,0 +1,181 @@
#include "SLIPEncodedBluetoothSerial.h"
#include "BluetoothSerial.h"
/*
CONSTRUCTOR
*/
//instantiate with the tranmission layer
//use BluetoothSerial
SLIPEncodedBluetoothSerial::SLIPEncodedBluetoothSerial(BluetoothSerial &s){
serial = &s;
rstate = CHAR;
}
static const uint8_t eot = 0300;
static const uint8_t slipesc = 0333;
static const uint8_t slipescend = 0334;
static const uint8_t slipescesc = 0335;
/*
SERIAL METHODS
*/
bool SLIPEncodedBluetoothSerial::endofPacket()
{
if(rstate == SECONDEOT)
{
rstate = CHAR;
return true;
}
if (rstate==FIRSTEOT)
{
if(serial->available())
{
uint8_t c =serial->peek();
if(c==eot)
{
serial->read(); // throw it on the floor
}
}
rstate = CHAR;
return true;
}
return false;
}
int SLIPEncodedBluetoothSerial::available(){
back:
int cnt = serial->available();
if(cnt==0)
return 0;
if(rstate==CHAR)
{
uint8_t c =serial->peek();
if(c==slipesc)
{
rstate = SLIPESC;
serial->read(); // throw it on the floor
goto back;
}
else if( c==eot)
{
rstate = FIRSTEOT;
serial->read(); // throw it on the floor
goto back;
}
return 1; // we may have more but this is the only sure bet
}
else if(rstate==SLIPESC)
return 1;
else if(rstate==FIRSTEOT)
{
if(serial->peek()==eot)
{
rstate = SECONDEOT;
serial->read(); // throw it on the floor
return 0;
}
rstate = CHAR;
}else if (rstate==SECONDEOT) {
rstate = CHAR;
}
return 0;
}
//reads a byte from the buffer
int SLIPEncodedBluetoothSerial::read(){
back:
uint8_t c = serial->read();
if(rstate==CHAR)
{
if(c==slipesc)
{
rstate=SLIPESC;
goto back;
}
else if(c==eot){
return -1; // xxx this is an error
}
return c;
}
else
if(rstate==SLIPESC)
{
rstate=CHAR;
if(c==slipescend)
return eot;
else if(c==slipescesc)
return slipesc;
else {
// insert some error code here
return -1;
}
}
else
return -1;
}
// as close as we can get to correct behavior
int SLIPEncodedBluetoothSerial::peek(){
uint8_t c = serial->peek();
if(rstate==SLIPESC)
{
if(c==slipescend)
return eot;
else if(c==slipescesc)
return slipesc;
}
return c;
}
//the arduino and wiring libraries have different return types for the write function
#if defined(WIRING) || defined(BOARD_DEFS_H)
//encode SLIP
void SLIPEncodedBluetoothSerial::write(uint8_t b){
if(b == eot){
serial->write(slipesc);
return serial->write(slipescend);
} else if(b==slipesc) {
serial->write(slipesc);
return serial->write(slipescesc);
} else {
return serial->write(b);
}
}
void SLIPEncodedBluetoothSerial::write(const uint8_t *buffer, size_t size) { while(size--) write(*buffer++); }
#else
//encode SLIP
size_t SLIPEncodedBluetoothSerial::write(uint8_t b){
if(b == eot){
serial->write(slipesc);
return serial->write(slipescend);
} else if(b==slipesc) {
serial->write(slipesc);
return serial->write(slipescesc);
} else {
return serial->write(b);
}
}
size_t SLIPEncodedBluetoothSerial::write(const uint8_t *buffer, size_t size) { size_t result=0; while(size--) result = write(*buffer++); return result; }
#endif
void SLIPEncodedBluetoothSerial::begin(String name){
serial->begin(name);
}
//SLIP specific method which begins a transmitted packet
void SLIPEncodedBluetoothSerial::beginPacket() { serial->write(eot); }
//signify the end of the packet with an EOT
void SLIPEncodedBluetoothSerial::endPacket(){
serial->write(eot);
}
void SLIPEncodedBluetoothSerial::flush(){
serial->flush();
}

View File

@ -0,0 +1,62 @@
/*
Extends the Serial class to encode SLIP over serial
*/
#ifndef SLIPEncodedBluetoothSerial_h
#define SLIPEncodedBluetoothSerial_h
#include "Arduino.h"
#include <Stream.h>
#include "BluetoothSerial.h"
class SLIPEncodedBluetoothSerial: public Stream{
private:
enum erstate {CHAR, FIRSTEOT, SECONDEOT, SLIPESC } rstate;
//the serial port used
BluetoothSerial * serial;
public:
//the serial port used
SLIPEncodedBluetoothSerial(BluetoothSerial & );
int available();
int read();
int peek();
void flush();
//same as Serial.begin
void begin(String);
//SLIP specific method which begins a transmitted packet
void beginPacket();
//SLIP specific method which ends a transmittedpacket
void endPacket();
// SLIP specific method which indicates that an EOT was received
bool endofPacket();
//the arduino and wiring libraries have different return types for the write function
#if defined(WIRING) || defined(BOARD_DEFS_H)
void write(uint8_t b);
void write(const uint8_t *buffer, size_t size);
#else
//overrides the Stream's write function to encode SLIP
size_t write(uint8_t b);
size_t write(const uint8_t *buffer, size_t size);
//using Print::write;
#endif
};
#endif

View File

@ -3,33 +3,19 @@
#include "Wire.h"
#include "MPU6050_6Axis_MotionApps20.h"
#include <OSCBoards.h>
#include <OSCMessage.h>
/*
Make an OSC message and send it over 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 dont have Serial
#endif
#include "BluetoothSerial.h"
#include <SLIPEncodedSerial.h>
#include "SLIPEncodedBluetoothSerial.h"
#if !defined(CONFIG_BT_ENABLED) || !defined(CONFIG_BLUEDROID_ENABLED)
#error Bluetooth is not enabled! Please run `make menuconfig` to and enable it
#endif
#include <OSCMessage.h>
BluetoothSerial SerialBT;
SLIPEncodedSerial SLIPSerial(SerialBT);
SLIPEncodedBluetoothSerial SLIPBTSerial(SerialBT);
SLIPEncodedSerial SLIPSerial(Serial);
MPU6050 mpu;
@ -94,7 +80,7 @@ byte pressed[] = {0, 0, 0, 0};
byte KEYLEN = 4;
OSCMessage msg("/accel/");
// OSCMessage gmsg("/gyro/");
OSCMessage gmsg("/gyro/");
OSCMessage emsg("/error/");
OSCMessage kmsg("/keys/");
OSCMessage qmsg("/quaternion/");
@ -102,37 +88,16 @@ OSCMessage qmsg("/quaternion/");
void setup() {
Wire.begin();
Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties
SLIPSerial.begin(115200); // set this as high as you can reliably run on your platform
/*
// Bluetooth serial
BLEDevice::init("Long name works now");
BLEServer *pServer = BLEDevice::createServer();
BLEService *pService = pServer->createService(SERVICE_UUID);
BLECharacteristic *pCharacteristic = pService->createCharacteristic(
CHARACTERISTIC_UUID,
BLECharacteristic::PROPERTY_READ |
BLECharacteristic::PROPERTY_WRITE
);
pCharacteristic->setValue("Hello World says Neil");
pService->start();
// BLEAdvertising *pAdvertising = pServer->getAdvertising(); // this still is working for backward compatibility
BLEAdvertising *pAdvertising = BLEDevice::getAdvertising();
pAdvertising->addServiceUUID(SERVICE_UUID);
pAdvertising->setScanResponse(true);
pAdvertising->setMinPreferred(0x06); // functions that help with iPhone connections issue
pAdvertising->setMinPreferred(0x12);
BLEDevice::startAdvertising();
*/
SerialBT.begin("wavey wind");
// Keys
for(int i = 0; i < KEYLEN; i++) {
pinMode(keys[i], INPUT_PULLUP);
}
Serial.begin(115200); // set this as high as you can reliably run on your platform
SerialBT.begin("wavey wind");
// Motion processor init
mpu.initialize();
mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_250);
mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_2);
@ -142,12 +107,23 @@ void setup() {
// supply your own gyro offsets here, scaled for min sensitivity
// !!! Run Zero IMU to get readings
/* First proto (right hand, black&blue)
mpu.setXGyroOffset(76);
mpu.setYGyroOffset(68);
mpu.setZGyroOffset(10);
mpu.setXAccelOffset(-3527);
mpu.setYAccelOffset(-913);
mpu.setZAccelOffset(1027);
*/
/* Second proto, translucent / white */
mpu.setXGyroOffset(-3650);
mpu.setYGyroOffset(-2531);
mpu.setZGyroOffset(1131);
mpu.setXAccelOffset(162);
mpu.setYAccelOffset(-16);
mpu.setZAccelOffset(-12);
// make sure it worked (returns 0 if so)
if (devStatus == 0) {
@ -167,11 +143,7 @@ void setup() {
// get expected DMP packet size for later comparison
packetSize = mpu.dmpGetFIFOPacketSize();
} else {
emsg.add("DMP Initialization failed (code " + String(devStatus) + ")");
SLIPSerial.beginPacket();
emsg.send(SLIPSerial);
SLIPSerial.endPacket();
emsg.empty();
Serial.println("DMP Initialization failed (code " + String(devStatus) + ")");
// ERROR!
// 1 = initial memory load failed
// 2 = DMP configuration updates failed
@ -190,14 +162,21 @@ void loop() {
// display quaternion values in easy matrix form: w x y z
mpu.dmpGetQuaternion(&q, fifoBuffer);
//oscmsg = qOSC(q.w, q.x, q.y, q.z);
qmsg.add(q.w);
qmsg.add(q.x);
qmsg.add(q.y);
qmsg.add(q.z);
SLIPBTSerial.beginPacket();
qmsg.send(SLIPBTSerial);
SLIPBTSerial.endPacket();
SLIPSerial.beginPacket();
qmsg.send(SLIPSerial);
SLIPSerial.endPacket();
qmsg.send(SerialBT);
qmsg.empty();
#endif
@ -248,37 +227,35 @@ void loop() {
#endif
// Send over serial
// Send (accel) over serial
msg.add(AcX);
msg.add(AcY);
msg.add(AcZ);
SLIPSerial.beginPacket();
msg.send(SLIPSerial);
SLIPSerial.endPacket();
msg.send(SerialBT);
msg.empty();
/*
gmsg.add(GyX);
gmsg.add(GyY);
gmsg.add(GyZ);
SLIPSerial.beginPacket();
gmsg.send(SLIPSerial);
SLIPSerial.endPacket();
gmsg.empty();
*/
SLIPBTSerial.beginPacket();
msg.send(SLIPBTSerial);
SLIPBTSerial.endPacket();
msg.empty();
// Send keys
for(int i = 0; i < KEYLEN; i++) {
pressed[i] = !digitalRead(keys[i]);
kmsg.add(pressed[i]);
}
SLIPSerial.beginPacket();
kmsg.send(SLIPSerial);
SLIPSerial.endPacket();
kmsg.send(SerialBT);
SLIPBTSerial.beginPacket();
kmsg.send(SLIPBTSerial);
SLIPBTSerial.endPacket();
kmsg.empty();
}
}

View File

@ -92,11 +92,10 @@ void setup() {
SLIPSerial.begin(115200); // set this as high as you can reliably run on your platform
// Keys
for(int i = 0; i < KEYLEN; i++) {
pinMode(keys[i], INPUT_PULLUP);
}
mpu.initialize();
mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_250);
mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_2);
@ -106,12 +105,23 @@ void setup() {
// supply your own gyro offsets here, scaled for min sensitivity
// !!! Run Zero IMU to get readings
/* First proto (right hand, black&blue)
mpu.setXGyroOffset(76);
mpu.setYGyroOffset(68);
mpu.setZGyroOffset(10);
mpu.setXAccelOffset(-3527);
mpu.setYAccelOffset(-913);
mpu.setZAccelOffset(1027);
*/
/* Second proto, translucent / white */
mpu.setXGyroOffset(-3650);
mpu.setYGyroOffset(-2531);
mpu.setZGyroOffset(1131);
mpu.setXAccelOffset(162);
mpu.setYAccelOffset(-16);
mpu.setZAccelOffset(-12);
// make sure it worked (returns 0 if so)
if (devStatus == 0) {