Austin Buchan / mbed_zumy_packet

Publishing for Biomimetics.

Dependencies:   CRC MODDMA MODSERIAL MPU6050IMU PID QEI mbed-rtos mbed-src

packet_parser.cpp@0:8cfa73bb68e4, 2016-05-31 (annotated)

Committer:
abuchan
Date:
Tue May 31 17:04:59 2016 +0000
Revision:
0:8cfa73bb68e4
Publishing for Biomimetics.

Who changed what in which revision?

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