#include <SPI.h>
#include <i2c_t3.h>
#include "A_ConfigDefines.h"
#include <SPI_MSTransfer.h>
//#include "TeensyThreads.h"
//Threads::Mutex MST_QUEUE;
#include "circular_buffer.h"
#define SPI_SPEED 30000000
#define OT_CALC 100*(30000000/SPI_SPEED)
SPI_MSTransfer teensy_gpio = SPI_MSTransfer("Serial", SPI_MST_CS, &SPI_MST_BUS, SPI_SPD ); // bad with default timeouts
SPI_MSTransfer W1 = SPI_MSTransfer("Wire1", SPI_MST_CS, &SPI_MST_BUS, SPI_SPD ); // bad with default timeouts
uint32_t OverTime = 0;
void setup() {
Serial.begin(115200);
while (!Serial && millis() < 2000 ) {}
Serial.println("\n" __FILE__ " " __DATE__ " " __TIME__);
teensy_gpio.onTransfer(myCallback);
teensy_gpio.onTransfer(myCallback8);
#ifdef SPI_MST_SCK
SPI_MST_BUS.setSCK( SPI_MST_SCK );
#endif
SPI_MST_BUS.begin();
//threads.addThread(SPI_MST_Thread);
Wire1.begin(I2C_MASTER, 0, 0, 0, 0, I2C_PULLUP_INT, 100000, I2C_OP_MODE_IMM);
Serial.println(SPI_MSTransfer::stmca.capacity());
Serial.println(SPI_MSTransfer::stmca.max_size());
// teensy_gpio.debug(Serial);
W1.onDetect([](AsyncMST info) {
W1.begin(I2C_SLAVE, 0x22, I2C_PINS_37_38, I2C_PULLUP_INT, 100000);
Wire1.beginTransmission(0x22);
Wire1.write("Hello Master!", 13);
Wire1.endTransmission();
W1.onReceive(hello);
}); // end of onDetect callback
//threads.setTimeSlice(0, 1);
//threads.setTimeSlice(1, 10);
delay(3000);
}
void loop() {
W1.events(1);
//teensy_gpio.events(0);
static uint32_t wire_write_timer = millis();
if ( millis() - wire_write_timer >= 1000 ) {
wire_write_timer = millis();
//Serial.println(millis());
char text1[] = "Yellow World -- SPI_MST";
//{ Threads::Scope scope(MST_QUEUE);
Wire1.beginTransmission(0x22);
Wire1.write(text1, sizeof(text1));
Wire1.sendTransmission();
static uint8_t t = 1;
Wire1.beginTransmission(0x22);
t++;
Wire1.write(t);
Wire1.sendTransmission();
//}
}
static uint32_t _timer = micros();
static uint32_t led_timer = millis();
if ( millis() - led_timer >= 100 ) {
led_timer = millis();
//{ Threads::Scope scope(MST_QUEUE);
teensy_gpio.pinToggle(LED_BUILTIN);
//}
delay(1);
}
if ( micros() - _timer >= 1000 ) {
_timer = micros();
uint8_t bufff[24];
for (uint8_t i = 0; i < sizeof(bufff); i++) bufff[i] = i;
uint32_t _time = micros();
uint16_t *buf;
double MST_PrintVals[12];
buf = (uint16_t *)MST_PrintVals;
int ii = 0;
static uint16_t __count = 0;
static uint16_t __countB = 0;
for ( uint32_t i = 0; i < sizeof(MST_PrintVals) / sizeof( MST_PrintVals[0] ); i++ ) MST_PrintVals[i] = __count++;
_time = micros();
__countB++;
if ( __countB % 25 ) {
uint32_t t1 = micros();
//{ Threads::Scope scope(MST_QUEUE);
teensy_gpio.transfer16((uint16_t *)MST_PrintVals, sizeof(MST_PrintVals) / 2, 60, 2); // DEBUGHACK output
//}
}
else {
uint32_t t2 = micros();
//{ Threads::Scope scope(MST_QUEUE);
teensy_gpio.transfer16((uint16_t *)MST_PrintVals, sizeof(MST_PrintVals) / 2, 55, 2);
//}
}
return;
if ( _time > OT_CALC ) OverTime++;
}
}
void hello(size_t count, AsyncMST info) {
Serial.print("Size: "); Serial.print(count);
Serial.print(" Slave: "); Serial.print(info.slave);
Serial.print(" Port: "); Serial.println(info.port);
if ( count > 10 ) {
uint8_t buf[count] = { 0 };
W1.read(buf, count);
for ( uint16_t i = 0; i < count; i++ ) {
Serial.print((char) buf[i]);
} Serial.println();
}
else {
Serial.print("Count: "); Serial.println(W1.read());
}
Serial.println();
}
//void SPI_MST_Thread() {
//while (1) {
//{ Threads::Scope scope(MST_QUEUE);
// teensy_gpio.events(0);
//}
//threads.yield();
//}
//}
void myCallback8(uint8_t *buffer, uint16_t length, AsyncMST info) {
Serial.print("PacketID: "); Serial.println(info.packetID);
Serial.print("Length: "); Serial.println(length);
for ( uint16_t i = 0; i < length; i++ ) {
Serial.print(buffer[i], HEX); Serial.print(" ");
}
Serial.println();
}
void myCallback(uint16_t *buffer, uint16_t length, AsyncMST info) {
//return;
Serial.print("PacketID: "); Serial.println(info.packetID);
Serial.print("Length: "); Serial.println(length);
for ( uint16_t i = 0; i < length; i++ ) {
Serial.print(buffer[i], HEX); Serial.print(" ");
}
Serial.println();
}
#include <SPI_MSTransfer.h>
#include <i2c_t3.h>
SPI_MSTransfer slave = SPI_MSTransfer("SLAVE", "STANDALONE");
void myCallback8(uint8_t *buffer, uint16_t length, AsyncMST info) {
Serial.print("PacketID: "); Serial.println(info.packetID);
Serial.print("Length: "); Serial.println(length);
for ( uint16_t i = 0; i < length; i++ ) {
Serial.print(buffer[i], HEX); Serial.print(" ");
}
Serial.println();
}
void setup() {
Serial.begin(115200);
//FastLED.addLeds<WS2812B, DATA_PIN>(leds, NUM_LEDS);
while (!Serial && millis() < 5000 )
{ Serial.print( "Teensy NOT Online @ millis=" );
Serial.println( millis() );
delay(30);
}
Serial.print( "Teensy Online @ millis=" );
Serial.println( millis() );
Serial.println("\n" __FILE__ " " __DATE__ " " __TIME__);
if ( ARM_DWT_CYCCNT == ARM_DWT_CYCCNT ) {
Serial.print( "Cycle Counter Not Enabled :" );
Serial.println( ARM_DWT_CYCCNT );
}
if ( ARM_DWT_CYCCNT == ARM_DWT_CYCCNT ) {
// Enable CPU Cycle Count
ARM_DEMCR |= ARM_DEMCR_TRCENA;
ARM_DWT_CTRL |= ARM_DWT_CTRL_CYCCNTENA;
}
if ( ARM_DWT_CYCCNT != ARM_DWT_CYCCNT ) {
Serial.print( "Cycle Counter Enabled! :" );
Serial.println( ARM_DWT_CYCCNT );
}
slave.begin( );
slave.onTransfer(myCallback);
slave.onTransfer(myCallback8);
}
void led() {
digitalWrite(13, !digitalRead(13));
}
void loop() {
slave.events();
static uint32_t retry = millis();
if ( millis() - retry > 200 ) {
retry = millis();
uint16_t buf[96];
for (uint16_t i = 0; i < sizeof(buf) / 2; i++) buf[i] = i;
slave.transfer16(buf, sizeof(buf) / 2, 69);
}
}
static uint16_t last_packetID = 0;
elapsedMillis TogClk;
uint32_t TogCnt = 0;
uint32_t FaFCnt = 0;
void myCallback(uint16_t *buffer, uint16_t length, AsyncMST info) {
// Serial.print("PacketID: "); Serial.println(info.packetID);
// Serial.print("Length: "); Serial.println(length);
for ( uint16_t i = 0; i < length; i++ ) {
Serial.print(buffer[i], HEX); Serial.print(" ");
}
Serial.println();
return;
if ( 55 == info.packetID ) {
if ( 48 != length ) {
Serial.print("Bad Length: "); Serial.println(length); // If this shows then the SPI Passed array size is nor wrong
}
else if ( 0 != info.error ) {
Serial.println("\nBad CRC: ");
}
else {
double* MST_PrintVals;
MST_PrintVals = (double *)buffer;
{
//trig conversions
float rad2deg = 180.0f / PI;
float deg2rad = PI / 180.0f;
int textLength = 12 * 31;
char text[textLength];
char utcText[30];
char tsIMUText[30];
char tsGPSText[30];
// KF parameters
char latText[30];
char lonText[30];
char altText[30];
char pk1xText[30];
char pk1yText[30];
char pk1zText[30];
char nuk1xText[30];
char nuk1yText[30];
char nuk1zText[30];
int ii = 0;
dtostrf( MST_PrintVals[ii] , 10, 6, utcText);
ii++;
dtostrf( MST_PrintVals[ii] * 0.000001f, 10, 4, tsIMUText);
ii++;
dtostrf( MST_PrintVals[ii] * 0.000001f, 10, 4, tsGPSText);
ii++;
dtostrf( MST_PrintVals[ii] *rad2deg, 10, 6, latText);
ii++;
dtostrf( MST_PrintVals[ii] *rad2deg, 10, 6, lonText);
ii++;
dtostrf( MST_PrintVals[ii] , 10, 4, altText);
ii++;
dtostrf(sqrt( MST_PrintVals[ii] ), 10, 4, pk1xText);
ii++;
dtostrf(sqrt( MST_PrintVals[ii] ), 10, 4, pk1yText);
ii++;
dtostrf(sqrt( MST_PrintVals[ii] ), 10, 4, pk1zText);
ii++;
dtostrf( MST_PrintVals[ii] , 10, 4, nuk1xText);
ii++;
dtostrf( MST_PrintVals[ii] , 10, 4, nuk1yText);
ii++;
dtostrf( MST_PrintVals[ii] , 10, 4, nuk1zText);
// Create single text parameter and print it
snprintf(text, textLength, "%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s",
utcText, tsIMUText, tsGPSText,
latText, lonText, altText,
pk1xText, pk1yText, pk1zText,
nuk1xText, nuk1yText, nuk1zText);
Serial.println(text);
//Serial.send_now();
}
}
}
else if ( 60 == info.packetID ) {
if ( 48 != length ) {
Serial.print("Bad Length: "); Serial.println(length); // If this shows then the SPI Passed array size is nor wrong
}
else if ( 0 != info.error ) {
Serial.println("\nBad CRC: ");
}
else {
double* MST_PrintVals;
MST_PrintVals = (double *)buffer;
// This provides a terse output with error checked based on MASTER output to the 12 DOUBLE value Array
{
static uint16_t TogLast = 0;
static uint16_t TogHz = 0;
static uint16_t FaFHz = 0;
static double LastVal = 100000;
static uint32_t ChkErr = 0;
static uint32_t CBcount = 0;
if ( digitalReadFast( LED_BUILTIN) != TogLast ) {
TogLast = !TogLast;
TogCnt++;
}
if ( TogClk >= 1000 ) {
TogClk -= 1000;
TogHz = TogCnt;
TogCnt = 0;
FaFHz = FaFCnt;
FaFCnt = 0;
}
if ( last_packetID != info.packetID ) {
LastVal += 12;
if ( LastVal > 65536 && LastVal < 100000 ) LastVal -= 65536;
}
elapsedMillis LastCB;
double DiffMiss = 0;
uint32_t ErrSeen = 0;
CBcount++;
FaFCnt++;
for ( uint32_t ii = 0; ii < 12; ii++ ) {
LastVal++;
if ( 65536 == LastVal ) LastVal = 0;
if ( LastCB > 10 || 100001 == LastVal || 100013 == LastVal ) LastVal = MST_PrintVals[ii];
if ( LastVal != MST_PrintVals[ii] ) {
DiffMiss = LastVal - MST_PrintVals[ii];
ChkErr++;
Serial.print("\nBad LASTVAL TEST INCREMENT <<<<<<<<<<<<<<<<<<<< DIFF OF> ");
Serial.println( DiffMiss );
LastVal = MST_PrintVals[ii];
ErrSeen++;
}
if ( 0 != ErrSeen || 1 > ii ) {
Serial.print( MST_PrintVals[ii] );
Serial.print(",");
}
else {
Serial.print(" #,");
}
}
Serial.print( CBcount );
Serial.print(",");
Serial.print( ChkErr );
Serial.print(" [");
Serial.print( FaFHz );
Serial.print(" ,");
Serial.print( TogHz );
Serial.println();
LastCB = 0;
}
}
}
else {
Serial.print("PacketID: ");
Serial.println(info.packetID);
}
last_packetID = info.packetID;
}
if ( _wire_callback_active ) {
[COLOR="#FF0000"]_wire_callback_readpos = mtsca.peek_front()[4]; [/COLOR]
for ( uint16_t i = 0; i < size; i++ ) buffer[i] = (uint8_t)mtsca.peek_front()[i+5];
return 0;
}
if ( wire_port != -1 ) {
if ( _wire_callback_active ) {
uint8_t _shift = mtsca.peek_front()[4] - available();
for ( uint16_t i = 0; i < size; i++ ) {
if ( _wire_callback_readpos >= mtsca.peek_front()[4] ) break;
buffer[i] = (uint8_t)mtsca.peek_front()[i+5+_shift];
_wire_callback_readpos++;
}
if ( size >= mtsca.peek_front()[4] ) return mtsca.peek_front()[4]-_shift;
return size;
}
_wire_callback_readpos
[ATTACH]13608._xfImport[/ATTACH]
SPI_MSTransfer W1 = SPI_MSTransfer("Wire", SPI_MST_CS, &SPI_MST_BUS, SPI_SPD ); // bad with default timeouts
and
W1.begin(I2C_SLAVE, 0x22, I2C_PINS_18_19, I2C_PULLUP_INT, 100000);
OT_CALC==100 micros() _time==9
ACK PacketID 60: 5uS
OT_CALC==100 micros() _time==10
ACK PacketID 60: 5uS
OT_CALC==100 micros() _time==10
ACK PacketID 60: 5uS
OT_CALC==100 micros() _time==10
ACK PacketID 60: 6uS
OT_CALC==100 micros() _time==9
0 0 300 40D6 0 0 340 40D6 0 0 380 40D6 0 0 3C0 40D6 0 0 400 40D6 0 0 440 40D6 0 0 480 40D6 0 0 4C0 40D6 0 0 500 40D6 0 0 540 40D6 0 0 580 40D6 0 0 5C0 40D6
0 0 D600 40D5 0 0 D640 40D5 0 0 D680 40D5 0 0 D6C0 40D5 0 0 D700 40D5 0 0 D740 40D5 0 0 D780 40D5 0 0 D7C0 40D5 0 0 D800 40D5 0 0 D840 40D5 0 0 D880 40D5 0 0 D8C0 40D5
0 0 D900 40D5 0 0 D940 40D5 0 0 D980 40D5 0 0 D9C0 40D5 0 0 DA00 40D5 0 0 DA40 40D5 0 0 DA80 40D5 0 0 DAC0 40D5 0 0 DB00 40D5 0 0 DB40 40D5 0 0 DB80 40D5 0 0 DBC0 40D5
0 0 DC00 40D5 0 0 DC40 40D5 0 0 DC80 40D5 0 0 DCC0 40D5 0 0 DD00 40D5 0 0 DD40 40D5 0 0 DD80 40D5 0 0 DDC0 40D5 0 0 DE00 40D5 0 0 DE40 40D5 0 0 DE80 40D5 0 0 DEC0 40D5
0 0 DF00 40D5 0 0 DF40 40D5 0 0 DF80 40D5 0 0 DFC0 40D5 0 0 E000 40D5 0 0 E040 40D5 0 0 E080 40D5 0 0 E0C0 40D5 0 0 E100 40D5 0 0 E140 40D5 0 0 E180 40D5 0 0 E1C0 40D5
0 0 E200 40D5 0 0 E240 40D5 0 0 E280 40D5 0 0 E2C0 40D5 0 0 E300 40D5 0 0 E340 40D5 0 0 E380 40D5 0 0 E3C0 40D5 0 0 E400 40D5 0 0 E440 40D5 0 0 E480 40D5 0 0 E4C0 40D5
0 0 3300 40D6 0 0 3340 40D6 0 0 3380 40D6 0 0 33C0 40D6 0 0 3400 40D6 0 0 3440 40D6 0 0 3480 40D6 0 0 34C0 40D6 0 0 3500 40D6 0 0 3540 40D6 0 0 3580 40D6 0 0 35C0 40D6
0 0 3600 40D6 0 0 3640 40D6 0 0 3680 40D6 0 0 36C0 40D6 0 0 3700 40D6 0 0 3740 40D6 0 0 3780 40D6 0 0 37C0 40D6 0 0 3800 40D6 0 0 3840 40D6 0 0 3880 40D6 0 0 38C0 40D6
0 0 3900 40D6 0 0 3940 40D6 0 0 3980 40D6 0 0 39C0 40D6 0 0 3A00 40D6 0 0 3A40 40D6 0 0 3A80 40D6 0 0 3AC0 40D6 0 0 3B00 40D6 0 0 3B40 40D6 0 0 3B80 40D6 0 0 3BC0 40D6
0 0 3C00 40D6 0 0 3C40 40D6 0 0 3C80 40D6 0 0 3CC0 40D6 0 0 3D00 40D6 0 0 3D40 40D6 0 0 3D80 40D6 0 0 3DC0 40D6 0 0 3E00 40D6 0 0 3E40 40D6 0 0 3E80 40D6 0 0 3EC0 40D6
0 0 3F00 40D6 0 0 3F40 40D6 0 0 3F80 40D6 0 0 3FC0 40D6 0 0 4000 40D6 0 0 4040 40D6 0 0 4080 40D6 0 0 40C0 40D6 0 0 4100 40D6 0 0 4140 40D6 0 0 4180 40D6 0 0 41C0 40D6
0 0 4200 40D6 0 0 4240 40D6 0 0 4280 40D6 0 0 42C0 40D6 0 0 4300 40D6 0 0 4340 40D6 0 0 4380 40D6 0 0 43C0 40D6 0 0 4400 40D6 0 0 4440 40D6 0 0 4480 40D6 0 0 44C0 40D6
0 0 1500 40D6 0 0 1540 40D6 0 0 1580 40D6 0 0 15C0 40D6 0 0 1600 40D6 0 0 1640 40D6 0 0 1680 40D6 0 0 16C0 40D6 0 0 1700 40D6 0 0 1740 40D6 0 0 1780 40D6 0 0 17C0 40D6
0 0 1800 40D6 0 0 1840 40D6 0 0 1880 40D6 0 0 18C0 40D6 0 0 1900 40D6 0 0 1940 40D6 0 0 1980 40D6 0 0 19C0 40D6 0 0 1A00 40D6 0 0 1A40 40D6 0 0 1A80 40D6 0 0 1AC0 40D6
0 0 1B00 40D6 0 0 1B40 40D6 0 0 1B80 40D6 0 0 1BC0 40D6
events() ? post your code?I just tested the new library with the t3.5 (Master) and T3.2 (slave) combination. I didn't test it with the T3.5/3.5 combo yet before you ask. With the 3.5/3.2 combo I running into an interesting situation but before that the master is at 168 and the slave is at 120.
The master does the transfer16's to slave no problem but they come in bunches of 2 (probably the queue empting ). Then there is a noticeable pause on the slave. The master, on the otherhand, shows no i2c info coming back or transfer16's from the slave. I Checked the wiring a couple times and hooked up an sensor just to make sure. Tested the i2c transfers with and without the slave doing transfers to the master and the same thing. Know I am missing something.
On the only thing I changed in the posted sketch is as follows:
Code:SPI_MSTransfer W1 = SPI_MSTransfer("Wire", SPI_MST_CS, &SPI_MST_BUS, SPI_SPD ); // bad with default timeouts and W1.begin(I2C_SLAVE, 0x22, I2C_PINS_18_19, I2C_PULLUP_INT, 100000);
and of course I changed all the wire1's to wire calls.
Don't think this has anything to do with the newest version of the library as I saw the same behavior in the previous version of the I2c version.
Issue with speeds again, but then why would the slave receive and not the master?
//#include "FastLED.h"
#include <SPI.h>
#include <i2c_t3.h>
#include "A_ConfigDefines.h"
#include <SPI_MSTransfer.h>
#include "TeensyThreads.h"
#include <EventResponder.h>
EventResponder eresp;
//0x99BB, 0x0003, 0x99B8
Threads::Mutex MST_QUEUE;
#include "circular_buffer.h"
//#define SPI_SPEED 30500
#define SPI_SPEED 30000000
#define OT_CALC 100*(30000000/SPI_SPEED)
SPI_MSTransfer teensy_gpio = SPI_MSTransfer("Serial", SPI_MST_CS, &SPI_MST_BUS, SPI_SPEED ); // bad with default timeouts
SPI_MSTransfer W0 = SPI_MSTransfer("Wire", SPI_MST_CS, &SPI_MST_BUS, SPI_SPEED ); // bad with default timeouts
SPI_MSTransfer W1 = SPI_MSTransfer("Wire1", SPI_MST_CS, &SPI_MST_BUS, SPI_SPEED ); // bad with default timeouts
SPI_MSTransfer sSPI1 = SPI_MSTransfer("SPI1", SPI_MST_CS, &SPI_MST_BUS, SPI_SPEED ); // bad with default timeouts
void I2Cscan(SPI_MSTransfer &port) {
SPI_MSTransfer* thePort;
thePort = &port;
// scan for i2c devices
byte error, address;
int nDevices;
Serial.println("Scanning...");
nDevices = 0;
for (address = 1; address < 127; address++ ) {
// The i2c_scanner uses the return value of
// the Write.endTransmisstion to see if
// a device did acknowledge to the address.
thePort->beginTransmission(address);
error = thePort->endTransmission();
if (error == 0) {
Serial.print("I2C device found at address 0x");
if (address < 16) Serial.print("0");
Serial.print(address, HEX);
Serial.println(" !");
nDevices++;
}
else if (error == 4) {
Serial.print("Unknow error at address 0x");
if (address < 16) Serial.print("0");
Serial.println(address, HEX);
}
}
if (nDevices == 0) {
Serial.println("No I2C devices found\n");
while (1) {};
} else {
Serial.println("done\n");
}
}
void myCallback8(uint8_t *buffer, uint16_t length, AsyncMST info) {
Serial.print("PacketID: "); Serial.println(info.packetID);
Serial.print("Length: "); Serial.println(length);
for ( uint16_t i = 0; i < length; i++ ) {
Serial.print(buffer[i], HEX); Serial.print(" ");
}
Serial.println();
}
void myCallback(uint16_t *buffer, uint16_t length, AsyncMST info) {
//return;
Serial.print("PacketID: "); Serial.println(info.packetID);
Serial.print("Length: "); Serial.println(length);
for ( uint16_t i = 0; i < length; i++ ) {
Serial.print(buffer[i], HEX); Serial.print(" ");
}
Serial.println();
}
uint32_t OverTime = 0;
void setup() {
// SPI_MSTransfer::mtsca._init();
Serial.begin(115200);
while (!Serial && millis() < 2000 ) {}
Serial.println("\n" __FILE__ " " __DATE__ " " __TIME__);
teensy_gpio.onTransfer(myCallback);
teensy_gpio.onTransfer(myCallback8);
threads.addThread(SPI_MST_Thread);
Circular_Buffer<double, 16> myFloats;
myFloats.push_back(3.14159);
myFloats.push_back(12.3456);
myFloats.push_back(78.91234);
myFloats.push_back(7.91234);
myFloats.push_back(11.91234);
myFloats.push_back(58.91234);
myFloats.push_back(18.91234);
// myFloats.push_back(2.91234);
Serial.print("SUM: "); Serial.println(myFloats.sum(), 5);
Serial.print("MIN: "); Serial.println(myFloats.min(), 5);
Serial.print("MAX: "); Serial.println(myFloats.max(), 5);
Serial.print("MEDIAN: "); Serial.println(myFloats.median(1), 5);
uint8_t blah[5];
// qsort(blah, 5, sizeof(uint8_t), *(uint8_t*)a - *(uint8_t*)b);
Serial.print("AVG: "); Serial.println(myFloats.average(), 5);
uint8_t _size = myFloats.size();
Serial.print("Deviation: "); Serial.println(myFloats.deviation(), 8);
for ( uint8_t i = 0; i < _size; i++ ) {
Serial.print(myFloats.pop_front(), 5); Serial.print(" ");
} Serial.println();
// while(1);
//uint16_t v = 1010;
//
//v--;
//v |= v >> 1;
//v |= v >> 2;
//v |= v >> 4;
//v |= v >> 8;
//v |= v >> 16;
//v++;
//
//Serial.println(v);
//uint16_t v = 300;
// Serial.println(v);
// Circular_Buffer<uint16_t, 4> cb2;
// I2Cscan(W0);
Serial.println(SPI_MSTransfer::stmca.capacity());
Serial.println(SPI_MSTransfer::stmca.max_size());
// teensy_gpio.debug(Serial);
threads.setTimeSlice(0, 5);
threads.setTimeSlice(1, 0);
// W1.begin(I2C_MASTER, 0x00, I2C_PINS_37_38, I2C_PULLUP_EXT, 400000);
// W0.begin(I2C_SLAVE, 0x22, I2C_PINS_18_19, I2C_PULLUP_EXT, 100000);
// W0.onReceive(hello);
//
// teensy_gpio.onDetect([](AsyncMST info) {
// { Threads::Scope scope(MST_QUEUE);
// W1.begin(I2C_MASTER, 0x00, I2C_PINS_37_38, I2C_PULLUP_EXT, 400000);
// W0.begin(I2C_SLAVE, 0x22, I2C_PINS_18_19, I2C_PULLUP_EXT, 100000);
// // W0.onReceive(hello);
// }
// }); // end of onDetect callback
Wire.begin();
// delay(2000);
// W0.begin(I2C_SLAVE, 0x22, I2C_PINS_18_19, I2C_PULLUP_EXT, 100000);
}
void hello(size_t count, AsyncMST info) {
Serial.print("Size: "); Serial.print(count);
Serial.print(" Slave: "); Serial.print(info.slave);
Serial.print(" Port: "); Serial.println(info.port);
for ( uint16_t i = 0; i < count; i++ ) {
Serial.print((char)W0.read());
} Serial.println();
}
void SPI_MST_Thread() {
while (1) {
{ Threads::Scope scope(MST_QUEUE);
teensy_gpio.events(0);
}
threads.yield();
}
}
void loop() {
static uint32_t wire_write_timer = millis();
static uint32_t _timer = micros();
static uint32_t led_timer = millis();
if ( millis() - led_timer >= 100 ) {
led_timer = millis();
{ Threads::Scope scope(MST_QUEUE);
teensy_gpio.pinToggle(LED_BUILTIN);
}
// Serial.println("LED TOGGLE!");
// delay(1);
}
// return;
if ( micros() - _timer >= 1000 ) {
_timer = micros();
uint8_t bufff[24];
for (uint8_t i = 0; i < sizeof(bufff); i++) bufff[i] = i;
// teensy_gpio.transfer(bufff, sizeof(bufff), 69, 2);
//uint16_t notify = 0xAD00;
//Serial.println(notify |= 1 << 0,HEX);
// set: notify |= 1 << 7;
// unset: notify &= ~(1 << 7);
// Serial.println(millis());
// W0.beginTransmission(0x20);
// uint8_t ibuffer[3] = { 0x0C, 0xFF, 0xFF };
// W0.write(ibuffer, 3); // set pullups
// W0.endTransmission();
//
// W0.beginTransmission(0x20);
// W0.write(0x12); // set bankA to read
// W0.endTransmission();
//
// W0.requestFrom(0x20, 2); // read bankA and bankB
// Serial.print(W0.read()); Serial.print(":"); Serial.println(W0.read());
// Serial.println( W0.requestFrom(0x48, 1, 1));
// uint8_t buffer[48];
//
// for ( uint8_t i = 0; i < sizeof(buffer); i++ ) {
// buffer[i] = i;
// }
// uint32_t ___time = micros();
// teensy_gpio.transfer(buffer, sizeof(buffer), 10750,1);
// Serial.print("T: "); Serial.println(micros() - ___time);
uint32_t _time = micros();
// SPIClass teensy_gpio;
//SPI3 = (SPIClass)teensy_gpio;
uint16_t *buf;
double MST_PrintVals[12];
buf = (uint16_t *)MST_PrintVals;
int ii = 0;
static uint16_t __count = 0;
static uint16_t __countB = 0;
for ( uint32_t i = 0; i < sizeof(MST_PrintVals) / sizeof( MST_PrintVals[0] ); i++ ) MST_PrintVals[i] = __count++;
// Serial.print("F&F (OT=");
// Serial.print( OverTime );
// Serial.print(")");
_time = micros();
__countB++;
if ( __countB % 25 ) {
Serial.println();
Serial.print("ACK PacketID 60: ");
uint32_t t1 = micros();
{ Threads::Scope scope(MST_QUEUE);
teensy_gpio.transfer16((uint16_t *)MST_PrintVals, sizeof(MST_PrintVals) / 2, 60, 2); // DEBUGHACK output
}
Serial.print(micros() - t1);
Serial.println("uS");
}
else {
Serial.println();
Serial.print("F&F PacketID 55: ");
uint32_t t2 = micros();
{ Threads::Scope scope(MST_QUEUE);
teensy_gpio.transfer16((uint16_t *)MST_PrintVals, sizeof(MST_PrintVals) / 2, 55, 2);
}
Serial.print(micros() - t2);
Serial.println("uS");
}
// return;
_time = micros() - _time;
Serial.print(" OT_CALC==");
Serial.print(OT_CALC);
Serial.print(" micros() _time==");
Serial.println(_time);
if ( _time > OT_CALC ) OverTime++;
// teensy_gpio.events();
}
}
#include <SPI_MSTransfer.h>
#include <i2c_t3.h>
SPI_MSTransfer slave = SPI_MSTransfer("SLAVE", "STANDALONE");
void myCallback8(uint8_t *buffer, uint16_t length, AsyncMST info) {
Serial.print("PacketID: "); Serial.println(info.packetID);
Serial.print("Length: "); Serial.println(length);
for ( uint16_t i = 0; i < length; i++ ) {
Serial.print(buffer[i], HEX); Serial.print(" ");
}
Serial.println();
}
void setup() {
Serial.begin(115200);
while (!Serial && millis() < 5000 )
{ Serial.print( "Teensy NOT Online @ millis=" );
Serial.println( millis() );
delay(30);
}
Serial.print( "Teensy Online @ millis=" );
Serial.println( millis() );
Serial.println("\n" __FILE__ " " __DATE__ " " __TIME__);
if ( ARM_DWT_CYCCNT == ARM_DWT_CYCCNT ) {
Serial.print( "Cycle Counter Not Enabled :" );
Serial.println( ARM_DWT_CYCCNT );
}
if ( ARM_DWT_CYCCNT == ARM_DWT_CYCCNT ) {
// Enable CPU Cycle Count
ARM_DEMCR |= ARM_DEMCR_TRCENA;
ARM_DWT_CTRL |= ARM_DWT_CTRL_CYCCNTENA;
}
if ( ARM_DWT_CYCCNT != ARM_DWT_CYCCNT ) {
Serial.print( "Cycle Counter Enabled! :" );
Serial.println( ARM_DWT_CYCCNT );
}
SPI1.begin();
SPI2.begin();
SPI1.setSCK(20); SPI1.setMOSI(21); SPI1.setMISO(5); SPI1.begin();
SPI2.setSCK(46); SPI2.setMOSI(44); SPI2.setMISO(45); SPI2.begin();
SPI1.beginTransaction(SPISettings(4000000, MSBFIRST, SPI_MODE0)); // default speed
SPI2.beginTransaction(SPISettings(4000000, MSBFIRST, SPI_MODE0)); // default speed
slave.begin( );
slave.onTransfer(myCallback);
slave.onTransfer(myCallback8);
// slave.debug(Serial); // SPI_MST Debug error tracking
// Wire.begin(I2C_SLAVE, 0x22, I2C_PINS_18_19, I2C_PULLUP_EXT, 100000);
}
void led() {
digitalWrite(13, !digitalRead(13));
}
void loop() {
slave.events();
// return;
static uint32_t retry = millis();
if ( millis() - retry > 100 ) {
retry = millis();
uint16_t buf[96];
for (uint16_t i = 0; i < sizeof(buf) / 2; i++) buf[i] = i;
slave.transfer16(buf, sizeof(buf) / 2, 69);
}
}
static uint16_t last_packetID = 0;
elapsedMillis TogClk;
uint32_t TogCnt = 0;
uint32_t FaFCnt = 0;
void myCallback(uint16_t *buffer, uint16_t length, AsyncMST info) {
// Serial.print("PacketID: "); Serial.println(info.packetID);
// Serial.print("Length: "); Serial.println(length);
for ( uint16_t i = 0; i < length; i++ ) {
Serial.print(buffer[i], HEX); Serial.print(" ");
}
Serial.println();
return;
if ( 55 == info.packetID ) {
if ( 48 != length ) {
Serial.print("Bad Length: "); Serial.println(length); // If this shows then the SPI Passed array size is nor wrong
}
else if ( 0 != info.error ) {
Serial.println("\nBad CRC: ");
}
else {
double* MST_PrintVals;
MST_PrintVals = (double *)buffer;
{
//trig conversions
float rad2deg = 180.0f / PI;
float deg2rad = PI / 180.0f;
int textLength = 12 * 31;
char text[textLength];
char utcText[30];
char tsIMUText[30];
char tsGPSText[30];
// KF parameters
char latText[30];
char lonText[30];
char altText[30];
char pk1xText[30];
char pk1yText[30];
char pk1zText[30];
char nuk1xText[30];
char nuk1yText[30];
char nuk1zText[30];
int ii = 0;
dtostrf( MST_PrintVals[ii] , 10, 6, utcText);
ii++;
dtostrf( MST_PrintVals[ii] * 0.000001f, 10, 4, tsIMUText);
ii++;
dtostrf( MST_PrintVals[ii] * 0.000001f, 10, 4, tsGPSText);
ii++;
dtostrf( MST_PrintVals[ii] *rad2deg, 10, 6, latText);
ii++;
dtostrf( MST_PrintVals[ii] *rad2deg, 10, 6, lonText);
ii++;
dtostrf( MST_PrintVals[ii] , 10, 4, altText);
ii++;
dtostrf(sqrt( MST_PrintVals[ii] ), 10, 4, pk1xText);
ii++;
dtostrf(sqrt( MST_PrintVals[ii] ), 10, 4, pk1yText);
ii++;
dtostrf(sqrt( MST_PrintVals[ii] ), 10, 4, pk1zText);
ii++;
dtostrf( MST_PrintVals[ii] , 10, 4, nuk1xText);
ii++;
dtostrf( MST_PrintVals[ii] , 10, 4, nuk1yText);
ii++;
dtostrf( MST_PrintVals[ii] , 10, 4, nuk1zText);
// Create single text parameter and print it
snprintf(text, textLength, "%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s",
utcText, tsIMUText, tsGPSText,
latText, lonText, altText,
pk1xText, pk1yText, pk1zText,
nuk1xText, nuk1yText, nuk1zText);
Serial.println(text);
//Serial.send_now();
}
}
}
else if ( 60 == info.packetID ) {
if ( 48 != length ) {
Serial.print("Bad Length: "); Serial.println(length); // If this shows then the SPI Passed array size is nor wrong
}
else if ( 0 != info.error ) {
Serial.println("\nBad CRC: ");
}
else {
double* MST_PrintVals;
MST_PrintVals = (double *)buffer;
// This provides a terse output with error checked based on MASTER output to the 12 DOUBLE value Array
{
static uint16_t TogLast = 0;
static uint16_t TogHz = 0;
static uint16_t FaFHz = 0;
static double LastVal = 100000;
static uint32_t ChkErr = 0;
static uint32_t CBcount = 0;
if ( digitalReadFast( LED_BUILTIN) != TogLast ) {
TogLast = !TogLast;
TogCnt++;
}
if ( TogClk >= 1000 ) {
TogClk -= 1000;
TogHz = TogCnt;
TogCnt = 0;
FaFHz = FaFCnt;
FaFCnt = 0;
}
if ( last_packetID != info.packetID ) {
LastVal += 12;
if ( LastVal > 65536 && LastVal < 100000 ) LastVal -= 65536;
}
elapsedMillis LastCB;
double DiffMiss = 0;
uint32_t ErrSeen = 0;
CBcount++;
FaFCnt++;
for ( uint32_t ii = 0; ii < 12; ii++ ) {
LastVal++;
if ( 65536 == LastVal ) LastVal = 0;
if ( LastCB > 10 || 100001 == LastVal || 100013 == LastVal ) LastVal = MST_PrintVals[ii];
if ( LastVal != MST_PrintVals[ii] ) {
DiffMiss = LastVal - MST_PrintVals[ii];
ChkErr++;
Serial.print("\nBad LASTVAL TEST INCREMENT <<<<<<<<<<<<<<<<<<<< DIFF OF> ");
Serial.println( DiffMiss );
LastVal = MST_PrintVals[ii];
ErrSeen++;
}
if ( 0 != ErrSeen || 1 > ii ) {
Serial.print( MST_PrintVals[ii] );
Serial.print(",");
}
else {
Serial.print(" #,");
}
}
Serial.print( CBcount );
Serial.print(",");
Serial.print( ChkErr );
Serial.print(" [");
Serial.print( FaFHz );
Serial.print(" ,");
Serial.print( TogHz );
Serial.println();
LastCB = 0;
}
}
}
else {
Serial.print("PacketID: ");
Serial.println(info.packetID);
}
last_packetID = info.packetID;
}
0.00, #, #, #, #, #, #, #, #, #, #, #,1,0
12.00, #, #, #, #, #, #, #, #, #, #, #,2,0
24.00, #, #, #, #, #, #, #, #, #, #, #,3,0
36.00, #, #, #, #, #, #, #, #, #, #, #,4,0
48.00, #, #, #, #, #, #, #, #, #, #, #,5,0
60.00, #, #, #, #, #, #, #, #, #, #, #,6,0
72.00, #, #, #, #, #, #, #, #, #, #, #,7,0
84.00, #, #, #, #, #, #, #, #, #, #, #,8,0
96.00, #, #, #, #, #, #, #, #, #, #, #,9,0
108.00, #, #, #, #, #, #, #, #, #, #, #,10,0
120.00, #, #, #, #, #, #, #, #, #, #, #,11,0
132.00, #, #, #, #, #, #, #, #, #, #, #,12,0
144.00, #, #, #, #, #, #, #, #, #, #, #,13,0
156.00, #, #, #, #, #, #, #, #, #, #, #,14,0 #, #, #,23,0
Bad LASTVAL TEST INCREMENT <<<<<<<<<<<<<<<<<<<< DIFF OF> -12.00
288.00,289.00,290.00,291.00,292.00,293.00,294.00,295.00,296.00,297.00,298.00,299.00,24,1
300.00, #, #, #, #, #, #, #, #, #, #, #,25,1
Bad LASTVAL TEST INCREMENT <<<<<<<<<<<<<<<<<<<< DIFF OF> -300.00
612.00,613.00,614.00,615.00,616.00,617.00,618.00,619.00,620.00,621.00,622.00,623.00,26,2
624.00, #, #, #, #, #, #, #, #, #, #, #,27,2