Kevin Tseng / Mbed 2 deprecated DSHOT_test

Test

Dependencies:   mbed MODDMA

dshot.cpp@1:e2cd5b82d2e1, 2019-04-25 (annotated)

Committer:
ktseng
Date:
Thu Apr 25 19:21:16 2019 +0000
Revision:
1:e2cd5b82d2e1
Parent:
0:96e88f67730b 
Test

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ktseng 0:96e88f67730b 1 #include "dshot.h"
ktseng 0:96e88f67730b 2
ktseng 0:96e88f67730b 3
ktseng 0:96e88f67730b 4
ktseng 0:96e88f67730b 5 uint8_t dshotCmdBuffer[DSHOT_BUFFER_LENGTH];
ktseng 0:96e88f67730b 6 uint16_t prevDshotThrottle = 0;
ktseng 0:96e88f67730b 7 bool dshotThrottleChange = false;
ktseng 0:96e88f67730b 8
ktseng 0:96e88f67730b 9 void resetDshotThrottleChange(void){
ktseng 0:96e88f67730b 10 dshotThrottleChange = false;
ktseng 0:96e88f67730b 11 }
ktseng 0:96e88f67730b 12
ktseng 0:96e88f67730b 13 bool checkDshotThrottleChange(void){
ktseng 0:96e88f67730b 14 return dshotThrottleChange;
ktseng 0:96e88f67730b 15 }
ktseng 0:96e88f67730b 16
ktseng 0:96e88f67730b 17 uint16_t getDshotThrottle(void){
ktseng 0:96e88f67730b 18 return prevDshotThrottle;
ktseng 0:96e88f67730b 19 }
ktseng 0:96e88f67730b 20
ktseng 0:96e88f67730b 21 uint8_t calcDshotCRC(uint16_t dshotPacket){
ktseng 0:96e88f67730b 22 uint8_t crc = 0;
ktseng 0:96e88f67730b 23
ktseng 0:96e88f67730b 24 for (int i = 0; i<3; i++) {
ktseng 0:96e88f67730b 25 crc ^= dshotPacket;
ktseng 0:96e88f67730b 26 dshotPacket >>= 4;
ktseng 0:96e88f67730b 27 }
ktseng 0:96e88f67730b 28
ktseng 0:96e88f67730b 29 crc &= 0xf;
ktseng 0:96e88f67730b 30 return crc;
ktseng 0:96e88f67730b 31 }
ktseng 0:96e88f67730b 32
ktseng 0:96e88f67730b 33 void setupDshotCmdBuffer(uint16_t dshotCmd){
ktseng 0:96e88f67730b 34 uint32_t modifier = ( + DSHOT_CLK / 2) / DSHOT_CLK;
ktseng 0:96e88f67730b 35 memset(dshotCmdBuffer, 0 , DSHOT_BUFFER_LENGTH);
ktseng 0:96e88f67730b 36
ktseng 0:96e88f67730b 37 for(int i=0; i<DSHOT_CMD_LENGTH; i++){ // scan all the bits in the packet
ktseng 0:96e88f67730b 38 if( (bool)((1 << i) & dshotCmd)){
ktseng 0:96e88f67730b 39 dshotCmdBuffer[15-i] = (modifier * DSHOT_1_TIME) >> 8; // pack buffer MSB first
ktseng 0:96e88f67730b 40 } else {
ktseng 0:96e88f67730b 41 dshotCmdBuffer[15-i] = (modifier * DSHOT_0_TIME) >> 8; // pack buffer MSB first
ktseng 0:96e88f67730b 42 }
ktseng 0:96e88f67730b 43 }
ktseng 0:96e88f67730b 44 }
ktseng 0:96e88f67730b 45
ktseng 0:96e88f67730b 46 void dshotOutput(uint16_t dshotValue){
ktseng 0:96e88f67730b 47 uint16_t packet = 0;
ktseng 0:96e88f67730b 48 uint8_t crc = 0;
ktseng 0:96e88f67730b 49
ktseng 0:96e88f67730b 50 //checks bounds for dshot throttle value
ktseng 0:96e88f67730b 51 if (dshotValue < 48) dshotValue = 48;
ktseng 0:96e88f67730b 52 else if (dshotValue > 2047) dshotValue = 2047;
ktseng 0:96e88f67730b 53
ktseng 0:96e88f67730b 54 packet = dshotValue << 1;
ktseng 0:96e88f67730b 55 crc = calcDshotCRC(packet);
ktseng 0:96e88f67730b 56 packet = (packet << 4) | crc;
ktseng 0:96e88f67730b 57 setupDshotCmdBuffer(packet);
ktseng 0:96e88f67730b 58 //writeDshotCmd();
ktseng 0:96e88f67730b 59 }
ktseng 0:96e88f67730b 60
ktseng 0:96e88f67730b 61 void setDshotThrottle(uint16_t throttle){
ktseng 0:96e88f67730b 62 prevDshotThrottle = throttle;
ktseng 0:96e88f67730b 63 dshotThrottleChange = true;
ktseng 0:96e88f67730b 64 dshotOutput(prevDshotThrottle + 48);
ktseng 0:96e88f67730b 65 }