Implemented the cm510_read and cm510_write functions in the simulator..

bioloid_controller_cm510/src/sim/serial_console.cpp

@@ -67,7 +67,7 @@ int cm510_printf(const char *fmt, ...)
 int cm510_scanf(const char *fmt, ...)
 {
   va_list ap;
-  int done,num;;
+  int done,num;
 
   if(rs232_enabled)
   {
@@ -101,6 +101,39 @@ int cm510_scanf(const char *fmt, ...)
 
 }
 
+uint8_t cm510_read(uint8_t *data,uint8_t len)
+{
+  int num,read=0;
+  CEventServer *event_server=CEventServer::instance();
+  std::list<std::string> events;
+
+  try{
+    events.push_back(serial_console.get_rx_event_id());
+    do{
+      if((num=serial_console.get_num_data())==0)
+      {
+        event_server->wait_first(events,1000);
+        num=serial_console.get_num_data();
+      }
+      if(num>len)
+        serial_console.read((unsigned char *)data,len);
+      else
+        serial_console.read((unsigned char *)&data[read],num);
+      read+=num;
+    }while(read<len);
+  }catch(CException &e){
+    std::cout << e.what() << std::endl;
+  }
+
+  return read;
+}
+
+uint8_t cm510_write(uint8_t *data,uint8_t len)
+{
+  return serial_console.write((unsigned char *)data,len);
+}
+
+
 /* pubic functions */
 void serial_console_init(uint32_t baudrate)
 {