From 775ee44f6deb047cbce6669349bd3942090870bb Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Sergi=20Hern=C3=A1ndez?= <shernand@iri.upc.edu>
Date: Thu, 20 Oct 2016 23:04:07 +0200
Subject: [PATCH] Implemented the cm510_read and cm510_write functions in the
simulator..
---
.../src/sim/serial_console.cpp | 35 ++++++++++++++++++-
1 file changed, 34 insertions(+), 1 deletion(-)
diff --git a/bioloid_controller_cm510/src/sim/serial_console.cpp b/bioloid_controller_cm510/src/sim/serial_console.cpp
index 676bae7..891cecb 100644
--- a/bioloid_controller_cm510/src/sim/serial_console.cpp
+++ b/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)
{
--
GitLab