BLDC control for jumping robot.

Dependencies:   CRC MODSERIAL mbed-dev mbed-rtos

Fork of mbed_BLDC_driver_KL25Z by Duncan Haldane

Committer:
abuchan
Date:
Wed Sep 28 23:54:47 2016 +0000
Revision:
2:aa0bdbe1fe80
Adapting to uses packet based UART communication.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
abuchan 2:aa0bdbe1fe80 1 #include "packet_parser.h"
abuchan 2:aa0bdbe1fe80 2
abuchan 2:aa0bdbe1fe80 3
abuchan 2:aa0bdbe1fe80 4 Thread* global_thread_ = NULL;
abuchan 2:aa0bdbe1fe80 5
abuchan 2:aa0bdbe1fe80 6 void dma_complete_signal(MODSERIAL_IRQ_INFO *q) {
abuchan 2:aa0bdbe1fe80 7 if (global_thread_ != NULL) {
abuchan 2:aa0bdbe1fe80 8 global_thread_->signal_set(DMA_COMPLETE_FLAG);
abuchan 2:aa0bdbe1fe80 9 }
abuchan 2:aa0bdbe1fe80 10 }
abuchan 2:aa0bdbe1fe80 11
abuchan 2:aa0bdbe1fe80 12 PacketParser::PacketParser(
abuchan 2:aa0bdbe1fe80 13 uint32_t baudrate, PinName tx_pin, PinName rx_pin, PinName tx_led, PinName rx_led) :
abuchan 2:aa0bdbe1fe80 14 pc_(tx_pin, rx_pin), /*dma_(),*/
abuchan 2:aa0bdbe1fe80 15 tx_led_(tx_led), send_thread_(&PacketParser::thread_starter, this),
abuchan 2:aa0bdbe1fe80 16 rx_led_(rx_led) {
abuchan 2:aa0bdbe1fe80 17
abuchan 2:aa0bdbe1fe80 18 pc_.baud(baudrate);
abuchan 2:aa0bdbe1fe80 19 //pc_.MODDMA(&dma_);
abuchan 2:aa0bdbe1fe80 20 //pc_.attach_dmaSendComplete(this, &PacketParser::send_complete);
abuchan 2:aa0bdbe1fe80 21 //pc_.attach_dmaSendComplete(&dma_complete_signal);
abuchan 2:aa0bdbe1fe80 22 global_thread_ = &send_thread_;
abuchan 2:aa0bdbe1fe80 23
abuchan 2:aa0bdbe1fe80 24 out_pkt_ = NULL;
abuchan 2:aa0bdbe1fe80 25 tx_sequence_ = 0;
abuchan 2:aa0bdbe1fe80 26
abuchan 2:aa0bdbe1fe80 27 pc_.attach(this, &PacketParser::receive_callback, MODSERIAL::RxIrq);
abuchan 2:aa0bdbe1fe80 28 in_pkt_ = (packet_union_t*)in_box_.alloc();
abuchan 2:aa0bdbe1fe80 29 in_pkt_idx_ = 0;
abuchan 2:aa0bdbe1fe80 30 in_pkt_len_ = MAX_PACKET_LENGTH;
abuchan 2:aa0bdbe1fe80 31 in_pkt_crc_ = 0;
abuchan 2:aa0bdbe1fe80 32
abuchan 2:aa0bdbe1fe80 33 send_thread_.signal_set(START_THREAD_FLAG);
abuchan 2:aa0bdbe1fe80 34 }
abuchan 2:aa0bdbe1fe80 35
abuchan 2:aa0bdbe1fe80 36 packet_union_t* PacketParser::get_received_packet(void) {
abuchan 2:aa0bdbe1fe80 37 osEvent evt = in_box_.get(0);
abuchan 2:aa0bdbe1fe80 38 if (evt.status == osEventMail) {
abuchan 2:aa0bdbe1fe80 39 return (packet_union_t*)evt.value.p;
abuchan 2:aa0bdbe1fe80 40 } else {
abuchan 2:aa0bdbe1fe80 41 return NULL;
abuchan 2:aa0bdbe1fe80 42 }
abuchan 2:aa0bdbe1fe80 43 }
abuchan 2:aa0bdbe1fe80 44
abuchan 2:aa0bdbe1fe80 45 void PacketParser::free_received_packet(packet_union_t* packet) {
abuchan 2:aa0bdbe1fe80 46 in_box_.free(packet);
abuchan 2:aa0bdbe1fe80 47 }
abuchan 2:aa0bdbe1fe80 48
abuchan 2:aa0bdbe1fe80 49 packet_union_t* PacketParser::get_send_packet(void) {
abuchan 2:aa0bdbe1fe80 50 return (packet_union_t*)out_box_.alloc();
abuchan 2:aa0bdbe1fe80 51 }
abuchan 2:aa0bdbe1fe80 52
abuchan 2:aa0bdbe1fe80 53 void PacketParser::send_packet(packet_union_t* packet) {
abuchan 2:aa0bdbe1fe80 54 out_box_.put(packet);
abuchan 2:aa0bdbe1fe80 55 }
abuchan 2:aa0bdbe1fe80 56
abuchan 2:aa0bdbe1fe80 57 void PacketParser::thread_starter(void const *p) {
abuchan 2:aa0bdbe1fe80 58 PacketParser* instance = (PacketParser*)p;
abuchan 2:aa0bdbe1fe80 59 instance->send_worker();
abuchan 2:aa0bdbe1fe80 60 }
abuchan 2:aa0bdbe1fe80 61
abuchan 2:aa0bdbe1fe80 62 void PacketParser::send_worker(void) {
abuchan 2:aa0bdbe1fe80 63 send_thread_.signal_wait(START_THREAD_FLAG);
abuchan 2:aa0bdbe1fe80 64 while(true) {
abuchan 2:aa0bdbe1fe80 65 osEvent evt = out_box_.get();
abuchan 2:aa0bdbe1fe80 66 if (evt.status == osEventMail) {
abuchan 2:aa0bdbe1fe80 67 tx_led_ = 1;
abuchan 2:aa0bdbe1fe80 68 out_pkt_ = (packet_union_t*)evt.value.p;
abuchan 2:aa0bdbe1fe80 69 out_pkt_->packet.header.start = 0;
abuchan 2:aa0bdbe1fe80 70 out_pkt_->packet.header.sequence = tx_sequence_++;
abuchan 2:aa0bdbe1fe80 71 uint8_t crc_value = calculate_crc8(out_pkt_->raw, out_pkt_->packet.header.length-1);
abuchan 2:aa0bdbe1fe80 72 out_pkt_->raw[out_pkt_->packet.header.length-1] = crc_value;
abuchan 2:aa0bdbe1fe80 73 //pc_.dmaSend(out_pkt_->raw, out_pkt_->packet.header.length);
abuchan 2:aa0bdbe1fe80 74 for (int i = 0; i < out_pkt_->packet.header.length; i++) {
abuchan 2:aa0bdbe1fe80 75 pc_.putc(out_pkt_->raw[i]);
abuchan 2:aa0bdbe1fe80 76 }
abuchan 2:aa0bdbe1fe80 77 tx_led_ = 0;
abuchan 2:aa0bdbe1fe80 78 send_thread_.signal_wait(DMA_COMPLETE_FLAG);
abuchan 2:aa0bdbe1fe80 79 tx_led_ = 1;
abuchan 2:aa0bdbe1fe80 80 send_thread_.signal_clr(DMA_COMPLETE_FLAG);
abuchan 2:aa0bdbe1fe80 81 out_box_.free(out_pkt_);
abuchan 2:aa0bdbe1fe80 82 out_pkt_ = NULL;
abuchan 2:aa0bdbe1fe80 83 tx_led_ = 0;
abuchan 2:aa0bdbe1fe80 84 }
abuchan 2:aa0bdbe1fe80 85 Thread::yield();
abuchan 2:aa0bdbe1fe80 86 }
abuchan 2:aa0bdbe1fe80 87 }
abuchan 2:aa0bdbe1fe80 88
abuchan 2:aa0bdbe1fe80 89 void PacketParser::send_complete(MODSERIAL_IRQ_INFO *q) {
abuchan 2:aa0bdbe1fe80 90 tx_led_ = 1;
abuchan 2:aa0bdbe1fe80 91 if (out_pkt_ != NULL) {
abuchan 2:aa0bdbe1fe80 92 out_box_.free(out_pkt_);
abuchan 2:aa0bdbe1fe80 93 out_pkt_ = NULL;
abuchan 2:aa0bdbe1fe80 94 }
abuchan 2:aa0bdbe1fe80 95 tx_led_ = 0;
abuchan 2:aa0bdbe1fe80 96 }
abuchan 2:aa0bdbe1fe80 97
abuchan 2:aa0bdbe1fe80 98 void PacketParser::receive_callback(MODSERIAL_IRQ_INFO *q) {
abuchan 2:aa0bdbe1fe80 99 rx_led_ = 1;
abuchan 2:aa0bdbe1fe80 100 MODSERIAL* serial = q->serial;
abuchan 2:aa0bdbe1fe80 101
abuchan 2:aa0bdbe1fe80 102 if (in_pkt_ != NULL) {
abuchan 2:aa0bdbe1fe80 103 while(serial->readable()) {
abuchan 2:aa0bdbe1fe80 104 char c = serial->getc();
abuchan 2:aa0bdbe1fe80 105
abuchan 2:aa0bdbe1fe80 106 // If we just received the second character, set packet length
abuchan 2:aa0bdbe1fe80 107 if (in_pkt_idx_ == 1) {
abuchan 2:aa0bdbe1fe80 108 in_pkt_len_ = c;
abuchan 2:aa0bdbe1fe80 109 }
abuchan 2:aa0bdbe1fe80 110
abuchan 2:aa0bdbe1fe80 111 // If there has been a parse error, reset packet buffer
abuchan 2:aa0bdbe1fe80 112 if ((in_pkt_idx_ == 0 && c != 0) || in_pkt_len_ < sizeof(header_t)+1 ) {
abuchan 2:aa0bdbe1fe80 113 in_pkt_idx_ = 0;
abuchan 2:aa0bdbe1fe80 114 in_pkt_len_ = MAX_PACKET_LENGTH;
abuchan 2:aa0bdbe1fe80 115 in_pkt_crc_ = 0;
abuchan 2:aa0bdbe1fe80 116
abuchan 2:aa0bdbe1fe80 117 // Store byte in packet buffer and update CRC
abuchan 2:aa0bdbe1fe80 118 } else {
abuchan 2:aa0bdbe1fe80 119 in_pkt_->raw[in_pkt_idx_++] = c;
abuchan 2:aa0bdbe1fe80 120 in_pkt_crc_ = update_crc8(in_pkt_crc_, c);
abuchan 2:aa0bdbe1fe80 121 }
abuchan 2:aa0bdbe1fe80 122
abuchan 2:aa0bdbe1fe80 123 // If we just received the last character, put valid packets in mailbox
abuchan 2:aa0bdbe1fe80 124 // and reset packet buffer
abuchan 2:aa0bdbe1fe80 125 if (in_pkt_idx_ == in_pkt_len_) {
abuchan 2:aa0bdbe1fe80 126 if (in_pkt_crc_ == 0) {
abuchan 2:aa0bdbe1fe80 127 in_box_.put(in_pkt_);
abuchan 2:aa0bdbe1fe80 128 in_pkt_ = (packet_union_t*)in_box_.alloc();
abuchan 2:aa0bdbe1fe80 129 }
abuchan 2:aa0bdbe1fe80 130 in_pkt_idx_ = 0;
abuchan 2:aa0bdbe1fe80 131 in_pkt_len_ = MAX_PACKET_LENGTH;
abuchan 2:aa0bdbe1fe80 132 in_pkt_crc_ = 0;
abuchan 2:aa0bdbe1fe80 133 }
abuchan 2:aa0bdbe1fe80 134 }
abuchan 2:aa0bdbe1fe80 135 } else {
abuchan 2:aa0bdbe1fe80 136 in_pkt_ = (packet_union_t*)in_box_.alloc();
abuchan 2:aa0bdbe1fe80 137 }
abuchan 2:aa0bdbe1fe80 138
abuchan 2:aa0bdbe1fe80 139 rx_led_ = 0;
abuchan 2:aa0bdbe1fe80 140 }