├── .gitignore
├── Arda
    ├── dma_v1.ino
    └── dshot_simple_api
    │   ├── .gitignore
    │   ├── include
    │       └── dshot_api.h
    │   ├── platformio.ini
    │   └── src
    │       ├── dshot_api.cpp
    │       └── main.cpp
├── ESCPID
    ├── AWPID.cpp
    ├── AWPID.h
    ├── DSHOT.cpp
    ├── DSHOT.h
    ├── ESCCMD.cpp
    ├── ESCCMD.h
    ├── ESCPID.h
    └── ESCPID.ino
├── LICENSE
├── README.md
├── _config.yml
├── docs
    ├── BLHeli32 cmd.pdf
    ├── K64P144M120SF5RM.pdf
    ├── KISS_telemetry.pdf
    ├── PID AW explained.pdf
    ├── PID.pdf
    ├── RT1060.pdf
    ├── RT1060_eFlexPWM.pdf
    └── RT1060_pin_mux.pdf
└── host
    ├── host.c
    └── host.h


/.gitignore:
--------------------------------------------------------------------------------
1 | 
2 | .DS_Store
3 | host/host
4 | 


--------------------------------------------------------------------------------
/Arda/dma_v1.ino:
--------------------------------------------------------------------------------
  1 | #include "DMAChannel.h"
  2 | #include <array>
  3 | 
  4 | IntervalTimer timer;
  5 | 
  6 | DMAChannel dma;
  7 | DMAChannel dma2;
  8 | DMAChannel dma3;
  9 | DMAChannel dma4;
 10 | DMAChannel dma5;
 11 | DMAChannel dma6;
 12 | 
 13 | const uint16_t short_pulse = uint64_t(F_BUS) * 625 / 1000000000;
 14 | const uint16_t long_pulse = uint64_t(F_BUS) * 1250 / 1000000000;
 15 | const uint16_t bit_length = uint64_t(F_BUS) * 1670 / 1000000000;
 16 | 
 17 | std::array<volatile uint16_t, 18> dma_source = {
 18 |   short_pulse, long_pulse,
 19 |   short_pulse, long_pulse,
 20 |   short_pulse, long_pulse,
 21 |   short_pulse, long_pulse,
 22 |   short_pulse, long_pulse,
 23 |   short_pulse, long_pulse,
 24 |   short_pulse, long_pulse,
 25 |   short_pulse, long_pulse,
 26 |   0, 0
 27 | };
 28 | 
 29 | std::array<volatile uint16_t, 18> dma_source2 = {
 30 |   short_pulse, long_pulse,
 31 |   short_pulse, long_pulse,
 32 |   short_pulse, long_pulse,
 33 |   short_pulse, long_pulse,
 34 |   short_pulse, long_pulse,
 35 |   short_pulse, long_pulse,
 36 |   short_pulse, long_pulse,
 37 |   short_pulse, long_pulse,
 38 |   0, 0
 39 | };
 40 | 
 41 | std::array<volatile uint16_t, 18> dma_source3 = {
 42 |   short_pulse, long_pulse,
 43 |   short_pulse, long_pulse,
 44 |   short_pulse, long_pulse,
 45 |   short_pulse, long_pulse,
 46 |   short_pulse, long_pulse,
 47 |   short_pulse, long_pulse,
 48 |   short_pulse, long_pulse,
 49 |   short_pulse, long_pulse,
 50 |   0, 0
 51 | };
 52 | 
 53 | std::array<volatile uint16_t, 18> dma_source4 = {
 54 |   short_pulse, long_pulse,
 55 |   short_pulse, long_pulse,
 56 |   short_pulse, long_pulse,
 57 |   short_pulse, long_pulse,
 58 |   short_pulse, long_pulse,
 59 |   short_pulse, long_pulse,
 60 |   short_pulse, long_pulse,
 61 |   short_pulse, long_pulse,
 62 |   0, 0
 63 | };
 64 | 
 65 | std::array<volatile uint16_t, 18> dma_source5 = {
 66 |   short_pulse, long_pulse,
 67 |   short_pulse, long_pulse,
 68 |   short_pulse, long_pulse,
 69 |   short_pulse, long_pulse,
 70 |   short_pulse, long_pulse,
 71 |   short_pulse, long_pulse,
 72 |   short_pulse, long_pulse,
 73 |   short_pulse, long_pulse,
 74 |   0, 0
 75 | };
 76 | 
 77 | std::array<volatile uint16_t, 18> dma_source6 = {
 78 |   short_pulse, long_pulse,
 79 |   short_pulse, long_pulse,
 80 |   short_pulse, long_pulse,
 81 |   short_pulse, long_pulse,
 82 |   short_pulse, long_pulse,
 83 |   short_pulse, long_pulse,
 84 |   short_pulse, long_pulse,
 85 |   short_pulse, long_pulse,
 86 |   0, 0
 87 | };
 88 | 
 89 | void myISR() {
 90 |   dma.clearInterrupt();
 91 |   FTM0_SC = 0; // disable FTM0
 92 | }
 93 | 
 94 | void setup() {
 95 |   setupDMA();
 96 | 
 97 |   timer.begin(myFunc, 200);
 98 | 
 99 |   // FTM0_SC = FTM_SC_CLKS(1); // enable timer with busclock
100 | }
101 | 
102 | void loop() {
103 | }
104 | 
105 | void myFunc() {
106 |   FTM0_SC = FTM_SC_CLKS(1);
107 | }
108 | 
109 | 
110 | void setupDMA() {
111 |   CORE_PIN22_CONFIG = PORT_PCR_MUX(4) | PORT_PCR_DSE | PORT_PCR_SRE; // FTM0_CH0
112 |   CORE_PIN23_CONFIG = PORT_PCR_MUX(4) | PORT_PCR_DSE | PORT_PCR_SRE; // FTM0_CH1
113 |   CORE_PIN9_CONFIG = PORT_PCR_MUX(4) | PORT_PCR_DSE | PORT_PCR_SRE; // FTM0_CH2
114 |   CORE_PIN10_CONFIG = PORT_PCR_MUX(4) | PORT_PCR_DSE | PORT_PCR_SRE; // FTM0_CH3
115 |   CORE_PIN6_CONFIG = PORT_PCR_MUX(4) | PORT_PCR_DSE | PORT_PCR_SRE; // FTM0_CH4
116 |   CORE_PIN20_CONFIG = PORT_PCR_MUX(4) | PORT_PCR_DSE | PORT_PCR_SRE; // FTM0_CH5
117 | 
118 |   // CORE_PIN5_CONFIG = PORT_PCR_MUX(4) | PORT_PCR_DSE | PORT_PCR_SRE;
119 | 
120 |   dma.sourceBuffer(dma_source.data(), sizeof(dma_source));
121 |   dma.destination((uint16_t&) FTM0_C0V);
122 |   dma.triggerAtHardwareEvent(DMAMUX_SOURCE_FTM0_CH7);
123 |   dma.interruptAtCompletion();
124 |   dma.attachInterrupt(myISR);
125 |   dma.enable();
126 | 
127 |   dma2.sourceBuffer(dma_source2.data(), sizeof(dma_source2));
128 |   dma2.destination((uint16_t&) FTM0_C1V);
129 |   dma2.triggerAtTransfersOf(dma);
130 |   dma2.triggerAtCompletionOf(dma);
131 |   dma2.enable();
132 | 
133 |   dma3.sourceBuffer(dma_source3.data(), sizeof(dma_source3));
134 |   dma3.destination((uint16_t&) FTM0_C2V);
135 |   dma3.triggerAtTransfersOf(dma2);
136 |   dma3.triggerAtCompletionOf(dma2);
137 |   dma3.enable();
138 | 
139 |   dma4.sourceBuffer(dma_source4.data(), sizeof(dma_source4));
140 |   dma4.destination((uint16_t&) FTM0_C3V);
141 |   dma4.triggerAtTransfersOf(dma3);
142 |   dma4.triggerAtCompletionOf(dma3);
143 |   dma4.enable();
144 | 
145 |   dma5.sourceBuffer(dma_source5.data(), sizeof(dma_source5));
146 |   dma5.destination((uint16_t&) FTM0_C4V);
147 |   dma5.triggerAtTransfersOf(dma4);
148 |   dma5.triggerAtCompletionOf(dma4);
149 |   dma5.enable();
150 | 
151 |   dma6.sourceBuffer(dma_source6.data(), sizeof(dma_source6));
152 |   dma6.destination((uint16_t&) FTM0_C5V);
153 |   dma6.triggerAtTransfersOf(dma5);
154 |   dma6.triggerAtCompletionOf(dma5);
155 |   dma6.enable();
156 | 
157 |   FTM0_C0SC = FTM_CSC_MSB | FTM_CSC_ELSB | FTM_CSC_CHIE;
158 |   FTM0_C0V = 0;
159 |   FTM0_C1SC = FTM_CSC_MSB | FTM_CSC_ELSB | FTM_CSC_CHIE;
160 |   FTM0_C1V = 0;
161 |   FTM0_C2SC = FTM_CSC_MSB | FTM_CSC_ELSB | FTM_CSC_CHIE;
162 |   FTM0_C2V = 0;
163 |   FTM0_C3SC = FTM_CSC_MSB | FTM_CSC_ELSB | FTM_CSC_CHIE;
164 |   FTM0_C3V = 0;
165 |   FTM0_C4SC = FTM_CSC_MSB | FTM_CSC_ELSB | FTM_CSC_CHIE;
166 |   FTM0_C4V = 0;
167 |   FTM0_C5SC = FTM_CSC_MSB | FTM_CSC_ELSB | FTM_CSC_CHIE;
168 |   FTM0_C5V = 0;
169 | 
170 |   FTM0_C7SC = FTM_CSC_CHIE | FTM_CSC_DMA | FTM_CSC_MSA | FTM_CSC_ELSA; // output compare, enable DMA trigger
171 |   FTM0_C7V = 0;
172 | 
173 | 
174 |   FTM0_SC = 0; // disable FTM0
175 |   FTM0_CNT = 0; // contains the FTM counter value
176 |   FTM0_MOD = bit_length; // the modulo value for the FTM counter
177 |   FTM0_CNTIN = 0; // counter initial value
178 | }
179 | 


--------------------------------------------------------------------------------
/Arda/dshot_simple_api/.gitignore:
--------------------------------------------------------------------------------
1 | .pio
2 | .pioenvs
3 | .piolibdeps
4 | .clang_complete
5 | .gcc-flags.json
6 | 


--------------------------------------------------------------------------------
/Arda/dshot_simple_api/include/dshot_api.h:
--------------------------------------------------------------------------------
 1 | #ifndef DHOT_API_H
 2 | #define DHOT_API_H
 3 | 
 4 | #include <Arduino.h>
 5 | #include "DMAChannel.h"
 6 | 
 7 | #define DSHOT_FREQ 500
 8 | 
 9 | #define TLM_LENGTH 10
10 | #define DSHOT_LENGTH 16
11 | #define DMA_LENGTH 18
12 | 
13 | //const uint16_t SP = uint64_t(F_BUS) * 625 / 1000000000; //short pulse
14 | //const uint16_t LP = uint64_t(F_BUS) * 1250 / 1000000000; // long pulse
15 | //const uint16_t BIT_LENGTH = uint64_t(F_BUS) * 1670 / 1000000000;
16 | // F_BUS is 60MHz
17 | #define SP 37
18 | #define LP 75
19 | #define BIT_LENGTH 100
20 | 
21 | #define DEFAULT_MODE 0
22 | #define ARMING_MODE 1
23 | #define SEND_COMMAND_MODE 2
24 | #define STOP_MODE 3
25 | 
26 | struct TlmData {
27 |   uint8_t temperature;
28 |   uint16_t voltage;
29 |   uint16_t current;
30 |   uint16_t consumption;
31 |   uint16_t rpm;
32 |   bool crcCheck;
33 | };
34 | 
35 | void readTlm();
36 | uint16_t getRpm(); 
37 | void startDshot();
38 | bool isTlmAvailable();
39 | void resetTlmFlag();
40 | void armingMotor();
41 | void enable3d();
42 | void disable3d();
43 | void setControlLoopFrequency(uint16_t freq);
44 | void enableTlm();
45 | void disableTlm();
46 | void setThrottle(uint16_t newThrottle);
47 | void stopMotor();
48 | 
49 | void sendDshot();
50 | void ISR_timer();
51 | void ISR_DMA();
52 | void setupDMA();
53 | uint8_t updateCrc8(uint8_t crc, uint8_t crc_seed);
54 | uint8_t calculateCrc8(const uint8_t *Buf, const uint8_t BufLen);
55 | 
56 | #endif
57 | 


--------------------------------------------------------------------------------
/Arda/dshot_simple_api/platformio.ini:
--------------------------------------------------------------------------------
 1 | ; PlatformIO Project Configuration File
 2 | ;
 3 | ;   Build options: build flags, source filter
 4 | ;   Upload options: custom upload port, speed and extra flags
 5 | ;   Library options: dependencies, extra library storages
 6 | ;   Advanced options: extra scripting
 7 | ;
 8 | ; Please visit documentation for the other options and examples
 9 | ; https://docs.platformio.org/page/projectconf.html
10 | 
11 | [env:teensy35]
12 | platform = teensy
13 | board = teensy35
14 | framework = arduino
15 | 


--------------------------------------------------------------------------------
/Arda/dshot_simple_api/src/dshot_api.cpp:
--------------------------------------------------------------------------------
  1 | #include <Arduino.h>
  2 | #include "dshot_api.h"
  3 | 
  4 | DMAChannel dma;
  5 | volatile uint16_t dma_source[DMA_LENGTH];
  6 | volatile uint16_t dataToSend;
  7 | 
  8 | 
  9 | volatile bool flagTlm = false;
 10 | volatile bool requestTlm = false;
 11 | volatile uint16_t throttle;
 12 | 
 13 | volatile uint8_t mode = DEFAULT_MODE;
 14 | 
 15 | volatile long counter = 0;
 16 | volatile bool armed = false;
 17 | volatile bool commandSent = false;
 18 | 
 19 | IntervalTimer timer;
 20 | 
 21 | struct TlmData tlmData;
 22 | 
 23 | void readTlm() {
 24 |   noInterrupts();
 25 |   if (isTlmAvailable()) {
 26 |     resetTlmFlag();
 27 |     interrupts();
 28 |     static uint8_t bufferTlm[TLM_LENGTH];
 29 |     while (Serial1.available() < TLM_LENGTH);
 30 |     // noInterrupts();
 31 |     for (int i = 0; i < TLM_LENGTH; i++) {
 32 |       bufferTlm[i] = Serial1.read();
 33 |     }
 34 |     tlmData.temperature = bufferTlm[0];
 35 |     tlmData.voltage = (bufferTlm[1] << 8) | bufferTlm[2];
 36 |     tlmData.current = (bufferTlm[3] << 8) | bufferTlm[4];
 37 |     tlmData.consumption = (bufferTlm[5] << 8) | bufferTlm[6];
 38 |     tlmData.rpm = (bufferTlm[7] << 8) | bufferTlm[8];
 39 |     tlmData.crcCheck = bufferTlm[9] == calculateCrc8(bufferTlm, TLM_LENGTH-1);
 40 | 
 41 |     /* TODO: stat for crcError */
 42 |     if (tlmData.crcCheck) {
 43 |       /* no error */
 44 |     } else {
 45 |       /* crc error */
 46 |     }
 47 |     /*for (int i = 0; i < TLM_LENGTH; i++) { // for debug
 48 |       Serial.print(bufferTlm[i]);
 49 |       Serial.print(" ");
 50 |     }
 51 |     Serial.println();*/
 52 |   } else {
 53 |     interrupts(); 
 54 |   }
 55 | }
 56 | 
 57 | uint16_t getRpm() {
 58 |   return tlmData.rpm;
 59 | }
 60 | 
 61 | void startDshot() {
 62 |   setupDMA();
 63 |   delay(100);
 64 |   disableTlm();
 65 | }
 66 | 
 67 | bool isTlmAvailable() {
 68 |   return flagTlm;
 69 | }
 70 | 
 71 | void resetTlmFlag() {
 72 |   flagTlm = false;
 73 | }
 74 | 
 75 | void armingMotor() {
 76 |   if (!armed) {
 77 |     mode = ARMING_MODE;
 78 |     throttle = 0;
 79 |     timer.begin(ISR_timer, 2000); // 2ms (500Hz)
 80 |     while (!armed);
 81 |     noInterrupts();
 82 |     mode = DEFAULT_MODE;
 83 |     interrupts();
 84 |   }
 85 | }
 86 | 
 87 | void enable3d() {
 88 |   uint16_t copyThrottle = throttle;
 89 |   noInterrupts();
 90 |   mode = SEND_COMMAND_MODE;
 91 |   throttle = 10;
 92 |   interrupts();
 93 |   while(!commandSent);
 94 |   noInterrupts();
 95 |   commandSent = false;
 96 |   throttle = 12;
 97 |   interrupts();
 98 |   while(!commandSent);
 99 |   noInterrupts();
100 |   commandSent = false;
101 |   mode = DEFAULT_MODE;
102 |   throttle = copyThrottle;
103 |   interrupts();
104 | }
105 | 
106 | void disable3d() {
107 |   uint16_t copyThrottle = throttle;
108 |   noInterrupts();
109 |   mode = SEND_COMMAND_MODE;
110 |   throttle = 9;
111 |   interrupts();
112 |   while(!commandSent);
113 |   noInterrupts();
114 |   commandSent = false;
115 |   throttle = 12;
116 |   interrupts();
117 |   while(!commandSent);
118 |   noInterrupts();
119 |   commandSent = false;
120 |   mode = DEFAULT_MODE;
121 |   throttle = copyThrottle;
122 |   interrupts();
123 | }
124 | 
125 | void setControlLoopFrequency(uint16_t freq) {
126 |   timer.update(1000000 / freq);
127 | }
128 | 
129 | void enableTlm() {
130 |   if (!requestTlm) {
131 |     noInterrupts();
132 |     requestTlm = true;
133 |     interrupts();
134 |   }
135 | }
136 | 
137 | void disableTlm() {
138 |   if (requestTlm) {
139 |     noInterrupts();
140 |     requestTlm = false;
141 |     interrupts();
142 |   }
143 | }
144 | 
145 | void setThrottle(uint16_t newThrottle) {
146 |   // maybe add saturation
147 |   if (armed) {
148 |     noInterrupts();
149 |     throttle = newThrottle;
150 |     requestTlm = true;
151 |     interrupts();
152 |   }
153 | }
154 | 
155 | void stopMotor() {
156 |   throttle = 0;
157 |   mode = STOP_MODE;
158 | }
159 | 
160 | 
161 | 
162 | 
163 | void sendDshot() {
164 |   dataToSend = uint16_t((throttle << 5)) | uint16_t((requestTlm << 4));
165 |   dataToSend |= ((dataToSend >> 4) ^ (dataToSend >> 8) ^ (dataToSend >> 12)) & 0x0f; // crc
166 |   for (int i = 0; i < DSHOT_LENGTH; i++) {
167 |     dma_source[i] = (dataToSend & (1 << (DSHOT_LENGTH - 1 - i))) ? LP : SP;
168 |   }
169 |   FTM0_SC = FTM_SC_CLKS(1);
170 |   if (requestTlm) {
171 |     flagTlm = true;
172 |   }
173 | }
174 | 
175 | // modify this function for control loop
176 | void ISR_timer() {
177 |   /* TODO: add PID control */
178 |   // throttle = ...;
179 | 
180 |   sendDshot();
181 |   if (mode == ARMING_MODE) {
182 |     if (!armed && ++counter > 10) {
183 |       armed = true;
184 |       counter = 0;
185 |     }
186 |   } else if (mode == SEND_COMMAND_MODE) {
187 |     if (!commandSent && counter <= 10) {
188 |       counter++;
189 |     }
190 |     if (!commandSent && counter > 10) {
191 |       commandSent = true;
192 |       counter = 0;
193 |     }
194 |   }
195 | }
196 | 
197 | void ISR_DMA() {
198 |   dma.clearInterrupt();
199 |   FTM0_SC = 0;
200 |   while (Serial1.available()) {
201 |     Serial1.read();
202 |   }
203 | }
204 | 
205 | void setupDMA() {
206 |   CORE_PIN9_CONFIG = PORT_PCR_MUX(4) | PORT_PCR_DSE | PORT_PCR_SRE;
207 | 
208 |   dma.sourceBuffer(dma_source, sizeof(dma_source));
209 |   dma.destination((uint16_t&) FTM0_C2V);
210 |   dma.triggerAtHardwareEvent(DMAMUX_SOURCE_FTM0_CH7);
211 |   dma.interruptAtCompletion();
212 |   dma.attachInterrupt(ISR_DMA);
213 |   dma.enable();
214 | 
215 |   FTM0_C2SC = FTM_CSC_MSB | FTM_CSC_ELSB; // edge-aligned pwm output
216 |   FTM0_C2V = 0;
217 |   FTM0_C7SC = FTM_CSC_CHIE | FTM_CSC_DMA | FTM_CSC_MSA | FTM_CSC_ELSA;
218 |   FTM0_C7V = 0;
219 | 
220 |   FTM0_SC = 0;
221 |   FTM0_CNT = 0;
222 |   FTM0_MOD = BIT_LENGTH;
223 |   FTM0_CNTIN = 0;
224 | }
225 | 
226 | uint8_t updateCrc8(uint8_t crc, uint8_t crc_seed) {
227 |   uint8_t crc_u = crc;
228 |   crc_u ^= crc_seed;
229 | 
230 |   for (int i = 0; i < 8; i++) {
231 |     crc_u = ( crc_u & 0x80 ) ? 0x7 ^ ( crc_u << 1 ) : ( crc_u << 1 );
232 |   }
233 | 
234 |   return (crc_u);
235 | }
236 | 
237 | uint8_t calculateCrc8(const uint8_t *Buf, const uint8_t BufLen) {
238 |   uint8_t crc = 0;
239 |   for (int i = 0; i < BufLen; i++) {
240 |     crc = updateCrc8(Buf[i], crc);
241 |   }
242 | 
243 |   return crc;
244 | }
245 | 


--------------------------------------------------------------------------------
/Arda/dshot_simple_api/src/main.cpp:
--------------------------------------------------------------------------------
 1 | #include <Arduino.h>
 2 | #include "dshot_api.h"
 3 | 
 4 | bool flagErr = false;
 5 | 
 6 | uint8_t bufferTlm[TLM_LENGTH];
 7 | 
 8 | void setup() {
 9 |   // configuring serial communications
10 |   Serial.begin(115200); // communicating over usb
11 |   Serial1.begin(115200); // communicating over UART1
12 |   while(!Serial); // waits for the usb communication to be established
13 |   delay(100);
14 |   Serial.println("start"); // for debug
15 | 
16 |   startDshot(); // sets the DMA up and disables telemetry for initialization
17 | 
18 |   armingMotor(); // arms the motor by sending 0 command at least 10 times
19 |   delay(500); // for debug - giving time to esc to say "ARMED"
20 | 
21 |   enable3d(); // enables the 3D flight mode
22 |   setControlLoopFrequency(DSHOT_FREQ); // sets the control loop frequency for rpm control
23 |   enableTlm(); // enables the telemetry
24 | }
25 | 
26 | void loop() {
27 |   if (Serial.available()) {
28 |     String sentThrottle = "";
29 |     char temp;
30 |     while (Serial.available()) {
31 |       temp = Serial.read();
32 |       if (temp != '\n') {
33 |         sentThrottle += temp;
34 |       }
35 |     }
36 |     setThrottle(sentThrottle.toInt()); // updates the throttle value
37 |   }
38 | 
39 |   readTlm();
40 | }
41 | 


--------------------------------------------------------------------------------
/ESCPID/AWPID.cpp:
--------------------------------------------------------------------------------
  1 | /*
  2 |  *  AWPID:    Anti-windup PID API
  3 |  *
  4 |  *  Note:     Best viewed using Arduino IDE with tab space = 2
  5 |  *
  6 |  *  Authors:  Jacques Gangloff
  7 |  *  Date:     May 2019
  8 |  */
  9 | 
 10 | #define AWPID_FILTERED_MES
 11 | 
 12 | // Includes
 13 | #include <Arduino.h>
 14 | #include <math.h>
 15 | #include "AWPID.h"
 16 | 
 17 | // Global variables
 18 | float   AWPID_Kp[AWPID_MAX_NB];       // Proportional gain
 19 | float   AWPID_Ki[AWPID_MAX_NB];       // Integral gain
 20 | float   AWPID_Kd[AWPID_MAX_NB];       // Derivative gain
 21 | float   AWPID_f[AWPID_MAX_NB];        // Pole of the derivative term lowpass filter
 22 | float   AWPID_Min[AWPID_MAX_NB];      // Lower saturation
 23 | float   AWPID_Max[AWPID_MAX_NB];      // Upper saturation
 24 | float   AWPID_u0[AWPID_MAX_NB];       // Control signal before saturation
 25 | float   AWPID_e0[AWPID_MAX_NB];       // Error signal
 26 | float   AWPID_e1[AWPID_MAX_NB];       // Last sample error signal
 27 | float   AWPID_ui0[AWPID_MAX_NB];      // Integral term
 28 | float   AWPID_ui1[AWPID_MAX_NB];      // Last sample integral term
 29 | float   AWPID_ud0[AWPID_MAX_NB];      // Derivative term
 30 | float   AWPID_ud1[AWPID_MAX_NB];      // Last sample derivative term
 31 | #ifdef AWPID_FILTERED_MES
 32 | float   AWPID_me1[AWPID_MAX_NB];      // Last sample measurement
 33 | uint8_t AWPID_minit[AWPID_MAX_NB];    // Init flag for mes history
 34 | #endif
 35 | uint8_t AWPID_n = 0;                  // Number of initialized PIDs
 36 | 
 37 | //
 38 | //  Initialisation of the PID parametrs
 39 | //
 40 | //  Nb:   number of controllers
 41 | //  Kp:   proportional gain
 42 | //  Ki:   integral gain
 43 | //  Kd:   derivative gain
 44 | //  f:    derivative filter pole
 45 | //        ( 0 < f < 1, if f == 1, derivative action is cancelled)
 46 | //  Sat:  saturation (>0)
 47 | //
 48 | //  Controller equation:
 49 | //
 50 | //  C(z) = Kp + Ki z / ( z - 1 ) + Kd( z - 1 ) / (z - f )
 51 | //
 52 | void AWPID_init(    uint8_t n,
 53 |                     float  *Kp,
 54 |                     float  *Ki,
 55 |                     float  *Kd,
 56 |                     float  *f,
 57 |                     float  *Min,
 58 |                     float  *Max
 59 |                      )  {
 60 | 
 61 |   int      i;
 62 | 
 63 |   if ( n <= AWPID_MAX_NB )
 64 |     AWPID_n = n;
 65 |   else
 66 |     AWPID_n = AWPID_MAX_NB;
 67 | 
 68 |   for ( i = 0; i < AWPID_n; i++ )  {
 69 | 
 70 |     // Definition of the PID tuning parameters
 71 |     AWPID_Kp[i] =   Kp[i];
 72 |     AWPID_Ki[i] =   Ki[i];
 73 |     AWPID_Kd[i] =   Kd[i];
 74 |     AWPID_f[i] =    f[i];
 75 |     AWPID_Min[i] =  Min[i];
 76 |     AWPID_Max[i] =  Max[i];
 77 | 
 78 |     // Initialisation of the PID internal variables
 79 |     AWPID_u0[i] =   0.0;
 80 | 
 81 |     AWPID_e0[i] =   0.0;
 82 |     AWPID_e1[i] =   0.0;
 83 | 
 84 |     AWPID_ui0[i] =  0.0;
 85 |     AWPID_ui1[i] =  0.0;
 86 | 
 87 |     AWPID_ud0[i] =  0.0;
 88 |     AWPID_ud1[i] =  0.0;
 89 |     
 90 |     #ifdef AWPID_FILTERED_MES
 91 |     AWPID_me1[i] =  0.0;
 92 |     AWPID_minit[i] = 0;
 93 |     #endif
 94 |     }
 95 | 
 96 |   return;
 97 |   }
 98 | 
 99 | //
100 | //  Reset internal variables of the PID
101 | //
102 | void AWPID_reset( void )  {
103 |   int i;
104 |   
105 |   for ( i = 0; i < AWPID_n; i++ )  {
106 |     // Initialisation of the PID internal variables
107 |     AWPID_u0[i] =   0.0;
108 | 
109 |     AWPID_e0[i] =   0.0;
110 |     AWPID_e1[i] =   0.0;
111 | 
112 |     AWPID_ui0[i] =  0.0;
113 |     AWPID_ui1[i] =  0.0;
114 | 
115 |     AWPID_ud0[i] =  0.0;
116 |     AWPID_ud1[i] =  0.0;
117 |     
118 |     #ifdef AWPID_FILTERED_MES
119 |     AWPID_me1[i] =  0.0;
120 |     AWPID_minit[i] = 0;
121 |     #endif
122 |   }
123 | }
124 | 
125 | //
126 | //  Computation of the ith control signal
127 | //
128 | void AWPID_control( uint8_t i,
129 |                     float Reference,
130 |                     float Measurement,
131 |                     float *Control )  {
132 | 
133 |   if ( i >= AWPID_n )
134 |     return;
135 | 
136 |   // Computation of the error
137 |   AWPID_e1[i] = AWPID_e0[i];
138 |   AWPID_e0[i] = Reference - Measurement;
139 | 
140 |   // Computation of the derivative term
141 |   AWPID_ud1[i] =  AWPID_ud0[i];
142 |   #ifdef AWPID_FILTERED_MES
143 |     if ( AWPID_minit[i] == 0 )  {
144 |       AWPID_me1[i] = Measurement;
145 |       AWPID_minit[i] = 1;
146 |     }
147 |     AWPID_ud0[i] =  AWPID_f[i] * AWPID_ud1[i] -
148 |                     AWPID_Kd[i] * ( Measurement - AWPID_me1[i] );
149 |     AWPID_me1[i] = Measurement;
150 |   #else
151 |   AWPID_ud0[i] =  AWPID_f[i] * AWPID_ud1[i] +
152 |                   AWPID_Kd[i] * ( AWPID_e0[i] - AWPID_e1[i] );
153 |   #endif
154 | 
155 |   // Integral term computation
156 | 
157 |   AWPID_ui1[i] =   AWPID_ui0[i];
158 | 
159 |   // Anti-windup only if the integral gain is non null
160 |   if ( AWPID_Ki[i] )  {
161 | 
162 |     AWPID_u0[i] = AWPID_Kp[i] * AWPID_e0[i] +
163 |                   AWPID_ud0[i] +
164 |                   AWPID_ui1[i] + AWPID_Ki[i] * AWPID_e0[i];
165 | 
166 |     // No saturation
167 |     if ( ( AWPID_u0[i] <= AWPID_Max[i] ) && ( AWPID_u0[i] >= AWPID_Min[i] ) )
168 |       AWPID_ui0[i] = AWPID_ui1[i] + AWPID_Kp[i] * AWPID_Ki[i] * AWPID_e0[i];
169 | 
170 |     // Upper limit saturation: recalculation of the integral term
171 |     // With this adjusment, the control signal equals exactly the saturation
172 |     if ( AWPID_u0[i] > AWPID_Max[i] )
173 |       AWPID_ui0[i] =  ( AWPID_Max[i] -
174 |                       ( AWPID_Kp[i] * AWPID_e0[i] + AWPID_ud0[i] ) );
175 | 
176 |     // Lower limit saturation: recalculation of the integral term
177 |     // With this adjusment, the control signal equals exactly the saturation
178 |     if ( AWPID_u0[i] < AWPID_Min[i] )
179 |       AWPID_ui0[i] =  ( AWPID_Min[i] -
180 |                       ( AWPID_Kp[i] * AWPID_e0[i] + AWPID_ud0[i] ) );
181 |     }
182 | 
183 |   // Control signal computation
184 |   *Control = AWPID_Kp[i] * AWPID_e0[i] + AWPID_ud0[i] + AWPID_ui0[i];
185 | 
186 |   // Saturation
187 |   if ( *Control > AWPID_Max[i] )
188 |     *Control = AWPID_Max[i];
189 | 
190 |   if ( *Control < AWPID_Min[i] )
191 |     *Control = AWPID_Min[i];
192 | 
193 |   return;
194 |   }
195 | 
196 | //
197 | // Tuning of the ith PID.
198 | // Can be called on-the-fly. 
199 | //
200 | void AWPID_tune(    uint8_t i,
201 |                     float  Kp,
202 |                     float  Ki,
203 |                     float  Kd,
204 |                     float  f,
205 |                     float  Min,
206 |                     float  Max
207 |                      )  {
208 |   if ( i >= AWPID_n )
209 |     return;
210 |   
211 |   // Definition of the PID tuning parameters
212 |   AWPID_Kp[i] =   Kp;
213 |   AWPID_Ki[i] =   Ki;
214 |   AWPID_Kd[i] =   Kd;
215 |   AWPID_f[i] =    f;
216 |   AWPID_Min[i] =  Min;
217 |   AWPID_Max[i] =  Max;
218 | 
219 | }                  
220 | 


--------------------------------------------------------------------------------
/ESCPID/AWPID.h:
--------------------------------------------------------------------------------
 1 | /*
 2 |  *  Definitions for AWPID.cpp
 3 |  *
 4 |  */
 5 | 
 6 | #ifndef __AWPID_H
 7 | #define __AWPID_H
 8 | 
 9 | // Defines
10 | #define AWPID_MAX_NB          6    // Maximum number of PIDs
11 | 
12 | // Function prototypes
13 | void AWPID_init( uint8_t, float*, float*, float*, float*, float*, float* );
14 | void AWPID_reset( void );
15 | void AWPID_control( uint8_t, float, float, float* );
16 | void AWPID_tune( uint8_t, float, float, float, float, float, float );
17 | 
18 | #endif
19 | 


--------------------------------------------------------------------------------
/ESCPID/DSHOT.cpp:
--------------------------------------------------------------------------------
  1 | /*
  2 |  *  DSHOT:    Generation of up to 6 DSHOT signals using DMA
  3 |  *
  4 |  *  Note:     Best viewed using Arduino IDE with tab space = 2
  5 |  *
  6 |  *  Authors:  Arda Yiğit and Jacques Gangloff
  7 |  *  Date:     May 2019
  8 |  */
  9 | 
 10 | // Includes
 11 | #include <Arduino.h>
 12 | #include "DMAChannel.h"
 13 | #include "DSHOT.h"
 14 | 
 15 | /*
 16 |  *  Constants
 17 |  */
 18 | 
 19 | #if defined(__IMXRT1062__) // teensy 4.0
 20 |   #define F_TMR F_BUS_ACTUAL
 21 | #else // teensy 3.5
 22 |   #define F_TMR F_BUS
 23 | #endif
 24 | 
 25 | /* Defining DSHOT600 timings expressed in F_TMR periods
 26 |  * DSHOT600 has the following timings:
 27 |  *
 28 |  *          1670ns
 29 |  *          --------->
 30 |  *          ______
 31 |  * 1 bit :  |     |___|
 32 |  *          1250ns
 33 |  *          ____
 34 |  * 0 bit :  |   |_____|
 35 |  *          625ns
 36 |  *
 37 |  * On the teensy 3.5, F_TMR == 60000000 (60MHz)
 38 |  * On the teensy 4.0, F_TMR == 600000000 (600Mhz)
 39 |  */
 40 | const uint16_t DSHOT_short_pulse  = uint64_t(F_TMR) * DSHOT_SP_DURATION / 1000000000;     // DSHOT short pulse duration (nb of F_BUS periods)
 41 | const uint16_t DSHOT_long_pulse   = uint64_t(F_TMR) * DSHOT_LP_DURATION / 1000000000;     // DSHOT long pulse duration (nb of F_BUS periods)
 42 | const uint16_t DSHOT_bit_length   = uint64_t(F_TMR) * DSHOT_BT_DURATION / 1000000000;     // DSHOT bit duration (nb of F_BUS periods)
 43 | 
 44 | /*
 45 |  *  Global variables
 46 |  */
 47 | 
 48 | // Number of initialized DSHOT outputs
 49 | uint8_t             DSHOT_n;
 50 | 
 51 | #if defined(__IMXRT1062__) // teensy 4.0
 52 | 
 53 | // DMA eFlexPWM modules
 54 | volatile IMXRT_FLEXPWM_t  *DSHOT_mods[DSHOT_NB_DMA_CHAN]  = { &IMXRT_FLEXPWM2, 
 55 |                                                               &IMXRT_FLEXPWM1,  
 56 |                                                               &IMXRT_FLEXPWM1,  
 57 |                                                               &IMXRT_FLEXPWM4,  
 58 |                                                               &IMXRT_FLEXPWM4, 
 59 |                                                               &IMXRT_FLEXPWM2
 60 |                                                             };
 61 | 
 62 | // DMA eFlexPWM submodules
 63 | volatile uint8_t          DSHOT_sm[DSHOT_NB_DMA_CHAN]     = { 0, 
 64 |                                                               3, 
 65 |                                                               2, 
 66 |                                                               0, 
 67 |                                                               1, 
 68 |                                                               2
 69 |                                                             };
 70 | 
 71 | // DMA eFlexPWM submodule PWM channel selector: A=0, B=1, X=2
 72 | volatile uint8_t  	      DSHOT_abx[DSHOT_NB_DMA_CHAN]    = { 0, 
 73 |                                                               0, 
 74 |                                                               2, 
 75 |                                                               0, 
 76 |                                                               0, 
 77 |                                                               1
 78 |                                                             };
 79 | 
 80 | // Output pins
 81 | volatile uint8_t          DSHOT_pin[DSHOT_NB_DMA_CHAN]    = { 4, 
 82 |                                                               8, 
 83 |                                                               24, 
 84 |                                                               22, 
 85 |                                                               23, 
 86 |                                                               9
 87 |                                                             };
 88 | 
 89 | // Output pin ALT mux
 90 | volatile uint8_t          DSHOT_pinmux[DSHOT_NB_DMA_CHAN] = { 1, 
 91 |                                                               6, 
 92 |                                                               4, 
 93 |                                                               1, 
 94 |                                                               1, 
 95 |                                                               2
 96 |                                                             };
 97 | 
 98 | // DMA source
 99 | volatile uint8_t          DSHOT_dmamux[DSHOT_NB_DMA_CHAN] = { DMAMUX_SOURCE_FLEXPWM2_WRITE0,
100 |                                                               DMAMUX_SOURCE_FLEXPWM1_WRITE3,
101 |                                                               DMAMUX_SOURCE_FLEXPWM1_WRITE2,
102 |                                                               DMAMUX_SOURCE_FLEXPWM4_WRITE0,
103 |                                                               DMAMUX_SOURCE_FLEXPWM4_WRITE1,
104 |                                                               DMAMUX_SOURCE_FLEXPWM2_WRITE2
105 |                                                             };
106 | 
107 | #else // teensy 3.5
108 | 
109 | // DMA FTM channel values references
110 | volatile uint32_t*  DSHOT_DMA_chan_teensy[DSHOT_NB_DMA_CHAN] ={   &FTM0_C0V,
111 |                                                                   &FTM0_C1V,
112 |                                                                   &FTM0_C4V,
113 |                                                                   &FTM0_C5V,
114 |                                                                   &FTM0_C6V,
115 |                                                                   &FTM0_C7V };
116 | 
117 | // DMA FTM channel status and control register
118 | volatile uint32_t*  DSHOT_DMA_chsc_teensy[DSHOT_NB_DMA_CHAN] ={   &FTM0_C0SC,
119 |                                                                   &FTM0_C1SC,
120 |                                                                   &FTM0_C4SC,
121 |                                                                   &FTM0_C5SC,
122 |                                                                   &FTM0_C6SC,
123 |                                                                   &FTM0_C7SC };
124 | 
125 | 
126 | // Output pins
127 | volatile uint32_t*  DSHOT_DMA_pin_teensy[DSHOT_NB_DMA_CHAN] ={    &CORE_PIN22_CONFIG,
128 |                                                                   &CORE_PIN23_CONFIG,
129 |                                                                   &CORE_PIN6_CONFIG,
130 |                                                                   &CORE_PIN20_CONFIG,
131 |                                                                   &CORE_PIN21_CONFIG,
132 |                                                                   &CORE_PIN5_CONFIG };
133 | 
134 | #endif
135 | 
136 | // DMA objects
137 | DMAChannel          dma[DSHOT_MAX_OUTPUTS];
138 | 
139 | // DMA data
140 | volatile uint16_t   DSHOT_dma_data[DSHOT_MAX_OUTPUTS][DSHOT_DMA_LENGTH];
141 | 
142 | #if defined(__IMXRT1062__) // teensy 4.0
143 | 
144 | /* 
145 |  * DMA termination interrupt service routine (ISR) for each DMA channel
146 |  */
147 | #define DSHOT_DMA_interrupt_routine( DSHOT_CHANNEL ) \
148 | void DSHOT_DMA_interrupt_routine_ ## DSHOT_CHANNEL( void ) { \
149 |   dma[DSHOT_CHANNEL].clearInterrupt( ); \
150 |   (*DSHOT_mods[DSHOT_CHANNEL]).MCTRL &= FLEXPWM_MCTRL_RUN( 1 << DSHOT_sm[DSHOT_CHANNEL] );  \
151 | }
152 | 
153 | DSHOT_DMA_interrupt_routine( 0 );
154 | DSHOT_DMA_interrupt_routine( 1 );
155 | DSHOT_DMA_interrupt_routine( 2 );
156 | DSHOT_DMA_interrupt_routine( 3 );
157 | DSHOT_DMA_interrupt_routine( 4 );
158 | DSHOT_DMA_interrupt_routine( 5 );
159 | 
160 | void (*DSHOT_DMA_ISR[6])()  = { DSHOT_DMA_interrupt_routine_0,
161 |                                 DSHOT_DMA_interrupt_routine_1,
162 |                                 DSHOT_DMA_interrupt_routine_2,
163 |                                 DSHOT_DMA_interrupt_routine_3,
164 |                                 DSHOT_DMA_interrupt_routine_4,
165 |                                 DSHOT_DMA_interrupt_routine_5
166 |                               };
167 | 
168 | #else // teensy 3.5
169 | 
170 | /*
171 |  *  DMA termination interrupt service routine (ISR)
172 |  */
173 | void DSHOT_DMA_interrupt_routine( void ) {
174 | 
175 |   dma[0].clearInterrupt( );
176 | 
177 |   // Disable FTM0
178 |   FTM0_SC = 0;
179 | }
180 | 
181 | #endif
182 | 
183 | /*
184 |  *  Initialize the DMA hardware in order to be able
185 |  *  to generate 6 DSHOT outputs.
186 |  */
187 | void DSHOT_init( int n ) {
188 |   int i, j;
189 | 
190 |   if ( n <= DSHOT_MAX_OUTPUTS )
191 |     DSHOT_n = n;
192 |   else
193 |     DSHOT_n = DSHOT_MAX_OUTPUTS;
194 | 
195 |   // Initialize DMA data
196 |   for ( i = 0; i < DSHOT_n; i++ ) {
197 |     for ( j = 0; j < DSHOT_DMA_LENGTH; j++ ) {
198 |       DSHOT_dma_data[i][j] = 0;
199 |     }
200 |   }
201 | 
202 | #if defined(__IMXRT1062__) // teensy 4.0
203 | 
204 |   // Configure pins on the board as DSHOT outputs
205 |   // These pins are configured as eFlexPWM (FLEXPWMn) PWM outputs
206 |   for ( i = 0; i < DSHOT_n; i++ ) {
207 |     *(portConfigRegister( DSHOT_pin[i] ))  = DSHOT_pinmux[i];
208 |   }
209 | 
210 |   // Configure eFlexPWM modules and submodules for PWM generation
211 |   // --- submodule specific registers ---
212 |   // INIT: initial counter value
213 |   // VAL0: PWM_X compare value
214 |   // VAL1: counter max value
215 |   // VAL2: must be 0 for edge-aligned PWM
216 |   // VAL3: PWM_A compare value
217 |   // VAL4: must be 0 for edge-aligned PWM
218 |   // VAL5: PWM_B compare value
219 |   // OCTRL: invert polarity of PWMq FLEXPWM_SMOCTRL_POLq
220 |   // DMAEN: FLEXPWM_SMDMAEN_VALDE to enable DMA
221 |   // --- module specific registers ---
222 |   // OUTEN: output enable for submodule n and PWM q FLEXPWM_OUTEN_PWMq_EN( 1 << n )
223 |   for ( i = 0; i < DSHOT_n; i++ ) {
224 |     (*DSHOT_mods[i]).SM[DSHOT_sm[i]].INIT = 0;
225 |     (*DSHOT_mods[i]).SM[DSHOT_sm[i]].VAL0 = 0;
226 |     (*DSHOT_mods[i]).SM[DSHOT_sm[i]].VAL1 = DSHOT_bit_length;
227 |     (*DSHOT_mods[i]).SM[DSHOT_sm[i]].VAL2 = 0;
228 |     (*DSHOT_mods[i]).SM[DSHOT_sm[i]].VAL3 = 0;
229 |     (*DSHOT_mods[i]).SM[DSHOT_sm[i]].VAL4 = 0;
230 |     (*DSHOT_mods[i]).SM[DSHOT_sm[i]].VAL5 = 0;
231 |     if ( DSHOT_abx[i] == 2 ) {
232 |       (*DSHOT_mods[i]).SM[DSHOT_sm[i]].OCTRL = FLEXPWM_SMOCTRL_POLX;
233 |       (*DSHOT_mods[i]).OUTEN |= FLEXPWM_OUTEN_PWMX_EN(1 << DSHOT_sm[i]);
234 |     } else if ( DSHOT_abx[i] == 1 ) {
235 |       (*DSHOT_mods[i]).OUTEN |= FLEXPWM_OUTEN_PWMB_EN(1 << DSHOT_sm[i]);
236 |     } else {
237 |       (*DSHOT_mods[i]).OUTEN |= FLEXPWM_OUTEN_PWMA_EN(1 << DSHOT_sm[i]);
238 |     }
239 |     (*DSHOT_mods[i]).SM[DSHOT_sm[i]].DMAEN = FLEXPWM_SMDMAEN_VALDE;
240 |   }
241 | 
242 |   // Each DMA channel is linked to a unique eFlexPWM submodule
243 |   // DMA channels are triggered by independant hardware events
244 |   for ( i = 0; i < DSHOT_n; i++ ) {
245 |     dma[i].sourceBuffer( DSHOT_dma_data[i], DSHOT_DMA_LENGTH * sizeof( uint16_t ) );
246 |     if ( DSHOT_abx[i] == 2 ) {
247 |       dma[i].destination( (uint16_t&) (*DSHOT_mods[i]).SM[DSHOT_sm[i]].VAL0 );
248 |     } else if ( DSHOT_abx[i] == 1 ) {
249 |       dma[i].destination( (uint16_t&) (*DSHOT_mods[i]).SM[DSHOT_sm[i]].VAL5 );
250 |     } else {
251 |       dma[i].destination( (uint16_t&) (*DSHOT_mods[i]).SM[DSHOT_sm[i]].VAL3 );
252 |     }
253 |     dma[i].triggerAtHardwareEvent( DSHOT_dmamux[i] );
254 |     dma[i].interruptAtCompletion( );
255 |     dma[i].attachInterrupt( DSHOT_DMA_ISR[i] );
256 |     dma[i].enable( );
257 |   }
258 | 
259 | #else
260 | 
261 |   // Configure pins on the board as DSHOT outputs
262 |   // These pins are configured as FlexTimer (FTM0) PWM outputs
263 |   // PORT_PCR_DSE: high current output
264 |   // PORT_PCR_SRE: slow slew rate
265 |   for ( i = 0; i < DSHOT_n; i++ ) {
266 |     *DSHOT_DMA_pin_teensy[i] = PORT_PCR_MUX(4) | PORT_PCR_DSE | PORT_PCR_SRE;
267 |   }
268 | 
269 |   // First DMA channel is the only one triggered by the bit clock
270 |   dma[0].sourceBuffer( DSHOT_dma_data[0], DSHOT_DMA_LENGTH * sizeof( uint16_t ) );
271 |   dma[0].destination( (uint16_t&) *DSHOT_DMA_chan_teensy[0] );
272 |   dma[0].triggerAtHardwareEvent( DMAMUX_SOURCE_FTM0_CH2 );
273 |   dma[0].interruptAtCompletion( );
274 |   dma[0].attachInterrupt( DSHOT_DMA_interrupt_routine );
275 |   dma[0].enable( );
276 | 
277 |   // Other DMA channels are trigered by the previoux DMA channel
278 |   for ( i = 1; i < DSHOT_n; i++ ) {
279 |     dma[i].sourceBuffer( DSHOT_dma_data[i], DSHOT_DMA_LENGTH * sizeof( uint16_t ) );
280 |     dma[i].destination( (uint16_t&) *DSHOT_DMA_chan_teensy[i] );
281 |     dma[i].triggerAtTransfersOf( dma[i-1] );
282 |     dma[i].triggerAtCompletionOf( dma[i-1] );
283 |     dma[i].enable( );
284 |   }
285 | 
286 |   // FTM0_CNSC: status and control register
287 |   // FTM_CSC_MSB | FTM_CSC_ELSB:
288 |   // edge aligned PWM with high-true pulses
289 |   for ( i = 0; i < DSHOT_n; i++ ) {
290 |     *DSHOT_DMA_chsc_teensy[i] = FTM_CSC_MSB | FTM_CSC_ELSB;
291 |   }
292 | 
293 |   // FTM0_CNV = 0: initialize the counter channel N at 0
294 |   for ( i = 0; i < DSHOT_n; i++ ) {
295 |     *DSHOT_DMA_chan_teensy[i] = 0;
296 |   }
297 | 
298 |   // FTM0 channel 2 is the main clock
299 |   // FTM_CSC_CHIE: enable interrupt
300 |   // FTM_CSC_DMA: enable DMA
301 |   // FTM_CSC_MSA: toggle output on match
302 |   // FTM0_C2V = 0: initialize the counter channel 2 at 0
303 |   FTM0_C2SC = FTM_CSC_CHIE | FTM_CSC_DMA | FTM_CSC_MSA | FTM_CSC_ELSA;
304 |   FTM0_C2V = 0;
305 | 
306 |   // Initialize FTM0
307 |   FTM0_SC = 0;                  // Disable FTM0
308 |   FTM0_CNT = 0;                 // Contains the FTM counter value
309 |   FTM0_MOD = DSHOT_bit_length;  // The modulo value for the FTM counter
310 |   FTM0_CNTIN = 0;               // Counter initial value
311 | 
312 | #endif
313 | 
314 | }
315 | 
316 | //
317 | //  Send the DSHOT signal through all the configured channels
318 | //  "cmd" points to the DSHOT_MAX_OUTPUTS DSHOT commands to send
319 | //  Telemetry is requested with "tlm", CRC bits are added
320 | //
321 | //  Returns an error code in case of failure, 0 otherwise:
322 | //
323 | int DSHOT_send( uint16_t *cmd, uint8_t *tlm ) {
324 |   int       i, j;
325 |   uint16_t  data;
326 | 
327 |   // Initialize DMA buffers
328 |   for ( i = 0; i < DSHOT_n; i++ ) {
329 | 
330 |     // Check cmd value
331 |     if ( cmd[i] > DSHOT_MAX_VALUE ) {
332 |       return DSHOT_ERROR_RANGE;
333 |     }
334 | 
335 |     // Compute the packet to send
336 |     // 11 first MSB = command
337 |     // 12th MSB = telemetry request
338 |     // 4 LSB = CRC
339 |     data = ( cmd[i] << 5 ) | ( tlm[i] << 4 );
340 |     data |= ( ( data >> 4 ) ^ ( data >> 8 ) ^ ( data >> 12 ) ) & 0x0f;
341 | 
342 |     // Generate DSHOT timings corresponding to the packet
343 |     for ( j = 0; j < DSHOT_DSHOT_LENGTH; j++ )  {
344 |       if ( data & ( 1 << ( DSHOT_DSHOT_LENGTH - 1 - j ) ) ) {
345 |         DSHOT_dma_data[i][j] = DSHOT_long_pulse;
346 |       } else {
347 |         DSHOT_dma_data[i][j] = DSHOT_short_pulse;
348 |       }
349 |     }
350 |   }
351 | 
352 |   // Clear error flag on all DMA channels
353 |   for ( i = 0; i < DSHOT_n; i++ ) {
354 |     dma[i].clearError( );
355 |   }
356 | 
357 | #if defined(__IMXRT1062__) // teensy 4.0
358 | 
359 |   // Start DMA by activating the clocks
360 |   // Clocks are disabled again by the DMA ISRs
361 |   IMXRT_FLEXPWM2.MCTRL |= FLEXPWM_MCTRL_RUN(15); 
362 |   IMXRT_FLEXPWM1.MCTRL |= FLEXPWM_MCTRL_RUN(15); 
363 |   IMXRT_FLEXPWM4.MCTRL |= FLEXPWM_MCTRL_RUN(15); 
364 |   /*
365 |   for ( i = 0; i < DSHOT_n; i++ ) {
366 |     (*DSHOT_mods[i]).MCTRL |= FLEXPWM_MCTRL_RUN( 1 << DSHOT_sm[i] ); 
367 |   }
368 | */
369 | #else
370 | 
371 |   // Start DMA by activating the clock
372 |   // The clock is disabled again by the DMA interrupt on channel 0
373 |   FTM0_SC = FTM_SC_CLKS(1);
374 | 
375 | #endif
376 | 
377 |   // Wait the theoretical time needed by DMA + some margin
378 |   delayMicroseconds( (unsigned int)( ( DSHOT_BT_DURATION * ( DSHOT_DMA_LENGTH + DSHOT_DMA_MARGIN ) ) / 1000 ) );
379 | 
380 | #if !defined(__IMXRT1062__) // teensy 3.5
381 | 
382 |   // Check if FMT0 was disabled by the DMA ISR
383 |   // Check only bits 3 and 4: non null if a clock source is set
384 |   // TODO: test this error code
385 |   if ( FTM0_SC & (3 << 3) ) {
386 |     return DSHOT_ERROR_TIMEOUT;
387 |   }
388 | 
389 | #endif
390 | 
391 |   // Check if there is a DMA error
392 |   // TODO: test this error code
393 |   for ( i = 0; i < DSHOT_n; i++ ) {
394 |     if ( dma[i].error( ) ) {
395 |       return DSHOT_ERROR_DMA;
396 |     }
397 |   }
398 | 
399 |   return 0;
400 | }
401 | 


--------------------------------------------------------------------------------
/ESCPID/DSHOT.h:
--------------------------------------------------------------------------------
 1 | /*
 2 |  *  Definitions for DSHOT.cpp
 3 |  *
 4 |  */
 5 | 
 6 | #ifndef __DSHOT_H
 7 | #define __DSHOT_H
 8 | 
 9 | // Defines
10 | #define DSHOT_MAX_OUTPUTS         6             // Maximum number of DSHOT outputs on teensy 3.5
11 | #define DSHOT_NB_DMA_CHAN         6             // Number of accessible DMA channels
12 | #if DSHOT_NB_DMA_CHAN < DSHOT_MAX_OUTPUTS
13 |   #error ESCCMD_NB_UART should be >= DSHOT_MAX_OUTPUTS
14 | #endif
15 | #define DSHOT_DMA_LENGTH          18            // Number of steps of one DMA sequence (the two last values are zero)
16 | #define DSHOT_DMA_MARGIN          2             // Number of additional bit duration to wait until checking if DMA is over
17 | #define DSHOT_DSHOT_LENGTH        16            // Number of bits in a DSHOT sequence
18 | #define DSHOT_BT_DURATION         1670          // Duration of 1 DSHOT600 bit in ns
19 | #define DSHOT_LP_DURATION         1250          // Duration of a DSHOT600 long pulse in ns
20 | #define DSHOT_SP_DURATION         625           // Duration of a DSHOT600 short pulse in ns
21 | #define DSHOT_MAX_VALUE           2047          // Maximum DSHOT value
22 | 
23 | #define DSHOT_ERROR_DMA           -1            // DMA error
24 | #define DSHOT_ERROR_TIMEOUT       -2            // Timeout : DMA duration is abnormally great
25 | #define DSHOT_ERROR_RANGE         -3            // Value out of range
26 | #define DSHOT_ERROR_INTERNAL      -4            // Internal error
27 | 
28 | // Function prototypes
29 | void DSHOT_init( int );
30 | int DSHOT_send( uint16_t*, uint8_t* );
31 | 
32 | #endif
33 | 


--------------------------------------------------------------------------------
/ESCPID/ESCCMD.cpp:
--------------------------------------------------------------------------------
   1 | /*
   2 |  *  ESCCMD:   ESC DSHOT command packets formating API
   3 |  *
   4 |  *  Note:     Best viewed using Arduino IDE with tab space = 2
   5 |  *
   6 |  *  Authors:  Arda Yiğit and Jacques Gangloff
   7 |  *  Date:     May 2019
   8 |  */
   9 | 
  10 | // Includes
  11 | #include <Arduino.h>
  12 | #include "DSHOT.h"
  13 | #include "ESCCMD.h"
  14 | 
  15 | // ESC emulation
  16 | //#define ESCCMD_ESC_EMULATION                              // Uncomment to activate ESC emulation
  17 | #define ESCCMD_ESC_EMU_PKT_LOSS                             // Uncomment to emulate packet loss
  18 | #define ESCCMD_ESC_FRACTION_PKTLOSS       300               // One out of x packets lost
  19 | 
  20 | // Error handling
  21 | #define ESCCMD_ERROR( code )              { ESCCMD_last_error[i] = code; return code; }
  22 | 
  23 | //
  24 | //  Global variables
  25 | //
  26 | volatile uint8_t    ESCCMD_n;                               // Number of initialized outputs
  27 | 
  28 | volatile uint8_t    ESCCMD_state[ESCCMD_MAX_ESC];           // Current state of the cmd subsystem
  29 | uint16_t            ESCCMD_CRC_errors[ESCCMD_MAX_ESC];      // Overall number of CRC error since start
  30 | int8_t              ESCCMD_last_error[ESCCMD_MAX_ESC];      // Last error code
  31 | uint16_t            ESCCMD_cmd[ESCCMD_MAX_ESC];             // Last command
  32 | uint16_t            ESCCMD_throttle_wd[ESCCMD_MAX_ESC];     // Throttle watchdog counter
  33 | uint8_t             ESCCMD_tlm_deg[ESCCMD_MAX_ESC];         // ESC temperature (°C)
  34 | uint16_t            ESCCMD_tlm_volt[ESCCMD_MAX_ESC];        // Voltage of the ESC power supply (0.01V)
  35 | uint16_t            ESCCMD_tlm_amp[ESCCMD_MAX_ESC];         // ESC current (0.01A)
  36 | uint16_t            ESCCMD_tlm_mah[ESCCMD_MAX_ESC];         // ESC consumption (mAh)
  37 | uint16_t            ESCCMD_tlm_rpm[ESCCMD_MAX_ESC];         // ESC electrical rpm (100rpm)
  38 | uint8_t             ESCCMD_tlm[ESCCMD_MAX_ESC];             // Set to 1 when asking for telemetry
  39 | uint8_t             ESCCMD_tlm_pend[ESCCMD_MAX_ESC];        // Flag indicating a pending telemetry data request
  40 | uint8_t             ESCCMD_tlm_valid[ESCCMD_MAX_ESC];       // Flag indicating the validity of telemetry data
  41 | uint8_t             ESCCMD_tlm_lost_cnt[ESCCMD_MAX_ESC];    // Lost packet counter of telemetry data
  42 | uint64_t            ESCCMD_tic_counter = 0;                 // Counts the number of clock iterations
  43 | 
  44 | volatile uint16_t   ESCCMD_tic_pend = 0;                    // Number of timer tic waiting for ackowledgement
  45 | volatile uint8_t    ESCCMD_init_flag = 0;                   // Subsystem initialization flag
  46 | volatile uint8_t    ESCCMD_timer_flag = 0;                  // Periodic loop enable/disable flag
  47 | 
  48 | IntervalTimer       ESCCMD_timer;                           // Timer object
  49 | HardwareSerial*     ESCCMD_serial[ESCCMD_NB_UART] = {       // Array of Serial objects
  50 |                                                 &Serial1,
  51 |                                                 &Serial2,
  52 |                                                 &Serial3,
  53 |                                                 &Serial4,
  54 |                                                 &Serial5,
  55 |                                                 &Serial6 };
  56 | uint8_t             ESCCMD_bufferTlm[ESCCMD_NB_UART][ESCCMD_TLM_LENGTH];
  57 | 
  58 | #ifdef ESCCMD_ESC_EMULATION
  59 | #define             ESCCMD_EMU_TLM_MAX      5               // Max number of telemetry packets
  60 | #define             ESCCMD_EMU_TLM_DEG      25              // Nominal temperature (deg)
  61 | #define             ESCCMD_EMU_TLM_VOLT     1200            // Nominal voltage (0.01V)
  62 | #define             ESCCMD_EMU_TLM_AMP      2500            // Nominal current (0.01A)
  63 | #define             ESCCMD_EMU_TLM_MAH      1000            // Nominal consumption (mAh)
  64 | #define             ESCCMD_EMU_TLM_RPM      10              // Nominal electrical rpm (100rpm)
  65 | #define             ESCCMD_EMU_TLM_NOISE    3.0             // Percentge of emulated noise (%)
  66 | #define             ESCCMD_EMU_MOTOR_KV     2600            // Kv of the emulated motor
  67 | 
  68 | uint8_t             ESCCMD_tlm_emu_nb[ESCCMD_MAX_ESC] = {}; // Number of available packets
  69 | 
  70 | uint8_t             ESCCMD_tlm_emu_deg[ESCCMD_EMU_TLM_MAX][ESCCMD_MAX_ESC];
  71 | uint16_t            ESCCMD_tlm_emu_volt[ESCCMD_EMU_TLM_MAX][ESCCMD_MAX_ESC];
  72 | uint16_t            ESCCMD_tlm_emu_amp[ESCCMD_EMU_TLM_MAX][ESCCMD_MAX_ESC];
  73 | uint16_t            ESCCMD_tlm_emu_mah[ESCCMD_EMU_TLM_MAX][ESCCMD_MAX_ESC];
  74 | uint16_t            ESCCMD_tlm_emu_rpm[ESCCMD_EMU_TLM_MAX][ESCCMD_MAX_ESC];
  75 | #endif
  76 | 
  77 | 
  78 | //
  79 | //  Initialization
  80 | //
  81 | void ESCCMD_init( uint8_t n )  {
  82 |   static int i;
  83 | 
  84 |   if ( ESCCMD_init_flag )
  85 |     return;
  86 | 
  87 |   if ( n <= ESCCMD_MAX_ESC )
  88 |     ESCCMD_n = n;
  89 |   else
  90 |     ESCCMD_n = ESCCMD_MAX_ESC;
  91 | 
  92 |   // Initialize data arrays to zero
  93 |   for ( i = 0; i < ESCCMD_n; i++ ) {
  94 |     ESCCMD_state[i]       = 0;
  95 |     ESCCMD_CRC_errors[i]  = 0;
  96 |     ESCCMD_last_error[i]  = 0;
  97 |     ESCCMD_cmd[i]         = 0;
  98 |     ESCCMD_throttle_wd[i] = 0;
  99 |     ESCCMD_tlm_deg[i]     = 0;
 100 |     ESCCMD_tlm_volt[i]    = 0;
 101 |     ESCCMD_tlm_amp[i]     = 0;
 102 |     ESCCMD_tlm_mah[i]     = 0;
 103 |     ESCCMD_tlm_rpm[i]     = 0;
 104 |     ESCCMD_tlm[i]         = 0;
 105 |     ESCCMD_tlm_pend[i]    = 0;
 106 |     ESCCMD_tlm_valid[i]   = 0;
 107 |     ESCCMD_tlm_lost_cnt[i]= 0;
 108 |   }
 109 | 
 110 |   // Initialize telemetry UART channels
 111 |   for ( i = 0; i < ESCCMD_n; i++ )  {
 112 |     ESCCMD_serial[i]->begin( ESCCMD_TLM_UART_SPEED );
 113 |   }
 114 | 
 115 |   // Initialize DSHOT generation subsystem
 116 |   DSHOT_init( ESCCMD_n );
 117 | 
 118 |   // Set the initialization flag
 119 |   ESCCMD_init_flag = 1;
 120 | }
 121 | 
 122 | //
 123 | //  Arm all ESCs
 124 | //
 125 | //  Return values: see defines
 126 | //
 127 | int ESCCMD_arm_all( void )  {
 128 |   static int i, k;
 129 | 
 130 |   // Check if everything is initialized
 131 |   if ( !ESCCMD_init_flag )
 132 |     return ESCCMD_ERROR_INIT;
 133 | 
 134 |   // Check if all the ESCs are in the initial state
 135 |   for ( i = 0; i < ESCCMD_n; i++ )
 136 |     if ( ESCCMD_state[i] & ESCCMD_STATE_ARMED )
 137 |       ESCCMD_ERROR( ESCCMD_ERROR_SEQ )
 138 | 
 139 |   // Define stop command
 140 |   for ( i = 0; i < ESCCMD_n; i++ )  {
 141 |     ESCCMD_cmd[i] = DSHOT_CMD_MOTOR_STOP;
 142 |     ESCCMD_tlm[i] = 0;
 143 |   }
 144 | 
 145 |   // Send command ESCCMD_CMD_ARMING_REP times
 146 |   for ( i = 0; i < ESCCMD_CMD_ARMING_REP; i++ )  {
 147 | 
 148 |     // Send DSHOT signal to all ESCs
 149 |     if ( DSHOT_send( ESCCMD_cmd, ESCCMD_tlm ) ) {
 150 |       for ( k = 0; k < ESCCMD_n; k++ )
 151 |         ESCCMD_last_error[k] = ESCCMD_ERROR_DSHOT;
 152 |       return ESCCMD_ERROR_DSHOT;
 153 |     }
 154 | 
 155 |     // Wait some time
 156 |     delayMicroseconds( 2 * ESCCMD_CMD_DELAY );
 157 |   }
 158 | 
 159 |   // Set the arming flag
 160 |   for ( i = 0; i < ESCCMD_n; i++ )
 161 |     ESCCMD_state[i] |= ESCCMD_STATE_ARMED;
 162 | 
 163 |   return 0;
 164 | }
 165 | 
 166 | //
 167 | //  Activate 3D mode
 168 | //
 169 | //  Return values: see defines
 170 | //
 171 | int ESCCMD_3D_on( void )  {
 172 |   static int i, k;
 173 | 
 174 |   // Check if everything is initialized
 175 |   if ( !ESCCMD_init_flag )
 176 |     return ESCCMD_ERROR_INIT;
 177 |   
 178 |   // Check if timer is disabled
 179 |   if ( ESCCMD_timer_flag )
 180 |     return ESCCMD_ERROR_PARAM;
 181 | 
 182 |   for ( i = 0; i < ESCCMD_n; i++ )  {
 183 |     // Check if ESCs are stopped
 184 |     if ( ESCCMD_state[i] & ESCCMD_STATE_START )
 185 |       ESCCMD_ERROR( ESCCMD_ERROR_SEQ )
 186 |   }
 187 | 
 188 |   // Define 3D on command
 189 |   for ( i = 0; i < ESCCMD_n; i++ )  {
 190 |     ESCCMD_cmd[i] = DSHOT_CMD_3D_MODE_ON;
 191 |     ESCCMD_tlm[i] = 1;
 192 |   }
 193 | 
 194 |   // Send command ESCCMD_CMD_REPETITION times
 195 |   for ( i = 0; i < ESCCMD_CMD_REPETITION; i++ )  {
 196 | 
 197 |     // Send DSHOT signal to all ESCs
 198 |     if ( DSHOT_send( ESCCMD_cmd, ESCCMD_tlm ) ) {
 199 |       for ( k = 0; k < ESCCMD_n; k++ )
 200 |         ESCCMD_last_error[k] = ESCCMD_ERROR_DSHOT;
 201 |       return ESCCMD_ERROR_DSHOT;
 202 |     }
 203 | 
 204 |     // Wait some time
 205 |     delayMicroseconds( ESCCMD_CMD_DELAY );
 206 |   }
 207 | 
 208 |   // Define save settings command
 209 |   for ( i = 0; i < ESCCMD_n; i++ )  {
 210 |     ESCCMD_cmd[i] = DSHOT_CMD_SAVE_SETTINGS;
 211 |     ESCCMD_tlm[i] = 1;
 212 |   }
 213 | 
 214 |   // Send command ESCCMD_CMD_REPETITION times
 215 |   for ( i = 0; i < ESCCMD_CMD_REPETITION; i++ )  {
 216 | 
 217 |     // Send DSHOT signal to all ESCs
 218 |     if ( DSHOT_send( ESCCMD_cmd, ESCCMD_tlm ) ) {
 219 |       for ( k = 0; k < ESCCMD_n; k++ )
 220 |         ESCCMD_last_error[k] = ESCCMD_ERROR_DSHOT;
 221 |       return ESCCMD_ERROR_DSHOT;
 222 |     }
 223 | 
 224 |     // Wait some time
 225 |     delayMicroseconds( ESCCMD_CMD_DELAY );
 226 |   }
 227 | 
 228 |   // Set the 3D mode flag
 229 |   for ( i = 0; i < ESCCMD_n; i++ )
 230 |     ESCCMD_state[i] |= ESCCMD_STATE_3D;
 231 | 
 232 |   // Minimum delay before next command
 233 |   delayMicroseconds( ESCCMD_CMD_SAVE_DELAY );
 234 | 
 235 |   // Flush incoming serial buffers due to a transcient voltage
 236 |   // appearing on Tx when saving, generating a 0xff serial byte
 237 |   for ( i = 0; i < ESCCMD_n; i++ )
 238 |     ESCCMD_serial[i]->clear( );
 239 | 
 240 |   // ESC is disarmed after previous delay
 241 |   for ( i = 0; i < ESCCMD_n; i++ )
 242 |     ESCCMD_state[i] &= ~(ESCCMD_STATE_ARMED);
 243 | 
 244 |   return 0;
 245 | }
 246 | 
 247 | //
 248 | //  Deactivate 3D mode
 249 | //
 250 | //  Return values: see defines
 251 | //
 252 | int ESCCMD_3D_off( void )  {
 253 |   static int i, k;
 254 | 
 255 |   // Check if everything is initialized
 256 |   if ( !ESCCMD_init_flag )
 257 |     return ESCCMD_ERROR_INIT;
 258 |   
 259 |   // Check if timer is disabled
 260 |   if ( ESCCMD_timer_flag )
 261 |     return ESCCMD_ERROR_PARAM;
 262 | 
 263 |   for ( i = 0; i < ESCCMD_n; i++ )  {
 264 |     // Check if ESCs are stopped
 265 |     if ( ESCCMD_state[i] & ESCCMD_STATE_START )
 266 |       ESCCMD_ERROR( ESCCMD_ERROR_SEQ )
 267 |   }
 268 | 
 269 |   // Define 3D off command
 270 |   for ( i = 0; i < ESCCMD_n; i++ )  {
 271 |     ESCCMD_cmd[i] = DSHOT_CMD_3D_MODE_OFF;
 272 |     ESCCMD_tlm[i] = 1;
 273 |   }
 274 | 
 275 |   // Send command ESCCMD_CMD_REPETITION times
 276 |   for ( i = 0; i < ESCCMD_CMD_REPETITION; i++ )  {
 277 | 
 278 |     // Send DSHOT signal to all ESCs
 279 |     if ( DSHOT_send( ESCCMD_cmd, ESCCMD_tlm ) ) {
 280 |       for ( k = 0; k < ESCCMD_n; k++ )
 281 |         ESCCMD_last_error[k] = ESCCMD_ERROR_DSHOT;
 282 |       return ESCCMD_ERROR_DSHOT;
 283 |     }
 284 | 
 285 |     // Wait some time
 286 |     delayMicroseconds( ESCCMD_CMD_DELAY );
 287 |   }
 288 | 
 289 |   // Define save settings command
 290 |   for ( i = 0; i < ESCCMD_n; i++ )  {
 291 |     ESCCMD_cmd[i] = DSHOT_CMD_SAVE_SETTINGS;
 292 |     ESCCMD_tlm[i] = 1;
 293 |   }
 294 | 
 295 |   // Send command ESCCMD_CMD_REPETITION times
 296 |   for ( i = 0; i < ESCCMD_CMD_REPETITION; i++ )  {
 297 | 
 298 |     // Send DSHOT signal to all ESCs
 299 |     if ( DSHOT_send( ESCCMD_cmd, ESCCMD_tlm ) ) {
 300 |       for ( k = 0; k < ESCCMD_n; k++ )
 301 |         ESCCMD_last_error[k] = ESCCMD_ERROR_DSHOT;
 302 |       return ESCCMD_ERROR_DSHOT;
 303 |     }
 304 | 
 305 |     // Wait some time
 306 |     delayMicroseconds( ESCCMD_CMD_DELAY );
 307 |   }
 308 | 
 309 |   // Clear the 3D mode flag
 310 |   for ( i = 0; i < ESCCMD_n; i++ )
 311 |     ESCCMD_state[i] &= ~(ESCCMD_STATE_3D);
 312 | 
 313 |   // Minimum delay before next command
 314 |   delayMicroseconds( ESCCMD_CMD_SAVE_DELAY );
 315 | 
 316 |   // Flush incoming serial buffers due to a transcient voltage
 317 |   // appearing on Tx when saving, generating a 0xff serial byte
 318 |   for ( i = 0; i < ESCCMD_n; i++ )
 319 |     ESCCMD_serial[i]->clear( );
 320 | 
 321 |   // ESC is disarmed after previous delay
 322 |   for ( i = 0; i < ESCCMD_n; i++ )
 323 |     ESCCMD_state[i] &= ~(ESCCMD_STATE_ARMED);
 324 | 
 325 |   return 0;
 326 | }
 327 | 
 328 | //
 329 | //  Start periodic loop. ESC should be armed.
 330 | //
 331 | //  Return values: see defines
 332 | //
 333 | int ESCCMD_start_timer( void )  {
 334 |   static int i;
 335 | 
 336 |   // Check if everything is initialized
 337 |   if ( !ESCCMD_init_flag )
 338 |     return ESCCMD_ERROR_INIT;
 339 | 
 340 |   // Check if timer already started
 341 |   if ( ESCCMD_timer_flag )
 342 |     return ESCCMD_ERROR_SEQ;
 343 | 
 344 |   // Checks
 345 |   for ( i = 0; i < ESCCMD_n; i++ )  {
 346 |     // Check if all the ESCs are armed
 347 |     if ( !( ESCCMD_state[i] & ESCCMD_STATE_ARMED ) )
 348 |       ESCCMD_ERROR( ESCCMD_ERROR_SEQ )
 349 | 
 350 |     // Check if ESCs are stopped
 351 |     if ( ESCCMD_state[i] & ESCCMD_STATE_START )
 352 |       ESCCMD_ERROR( ESCCMD_ERROR_SEQ )
 353 |   }
 354 | 
 355 |   // Initialize ESC structure and clear UART input buffer
 356 |   for ( i = 0; i < ESCCMD_n; i++ )  {
 357 |     ESCCMD_cmd[i] = DSHOT_CMD_MOTOR_STOP;
 358 |     ESCCMD_tlm[i] = 0;
 359 |     ESCCMD_tlm_pend[i] = 0;
 360 |     ESCCMD_tlm_lost_cnt[i] = 0;
 361 |     ESCCMD_CRC_errors[i] = 0;
 362 |     ESCCMD_last_error[i]  = 0;
 363 |     ESCCMD_throttle_wd[i] = ESCCMD_THWD_LEVEL;
 364 |     ESCCMD_serial[i]->clear( );
 365 |   }
 366 | 
 367 |   ESCCMD_tic_pend = 0;
 368 | 
 369 |   // Initialize timer
 370 |   ESCCMD_timer.begin( ESCCMD_ISR_timer, ESCCMD_TIMER_PERIOD );
 371 |   
 372 |   // Raise the timer flag
 373 |   ESCCMD_timer_flag = 1;
 374 | 
 375 |   return 0;
 376 | }
 377 | 
 378 | //
 379 | //  Stop periodic loop. ESC should be armed.
 380 | //
 381 | //  Return values: see defines
 382 | //
 383 | int ESCCMD_stop_timer( void )  {
 384 |   static int i;
 385 | 
 386 |   // Check if everything is initialized
 387 |   if ( !ESCCMD_init_flag )
 388 |     return ESCCMD_ERROR_INIT;
 389 | 
 390 |   // Check if timer started
 391 |   if ( !ESCCMD_timer_flag )
 392 |     return ESCCMD_ERROR_SEQ;
 393 | 
 394 |   // Stop timer
 395 |   ESCCMD_timer.end();
 396 |   ESCCMD_timer_flag = 0;
 397 | 
 398 |   // Update ESC state
 399 |   for ( i = 0; i < ESCCMD_n; i++ )  {
 400 |     ESCCMD_cmd[i] = DSHOT_CMD_MOTOR_STOP;
 401 |     ESCCMD_tlm[i] = 0;
 402 |     ESCCMD_state[i] &= ~( ESCCMD_STATE_ARMED | ESCCMD_STATE_START );
 403 |   }
 404 | 
 405 |   return 0;
 406 | }
 407 | 
 408 | //
 409 | //  Define throttle of ESC number i:
 410 | //    Default mode: 0 -> 1999
 411 | //    3D mode     : -999 -> 999
 412 | //
 413 | int ESCCMD_throttle( uint8_t i, int16_t throttle ) {
 414 |   static uint8_t local_state;
 415 | 
 416 |   // Check if everything is initialized
 417 |   if ( !ESCCMD_init_flag )
 418 |     return ESCCMD_ERROR_INIT;
 419 | 
 420 |   // Check if motor is within range
 421 |   if ( i >= ESCCMD_n )
 422 |     return ESCCMD_ERROR_PARAM;
 423 |   
 424 |   // Check if timer started
 425 |   if ( !ESCCMD_timer_flag )
 426 |     ESCCMD_ERROR( ESCCMD_ERROR_SEQ );
 427 | 
 428 |   // Define a local copy of the state
 429 |   noInterrupts();
 430 |   local_state = ESCCMD_state[i];
 431 |   interrupts();
 432 | 
 433 |   // Check if ESC is armed
 434 |   if ( !( local_state & ESCCMD_STATE_ARMED ) )
 435 |     ESCCMD_ERROR( ESCCMD_ERROR_SEQ )
 436 | 
 437 |   // Define throttle depending on the mode
 438 |   if ( local_state & ESCCMD_STATE_3D )  {
 439 |     // Check limits
 440 |     if ( ( throttle < ESCCMD_MIN_3D_THROTTLE ) || ( throttle > ESCCMD_MAX_3D_THROTTLE ))
 441 |       ESCCMD_ERROR( ESCCMD_ERROR_PARAM )
 442 | 
 443 |     // 3D mode
 444 |     // 48 - 1047    : positive direction (48 slowest)
 445 |     // 1048 - 2047  : negative direction (1048 slowest)
 446 |     if ( throttle >= 0 )
 447 |       ESCCMD_cmd[i] = DSHOT_CMD_MAX + 1 + throttle;
 448 |     else
 449 |       ESCCMD_cmd[i] = DSHOT_CMD_MAX + 1 + ESCCMD_MAX_3D_THROTTLE - throttle;
 450 |   }
 451 |   else {
 452 | 
 453 |     // Check limits
 454 |     if ( ( throttle < 0 ) || ( throttle > ESCCMD_MAX_THROTTLE ))
 455 |       ESCCMD_ERROR( ESCCMD_ERROR_PARAM )
 456 | 
 457 |     // Default mode
 458 |     ESCCMD_cmd[i] = DSHOT_CMD_MAX + 1 + throttle;
 459 |   }
 460 | 
 461 |   // Switch start mode on only if needed
 462 |   if ( !( local_state & ESCCMD_STATE_START ) ) {
 463 |     noInterrupts();
 464 |     ESCCMD_state[i] |= ESCCMD_STATE_START;
 465 |     interrupts();
 466 |     ESCCMD_tlm[i] = 1;
 467 |   }
 468 | 
 469 |   // Reset the throttle watchdog
 470 |   if ( ESCCMD_throttle_wd[i] >= ESCCMD_THWD_LEVEL ) {
 471 |     // If watchdog was previously triggered:
 472 |     //  Clear UART input buffer
 473 |     //  Also clear pending errors, pending packets...
 474 |     ESCCMD_serial[i]->clear( );
 475 |     ESCCMD_tlm_pend[i] = 0;
 476 |     ESCCMD_tlm_lost_cnt[i] = 0;
 477 |     ESCCMD_CRC_errors[i] = 0;
 478 |     ESCCMD_last_error[i]  = 0;
 479 |   }
 480 |   ESCCMD_throttle_wd[i] = 0;
 481 | 
 482 |   return 0;
 483 | }
 484 | 
 485 | //
 486 | //  Stop motor number i
 487 | //
 488 | int ESCCMD_stop( uint8_t i ) {
 489 |   static uint8_t local_state;
 490 | 
 491 |   // Check if everything is initialized
 492 |   if ( !ESCCMD_init_flag )
 493 |     return ESCCMD_ERROR_INIT;
 494 | 
 495 |   // Check if motor is within range
 496 |   if ( i >= ESCCMD_n )
 497 |     return ESCCMD_ERROR_PARAM;
 498 |   
 499 |   // Check if timer started
 500 |   if ( !ESCCMD_timer_flag )
 501 |     ESCCMD_ERROR( ESCCMD_ERROR_SEQ );
 502 | 
 503 |   // Define a local copy of the state
 504 |   noInterrupts();
 505 |   local_state = ESCCMD_state[i];
 506 |   interrupts();
 507 | 
 508 |   // Check if ESC is armed
 509 |   if ( !( local_state & ESCCMD_STATE_ARMED ) )
 510 |     ESCCMD_ERROR( ESCCMD_ERROR_SEQ )
 511 | 
 512 |   // Set command to stop
 513 |   ESCCMD_cmd[i] = DSHOT_CMD_MOTOR_STOP;
 514 | 
 515 |   // Switch start mode off only if needed
 516 |   if ( local_state & ESCCMD_STATE_START  ) {
 517 |     noInterrupts();
 518 |     ESCCMD_state[i] &= ~ESCCMD_STATE_START;
 519 |     interrupts();
 520 |     ESCCMD_tlm[i] = 0;
 521 |   }
 522 | 
 523 |   return 0;
 524 | }
 525 | 
 526 | //
 527 | //  Return last error code of motor number i
 528 | //
 529 | int ESCCMD_read_err( uint8_t i, int8_t *err )  {
 530 | 
 531 |   // Check if everything is initialized
 532 |   if ( !ESCCMD_init_flag )
 533 |     return ESCCMD_ERROR_INIT;
 534 | 
 535 |   // Check if motor is within range
 536 |   if ( i >= ESCCMD_n )
 537 |     return ESCCMD_ERROR_PARAM;
 538 | 
 539 |   // Return error code
 540 |   *err = ESCCMD_last_error[i];
 541 |   
 542 |   // Clear error
 543 |   ESCCMD_last_error[i] = 0;
 544 | 
 545 |   return 0;
 546 | }
 547 | 
 548 | //
 549 | //  Return last command code of motor number i
 550 | //
 551 | int ESCCMD_read_cmd( uint8_t i, uint16_t *cmd )  {
 552 | 
 553 |   // Check if everything is initialized
 554 |   if ( !ESCCMD_init_flag )
 555 |     return ESCCMD_ERROR_INIT;
 556 | 
 557 |   // Check if motor is within range
 558 |   if ( i >= ESCCMD_n )
 559 |     return ESCCMD_ERROR_PARAM;
 560 | 
 561 | 
 562 |   *cmd = ESCCMD_cmd[i];
 563 | 
 564 |   return 0;
 565 | }
 566 | 
 567 | //
 568 | //  Read telemetry status of ESC number i
 569 | //
 570 | int ESCCMD_read_tlm_status( uint8_t i )  {
 571 |   static uint8_t local_state;
 572 | 
 573 |   // Check if everything is initialized
 574 |   if ( !ESCCMD_init_flag )
 575 |     return ESCCMD_ERROR_INIT;
 576 | 
 577 |   // Check if motor is within range
 578 |   if ( i >= ESCCMD_n )
 579 |     return ESCCMD_ERROR_PARAM;
 580 |   
 581 |   // Check if timer started
 582 |   if ( !ESCCMD_timer_flag )
 583 |     ESCCMD_ERROR( ESCCMD_ERROR_SEQ );
 584 | 
 585 |   // Define a local copy of the state
 586 |   noInterrupts();
 587 |   local_state = ESCCMD_state[i];
 588 |   interrupts();
 589 | 
 590 |   // Check if ESC is armed
 591 |   if ( !( local_state & ESCCMD_STATE_ARMED ) )
 592 |     ESCCMD_ERROR( ESCCMD_ERROR_SEQ )
 593 | 
 594 |   // Check if telemetry is valid and active
 595 |   if ( ESCCMD_tlm_valid[i] && ESCCMD_tlm[i] )  {
 596 |     return 0;
 597 |   }
 598 |   else
 599 |     return ESCCMD_ERROR_TLM_INVAL;
 600 | }
 601 | 
 602 | //
 603 | //  Read temperature of motor number i
 604 | //  Unit is degree Celsius
 605 | //
 606 | int ESCCMD_read_deg( uint8_t i, uint8_t *deg )  {
 607 |   static uint8_t local_state;
 608 | 
 609 |   // Check if everything is initialized
 610 |   if ( !ESCCMD_init_flag )
 611 |     return ESCCMD_ERROR_INIT;
 612 | 
 613 |   // Check if motor is within range
 614 |   if ( i >= ESCCMD_n )
 615 |     return ESCCMD_ERROR_PARAM;
 616 |   
 617 |   // Check if timer started
 618 |   if ( !ESCCMD_timer_flag )
 619 |     ESCCMD_ERROR( ESCCMD_ERROR_SEQ );
 620 | 
 621 |   // Define a local copy of the state
 622 |   noInterrupts();
 623 |   local_state = ESCCMD_state[i];
 624 |   interrupts();
 625 | 
 626 |   // Check if ESC is armed
 627 |   if ( !( local_state & ESCCMD_STATE_ARMED ) )
 628 |     ESCCMD_ERROR( ESCCMD_ERROR_SEQ )
 629 | 
 630 |   // Check if telemetry is valid
 631 |   if ( ESCCMD_tlm_valid[i] )  {
 632 |     *deg = ESCCMD_tlm_deg[i];
 633 |   }
 634 |   else
 635 |     return ESCCMD_ERROR_TLM_INVAL;
 636 | 
 637 |   return 0;
 638 | }
 639 | 
 640 | //
 641 | //  Read dc power supply voltage of motor number i
 642 | //  Unit is Volt
 643 | //
 644 | int ESCCMD_read_volt( uint8_t i, uint16_t *volt )  {
 645 |   static uint8_t local_state;
 646 | 
 647 |   // Check if everything is initialized
 648 |   if ( !ESCCMD_init_flag )
 649 |     return ESCCMD_ERROR_INIT;
 650 | 
 651 |   // Check if motor is within range
 652 |   if ( i >= ESCCMD_n )
 653 |     return ESCCMD_ERROR_PARAM;
 654 |   
 655 |   // Check if timer started
 656 |   if ( !ESCCMD_timer_flag )
 657 |     ESCCMD_ERROR( ESCCMD_ERROR_SEQ );
 658 | 
 659 |   // Define a local copy of the state
 660 |   noInterrupts();
 661 |   local_state = ESCCMD_state[i];
 662 |   interrupts();
 663 | 
 664 |   // Check if ESC is armed
 665 |   if ( !( local_state & ESCCMD_STATE_ARMED ) )
 666 |     ESCCMD_ERROR( ESCCMD_ERROR_SEQ )
 667 | 
 668 |   // Check if telemetry is valid
 669 |   if ( ESCCMD_tlm_valid[i] )  {
 670 |     *volt = ESCCMD_tlm_volt[i];
 671 |   }
 672 |   else
 673 |     return ESCCMD_ERROR_TLM_INVAL;
 674 | 
 675 |   return 0;
 676 | }
 677 | 
 678 | //
 679 | //  Read current of motor number i
 680 | //  Unit is Ampere
 681 | //
 682 | int ESCCMD_read_amp( uint8_t i, uint16_t *amp )  {
 683 |   static uint8_t local_state;
 684 | 
 685 |   // Check if everything is initialized
 686 |   if ( !ESCCMD_init_flag )
 687 |     return ESCCMD_ERROR_INIT;
 688 | 
 689 |   // Check if motor is within range
 690 |   if ( i >= ESCCMD_n )
 691 |     return ESCCMD_ERROR_PARAM;
 692 |   
 693 |   // Check if timer started
 694 |   if ( !ESCCMD_timer_flag )
 695 |     ESCCMD_ERROR( ESCCMD_ERROR_SEQ );
 696 | 
 697 |   // Define a local copy of the state
 698 |   noInterrupts();
 699 |   local_state = ESCCMD_state[i];
 700 |   interrupts();
 701 | 
 702 |   // Check if ESC is armed
 703 |   if ( !( local_state & ESCCMD_STATE_ARMED ) )
 704 |     ESCCMD_ERROR( ESCCMD_ERROR_SEQ )
 705 | 
 706 |   // Check if telemetry is valid
 707 |   if ( ESCCMD_tlm_valid[i] )  {
 708 |     *amp = ESCCMD_tlm_amp[i];
 709 |   }
 710 |   else
 711 |     return ESCCMD_ERROR_TLM_INVAL;
 712 | 
 713 |   return 0;
 714 | }
 715 | 
 716 | //
 717 | //  Read consumption of motor number i
 718 | //  Unit is milli Ampere.hour
 719 | //
 720 | int ESCCMD_read_mah( uint8_t i, uint16_t *mah )  {
 721 |   static uint8_t local_state;
 722 | 
 723 |   // Check if everything is initialized
 724 |   if ( !ESCCMD_init_flag )
 725 |     return ESCCMD_ERROR_INIT;
 726 | 
 727 |   // Check if motor is within range
 728 |   if ( i >= ESCCMD_n )
 729 |     return ESCCMD_ERROR_PARAM;
 730 |   
 731 |   // Check if timer started
 732 |   if ( !ESCCMD_timer_flag )
 733 |     ESCCMD_ERROR( ESCCMD_ERROR_SEQ );
 734 | 
 735 |   // Define a local copy of the state
 736 |   noInterrupts();
 737 |   local_state = ESCCMD_state[i];
 738 |   interrupts();
 739 | 
 740 |   // Check if ESC is armed
 741 |   if ( !( local_state & ESCCMD_STATE_ARMED ) )
 742 |     ESCCMD_ERROR( ESCCMD_ERROR_SEQ )
 743 | 
 744 |   // Check if telemetry is valid
 745 |   if ( ESCCMD_tlm_valid[i] )  {
 746 |     *mah = ESCCMD_tlm_mah[i];
 747 |   }
 748 |   else
 749 |     return ESCCMD_ERROR_TLM_INVAL;
 750 | 
 751 |   return 0;
 752 | }
 753 | 
 754 | //
 755 | //  Read shaft rotational velocity of motor number i
 756 | //  Unit is round per minute
 757 | //  The sign of the measurement depends on the last throttle sign
 758 | //
 759 | int ESCCMD_read_rpm( uint8_t i, int16_t *rpm )  {
 760 |   static uint8_t local_state;
 761 | 
 762 |   // Check if everything is initialized
 763 |   if ( !ESCCMD_init_flag )
 764 |     return ESCCMD_ERROR_INIT;
 765 | 
 766 |   // Check if motor is within range
 767 |   if ( i >= ESCCMD_n )
 768 |     return ESCCMD_ERROR_PARAM;
 769 |   
 770 |   // Check if timer started
 771 |   if ( !ESCCMD_timer_flag )
 772 |     ESCCMD_ERROR( ESCCMD_ERROR_SEQ );
 773 | 
 774 |   // Define a local copy of the state
 775 |   noInterrupts();
 776 |   local_state = ESCCMD_state[i];
 777 |   interrupts();
 778 | 
 779 |   // Check if ESC is armed
 780 |   if ( !( local_state & ESCCMD_STATE_ARMED ) )
 781 |     ESCCMD_ERROR( ESCCMD_ERROR_SEQ )
 782 | 
 783 |   // Check if telemetry is valid
 784 |   if ( ESCCMD_tlm_valid[i] )  {
 785 |     // Check current mode
 786 |     if ( local_state & ESCCMD_STATE_3D )  {
 787 |       // 3D mode
 788 |       // 48 - 1047    : positive direction (48 slowest)
 789 |       // 1048 - 2047  : negative direction (1048 slowest)
 790 |       if ( ESCCMD_cmd[i] > DSHOT_CMD_MAX + 1 + ESCCMD_MAX_3D_THROTTLE )
 791 |         *rpm = -ESCCMD_tlm_rpm[i];  // 3D mode negative direction
 792 |       else
 793 |         *rpm = ESCCMD_tlm_rpm[i];   // 3D mode positive direction
 794 |     }
 795 |     else {
 796 |       // Default mode
 797 |       *rpm = ESCCMD_tlm_rpm[i];
 798 |     }
 799 |     
 800 |     // Convert electrical rpm * 100 into motor rpm * 10
 801 |     *rpm = ( *rpm * 10 * 2 ) / ESCCMD_TLM_NB_POLES;
 802 |   }
 803 |   else
 804 |     return ESCCMD_ERROR_TLM_INVAL;
 805 | 
 806 |   return 0;
 807 | }
 808 | 
 809 | //
 810 | //  Read next serial packet
 811 | //
 812 | uint8_t *ESCCMD_read_packet( uint8_t i )  {
 813 |   #ifndef ESCCMD_ESC_EMULATION
 814 |   static int      buffer_idx[ESCCMD_NB_UART] = { 0 };
 815 |   static int      serial_ret;
 816 |   #endif
 817 |   
 818 |   #ifdef ESCCMD_ESC_EMULATION
 819 |   uint8_t *pt_c;
 820 |   
 821 |   // Check if a packet is available
 822 |   if ( ESCCMD_tlm_emu_nb[i] )  {
 823 |   
 824 |     ESCCMD_tlm_emu_nb[i]--;
 825 |     
 826 |     pt_c = &ESCCMD_tlm_emu_deg[ESCCMD_tlm_emu_nb[i]][i];
 827 |     ESCCMD_bufferTlm[i][0] = pt_c[0];
 828 |     pt_c = (uint8_t*)&ESCCMD_tlm_emu_volt[ESCCMD_tlm_emu_nb[i]][i];
 829 |     ESCCMD_bufferTlm[i][1] = pt_c[1];
 830 |     ESCCMD_bufferTlm[i][2] = pt_c[0];
 831 |     pt_c = (uint8_t*)&ESCCMD_tlm_emu_amp[ESCCMD_tlm_emu_nb[i]][i];
 832 |     ESCCMD_bufferTlm[i][3] = pt_c[1];
 833 |     ESCCMD_bufferTlm[i][4] = pt_c[0];
 834 |     pt_c = (uint8_t*)&ESCCMD_tlm_emu_mah[ESCCMD_tlm_emu_nb[i]][i];
 835 |     ESCCMD_bufferTlm[i][5] = pt_c[1];
 836 |     ESCCMD_bufferTlm[i][6] = pt_c[0];
 837 |     pt_c = (uint8_t*)&ESCCMD_tlm_emu_rpm[ESCCMD_tlm_emu_nb[i]][i];
 838 |     ESCCMD_bufferTlm[i][7] = pt_c[1];
 839 |     ESCCMD_bufferTlm[i][8] = pt_c[0];
 840 |     ESCCMD_bufferTlm[i][9] = ESCCMD_crc8( ESCCMD_bufferTlm[i], ESCCMD_TLM_LENGTH - 1 );
 841 |     
 842 |     return ESCCMD_bufferTlm[i];
 843 |   }
 844 |   #else
 845 |   // Read all bytes in rx buffer up to packet length
 846 |   while ( ( ESCCMD_serial[i]->available( ) ) && 
 847 |           ( buffer_idx[i] < ESCCMD_TLM_LENGTH ) ) {
 848 |           
 849 |     serial_ret = ESCCMD_serial[i]->read( );
 850 |       
 851 |     if ( serial_ret >= 0 )  {
 852 |       ESCCMD_bufferTlm[i][buffer_idx[i]] = (uint8_t)serial_ret;
 853 |       buffer_idx[i]++;
 854 |     }
 855 |   }
 856 |   
 857 |   // Check if a complete packet has arrived
 858 |   if ( buffer_idx[i] == ESCCMD_TLM_LENGTH  )  {
 859 |     
 860 |     // Reset byte index in packet buffer
 861 |     buffer_idx[i] = 0;
 862 |     
 863 |     // Return pointer to buffer
 864 |     return ESCCMD_bufferTlm[i];
 865 |   }
 866 |   #endif
 867 | 
 868 |   return NULL;
 869 | }
 870 | 
 871 | //
 872 | // Extract data from the current packet
 873 | //
 874 | int ESCCMD_extract_packet_data( uint8_t i )  {
 875 | 
 876 |   // Extract packet data
 877 |         
 878 |   ESCCMD_tlm_deg[i]     =   ESCCMD_bufferTlm[i][0];
 879 |   ESCCMD_tlm_volt[i]    = ( ESCCMD_bufferTlm[i][1] << 8 ) | ESCCMD_bufferTlm[i][2];
 880 |   ESCCMD_tlm_amp[i]     = ( ESCCMD_bufferTlm[i][3] << 8 ) | ESCCMD_bufferTlm[i][4];
 881 |   ESCCMD_tlm_mah[i]     = ( ESCCMD_bufferTlm[i][5] << 8 ) | ESCCMD_bufferTlm[i][6];
 882 |   ESCCMD_tlm_rpm[i]     = ( ESCCMD_bufferTlm[i][7] << 8 ) | ESCCMD_bufferTlm[i][8];
 883 |   ESCCMD_tlm_valid[i]   = ( ESCCMD_bufferTlm[i][9] == ESCCMD_crc8( ESCCMD_bufferTlm[i], ESCCMD_TLM_LENGTH - 1 ) );
 884 | 
 885 |   // If crc is invalid, increment crc error counter
 886 |   // and flush UART buffer
 887 |   if ( !ESCCMD_tlm_valid[i] ) {
 888 | 
 889 |     ESCCMD_CRC_errors[i]++;
 890 | 
 891 |     ESCCMD_last_error[i] = ESCCMD_ERROR_TLM_CRC;
 892 | 
 893 |     // Check for excessive CRC errors
 894 |     if ( ESCCMD_CRC_errors[i] >= ESCCMD_TLM_MAX_CRC_ERR ) {
 895 |       ESCCMD_last_error[i] = ESCCMD_ERROR_TLM_CRCMAX;
 896 |     }
 897 |     
 898 |     // Flush UART incoming buffer
 899 |     // If ESC is transmitting, need to wait for some byte(s) to come in
 900 |     do {
 901 |       ESCCMD_serial[i]->clear( );
 902 |       delayMicroseconds( ESCCMD_TLM_BYTE_TIME * 2 );
 903 |     } while ( ESCCMD_serial[i]->available( ) );
 904 |     
 905 |     // Reset pending packet counter
 906 |     ESCCMD_tlm_pend[i] = 0;
 907 | 
 908 |     // Reset lost packet counter
 909 |     ESCCMD_tlm_lost_cnt[i] = 0;
 910 |     
 911 |     return ESCCMD_last_error[i];
 912 |   }
 913 |   else {
 914 |     // Make some verifications on the telemetry
 915 | 
 916 |     // Check for overtheating of the ESC
 917 |     if ( ESCCMD_tlm_deg[i] >= ESCCMD_TLM_MAX_TEMP ) {
 918 |       ESCCMD_last_error[i] = ESCCMD_ERROR_TLM_TEMP;
 919 |       return ESCCMD_last_error[i];
 920 |     }
 921 |   }
 922 | 
 923 |   return 0;
 924 | }
 925 | 
 926 | #ifdef ESCCMD_ESC_EMULATION
 927 | //
 928 | //  Emulate telemetry
 929 | //
 930 | void ESCCMD_emulate_tlm( uint8_t i )   {
 931 |   static uint8_t  local_state;
 932 |   
 933 |   // Bufferize emulated telemetry data
 934 |   if ( ESCCMD_tlm_emu_nb[i] < ESCCMD_EMU_TLM_MAX ) {
 935 |     if ( ESCCMD_tlm[i] )  {
 936 |       ESCCMD_tlm_emu_deg[ESCCMD_tlm_emu_nb[i]][i]  = (uint8_t)( ESCCMD_EMU_TLM_DEG *   ( 1.0 + ESCCMD_EMU_TLM_NOISE / 100.0 * (float)( rand( ) - RAND_MAX / 2 ) / ( RAND_MAX / 2 ) ) );
 937 |       ESCCMD_tlm_emu_volt[ESCCMD_tlm_emu_nb[i]][i] = (uint16_t)( ESCCMD_EMU_TLM_VOLT * ( 1.0 + ESCCMD_EMU_TLM_NOISE / 100.0 * (float)( rand( ) - RAND_MAX / 2 ) / ( RAND_MAX / 2 ) ) );
 938 |       ESCCMD_tlm_emu_amp[ESCCMD_tlm_emu_nb[i]][i]  = (uint16_t)( ESCCMD_EMU_TLM_AMP *  ( 1.0 + ESCCMD_EMU_TLM_NOISE / 100.0 * (float)( rand( ) - RAND_MAX / 2 ) / ( RAND_MAX / 2 ) ) );
 939 |       ESCCMD_tlm_emu_mah[ESCCMD_tlm_emu_nb[i]][i]  = (uint16_t)( ESCCMD_EMU_TLM_MAH *  ( 1.0 + ESCCMD_EMU_TLM_NOISE / 100.0 * (float)( rand( ) - RAND_MAX / 2 ) / ( RAND_MAX / 2 ) ) );
 940 | 
 941 |       // Define a local copy of the state
 942 |       noInterrupts();
 943 |       local_state = ESCCMD_state[i];
 944 |       interrupts();
 945 | 
 946 |       // Compute rpm according to throttle cmd
 947 |       if ( local_state & ESCCMD_STATE_3D )  {
 948 |       
 949 |         // 3D mode
 950 |         if ( ESCCMD_cmd[i] > DSHOT_CMD_MAX + 1 + ESCCMD_MAX_3D_THROTTLE ) {
 951 |           // Negative direction
 952 |           ESCCMD_tlm_emu_rpm[ESCCMD_tlm_emu_nb[i]][i] = (uint16_t)(  (float)( ESCCMD_cmd[i] - ( DSHOT_CMD_MAX + 2 + ESCCMD_MAX_3D_THROTTLE ) ) / ESCCMD_MAX_3D_THROTTLE
 953 |                                                                 * (float)( ESCCMD_EMU_TLM_VOLT / 100 ) * ESCCMD_EMU_MOTOR_KV
 954 |                                                                 / 100.0 * ESCCMD_TLM_NB_POLES / 2 ); 
 955 |         }
 956 |         else  {
 957 |           // Positive direction
 958 |           ESCCMD_tlm_emu_rpm[ESCCMD_tlm_emu_nb[i]][i] = (uint16_t)(  (float)( ESCCMD_cmd[i] - ( DSHOT_CMD_MAX + 1 ) ) / ESCCMD_MAX_3D_THROTTLE
 959 |                                                                 * (float)( ESCCMD_EMU_TLM_VOLT / 100 ) * ESCCMD_EMU_MOTOR_KV
 960 |                                                                 / 100.0  * ESCCMD_TLM_NB_POLES / 2 ); 
 961 |         }
 962 |       }
 963 |       else  {
 964 |         // Normal mode
 965 |         ESCCMD_tlm_emu_rpm[ESCCMD_tlm_emu_nb[i]][i] = (uint16_t)(  (float)( ESCCMD_cmd[i] - ( DSHOT_CMD_MAX + 1 ) ) / ESCCMD_MAX_THROTTLE
 966 |                                                                 * (float)( ESCCMD_EMU_TLM_VOLT / 100 ) * ESCCMD_EMU_MOTOR_KV
 967 |                                                                 / 100.0  * ESCCMD_TLM_NB_POLES / 2 );
 968 |       }
 969 |     }
 970 |     
 971 |     // Increment tlm counter according to packet loss statistics
 972 |     #ifdef ESCCMD_ESC_EMU_PKT_LOSS
 973 |     if ( (int)( ( (float)rand( ) / (float)RAND_MAX * (float)ESCCMD_ESC_FRACTION_PKTLOSS ) ) ) {
 974 |       ESCCMD_tlm_emu_nb[i]++;
 975 |     }
 976 |     #else
 977 |     ESCCMD_tlm_emu_nb[i]++;
 978 |     #endif
 979 |   }
 980 | }
 981 | #endif
 982 | 
 983 | //
 984 | //  This routine should be called within the main loop
 985 | //  Returns ESCCMD_TIC_OCCURED when a tic occurs, 
 986 | //  Return 0 otherwise.
 987 | //
 988 | int ESCCMD_tic( void )  {
 989 |   static int      i;
 990 |   static uint16_t local_tic_pend;
 991 |  
 992 |   // Check if timer started
 993 |   if ( !ESCCMD_timer_flag ) {
 994 |     return 0;
 995 |   }
 996 |     
 997 |   //// Read telemetry
 998 |   
 999 |   for ( i = 0; i < ESCCMD_n; i++ )  {
1000 |     
1001 |     // Process all available telemetry packets
1002 |     while ( ESCCMD_read_packet( i ) )  {
1003 |         
1004 |       // Update pending packet counter
1005 |       if ( ESCCMD_tlm_pend[i] ) {
1006 |         ESCCMD_tlm_pend[i]--;
1007 |       }
1008 |       else  {
1009 |         // Spurious packet ?
1010 |         ESCCMD_last_error[i] = ESCCMD_ERROR_TLM_PEND;
1011 |       }
1012 |       
1013 |       // Extract packet data
1014 |       ESCCMD_extract_packet_data( i );
1015 |     }
1016 |         
1017 |     // Check for exceeding packet pending
1018 |     if ( ESCCMD_tlm_pend[i] > ESCCMD_TLM_MAX_PEND ) {
1019 | 
1020 |       // Packet is considered as lost: update packet lost counter
1021 |       if ( ESCCMD_tlm_lost_cnt[i] >= ESCCMD_TLM_MAX_PKT_LOSS )  {
1022 |         ESCCMD_last_error[i] = ESCCMD_ERROR_TLM_LOST;
1023 |       }
1024 |       else  {
1025 |         ESCCMD_tlm_lost_cnt[i]++;
1026 |       }
1027 | 
1028 |       // Decrement packet pending counter
1029 |       ESCCMD_tlm_pend[i]--;
1030 |     }
1031 |   }
1032 |   
1033 |   //// Process clock tics
1034 |   
1035 |   noInterrupts();
1036 |   local_tic_pend = ESCCMD_tic_pend;
1037 |   interrupts();
1038 | 
1039 |   if ( local_tic_pend ) {
1040 | 
1041 |     // Acknowledgement of one timer clock event
1042 |     noInterrupts();
1043 |     ESCCMD_tic_pend--;
1044 |     interrupts();
1045 | 
1046 |     // Update counters
1047 |     ESCCMD_tic_counter++;
1048 |     
1049 |     if ( !( ESCCMD_tic_counter % ESCCMD_TLM_PER ) ) {
1050 |       
1051 |       // Clear stat counters every ESCCMD_TLM_PER iterations
1052 |       for ( i = 0; i < ESCCMD_n; i++ )  {
1053 |         ESCCMD_tlm_lost_cnt[i] = 0;
1054 |         ESCCMD_CRC_errors[i] = 0; 
1055 |       }
1056 |     }
1057 | 
1058 |     // Check if everything is initialized
1059 |     if ( !ESCCMD_init_flag )  {
1060 |       for ( i = 0; i < ESCCMD_n; i++ )  {
1061 |         ESCCMD_last_error[i] = ESCCMD_ERROR_INIT;
1062 |       }
1063 |       return ESCCMD_TIC_OCCURED;
1064 |     }
1065 | 
1066 |     // Check if all ESC are armed
1067 |     for ( i = 0; i < ESCCMD_n; i++ )  {
1068 |       if ( !( ESCCMD_state[i] & ESCCMD_STATE_ARMED ) )  {
1069 |         ESCCMD_last_error[i] = ESCCMD_ERROR_SEQ;
1070 |         return ESCCMD_TIC_OCCURED;
1071 |       }
1072 |     }
1073 | 
1074 |     // Throttle watchdog
1075 |     for ( i = 0; i < ESCCMD_n; i++ )  {
1076 |       if ( ESCCMD_throttle_wd[i] >= ESCCMD_THWD_LEVEL )  {
1077 |         // Watchdog triggered on ESC number i
1078 |         ESCCMD_cmd[i] = DSHOT_CMD_MOTOR_STOP;
1079 |         ESCCMD_tlm[i] = 0;
1080 |         ESCCMD_last_error[i] = 0;
1081 |         ESCCMD_tlm_lost_cnt[i] = 0;
1082 |         ESCCMD_CRC_errors[i] = 0;
1083 |         noInterrupts();
1084 |         ESCCMD_state[i] &= ~( ESCCMD_STATE_START );
1085 |         interrupts();
1086 |       }
1087 |       else {
1088 |         ESCCMD_throttle_wd[i]++;
1089 |       }
1090 |     }
1091 | 
1092 |     // Send current command
1093 |     if ( DSHOT_send( ESCCMD_cmd, ESCCMD_tlm ) ) {
1094 |       for ( i = 0; i < ESCCMD_n; i++ )  {
1095 |         ESCCMD_last_error[i] = ESCCMD_ERROR_DSHOT;
1096 |       }
1097 |     }
1098 |     else  {
1099 |       delayMicroseconds( ESCCMD_CMD_DELAY );
1100 |   
1101 |       // Update telemetry packet pending counter
1102 |       for ( i = 0; i < ESCCMD_n; i++ )  {
1103 |         if ( ESCCMD_tlm[i] )  {
1104 |           ESCCMD_tlm_pend[i]++;
1105 |           #ifdef ESCCMD_ESC_EMULATION
1106 |           ESCCMD_emulate_tlm( i );
1107 |           #endif
1108 |         }
1109 |       }
1110 |     }
1111 |     
1112 |     // Inform caller that a clock tic occured
1113 |     return ESCCMD_TIC_OCCURED;
1114 |   }
1115 | 
1116 |   return 0;
1117 | }
1118 | 
1119 | //
1120 | // crc8 calculation
1121 | //
1122 | uint8_t ESCCMD_update_crc8( uint8_t crc, uint8_t crc_seed ) {
1123 |   static uint8_t crc_u;
1124 | 
1125 |   crc_u = crc;
1126 |   crc_u ^= crc_seed;
1127 | 
1128 |   for ( int i = 0; i < 8; i++ ) {
1129 |     crc_u = ( crc_u & 0x80 ) ? 0x7 ^ ( crc_u << 1 ) : ( crc_u << 1 );
1130 |   }
1131 | 
1132 |   return crc_u;
1133 | }
1134 | 
1135 | uint8_t ESCCMD_crc8( uint8_t* buf, uint8_t buflen ) {
1136 |   static uint8_t crc;
1137 | 
1138 |   crc = 0;
1139 | 
1140 |   for ( int i = 0; i < buflen; i++ ) {
1141 |     crc = ESCCMD_update_crc8( buf[i], crc );
1142 |   }
1143 | 
1144 |   return crc;
1145 | }
1146 | 
1147 | //
1148 | //  Timer ISR
1149 | //
1150 | void ESCCMD_ISR_timer( void ) {
1151 |   static int i;
1152 | 
1153 |   // Check for maximum missed tics (ESC watchdog timer = 250ms on a KISS ESC)
1154 |   if ( ESCCMD_tic_pend >= ESCCMD_TIMER_MAX_MISS )  {
1155 | 
1156 |     // ESC watchdog switch to disarmed and stopped mode
1157 |     for ( i = 0; i < ESCCMD_n; i++ )  {
1158 |       ESCCMD_state[i] &= ~( ESCCMD_STATE_ARMED | ESCCMD_STATE_START );
1159 |     }
1160 |   }
1161 |   else  {
1162 |     ESCCMD_tic_pend++;
1163 |   }
1164 | }
1165 | 


--------------------------------------------------------------------------------
/ESCPID/ESCCMD.h:
--------------------------------------------------------------------------------
  1 | /*
  2 |  *  Global definitions for ESCCMD
  3 |  *  NB: Arduino IDE automatically adds prototypes of functions
  4 |  *      found in all .ino files.
  5 |  *
  6 |  */
  7 | 
  8 | #ifndef __ESCCMD_H
  9 | #define __ESCCMD_H
 10 | 
 11 | // Defines
 12 | #define ESCCMD_MAX_ESC          DSHOT_MAX_OUTPUTS // Max number of ESCs
 13 | #define ESCCMD_NB_UART          6                 // Number of UARTS available
 14 | #if ESCCMD_NB_UART < ESCCMD_MAX_ESC
 15 |   #error ESCCMD_NB_UART should be >= ESCCMD_MAX_ESC
 16 | #endif
 17 | 
 18 | #define ESCCMD_STATE_ARMED      1                 // Mask for the arming flag
 19 | #define ESCCMD_STATE_3D         2                 // Mask for the default/3D mode
 20 | #define ESCCMD_STATE_START      4                 // Mask for the motor start/stop bit
 21 | 
 22 | #define ESCCMD_CMD_REPETITION   10                // Number of time commands have to be repeated to be acknowledged by ESC
 23 | #define ESCCMD_CMD_ARMING_REP   25                // Number of command repetition to arm
 24 | #define ESCCMD_CMD_DELAY        50                // Delay between two consecutive DSHOT transmissions (us)
 25 | #define ESCCMD_CMD_SAVE_DELAY   250000            // Minimum time to wait after a save command (us)
 26 | 
 27 | #define ESCCMD_TIMER_PERIOD     2000              // Periodic loop period (us)
 28 | #define ESCCMD_ESC_WATCHDOG     250000            // ESC arming watchdog timer (us)
 29 | #define ESCCMD_TIMER_MAX_MISS   ( ESCCMD_ESC_WATCHDOG / ESCCMD_TIMER_PERIOD )
 30 |                                                   // Maximum missed tics before watchdog is triggered
 31 | #define ESCCMD_THWD_LEVEL       20                // Maximum number of periods without throttle refresh
 32 | 
 33 | #define ESCCMD_TLM_UART_SPEED   115200            // Baudrate of the telemetry serial transmission
 34 | #define ESCCMD_TLM_BYTE_TIME    87                // Duration of one byte transmission (us)
 35 | #define ESCCMD_TLM_LENGTH       10                // Number of bytes in the telemetry packet
 36 | #define ESCCMD_TLM_MAX_PEND     1                 // Maximum number of telemetry packet pending
 37 | #define ESCCMD_TLM_NB_POLES     14                // Number of motor poles
 38 | #define ESCCMD_TLM_MAX_TEMP     100               // Maximum ESC temperature (°C)
 39 | #define ESCCMD_TLM_MAX_CRC_ERR  5                 // Maximum CRC errors (per ESCCMD_TLM_PER iterations)
 40 | #define ESCCMD_TLM_MAX_PKT_LOSS 50                // Maximum packet loss (per ESCCMD_TLM_PER iterations)
 41 | #define ESCCMD_TLM_PER          1000              // If 100, error thresholds are in percent     
 42 | 
 43 | #define ESCCMD_MAX_THROTTLE     1999              // Max default throttle value
 44 | #define ESCCMD_MAX_3D_THROTTLE  999               // Max 3D throttle value
 45 | #define ESCCMD_MIN_3D_THROTTLE  -999              // Min 3D throttle value
 46 | 
 47 | #define ESCCMD_BEEP_DURATION    50                // Duration of a beep (ms)
 48 | #define ESCCMD_ERROR_DSHOT      -1                // DSHOT error
 49 | #define ESCCMD_ERROR_SEQ        -2                // Invalid function call sequence error
 50 | #define ESCCMD_ERROR_INIT       -3                // Call of non initialized function
 51 | #define ESCCMD_ERROR_PARAM      -4                // Invalid parameter error
 52 | #define ESCCMD_ERROR_TLM_CRC    -5                // CRC error in telemetry packet
 53 | #define ESCCMD_ERROR_TLM_INVAL  -6                // Invalid telemetry error
 54 | #define ESCCMD_ERROR_TLM_PEND   -7                // Maximum number of pending telemetry packet reached
 55 | #define ESCCMD_ERROR_TLM_TEMP   -8                // Maximum ESC temperature reached
 56 | #define ESCCMD_ERROR_TLM_CRCMAX -9                // Maximum allowed CRC errors reached
 57 | #define ESCCMD_ERROR_TLM_LOST   -10               // Lost packet(s) detected
 58 | 
 59 | #define ESCCMD_TIC_OCCURED      1                 // A new timer tic has occured
 60 | 
 61 | // enums: borrowed from betaflight pwm_output.h
 62 | typedef enum {
 63 |   DSHOT_CMD_MOTOR_STOP = 0,
 64 |   DSHOT_CMD_BEACON1,
 65 |   DSHOT_CMD_BEACON2,
 66 |   DSHOT_CMD_BEACON3,
 67 |   DSHOT_CMD_BEACON4,
 68 |   DSHOT_CMD_BEACON5,
 69 |   DSHOT_CMD_ESC_INFO,                       // V2 includes settings
 70 |   DSHOT_CMD_SPIN_DIRECTION_1,
 71 |   DSHOT_CMD_SPIN_DIRECTION_2,
 72 |   DSHOT_CMD_3D_MODE_OFF,                    // 9
 73 |   DSHOT_CMD_3D_MODE_ON,                     // 10
 74 |   DSHOT_CMD_SETTINGS_REQUEST,               // Currently not implemented
 75 |   DSHOT_CMD_SAVE_SETTINGS,                  // 12
 76 |   DSHOT_CMD_SPIN_DIRECTION_NORMAL = 20,
 77 |   DSHOT_CMD_SPIN_DIRECTION_REVERSED = 21,
 78 |   DSHOT_CMD_LED0_ON,                        // BLHeli32 only
 79 |   DSHOT_CMD_LED1_ON,                        // BLHeli32 only
 80 |   DSHOT_CMD_LED2_ON,                        // BLHeli32 only
 81 |   DSHOT_CMD_LED3_ON,                        // BLHeli32 only
 82 |   DSHOT_CMD_LED0_OFF,                       // BLHeli32 only
 83 |   DSHOT_CMD_LED1_OFF,                       // BLHeli32 only
 84 |   DSHOT_CMD_LED2_OFF,                       // BLHeli32 only
 85 |   DSHOT_CMD_LED3_OFF,                       // BLHeli32 only
 86 |   DSHOT_CMD_AUDIO_STREAM_MODE_ON_OFF = 30,  // KISS audio Stream mode on/Off
 87 |   DSHOT_CMD_SILENT_MODE_ON_OFF = 31,        // KISS silent Mode on/Off
 88 |   DSHOT_CMD_SIGNAL_LINE_TELEMETRY_DISABLE = 32,
 89 |   DSHOT_CMD_SIGNAL_LINE_CONTINUOUS_ERPM_TELEMETRY = 33,
 90 |   DSHOT_CMD_MAX = 47
 91 | } ESCCMD_codes;
 92 | 
 93 | // Function prototypes
 94 | void    ESCCMD_init( uint8_t );
 95 | int     ESCCMD_arm_all( void );
 96 | int     ESCCMD_3D_on( void );
 97 | int     ESCCMD_3D_off( void );
 98 | int     ESCCMD_start_timer( void );
 99 | int     ESCCMD_stop_timer( void );
100 | int     ESCCMD_throttle( uint8_t, int16_t );
101 | int     ESCCMD_stop( uint8_t );
102 | int     ESCCMD_read_err( uint8_t, int8_t* );
103 | int     ESCCMD_read_cmd( uint8_t, uint16_t* );
104 | int     ESCCMD_read_tlm_status( uint8_t );
105 | int     ESCCMD_read_deg( uint8_t, uint8_t* );
106 | int     ESCCMD_read_volt( uint8_t, uint16_t* );
107 | int     ESCCMD_read_amp( uint8_t, uint16_t* );
108 | int     ESCCMD_read_mah( uint8_t, uint16_t* );
109 | int     ESCCMD_read_rpm( uint8_t, int16_t* );
110 | uint8_t *ESCCMD_read_packet( uint8_t );
111 | int     ESCCMD_extract_packet_data( uint8_t );
112 | int     ESCCMD_tic( void );
113 | uint8_t ESCCMD_update_crc8( uint8_t, uint8_t );
114 | uint8_t ESCCMD_crc8( uint8_t*, uint8_t );
115 | void    ESCCMD_ISR_timer( void );
116 | 
117 | #endif
118 | 


--------------------------------------------------------------------------------
/ESCPID/ESCPID.h:
--------------------------------------------------------------------------------
 1 | /*
 2 |  *  Definitions for ESCPID.ino
 3 |  *
 4 |  */
 5 | 
 6 | #ifndef __ESCPID_H
 7 | #define __ESCPID_H
 8 | 
 9 | // Defines
10 | #define ESCPID_NB_ESC             2                 // Number of ESCs
11 | #define ESCPID_MAX_ESC            6                 // Max number of ESCs
12 | 
13 | #define ESCPID_USB_UART_SPEED     115200            // Baudrate of the teeensy USB serial link
14 | 
15 | #define ESCPID_PID_P              400               // Default PID proportional gain
16 | #define ESCPID_PID_I              1050              // Default PID integral gain
17 | #define ESCPID_PID_D              1000              // Default PID derivative gain
18 | #define ESCPID_PID_F              9900              // Default PID derivative filtering pole
19 | #define ESCPID_PID_MIN            1                 // Default PID min control input value
20 | #define ESCPID_PID_MAX            300               // Default PID max control input value
21 | #define ESCPID_PID_ADAPT_GAIN     0.0001            // Range adaptation gain for PID coefficient
22 | 
23 | #define ESCPID_COMM_MAGIC         0x43305735        // Magic number: "teensy35" in leet speech
24 | #define ESCPID_RESET_GAIN         0xffff            // PIDf gain value that triggers teensy reset
25 | #define ESCPID_RESET_DELAY        1500              // Delay between reception of reset cmd and effective reset (ms)
26 | #define ESCPID_COMM_WD_LEVEL      20                // Maximum number of periods without reference refresh
27 | 
28 | #define ESCPID_ERROR_MAGIC        -1                // Magic number error code
29 | 
30 | // Teensy->host communication data structure
31 | // sizeof(ESCPID_comm)=64 to match USB 1.0 buffer size
32 | typedef struct {
33 |   uint32_t      magic;                        // Magic number
34 |   int8_t        err[ESCPID_MAX_ESC];          // Last error number
35 |   uint8_t       deg[ESCPID_MAX_ESC];          // ESC temperature (°C)
36 |   uint16_t      cmd[ESCPID_MAX_ESC];          // Current ESC command value
37 |   uint16_t      volt[ESCPID_MAX_ESC];         // Voltage of the ESC power supply (0.01V)
38 |   uint16_t      amp[ESCPID_MAX_ESC];          // ESC current (0.01A)
39 |   int16_t       rpm[ESCPID_MAX_ESC];          // Motor rpm (10 rpm)
40 | } ESCPIDcomm_struct_t;
41 | 
42 | // Host->teensy communication data structure
43 | // sizeof(Host_comm)=64 to match USB 1.0 buffer size
44 | typedef struct {
45 |   uint32_t      magic;                        // Magic number
46 |   int16_t       RPM_r[ESCPID_MAX_ESC];        // Velocity reference (10 rpm)
47 |   uint16_t      PID_P[ESCPID_MAX_ESC];        // PID proportional gain
48 |   uint16_t      PID_I[ESCPID_MAX_ESC];        // PID integral gain
49 |   uint16_t      PID_D[ESCPID_MAX_ESC];        // PID derivative gain
50 |   uint16_t      PID_f[ESCPID_MAX_ESC];        // PID filtering pole
51 | } Hostcomm_struct_t;
52 | 
53 | #endif
54 | 


--------------------------------------------------------------------------------
/ESCPID/ESCPID.ino:
--------------------------------------------------------------------------------
  1 | /*
  2 |  *  ESCPID:   PID control of up to 6 ESCs using teensy 3.5 MCU
  3 |  *
  4 |  *  Note:     Best viewed using Arduino IDE with tab space = 2
  5 |  *
  6 |  *  Authors:  Arda Yiğit and Jacques Gangloff
  7 |  *  Date:     May 2019
  8 |  */
  9 | 
 10 | // Includes
 11 | #include <Arduino.h>
 12 | #include "DSHOT.h"
 13 | #include "ESCCMD.h"
 14 | #include "AWPID.h"
 15 | #include "ESCPID.h"
 16 | 
 17 | // Globals
 18 | float     ESCPID_Reference[ESCPID_NB_ESC] = {};
 19 | float     ESCPID_Measurement[ESCPID_NB_ESC] = {};
 20 | float     ESCPID_Control[ESCPID_NB_ESC] = {};
 21 | uint16_t  ESCPID_comm_wd = 0;
 22 | 
 23 | float     ESCPID_Kp[ESCPID_NB_ESC];
 24 | float     ESCPID_Ki[ESCPID_NB_ESC];
 25 | float     ESCPID_Kd[ESCPID_NB_ESC];
 26 | float     ESCPID_f[ESCPID_NB_ESC];
 27 | float     ESCPID_Min[ESCPID_NB_ESC];
 28 | float     ESCPID_Max[ESCPID_NB_ESC];
 29 | 
 30 | ESCPIDcomm_struct_t ESCPID_comm = {
 31 |                                   ESCPID_COMM_MAGIC,
 32 |                                   {},
 33 |                                   {},
 34 |                                   {},
 35 |                                   {},
 36 |                                   {},
 37 |                                   {}
 38 |                                   };
 39 | Hostcomm_struct_t   Host_comm =   {
 40 |                                   ESCPID_COMM_MAGIC,
 41 |                                   {},
 42 |                                   {},
 43 |                                   {},
 44 |                                   {},
 45 |                                   {}
 46 |                                   };
 47 | 
 48 | //
 49 | // Manage communication with the host
 50 | //
 51 | int ESCPID_comm_update( void ) {
 52 |   static int          i;
 53 |   static uint8_t      *ptin   = (uint8_t*)(&Host_comm),
 54 |                       *ptout  = (uint8_t*)(&ESCPID_comm);
 55 |   static int          ret;
 56 |   static int          in_cnt = 0;
 57 |   
 58 |   ret = 0;
 59 |   
 60 |   // Read all incoming bytes available until incoming structure is complete
 61 |   while(  ( Serial.available( ) > 0 ) && 
 62 |           ( in_cnt < (int)sizeof( Host_comm ) ) )
 63 |     ptin[in_cnt++] = Serial.read( );
 64 |   
 65 |   // Check if a complete incoming packet is available
 66 |   if ( in_cnt == (int)sizeof( Host_comm ) ) {
 67 |   
 68 |     // Clear incoming bytes counter
 69 |     in_cnt = 0;
 70 |     
 71 |     // Look for a reset command
 72 |     // If first ESC has 0xffff for PID and f gains, reset teensy
 73 |     if (  ( Host_comm.PID_P[0] == ESCPID_RESET_GAIN ) &&
 74 |           ( Host_comm.PID_I[0] == ESCPID_RESET_GAIN ) &&
 75 |           ( Host_comm.PID_D[0] == ESCPID_RESET_GAIN ) &&
 76 |           ( Host_comm.PID_f[0] == ESCPID_RESET_GAIN ) ) {
 77 |       
 78 |       // Give time to host to close serial port
 79 |       delay( ESCPID_RESET_DELAY );
 80 | 
 81 |       // Reset command
 82 |       SCB_AIRCR = 0x05FA0004;
 83 |     }
 84 |     
 85 |     // Check the validity of the magic number
 86 |     if ( Host_comm.magic != ESCPID_COMM_MAGIC ) {
 87 |     
 88 |       // Flush input buffer
 89 |       while ( Serial.available( ) )
 90 |         Serial.read( );
 91 |     
 92 |       ret = ESCPID_ERROR_MAGIC;
 93 |     }
 94 |     else {
 95 |     
 96 |       // Valid packet received
 97 |       
 98 |       // Reset the communication watchdog
 99 |       ESCPID_comm_wd = 0;
100 |       
101 |       // Update the reference
102 |       for ( i = 0; i < ESCPID_NB_ESC; i++ )
103 |         ESCPID_Reference[i] = Host_comm.RPM_r[i];
104 |       
105 |       // Update PID tuning parameters
106 |       for ( i = 0; i < ESCPID_NB_ESC; i++ ) {
107 |         
108 |         // Gain conversion from int to float
109 |         ESCPID_Kp[i] =  ESCPID_PID_ADAPT_GAIN * Host_comm.PID_P[i];
110 |         ESCPID_Ki[i] =  ESCPID_PID_ADAPT_GAIN * Host_comm.PID_I[i];
111 |         ESCPID_Kd[i] =  ESCPID_PID_ADAPT_GAIN * Host_comm.PID_D[i];
112 |         ESCPID_f[i] =   ESCPID_PID_ADAPT_GAIN * Host_comm.PID_f[i];
113 |         
114 |         // Update PID tuning
115 |         AWPID_tune(     i,
116 |                         ESCPID_Kp[i],
117 |                         ESCPID_Ki[i],
118 |                         ESCPID_Kd[i],
119 |                         ESCPID_f[i],
120 |                         ESCPID_Min[i],
121 |                         ESCPID_Max[i]
122 |                        );
123 |       }
124 |       
125 |       // Update output data structure values
126 |       // If telemetry is invalid, data structure remains unmodified
127 |       for ( i = 0; i < ESCPID_NB_ESC; i++ ) {
128 |         ESCCMD_read_err( i, &ESCPID_comm.err[i] );
129 |         ESCCMD_read_cmd( i, &ESCPID_comm.cmd[i] );
130 |         ESCCMD_read_deg( i, &ESCPID_comm.deg[i] );
131 |         ESCCMD_read_volt( i, &ESCPID_comm.volt[i] );
132 |         ESCCMD_read_amp( i, &ESCPID_comm.amp[i] );
133 |         ESCCMD_read_rpm( i, &ESCPID_comm.rpm[i] );
134 |       }
135 |       
136 |       // Send data structure to host
137 |       Serial.write( ptout, sizeof( ESCPID_comm ) );
138 |       
139 |       // Force immediate transmission
140 |       Serial.send_now( );
141 |     }
142 |   }
143 | 
144 |   return ret;
145 | }
146 | 
147 | //
148 | //  Arduino setup function
149 | //
150 | void setup() {
151 |   int i;
152 | 
153 |   // Initialize USB serial link
154 |   Serial.begin( ESCPID_USB_UART_SPEED );
155 | 
156 |   // Initialize PID gains
157 |   for ( i = 0; i < ESCPID_NB_ESC; i++ ) {
158 |     ESCPID_Kp[i] =  ESCPID_PID_ADAPT_GAIN * ESCPID_PID_P;
159 |     ESCPID_Ki[i] =  ESCPID_PID_ADAPT_GAIN * ESCPID_PID_I;
160 |     ESCPID_Kd[i] =  ESCPID_PID_ADAPT_GAIN * ESCPID_PID_D;
161 |     ESCPID_f[i] =   ESCPID_PID_ADAPT_GAIN * ESCPID_PID_F;
162 |     ESCPID_Min[i] = ESCPID_PID_MIN;
163 |     ESCPID_Max[i] = ESCPID_PID_MAX;
164 |   }
165 | 
166 |   // Initialize PID subsystem
167 |   AWPID_init( ESCPID_NB_ESC, 
168 |               ESCPID_Kp, 
169 |               ESCPID_Ki, 
170 |               ESCPID_Kd, 
171 |               ESCPID_f, 
172 |               ESCPID_Min, 
173 |               ESCPID_Max );
174 | 
175 |   // Initialize the CMD subsystem
176 |   ESCCMD_init( ESCPID_NB_ESC );
177 | 
178 |   // Arming ESCs
179 |   ESCCMD_arm_all( );
180 |   
181 |   // Switch 3D mode on
182 |   ESCCMD_3D_on( );
183 | 
184 |   // Arming ESCs
185 |   ESCCMD_arm_all( );
186 |   
187 |   // Start periodic loop
188 |   ESCCMD_start_timer( );
189 |   
190 |   // Stop all motors
191 |   for ( i = 0; i < ESCPID_NB_ESC; i++ ) {
192 |     ESCCMD_stop( i );
193 |   }
194 | 
195 |   // Reference watchdog is initially triggered
196 |   ESCPID_comm_wd = ESCPID_COMM_WD_LEVEL;
197 | }
198 | 
199 | //
200 | //  Arduino main loop
201 | //
202 | void loop( ) {
203 |   static int    i, ret;
204 | 
205 |   // Check for next timer event
206 |   ret = ESCCMD_tic( );
207 | 
208 |   // Bidirectional serial exchange with host
209 |   ESCPID_comm_update(  );
210 | 
211 |   if ( ret == ESCCMD_TIC_OCCURED )  {
212 | 
213 |     // Process timer event
214 | 
215 |     // Read all measurements and compute current control signal
216 |     for ( i = 0; i < ESCPID_NB_ESC; i++ ) {
217 |     
218 |       // Compute control signal only if telemetry is valid
219 |       // In case of invalid telemetry, last control signal is sent
220 |       // If motor is stopped, don't update PID to avoid integral term windup
221 |       if ( !ESCCMD_read_tlm_status( i ) ) {
222 |       
223 |         // Update measurement
224 |         ESCPID_Measurement[i] = ESCPID_comm.rpm[i];
225 |         
226 |         // Update control signal
227 |         if ( ESCPID_Reference[i] >= 0 )
228 |           AWPID_control(  i, 
229 |                           ESCPID_Reference[i], 
230 |                           ESCPID_Measurement[i], 
231 |                           &ESCPID_Control[i] );
232 |         else  {
233 |           AWPID_control(  i, 
234 |                           -ESCPID_Reference[i], 
235 |                           -ESCPID_Measurement[i], 
236 |                           &ESCPID_Control[i] );
237 |           ESCPID_Control[i] *= -1.0;
238 |         }
239 |       }
240 |       
241 |       // Send control signal if reference has been sufficiently refreshed
242 |       if ( ESCPID_comm_wd < ESCPID_COMM_WD_LEVEL ) {
243 |         ret = ESCCMD_throttle( i, (int16_t)ESCPID_Control[i] );
244 |       }
245 |       else {
246 |         AWPID_reset( );
247 |       }
248 |     }
249 |     
250 |     // Update watchdog
251 |     if ( ESCPID_comm_wd < ESCPID_COMM_WD_LEVEL )  {
252 |       ESCPID_comm_wd++;
253 |     }
254 |   }
255 | }
256 | 


--------------------------------------------------------------------------------
/LICENSE:
--------------------------------------------------------------------------------
  1 |                     GNU GENERAL PUBLIC LICENSE
  2 |                        Version 3, 29 June 2007
  3 | 
  4 |  Copyright (C) 2007 Free Software Foundation, Inc. <https://fsf.org/>
  5 |  Everyone is permitted to copy and distribute verbatim copies
  6 |  of this license document, but changing it is not allowed.
  7 | 
  8 |                             Preamble
  9 | 
 10 |   The GNU General Public License is a free, copyleft license for
 11 | software and other kinds of works.
 12 | 
 13 |   The licenses for most software and other practical works are designed
 14 | to take away your freedom to share and change the works.  By contrast,
 15 | the GNU General Public License is intended to guarantee your freedom to
 16 | share and change all versions of a program--to make sure it remains free
 17 | software for all its users.  We, the Free Software Foundation, use the
 18 | GNU General Public License for most of our software; it applies also to
 19 | any other work released this way by its authors.  You can apply it to
 20 | your programs, too.
 21 | 
 22 |   When we speak of free software, we are referring to freedom, not
 23 | price.  Our General Public Licenses are designed to make sure that you
 24 | have the freedom to distribute copies of free software (and charge for
 25 | them if you wish), that you receive source code or can get it if you
 26 | want it, that you can change the software or use pieces of it in new
 27 | free programs, and that you know you can do these things.
 28 | 
 29 |   To protect your rights, we need to prevent others from denying you
 30 | these rights or asking you to surrender the rights.  Therefore, you have
 31 | certain responsibilities if you distribute copies of the software, or if
 32 | you modify it: responsibilities to respect the freedom of others.
 33 | 
 34 |   For example, if you distribute copies of such a program, whether
 35 | gratis or for a fee, you must pass on to the recipients the same
 36 | freedoms that you received.  You must make sure that they, too, receive
 37 | or can get the source code.  And you must show them these terms so they
 38 | know their rights.
 39 | 
 40 |   Developers that use the GNU GPL protect your rights with two steps:
 41 | (1) assert copyright on the software, and (2) offer you this License
 42 | giving you legal permission to copy, distribute and/or modify it.
 43 | 
 44 |   For the developers' and authors' protection, the GPL clearly explains
 45 | that there is no warranty for this free software.  For both users' and
 46 | authors' sake, the GPL requires that modified versions be marked as
 47 | changed, so that their problems will not be attributed erroneously to
 48 | authors of previous versions.
 49 | 
 50 |   Some devices are designed to deny users access to install or run
 51 | modified versions of the software inside them, although the manufacturer
 52 | can do so.  This is fundamentally incompatible with the aim of
 53 | protecting users' freedom to change the software.  The systematic
 54 | pattern of such abuse occurs in the area of products for individuals to
 55 | use, which is precisely where it is most unacceptable.  Therefore, we
 56 | have designed this version of the GPL to prohibit the practice for those
 57 | products.  If such problems arise substantially in other domains, we
 58 | stand ready to extend this provision to those domains in future versions
 59 | of the GPL, as needed to protect the freedom of users.
 60 | 
 61 |   Finally, every program is threatened constantly by software patents.
 62 | States should not allow patents to restrict development and use of
 63 | software on general-purpose computers, but in those that do, we wish to
 64 | avoid the special danger that patents applied to a free program could
 65 | make it effectively proprietary.  To prevent this, the GPL assures that
 66 | patents cannot be used to render the program non-free.
 67 | 
 68 |   The precise terms and conditions for copying, distribution and
 69 | modification follow.
 70 | 
 71 |                        TERMS AND CONDITIONS
 72 | 
 73 |   0. Definitions.
 74 | 
 75 |   "This License" refers to version 3 of the GNU General Public License.
 76 | 
 77 |   "Copyright" also means copyright-like laws that apply to other kinds of
 78 | works, such as semiconductor masks.
 79 | 
 80 |   "The Program" refers to any copyrightable work licensed under this
 81 | License.  Each licensee is addressed as "you".  "Licensees" and
 82 | "recipients" may be individuals or organizations.
 83 | 
 84 |   To "modify" a work means to copy from or adapt all or part of the work
 85 | in a fashion requiring copyright permission, other than the making of an
 86 | exact copy.  The resulting work is called a "modified version" of the
 87 | earlier work or a work "based on" the earlier work.
 88 | 
 89 |   A "covered work" means either the unmodified Program or a work based
 90 | on the Program.
 91 | 
 92 |   To "propagate" a work means to do anything with it that, without
 93 | permission, would make you directly or secondarily liable for
 94 | infringement under applicable copyright law, except executing it on a
 95 | computer or modifying a private copy.  Propagation includes copying,
 96 | distribution (with or without modification), making available to the
 97 | public, and in some countries other activities as well.
 98 | 
 99 |   To "convey" a work means any kind of propagation that enables other
100 | parties to make or receive copies.  Mere interaction with a user through
101 | a computer network, with no transfer of a copy, is not conveying.
102 | 
103 |   An interactive user interface displays "Appropriate Legal Notices"
104 | to the extent that it includes a convenient and prominently visible
105 | feature that (1) displays an appropriate copyright notice, and (2)
106 | tells the user that there is no warranty for the work (except to the
107 | extent that warranties are provided), that licensees may convey the
108 | work under this License, and how to view a copy of this License.  If
109 | the interface presents a list of user commands or options, such as a
110 | menu, a prominent item in the list meets this criterion.
111 | 
112 |   1. Source Code.
113 | 
114 |   The "source code" for a work means the preferred form of the work
115 | for making modifications to it.  "Object code" means any non-source
116 | form of a work.
117 | 
118 |   A "Standard Interface" means an interface that either is an official
119 | standard defined by a recognized standards body, or, in the case of
120 | interfaces specified for a particular programming language, one that
121 | is widely used among developers working in that language.
122 | 
123 |   The "System Libraries" of an executable work include anything, other
124 | than the work as a whole, that (a) is included in the normal form of
125 | packaging a Major Component, but which is not part of that Major
126 | Component, and (b) serves only to enable use of the work with that
127 | Major Component, or to implement a Standard Interface for which an
128 | implementation is available to the public in source code form.  A
129 | "Major Component", in this context, means a major essential component
130 | (kernel, window system, and so on) of the specific operating system
131 | (if any) on which the executable work runs, or a compiler used to
132 | produce the work, or an object code interpreter used to run it.
133 | 
134 |   The "Corresponding Source" for a work in object code form means all
135 | the source code needed to generate, install, and (for an executable
136 | work) run the object code and to modify the work, including scripts to
137 | control those activities.  However, it does not include the work's
138 | System Libraries, or general-purpose tools or generally available free
139 | programs which are used unmodified in performing those activities but
140 | which are not part of the work.  For example, Corresponding Source
141 | includes interface definition files associated with source files for
142 | the work, and the source code for shared libraries and dynamically
143 | linked subprograms that the work is specifically designed to require,
144 | such as by intimate data communication or control flow between those
145 | subprograms and other parts of the work.
146 | 
147 |   The Corresponding Source need not include anything that users
148 | can regenerate automatically from other parts of the Corresponding
149 | Source.
150 | 
151 |   The Corresponding Source for a work in source code form is that
152 | same work.
153 | 
154 |   2. Basic Permissions.
155 | 
156 |   All rights granted under this License are granted for the term of
157 | copyright on the Program, and are irrevocable provided the stated
158 | conditions are met.  This License explicitly affirms your unlimited
159 | permission to run the unmodified Program.  The output from running a
160 | covered work is covered by this License only if the output, given its
161 | content, constitutes a covered work.  This License acknowledges your
162 | rights of fair use or other equivalent, as provided by copyright law.
163 | 
164 |   You may make, run and propagate covered works that you do not
165 | convey, without conditions so long as your license otherwise remains
166 | in force.  You may convey covered works to others for the sole purpose
167 | of having them make modifications exclusively for you, or provide you
168 | with facilities for running those works, provided that you comply with
169 | the terms of this License in conveying all material for which you do
170 | not control copyright.  Those thus making or running the covered works
171 | for you must do so exclusively on your behalf, under your direction
172 | and control, on terms that prohibit them from making any copies of
173 | your copyrighted material outside their relationship with you.
174 | 
175 |   Conveying under any other circumstances is permitted solely under
176 | the conditions stated below.  Sublicensing is not allowed; section 10
177 | makes it unnecessary.
178 | 
179 |   3. Protecting Users' Legal Rights From Anti-Circumvention Law.
180 | 
181 |   No covered work shall be deemed part of an effective technological
182 | measure under any applicable law fulfilling obligations under article
183 | 11 of the WIPO copyright treaty adopted on 20 December 1996, or
184 | similar laws prohibiting or restricting circumvention of such
185 | measures.
186 | 
187 |   When you convey a covered work, you waive any legal power to forbid
188 | circumvention of technological measures to the extent such circumvention
189 | is effected by exercising rights under this License with respect to
190 | the covered work, and you disclaim any intention to limit operation or
191 | modification of the work as a means of enforcing, against the work's
192 | users, your or third parties' legal rights to forbid circumvention of
193 | technological measures.
194 | 
195 |   4. Conveying Verbatim Copies.
196 | 
197 |   You may convey verbatim copies of the Program's source code as you
198 | receive it, in any medium, provided that you conspicuously and
199 | appropriately publish on each copy an appropriate copyright notice;
200 | keep intact all notices stating that this License and any
201 | non-permissive terms added in accord with section 7 apply to the code;
202 | keep intact all notices of the absence of any warranty; and give all
203 | recipients a copy of this License along with the Program.
204 | 
205 |   You may charge any price or no price for each copy that you convey,
206 | and you may offer support or warranty protection for a fee.
207 | 
208 |   5. Conveying Modified Source Versions.
209 | 
210 |   You may convey a work based on the Program, or the modifications to
211 | produce it from the Program, in the form of source code under the
212 | terms of section 4, provided that you also meet all of these conditions:
213 | 
214 |     a) The work must carry prominent notices stating that you modified
215 |     it, and giving a relevant date.
216 | 
217 |     b) The work must carry prominent notices stating that it is
218 |     released under this License and any conditions added under section
219 |     7.  This requirement modifies the requirement in section 4 to
220 |     "keep intact all notices".
221 | 
222 |     c) You must license the entire work, as a whole, under this
223 |     License to anyone who comes into possession of a copy.  This
224 |     License will therefore apply, along with any applicable section 7
225 |     additional terms, to the whole of the work, and all its parts,
226 |     regardless of how they are packaged.  This License gives no
227 |     permission to license the work in any other way, but it does not
228 |     invalidate such permission if you have separately received it.
229 | 
230 |     d) If the work has interactive user interfaces, each must display
231 |     Appropriate Legal Notices; however, if the Program has interactive
232 |     interfaces that do not display Appropriate Legal Notices, your
233 |     work need not make them do so.
234 | 
235 |   A compilation of a covered work with other separate and independent
236 | works, which are not by their nature extensions of the covered work,
237 | and which are not combined with it such as to form a larger program,
238 | in or on a volume of a storage or distribution medium, is called an
239 | "aggregate" if the compilation and its resulting copyright are not
240 | used to limit the access or legal rights of the compilation's users
241 | beyond what the individual works permit.  Inclusion of a covered work
242 | in an aggregate does not cause this License to apply to the other
243 | parts of the aggregate.
244 | 
245 |   6. Conveying Non-Source Forms.
246 | 
247 |   You may convey a covered work in object code form under the terms
248 | of sections 4 and 5, provided that you also convey the
249 | machine-readable Corresponding Source under the terms of this License,
250 | in one of these ways:
251 | 
252 |     a) Convey the object code in, or embodied in, a physical product
253 |     (including a physical distribution medium), accompanied by the
254 |     Corresponding Source fixed on a durable physical medium
255 |     customarily used for software interchange.
256 | 
257 |     b) Convey the object code in, or embodied in, a physical product
258 |     (including a physical distribution medium), accompanied by a
259 |     written offer, valid for at least three years and valid for as
260 |     long as you offer spare parts or customer support for that product
261 |     model, to give anyone who possesses the object code either (1) a
262 |     copy of the Corresponding Source for all the software in the
263 |     product that is covered by this License, on a durable physical
264 |     medium customarily used for software interchange, for a price no
265 |     more than your reasonable cost of physically performing this
266 |     conveying of source, or (2) access to copy the
267 |     Corresponding Source from a network server at no charge.
268 | 
269 |     c) Convey individual copies of the object code with a copy of the
270 |     written offer to provide the Corresponding Source.  This
271 |     alternative is allowed only occasionally and noncommercially, and
272 |     only if you received the object code with such an offer, in accord
273 |     with subsection 6b.
274 | 
275 |     d) Convey the object code by offering access from a designated
276 |     place (gratis or for a charge), and offer equivalent access to the
277 |     Corresponding Source in the same way through the same place at no
278 |     further charge.  You need not require recipients to copy the
279 |     Corresponding Source along with the object code.  If the place to
280 |     copy the object code is a network server, the Corresponding Source
281 |     may be on a different server (operated by you or a third party)
282 |     that supports equivalent copying facilities, provided you maintain
283 |     clear directions next to the object code saying where to find the
284 |     Corresponding Source.  Regardless of what server hosts the
285 |     Corresponding Source, you remain obligated to ensure that it is
286 |     available for as long as needed to satisfy these requirements.
287 | 
288 |     e) Convey the object code using peer-to-peer transmission, provided
289 |     you inform other peers where the object code and Corresponding
290 |     Source of the work are being offered to the general public at no
291 |     charge under subsection 6d.
292 | 
293 |   A separable portion of the object code, whose source code is excluded
294 | from the Corresponding Source as a System Library, need not be
295 | included in conveying the object code work.
296 | 
297 |   A "User Product" is either (1) a "consumer product", which means any
298 | tangible personal property which is normally used for personal, family,
299 | or household purposes, or (2) anything designed or sold for incorporation
300 | into a dwelling.  In determining whether a product is a consumer product,
301 | doubtful cases shall be resolved in favor of coverage.  For a particular
302 | product received by a particular user, "normally used" refers to a
303 | typical or common use of that class of product, regardless of the status
304 | of the particular user or of the way in which the particular user
305 | actually uses, or expects or is expected to use, the product.  A product
306 | is a consumer product regardless of whether the product has substantial
307 | commercial, industrial or non-consumer uses, unless such uses represent
308 | the only significant mode of use of the product.
309 | 
310 |   "Installation Information" for a User Product means any methods,
311 | procedures, authorization keys, or other information required to install
312 | and execute modified versions of a covered work in that User Product from
313 | a modified version of its Corresponding Source.  The information must
314 | suffice to ensure that the continued functioning of the modified object
315 | code is in no case prevented or interfered with solely because
316 | modification has been made.
317 | 
318 |   If you convey an object code work under this section in, or with, or
319 | specifically for use in, a User Product, and the conveying occurs as
320 | part of a transaction in which the right of possession and use of the
321 | User Product is transferred to the recipient in perpetuity or for a
322 | fixed term (regardless of how the transaction is characterized), the
323 | Corresponding Source conveyed under this section must be accompanied
324 | by the Installation Information.  But this requirement does not apply
325 | if neither you nor any third party retains the ability to install
326 | modified object code on the User Product (for example, the work has
327 | been installed in ROM).
328 | 
329 |   The requirement to provide Installation Information does not include a
330 | requirement to continue to provide support service, warranty, or updates
331 | for a work that has been modified or installed by the recipient, or for
332 | the User Product in which it has been modified or installed.  Access to a
333 | network may be denied when the modification itself materially and
334 | adversely affects the operation of the network or violates the rules and
335 | protocols for communication across the network.
336 | 
337 |   Corresponding Source conveyed, and Installation Information provided,
338 | in accord with this section must be in a format that is publicly
339 | documented (and with an implementation available to the public in
340 | source code form), and must require no special password or key for
341 | unpacking, reading or copying.
342 | 
343 |   7. Additional Terms.
344 | 
345 |   "Additional permissions" are terms that supplement the terms of this
346 | License by making exceptions from one or more of its conditions.
347 | Additional permissions that are applicable to the entire Program shall
348 | be treated as though they were included in this License, to the extent
349 | that they are valid under applicable law.  If additional permissions
350 | apply only to part of the Program, that part may be used separately
351 | under those permissions, but the entire Program remains governed by
352 | this License without regard to the additional permissions.
353 | 
354 |   When you convey a copy of a covered work, you may at your option
355 | remove any additional permissions from that copy, or from any part of
356 | it.  (Additional permissions may be written to require their own
357 | removal in certain cases when you modify the work.)  You may place
358 | additional permissions on material, added by you to a covered work,
359 | for which you have or can give appropriate copyright permission.
360 | 
361 |   Notwithstanding any other provision of this License, for material you
362 | add to a covered work, you may (if authorized by the copyright holders of
363 | that material) supplement the terms of this License with terms:
364 | 
365 |     a) Disclaiming warranty or limiting liability differently from the
366 |     terms of sections 15 and 16 of this License; or
367 | 
368 |     b) Requiring preservation of specified reasonable legal notices or
369 |     author attributions in that material or in the Appropriate Legal
370 |     Notices displayed by works containing it; or
371 | 
372 |     c) Prohibiting misrepresentation of the origin of that material, or
373 |     requiring that modified versions of such material be marked in
374 |     reasonable ways as different from the original version; or
375 | 
376 |     d) Limiting the use for publicity purposes of names of licensors or
377 |     authors of the material; or
378 | 
379 |     e) Declining to grant rights under trademark law for use of some
380 |     trade names, trademarks, or service marks; or
381 | 
382 |     f) Requiring indemnification of licensors and authors of that
383 |     material by anyone who conveys the material (or modified versions of
384 |     it) with contractual assumptions of liability to the recipient, for
385 |     any liability that these contractual assumptions directly impose on
386 |     those licensors and authors.
387 | 
388 |   All other non-permissive additional terms are considered "further
389 | restrictions" within the meaning of section 10.  If the Program as you
390 | received it, or any part of it, contains a notice stating that it is
391 | governed by this License along with a term that is a further
392 | restriction, you may remove that term.  If a license document contains
393 | a further restriction but permits relicensing or conveying under this
394 | License, you may add to a covered work material governed by the terms
395 | of that license document, provided that the further restriction does
396 | not survive such relicensing or conveying.
397 | 
398 |   If you add terms to a covered work in accord with this section, you
399 | must place, in the relevant source files, a statement of the
400 | additional terms that apply to those files, or a notice indicating
401 | where to find the applicable terms.
402 | 
403 |   Additional terms, permissive or non-permissive, may be stated in the
404 | form of a separately written license, or stated as exceptions;
405 | the above requirements apply either way.
406 | 
407 |   8. Termination.
408 | 
409 |   You may not propagate or modify a covered work except as expressly
410 | provided under this License.  Any attempt otherwise to propagate or
411 | modify it is void, and will automatically terminate your rights under
412 | this License (including any patent licenses granted under the third
413 | paragraph of section 11).
414 | 
415 |   However, if you cease all violation of this License, then your
416 | license from a particular copyright holder is reinstated (a)
417 | provisionally, unless and until the copyright holder explicitly and
418 | finally terminates your license, and (b) permanently, if the copyright
419 | holder fails to notify you of the violation by some reasonable means
420 | prior to 60 days after the cessation.
421 | 
422 |   Moreover, your license from a particular copyright holder is
423 | reinstated permanently if the copyright holder notifies you of the
424 | violation by some reasonable means, this is the first time you have
425 | received notice of violation of this License (for any work) from that
426 | copyright holder, and you cure the violation prior to 30 days after
427 | your receipt of the notice.
428 | 
429 |   Termination of your rights under this section does not terminate the
430 | licenses of parties who have received copies or rights from you under
431 | this License.  If your rights have been terminated and not permanently
432 | reinstated, you do not qualify to receive new licenses for the same
433 | material under section 10.
434 | 
435 |   9. Acceptance Not Required for Having Copies.
436 | 
437 |   You are not required to accept this License in order to receive or
438 | run a copy of the Program.  Ancillary propagation of a covered work
439 | occurring solely as a consequence of using peer-to-peer transmission
440 | to receive a copy likewise does not require acceptance.  However,
441 | nothing other than this License grants you permission to propagate or
442 | modify any covered work.  These actions infringe copyright if you do
443 | not accept this License.  Therefore, by modifying or propagating a
444 | covered work, you indicate your acceptance of this License to do so.
445 | 
446 |   10. Automatic Licensing of Downstream Recipients.
447 | 
448 |   Each time you convey a covered work, the recipient automatically
449 | receives a license from the original licensors, to run, modify and
450 | propagate that work, subject to this License.  You are not responsible
451 | for enforcing compliance by third parties with this License.
452 | 
453 |   An "entity transaction" is a transaction transferring control of an
454 | organization, or substantially all assets of one, or subdividing an
455 | organization, or merging organizations.  If propagation of a covered
456 | work results from an entity transaction, each party to that
457 | transaction who receives a copy of the work also receives whatever
458 | licenses to the work the party's predecessor in interest had or could
459 | give under the previous paragraph, plus a right to possession of the
460 | Corresponding Source of the work from the predecessor in interest, if
461 | the predecessor has it or can get it with reasonable efforts.
462 | 
463 |   You may not impose any further restrictions on the exercise of the
464 | rights granted or affirmed under this License.  For example, you may
465 | not impose a license fee, royalty, or other charge for exercise of
466 | rights granted under this License, and you may not initiate litigation
467 | (including a cross-claim or counterclaim in a lawsuit) alleging that
468 | any patent claim is infringed by making, using, selling, offering for
469 | sale, or importing the Program or any portion of it.
470 | 
471 |   11. Patents.
472 | 
473 |   A "contributor" is a copyright holder who authorizes use under this
474 | License of the Program or a work on which the Program is based.  The
475 | work thus licensed is called the contributor's "contributor version".
476 | 
477 |   A contributor's "essential patent claims" are all patent claims
478 | owned or controlled by the contributor, whether already acquired or
479 | hereafter acquired, that would be infringed by some manner, permitted
480 | by this License, of making, using, or selling its contributor version,
481 | but do not include claims that would be infringed only as a
482 | consequence of further modification of the contributor version.  For
483 | purposes of this definition, "control" includes the right to grant
484 | patent sublicenses in a manner consistent with the requirements of
485 | this License.
486 | 
487 |   Each contributor grants you a non-exclusive, worldwide, royalty-free
488 | patent license under the contributor's essential patent claims, to
489 | make, use, sell, offer for sale, import and otherwise run, modify and
490 | propagate the contents of its contributor version.
491 | 
492 |   In the following three paragraphs, a "patent license" is any express
493 | agreement or commitment, however denominated, not to enforce a patent
494 | (such as an express permission to practice a patent or covenant not to
495 | sue for patent infringement).  To "grant" such a patent license to a
496 | party means to make such an agreement or commitment not to enforce a
497 | patent against the party.
498 | 
499 |   If you convey a covered work, knowingly relying on a patent license,
500 | and the Corresponding Source of the work is not available for anyone
501 | to copy, free of charge and under the terms of this License, through a
502 | publicly available network server or other readily accessible means,
503 | then you must either (1) cause the Corresponding Source to be so
504 | available, or (2) arrange to deprive yourself of the benefit of the
505 | patent license for this particular work, or (3) arrange, in a manner
506 | consistent with the requirements of this License, to extend the patent
507 | license to downstream recipients.  "Knowingly relying" means you have
508 | actual knowledge that, but for the patent license, your conveying the
509 | covered work in a country, or your recipient's use of the covered work
510 | in a country, would infringe one or more identifiable patents in that
511 | country that you have reason to believe are valid.
512 | 
513 |   If, pursuant to or in connection with a single transaction or
514 | arrangement, you convey, or propagate by procuring conveyance of, a
515 | covered work, and grant a patent license to some of the parties
516 | receiving the covered work authorizing them to use, propagate, modify
517 | or convey a specific copy of the covered work, then the patent license
518 | you grant is automatically extended to all recipients of the covered
519 | work and works based on it.
520 | 
521 |   A patent license is "discriminatory" if it does not include within
522 | the scope of its coverage, prohibits the exercise of, or is
523 | conditioned on the non-exercise of one or more of the rights that are
524 | specifically granted under this License.  You may not convey a covered
525 | work if you are a party to an arrangement with a third party that is
526 | in the business of distributing software, under which you make payment
527 | to the third party based on the extent of your activity of conveying
528 | the work, and under which the third party grants, to any of the
529 | parties who would receive the covered work from you, a discriminatory
530 | patent license (a) in connection with copies of the covered work
531 | conveyed by you (or copies made from those copies), or (b) primarily
532 | for and in connection with specific products or compilations that
533 | contain the covered work, unless you entered into that arrangement,
534 | or that patent license was granted, prior to 28 March 2007.
535 | 
536 |   Nothing in this License shall be construed as excluding or limiting
537 | any implied license or other defenses to infringement that may
538 | otherwise be available to you under applicable patent law.
539 | 
540 |   12. No Surrender of Others' Freedom.
541 | 
542 |   If conditions are imposed on you (whether by court order, agreement or
543 | otherwise) that contradict the conditions of this License, they do not
544 | excuse you from the conditions of this License.  If you cannot convey a
545 | covered work so as to satisfy simultaneously your obligations under this
546 | License and any other pertinent obligations, then as a consequence you may
547 | not convey it at all.  For example, if you agree to terms that obligate you
548 | to collect a royalty for further conveying from those to whom you convey
549 | the Program, the only way you could satisfy both those terms and this
550 | License would be to refrain entirely from conveying the Program.
551 | 
552 |   13. Use with the GNU Affero General Public License.
553 | 
554 |   Notwithstanding any other provision of this License, you have
555 | permission to link or combine any covered work with a work licensed
556 | under version 3 of the GNU Affero General Public License into a single
557 | combined work, and to convey the resulting work.  The terms of this
558 | License will continue to apply to the part which is the covered work,
559 | but the special requirements of the GNU Affero General Public License,
560 | section 13, concerning interaction through a network will apply to the
561 | combination as such.
562 | 
563 |   14. Revised Versions of this License.
564 | 
565 |   The Free Software Foundation may publish revised and/or new versions of
566 | the GNU General Public License from time to time.  Such new versions will
567 | be similar in spirit to the present version, but may differ in detail to
568 | address new problems or concerns.
569 | 
570 |   Each version is given a distinguishing version number.  If the
571 | Program specifies that a certain numbered version of the GNU General
572 | Public License "or any later version" applies to it, you have the
573 | option of following the terms and conditions either of that numbered
574 | version or of any later version published by the Free Software
575 | Foundation.  If the Program does not specify a version number of the
576 | GNU General Public License, you may choose any version ever published
577 | by the Free Software Foundation.
578 | 
579 |   If the Program specifies that a proxy can decide which future
580 | versions of the GNU General Public License can be used, that proxy's
581 | public statement of acceptance of a version permanently authorizes you
582 | to choose that version for the Program.
583 | 
584 |   Later license versions may give you additional or different
585 | permissions.  However, no additional obligations are imposed on any
586 | author or copyright holder as a result of your choosing to follow a
587 | later version.
588 | 
589 |   15. Disclaimer of Warranty.
590 | 
591 |   THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY
592 | APPLICABLE LAW.  EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT
593 | HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY
594 | OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO,
595 | THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
596 | PURPOSE.  THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM
597 | IS WITH YOU.  SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF
598 | ALL NECESSARY SERVICING, REPAIR OR CORRECTION.
599 | 
600 |   16. Limitation of Liability.
601 | 
602 |   IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING
603 | WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS
604 | THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY
605 | GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE
606 | USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF
607 | DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD
608 | PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS),
609 | EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF
610 | SUCH DAMAGES.
611 | 
612 |   17. Interpretation of Sections 15 and 16.
613 | 
614 |   If the disclaimer of warranty and limitation of liability provided
615 | above cannot be given local legal effect according to their terms,
616 | reviewing courts shall apply local law that most closely approximates
617 | an absolute waiver of all civil liability in connection with the
618 | Program, unless a warranty or assumption of liability accompanies a
619 | copy of the Program in return for a fee.
620 | 
621 |                      END OF TERMS AND CONDITIONS
622 | 
623 |             How to Apply These Terms to Your New Programs
624 | 
625 |   If you develop a new program, and you want it to be of the greatest
626 | possible use to the public, the best way to achieve this is to make it
627 | free software which everyone can redistribute and change under these terms.
628 | 
629 |   To do so, attach the following notices to the program.  It is safest
630 | to attach them to the start of each source file to most effectively
631 | state the exclusion of warranty; and each file should have at least
632 | the "copyright" line and a pointer to where the full notice is found.
633 | 
634 |     <one line to give the program's name and a brief idea of what it does.>
635 |     Copyright (C) <year>  <name of author>
636 | 
637 |     This program is free software: you can redistribute it and/or modify
638 |     it under the terms of the GNU General Public License as published by
639 |     the Free Software Foundation, either version 3 of the License, or
640 |     (at your option) any later version.
641 | 
642 |     This program is distributed in the hope that it will be useful,
643 |     but WITHOUT ANY WARRANTY; without even the implied warranty of
644 |     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
645 |     GNU General Public License for more details.
646 | 
647 |     You should have received a copy of the GNU General Public License
648 |     along with this program.  If not, see <https://www.gnu.org/licenses/>.
649 | 
650 | Also add information on how to contact you by electronic and paper mail.
651 | 
652 |   If the program does terminal interaction, make it output a short
653 | notice like this when it starts in an interactive mode:
654 | 
655 |     <program>  Copyright (C) <year>  <name of author>
656 |     This program comes with ABSOLUTELY NO WARRANTY; for details type `show w'.
657 |     This is free software, and you are welcome to redistribute it
658 |     under certain conditions; type `show c' for details.
659 | 
660 | The hypothetical commands `show w' and `show c' should show the appropriate
661 | parts of the General Public License.  Of course, your program's commands
662 | might be different; for a GUI interface, you would use an "about box".
663 | 
664 |   You should also get your employer (if you work as a programmer) or school,
665 | if any, to sign a "copyright disclaimer" for the program, if necessary.
666 | For more information on this, and how to apply and follow the GNU GPL, see
667 | <https://www.gnu.org/licenses/>.
668 | 
669 |   The GNU General Public License does not permit incorporating your program
670 | into proprietary programs.  If your program is a subroutine library, you
671 | may consider it more useful to permit linking proprietary applications with
672 | the library.  If this is what you want to do, use the GNU Lesser General
673 | Public License instead of this License.  But first, please read
674 | <https://www.gnu.org/licenses/why-not-lgpl.html>.
675 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
 1 | # teensySHOT
 2 | - DSHOT communication with ESC using Teensy 3.5 or Teensy 4.0 development boards
 3 | - DSHOT600 is generated using DMA
 4 | - Telemetry is received through UART at 115200bps
 5 | - Velocity PID control running at 500Hz is implemented for up to 6 motors
 6 | 
 7 | ## File Description
 8 | - ESCPID: code running on the Teensy
 9 | 	- DSHOT.cpp: DSHOT600 communication.
10 | 	- AWPID.cpp: Anti-Windup PID.
11 | 	- ESCCMD.cpp: Bidirectional communication between Teensy and ESC.
12 | 	- ESCPID.ino: main program.
13 | - host: code running on a Linux platform connected to the teensy. 
14 | - docs: useful documentation (MCU datasheets, DSHOT command description, telemetry protocol explanation, PID description...). 
15 | 
16 | ## API Description
17 | 
18 | ### Incoming data
19 | Velocity reference and PID parameters.
20 | ### Out-coming Data
21 | Data structure containing telemetry data, error code and last DSHOT command.
22 | ### Tunable Macros
23 | - ESCCMD_ESC_EMULATION (ESCCMD.cpp): flag to enable or disable ESC emulation. When enabled, the telemetry data is emulated. Packet loss can also be emulated. This helps to debug the code without ESC.
24 | - ESCPID_NB_ESC (ESCPID.h): define the number of ESCs that are handled by teensySHOT.
25 | - ESCPID_PID_MAX (ESCPID.h): Maximum PID control value. The maximum allowed is 999. A lower value may limit the motor maximum RPM in case of controller instability. Start testing with a low value and when everything is ok, raise it to the max. 
26 | 
27 | ## Wiring
28 | teensySHOT can communicate with up to 6 ESCs. The following pins are to be used with the current version. It is possible to change some DSHOT pins using muxing options of the MCU (tutorial in progress). 
29 | <table>
30 |   <tr> <td>   </td> <td colspan="2">  Teensy 3.5 </td> <td colspan="2">  Teensy 4.0 </td> </tr>
31 |   <tr> <td>   </td> <td> DSHOT </td> <td> TLM/RX </td> <td> DSHOT </td> <td> TLM/RX </td> </tr>
32 |   <tr> <td> 1 </td> <td>   22  </td> <td>    0   </td> <td>   4   </td> <td>    0   </td> </tr>
33 |   <tr> <td> 2 </td> <td>   23  </td> <td>    9   </td> <td>   8   </td> <td>    7   </td> </tr>
34 |   <tr> <td> 3 </td> <td>    6  </td> <td>    7   </td> <td>  24   </td> <td>   15   </td> </tr>
35 |   <tr> <td> 4 </td> <td>   20  </td> <td>   31   </td> <td>  22   </td> <td>   16   </td> </tr>
36 |   <tr> <td> 5 </td> <td>   21  </td> <td>   34   </td> <td>  23   </td> <td>   21   </td> </tr>
37 |   <tr> <td> 6 </td> <td>    5  </td> <td>   47   </td> <td>   9   </td> <td>   25   </td> </tr>
38 | </table>
39 | 
40 | ## Authors
41 | - [Arda Yiğit](mailto:arda.yigit@unistra.fr): DMA programming and ESC communication
42 | - [Jacques Gangloff](mailto:jacques.gangloff@unistra.fr): higher level API, debugging
43 | 


--------------------------------------------------------------------------------
/_config.yml:
--------------------------------------------------------------------------------
1 | theme: jekyll-theme-cayman


--------------------------------------------------------------------------------
/docs/BLHeli32 cmd.pdf:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/jacqu/teensyshot/ad9bcc7f93a9efcb3962df9a7b0297d0206e0e6f/docs/BLHeli32 cmd.pdf


--------------------------------------------------------------------------------
/docs/K64P144M120SF5RM.pdf:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/jacqu/teensyshot/ad9bcc7f93a9efcb3962df9a7b0297d0206e0e6f/docs/K64P144M120SF5RM.pdf


--------------------------------------------------------------------------------
/docs/KISS_telemetry.pdf:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/jacqu/teensyshot/ad9bcc7f93a9efcb3962df9a7b0297d0206e0e6f/docs/KISS_telemetry.pdf


--------------------------------------------------------------------------------
/docs/PID AW explained.pdf:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/jacqu/teensyshot/ad9bcc7f93a9efcb3962df9a7b0297d0206e0e6f/docs/PID AW explained.pdf


--------------------------------------------------------------------------------
/docs/PID.pdf:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/jacqu/teensyshot/ad9bcc7f93a9efcb3962df9a7b0297d0206e0e6f/docs/PID.pdf


--------------------------------------------------------------------------------
/docs/RT1060.pdf:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/jacqu/teensyshot/ad9bcc7f93a9efcb3962df9a7b0297d0206e0e6f/docs/RT1060.pdf


--------------------------------------------------------------------------------
/docs/RT1060_eFlexPWM.pdf:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/jacqu/teensyshot/ad9bcc7f93a9efcb3962df9a7b0297d0206e0e6f/docs/RT1060_eFlexPWM.pdf


--------------------------------------------------------------------------------
/docs/RT1060_pin_mux.pdf:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/jacqu/teensyshot/ad9bcc7f93a9efcb3962df9a7b0297d0206e0e6f/docs/RT1060_pin_mux.pdf


--------------------------------------------------------------------------------
/host/host.c:
--------------------------------------------------------------------------------
  1 | /*
  2 |  *  Communication with ESCPID code running on teensy 3.5
  3 |  *   JG, June 2019
  4 |  *   To compile : gcc -Wall -o host host.c
  5 |  */
  6 | 
  7 | #include <fcntl.h>
  8 | #include <termios.h>
  9 | #include <stdlib.h>
 10 | #include <stdio.h>
 11 | #include <stdint.h>
 12 | #include <unistd.h>
 13 | #include <time.h>
 14 | #include <string.h>
 15 | #include <strings.h>
 16 | #include <limits.h>
 17 | #include <dirent.h>
 18 | #include <sys/ioctl.h>
 19 | #include <sys/param.h>
 20 | #include <sys/types.h>
 21 | #include <sys/stat.h>
 22 | #if defined(__linux__)
 23 | #include <linux/serial.h>
 24 | #include <linux/input.h>
 25 | #endif
 26 | #include "host.h"
 27 | 
 28 | // Flags
 29 | #define HOST_STANDALONE                             // main is added
 30 | 
 31 | #define HOST_MAX_DEVICES    5                       // Max number of teensys
 32 | 
 33 | // Defines
 34 | // Note on USB port <-> devices relationship on RPI 3b+:
 35 | // Bottom, away from RJ45 : platform-3f980000.usb-usb-0:1.2:1.0
 36 | // Bottom, next RJ45      : platform-3f980000.usb-usb-0:1.1.3:1.0
 37 | // Top, away from RJ45    : platform-3f980000.usb-usb-0:1.3:1.0
 38 | // Top, next RJ45         : platform-3f980000.usb-usb-0:1.1.2:1.0
 39 | //#define HOST_MODEMDEVICE    "/dev/serial/by-path/platform-3f980000.usb-usb-0:1.2:1.0"
 40 | //#define HOST_MODEMDEVICE    "/dev/serial/by-id/usb-Teensyduino_USB_Serial_4367700-if00"
 41 | //#define HOST_MODEMDEVICE    "/dev/ttyACM0"
 42 | //#define HOST_MODEMDEVICE    "/dev/tty.usbmodem43677001"
 43 | //#define HOST_MODEMDEVICE      "/dev/tty.usbmodem54887501"
 44 | #define HOST_DEV_SERIALNB     4367700              	// Serial number of the teensy
 45 | //#define HOST_DEV_SERIALNB      54887501
 46 | #define HOST_DEV_SERIALLG     10                    // Max length of a serial number
 47 | 
 48 | #if defined(__linux__)
 49 | #define HOST_SERIAL_DEV_DIR   "/dev/serial/by-id/"
 50 | #elif defined(__APPLE__)
 51 | #define HOST_SERIAL_DEV_DIR   "/dev/"
 52 | #else
 53 | #define HOST_SERIAL_DEV_DIR   "/dev/"
 54 | #endif
 55 | 
 56 | #define HOST_BAUDRATE       B115200                 // Serial baudrate
 57 | #define HOST_READ_TIMEOUT   5                       // Tenth of second
 58 | #define HOST_NB_PING        1000                    // Nb roundtrip communication
 59 | #define HOST_STEP_REF       200                     // Velocity reference step size (10 rpm motor)
 60 | #define HOST_PERIOD         10000                   // Period of serial exchange (us)
 61 | #define HOST_STEP_PERIOD    100                     // Duration of a step (intertions)
 62 | 
 63 | // Globals
 64 | int                 Host_fd[HOST_MAX_DEVICES] = 
 65 |                           { HOST_ERROR_FD, 
 66 |                             HOST_ERROR_FD,
 67 |                             HOST_ERROR_FD,
 68 |                             HOST_ERROR_FD,
 69 |                             HOST_ERROR_FD };        // Serial port file descriptor
 70 |                             
 71 | char                Host_devname[HOST_MAX_DEVICES][PATH_MAX] =
 72 |                           { "",
 73 |                             "",
 74 |                             "",
 75 |                             "",
 76 |                             "" };                   // Serial port devname used to get fd with open
 77 |                             
 78 | struct termios      Host_oldtio[HOST_MAX_DEVICES];  // Backup of initial tty configuration
 79 | 
 80 | ESCPIDcomm_struct_t ESCPID_comm[HOST_MAX_DEVICES];
 81 | Hostcomm_struct_t   Host_comm[HOST_MAX_DEVICES];
 82 | 
 83 | //
 84 | //  Get the device name from the device serial number
 85 | //
 86 | char *Host_name_from_serial( uint32_t serial_nb ) {
 87 |   DIR           *d;
 88 |   struct dirent *dir;
 89 |   char          serial_nb_char[HOST_DEV_SERIALLG];
 90 |   static char   portname[PATH_MAX];
 91 |   
 92 |   // Convert serial number into string
 93 |   snprintf( serial_nb_char, HOST_DEV_SERIALLG, "%d", serial_nb );
 94 |   
 95 |   // Open directory where serial devices can be found
 96 |   d = opendir( HOST_SERIAL_DEV_DIR );
 97 |   
 98 |   // Look for a device name contining teensy serial number
 99 |   if ( d ) {
100 |   
101 |     // Scan each file in the directory
102 |     while ( ( dir = readdir( d ) ) != NULL ) {
103 |       if ( strstr( dir->d_name, serial_nb_char ) )  {
104 |       
105 |         // A match is a device name containing the serial number
106 |         snprintf( portname, PATH_MAX, "%s%s", HOST_SERIAL_DEV_DIR, dir->d_name );
107 |         return portname;
108 |       }
109 |     }
110 |     closedir( d );
111 |   }
112 |   
113 |   return NULL;
114 | }
115 | 
116 | //
117 | //  Get the file descriptor index which device name contains
118 | //  specified serial number. 
119 | //  Returns -1 if no matching fd is found.
120 | //
121 | int Host_get_fd( uint32_t serial_nb ) {
122 |   int   i;
123 |   char  serial_nb_char[HOST_DEV_SERIALLG];
124 |   
125 |   // Convert serial number into string
126 |   snprintf( serial_nb_char, HOST_DEV_SERIALLG, "%d", serial_nb );
127 |     
128 |   for ( i = 0; i < HOST_MAX_DEVICES; i++ )
129 |     if ( Host_fd[i] != HOST_ERROR_FD )
130 |         if ( strstr( Host_devname[i], serial_nb_char ) )
131 |           return i;
132 | 
133 |   return HOST_ERROR_FD;
134 | }
135 | 
136 | //
137 | //  Initialize serial port
138 | //
139 | int Host_init_port( uint32_t serial_nb )  {
140 |   struct  termios newtio;
141 |   int     check_fd;
142 |   int     i, fd_idx;
143 |   char    *portname;
144 |   
145 |   // Check if device plugged in
146 |   portname = Host_name_from_serial( serial_nb );
147 |   if ( !portname )
148 |     return HOST_ERROR_DEV;
149 | 
150 |   // Open device
151 |   check_fd = open( portname, O_RDWR | O_NOCTTY | O_NONBLOCK );
152 |   
153 |   if ( check_fd < 0 )  {
154 |     perror( portname );
155 |     return HOST_ERROR_DEV;
156 |   }
157 |   
158 |   // Look for an empty slot to store the fd
159 |   for ( fd_idx = 0; fd_idx < HOST_MAX_DEVICES; fd_idx++ )
160 |     if ( Host_fd[fd_idx] == HOST_ERROR_FD )
161 |       break;
162 |       
163 |   // Close fd and throw an error if all slots are used
164 |   if ( fd_idx == HOST_MAX_DEVICES ) {
165 |     close( check_fd );
166 |     return HOST_ERROR_MAX_DEV;
167 |   }
168 |   
169 |   // Store the fd and the corresponding devname
170 |   Host_fd[fd_idx] = check_fd;
171 |   strncpy( Host_devname[fd_idx], portname, PATH_MAX );
172 |   
173 |   // Initialize corresponding data structure
174 |   for ( i = 0; i < ESCPID_MAX_ESC; i++ )  {
175 |     Host_comm[fd_idx].magic =     ESCPID_COMM_MAGIC;
176 |     Host_comm[fd_idx].RPM_r[i] =  0;
177 |     Host_comm[fd_idx].PID_P[i] =  ESCPID_PID_P;
178 |     Host_comm[fd_idx].PID_I[i] =  ESCPID_PID_I;
179 |     Host_comm[fd_idx].PID_D[i] =  ESCPID_PID_D;
180 |     Host_comm[fd_idx].PID_f[i] =  ESCPID_PID_F;
181 |   }
182 | 
183 |   /* Save current port settings */
184 |   tcgetattr( check_fd, &Host_oldtio[fd_idx] );
185 | 
186 |   /* Define new settings */
187 |   bzero( &newtio, sizeof(newtio) );
188 |   cfmakeraw( &newtio );
189 | 
190 |   newtio.c_cflag =      HOST_BAUDRATE | CS8 | CLOCAL | CREAD;
191 |   newtio.c_iflag =      IGNPAR;
192 |   newtio.c_oflag =      0;
193 |   newtio.c_lflag =      0;
194 |   newtio.c_cc[VTIME] =  0;
195 |   newtio.c_cc[VMIN] =   0;
196 | 
197 |   #if defined(__APPLE__)
198 |   cfsetispeed( &newtio, HOST_BAUDRATE );
199 |   cfsetospeed( &newtio, HOST_BAUDRATE );
200 |   #endif
201 | 
202 |   /* Apply the settings */
203 |   tcflush( check_fd, TCIFLUSH );
204 |   tcsetattr( check_fd, TCSANOW, &newtio );
205 | 
206 |   return 0;
207 | }
208 | 
209 | //
210 | //  Release serial port
211 | //
212 | void Host_release_port( uint32_t serial_nb )  {
213 |   int   fd_idx;
214 |     
215 |   // Get fd index from serial number
216 |   fd_idx = Host_get_fd( serial_nb );
217 |   
218 |   if ( fd_idx != HOST_ERROR_FD ) {
219 |     // Restore initial settings if needed
220 |     tcsetattr( Host_fd[fd_idx], TCSANOW, &Host_oldtio[fd_idx] );
221 |     close( Host_fd[fd_idx] );
222 |     
223 |     // Clear fd and corresponding devname
224 |     Host_fd[fd_idx] = HOST_ERROR_FD;
225 |     strncpy( Host_devname[fd_idx], "", PATH_MAX );
226 |   }
227 | }
228 | 
229 | //
230 | // Manage communication with the teensy connected to portname
231 | //
232 | int Host_comm_update( uint32_t            serial_nb,
233 |                       int16_t             *RPM_r,
234 |                       uint16_t            *PID_P,
235 |                       uint16_t            *PID_I,
236 |                       uint16_t            *PID_D,
237 |                       uint16_t            *PID_f,
238 |                       ESCPIDcomm_struct_t **comm ) {
239 |                       
240 |   int                 i, ret, res = 0, fd_idx;
241 |   uint8_t             *pt_in;
242 |   struct timespec     start, cur;
243 |   unsigned long long  elapsed_us;
244 |   
245 |   // Get fd index
246 |   fd_idx = Host_get_fd( serial_nb );
247 |   
248 |   // Check if fd index is valid
249 |   if ( fd_idx == HOST_ERROR_FD )
250 |     return HOST_ERROR_FD;
251 |   
252 |   // Update output data structue
253 |   for ( i = 0; i < ESCPID_MAX_ESC; i++ )  {
254 |     Host_comm[fd_idx].RPM_r[i] = RPM_r[i];
255 |     Host_comm[fd_idx].PID_P[i] = PID_P[i];
256 |     Host_comm[fd_idx].PID_I[i] = PID_I[i];
257 |     Host_comm[fd_idx].PID_D[i] = PID_D[i];
258 |     Host_comm[fd_idx].PID_f[i] = PID_f[i];
259 |   }
260 |    
261 |   // Send output structure
262 |   res = write( Host_fd[fd_idx], &Host_comm[fd_idx], sizeof( Host_comm[fd_idx] ) );
263 |   if ( res < 0 )  {
264 |     perror( "write Host_comm" );
265 |     return HOST_ERROR_WRITE_SER;
266 |   }
267 |   
268 |   // Flush output buffer
269 |   fsync( Host_fd[fd_idx] );
270 | 
271 |   // Wait for response
272 | 
273 |   // Get current time
274 |   clock_gettime( CLOCK_MONOTONIC, &start );
275 | 
276 |   // Reset byte counter and magic number
277 |   res = 0;
278 |   ESCPID_comm[fd_idx].magic = 0;
279 |   pt_in = (uint8_t*)(&ESCPID_comm[fd_idx]);
280 | 
281 |   do  {
282 |     ret = read( Host_fd[fd_idx], &pt_in[res], 1 );
283 | 
284 |     // Data received
285 |     if ( ret > 0 )  {
286 |       res += ret;
287 |     }
288 | 
289 |     // Read error
290 |     if ( ret < 0 )
291 |       break;
292 | 
293 |     // Compute time elapsed
294 |     clock_gettime( CLOCK_MONOTONIC, &cur );
295 |     elapsed_us =  ( cur.tv_sec * 1e6 + cur.tv_nsec / 1e3 ) -
296 |                   ( start.tv_sec * 1e6 + start.tv_nsec / 1e3 );
297 | 
298 |     // Timeout
299 |     if ( elapsed_us / 100000 > HOST_READ_TIMEOUT )
300 |       break;
301 | 
302 |   } while ( res < sizeof( ESCPID_comm[fd_idx] ) );
303 | 
304 |   // Check response size
305 |   if ( res != sizeof( ESCPID_comm[fd_idx] ) )  {
306 |     fprintf( stderr, "Packet with bad size received.\n" );
307 | 
308 |     // Flush input buffer
309 |     while ( ( ret = read( Host_fd[fd_idx], pt_in, 1 ) ) )
310 |       if ( ret <= 0 )
311 |         break;
312 |         
313 |     return HOST_ERROR_BAD_PK_SZ;
314 |   }
315 | 
316 |   // Check magic number
317 |   if ( ESCPID_comm[fd_idx].magic !=  ESCPID_COMM_MAGIC )  {
318 |     fprintf( stderr, "Invalid magic number.\n" );
319 |     return HOST_ERROR_MAGIC;
320 |   }
321 |   
322 |   // Return pointer to ESCPID_comm structure
323 |   *comm = &ESCPID_comm[fd_idx];
324 |   
325 |   // Print rountrip duration
326 |   #ifdef HOST_STANDALONE
327 |   fprintf( stderr, "Delay: %llu us\n", elapsed_us );
328 |   #endif
329 | 
330 |   return 0;
331 | }
332 | 
333 | #ifdef HOST_STANDALONE
334 | //
335 | //  main
336 | //
337 | int main( int argc, char *argv[] )  {
338 | 
339 |   int                 i, k, ret;
340 |   int16_t             RPM_r[ESCPID_MAX_ESC];
341 |   uint16_t            PID_P[ESCPID_MAX_ESC];
342 |   uint16_t            PID_I[ESCPID_MAX_ESC];
343 |   uint16_t            PID_D[ESCPID_MAX_ESC];
344 |   uint16_t            PID_f[ESCPID_MAX_ESC];
345 |   ESCPIDcomm_struct_t *comm;
346 |   
347 |   // Initialize tunable PID data
348 |   for ( i = 0; i < ESCPID_MAX_ESC; i++ )  {
349 |     RPM_r[i] = HOST_STEP_REF;
350 |     PID_P[i] = ESCPID_PID_P;
351 |     PID_I[i] = ESCPID_PID_I;
352 |     PID_D[i] = ESCPID_PID_D;
353 |     PID_f[i] = ESCPID_PID_F;
354 |   }
355 |   
356 |   // Initialize serial port
357 |   if ( Host_init_port( HOST_DEV_SERIALNB ) )  {
358 |     fprintf( stderr, "Error initializing serial port.\n" );
359 |     exit( -1 );
360 |   }
361 | 
362 |   // Testing roundtrip serial link duration
363 |   for ( i = 0; i < HOST_NB_PING; i++ )  {
364 |   
365 |     // Serial exchange with teensy
366 |     if ( ( ret = Host_comm_update(  HOST_DEV_SERIALNB,
367 |                                     RPM_r,
368 |                                     PID_P,
369 |                                     PID_I,
370 |                                     PID_D,
371 |                                     PID_f,
372 |                                     &comm ) ) )  {
373 |       fprintf( stderr, "Error %d in Host_comm_update.\n", ret );
374 |       break;
375 |     }
376 |     
377 |     // Display telemetry
378 |     for ( k = 0; k < ESCPID_NB_ESC; k++ )
379 |       fprintf(  stderr,
380 |                 "#:%d.%d\terr:%d\tdeg:%d\tcmd:%d\tmV:%d\tmA:%d\trpm_r:%d\trpm:%d\n",
381 |                 i,
382 |                 k,
383 |                 comm->err[k],
384 |                 comm->deg[k],
385 |                 comm->cmd[k],
386 |                 comm->volt[k] * 10,
387 |                 comm->amp[k] * 10,
388 |                 RPM_r[k] * 10,
389 |                 comm->rpm[k] * 10 );
390 |     
391 |     // Update reference
392 |     if ( !( i % HOST_STEP_PERIOD ) )
393 |       for ( k = 0; k < ESCPID_MAX_ESC; k++ )
394 |         RPM_r[k] *= -1;
395 |               
396 |     // Wait loop period
397 |     usleep( HOST_PERIOD );
398 |   }
399 | 
400 |   // Restoring serial port initial configuration
401 |   Host_release_port( HOST_DEV_SERIALNB );
402 | 
403 |   return 0;
404 | }
405 | #endif
406 | 


--------------------------------------------------------------------------------
/host/host.h:
--------------------------------------------------------------------------------
 1 | /*
 2 |  *  Definitions for host.c
 3 |  */
 4 |  
 5 | #ifndef __HOST_H
 6 | #define __HOST_H
 7 | 
 8 | #include "../ESCPID/ESCPID.h"
 9 | 
10 | // Defines
11 | #define HOST_ERROR_FD         -1        // Non existant file descriptor
12 | #define HOST_ERROR_DEV        -2        // Non existant serial device
13 | #define HOST_ERROR_MAX_DEV    -3        // Maximum devices limit 
14 | #define HOST_ERROR_WRITE_SER  -4        // Write error on serial
15 | #define HOST_ERROR_BAD_PK_SZ  -5        // Bad incoming packet size error
16 | #define HOST_ERROR_MAGIC      -6        // Bad magic number received
17 | 
18 | // Prototypes
19 | char *Host_name_from_serial(  uint32_t );
20 | int   Host_get_fd(            uint32_t );
21 | int   Host_init_port(         uint32_t );
22 | void  Host_release_port(      uint32_t );
23 | int   Host_comm_update(       uint32_t, 
24 |                               int16_t*, 
25 |                               uint16_t*, 
26 |                               uint16_t*, 
27 |                               uint16_t*, 
28 |                               uint16_t*,
29 |                               ESCPIDcomm_struct_t** );
30 | 
31 | #endif


--------------------------------------------------------------------------------