Implementing bidirectional DShot on Teensy 3.2

Status
Not open for further replies.

clingmanr

New member
I am a relative newbie, but I am trying to accomplish something a bit advanced/technical. I am trying to implement bidirectional DShot communication with a DShot600 ESC. Using the DSHot protocol this way entails sending the 16 bit command packet to the ESC signal line at 600,000 bits per second (with each bit encoded by pulse width, 1.67 usec total per bit), followed by listening for a returning telemetry packet in the same format.

The transmitting part is already mostly solved and working, as I can already find a few implementations of DShot transmitting using DMA (like the teensyshot library or some code provided by christoph in this thread). The receiving part looks more challenging (the only published code I can find that does this is a fork of BLHeli_S written in assembly and some code in the BetaFlight code base that I can't seem to get a grip on), and this is where I could use some suggestions on how i might proceed. Ideally I would be able to quickly switch one pin from transmitting to receiving and back again as the protocol intends, but as I am not knowledgeable as to how to do this I was considering hooking the ESC signal line to two separate pins for transmitting and receiving, then just listening for the telemetry packets when a command packet is not being sent. So far I have two ideas of directions to try to go with this, but please tell me if I'm missing something easier.

The first idea is to use the FreqMeasureMulti library to watch the receiving pin and give me the pulse widths in nanoseconds to then translate into the bits. The problem I suspect this approach would run into is that both libraries configure and use FTM0, which I'm guessing would conflict and prevent them from working properly. Would it be simple to convert the FreqMeasure library to use FTM1 or FTM2 to let them work together?

The second idea is to basically make my own application specific version of option 1. I'm thinking something like setting the receiving pin up as an interrupt pin for pin change, then trying to figure out how to use the ISR to record the beginning and end of each pulse in nanoseconds (or clock cycles?) to then parse into the bits.

This is all a bit of a challenge because the difference between a 0 bit and a 1 bit is a few hundred nanoseconds of difference in pulse length, but I don't think that there is a baked in method of measuring time below micros().

Any suggestions for implementing one of these options? Any other options? Any easy way to do both transmitting and reveiving on the same pin?
 
Not a great design - why not just use a standard serial baudrate?

Anyway I suspect the easiest way to decode it is to set up a UART for 6Mbaud (if this is possible),
one start one stop no parity with inverted sense (space = 0, mark = 1)

Then the zero pattern after inversion looks like 0001111111 = start, 0b11111100, stop
and one pattern looks like 0000000111 = start, 0b11000000, stop
(serial is LSB first).

For timing variation you'd need to intepret 0xFE, 0xFC, 0xF8 as a zero, 0xE0, 0xC0, 0x80 as a one.

You'd need to check the arrival time to detect inter-frame gaps (2us more). That might be best done
by using a loop to read a whole frame, timing out after 2us waiting for available().
 
Not a great design - why not just use a standard serial baudrate?

And this is why I asked the question, I thought that there was probably a fairly simple approach I was missing. Thanks for the help!

I've never used serial communication for baud rates outside of the typical speeds baked into components/sensors, so I didn't consider just using a non-standard rate.

It sounds like I will have to do a bit of reading in order to fully grasp all you are saying about setup, bit patterns, and byte parsing. If the transmitting and receiving is all on one wire of the ESC, with this approach was I at least on the right track of using separate transmit and receive pins on the Teensy and using a CTS pin to interrupt transmitting in order to listen to the receiving line?
 
About nine months ago, I implemented Bidirectional Dshot 600/1200 with support for telemetry using a Teensy 4.0. It very worked well. I used the SPI peripherals in the manner that MarkT suggested. I used the SPI peripherals because it allowed me to take advantage of the circular buffering to continually re-send the speed command at an 8kHz rate with virtually zero overhead on the main processing loop. It was a good amount of work to figure it out, and I had to buy an oscilloscope to debug and test the software. It was done via direct register access to the SPI peripherals (no SPI library), and I spent a lot of time re-reading the IMXRT1060 Manual. With the Teensy 4.0, you are limited to three ESCs, but I only needed to control two motors.

I am now re-writing my code (I wouldn't call it a library) to implement "normal" (not bidirectional) dShot without telemetry. The irony is I had it working before I learned that Dshot telemetry required the inverted signaling in the bidirectional dShot protocol (I didn't save my first version... just started re-writing it... bummer). For my purposes, the eRPM values jumped around a fair amount and were not accurate enough for my purposes. If you need precise speed and/or motor location, use/create a sensored motor. I am switching because the telemetry adds some processing overhead to the ESC, and for my purposes I need/want the ESC to be unfettered by that processing.

Please let me know what you did. The Teensy 4.0 has 7 UARTs and the 4.1 has 8 UARTs. I was tempted to go that route, but I only needed to control two ESCs and the idea of automated continuous Dshot control pulled me toward the SPI peripherals.
 
I implemented Bidirectional Dshot 600/1200 with support for telemetry using a Teensy 4.0. It very worked well.

Would you be willing to share some of your code as an example?

I am interested in standing up bidrectional dshot1200 on a teensy 4.0 as well and found your post while looking for the best way to get started.
 
Is there a good definition of the protocol? All I've found is forum and github discussions - enough to understand the approach, but not enough to write a driver.
 
Some details about dShot implementation on Teensy 4

While I have some pride at having figured this out, I am not proud of my spaghetti code. I do intend to do a re-write and re-structure it so it is useable by others. Until then I am happy to share my notes, code-snippets, and important insights. However, I am starting to build bigger Bots, and will be switching to VESC based ESCs (instead of BLHELI32).

The starting point, as requested by brtaylor, is the definition of the protocol. There is no single concise document... however, here is a link to the key point in a thread where the details are exposed:
https://github.com/betaflight/betaflight/pull/8554#issuecomment-512507625
and a good overview of the timing of the signal is here:
https://blck.mn/2016/11/dshot-the-new-kid-on-the-block/

Even with the above information, I would not have been able to get my implementation to work without a digital scope (which I bought just for this project).

The most important detail to be aware of is that "dShot" and "bidirectional dShot" are two different protocols. They share similarities, but they are different. "dShot" was developed/defined first and did not include the "eRPM telemetry" data. "dShot" is a simpler protocol to implement since it only drives the signal from the teensy to the ESC. Via "dShot" commands, you can request telemetry data from the ESC, but that data will be communicated on a separate (third) wire (not available on all ESCs). In my first implementation, I implemented this fully on a Teensy 4 via the SPI peripheral with the telemetry data being read by the UART peripheral. As I wrote above, I did not save my original dShot code, and am no longer using the UART code to receive the telemetry data (but do have it-- another big hack job).

The dShot telemetry data is a data packet containing information like temperature, voltage, current, accumulated current consumption, RPM values, etc... Both "dShot" and "Bidirectional dShot" support the ability to request this data. Both "dShot" and "Bidirectional dShot" are one wire protocols. What makes "bidirectional dShot" different is that it returns the eRPM motor speed after every dShot command using the same wire to receive the eRPM value.

"Bidirectional dShot" is a bit more complicated because it requires the wire to made bidirectional. The encoded the dShot command is sent on the same wire as the received eRPM data.

The BLheli32 firmware requires a minimum command rate of 1000hz (below that rate the firmware stops the motor). I chose to operate at 8Khz. Much higher is possible; but higher speeds come at the expense of stressing the BLheli32 firmware (sskaug advises against higher than 32kHz).

I think both "dShot" and "Bidirectional dShot" have a max speed of dShot-1200 because the protocol creators specified a fixed minimum 40microsecond delay between dShot commands. I can expand on this if you want, I spent some time looking at higher rates.

Here is a link to a post with a graph showing the eRPM telemtry data (using "bidirectional dShot-600 at 8kHz):
https://github.com/bitdump/BLHeli/issues/446

When looking at the timing data from "dshot-the-new-kid-on-the-block", I noticed that the "high-pulse/low pulse" ratios were defined perfectly bit 8 bits; therefore I chose to encode each bit as a single byte. Many others have used 3 bits to represent one bit (I have no idea if their error rates were higher than mine).

Code:
//This bit pattern represents a ZERO bit: 1110 0000 = 0xE0  37.5% pulse width
#define ZERO_BIT 0xE0
// This bit pattern represents an INVERTED ZERO bit: 0001 1111 = 0x1F
#define INV_ZERO_BIT 0x1F
//This bit pattern represents a ONE bit: 1111 1100 = 0xFC  75% pulse width
#define ONE_BIT 0xFC
// This bit pattern represents an INVERTED  ONE bit: 0000 0011 = 0x03
#define INV_ONE_BIT 0x03

Using the above definitions, I would create byte arrays, with each byte representing one bit, to fabricate the dShot command I want to send.

At this point, pick your teensy peripheral to generate the signal.

I chose the SPI because I thought I could use the SPIs ability to send data from its buffer in a circular fashion. I did not end up using that capability, but instead have a 8000Hz timer that triggers a resend. The command comes from some shared memory, so my main program loop just updates the shared memory whenever it wants a speed change. After setup, the my ISRs access the SPI at 8000Hz, and my main program just updates the speed commands. I do have two ISRs in play for each ESC: one for sending the command, and one for receiving the eRPM telemetry data. The eRPM telemetry data is just saved to a memory (and accessed when needed).

The big limitation to using the Teensy 4 SPI peripheral is that there are only three of them (and two of those don't have exposed pins).

Let me know what more you'd like. I can share code snippets, but do not have any library for doing this. All my SPI interactions were done via director register access to the SPIs. I spent hours re-reading details in the .MX RT1060 Processor Reference Manual.

For example, here is my SPI setup code (for two ESCs):

Code:
//DISCLAIMER: Some of the comments below may be out of date.
IntervalTimer ESCtimer;
/*
 * ESC_SPI_setup() must be called from setup.
 * It sets up BOTH the LEFT and RIGHT ESCs.
 */
void ESC_SPI_setup()
{
  isr_cnt_right=0;  
  SetupSpeedCommandArray();
  speedIndex=0;
  // check how SetupSpeedCommandArray() looks... looks perfect
  //for (i=0;i<64;i++)
  //  Serial.printf("%d\tRIGHT\t%d\t%8X\t%8X\t%8X\t%8X\tLEFT\t%d\t%8X\t%8X\t%8X\t%8X\n",i,SpeedArrayRightSentValue[i],DshotSpeedArrayRight[i][0],DshotSpeedArrayRight[i][1],DshotSpeedArrayRight[i][2],DshotSpeedArrayRight[i][3],SpeedArrayLeftSentValue[i],DshotSpeedArrayLeft[i][0],DshotSpeedArrayLeft[i][1],DshotSpeedArrayLeft[i][2],DshotSpeedArrayLeft[i][3]);
  
  // Setup Telemetry Clock
  // ClckFreq = dSHOT_BIT_RATE*5/4;
  // we want to triple sample, that way two out of three gives us something reliable
  // ClckFreq = dSHOT_BIT_RATE*15/4;  <-- TRIPLE SAMPLING RATE
  pinMode(dSHOT_SLAVE_CLOCK_GENERATOR_PIN, OUTPUT); // this will start activating the SPI-SLAVE clock
  analogWriteResolution(2);
  analogWriteFrequency(dSHOT_SLAVE_CLOCK_GENERATOR_PIN, (dSHOT_BIT_RATE*DSHOT_TELEMETRY_SAMPLING_RATE*5/4)); // scope does show Telemetry bits have 720KHz rate
  analogWrite(dSHOT_SLAVE_CLOCK_GENERATOR_PIN, 2);  // this will activate the SPI-SLAVE clock
  delay(1); //let it run.
  pinMode(dSHOT_SLAVE_CLOCK_GENERATOR_PIN, INPUT_PULLUP); // this will de-activate the SPI-SLAVE clock

  //setup dshot active pin... used in SLAVE mode
  pinMode (dSHOT_EXTERNAL_CHIP_SELECT_PIN, OUTPUT); // active LOW
  digitalWriteFast (dSHOT_EXTERNAL_CHIP_SELECT_PIN, HIGH);  // inactive HIGH
  pinMode (SCOPE_TRIGGER_PIN, OUTPUT); // active LOW
  digitalWriteFast (SCOPE_TRIGGER_PIN, HIGH);  // inactive HIGH
  pinMode(dSHOT_ACTUAL_SPI_CHIP_SELECT_PIN, INPUT);
  pinMode(dSHOT_ACTUAL_SPI1_CHIP_SELECT_PIN, INPUT);
  //digitalWrite(dSHOT_ACTUAL_SPI1_CHIP_SELECT_PIN, LOW);

//Serial.printf( "SPI MISO: %d MOSI: %d, SCK: %d CS: %d\n", hardware().miso_pin[0],  hardware().mosi_pin[0],  hardware().sck_pin[0],  hardware().cs_pin[0] );

  //i=0   hardware().cs_pin[i]=10   hardware().sck_mux[i]=13 
  SPI.begin(); 
  SPI.setCS(10); // This is for SLAVE mode... 
  SPI.beginTransaction(SPISettings(20000000, MSBFIRST, SPI_MODE0));


  //i=0   hardware().cs_pin[i]=10   hardware().sck_mux[i]=13 
  //i=0   hardware().cs_pin[i]=0   hardware().sck_mux[i]=12 
  SPI1.begin(); 
  SPI1.setCS(0); // This is for SLAVE mode... 
  SPI1.beginTransaction(SPISettings(20000000, MSBFIRST, SPI_MODE0));   
  //Serial.printf(" SPI:  IOMUXC_LPSPI4_PCS0_SELECT_INPUT = %X\n", (IOMUXC_LPSPI4_PCS0_SELECT_INPUT));
  //Serial.printf(" SPI:  IOMUXC_SW_MUX_CTL_PAD_GPIO_B0_00 = %X\n", (IOMUXC_SW_MUX_CTL_PAD_GPIO_B0_00));
  //Serial.printf("SPI1:  IOMUXC_LPSPI3_PCS0_SELECT_INPUT = %X\n", (IOMUXC_LPSPI3_PCS0_SELECT_INPUT));
  //Serial.printf("SPI1:  IOMUXC_SW_MUX_CTL_PAD_GPIO_AD_B0_03 = %X\n", (IOMUXC_SW_MUX_CTL_PAD_GPIO_AD_B0_03));

//Serial.printf( "SPI MISO: %d MOSI: %d, SCK: %d CS: %d\n", hardware().miso_pin[0],  hardware().mosi_pin[0],  hardware().sck_pin[0],  hardware().cs_pin[0] );
//    SPI.setClockDivider_noInline(4800000);
//   SPI1.setClockDivider_noInline(4800000);

  // initialize SPI via DIRECT hardware writes
#ifdef INCLUDE_OUTPUT_DEBUG_INFO
//  print_SPI_registers("SPI Setup Top",true);
#endif
  // Do NOT reset SPI since we used the SPI library to set it up... we will just overwrite the settings with what we want.
  //LPSPI4_CR=0x02; //Disable and reset SPI: *(LPSPI4_BASE+0x10) = 0x02
  //LPSPI4_CR = 0x00;//Disable SPI: *(LPSPI4_BASE+0x10) = 0x00
  LPSPI4_CR = 0x0300; //Disable SPI, reset FIFOs
  asm volatile("nop");
  LPSPI3_CR = 0x0300; //Disable SPI, reset FIFOs
  asm volatile("nop");
  // Most of the following registers can only be changed while the SPI is disabled.
  LPSPI4_IER=0x00; // (LPSPI4_BASE+0x18) = 0x00  Interrupts Off (off by default after reset)
  asm volatile("nop");
  LPSPI4_DER=0x00; // (LPSPI4_BASE+0x1C) = 0x00  DMA Off (off by default after reset)
  asm volatile("nop");
  LPSPI3_IER=0x00; // (LPSPI4_BASE+0x18) = 0x00  Interrupts Off (off by default after reset)
  asm volatile("nop");
  LPSPI3_DER=0x00; // (LPSPI4_BASE+0x1C) = 0x00  DMA Off (off by default after reset)
  asm volatile("nop");
  //0x0100;  //CIRFIFO enabled ... We may need to set this AFTER we load the data into the xmit FIFO
  LPSPI4_CFGR0 = 0x00; //LPSPI_CFGR0_CIRFIFO ; // | LPSPI_CFGR0_HREN | LPSPI_CFGR0_HRSEL;
  asm volatile("nop");
  LPSPI3_CFGR0 = 0x00; //LPSPI_CFGR0_CIRFIFO ; // | LPSPI_CFGR0_HREN | LPSPI_CFGR0_HRSEL;
  asm volatile("nop");
  //LPSPI4_CFGR1 = LPSPI_CFGR1_OUTCFG | LPSPI_CFGR1_PINCFG(2) | LPSPI_CFGR1_NOSTALL | LPSPI_CFGR1_MASTER ; 
  LPSPI4_CFGR1 = LPSPI_CFGR1_PINCFG(2) | LPSPI_CFGR1_NOSTALL | LPSPI_CFGR1_MASTER ; 
  asm volatile("nop");
  LPSPI3_CFGR1 = LPSPI_CFGR1_PINCFG(2) | LPSPI_CFGR1_NOSTALL | LPSPI_CFGR1_MASTER ; 
  asm volatile("nop");
  // clock config...
  //Will need to play around with these four 8-bit values.
  //SCKPCS : SCK-to-PCS Delay : delay from the last SCK edge to the PCS negation 
  //PCSSCK : PCS-to-SCK Delay : delay from the PCS assertion to the first SCK edge
  //DBT : Delay Between Transfers : thinking of enforcing the delay by having an output value of 0.  //Not sure what happens to the MOSI signal during this period.
  //SCKDIV : SCK Divider : configures the divide ratio of the SCK pin.
  // leave the clock alone for now
  //LPSPI4_CCR = 0x00001d3A;
  LPSPI4_CCR = DSHOT_SCKDIV; // for dShot-1200 use 0x00000018; for dShot-60 use 0x00000030; 
  asm volatile("nop");
  LPSPI3_CCR = DSHOT_SCKDIV; // for dShot-1200 use 0x00000018; for dShot-60 use 0x00000030; 
  asm volatile("nop");
  //LPSPI4_TCR = LPSPI_TCR_RXMSK | LPSPI_TCR_TXMSK; // Mask RX characters (do not store); BF(+1) is 192 bits (6x32), 9F(+1) is 160 bits (5x32)
  //asm volatile("nop");
  //LPSPI3_TCR = LPSPI_TCR_RXMSK | LPSPI_TCR_TXMSK; // Mask RX characters (do not store); BF(+1) is 192 bits (6x32), 9F(+1) is 160 bits (5x32)
  //asm volatile("nop");
  // we'll let the first dShot command enable the SPI
  LPSPI4_CR=0x00; // enable SPI = 0x01  // will be enabled at first command
  LPSPI3_CR=0x00; // enable SPI = 0x01 
  //asm volatile("nop");
  #ifdef INCLUDE_OUTPUT_DEBUG_INFO
  //print_SPI_registers("SPI Setup Bottom",true);
  #endif

  /********************************************************************
  *** No longer planning to use the Frame Complete Interrupt
  *** Instead using a 8000Hz timer to update ESCs using dShot via SPI 
  *********************************************************************/
// HOWEVER, we will use the interrupt to capture the telemetry frame back from the ESC
  //RIGHT_ESC is LPSPI4
  attachInterruptVector(IRQ_LPSPI4, ISR_SPI_Right_Telemetry);
  NVIC_SET_PRIORITY(IRQ_LPSPI4, 120); // set priority
  NVIC_ENABLE_IRQ(IRQ_LPSPI4); // enable IRQ

  //LEFT_ESC is LPSPI3
  attachInterruptVector(IRQ_LPSPI3, ISR_SPI1_Left_Telemetry);
  NVIC_SET_PRIORITY(IRQ_LPSPI3, 124); // set priority
  NVIC_ENABLE_IRQ(IRQ_LPSPI3); // enable IRQ

  // SetUp 8KHz Timer that loads dShot 
  // before doing this, load the STOP command into the ESCs and SendCommandBuffer
  // create STOP values
  //  THIS is now done via speedIndex: set_dShot_command(ALL_ESC, 0, 0);
  ESCtimer.begin(ISR_ESC_Timer, 125);  // Update ESCs every 125 microseconds (8000Hz);
  
  // Must initialize ESCs with STOP command, otherwise they won't work.
}
 
Thanks for the quick and detailed response!

I'm sure I'll have more questions as I digest the manual and work on the implementation.

For now, I want to see if I understand your approach correctly...

ISR_ESC_Timer: Triggered every 125 microseconds
1. Disable slave clock generator
2. Disable slave chip select
3. Create byte array representing the dshot frame with currently desired throttle from shared memory
4. Place SPI hardware into master mode
5. Write command data in a blocking transaction
6. Place SPI hardware into slave mode
7. Enable slave clock generator
8. Enable slave chip select

ISR_SPI_X_Telemetry: Triggered whenever SPI hardware receives data in slave mode
1. Read byte data, decode into response bits
2. Decode response bits into dshot frame
3. Place eRPM value into shared memory


Clarifying Questions:

1. DSHOT_SLAVE_CLOCK_GENERATOR_PIN is generating a fake clock signal while the SPI is in slave mode in order to trick the hardware into sampling the clock-less ESC responses. Is there a physical loopback connection between this output pin and both SCK pins on the teensy? Same with CS?

2. Is the ESC's lone bidirectional data wire connected to both MOSI and MISO pins simultaneously?

Thanks!
 
You certainly dug in... Regarding 1...8 (SPI setup); correct on 3...8; For 1 & 2, I set those it up (getting them ready) and then (yes) disable it.

Regarding ISR_SPI_X_Telemetry, Yes... the nice thing is that the interrupt is generated once I have the full telemetry message (buffer hits configurable number of bytes).

Regarding Question 1: is it fake if its being used? Yes, the Teensy needs a clock signal when its in slave mode (and the ESC doesn't give you one), so we are creating one. Yes, there is a physical wire from the clock signal to the (two) SCK pins. Below are my wiring notes for the (Teensy) SPIs to the ESCs. Note, I am using a logic level converter.
Regarding Question 2: See the wiring notes below... The SPI-MOSI is being programmed to be the SLAVE as well as the MASTER; you can see how this is done in the code below.

I am adding three more code blocks that will fill in the missing details:
(1) ISR_ESC_Timer()
(2) ISR_SPI_X_Telemetry()
(3) Code that creates the dShot speed command array. Note: I pre-compute most of my speed commands during setup.
NOTE: a screwy thing about (bidirectional only?) dShot is that the values sent to the ESC mean this:
48 Min Reverse ... 1047 Max Reverse
1048 Min Forward ... 2047 Max Forward
Initially, I thought I did something wrong when the motors responded with 1047 AS MAX REVERSE AND 1048 MIN FORWARD! SSKAUG confirmed that's the way it is.
You'd think they'd pair the mins together... also note that there is not STOP in between the two directions (like the normal RC protocol for directional speeds).
NOTE: The +/- 50 power gap in my code below is my doing to avoid low motor speeds that cause these small motors problems.


Code:
/* 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
}
 
Last edited:
Here are some of the #defines and variable definitions that will likely help if/when you dig into all that code above.

All that was written almost a year ago, so it is not fresh in my mind. But these comments clearly contain some important details.

Code:
//Used for eRPM telemetry (short message)
#define TELEM_BUFFER_WORDS 6
volatile uint32_t ISR_rght_telem[TELEM_BUFFER_WORDS];  //we store: isr_cnt, speed (stored as int), up to (TELEM_BUFFER_WORDS - 2) words from SPI read
volatile uint32_t ISR_left_telem[TELEM_BUFFER_WORDS];  //we store: isr_cnt, speed (stored as int), up to (TELEM_BUFFER_WORDS - 2) words from SPI read
// Used for Full Message Telemetry  <- no longer being used
//extern uint32_t TelemRightBytesRead;
//extern  uint8_t TelemRightBuffer[64];
//extern uint32_t TelemLeftBytesRead;
//extern  uint8_t TelemLefttBuffer[64];

// These must be defined together!
// dSHOT_BIT_RATE is either 600,000 or 1,200,000
// for dShot-600 use 0x00000030; for dShot-1200 use 0x00000018; 
// The ESC firmware doesn't rely on exact (clock) timings... it is using the ratio of the low signal relative to the high signal to interpret 1s and 0s.
// The number of 32-bit words to collect for telemetry
#define DSHOT_TELEMETRY_SAMPLING_RATE 3
#define dSHOT_BIT_RATE 600000
#define DSHOT_SCKDIV 0x00000034
#define DSHOT_TELEMETRY_PREFIX_WORDS 2
// dshot-600 works well, so use that for now
// The timing on dShot-1200 appears to be off... it worked better before bidirectional... maybe my telemetry code is disrupting the timing and is breaking this.
//#define dSHOT_BIT_RATE 1200000
//#define DSHOT_SCKDIV 0x00000019
//#define DSHOT_TELEMETRY_PREFIX_WORDS 5
// The number of telemetry words we must watch for is based on DSHOT_TELEMETRY_SAMPLING_RATE (not bit-rate/buad)
// the number of leading 0xFF bits we get is based on baud rate (we "throw-away" leading 0xFFFFFFFF words).
// When using 3 samples per bit: use 3 (21 values * 3 = 61; this will fit in two 32-bit words, but could span three words)
// When using 5 samples per bit: use 5 (21 values * 5 = 105; this will fit in four 32-bit words, but could span five words)
// NOTICE: The number of words we must read in is the same as the Sampling rate (coincidence?)
// HOWEVER: There is a leading 30 microsecond delay before the telemetry is given to us...
//          and sometimes the ESCs take a bit longer (seen 32 usec delays).
/*    
 *              telemetry rate is times 5/4 Dshot rate
 *         bit rate     600000  1200000   600000  1200000
 *      over sample          3        3        5        5
 *  telemetry clock    2250000  4500000  3750000  7500000
 * time per bit(usec)     0.44     0.22     0.27     0.13
 *   bits in 30usec       67.5    135.0    112.5    225.0
 *    32-bits words        2.1      4.2      3.5      7.0
 *    Must round UP:         3        5        4        8
 * The above shows the number of additional words we must read in to get to the telemetry data.
 * These leading 0xFFFFFFFF words will be discarded.
 */
// This is how many words coming into the SPI will trigger the "time-to-read-data" interrupt.
// NOTE: the value we set in the SPI regisiter is one less than the above values (e.g. if you want five, put four).
// The SPI only has a 16 word receive (and transmit buffer).
#define DSHOT_TELEMETRY_WORDS (DSHOT_TELEMETRY_PREFIX_WORDS+DSHOT_TELEMETRY_SAMPLING_RATE-1)
 
Status
Not open for further replies.
Back
Top