Rewrite PSoC4 Uart::read() to avoid spurious buffer-empty errs & drop…

…ped chars; make arduino pubsub example

rosserial_arduino/msg/Nh_diag.msg

@@ -0,0 +1,2 @@
+uint16[8] nh_diag
+

rosserial_arduino/src/ros_lib/examples/pubsub_test_ar/pubsub_test_ar.ino

@@ -0,0 +1,60 @@
+/*
+ * rosserial PubSub Example
+ * Prints "hello world!" and toggles led
+ */
+
+#include <ros.h>
+#include <std_msgs/String.h>
+#include <std_msgs/Empty.h>
+#include <rosserial_arduino/Nh_diag.h>
+
+
+ros::NodeHandle  nh;
+
+
+bool ledOn;
+void messageCb( const std_msgs::Empty& toggle_msg){
+  ledOn = !ledOn;   // blink the led
+  digitalWrite(13, ledOn ? HIGH : LOW);
+}
+
+ros::Subscriber<std_msgs::Empty> sub("toggle_led", messageCb );
+
+uint32_t next_report_time;
+const uint32_t kReportIntervalMs = 1000;
+
+std_msgs::String str_msg;
+ros::Publisher chatter("chatter", &str_msg);
+
+rosserial_arduino::Nh_diag diag_msg;
+ros::Publisher nh_diag("nh_diag", &diag_msg);
+
+char hello[13] = "hello world!";
+
+int32_t loop_count;
+
+void setup()
+{
+  ledOn = false;
+  pinMode(13,OUTPUT);
+  nh.initNode();
+  nh.advertise(chatter);
+  nh.advertise(nh_diag);
+  nh.subscribe(sub);
+  next_report_time = millis();
+}
+
+void loop()
+{
+  if ((int32_t)(millis()-next_report_time) > 0) {
+    next_report_time += kReportIntervalMs;
+    str_msg.data = hello;
+    chatter.publish( &str_msg );
+    for (int i=0; i<8; ++i) {
+      diag_msg.nh_diag[i] = nh.nh_diagnostics[i];
+      nh.nh_diagnostics[i] = 0;
+    }
+    nh_diag.publish( &diag_msg );
+  }
+  nh.spinOnce();
+}

rosserial_psoc4/msg/Nh_diag.msg

@@ -0,0 +1,2 @@
+uint16[8] nh_diag
+

rosserial_psoc4/src/Cypress/Makefile

@@ -0,0 +1,96 @@
+# Makefile for building rosserial PSoC4 example apps
+# 
+# Set APP= to a name from catkin_ws/src/rosserial/rosserial_psoc4/src/ros_lib/examples
+# Run make from within the catkin_ws/src/rosserial/rosserial_psoc4/src/Cypress directory
+#  then copy output files to the Windows PSoC Creator environment:
+#    exec.ihx to <project name>.cydsn/exec.ihx
+#    makehex.bat to <project name>.cydsn/makehex.bat
+#  From <project name>.cydsn/ , run makehex.bat
+#  Use PSoC Creator "program" function (ctrl-F5) to program the Pioneer Kit board.
+
+# name of example application
+APP=pubsub_test
+
+CREATORDIR=./rosserial_psoc4/$(APP).cydsn
+$(info Using creator directory $(CREATORDIR))
+
+TOOLSDIR=/usr/share/arduino-1.5.4/hardware/tools
+
+PROJECT=$(APP)
+ODIR=./out/
+GENSRCDIR=$(CREATORDIR)/Generated_Source/PSoC4/
+COMPONENTLIB=./CyComponentLibrary.a
+PROJLIB=$(ODIR)$(PROJECT).a
+
+CC=$(TOOLSDIR)/g++_arm_none_eabi/bin/arm-none-eabi-gcc
+AR=$(TOOLSDIR)/g++_arm_none_eabi/bin/arm-none-eabi-ar
+ASM=$(TOOLSDIR)/g++_arm_none_eabi/bin/arm-none-eabi-as
+OBJCOPY=$(TOOLSDIR)/g++_arm_none_eabi/bin/arm-none-eabi-objcopy
+SIZE=$(TOOLSDIR)/g++_arm_none_eabi/bin/arm-none-eabi-size
+
+PROJLIBCFLAGS= -I. -Wno-main -I$(GENSRCDIR) -mcpu=cortex-m0 -mthumb -Wall -g -D DEBUG -ffunction-sections
+PROJLIBASMFLAGS= -I. -I$(GENSRCDIR) -mcpu=cortex-m0 -mthumb -g 
+LDFLAGS=-mthumb -march=armv6-m -T $(GENSRCDIR)cm0gcc.ld -g -Wl,-Map,$(ODIR)$(PROJECT).map -Wl,--gc-sections 
+
+PROJLIBSOURCES:=$(wildcard $(GENSRCDIR)*.c) $(GENSRCDIR)CyBootAsmGnu.s
+PROJLIBOBJS_C:=$(patsubst $(GENSRCDIR)%.c, $(ODIR)%.o, $(filter $(GENSRCDIR)%.c, $(PROJLIBSOURCES)))
+PROJLIBOBJS_C:=$(filter-out $(ODIR)Cm0Start.o, $(PROJLIBOBJS_C))
+PROJLIBOBJS_S:=$(patsubst $(GENSRCDIR)%.s, $(ODIR)%.o, $(filter $(GENSRCDIR)%.s, $(PROJLIBSOURCES)))
+
+SOURCEDIR=./libraries/ros_lib/
+SOURCES:=$(wildcard $(SOURCEDIR)*.cpp)
+OBJECTS:=$(patsubst $(SOURCEDIR)%.cpp, $(ODIR)%.o, $(SOURCES))
+OBJECTS:=$(filter-out $(ODIR)init.o $(ODIR)Uarts.o, $(OBJECTS))
+
+APPDIR=$(SOURCEDIR)examples/$(APP)/
+
+CXXFLAGS=  -I./libraries/ros_lib/ -Wno-main -mcpu=cortex-m0 -mthumb -Wall -g -D DEBUG -ffunction-sections -fno-rtti -fno-exceptions
+
+
+all: exec.ihx
+
+$(PROJLIBOBJS_C):$(ODIR)%.o:$(GENSRCDIR)%.c
+	#patch capitalization errors in Cypress API code templates
+	sed -i 's/\(#include[ \t]\+"\)cyLib.h"/\1CyLib.h"/' $<
+	sed -i 's/\(#include[ \t]\+<\)CYLIB.H>/\1CyLib.h>/' $<
+	sed -i 's/\(#include[ \t]\+<\)CYDEVICE_TRM.H>/\1cydevice_trm.h>/' $<
+	sed -i 's/\(#include[ \t]\+<\)UART_SCB_IRQ.H>/\1UART_SCB_IRQ.h>/' $<
+	sed -i 's/\(#include[ \t]\+<\)ADC_SAR_SEQ_IRQ.H>/\1ADC_SAR_SEQ_IRQ.h>/' $<
+	$(CC) $(PROJLIBCFLAGS) -c $< -o $@
+
+$(PROJLIBOBJS_S):$(ODIR)%.o:$(GENSRCDIR)%.s
+	$(ASM) $(PROJLIBASMFLAGS) -o $@ $< 
+
+$(PROJLIB):  $(PROJLIBOBJS_C) $(PROJLIBOBJS_S)
+	$(AR) -rs $@ $^
+
+$(ODIR)Cm0Start.o:$(GENSRCDIR)Cm0Start.c
+	$(CC) $(PROJLIBCFLAGS) -c $< -o $@
+
+$(ODIR)init.o:$(SOURCEDIR)init.cpp
+	$(CC) -I$(CREATORDIR) -I$(GENSRCDIR) $(CXXFLAGS) -Wa,-alh=$*.lst -c $< -o $@
+
+$(ODIR)Uarts.o:$(SOURCEDIR)Uarts.cpp
+	$(CC) -I$(CREATORDIR) -I$(GENSRCDIR) $(CXXFLAGS) -Wa,-alh=$*.lst -c $< -o $@
+
+$(ODIR)$(APP).o:$(APPDIR)$(APP).cpp
+	$(CC) -I$(CREATORDIR) -I$(GENSRCDIR) $(CXXFLAGS) -Wa,-alh=$*.lst -c $< -o $@
+
+$(OBJECTS):$(ODIR)%.o:$(SOURCEDIR)%.cpp
+	$(CC) $(CXXFLAGS) -Wa,-alh=$(ODIR)$*.lst -c $< -o $@
+
+exec.elf: $(OBJECTS) $(PROJLIB) $(ODIR)Cm0Start.o $(ODIR)init.o $(ODIR)Uarts.o $(ODIR)$(APP).o
+	$(CC) $(LDFLAGS) -Wl,--start-group  -o $@ $(OBJECTS) $(ODIR)$(APP).o $(ODIR)Cm0Start.o $(ODIR)init.o $(ODIR)Uarts.o $(PROJLIB) $(COMPONENTLIB) -Wl,--end-group
+
+exec.ihx: exec.elf
+	$(OBJCOPY) -O ihex $< $@
+	echo cyhextool -o ./CortexM0/ARM_GCC_441/Debug/$(APP).hex -f exec.ihx -prot ./Generated_Source/PSoC4/protect.hex -id 4C81193 -a EEPROM=90200000:0,PROGRAM=00000000:8000,CONFIG=80000000:1000,PROTECT=90400000:40 -meta 1101 -chipProt 01 | tr "/" "\\\\" > makehex.bat
+#	cp exec.ihx $(WINDOWS_SHARE)$(APP).cydsn
+#	cp makehex.bat $(WINDOWS_SHARE)$(APP).cydsn
+	$(SIZE) $<
+
+.PHONY: clean
+
+clean:
+	rm -rf $(ODIR)*
+

rosserial_psoc4/src/ros_lib/Uarts.cpp

@@ -0,0 +1,47 @@
+/* ========================================
+ *
+ * The following firmware was developed by Chuck Harrison
+ * This work is licensed under a Creative Commons Attribution 3.0 Unported License.
+ * 
+ * http://creativecommons.org/licenses/by/3.0/deed.en_US
+ * 
+ * You are free to:
+ * -To Share — to copy, distribute and transmit the work 
+ * -To Remix — to adapt the work 
+ * -To make commercial use of the work
+ *
+ * ========================================
+ */
+
+#include "ros.h"
+
+#include "Uarts.h"
+extern "C" {
+#include "device.h"
+}
+
+void Uart::begin(unsigned long baud) {
+  UART_Start();
+}; // TBD: set baud rate
+
+int Uart::read(void) {
+  int32_t data = -1;
+  if (UART_SpiUartGetRxBufferSize() != 0) {
+    // data is ready
+    data = (int32_t)UART_SpiUartReadRxData();
+  }
+  // clear error flags
+  if (UART_CHECK_INTR_RX(UART_INTR_RX_ERR)) {
+    // TODO: test for meaningful error (i.e. not Buffer empty) & return -1
+    UART_ClearRxInterruptSource(UART_INTR_RX_ERR);
+  }
+  return data;
+};
+
+size_t Uart::write(uint8_t data) {
+  UART_SpiUartWriteTxData((uint32_t)data);
+  return 1;
+};
+
+Uart Uart0;
+

rosserial_psoc4/src/ros_lib/Uarts.h

@@ -0,0 +1,32 @@
+/* ========================================
+ *
+ * The following firmware was developed by Chuck Harrison
+ * This work is licensed under a Creative Commons Attribution 3.0 Unported License.
+ * 
+ * http://creativecommons.org/licenses/by/3.0/deed.en_US
+ * 
+ * You are free to:
+ * -To Share — to copy, distribute and transmit the work 
+ * -To Remix — to adapt the work 
+ * -To make commercial use of the work
+ *
+ * ========================================
+ */
+
+#ifndef ROS_UARTS_H_
+#define ROS_UARTS_H_
+
+#include "HardwareSerial.h"
+
+class Uart : public HardwareSerial
+{
+  public:
+    void begin(unsigned long baud); // TBD: set baud rate
+    int read(void);
+    size_t write(uint8_t data);
+} ;
+
+extern Uart Uart0;
+
+#endif
+

rosserial_psoc4/src/ros_lib/examples/pubsub_test/BlueLed_psoc4.h

@@ -0,0 +1,22 @@
+/* include file for rosserial pubsub example on PSoC4 : led output
+ *
+ */
+
+#ifndef BLUELED_PSOC4_H
+#define BLUELED_PSOC4_H
+
+extern "C" {
+#include "device.h"
+}
+
+
+void blueLed_setup() {
+  Pin_BlueLED_SetDriveMode(Pin_BlueLED_DM_STRONG);
+}
+
+void setBlueLed(bool on) {
+  Pin_BlueLED_Write(on ? 0 : 0xFF);
+}
+
+#endif
+

rosserial_psoc4/src/ros_lib/examples/pubsub_test/UartStatus_psoc4.h

@@ -0,0 +1,21 @@
+/* include file for rosserial pubsub example on PSoC4 : uart status check
+ *
+ */
+
+#ifndef UARTSTATUS_PSOC4_H
+#define UARTSTATUS_PSOC4_H
+
+extern "C" {
+#include "device.h"
+}
+
+
+uint32_t getUartIntStatus() {
+  return UART_GetRxInterruptSource();
+}
+
+uint8_t getUartRxBufOvf() {
+  return UART_rxBufferOverflow;
+}
+#endif
+

rosserial_psoc4/src/ros_lib/examples/pubsub_test/pubsub_test.cpp

@@ -0,0 +1,79 @@
+/*
+ * rosserial PubSub Example
+ * Prints "hello world!" and toggles led
+ */
+
+#include <ros.h>
+#include <std_msgs/String.h>
+#include <std_msgs/Empty.h>
+#include <std_msgs/UInt32.h>
+#include <rosserial_psoc4/Nh_diag.h>
+
+#include "BlueLed_psoc4.h"
+#include "UartStatus_psoc4.h"
+#include "isnprintf.h"
+
+
+ros::NodeHandle  nh;
+
+
+bool ledOn;
+void messageCb( const std_msgs::Empty& toggle_msg){
+  ledOn = !ledOn;   // blink the led
+  setBlueLed(ledOn);
+}
+
+ros::Subscriber<std_msgs::Empty> sub("toggle_led", messageCb );
+
+uint32_t next_report_time;
+const uint32_t kReportIntervalMs = 1000;
+
+std_msgs::String str_msg;
+ros::Publisher chatter("chatter", &str_msg);
+
+rosserial_psoc4::Nh_diag diag_msg;
+ros::Publisher nh_diag("nh_diag", &diag_msg);
+
+std_msgs::UInt32 uartstat_msg;
+ros::Publisher uartstat("uartstat", &uartstat_msg);
+
+std_msgs::UInt32 bufovf_msg;
+ros::Publisher bufovf("bufovf", &bufovf_msg);
+
+char hello[13] = "hello world!";
+
+int32_t loop_count;
+
+void setup()
+{
+  blueLed_setup();
+  ledOn = false;
+  setBlueLed(ledOn);
+  nh.initNode();
+  nh.advertise(chatter);
+  nh.advertise(nh_diag);
+  nh.advertise(uartstat);
+  nh.advertise(bufovf);
+  nh.subscribe(sub);
+  next_report_time = SysTimer::millis();
+}
+
+void loop()
+{
+  if ((int32_t)(SysTimer::millis()-next_report_time) > 0) {
+    next_report_time += kReportIntervalMs;
+    str_msg.data = hello;
+    chatter.publish( &str_msg );
+    isnprintf(hello+6, 5, "%d", loop_count++);
+    for (int i=0; i<8; ++i) {
+      diag_msg.nh_diag[i] = nh.nh_diagnostics[i];
+      nh.nh_diagnostics[i] = 0;
+    }
+    nh_diag.publish( &diag_msg );
+    uartstat_msg.data = getUartIntStatus();
+    uartstat.publish( &uartstat_msg );
+    bufovf_msg.data = getUartRxBufOvf();
+    bufovf.publish( &bufovf_msg );
+  }
+  nh.spinOnce();
+}