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 }