dshot.cpp

00001 #include "dshot.h"
00002 
00003 
00004 
00005 uint8_t dshotCmdBuffer[DSHOT_BUFFER_LENGTH];
00006 uint16_t prevDshotThrottle = 0;
00007 bool dshotThrottleChange = false;
00008 
00009 void resetDshotThrottleChange(void){
00010     dshotThrottleChange = false;
00011 }
00012 
00013 bool checkDshotThrottleChange(void){
00014     return dshotThrottleChange;
00015 }
00016 
00017 uint16_t getDshotThrottle(void){
00018     return prevDshotThrottle;
00019 }
00020     
00021 uint8_t calcDshotCRC(uint16_t dshotPacket){
00022     uint8_t crc = 0;
00023     
00024     for (int i = 0; i<3; i++) {
00025         crc ^= dshotPacket;
00026         dshotPacket >>= 4;
00027     }
00028     
00029     crc &= 0xf;
00030     return crc;
00031 }
00032 
00033 void setupDshotCmdBuffer(uint16_t dshotCmd){
00034     uint32_t modifier = ( + DSHOT_CLK / 2) / DSHOT_CLK;
00035     memset(dshotCmdBuffer, 0 , DSHOT_BUFFER_LENGTH);
00036     
00037     for(int i=0; i<DSHOT_CMD_LENGTH; i++){ // scan all the bits in the packet
00038         if( (bool)((1 << i) & dshotCmd)){
00039             dshotCmdBuffer[15-i] = (modifier * DSHOT_1_TIME) >> 8; // pack buffer MSB first
00040         } else {
00041             dshotCmdBuffer[15-i] = (modifier * DSHOT_0_TIME) >> 8; // pack buffer MSB first
00042         }
00043     }
00044 }
00045 
00046 void dshotOutput(uint16_t dshotValue){
00047     uint16_t packet = 0;
00048     uint8_t crc = 0;
00049     
00050     //checks bounds for dshot throttle value
00051     if (dshotValue < 48) dshotValue = 48;
00052     else if (dshotValue > 2047) dshotValue = 2047;
00053     
00054     packet = dshotValue << 1;
00055     crc = calcDshotCRC(packet);
00056     packet = (packet << 4) | crc;
00057     setupDshotCmdBuffer(packet);
00058     //writeDshotCmd();
00059 }
00060 
00061 void setDshotThrottle(uint16_t throttle){
00062     prevDshotThrottle = throttle;
00063     dshotThrottleChange = true;
00064     dshotOutput(prevDshotThrottle + 48);
00065 }