Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-07 00:53:41 +08:00
Code Issues Actions 30 Packages Projects Releases Wiki Activity

datalink over IP test programm

Browse Source
This commit is contained in:
Antoine Drouin
2009-04-15 18:15:06 +00:00
parent 2d81b09453
commit 469bc0d130
8 changed files with 193 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/overo_a1.xml
+17
View File
@@ -0,0 +1,17 @@
<airframe name="wirly_ng">
<makefile>
#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
</makefile>
</airframe>
conf/autopilot/overo_ap.makefile
+17
View File
@@ -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
conf/autopilot/whirly_ap.makefile
+1 -1
View File
@@ -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)
conf/messages.xml
+5
View File
@@ -1018,6 +1018,11 @@
</message>
<message name="FMS_TIME" id="180">
<field name="tv_sec" type="uint32"/>
<field name="tv_nsec" type="uint32"/>
</message>
</class>
sw/airborne/fms/fms_network.c
+19 -12
View File
@@ -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);
}
sw/airborne/fms/fms_network.h
+5 -3
View File
@@ -5,12 +5,14 @@
#include <arpa/inet.h>
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 */
sw/airborne/fms/fms_test_datalink.c
+113
View File
@@ -0,0 +1,113 @@
#include <stdlib.h>
#include <unistd.h>
#include <sched.h>
#include <time.h>
#include <stdio.h>
#include <event.h>
#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, &param) == -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;
}
sw/airborne/fms/udp_transport.h
+16
View File
@@ -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; \