diff --git a/conf/airframes/overo_a1.xml b/conf/airframes/overo_a1.xml
new file mode 100644
index 0000000000..2eed7b8e54
--- /dev/null
+++ b/conf/airframes/overo_a1.xml
@@ -0,0 +1,17 @@
+
+
+
+
+#WHIRLY_USER = poine
+#WHIRLY_HOST = whirly-dev
+#WHIRLY_DIR = /home/poine
+
+WHIRLY_USER =
+WHIRLY_HOST = whirly-dev
+WHIRLY_DIR = ~
+
+include $(PAPARAZZI_SRC)/conf/autopilot/overo_ap.makefile
+
+
+
+
\ No newline at end of file
diff --git a/conf/autopilot/overo_ap.makefile b/conf/autopilot/overo_ap.makefile
new file mode 100644
index 0000000000..44258f73d9
--- /dev/null
+++ b/conf/autopilot/overo_ap.makefile
@@ -0,0 +1,17 @@
+ARCHI=geode
+
+SRC_FMS=fms
+
+ap.ARCHDIR = $(ARCHI)
+
+ap.LDFLAGS = -lm -levent -lrt
+
+ap.CFLAGS += -I$(SRC_FMS)
+
+ap.srcs=$(SRC_FMS)/fms_test_datalink.c
+
+ap.CFLAGS += -DDOWNLINK
+ap.CFLAGS += -DDOWNLINK_TRANSPORT=UdpTransport
+ap.srcs += $(SRC_FMS)/fms_network.c
+ap.srcs += $(SRC_FMS)/udp_transport.c
+ap.srcs += downlink.c
\ No newline at end of file
diff --git a/conf/autopilot/whirly_ap.makefile b/conf/autopilot/whirly_ap.makefile
index 31df4b2f9c..f4a5439c8a 100644
--- a/conf/autopilot/whirly_ap.makefile
+++ b/conf/autopilot/whirly_ap.makefile
@@ -6,7 +6,7 @@ SRC_FMS=fms
ap.ARCHDIR = $(ARCHI)
-ap.LDFLAGS = -lm -levent
+ap.LDFLAGS = -lm -levent -lrt
ap.CFLAGS += -I$(SRC_RDY) -I$(SRC_FMS)
diff --git a/conf/messages.xml b/conf/messages.xml
index 3d5638f914..73b2e78ded 100644
--- a/conf/messages.xml
+++ b/conf/messages.xml
@@ -1018,6 +1018,11 @@
+
+
+
+
+
diff --git a/sw/airborne/fms/fms_network.c b/sw/airborne/fms/fms_network.c
index f02af8552e..e0ab2a39fc 100644
--- a/sw/airborne/fms/fms_network.c
+++ b/sw/airborne/fms/fms_network.c
@@ -6,30 +6,37 @@
#include "fms_debug.h"
-struct FmsNetwork* network_new(const char* str_ip, const int port) {
+struct FmsNetwork* network_new(const char* str_ip_out, const int port_out, const int port_in) {
struct FmsNetwork* me = malloc(sizeof(struct FmsNetwork));
- // struct FgNetChannel* chan = malloc(sizeof (struct FgNetChannel));
int so_reuseaddr = 1;
struct protoent * pte = getprotobyname("UDP");
- me->socket = socket( PF_INET, SOCK_DGRAM, pte->p_proto);
- setsockopt(me->socket, SOL_SOCKET, SO_REUSEADDR,
+ me->socket_out = socket( PF_INET, SOCK_DGRAM, pte->p_proto);
+ setsockopt(me->socket_out, SOL_SOCKET, SO_REUSEADDR,
&so_reuseaddr, sizeof(so_reuseaddr));
-
- me->addr.sin_family = PF_INET;
- me->addr.sin_port = htons(port);
- me->addr.sin_addr.s_addr = inet_addr(str_ip);
+
+ me->addr_out.sin_family = PF_INET;
+ me->addr_out.sin_port = htons(port_out);
+ me->addr_out.sin_addr.s_addr = inet_addr(str_ip_out);
+
+ me->socket_in = socket( PF_INET, SOCK_DGRAM, pte->p_proto);
+ setsockopt(me->socket_in, SOL_SOCKET, SO_REUSEADDR,
+ &so_reuseaddr, sizeof(so_reuseaddr));
+
+ me->addr_in.sin_family = PF_INET;
+ me->addr_in.sin_port = htons(port_in);
+ me->addr_in.sin_addr.s_addr = htonl(INADDR_ANY);
+
+ bind(me->socket_in, (struct sockaddr *)&me->addr_in, sizeof(me->addr_in));
return me;
}
-
-
int network_write(struct FmsNetwork* me, char* buf, int len) {
- ssize_t byte_written = sendto(me->socket, buf, len, 0,
- (struct sockaddr*)&me->addr, sizeof(me->addr));
+ ssize_t byte_written = sendto(me->socket_out, buf, len, 0,
+ (struct sockaddr*)&me->addr_out, sizeof(me->addr_out));
if ( byte_written != len) {
TRACE(TRACE_ERROR, "error sending to network %d\n", byte_written);
}
diff --git a/sw/airborne/fms/fms_network.h b/sw/airborne/fms/fms_network.h
index a160e3af7e..19a59fd773 100644
--- a/sw/airborne/fms/fms_network.h
+++ b/sw/airborne/fms/fms_network.h
@@ -5,12 +5,14 @@
#include
struct FmsNetwork {
- int socket;
- struct sockaddr_in addr;
+ int socket_in;
+ int socket_out;
+ struct sockaddr_in addr_in;
+ struct sockaddr_in addr_out;
};
-extern struct FmsNetwork* network_new(const char* str_ip, const int port);
+extern struct FmsNetwork* network_new(const char* str_ip_out, const int port_out, const int port_in);
extern int network_write(struct FmsNetwork* me, char* buf, int len);
#endif /* FMS_NETWORK_H */
diff --git a/sw/airborne/fms/fms_test_datalink.c b/sw/airborne/fms/fms_test_datalink.c
new file mode 100644
index 0000000000..59a4a9c765
--- /dev/null
+++ b/sw/airborne/fms/fms_test_datalink.c
@@ -0,0 +1,113 @@
+#include
+#include
+#include
+#include
+#include
+
+#include
+
+#define LINK_HOST "127.0.0.1"
+#define LINK_PORT 4242
+#define DATALINK_PORT 4243
+#include "udp_transport.h"
+#include "fms_network.h"
+#include "downlink.h"
+
+#define PERIODIC_SEC 0
+#define PERIODIC_USEC 50000
+#define PERIODIC_NSEC (PERIODIC_USEC * 1000)
+#define ONE_SEC_NS 1000000000
+static struct event periodic_event;
+static struct event datalink_event;
+static struct timespec periodic_date;
+
+static struct FmsNetwork* network;
+
+#define PERIODIC_START() { \
+ clock_gettime(CLOCK_MONOTONIC, &periodic_date); \
+ struct timeval tv; \
+ evutil_timerclear(&tv); \
+ tv.tv_sec = PERIODIC_SEC; \
+ tv.tv_usec = PERIODIC_USEC; \
+ event_add(&periodic_event, &tv); \
+ }
+
+#define PERIODIC_RESCHEDULE() { \
+ periodic_date.tv_nsec += PERIODIC_NSEC; \
+ if (periodic_date.tv_nsec>= ONE_SEC_NS) { \
+ periodic_date.tv_nsec -= ONE_SEC_NS; \
+ periodic_date.tv_sec++; \
+ } \
+ struct timespec time_now; \
+ clock_gettime(CLOCK_MONOTONIC, &time_now); \
+ int32_t dt_ns = (int32_t)periodic_date.tv_nsec - (int32_t)time_now.tv_nsec; \
+ if (time_now.tv_sec != periodic_date.tv_sec) \
+ dt_ns += ONE_SEC_NS; \
+ struct timeval tv; \
+ evutil_timerclear(&tv); \
+ tv.tv_sec = PERIODIC_SEC; \
+ /* tv.tv_usec = PERIODIC_USEC; */ \
+ tv.tv_usec = dt_ns / 1000; \
+ event_add(&periodic_event, &tv); \
+ }
+
+
+static void periodic_task(int fd, short event, void *arg) {
+
+ // DOWNLINK_SEND_ALIVE(16, MD5SUM);
+
+ struct timespec ts;
+ clock_gettime(CLOCK_MONOTONIC, &ts);
+ DOWNLINK_SEND_FMS_TIME(&ts.tv_sec, &ts.tv_nsec);
+
+
+ PERIODIC_RESCHEDULE();
+
+}
+
+
+static void on_datalink_event(int fd, short event, void *arg) {
+ char buf[255];
+ int bytes_read;
+ bytes_read = read(fd, buf, sizeof(buf) - 1);
+ if (bytes_read == -1) {
+ perror("read");
+ return;
+ } else if (bytes_read == 0) {
+ fprintf(stderr, "Connection closed\n");
+ return;
+ }
+ printf("on_datalink_event, read %d\n", bytes_read);
+
+}
+
+
+int main(int argc , char** argv) {
+
+ /* Set real time priority */
+ struct sched_param param;
+ param.sched_priority = 90;
+ if (sched_setscheduler(0, SCHED_FIFO, ¶m) == -1) {
+ // TRACE(TRACE_ERROR, "sched_setscheduler failed");
+ perror("sched_setscheduler failed");
+ exit(EXIT_FAILURE);
+ }
+
+ /* Initalize event library */
+ event_init();
+
+ /* Add our periodic event */
+ evtimer_set(&periodic_event, periodic_task, &periodic_event);
+ PERIODIC_START();
+
+ /* */
+ network = network_new(LINK_HOST, LINK_PORT, DATALINK_PORT);
+ event_set(&datalink_event, network->socket_in, EV_READ, on_datalink_event, &datalink_event);
+ event_add(&datalink_event, NULL);
+
+ event_dispatch();
+
+ return 0;
+}
+
+
diff --git a/sw/airborne/fms/udp_transport.h b/sw/airborne/fms/udp_transport.h
index 3633ab949e..dd31607a89 100644
--- a/sw/airborne/fms/udp_transport.h
+++ b/sw/airborne/fms/udp_transport.h
@@ -42,9 +42,25 @@ extern uint16_t udpt_buf_idx;
UdpTransportPutUint8(_x); \
}
+#define UdpTransportPut2ByteByAddr(_byte) { \
+ UdpTransportPut1ByteByAddr(_byte); \
+ UdpTransportPut1ByteByAddr((const uint8_t*)_byte+1); \
+ }
+
+#define UdpTransportPut4ByteByAddr(_byte) { \
+ UdpTransportPut2ByteByAddr(_byte); \
+ UdpTransportPut2ByteByAddr((const uint8_t*)_byte+2); \
+ }
+
+
/* base types */
#define UdpTransportPutInt8ByAddr(_x) UdpTransportPut1ByteByAddr(_x)
#define UdpTransportPutUint8ByAddr(_x) UdpTransportPut1ByteByAddr((const uint8_t*)_x)
+#define UdpTransportPutInt16ByAddr(_x) UdpTransportPut2ByteByAddr((const uint8_t*)_x)
+#define UdpTransportPutUint16ByAddr(_x) UdpTransportPut2ByteByAddr((const uint8_t*)_x)
+#define UdpTransportPutInt32ByAddr(_x) UdpTransportPut4ByteByAddr((const uint8_t*)_x)
+#define UdpTransportPutUint32ByAddr(_x) UdpTransportPut4ByteByAddr((const uint8_t*)_x)
+#define UdpTransportPutFloatByAddr(_x) UdpTransportPut4ByteByAddr((const uint8_t*)_x)
#define UdpTransportPutArray(_put, _n, _x) { \
uint8_t _i; \