/* PINS being used:
* 3/4S Battery -> VI+ in BEC (Battery Eliminator Circuit) board
* GND Battery -> VI- in BEC board
* Teensy 5V -> 5v out BEC board
* Teensy GND -> GND BEC board
* P2(out)--> Chip select
* \--> P10 (Chip Select SPI)
* \--> P0 (Chip Select SPI1)
* P3(out) Scope trigger signal, and GND for scope
* P4(out) Clock for SPI telemetry
* \--> P13 SPI clock
* \--> P27 SPI1 clock
****** RC Receiver ******
* P7 (RX2 in) <- A5 on 3v<->5v conversion board
* \<-- B5 to RC Rec SBUS <- RC Receiver White Wire
* RC Receiver Black Wire --> GND (place next to white wire)
* RC Receiver Red Wire --> +5V
****** RIGHT MOTOR ******
* P10(in) --> P2(out)
* P11(out)--> A1 on logic level converter
* \----> B1 ----> Right ESC (white wire) SPI-MOSI -> Right ESC
* Right ESC (black wire) --> GND (place next to white wire)
* P13(out) -> SPI clock --> P4 (our actual clock to drive slave input for telemetry
****** LEFT MOTOR ******
* P0(in) --> P2(out)
* P26(out)--> A8 on logic level converter
* \----> B8 ----> Left ESC (white wire) SPI-MOSI -> Left ESC
* Left ESC (black wire) --> GND (place next to white wire)
* P27 (in) SPI1 clock --> P4 (our generated clock)
****** Logic Level Conversion Board ******
* * OE → 1K ohm resistor → 3.3V
* * VA --> Teensy 3.3V
* * VB --> 5V
* * GND --> GND
*/
// The "value" parameter below DOES NOT comes from the RC Transmitter
// It is a filtered & calculated value where -1000 represents max REVERSE and +1000 represents max FORWARD.
void SetSpeedCommandArray(uint32_t index, uint32_t whichESC, int32_t value)
{
uint32_t csum,mask,i,j,x,cp_value;
uint32_t buff[4];
//Treat +/- 50 around 0 as OFF
if (value<=-999)
value = 1047;
else if (value<-40)
value = (-1*value)+48; // map -50 to -999 to: 98 Min Reverse ... 1047 Max Reverse
else if (value<40)
value=0; //stop with values +/- 50
else if (value>=999)
value = 2047;
else //value >= 50
value += 1048; //map 50 to 999 to: 1098 Min Forward ... 2047 Max Forward
cp_value = value;
//#ifdef INCLUDE_OUTPUT_DEBUG_INFO
// Serial.printf("\nValue being sent to ESCs: %d %X ", value, value);
//#endif
// MUST leave in telemetry bit, even though we NEVER use it anymore
value = value<<1;
//calculate checksum
//csum = (value ^ (value >> 4) ^ (value >> 8)) & 0x0F; // Non-Inverted Checksum
csum = (~(value ^ (value >> 4) ^ (value >> 8))) & 0x0F; // Inverted Checksum
//add checksum
value = (value<<4) | csum;
mask = 0x8000; // set the mask's MSB (16th bit)... so we can iterate through each bit down to LSB
for (i=0;i<4;i++) // create four 32-bit words
{
// inner loop constructs one 32-bit word
if (mask&value) //bit==one
x=INV_ONE_BIT;
else
x=INV_ZERO_BIT;
mask >>= 1;
for (j=0;j<3;j++)
{
if (mask&value) //bit==one
x = (x<<8) | INV_ONE_BIT;
else
x = (x<<8) | INV_ZERO_BIT;
mask >>= 1;
}
//#ifdef INCLUDE_OUTPUT_DEBUG_INFO
// Serial.printf("\ni=%d %X %s", i, x, ShowBits(x));
//#endif
buff[i]=x;
}
//#ifdef INCLUDE_OUTPUT_DEBUG_INFO
// Serial.printf("\n");
//#endif
// Save speed command so we look at it or do some math later with it
if (whichESC&RIGHT_ESC)
SpeedArrayRightSentValue[index]=cp_value;
if (whichESC&LEFT_ESC)
SpeedArrayLeftSentValue[index]=cp_value;
//transfer data to where the ISR(s) will Use them.
uint32_t * rc=DshotSpeedArrayRight[index];
uint32_t * lc=DshotSpeedArrayLeft[index];
uint32_t * src;
cli();
if (whichESC&LEFT_ESC)
{
src=buff;
*lc++ = *src++;
*lc++ = *src++;
*lc++ = *src++;
*lc = *src;
}
if (whichESC&RIGHT_ESC)
{
src=buff;
*rc++ = *src++;
*rc++ = *src++;
*rc++ = *src++;
*rc = *src;
}
sei();
}
/*************************************************************************************************************************************
*** The 8000Hz timer will trigger the update ESCs via ESC_TimerSendCommand() (using dShot via SPI)
*** ESC_TimerSendCommand() will setup the Frame Complete Interrupt
*** The Frame Complete Interrupt is used to setup the receive of the telemetry data which will Trigger the "Receive Data Interrupt".
*** In the "Receive Data Interrupt", we collect the telemetry data, and leave the SPIs inactive.
**************************************************************************************************************************************/
// the following variable is only used in these ISRs
volatile uint32_t isr_sequenceRight[32]; // used to see the last 31 interrupts;
volatile uint32_t isr_sequenceLeft[32]; // used to see the last 31 interrupts;
volatile uint32_t isr_SPI_SLAVE_clock_active = 0;
//bit masks
#define SPI_CLOCK_RIGHT_ACTIVE 0x01
#define SPI_CLOCK_LEFT_ACTIVE 0x02
void ISR_ESC_Timer()
{
isr_cnt_timer++;
//isr_cnt_right++; // used for debugging
//isr_sequenceRight[(isr_cnt_right&31)]=0; // 0=Timer, 1=FCF, 2=RDF, 3=other
//isr_cnt_left++; // used for debugging
//isr_sequenceLeft[(isr_cnt_left&31)]=0; // 0=Timer, 1=FCF, 2=RDF, 3=other
//if (isr_cnt_timer%8000==0)
// Serial.printf("ISR timer=%d\n",isr_cnt_timer);
ESC_TimerSendCommand();
}
// HOWEVER, we will use the interrupt to capture the telemetry frame back from the ESC
void ISR_SPI_Right_Telemetry() //IRQ_LPSPI4
{
int i=0;
uint32_t data;
//isr_cnt_right++; // was used for debugging; now used to indicate a "new" telemetry value has occured... therefore, this has moved to the telemetry section.
// If this Interrupt is due to the Frame Complete Flag...
// (1) Activate PWM clock Clock <-- NOTE: This is a high frequency signal... try to shield it.
// (2) Set Slave Active via Chip Select (CS) (active LOW).
// NOTE: 1 & 2 are only done in the RIGHT ISR (don't do them for the left ISR.
// Actually, it shouldn't hurt doing these... after we receive the telemetry, we disable the SPI
// (3) setup to receive DSHOT_TELEMETRY_WORDS+1 32-bit words
// (4) Setup RX Data Interrupt
// (5) switch to slave mode,
// The double test below (looking at LPSPI_SR_RDF) is to avoid a corner case where the FCF gets set twice in a row.
data = LPSPI4_SR;
if ( (data&(LPSPI_SR_FCF|LPSPI_SR_RDF)) == LPSPI_SR_FCF)
{
//isr_sequenceRight[(isr_cnt_right&31)]=1; // 0=Timer, 1=FCF, 2=RDF, 3=other
LPSPI4_CR = 0x0000; //Disable SPI
asm volatile("nop");
if (isr_SPI_SLAVE_clock_active==0)
{
isr_SPI_SLAVE_clock_active = (SPI_CLOCK_RIGHT_ACTIVE | SPI_CLOCK_LEFT_ACTIVE);
pinMode(dSHOT_SLAVE_CLOCK_GENERATOR_PIN, OUTPUT); // this will start activating the SPI-SLAVE clock
analogWrite(dSHOT_SLAVE_CLOCK_GENERATOR_PIN, 2); // this will activate the SPI-SLAVE clock
pinMode(dSHOT_EXTERNAL_CHIP_SELECT_PIN, OUTPUT); // this may have been "PULLED-UP"
digitalWriteFast (dSHOT_EXTERNAL_CHIP_SELECT_PIN, LOW); // active LOW -- This triggers us to read the SPI bus -- Very important
}
IOMUXC_SW_MUX_CTL_PAD_GPIO_B0_00 = 0x13; //SPI SLAVE mode
LPSPI4_CR = 0x0300; //Leave Disabled SPI, reset FIFOs
asm volatile("nop");
LPSPI4_SR = 0x3f00;//Reset All flags, errors, and interrupts...
asm volatile("nop");
LPSPI4_CR = 0x0000; //Leave Disabled... stop FIFO Reset
asm volatile("nop");
LPSPI4_FCR= LPSPI_FCR_RXWATER(DSHOT_TELEMETRY_WORDS); // set RX watermark for (value+1) 32-bit words of data.
asm volatile("nop");
LPSPI4_IER= LPSPI_IER_RDIE ; // Disable all other interrupts, but Enable "Receive Data Interrupt" when we get our N 32-bit words
asm volatile("nop");
LPSPI4_CFGR1 = LPSPI_CFGR1_PINCFG(3) | LPSPI_CFGR1_NOSTALL ; // 3 = 11b : SOUT is used for input data and SIN is used for output data
asm volatile("nop");
// See page 2935 about setting this register for SLAVE mode.
LPSPI4_TCR = LPSPI_TCR_PCS(0) | 0x0000001F; //64 bits... we must "prime" the SPI to read by "writing" some data
asm volatile("nop");
// SPI protocol (built into the peripheral logic) requires we send as we receive
LPSPI4_TDR= 0x00;
asm volatile("nop");
LPSPI4_CR = 0x0001; //Enable SPI
asm volatile("nop");
// for DEBUGGING with Scope
#ifdef INCLUDE_OUTPUT_DEBUG_INFO
digitalWriteFast (SCOPE_TRIGGER_PIN, LOW); // active LOW -- This triggers the scope (debugging)
#endif
// Start SPI Slave Clock
}
else if (data&LPSPI_SR_RDF) //Receive Data Flag The Receive Data Flag is set whenever the number of words in the receive FIFO is greater than FCR[RXWATER] (FIFO Control Register)
{ // Read in TELEMETRY
isr_cnt_right++; // was used for debugging; now used to indicate a "new" telemetry value has occured... therefore, this has moved to the telemetry section.
//isr_sequenceRight[(isr_cnt_right&31)]=2; // 1=FCF, 2=RDF, 3=other
LPSPI4_CR = 0x0000; //Disable SPI
asm volatile("nop");
// Stop SPI Slave Clock
// indicate right motor is no longer using clock
isr_SPI_SLAVE_clock_active &= ~SPI_CLOCK_RIGHT_ACTIVE;
if (isr_SPI_SLAVE_clock_active==0)
{
pinMode(dSHOT_SLAVE_CLOCK_GENERATOR_PIN, INPUT_PULLUP); // this will de-activate the SPI-SLAVE clock
digitalWriteFast (dSHOT_EXTERNAL_CHIP_SELECT_PIN, HIGH); // inactive HIGH
}
IOMUXC_SW_MUX_CTL_PAD_GPIO_B0_00 = 0x03; //SPI SLAVE mode
//Do NOT reset FIFOs until we read our data!!!
uint32_t * ptr = ISR_rght_telem;
// Save isr_cnt and speed command so we have it, then do some math later with it
*ptr++ = isr_cnt_right;
*ptr++ = SpeedArrayRightSentValue[speedIndex];
i=2; //index of next item
while ((LPSPI4_RSR & LPSPI_RSR_RXEMPTY)==0) // Are there any words are in the Receive FIFO
{
data = LPSPI4_RDR; // this reads and REMOVES the data from the FIFO
if (data!=0xFFFFFFFF && i<TELEM_BUFFER_WORDS)
{
*ptr++ = data;
i++;
}
}
// Fill remaining words with something that would generate BAD checksum
while (i++ < TELEM_BUFFER_WORDS)
*ptr++ = 0xFFFFFFFA;
LPSPI4_CR = 0x0300; //Leave Disabled SPI, reset FIFOs
asm volatile("nop");
LPSPI4_SR = 0x3f00;//Reset All flags, errors, and interrupts...
asm volatile("nop");
LPSPI4_TCR = LPSPI_TCR_PCS(0);
asm volatile("nop");
LPSPI4_IER= 0x00; // Disable all SPI interrupts
asm volatile("nop");
LPSPI4_CR = 0x0000; //Leave Disabled... stop FIFO Reset
//asm volatile("nop");
#ifdef INCLUDE_OUTPUT_DEBUG_INFO
digitalWriteFast (SCOPE_TRIGGER_PIN, HIGH); // active LOW -- This shows when we completed the reading of the telemetry data
#endif
}
else
{ //should never get here.
#ifdef INCLUDE_OUTPUT_DEBUG_INFO
Serial.printf("\n************* SHOULD NOT BE HERE (right) *******************\n");
//print_SPI_registers("SPI1-Right-ISR...Other?",true); //DEBUGGING
#endif
/*
isr_sequenceRight[(isr_cnt_right&31)]=3; // 1=FCF, 2=RDF, 3=other
Serial.printf("\n(right) ISR Seq. (oldest)");
for (i=31;i>=0;i--)
{
Serial.printf(" %d", isr_sequenceRight[((isr_cnt_right-i)&31)]); // this acts as a big delay.
}
Serial.printf(" (newest)\n");
*/
LPSPI4_CR = 0x0300; //Leave Disabled SPI, reset FIFOs
asm volatile("nop");
LPSPI4_SR = 0x3f00;//Reset All flags, errors, and interrupts...
asm volatile("nop");
LPSPI4_CR = 0x0000; //Leave Disabled SPI, reset FIFOs
asm volatile("nop");
}
}
void ISR_SPI1_Left_Telemetry() //IRQ_LPSPI3
{
int i;
uint32_t data;
//isr_cnt_left++; // was used for debugging; now used to indicate a "new" telemetry value has occured... therefore, this has moved to the telemetry section.
LPSPI3_CR = 0x0000; //Disable SPI
asm volatile("nop");
// If this Interrupt is due to the Frame Complete Flag...
// (1) Activate PWM clock Clock <-- NOTE: This is a high frequency signal... try to shield it.
// (2) Set Slave Active via Chip Select (CS) (active LOW).
// (3) setup to receive DSHOT_TELEMETRY_WORDS+1 32-bit words
// (4) Setup RX Data Interrupt
// (5) switch to slave mode,
// The double test below (looking at LPSPI_SR_RDF) is to avoid a corner case where the FCF gets set twice in a row (it was happening)
data = LPSPI3_SR;
if ( (data&(LPSPI_SR_FCF|LPSPI_SR_RDF)) == LPSPI_SR_FCF)
{
//isr_sequenceLeft[(isr_cnt_left&31)]=1; // 0=Timer, 1=FCF, 2=RDF, 3=other
// Start SPI Slave Clock
if (isr_SPI_SLAVE_clock_active==0)
{
isr_SPI_SLAVE_clock_active = (SPI_CLOCK_RIGHT_ACTIVE | SPI_CLOCK_LEFT_ACTIVE);
pinMode(dSHOT_SLAVE_CLOCK_GENERATOR_PIN, OUTPUT); // this will start activating the SPI-SLAVE clock
analogWrite(dSHOT_SLAVE_CLOCK_GENERATOR_PIN, 2); // this will activate the SPI-SLAVE clock
pinMode(dSHOT_EXTERNAL_CHIP_SELECT_PIN, OUTPUT); // this may have been "PULLED-UP"
digitalWriteFast (dSHOT_EXTERNAL_CHIP_SELECT_PIN, LOW); // active LOW -- This triggers us to read the SPI bus -- Very important
}
//SPI1.setCS(0); this did not help... it does set SION (Software Input On Field) bit
IOMUXC_SW_MUX_CTL_PAD_GPIO_AD_B0_03 = 0x17; //SPI1 SLAVE mode
LPSPI3_CR = 0x0300; //Leave Disabled SPI, reset FIFOs
asm volatile("nop");
LPSPI3_SR = 0x3f00;//Reset All flags, errors, and interrupts...
asm volatile("nop");
LPSPI3_FCR= LPSPI_FCR_RXWATER(DSHOT_TELEMETRY_WORDS); // set RX watermark for (value+1) 32-bit words of data.
asm volatile("nop");
LPSPI3_IER= LPSPI_IER_RDIE ; // Disable all other interrupts, but Enable "Receive Data Interrupt" when we get our N 32-bit words
asm volatile("nop");
LPSPI3_CFGR1 = LPSPI_CFGR1_PINCFG(3) | LPSPI_CFGR1_NOSTALL ; // 3 = 11b : SOUT is used for input data and SIN is used for output data
asm volatile("nop");
LPSPI3_CR = 0x0000; //Leave Disabled... stop FIFO Reset
asm volatile("nop");
// See page 2935 about setting this register for SLAVE mode.
LPSPI3_TCR = LPSPI_TCR_PCS(0) | 0x0000001F; //32 bits... we must "prime" the SPI to read by "writing" some data
asm volatile("nop");
asm volatile("nop");
// SPI protocol (built into the peripheral logic) requires we send as we receive
LPSPI3_TDR= 0x00;
asm volatile("nop");
//enable SPI
LPSPI3_CR = 0x0001; //Enable SPI
asm volatile("nop");
}
else if (data&LPSPI_SR_RDF) //Receive Data Flag The Receive Data Flag is set whenever the number of words in the receive FIFO is greater than FCR[RXWATER] (FIFO Control Register)
{ // Read in TELEMETRY
isr_cnt_left++; // was used for debugging; now used to indicate a "new" telemetry value has occured... therefore, this has moved to the telemetry section.
//isr_sequenceLeft[(isr_cnt_left&31)]=2; // 0=Timer, 1=FCF, 2=RDF, 3=other
// Stop SPI Slave Clock
// indicate left motor is no longer using clock
isr_SPI_SLAVE_clock_active &= ~SPI_CLOCK_LEFT_ACTIVE;
if (isr_SPI_SLAVE_clock_active==0)
{
pinMode(dSHOT_SLAVE_CLOCK_GENERATOR_PIN, INPUT_PULLUP); // this will de-activate the SPI-SLAVE clock
digitalWriteFast (dSHOT_EXTERNAL_CHIP_SELECT_PIN, HIGH); // inactive HIGH
}
//Do NOT reset FIFOs until we read our data!!!
uint32_t * ptr = ISR_left_telem;
// Save isr_cnt and speed command so we have it, then do some math later with it
*ptr++ = isr_cnt_left;
*ptr++ = SpeedArrayLeftSentValue[speedIndex];
i=2; //index of next item
while ((LPSPI3_RSR & LPSPI_RSR_RXEMPTY)==0) // Are there any words are in the Receive FIFO
{
data = LPSPI3_RDR; // this reads and REMOVES the data from the FIFO
if (data!=0xFFFFFFFF && i<TELEM_BUFFER_WORDS)
{
*ptr++ = data;
i++;
}
}
// Fill remaining words with something that would generate BAD checksum
while (i++ < TELEM_BUFFER_WORDS)
*ptr++ = 0xFFFFFFFA;
LPSPI3_CR = 0x0300; //Leave Disabled SPI, reset FIFOs
asm volatile("nop");
LPSPI3_SR = 0x3f00;//Reset All flags, errors, and interrupts...
asm volatile("nop");
LPSPI3_TCR = LPSPI_TCR_PCS(0);
asm volatile("nop");
LPSPI3_IER= 0x00; // Disable all SPI interrupts
asm volatile("nop");
LPSPI3_CR = 0x0000; //Leave Disabled... stop FIFO Reset
asm volatile("nop");
}
else
{ //should never get here.
#ifdef INCLUDE_OUTPUT_DEBUG_INFO
Serial.printf("\n************* SHOULD NOT BE HERE (left) *******************\n");
//print_SPI_registers("SPI1-Left-ISR...Other?",true); //DEBUGGING
#endif
/*
isr_sequenceLeft[(isr_cnt_left&31)]=3; // 0=Timer, 1=FCF, 2=RDF, 3=other
Serial.printf("\n(left) ISR Seq. (oldest)");
for (i=31;i>=0;i--)
Serial.printf(" %d", isr_sequenceLeft[((isr_cnt_left-i)&31)]); // this acts as a big delay.
Serial.printf(" (newest)\n");
*/
LPSPI3_CR = 0x0300; //Leave Disabled SPI, reset FIFOs
asm volatile("nop");
LPSPI3_SR = 0x3f00;//Reset All flags, errors, and interrupts...
asm volatile("nop");
LPSPI3_CR = 0x0000; //Leave Disabled SPI, reset FIFOs
asm volatile("nop");
}
}
// ALERT: Teensy 4.0 maps the internal LPSPI-4 to the external pins on the Teensy for SPI (CS:pin10,MOSI:pin11,MISO:pin12,SCK:pin13)
// It looks like LPSPI-3 is mapped to the external pins on the Teensy for SPI1 (CS1:pin00,MOSI1:pin26,MISO1:pin01, SCK1:pin27)
// It looks like LPSPI-1 is mapped to the external pins on the Teensy for SPI2 (CS2:pin36,MOSI2:pin34,MISO2:pin35,SCK2:pin37)
// This next function is used by the ISR_Timer-Interrupt and takes no parameters.
void ESC_TimerSendCommand()
{
//int i=0;
isr_SPI_SLAVE_clock_active=0; // reset the flags that tell us who is still using the above signals for SPI SLAVE mode.
pinMode(dSHOT_SLAVE_CLOCK_GENERATOR_PIN, INPUT_PULLUP); // this will de-activate the SPI-SLAVE clock (if it was active)
// as soon as we enable the SPI and SPI1 (in a short while below), the Chip Select signals, pins: 10 and 0 which we've wired to this pin, will be driven low
pinMode(dSHOT_EXTERNAL_CHIP_SELECT_PIN, INPUT_PULLUP); // this should decrease noise to our SPI chip
//print_SPI_registers("ESC_SendCommand start",true);
IOMUXC_SW_MUX_CTL_PAD_GPIO_B0_00 = 0x03; //SPI MASTER mode:
IOMUXC_SW_MUX_CTL_PAD_GPIO_AD_B0_03 = 0x07; //SPI1 MASTER mode
#ifdef INCLUDE_OUTPUT_DEBUG_INFO
digitalWriteFast (SCOPE_TRIGGER_PIN, HIGH); // inactive HIGH
#endif
//RIGHT_ESC) //LPSPI4 to the external pins on the Teensy for SPI (CS:pin10,MOSI:pin11,MISO:pin12,SCK:pin13)
// IF module busy, count how often that happens, and wait... (this should never happen if our code is working as expected... it happened during my testing)
// WHAT SHOULD BE DONE IF THIS IS CALLED AND THE MODULE IS BUSY? An infinite loop is bad... just barge ahead.
if (LPSPI4_SR&LPSPI_SR_MBF)
{
cnt_spi_busy++;
// THIS SHOULD NOT HAPPEN!!!!!!!
#ifdef INCLUDE_OUTPUT_DEBUG_INFO
//Serial.printf(" SPI module (right) busy:=%d ", cnt_spi_busy); // this acts as a big delay.
#endif
/* DO NOT BLOCK!... just barge forward.
while (LPSPI4_SR&LPSPI_SR_MBF)
{
//i++;
#ifdef INCLUDE_OUTPUT_DEBUG_INFO
Serial.printf("."); // this acts as a big delay.
//delay(1);
//if (i==100)
// print_SPI_registers("\nBusy in Send Command",true);
#endif
if (LPSPI4_SR&LPSPI_SR_MBF)
{
asm volatile("nop"); asm volatile("nop"); asm volatile("nop");
asm volatile("nop"); asm volatile("nop"); asm volatile("nop");
}
}
Serial.printf("\n"); // this acts as a big delay.
*/
}
LPSPI4_CR = 0x0300; //Disable SPI, reset FIFOs
asm volatile("nop");
LPSPI4_SR = 0x3f00;//Reset All flags and errors
asm volatile("nop");
LPSPI4_CFGR1 = LPSPI_CFGR1_PINCFG(0) | LPSPI_CFGR1_NOSTALL | LPSPI_CFGR1_MASTER ; // removed LPSPI_CFGR1_OUTCFG
asm volatile("nop");
// Setup SPI for Frame Complete Interrupt
// telemetry read will be triggered at the end of our "send speed command".
LPSPI4_IER= LPSPI_IER_FCIE; // Frame Complete Interrupt Enable
asm volatile("nop");
uint32_t * ptr;
if (speedIndex>63) //something is wrong, this should not happen
ptr = DshotSpeedArrayRight[0]; //STOP
else
ptr = DshotSpeedArrayRight[speedIndex]; //Whatever speed we've been told to load
LPSPI4_TCR = LPSPI_TCR_RXMSK | 0x000007F; // Mask RX characters (do not store); BF(+1) is 192 bits (6x32), 9F(+1) is 160 bits (5x32)
asm volatile("nop");
LPSPI4_TDR= *ptr++;
asm volatile("nop");
LPSPI4_TDR= *ptr++;
asm volatile("nop");
LPSPI4_TDR= *ptr++;
asm volatile("nop");
LPSPI4_TDR= *ptr;
asm volatile("nop");
LPSPI4_CR = 0x01;//Enable SPI and GoGoGo
// LEFT_ESC //LPSPI3
// IF module busy, count how often that happens, and wait... (this should never happen if our code is working as expected... it happened during my testing)
// WHAT SHOULD BE DONE IF THIS IS CALLED AND THE MODULE IS BUSY? An infinite loop is bad... just barge ahead.
if (LPSPI3_SR&LPSPI_SR_MBF)
{
cnt_spi1_busy++;
#ifdef INCLUDE_OUTPUT_DEBUG_INFO
Serial.printf(" SPI1 module (left) busy:=%d ", cnt_spi1_busy); // this acts as a big delay.
#endif
}
LPSPI3_CR = 0x0300; //Disable SPI, reset FIFOs
asm volatile("nop");
LPSPI3_SR = 0x3f00;//Reset All flags and errors
asm volatile("nop");
LPSPI3_CFGR1 = LPSPI_CFGR1_PINCFG(0) | LPSPI_CFGR1_NOSTALL | LPSPI_CFGR1_MASTER ; // removed LPSPI_CFGR1_OUTCFG
asm volatile("nop");
// Setup SPI for Frame Complete Interrupt
// telemetry read will be triggered at the end of our "send speed command".
LPSPI3_IER= LPSPI_IER_FCIE; // Frame Complete Interrupt Enable
asm volatile("nop");
if (speedIndex>63) //something is wrong, this should not happen
ptr = DshotSpeedArrayLeft[0]; //STOP
else
ptr = DshotSpeedArrayLeft[speedIndex]; //Whatever speed we've been told to load
LPSPI3_TCR = LPSPI_TCR_RXMSK | 0x000007F; // Mask RX characters (do not store); BF(+1) is 192 bits (6x32), 9F(+1) is 160 bits (5x32)
asm volatile("nop");
LPSPI3_TDR= *ptr++;
asm volatile("nop");
LPSPI3_TDR= *ptr++;
asm volatile("nop");
LPSPI3_TDR= *ptr++;
asm volatile("nop");
LPSPI3_TDR= *ptr;
asm volatile("nop");
LPSPI3_CR = 0x01;//Enable SPI and GoGoGo
}