DSHOT150 algorithm using digital IO pins on the LPC-1768
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?
User | Revision | Line number | New 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 | } |