DSHOT150 algorithm using digital IO pins on the LPC-1768
Embed:
(wiki syntax)
Show/hide line numbers
DSHOT150.cpp
00001 #include "DSHOT150.h" 00002 #include "mbed.h" 00003 DSHOT150::DSHOT150(PinName pin) : _pin(pin) 00004 { 00005 _pin = 0; 00006 tel = 0; 00007 } 00008 void DSHOT150::write_zero() 00009 { 00010 int i = 0; 00011 while (i<19) { 00012 _pin.write(1); 00013 i++; 00014 } 00015 while (i<51) { 00016 _pin.write(0); 00017 i++; 00018 } 00019 } 00020 void DSHOT150::write_one() 00021 { 00022 int i = 0; 00023 while (i<40) { 00024 _pin.write(1); 00025 i++; 00026 } 00027 while (i<51) { 00028 _pin.write(0); 00029 i++; 00030 } 00031 } 00032 void DSHOT150::check_sum(unsigned int v) 00033 { 00034 v = v<<1; 00035 v = v|tel; 00036 uint8_t cs; 00037 for( int i = 0; i < 3; ++i){ 00038 cs ^= v; 00039 v>>=4; 00040 } 00041 cs&=0xF; 00042 packet[15] = cs&0x1; 00043 packet[14] = cs&0x2; 00044 packet[13] = cs&0x4; 00045 packet[12] = cs&0x8; 00046 } 00047 void DSHOT150::get_tel(bool v) 00048 { 00049 tel = v == true? 1 : 0; 00050 } 00051 void DSHOT150::send_packet() 00052 { 00053 for(int j = 0; j < 1000; ++j) { 00054 for(int i = 0; i < 16; ++i) { 00055 if(packet[i]) 00056 write_one(); 00057 else 00058 write_zero(); 00059 } 00060 wait_us(500); 00061 } 00062 } 00063 void DSHOT150::arm(){ 00064 throttle(0.25); 00065 throttle(0); 00066 } 00067 void DSHOT150::throttle(float speed) 00068 { 00069 unsigned int val; 00070 speed = speed > 1 ? 1 : speed; //Bound checking and restricitng of the input 00071 speed = speed < 0 ? 0 : speed; //Anything below 0 is converted to zero 00072 //Anything above 1 is converted to one 00073 00074 val = (unsigned int)(speed * 2000); //Scale the throttle value. 0 - 48 are reserved for the motor 00075 val +=48; //Throttle of zero starts at 48 00076 00077 check_sum(val); //Calculate the checksum and insert it into the packet 00078 00079 for(int i = 10; i >= 0; --i) { //Insert the throttle bits into the packet 00080 packet[i] = val&0x1; 00081 val = val>>1; 00082 } 00083 00084 packet[11] = tel; //Set the telemetry bit in the packet 00085 00086 send_packet(); 00087 }
Generated on Mon Aug 8 2022 12:24:01 by 1.7.2