BLDC control for jumping robot.
Dependencies: CRC MODSERIAL mbed-dev mbed-rtos
Fork of mbed_BLDC_driver_KL25Z by
packet_parser.cpp@2:aa0bdbe1fe80, 2016-09-28 (annotated)
- 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?
User | Revision | Line number | New 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 | } |