Compare commits

..

5 Commits

Author SHA1 Message Date
Jurij Podgoršek b019145c97 Dodan quaternion, serijski link 2022-11-27 09:47:28 +01:00
Jurij Podgoršek 9e0a0c5c89 MPU 9255 sketcha 2022-05-27 14:57:30 +02:00
Jurij Podgoršek 5e380911f7 Dodan gitignore 2022-05-27 14:56:57 +02:00
Jurij Podgoršek 3dc636ff7a Cleanup, dodan wemos prototip 2022-05-27 14:56:35 +02:00
Jurij Podgoršek a6b98ee5f2 Typo fix 2022-05-27 14:55:59 +02:00
63 changed files with 13066 additions and 497 deletions

2
.gitignore vendored 100644
View File

@ -0,0 +1,2 @@
\#*\#
.*.~undo-tree~

View File

@ -1,277 +0,0 @@
/**! @brief Class for managing storing "Adafruit" sensor calibration
in internal Flash memory with KVStore
for Arduino Nano 33 BLE
* **/
//https://os.mbed.com/docs/mbed-os/v6.12/apis/kvstore.html
//KVStore. TDBStore - Default implementation of the KVStore API. It provides static wear-leveling and quick access for when you have a small number of KV pairs.
//https://os.mbed.com/docs/mbed-os/v6.12/apis/data-architecture.html
#include "KVStore.h"
#include "kvstore_global_api.h"
/**! XYZ vector of offsets for zero-g, in m/s^2 */
float accel_zerog[3] = {0, 0, 0};
/**! XYZ vector of offsets for zero-rate, in rad/s */
float gyro_zerorate[3] = {0, 0, 0};
/**! XYZ vector of offsets for hard iron calibration (in uT) */
float mag_hardiron[3] = {0, 0, 0};
/**! The 3x3 matrix for soft-iron calibration (unitless) */
float mag_softiron[9] = {1, 0, 0, 0, 1, 0, 0, 0, 1};
/**! The magnetic field magnitude in uTesla */
float mag_field = 50;
const char* const ASC_KV_Key0 = "store_flag";
const char* const ASC_KV_Key1 = "store_offsets";
void setGyroCalibration(float x, float y, float z) {
gyro_zerorate[0]=x;
gyro_zerorate[1]=y;
gyro_zerorate[2]=z;
}
float getGyroXCal() {
return gyro_zerorate[0];
}
float getGyroYCal() {
return gyro_zerorate[1];
}
float getGyroZCal() {
return gyro_zerorate[2];
}
bool saveCalibration(void) {
Serial.println("Save Cal");
kv_reset("/kv/");
uint8_t flag=42;
kv_set(ASC_KV_Key0,&flag,sizeof(flag),0);
float offsets[16];
memcpy(offsets, accel_zerog, 12); // 3 x 4-byte floats
memcpy(offsets + 3, gyro_zerorate, 12); // 3 x 4-byte floats
memcpy(offsets + 6, mag_hardiron, 12); // 3 x 4-byte floats
offsets[9] = mag_field;
offsets[10] = mag_softiron[0];
offsets[11] = mag_softiron[4];
offsets[12] = mag_softiron[8];
offsets[13] = mag_softiron[1];
offsets[14] = mag_softiron[2];
offsets[15] = mag_softiron[5];
kv_set(ASC_KV_Key1, (uint8_t*)&offsets, sizeof(offsets), 0);
return true;
}
bool loadCalibration(void) {
uint8_t flag;
kv_get(ASC_KV_Key0,&flag,sizeof(flag),0);
if (flag=!42) {
Serial.print("FLAG NOT SET, CALIBRATION NOT FOUND");
return false;
}
float offsets[16];
kv_get(ASC_KV_Key1,(byte*)&offsets,sizeof(offsets),0);
accel_zerog[0] = offsets[0];
accel_zerog[1] = offsets[1];
accel_zerog[2] = offsets[2];
gyro_zerorate[0] = offsets[3];
gyro_zerorate[1] = offsets[4];
gyro_zerorate[2] = offsets[5];
mag_hardiron[0] = offsets[6];
mag_hardiron[1] = offsets[7];
mag_hardiron[2] = offsets[8];
mag_field = offsets[9];
mag_softiron[0] = offsets[10];
mag_softiron[1] = offsets[13];
mag_softiron[2] = offsets[14];
mag_softiron[3] = offsets[13];
mag_softiron[4] = offsets[11];
mag_softiron[5] = offsets[15];
mag_softiron[6] = offsets[14];
mag_softiron[7] = offsets[15];
mag_softiron[8] = offsets[12];
return true;
}
bool printCalibration(void) {
Serial.println(F("------------"));
Serial.print("Accelerometer: ");
Serial.print(accel_zerog[0]);
Serial.print(", ");
Serial.print(accel_zerog[1]);
Serial.print(", ");
Serial.print(accel_zerog[2]);
Serial.println();
Serial.print("Gyroscope: ");
Serial.print(gyro_zerorate[0]);
Serial.print(", ");
Serial.print(gyro_zerorate[1]);
Serial.print(", ");
Serial.print(gyro_zerorate[2]);
Serial.println();
Serial.print("Magnetometer Hard Iron: ");
Serial.print(mag_hardiron[0]);
Serial.print(", ");
Serial.print(mag_hardiron[1]);
Serial.print(", ");
Serial.print(mag_hardiron[2]);
Serial.println();
Serial.print("Magnetic Field: ");
Serial.print(mag_field);
Serial.println();
Serial.print("Magnetometer Soft Iron: ");
Serial.print(mag_softiron[0]);
Serial.print(", ");
Serial.print(mag_softiron[1]);
Serial.print(", ");
Serial.print(mag_softiron[2]);
Serial.println();
Serial.print(mag_softiron[3]);
Serial.print(", ");
Serial.print(mag_softiron[4]);
Serial.print(", ");
Serial.print(mag_softiron[5]);
Serial.println();
Serial.print(mag_softiron[6]);
Serial.print(", ");
Serial.print(mag_softiron[7]);
Serial.print(", ");
Serial.print(mag_softiron[8]);
Serial.println();
Serial.println(F("\n------------"));
return true;
}
bool printSavedCalibration(void) {
Serial.println(F("------------"));
uint8_t flag;
kv_get(ASC_KV_Key0,&flag,sizeof(flag),0);
if (flag=!42) {
Serial.print("FLAG NOT SET, CALIBRATION NOT FOUND");
return false;
}
float offsets[16];
kv_get(ASC_KV_Key1,(byte*)&offsets,sizeof(offsets),0);
accel_zerog[0] = offsets[0];
accel_zerog[1] = offsets[1];
accel_zerog[2] = offsets[2];
Serial.print("Accelerometer: ");
Serial.print(accel_zerog[0]);
Serial.print(", ");
Serial.print(accel_zerog[1]);
Serial.print(", ");
Serial.print(accel_zerog[2]);
Serial.println();
gyro_zerorate[0] = offsets[3];
gyro_zerorate[1] = offsets[4];
gyro_zerorate[2] = offsets[5];
Serial.print("Gyroscope: ");
Serial.print(gyro_zerorate[0]);
Serial.print(", ");
Serial.print(gyro_zerorate[1]);
Serial.print(", ");
Serial.print(gyro_zerorate[2]);
Serial.println();
mag_hardiron[0] = offsets[6];
mag_hardiron[1] = offsets[7];
mag_hardiron[2] = offsets[8];
Serial.println("Magnetometer Hard Iron: ");
Serial.print(mag_hardiron[0]);
Serial.print(", ");
Serial.print(mag_hardiron[1]);
Serial.print(", ");
Serial.print(mag_hardiron[2]);
Serial.println();
mag_field = offsets[9];
Serial.print("Magnetic Field: ");
Serial.print(mag_field);
Serial.println();
mag_softiron[0] = offsets[10];
mag_softiron[1] = offsets[13];
mag_softiron[2] = offsets[14];
mag_softiron[3] = offsets[13];
mag_softiron[4] = offsets[11];
mag_softiron[5] = offsets[15];
mag_softiron[6] = offsets[14];
mag_softiron[7] = offsets[15];
mag_softiron[8] = offsets[12];
Serial.print("Magnetometer Soft Iron: ");
Serial.print(mag_softiron[0]);
Serial.print(", ");
Serial.print(mag_softiron[1]);
Serial.print(", ");
Serial.print(mag_softiron[2]);
Serial.println();
Serial.print(mag_softiron[3]);
Serial.print(", ");
Serial.print(mag_softiron[4]);
Serial.print(", ");
Serial.print(mag_softiron[5]);
Serial.println();
Serial.print(mag_softiron[6]);
Serial.print(", ");
Serial.print(mag_softiron[7]);
Serial.print(", ");
Serial.print(mag_softiron[8]);
Serial.println();
/* for (uint16_t a = ee_addr; a < ee_addr + KVStore_CAL_SIZE; a++) {
uint8_t c = KVStore.read(a);
Serial.print("0x");
if (c < 0x10)
Serial.print('0');
Serial.print(c, HEX);
Serial.print(", ");
if ((a - ee_addr) % 16 == 15) {
Serial.println();
}
}
*/
Serial.println(F("\n------------"));
return true;
}

View File

@ -1,7 +1,7 @@
#include <Arduino_LSM9DS1.h>
#include <BLEMIDI_Transport.h>
#include "SensorFusion.h" //SF
#include <BLEMIDI_Transport.h>
//#include <hardware/BLEMIDI_ESP32_NimBLE.h>
//#include <hardware/BLEMIDI_ESP32.h>
//#include <hardware/BLEMIDI_nRF52.h>
@ -9,7 +9,6 @@
BLEMIDI_CREATE_DEFAULT_INSTANCE()
unsigned long tm = millis();
unsigned long t0 = millis();
bool isConnected = false;
@ -44,30 +43,18 @@ float gx, gy, gz, ax, ay, az, mx, my, mz;
float pitch, roll, yaw;
float deltat;
float gxoff = 0;
float gyoff = 0;
float gzoff = 0;
//Gyro Offset....: -605622 -24028 -338386
float goffx = -605622.0 / 1000000.0;
float goffy = -24028.0 / 1000000.0;
float goffz = -338386.0 / 1000000.0;
float fmap(float x, float in_min, float in_max, float out_min, float out_max)
{
return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}
byte highbits(float f) {
return int(f) % 128;
}
byte lowbits(float f) {
return int(f * 128) % 128;
};
unsigned long loops = 0;
unsigned long loopmillis = 0;
unsigned long midis = 0;
unsigned long midimillis = 0;
// -----------------------------------------------------------------------------
// When BLE connected, LED will turn on (indication that connection was successful)
// When receiving a NoteOn, LED will go out, on NoteOff, light comes back on.
@ -77,16 +64,8 @@ void setup()
{
Serial.begin(115200);
// while (!Serial) {
; // wait for serial port to connect. Needed for native USB
// }
loopmillis = millis();
midimillis = millis();
Serial.println("MIDI-BLE TEST 3");
Serial.println("2022-06-16");
Serial.println("MIDI-BLE TEST 1");
Serial.println("2022-03-02");
MIDI.begin();
@ -114,21 +93,7 @@ void setup()
digitalWrite(LED_BUILTIN, HIGH);
});
// gyro calibration
Serial.print("Loading calibration: ");
if (loadCalibration()) {
gxoff=getGyroXCal();
gyoff=getGyroYCal();
gyoff=getGyroZCal();
Serial.println("success :)");
} else {
Serial.println("failed :(");
}
printCalibration();
// motion sensor
// accelerometer
if (!IMU.begin()) {
Serial.println("Failed to initialize IMU!");
while (1);
@ -154,12 +119,6 @@ void setup()
Serial.println();
Serial.println("Magnetic Field in uT");
Serial.println("X\tY\tZ");
Serial.print("Setup took (ms): ");
Serial.println(millis()-loopmillis);
loopmillis = millis();
midimillis = millis();
}
// -----------------------------------------------------------------------------
@ -167,29 +126,15 @@ void setup()
// -----------------------------------------------------------------------------
void loop()
{
// now you should read the gyroscope, accelerometer (and magnetometer if you have it also)
// NOTE: the gyroscope data have to be in radians
// if you have them in degree convert them with: DEG_TO_RAD example: gx * DEG_TO_RAD
if (IMU.magneticFieldAvailable()) {
IMU.readMagneticField(mx, my, mz);
}
if (IMU.accelerationAvailable()&&IMU.gyroscopeAvailable()) {
tm = millis();
loops++;
if (!(loops%1000)) {
Serial.print("1000 loops took (ms): ");
Serial.println(tm-loopmillis);
loopmillis=tm;
}
MIDI.read();
IMU.readAcceleration(ax, ay, az);
/*
ax *= 9.81;
@ -199,9 +144,9 @@ void loop()
IMU.readGyroscope(gx, gy, gz);
gx -= gxoff;
gy -= gxoff;
gz -= gxoff;
gx -= goffx;
gy -= goffy;
gz -= goffz;
gx *= DEG_TO_RAD;
gy *= DEG_TO_RAD;
@ -211,119 +156,36 @@ void loop()
deltat = fusion.deltatUpdate(); //this have to be done before calling the fusion update
//choose only one of these two:
//fusion.MahonyUpdate(gx, gy, gz, ax, ay, az, deltat); //mahony is suggested if there isn't the mag and the mcu is slow
fusion.MadgwickUpdate(gx, gy, gz, ax, ay, az, -mx, my, mz, deltat); //else use the magwick, it is slower but more accurate
fusion.MadgwickUpdate(gx, gy, gz, ax, ay, az, mx, my, mz, deltat); //else use the magwick, it is slower but more accurate
pitch = fusion.getPitch();
roll = fusion.getRoll(); //you could also use getRollRadians() ecc
yaw = fusion.getYaw();
if (isConnected && (tm - t0) > 6)
{
t0 = tm;
switch (midis) {
case 0:
maccx = fmap(ax,-4.0,4.0, 0.0,127.0);
MIDI.sendControlChange(0 + hand, maccx, midichannel);
break;
case 1:
maccy = fmap(ay,-4.0,4.0, 0.0,127.0);
MIDI.sendControlChange(1 + hand, maccy, midichannel);
break;
case 2:
maccz = fmap(az,-4.0,4.0, 0.0,127.0);
MIDI.sendControlChange(2 + hand, maccz, midichannel);
break;
case 3:
mgyrox = map(gx, -2000*DEG_TO_RAD, 2000*DEG_TO_RAD, 0.0, 127.0);
MIDI.sendControlChange(3 + hand, mgyrox, midichannel);
break;
case 4:
mgyroy = map(gy, -2000*DEG_TO_RAD, 2000*DEG_TO_RAD, 0.0, 127.0);
MIDI.sendControlChange(4 + hand, mgyroy, midichannel);
break;
case 5:
mgyroz = map(gz, -2000*DEG_TO_RAD, 2000*DEG_TO_RAD, 0.0, 127.0);
MIDI.sendControlChange(5 + hand, mgyroz, midichannel);
break;
case 6:
accmag = sqrt(ax*ax+ay*ay+az*az);
// accmag = map(accmag, 0, 1000, 0,127);
MIDI.sendControlChange(6 + hand, accmag * 100, midichannel);
break;
case 7:
mmagx = fmap(mx, 0,60, 0,127);
MIDI.sendControlChange(7 + hand, mmagx, midichannel);
break;
case 8:
mmagy = fmap(my, 0,60, 0,127);
MIDI.sendControlChange(8 + hand, mmagy, midichannel);
break;
case 9:
mmagz = fmap(mz, 0,60, 0,127);
MIDI.sendControlChange(9 + hand, mmagz, midichannel);
break;
case 10:
roll = fusion.getRoll(); //you could also use getRollRadians() ecc
mroll = fmap(roll,-180,180,0,127);
MIDI.sendControlChange(10 + hand, mroll, midichannel);
break;
case 11:
pitch = fusion.getPitch();
mpitch = fmap(pitch,-90,90,0,127);
MIDI.sendControlChange(11 + hand, mpitch, midichannel);
break;
case 12:
MIDI.sendControlChange(12 + hand, myaw, midichannel);
yaw = fusion.getYaw();
myaw = fmap(yaw,0,360,0,127);
break;
case 13:
roll = fusion.getRoll(); //you could also use getRollRadians() ecc
mroll = fmap(roll,-180,180,0,127);
MIDI.sendControlChange(13 + hand, lowbits(mroll), midichannel);
break;
case 14:
pitch = fusion.getPitch();
mpitch = fmap(pitch,-90,90,0,127);
MIDI.sendControlChange(14 + hand, lowbits(mpitch), midichannel);
break;
case 15:
MIDI.sendControlChange(15 + hand, lowbits(myaw), midichannel);
yaw = fusion.getYaw();
myaw = fmap(yaw,0,360,0,127);
break;
}
maccx = fmap(ax,-4.0,4.0, 0.0,127.0);
maccy = fmap(ay,-4.0,4.0, 0.0,127.0);
maccz = fmap(az,-4.0,4.0, 0.0,127.0);
//mgyrox = gyrox/2000 * 64 + 64;
//mgyroy = gyroy/2000 * 64 + 64;
//mgyroz = gyroz/2000 * 64 + 64;
mgyrox = map(gx, -2000,2000, 0.0, 127.0);
mgyroy = map(gy, -2000,2000, 0.0, 127.0);
mgyroz = map(gz, -2000,2000, 0.0, 127.0);
// mz = max(10, abs(z) * 1000);
mmagx = fmap(mx, 0,60, 0,127);
mmagy = fmap(my, 0,60, 0,127);
mmagz = fmap(mz, 0,60, 0,127);
// maxx = max(magx, maxx);
// maxy = max(magy, maxy);
// maxz = max(magz, maxz);
maxx = max(magx, maxx);
maxy = max(magy, maxy);
maxz = max(magz, maxz);
// Serial.print(mx);
// Serial.print('\t');
@ -332,9 +194,30 @@ break;
// Serial.print('\t');
// Serial.println(z);
// Serial.println();
}
MIDI.read();
if (isConnected && (millis() - t0) > 10)
{
t0 = millis();
// MIDI.sendNoteOn (my, mx, 1); // note 60, velocity 100 on channel 1
MIDI.sendControlChange(0 + hand, maccx, midichannel);
MIDI.sendControlChange(1 + hand, maccy, midichannel);
MIDI.sendControlChange(2 + hand, maccz, midichannel);
MIDI.sendControlChange(3 + hand, mgyrox, midichannel);
MIDI.sendControlChange(4 + hand, mgyroy, midichannel);
MIDI.sendControlChange(5 + hand, mgyroz, midichannel);
MIDI.sendControlChange(6, accmag * 100, midichannel);
MIDI.sendControlChange(7 + hand, mmagx, midichannel);
MIDI.sendControlChange(8 + hand, mmagy, midichannel);
MIDI.sendControlChange(9 + hand, mmagz, midichannel);
MIDI.sendControlChange(10 + hand, mroll, midichannel);
MIDI.sendControlChange(11 + hand, mpitch, midichannel);
MIDI.sendControlChange(12 + hand, myaw, midichannel);
// Serial.print(mmagx);
// Serial.print('\t');
// Serial.print(mmagy);
@ -345,25 +228,5 @@ break;
// Serial.println("ping");
// delay(mz);
// MIDI.sendNoteOff(my, mx, 1);
midis=(midis+1)%16;
if (midis==0) {
Serial.print("MIDI sending took (ms): ");
Serial.println(tm-midimillis);
midimillis=tm;
}
}
}
}

View File

@ -21,7 +21,7 @@
Ideja izhaja iz zgodnješega projekta abstraktne vizualizacije, ki naj bi odsevala glasbo. Po izgradnji zgodnjega prototipa z vmesnikom na drsnikov na dotik, ki modulirajo prikaz, sem se vprašal zakaj bi program interpretiral glasbo? To namreč počnemo že mi, na poslušanje (lahko) odreagiramo s plesom, gibalni senzor pa bi lahko telesne gibe "ojačal" v oblike in barve, projecirane na površino.
Z gibalno rokvičko se opravljalka_ec vizualij lahko izogne ujetosti v neroden zaslon na dotitk in potopi raje v izkušnjo zvoka in videa med soustvarjanjem le-te. Organizirana bo tudi delavnica za izgradnjo večih rokavičk, ki se lahko med sabo povežejo v ad-hoc mrežo, kar skupini ljudi omogoči sodelovanje.
Z gibalno rokavičko se opravljalka_ec vizualij lahko izogne ujetosti v neroden zaslon na dotitk in potopi raje v izkušnjo zvoka in videa med soustvarjanjem le-te. Organizirana bo tudi delavnica za izgradnjo večih rokavičk, ki se lahko med sabo povežejo v ad-hoc mrežo, kar skupini ljudi omogoči sodelovanje.
Rokavica oz. rokavice bodo pošiljale podatke preko OSC (open sound control) protokola, kar odpira tudi možnost rabe za zvočno sintezo/modulacijo ali celo kot splošni vmesnik.

View File

@ -1,23 +0,0 @@
// Keyboard Matrix Tutorial Example
// baldengineer.com
// CC BY-SA 4.0
// JP1 is an input
byte keys[] = {16, 17, 5, 18};
byte pressed[] = {0, 0, 0, 0};
byte KEYLEN = 4;
void setup() {
Serial.begin(9600);
for(int i = 0; i < KEYLEN; i++) {
pinMode(keys[i], INPUT_PULLUP);
}
}
void loop() {
for(int i = 0; i < KEYLEN; i++) {
pressed[i] = !digitalRead(keys[i]);
Serial.print(String(pressed[i]) + " ");
}
Serial.println();
delay(50);
}

View File

@ -0,0 +1,127 @@
////////////////////////////////////////////////////////////////////////////
//
// This file is part of RTIMULib-Arduino
//
// Copyright (c) 2014-2015, richards-tech
//
// Permission is hereby granted, free of charge, to any person obtaining a copy of
// this software and associated documentation files (the "Software"), to deal in
// the Software without restriction, including without limitation the rights to use,
// copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the
// Software, and to permit persons to whom the Software is furnished to do so,
// subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
// INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
// PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
// HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
// OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
// SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
#include <Wire.h>
#include "I2Cdev.h"
#include "RTIMULib/RTIMUSettings.h"
#include "RTIMULib/RTIMU.h"
#include "RTIMULib/RTFusionRTQF.h"
#include "CalLib/CalLib.h"
#include <EEPROM.h>
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 100 // interval between pose displays
// SERIAL_PORT_SPEED defines the speed to use for the debug serial port
#define SERIAL_PORT_SPEED 115200
unsigned long lastDisplay;
unsigned long lastRate;
int sampleCount;
RTQuaternion gravity;
void setup()
{
int errcode;
Serial.begin(SERIAL_PORT_SPEED);
Wire.begin();
imu = RTIMU::createIMU(&settings); // create the imu object
Serial.print("ArduinoIMU starting using device "); Serial.println(imu->IMUName());
if ((errcode = imu->IMUInit()) < 0) {
Serial.print("Failed to init IMU: "); Serial.println(errcode);
}
if (imu->getCalibrationValid())
Serial.println("Using compass calibration");
else
Serial.println("No valid compass calibration data");
lastDisplay = lastRate = millis();
sampleCount = 0;
gravity.setScalar(0);
gravity.setX(0);
gravity.setY(0);
gravity.setZ(1);
}
void loop()
{
unsigned long now = millis();
unsigned long delta;
RTVector3 realAccel;
RTQuaternion rotatedGravity;
RTQuaternion fusedConjugate;
RTQuaternion qTemp;
int loopCount = 0;
while (imu->IMURead()) { // get the latest data if ready yet
// this flushes remaining data in case we are falling behind
if (++loopCount >= 10)
continue;
fusion.newIMUData(imu->getGyro(), imu->getAccel(), imu->getCompass(), imu->getTimestamp());
// do gravity rotation and subtraction
// create the conjugate of the pose
fusedConjugate = fusion.getFusionQPose().conjugate();
// now do the rotation - takes two steps with qTemp as the intermediate variable
qTemp = gravity * fusion.getFusionQPose();
rotatedGravity = fusedConjugate * qTemp;
// now adjust the measured accel and change the signs to make sense
realAccel.setX(-(imu->getAccel().x() - rotatedGravity.x()));
realAccel.setY(-(imu->getAccel().y() - rotatedGravity.y()));
realAccel.setZ(-(imu->getAccel().z() - rotatedGravity.z()));
sampleCount++;
if ((delta = now - lastRate) >= 1000) {
Serial.print("Sample rate: "); Serial.print(sampleCount);
if (!imu->IMUGyroBiasValid())
Serial.println(", calculating gyro bias");
else
Serial.println();
sampleCount = 0;
lastRate = now;
}
if ((now - lastDisplay) >= DISPLAY_INTERVAL) {
lastDisplay = now;
RTMath::display("Accel:", realAccel);
Serial.println();
}
}
}

View File

@ -0,0 +1,95 @@
////////////////////////////////////////////////////////////////////////////
//
// This file is part of RTIMULib-Arduino
//
// Copyright (c) 2014-2015, richards-tech
//
// Permission is hereby granted, free of charge, to any person obtaining a copy of
// this software and associated documentation files (the "Software"), to deal in
// the Software without restriction, including without limitation the rights to use,
// copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the
// Software, and to permit persons to whom the Software is furnished to do so,
// subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
// INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
// PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
// HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
// OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
// SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
#include "CalLib.h"
#ifdef __SAM3X8E__
// Due version
#include "DueFlash.h"
DueFlash flash;
void calLibErase(byte device)
{
uint32_t data = 0;
flash.write(CALLIB_START + sizeof(CALLIB_DATA) * device, &data, 1); // just destroy the valid byte
}
void calLibWrite(byte device, CALLIB_DATA *calData)
{
calData->validL = CALLIB_DATA_VALID_LOW;
calData->validH = CALLIB_DATA_VALID_HIGH;
flash.write(CALLIB_START + sizeof(CALLIB_DATA) * device, (uint32_t *)calData, sizeof(CALLIB_DATA) / 4);
}
boolean calLibRead(byte device, CALLIB_DATA *calData)
{
memcpy(calData, CALLIB_START + sizeof(CALLIB_DATA) * device, sizeof(CALLIB_DATA));
return calData->valid == CALLIB_DATA_VALID;
}
#else
// AVR version
#include <EEPROM.h>
void calLibErase(byte device)
{
EEPROM.write(sizeof(CALLIB_DATA) * device, 0); // just destroy the valid byte
}
void calLibWrite(byte device, CALLIB_DATA *calData)
{
byte *ptr = (byte *)calData;
byte length = sizeof(CALLIB_DATA);
int eeprom = sizeof(CALLIB_DATA) * device;
calData->validL = CALLIB_DATA_VALID_LOW;
calData->validH = CALLIB_DATA_VALID_HIGH;
for (byte i = 0; i < length; i++)
EEPROM.write(eeprom + i, *ptr++);
}
boolean calLibRead(byte device, CALLIB_DATA *calData)
{
byte *ptr = (byte *)calData;
byte length = sizeof(CALLIB_DATA);
int eeprom = sizeof(CALLIB_DATA) * device;
calData->magValid = false;
if ((EEPROM.read(eeprom) != CALLIB_DATA_VALID_LOW) ||
(EEPROM.read(eeprom + 1) != CALLIB_DATA_VALID_HIGH)) {
return false; // invalid data
}
for (byte i = 0; i < length; i++)
*ptr++ = EEPROM.read(eeprom + i);
return true;
}
#endif

View File

@ -0,0 +1,58 @@
////////////////////////////////////////////////////////////////////////////
//
// This file is part of RTIMULib-Arduino
//
// Copyright (c) 2014-2015, richards-tech
//
// Permission is hereby granted, free of charge, to any person obtaining a copy of
// this software and associated documentation files (the "Software"), to deal in
// the Software without restriction, including without limitation the rights to use,
// copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the
// Software, and to permit persons to whom the Software is furnished to do so,
// subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
// INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
// PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
// HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
// OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
// SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
#ifndef _CALLIB_H_
#define _CALLIB_H_
#include <Arduino.h>
#define CALLIB_DATA_VALID_LOW 0xfc // pattern to detect valid config - low byte
#define CALLIB_DATA_VALID_HIGH 0x15 // pattern to detect valid config - high byte
#ifdef __SAM3X8E__
#define CALLIB_START ((uint32_t *)(IFLASH1_ADDR + IFLASH1_SIZE - IFLASH1_PAGE_SIZE))
#endif
typedef struct
{
unsigned char validL; // should contain the valid pattern if a good config
unsigned char validH; // should contain the valid pattern if a good config
unsigned char magValid; // true if data valid
unsigned char pad;
float magMin[3]; // min values
float magMax[3]; // max values
} CALLIB_DATA;
// calLibErase() erases any current data in the EEPROM
void calLibErase(byte device);
// calLibWrite() writes new data to the EEPROM
void calLibWrite(byte device, CALLIB_DATA * calData);
// calLibRead() reads existing data and returns true if valid else false in not.
boolean calLibRead(byte device, CALLIB_DATA * calData);
#endif // _CALLIB_H_

File diff suppressed because it is too large Load Diff

View File

@ -0,0 +1,269 @@
// I2Cdev library collection - Main I2C device class header file
// Abstracts bit and byte I2C R/W functions into a convenient class
// 6/9/2012 by Jeff Rowberg <jeff@rowberg.net>
//
// Changelog:
// 2013-05-06 - add Francesco Ferrara's Fastwire v0.24 implementation with small modifications
// 2013-05-05 - fix issue with writing bit values to words (Sasquatch/Farzanegan)
// 2012-06-09 - fix major issue with reading > 32 bytes at a time with Arduino Wire
// - add compiler warnings when using outdated or IDE or limited I2Cdev implementation
// 2011-11-01 - fix write*Bits mask calculation (thanks sasquatch @ Arduino forums)
// 2011-10-03 - added automatic Arduino version detection for ease of use
// 2011-10-02 - added Gene Knight's NBWire TwoWire class implementation with small modifications
// 2011-08-31 - added support for Arduino 1.0 Wire library (methods are different from 0.x)
// 2011-08-03 - added optional timeout parameter to read* methods to easily change from default
// 2011-08-02 - added support for 16-bit registers
// - fixed incorrect Doxygen comments on some methods
// - added timeout value for read operations (thanks mem @ Arduino forums)
// 2011-07-30 - changed read/write function structures to return success or byte counts
// - made all methods static for multi-device memory savings
// 2011-07-28 - initial release
/* ============================================
I2Cdev device library code is placed under the MIT license
Copyright (c) 2013 Jeff Rowberg
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
===============================================
*/
#ifndef _I2CDEV_H_
#define _I2CDEV_H_
// -----------------------------------------------------------------------------
// I2C interface implementation setting
// -----------------------------------------------------------------------------
#define I2CDEV_IMPLEMENTATION I2CDEV_ARDUINO_WIRE
//#define I2CDEV_IMPLEMENTATION I2CDEV_BUILTIN_FASTWIRE
// comment this out if you are using a non-optimal IDE/implementation setting
// but want the compiler to shut up about it
#define I2CDEV_IMPLEMENTATION_WARNINGS
// -----------------------------------------------------------------------------
// I2C interface implementation options
// -----------------------------------------------------------------------------
#define I2CDEV_ARDUINO_WIRE 1 // Wire object from Arduino
#define I2CDEV_BUILTIN_NBWIRE 2 // Tweaked Wire object from Gene Knight's NBWire project
// ^^^ NBWire implementation is still buggy w/some interrupts!
#define I2CDEV_BUILTIN_FASTWIRE 3 // FastWire object from Francesco Ferrara's project
#define I2CDEV_I2CMASTER_LIBRARY 4 // I2C object from DSSCircuits I2C-Master Library at https://github.com/DSSCircuits/I2C-Master-Library
// -----------------------------------------------------------------------------
// Arduino-style "Serial.print" debug constant (uncomment to enable)
// -----------------------------------------------------------------------------
//#define I2CDEV_SERIAL_DEBUG
#ifdef ARDUINO
#if ARDUINO < 100
#include "WProgram.h"
#else
#include "Arduino.h"
#endif
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include <Wire.h>
#endif
#if I2CDEV_IMPLEMENTATION == I2CDEV_I2CMASTER_LIBRARY
#include <I2C.h>
#endif
#endif
// 1000ms default read timeout (modify with "I2Cdev::readTimeout = [ms];")
#define I2CDEV_DEFAULT_READ_TIMEOUT 1000
class I2Cdev {
public:
I2Cdev();
static int8_t readBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout);
static int8_t readBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout);
static int8_t readBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout);
static int8_t readBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout);
static int8_t readByte(uint8_t devAddr, uint8_t regAddr, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout);
static int8_t readWord(uint8_t devAddr, uint8_t regAddr, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout);
static int8_t readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout);
static int8_t readWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout);
static bool writeBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t data);
static bool writeBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t data);
static bool writeBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t data);
static bool writeBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t data);
static bool writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data);
static bool writeWord(uint8_t devAddr, uint8_t regAddr, uint16_t data);
static bool writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data);
static bool writeWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data);
static uint16_t readTimeout;
};
#if I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
//////////////////////
// FastWire 0.24
// This is a library to help faster programs to read I2C devices.
// Copyright(C) 2012
// Francesco Ferrara
//////////////////////
/* Master */
#define TW_START 0x08
#define TW_REP_START 0x10
/* Master Transmitter */
#define TW_MT_SLA_ACK 0x18
#define TW_MT_SLA_NACK 0x20
#define TW_MT_DATA_ACK 0x28
#define TW_MT_DATA_NACK 0x30
#define TW_MT_ARB_LOST 0x38
/* Master Receiver */
#define TW_MR_ARB_LOST 0x38
#define TW_MR_SLA_ACK 0x40
#define TW_MR_SLA_NACK 0x48
#define TW_MR_DATA_ACK 0x50
#define TW_MR_DATA_NACK 0x58
#define TW_OK 0
#define TW_ERROR 1
class Fastwire {
private:
static boolean waitInt();
public:
static void setup(int khz, boolean pullup);
static byte beginTransmission(byte device);
static byte write(byte value);
static byte writeBuf(byte device, byte address, byte *data, byte num);
static byte readBuf(byte device, byte address, byte *data, byte num);
static void reset();
static byte stop();
};
#endif
#if I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE
// NBWire implementation based heavily on code by Gene Knight <Gene@Telobot.com>
// Originally posted on the Arduino forum at http://arduino.cc/forum/index.php/topic,70705.0.html
// Originally offered to the i2cdevlib project at http://arduino.cc/forum/index.php/topic,68210.30.html
#define NBWIRE_BUFFER_LENGTH 32
class TwoWire {
private:
static uint8_t rxBuffer[];
static uint8_t rxBufferIndex;
static uint8_t rxBufferLength;
static uint8_t txAddress;
static uint8_t txBuffer[];
static uint8_t txBufferIndex;
static uint8_t txBufferLength;
// static uint8_t transmitting;
static void (*user_onRequest)(void);
static void (*user_onReceive)(int);
static void onRequestService(void);
static void onReceiveService(uint8_t*, int);
public:
TwoWire();
void begin();
void begin(uint8_t);
void begin(int);
void beginTransmission(uint8_t);
//void beginTransmission(int);
uint8_t endTransmission(uint16_t timeout=0);
void nbendTransmission(void (*function)(int)) ;
uint8_t requestFrom(uint8_t, int, uint16_t timeout=0);
//uint8_t requestFrom(int, int);
void nbrequestFrom(uint8_t, int, void (*function)(int));
void send(uint8_t);
void send(uint8_t*, uint8_t);
//void send(int);
void send(char*);
uint8_t available(void);
uint8_t receive(void);
void onReceive(void (*)(int));
void onRequest(void (*)(void));
};
#define TWI_READY 0
#define TWI_MRX 1
#define TWI_MTX 2
#define TWI_SRX 3
#define TWI_STX 4
#define TW_WRITE 0
#define TW_READ 1
#define TW_MT_SLA_NACK 0x20
#define TW_MT_DATA_NACK 0x30
#define CPU_FREQ 16000000L
#define TWI_FREQ 100000L
#define TWI_BUFFER_LENGTH 32
/* TWI Status is in TWSR, in the top 5 bits: TWS7 - TWS3 */
#define TW_STATUS_MASK (_BV(TWS7)|_BV(TWS6)|_BV(TWS5)|_BV(TWS4)|_BV(TWS3))
#define TW_STATUS (TWSR & TW_STATUS_MASK)
#define TW_START 0x08
#define TW_REP_START 0x10
#define TW_MT_SLA_ACK 0x18
#define TW_MT_SLA_NACK 0x20
#define TW_MT_DATA_ACK 0x28
#define TW_MT_DATA_NACK 0x30
#define TW_MT_ARB_LOST 0x38
#define TW_MR_ARB_LOST 0x38
#define TW_MR_SLA_ACK 0x40
#define TW_MR_SLA_NACK 0x48
#define TW_MR_DATA_ACK 0x50
#define TW_MR_DATA_NACK 0x58
#define TW_ST_SLA_ACK 0xA8
#define TW_ST_ARB_LOST_SLA_ACK 0xB0
#define TW_ST_DATA_ACK 0xB8
#define TW_ST_DATA_NACK 0xC0
#define TW_ST_LAST_DATA 0xC8
#define TW_SR_SLA_ACK 0x60
#define TW_SR_ARB_LOST_SLA_ACK 0x68
#define TW_SR_GCALL_ACK 0x70
#define TW_SR_ARB_LOST_GCALL_ACK 0x78
#define TW_SR_DATA_ACK 0x80
#define TW_SR_DATA_NACK 0x88
#define TW_SR_GCALL_DATA_ACK 0x90
#define TW_SR_GCALL_DATA_NACK 0x98
#define TW_SR_STOP 0xA0
#define TW_NO_INFO 0xF8
#define TW_BUS_ERROR 0x00
//#define _MMIO_BYTE(mem_addr) (*(volatile uint8_t *)(mem_addr))
//#define _SFR_BYTE(sfr) _MMIO_BYTE(_SFR_ADDR(sfr))
#ifndef sbi // set bit
#define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit))
#endif // sbi
#ifndef cbi // clear bit
#define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit))
#endif // cbi
extern TwoWire Wire;
#endif // I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE
#endif /* _I2CDEV_H_ */

View File

@ -0,0 +1,231 @@
///////////////////////////////////////////////////////////
//
// This file is part of RTArduLink
//
// Copyright (c) 2014-2015 richards-tech
//
// Permission is hereby granted, free of charge,
// to any person obtaining a copy of
// this software and associated documentation files
// (the "Software"), to deal in the Software without
// restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute,
// sublicense, and/or sell copies of the Software, and
// to permit persons to whom the Software is furnished
// to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice
// shall be included in all copies or substantial portions
// of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF
// ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED
// TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
// PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY
// CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
// OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR
// IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
// DEALINGS IN THE SOFTWARE.
#include "RTArduLink.h"
#include "RTArduLinkHAL.h"
#include "RTArduLinkUtils.h"
#include <string.h>
RTArduLink::RTArduLink()
{
}
RTArduLink::~RTArduLink()
{
}
void RTArduLink::begin(const char *identitySuffix)
{
RTARDULINK_PORT *portInfo;
if (!RTArduLinkHALEEPROMValid())
RTArduLinkHALEEPROMDefault();
m_identitySuffix = identitySuffix;
// now set up host and subsystem ports based on EEPROM configuration
for (int i = 0; i < RTARDULINKHAL_MAX_PORTS; i++) {
portInfo = m_ports + i;
portInfo->index = i;
portInfo->inUse = RTArduLinkHALConfigurePort(&(portInfo->portHAL), i);
RTArduLinkRXFrameInit(&(portInfo->RXFrame), &(portInfo->RXFrameBuffer));
}
m_hostPort = m_ports;
}
void RTArduLink::background()
{
unsigned char index;
RTARDULINK_PORT *portInfo;
for (index = 0; index < RTARDULINKHAL_MAX_PORTS; index++) {
portInfo = m_ports + index;
if (!portInfo->inUse)
continue;
while (RTArduLinkHALPortAvailable(&(portInfo->portHAL))) {
if (!RTArduLinkReassemble(&(portInfo->RXFrame), RTArduLinkHALPortRead(&(portInfo->portHAL)))) {
sendDebugMessage("Reassembly error");
} else {
if (portInfo->RXFrame.complete) {
processReceivedMessage(portInfo);
RTArduLinkRXFrameInit(&(portInfo->RXFrame), &(portInfo->RXFrameBuffer));
}
}
}
}
}
void RTArduLink::processReceivedMessage(RTARDULINK_PORT *portInfo)
{
RTARDULINK_MESSAGE *message; // a pointer to the message part of the frame
unsigned int address;
message = &(portInfo->RXFrameBuffer.message); // get the message pointer
address = RTArduLinkConvertUC2ToUInt(message->messageAddress);
switch (portInfo->index) {
case RTARDULINK_HOST_PORT:
processHostMessage(); // came from this upstream link
return;
case RTARDULINK_DAISY_PORT: // came from dasiy chain port
if (address != RTARDULINK_HOST_PORT) // true if it came from a daisy chained subsystem, not a directly connected subsystem
RTArduLinkConvertIntToUC2(address + RTARDULINKHAL_MAX_PORTS, message->messageAddress);
else
RTArduLinkConvertIntToUC2(RTARDULINK_DAISY_PORT, message->messageAddress);
break;
default:
RTArduLinkConvertIntToUC2(address + portInfo->index, message->messageAddress);
break;
}
// if get here, need to forward to host port
sendFrame(m_hostPort, &(portInfo->RXFrameBuffer), portInfo->RXFrameBuffer.messageLength);
}
void RTArduLink::processHostMessage()
{
RTARDULINK_MESSAGE *message; // a pointer to the message part of the frame
int identityLength;
int suffixLength;
unsigned int address;
message = &(m_hostPort->RXFrameBuffer.message); // get the message pointer
address = RTArduLinkConvertUC2ToUInt(message->messageAddress);
if (address == RTARDULINK_BROADCAST_ADDRESS) { // need to forward to downstream ports also
for (int i = RTARDULINK_HOST_PORT + 1; i < RTARDULINKHAL_MAX_PORTS; i++) {
if (m_ports[i].inUse)
sendFrame(m_ports + i, &(m_hostPort->RXFrameBuffer), m_hostPort->RXFrameBuffer.messageLength);
}
}
if ((address == RTARDULINK_MY_ADDRESS) || (address == RTARDULINK_BROADCAST_ADDRESS)) { // it's for me
switch (message->messageType)
{
case RTARDULINK_MESSAGE_POLL:
case RTARDULINK_MESSAGE_ECHO:
RTArduLinkConvertIntToUC2(RTARDULINK_MY_ADDRESS, message->messageAddress);
sendFrame(m_hostPort, &(m_hostPort->RXFrameBuffer), m_hostPort->RXFrameBuffer.messageLength); // just send the frame back as received
break;
case RTARDULINK_MESSAGE_IDENTITY:
identityLength = strlen(RTArduLinkHALConfig.identity);
suffixLength = strlen(m_identitySuffix);
memcpy(message->data, RTArduLinkHALConfig.identity, identityLength + 1); // copy in identity
if ((identityLength + suffixLength) < RTARDULINK_DATA_MAX_LEN - 1) {
memcpy(message->data + identityLength, m_identitySuffix, suffixLength + 1); // copy in suffix
} else {
suffixLength = 0;
}
RTArduLinkConvertIntToUC2(RTARDULINK_MY_ADDRESS, message->messageAddress);
message->data[RTARDULINK_DATA_MAX_LEN - 1] = 0; // make sure zero terminated if it was truncated
sendFrame(m_hostPort, &(m_hostPort->RXFrameBuffer), RTARDULINK_MESSAGE_HEADER_LEN + identityLength + suffixLength + 1);
break;
default:
if (message->messageType < RTARDULINK_MESSAGE_CUSTOM) { // illegal code
message->data[0] = RTARDULINK_RESPONSE_ILLEGAL_COMMAND;
message->data[1] = message->messageType; // this is the offending code
message->messageType = RTARDULINK_MESSAGE_ERROR;
RTArduLinkConvertIntToUC2(RTARDULINK_MY_ADDRESS, message->messageAddress);
sendFrame(m_hostPort, &(m_hostPort->RXFrameBuffer), RTARDULINK_MESSAGE_HEADER_LEN + 2);
break;
}
processCustomMessage(message->messageType, message->messageParam, message->data,
m_hostPort->RXFrameBuffer.messageLength - RTARDULINK_MESSAGE_HEADER_LEN); // see if anyone wants to process it
break;
}
return;
}
if (address >= RTARDULINKHAL_MAX_PORTS) { // need to pass it to the first subsystem
if (!m_ports[RTARDULINK_DAISY_PORT].inUse)
return; // there is no daisy chain port
RTArduLinkConvertIntToUC2(address - RTARDULINKHAL_MAX_PORTS, message->messageAddress); // adjust the address
sendFrame(m_ports +RTARDULINK_DAISY_PORT, &(m_hostPort->RXFrameBuffer), m_hostPort->RXFrameBuffer.messageLength);
return;
}
// if get here, needs to go to a local subsystem port
if (m_ports[address].inUse) {
RTArduLinkConvertIntToUC2(0, message->messageAddress); // indicates that the target should process it
sendFrame(m_ports + address, &(m_hostPort->RXFrameBuffer), m_hostPort->RXFrameBuffer.messageLength);
}
}
void RTArduLink::sendDebugMessage(const char *debugMessage)
{
RTARDULINK_FRAME frame;
int stringLength;
stringLength = strlen(debugMessage);
if (stringLength >= RTARDULINK_DATA_MAX_LEN)
stringLength = RTARDULINK_DATA_MAX_LEN-1;
memcpy(frame.message.data, debugMessage, stringLength);
frame.message.data[stringLength] = 0;
frame.message.messageType = RTARDULINK_MESSAGE_DEBUG;
RTArduLinkConvertIntToUC2(RTARDULINK_MY_ADDRESS, frame.message.messageAddress);
sendFrame(m_hostPort, &frame, RTARDULINK_MESSAGE_HEADER_LEN + stringLength + 1);
}
void RTArduLink::sendMessage(unsigned char messageType, unsigned char messageParam, unsigned char *data, int length)
{
RTARDULINK_FRAME frame;
RTArduLinkConvertIntToUC2(RTARDULINK_MY_ADDRESS, frame.message.messageAddress);
frame.message.messageType = messageType;
frame.message.messageParam = messageParam;
if (length > RTARDULINK_DATA_MAX_LEN)
length = RTARDULINK_DATA_MAX_LEN;
memcpy(frame.message.data, data, length);