Revision cf115e3d
ID | cf115e3dd10fc6e8ab344c1385a88be6932755b7 |
Fixed problem with serial rx. Rosserial now works.
scout_avr/Makefile | ||
---|---|---|
2 | 2 |
MCU=atmega328 |
3 | 3 |
F_CPU=8000000 |
4 | 4 |
SRC=src/*.cpp src/ros_lib/*.cpp |
5 |
HDR=src/*.h |
|
5 | 6 |
FLAGS=-Isrc/ros_lib -Isrc -mmcu=$(MCU) -DF_CPU=$(F_CPU)UL -funsigned-char -Os -fpack-struct -Wall |
6 | 7 |
|
7 | 8 |
all: scout_avr.hex |
... | ... | |
9 | 10 |
%.hex: %.elf |
10 | 11 |
avr-objcopy -j .text -j .data -O ihex $< $@ |
11 | 12 |
|
12 |
scout_avr.elf: $(SRC) |
|
13 |
scout_avr.elf: $(SRC) $(HDR)
|
|
13 | 14 |
avr-g++ $(FLAGS) $(SRC) -o scout_avr.elf |
14 | 15 |
|
15 | 16 |
download: scout_avr.hex |
scout_avr/launch/rosserial.launch | ||
---|---|---|
1 | 1 |
<launch> |
2 |
<node pkg="rosserial_python" type="serial_node.py" name="serial_node"> |
|
2 |
<node pkg="rosserial_python" type="serial_node.py" name="serial_node" output="screen">
|
|
3 | 3 |
<param name="~port" value="/dev/ttyUSB0" /> |
4 | 4 |
<param name="~baud" value="38400" /> |
5 | 5 |
</node> |
scout_avr/src/Atmega128rfa1.cpp | ||
---|---|---|
24 | 24 |
ISR(USART_RX_vect) |
25 | 25 |
{ |
26 | 26 |
char data = UDR0; |
27 |
if (rx_start == rx_end-1 || (rx_start == RX_BUFFER_SIZE-1 && rx_end == 0))
|
|
27 |
if (rx_end == rx_start-1 || (rx_start == 0 && rx_end == RX_BUFFER_SIZE-1))
|
|
28 | 28 |
{ |
29 | 29 |
// TODO warn of buffer overflow? |
30 | 30 |
} |
... | ... | |
70 | 70 |
|
71 | 71 |
int Atmega128rfa1::read() |
72 | 72 |
{ |
73 |
cli(); |
|
73 | 74 |
if (rx_start == rx_end) |
74 | 75 |
return -1; |
75 | 76 |
else |
... | ... | |
80 | 81 |
rx_start = 0; |
81 | 82 |
return ret; |
82 | 83 |
} |
84 |
sei(); |
|
83 | 85 |
} |
84 | 86 |
|
85 | 87 |
void Atmega128rfa1::write(uint8_t* data, int length) |
scout_avr/src/Atmega128rfa1.h | ||
---|---|---|
3 | 3 |
|
4 | 4 |
#include "ros/node_handle.h" |
5 | 5 |
|
6 |
#define MAX_SUBSCRIBERS 25
|
|
7 |
#define MAX_PUBLISHERS 25
|
|
8 |
#define INPUT_SIZE 512
|
|
9 |
#define OUTPUT_SIZE 512
|
|
6 |
#define MAX_SUBSCRIBERS 2 |
|
7 |
#define MAX_PUBLISHERS 2 |
|
8 |
#define INPUT_SIZE 128
|
|
9 |
#define OUTPUT_SIZE 128
|
|
10 | 10 |
|
11 |
#define RX_BUFFER_SIZE 1024
|
|
11 |
#define RX_BUFFER_SIZE 256
|
|
12 | 12 |
|
13 | 13 |
class Atmega128rfa1 |
14 | 14 |
{ |
scout_avr/src/main.cpp | ||
---|---|---|
1 | 1 |
#include "ros.h" |
2 |
#include <std_msgs/UInt16.h>
|
|
2 |
#include <std_msgs/Int16.h> |
|
3 | 3 |
|
4 | 4 |
ros::Publisher *ptest_out; |
5 | 5 |
|
6 |
void callback(const std_msgs::UInt16& msg)
|
|
6 |
void callback(const std_msgs::Int16& msg) |
|
7 | 7 |
{ |
8 | 8 |
ptest_out->publish(&msg); |
9 | 9 |
} |
... | ... | |
11 | 11 |
int main() |
12 | 12 |
{ |
13 | 13 |
ros::NodeHandle nh; |
14 |
std_msgs::UInt16 msg;
|
|
15 |
ros::Subscriber<std_msgs::UInt16> test_in("test_in", callback);
|
|
14 |
std_msgs::Int16 msg; |
|
15 |
ros::Subscriber<std_msgs::Int16> test_in("test_in", callback); |
|
16 | 16 |
ros::Publisher test_out("test_out", &msg); |
17 | 17 |
|
18 | 18 |
ptest_out = &test_out; |
... | ... | |
41 | 41 |
int main() |
42 | 42 |
{ |
43 | 43 |
char time[20]; |
44 |
int data; |
|
45 |
unsigned long now, next = 0; |
|
44 | 46 |
Atmega128rfa1 avr; |
45 | 47 |
avr.init(); |
46 |
unsigned long now, next = 0; |
|
47 | 48 |
while (1) |
48 | 49 |
{ |
49 | 50 |
now = avr.time(); |
50 | 51 |
if (now > next) { |
51 |
next = now + 100; |
|
52 |
ultoa(avr.time(), time, 10); |
|
53 |
avr.write((uint8_t*) time, strlen(time)); |
|
52 |
next = now + 500; |
|
53 |
//ultoa(avr.time(), time, 10); |
|
54 |
//avr.write((uint8_t*) time, strlen(time)); |
|
55 |
data = avr.read(); |
|
56 |
if (data >= 0) |
|
57 |
{ |
|
58 |
uint8_t data8 = data; |
|
59 |
avr.write((uint8_t*) "Received ", 9); |
|
60 |
avr.write(&data8, 1); |
|
61 |
} |
|
62 |
else |
|
63 |
avr.write((uint8_t*) ".", 1); |
|
54 | 64 |
avr.write((uint8_t*) "\n\r", 1); |
55 | 65 |
} |
56 | 66 |
} |
57 | 67 |
return 0; |
58 |
}*/ |
|
59 |
|
|
68 |
} |
|
69 |
*/ |
Also available in: Unified diff