Revision 2:aa0bdbe1fe80, committed 2016-09-28

Comitter:

abuchan

Date:

Wed Sep 28 23:54:47 2016 +0000

Parent:

1:d68c51a0a706

Commit message:

Adapting to uses packet based UART communication.

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/CRC.lib	Wed Sep 28 23:54:47 2016 +0000
@@ -0,0 +1,1 @@
+https://developer.mbed.org/users/abuchan/code/CRC/#f9af06c940b6

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MODSERIAL.lib	Wed Sep 28 23:54:47 2016 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/Sissors/code/MODSERIAL/#4737f8a5b018

--- a/main.cpp	Wed Sep 28 16:59:46 2016 +0000
+++ b/main.cpp	Wed Sep 28 23:54:47 2016 +0000
@@ -1,38 +1,99 @@
 #include "mbed.h"
+
 #include "enc.h"
+#include "protocol.h"
+#include "packet_parser.h"
+
+#define LED1 PTC2
+#define LED2 PTC3
+#define LED3 PTA18
+
+#define SERIAL_BAUDRATE 230400
+#define UART_TX PTD7
+#define UART_RX PTD6
 
 //Ticker RTI;
-Serial imProc(PTD7,PTD6);
-DigitalOut led_1(PTC2);
-DigitalOut led_2(PTC3);
-DigitalOut led_3(PTA18);
+//Serial imProc(UART_TX,UART_RX);
+DigitalOut led_1(LED1);
+//DigitalOut led_2(LED2);
+//DigitalOut led_3(LED3);
 enc motPos(PTC6,PTC7,PTC5,PTC4);
 PwmOut EN(PTA4);
 DigitalOut DR(PTA2);
 DigitalOut BRAKE(PTA1);
 AnalogIn motCurrent(PTB0);
 AnalogIn temp(PTE30);
-
+AnalogIn voltage(PTD5);
 
 void get_state(){
-    motPos.update_pos();
+  motPos.update_pos();
+}
+
+void fill_sensor_packet(packet_t* pkt, int32_t position, int32_t velocity, uint16_t current, uint16_t voltage, uint16_t temperature) {
+  pkt->header.type = PKT_TYPE_SENSOR;
+  pkt->header.length = sizeof(header_t) + sizeof(sensor_data_t) + 1;
+  sensor_data_t* sensor_data = (sensor_data_t*)pkt->data_crc;
+  sensor_data->position = position;
+  sensor_data->velocity = velocity;
+  sensor_data->current = current;
+  sensor_data->voltage = voltage;
+  sensor_data->temperature = temperature;
 }
 
 int main() {  
-        EN.period_us(50);
-    EN.write(0.9f);
-    DR.write(0);
-    BRAKE.write(1);
-      motPos.set_offset(2160);
-      led_1 = 1;
-//    RTI.attach(&get_state, 0.01f);
-    while(1) {
+  EN.period_us(50);
+  EN.write(0.9f);
+  DR.write(0);
+  BRAKE.write(1);
+  motPos.set_offset(2160);
+  led_1 = 1;
+//  RTI.attach(&get_state, 0.01f);
+
+  
+  Timer system_timer;
+  system_timer.start();
+  uint32_t last_time = system_timer.read_ms();
+  uint32_t current_time = last_time;
+  
+  
+  PacketParser parser(SERIAL_BAUDRATE, USBTX, USBRX, LED2, LED3);
+  packet_union_t* recv_pkt = NULL;
+  command_data_t* command;
+  
+  packet_union_t* sensor_pkt = parser.get_send_packet();
+  
+  while(1) {
+    recv_pkt = parser.get_received_packet();
+
+    if (recv_pkt != NULL) {
+      
+      switch (recv_pkt->packet.header.type) {
         
-        led_2 = 1;
-        led_3 = 0;
-        wait(0.5);
-        led_2 = 0;
-        led_3 = 1;
-        wait(0.5);
+        case PKT_TYPE_COMMAND:
+          command = (command_data_t*)recv_pkt->packet.data_crc;
+          if (sensor_pkt != NULL) {
+            fill_sensor_packet(&(sensor_pkt->packet),
+              motPos.ams_read(),
+              0,
+              motCurrent.read_u16(),
+              voltage.read_u16(),
+              temp.read_u16()
+            );
+            parser.send_packet(sensor_pkt);
+            sensor_pkt = parser.get_send_packet();
+          }
+          break;
+      }
+      
+      parser.free_received_packet(recv_pkt);
     }
+    
+    current_time = system_timer.read_ms();
+    if (current_time - last_time > 500) {
+      last_time = current_time;
+      led_1 = !led_1;
+    }
+  
+    Thread::yield();
+  }
 }

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed-dev.lib	Wed Sep 28 23:54:47 2016 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed-dev/#30b64687e01f

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed-rtos.lib	Wed Sep 28 23:54:47 2016 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed-rtos/#3da5f554d8bf

--- a/mbed-src.lib	Wed Sep 28 16:59:46 2016 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-http://mbed.org/users/mbed_official/code/mbed-src/#a11c0372f0ba

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/packet_parser.cpp	Wed Sep 28 23:54:47 2016 +0000
@@ -0,0 +1,140 @@
+#include "packet_parser.h"
+
+
+Thread* global_thread_ = NULL;
+
+void dma_complete_signal(MODSERIAL_IRQ_INFO *q) {
+  if (global_thread_ != NULL) {
+    global_thread_->signal_set(DMA_COMPLETE_FLAG);
+  }
+}
+
+PacketParser::PacketParser(
+  uint32_t baudrate, PinName tx_pin, PinName rx_pin, PinName tx_led, PinName rx_led) :
+  pc_(tx_pin, rx_pin), /*dma_(),*/
+  tx_led_(tx_led), send_thread_(&PacketParser::thread_starter, this),
+  rx_led_(rx_led) {
+  
+  pc_.baud(baudrate);
+  //pc_.MODDMA(&dma_);
+  //pc_.attach_dmaSendComplete(this, &PacketParser::send_complete);
+  //pc_.attach_dmaSendComplete(&dma_complete_signal);
+  global_thread_ = &send_thread_;
+
+  out_pkt_ = NULL;
+  tx_sequence_ = 0;
+
+  pc_.attach(this, &PacketParser::receive_callback, MODSERIAL::RxIrq);
+  in_pkt_ = (packet_union_t*)in_box_.alloc();
+  in_pkt_idx_ = 0;
+  in_pkt_len_ = MAX_PACKET_LENGTH;
+  in_pkt_crc_ = 0;
+  
+  send_thread_.signal_set(START_THREAD_FLAG);
+}
+
+packet_union_t* PacketParser::get_received_packet(void) {
+  osEvent evt = in_box_.get(0);
+  if (evt.status == osEventMail) {
+    return (packet_union_t*)evt.value.p;
+  } else {
+    return NULL;
+  }
+}
+
+void PacketParser::free_received_packet(packet_union_t* packet) {
+  in_box_.free(packet);
+}
+
+packet_union_t* PacketParser::get_send_packet(void) {
+  return (packet_union_t*)out_box_.alloc();
+}
+
+void PacketParser::send_packet(packet_union_t* packet) {
+  out_box_.put(packet);
+}
+
+void PacketParser::thread_starter(void const *p) {
+  PacketParser* instance = (PacketParser*)p;
+  instance->send_worker();
+}
+
+void PacketParser::send_worker(void) {
+  send_thread_.signal_wait(START_THREAD_FLAG);
+  while(true) {
+    osEvent evt = out_box_.get();
+    if (evt.status == osEventMail) {
+      tx_led_ = 1;
+      out_pkt_ = (packet_union_t*)evt.value.p;
+      out_pkt_->packet.header.start = 0;
+      out_pkt_->packet.header.sequence = tx_sequence_++;
+      uint8_t crc_value = calculate_crc8(out_pkt_->raw, out_pkt_->packet.header.length-1);
+      out_pkt_->raw[out_pkt_->packet.header.length-1] = crc_value;
+      //pc_.dmaSend(out_pkt_->raw, out_pkt_->packet.header.length);
+      for (int i = 0; i < out_pkt_->packet.header.length; i++) {
+        pc_.putc(out_pkt_->raw[i]);
+      }
+      tx_led_ = 0;
+      send_thread_.signal_wait(DMA_COMPLETE_FLAG);
+      tx_led_ = 1;
+      send_thread_.signal_clr(DMA_COMPLETE_FLAG);
+      out_box_.free(out_pkt_);
+      out_pkt_ = NULL;
+      tx_led_ = 0;
+    }
+    Thread::yield();
+  }
+}
+
+void PacketParser::send_complete(MODSERIAL_IRQ_INFO *q) {
+  tx_led_ = 1;
+  if (out_pkt_ != NULL) {
+    out_box_.free(out_pkt_);
+    out_pkt_ = NULL;
+  }
+  tx_led_ = 0;
+}
+
+void PacketParser::receive_callback(MODSERIAL_IRQ_INFO *q) {
+  rx_led_ = 1;
+  MODSERIAL* serial = q->serial;
+
+  if (in_pkt_ != NULL) {
+    while(serial->readable()) {
+      char c = serial->getc();
+
+      // If we just received the second character, set packet length
+      if (in_pkt_idx_ == 1) {
+        in_pkt_len_ = c;
+      }
+
+      // If there has been a parse error, reset packet buffer
+      if ((in_pkt_idx_ == 0 && c != 0) || in_pkt_len_ < sizeof(header_t)+1 ) {
+        in_pkt_idx_ = 0;
+        in_pkt_len_ = MAX_PACKET_LENGTH;
+        in_pkt_crc_ = 0;
+
+      // Store byte in packet buffer and update CRC
+      } else {
+        in_pkt_->raw[in_pkt_idx_++] = c;
+        in_pkt_crc_ = update_crc8(in_pkt_crc_, c);
+      }
+
+      // If we just received the last character, put valid packets in mailbox
+      // and reset packet buffer
+      if (in_pkt_idx_ == in_pkt_len_) {
+        if (in_pkt_crc_ == 0) {
+          in_box_.put(in_pkt_);
+          in_pkt_ = (packet_union_t*)in_box_.alloc();
+        }
+        in_pkt_idx_ = 0;
+        in_pkt_len_ = MAX_PACKET_LENGTH;
+        in_pkt_crc_ = 0;
+      }
+    }
+  } else {
+    in_pkt_ = (packet_union_t*)in_box_.alloc();
+  }
+
+  rx_led_ = 0;
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/packet_parser.h	Wed Sep 28 23:54:47 2016 +0000
@@ -0,0 +1,90 @@
+#ifndef PACKET_PARSER_H
+#define PACKET_PARSER_H
+
+#include "mbed.h"
+
+//#include "MODDMA.h"
+#include "MODSERIAL.h"
+#include "rtos.h"
+#include "lib_crc.h"
+
+#include "protocol.h"
+
+/**
+ * Defines the number of packets in the incoming and outgoing buffers.
+ */
+#define PACKET_BUFFER_LENGTH 4
+
+/**
+ * Thread flag to start work.
+ */
+#define START_THREAD_FLAG (1<<0)
+#define DMA_COMPLETE_FLAG (1<<1)
+
+class PacketParser {
+  
+  public:
+
+    /**
+     * Constructor.
+     *
+     * Creates a packet parsing protocol on the USB serial connection.
+     *
+     */
+    PacketParser(uint32_t baudrate, PinName tx_pin, PinName rx_pin, PinName tx_led, PinName rx_led);
+
+    /**
+     * Get a pointer to the next received packet, or NULL if there is no packet.
+     */
+    packet_union_t* get_received_packet(void);
+
+    /**
+     * Return a received packet to the packet pool. Must be called after using
+     * a packet from get_received_packet.
+     *
+     * @param packet - pointer to packet to be freed.
+     */
+    void free_received_packet(packet_union_t* packet);
+
+    /**
+     * Get a pointer to a packet to be sent. Will return NULL if there are no
+     * available outgoing packets.
+     */
+    packet_union_t* get_send_packet(void);
+
+    /**
+     * Send the packet returned by get_send_packet.
+     *
+     * @param packet - pointer to packet to be sent.
+     */
+    void send_packet(packet_union_t* packet);
+
+  private:
+
+    MODSERIAL pc_;
+    //MODDMA dma_;
+    
+    DigitalOut tx_led_;
+    uint32_t tx_sequence_;
+
+    Mail<packet_union_t, PACKET_BUFFER_LENGTH> out_box_;
+    packet_union_t* out_pkt_;
+    
+    static void thread_starter(void const *p);
+    Thread send_thread_;
+    
+    void send_worker(void);
+    void send_complete(MODSERIAL_IRQ_INFO *q);
+
+    DigitalOut rx_led_;
+    Mail<packet_union_t, PACKET_BUFFER_LENGTH> in_box_;
+    
+    packet_union_t* in_pkt_;
+    uint32_t in_pkt_idx_;
+    uint32_t in_pkt_len_;
+    uint8_t in_pkt_crc_;
+
+    void receive_callback(MODSERIAL_IRQ_INFO *q);
+};
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/protocol.h	Wed Sep 28 23:54:47 2016 +0000
@@ -0,0 +1,51 @@
+#ifndef PROTOCOL_H
+#define PROTOCOL_H
+
+#include <stdint.h>
+/**
+ * Packet type characters.
+ */
+#define PKT_TYPE_COMMAND  'C'
+#define PKT_TYPE_SENSOR   'S'
+
+/**
+ * Defines the total maximum size of a packet, including header
+ */
+#define MAX_PACKET_LENGTH 256
+
+/**
+ * Packet structure definitions
+ */
+typedef struct header_t {
+  uint8_t start;
+  uint8_t length;
+  char type;
+  uint8_t flags;
+  uint32_t sequence;
+} header_t;
+
+typedef struct packet_t {
+  header_t header;
+  uint8_t data_crc[MAX_PACKET_LENGTH-sizeof(header_t)];
+} packet_t;
+
+typedef union packet_union_t {
+  packet_t packet;
+  char raw[MAX_PACKET_LENGTH];
+} packet_union_t;
+
+typedef struct command_data_t {
+  int32_t position_setpoint;
+  uint16_t current_setpoint;
+} command_data_t;
+
+typedef struct sensor_data_t {
+  uint32_t time;
+  int32_t position;
+  int32_t velocity;
+  uint16_t current;
+  uint16_t voltage;
+  uint16_t temperature;
+} sensor_data_t;
+
+#endif
\ No newline at end of file