Blake West / DSHOT150

DSHOT150 algorithm using digital IO pins on the LPC-1768

Dependents:   DSHOT

DSHOT150.cpp@0:230008e7b5c8, 2019-04-27 (annotated)

Committer:
bwest32
Date:
Sat Apr 27 21:51:38 2019 +0000
Revision:
0:230008e7b5c8
First implementation of DSHOT150 on LPC-1768

Who changed what in which revision?

UserRevisionLine numberNew contents of line
bwest32 0:230008e7b5c8 1 #include "DSHOT150.h"
bwest32 0:230008e7b5c8 2 #include "mbed.h"
bwest32 0:230008e7b5c8 3 DSHOT150::DSHOT150(PinName pin) : _pin(pin)
bwest32 0:230008e7b5c8 4 {
bwest32 0:230008e7b5c8 5 _pin = 0;
bwest32 0:230008e7b5c8 6 tel = 0;
bwest32 0:230008e7b5c8 7 }
bwest32 0:230008e7b5c8 8 void DSHOT150::write_zero()
bwest32 0:230008e7b5c8 9 {
bwest32 0:230008e7b5c8 10 int i = 0;
bwest32 0:230008e7b5c8 11 while (i<19) {
bwest32 0:230008e7b5c8 12 _pin.write(1);
bwest32 0:230008e7b5c8 13 i++;
bwest32 0:230008e7b5c8 14 }
bwest32 0:230008e7b5c8 15 while (i<51) {
bwest32 0:230008e7b5c8 16 _pin.write(0);
bwest32 0:230008e7b5c8 17 i++;
bwest32 0:230008e7b5c8 18 }
bwest32 0:230008e7b5c8 19 }
bwest32 0:230008e7b5c8 20 void DSHOT150::write_one()
bwest32 0:230008e7b5c8 21 {
bwest32 0:230008e7b5c8 22 int i = 0;
bwest32 0:230008e7b5c8 23 while (i<40) {
bwest32 0:230008e7b5c8 24 _pin.write(1);
bwest32 0:230008e7b5c8 25 i++;
bwest32 0:230008e7b5c8 26 }
bwest32 0:230008e7b5c8 27 while (i<51) {
bwest32 0:230008e7b5c8 28 _pin.write(0);
bwest32 0:230008e7b5c8 29 i++;
bwest32 0:230008e7b5c8 30 }
bwest32 0:230008e7b5c8 31 }
bwest32 0:230008e7b5c8 32 void DSHOT150::check_sum(unsigned int v)
bwest32 0:230008e7b5c8 33 {
bwest32 0:230008e7b5c8 34 v = v<<1;
bwest32 0:230008e7b5c8 35 v = v|tel;
bwest32 0:230008e7b5c8 36 uint8_t cs;
bwest32 0:230008e7b5c8 37 for( int i = 0; i < 3; ++i){
bwest32 0:230008e7b5c8 38 cs ^= v;
bwest32 0:230008e7b5c8 39 v>>=4;
bwest32 0:230008e7b5c8 40 }
bwest32 0:230008e7b5c8 41 cs&=0xF;
bwest32 0:230008e7b5c8 42 packet[15] = cs&0x1;
bwest32 0:230008e7b5c8 43 packet[14] = cs&0x2;
bwest32 0:230008e7b5c8 44 packet[13] = cs&0x4;
bwest32 0:230008e7b5c8 45 packet[12] = cs&0x8;
bwest32 0:230008e7b5c8 46 }
bwest32 0:230008e7b5c8 47 void DSHOT150::get_tel(bool v)
bwest32 0:230008e7b5c8 48 {
bwest32 0:230008e7b5c8 49 tel = v == true? 1 : 0;
bwest32 0:230008e7b5c8 50 }
bwest32 0:230008e7b5c8 51 void DSHOT150::send_packet()
bwest32 0:230008e7b5c8 52 {
bwest32 0:230008e7b5c8 53 for(int j = 0; j < 1000; ++j) {
bwest32 0:230008e7b5c8 54 for(int i = 0; i < 16; ++i) {
bwest32 0:230008e7b5c8 55 if(packet[i])
bwest32 0:230008e7b5c8 56 write_one();
bwest32 0:230008e7b5c8 57 else
bwest32 0:230008e7b5c8 58 write_zero();
bwest32 0:230008e7b5c8 59 }
bwest32 0:230008e7b5c8 60 wait_us(500);
bwest32 0:230008e7b5c8 61 }
bwest32 0:230008e7b5c8 62 }
bwest32 0:230008e7b5c8 63 void DSHOT150::arm(){
bwest32 0:230008e7b5c8 64 throttle(0.25);
bwest32 0:230008e7b5c8 65 throttle(0);
bwest32 0:230008e7b5c8 66 }
bwest32 0:230008e7b5c8 67 void DSHOT150::throttle(float speed)
bwest32 0:230008e7b5c8 68 {
bwest32 0:230008e7b5c8 69 unsigned int val;
bwest32 0:230008e7b5c8 70 speed = speed > 1 ? 1 : speed; //Bound checking and restricitng of the input
bwest32 0:230008e7b5c8 71 speed = speed < 0 ? 0 : speed; //Anything below 0 is converted to zero
bwest32 0:230008e7b5c8 72 //Anything above 1 is converted to one
bwest32 0:230008e7b5c8 73
bwest32 0:230008e7b5c8 74 val = (unsigned int)(speed * 2000); //Scale the throttle value. 0 - 48 are reserved for the motor
bwest32 0:230008e7b5c8 75 val +=48; //Throttle of zero starts at 48
bwest32 0:230008e7b5c8 76
bwest32 0:230008e7b5c8 77 check_sum(val); //Calculate the checksum and insert it into the packet
bwest32 0:230008e7b5c8 78
bwest32 0:230008e7b5c8 79 for(int i = 10; i >= 0; --i) { //Insert the throttle bits into the packet
bwest32 0:230008e7b5c8 80 packet[i] = val&0x1;
bwest32 0:230008e7b5c8 81 val = val>>1;
bwest32 0:230008e7b5c8 82 }
bwest32 0:230008e7b5c8 83
bwest32 0:230008e7b5c8 84 packet[11] = tel; //Set the telemetry bit in the packet
bwest32 0:230008e7b5c8 85
bwest32 0:230008e7b5c8 86 send_packet();
bwest32 0:230008e7b5c8 87 }