diff --git a/conf/airframes/Poine/beth.xml b/conf/airframes/Poine/beth.xml index 7c19b47ee7..0715086e3c 100644 --- a/conf/airframes/Poine/beth.xml +++ b/conf/airframes/Poine/beth.xml @@ -122,8 +122,7 @@ main_stm32.srcs += downlink.c pprz_transport.c main_stm32.CFLAGS += -DUSE_OVERO_LINK -#main_stm32.CFLAGS += -DOVERO_LINK_MSG_UP=AutopilotMessageBethUp -DOVERO_LINK_MSG_DOWN=AutopilotMessageBethDown -main_stm32.CFLAGS += -DOVERO_LINK_MSG_UP=AutopilotMessageFoo -DOVERO_LINK_MSG_DOWN=AutopilotMessageFoo +main_stm32.CFLAGS += -DOVERO_LINK_MSG_UP=AutopilotMessageBethUp -DOVERO_LINK_MSG_DOWN=AutopilotMessageBethDown main_stm32.CFLAGS += -DOVERO_LINK_LED_OK=5 -DOVERO_LINK_LED_KO=4 -DUSE_DMA1_C2_IRQ main_stm32.srcs += lisa/lisa_overo_link.c lisa/arch/stm32/lisa_overo_link_arch.c @@ -150,7 +149,7 @@ main_stm32.srcs += $(SRC_BOOZ)/actuators/booz_actuators_asctec.c #\ # $(SRC_BOOZ_ARCH)/actuators/booz_actuators_asctec_arch.c main_stm32.CFLAGS += -DACTUATORS_ASCTEC_DEVICE=i2c1 -#-DBOOZ_START_DELAY=3 -DUSE_TIM2_IRQ +# -DBOOZ_START_DELAY=3 -DUSE_TIM2_IRQ main_stm32.CFLAGS += -DUSE_I2C1 main_stm32.srcs += i2c.c $(SRC_ARCH)/i2c_hw.c @@ -181,7 +180,6 @@ main_coders.CFLAGS += -DUSE_LED main_coders.srcs += $(SRC_ARCH)/led_hw.c main_coders.CFLAGS += -DUSE_SYS_TIME -DSYS_TIME_LED=7 main_coders.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(1./512.)' -#main_coders.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(1./10.)' main_coders.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c main_coders.CFLAGS += -DUSE_UART1 -DUART1_BAUD=B57600 @@ -210,10 +208,17 @@ SRC_FMS=fms main_overo.ARCHDIR = omap main_overo.CFLAGS = -I. -I$(SRC_FMS) main_overo.srcs = $(SRC_BETH)/main_overo.c +main_overo.CFLAGS += -DFMS_PERIODIC_FREQ=100 +main_overo.srcs += $(SRC_FMS)/fms_periodic.c +main_overo.srcs += $(SRC_FMS)/fms_serial_port.c +main_overo.LDFLAGS += -lrt main_overo.srcs += $(SRC_FMS)/fms_spi_link.c -#main_overo.CFLAGS += -DOVERO_LINK_MSG_UP=AutopilotMessageBethUp -DOVERO_LINK_MSG_DOWN=AutopilotMessageBethDown -main_overo.CFLAGS += -DOVERO_LINK_MSG_UP=AutopilotMessageFoo -DOVERO_LINK_MSG_DOWN=AutopilotMessageFoo +main_overo.CFLAGS += -DOVERO_LINK_MSG_UP=AutopilotMessageBethUp -DOVERO_LINK_MSG_DOWN=AutopilotMessageBethDown +main_overo.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=UdpTransport +main_overo.srcs += $(SRC_FMS)/udp_transport.c downlink.c +main_overo.srcs += $(SRC_FMS)/fms_network.c +main_overo.LDFLAGS += -levent # # diff --git a/sw/airborne/beth/main_coders.c b/sw/airborne/beth/main_coders.c index 9e2ba1e900..2d9e7b71a8 100644 --- a/sw/airborne/beth/main_coders.c +++ b/sw/airborne/beth/main_coders.c @@ -15,6 +15,8 @@ #include +#include "beth/bench_sensors.h" + /* * * PC.01 (ADC Channel11) ext1-20 coder_values[1] @@ -32,6 +34,8 @@ static inline void main_periodic( void ); static inline void main_event( void ); static inline void main_init_adc(void); +static inline void main_on_bench_sensors( void ); + //static inline void main_init_i2c2(void); //void i2c2_ev_irq_handler(void); //void i2c2_er_irq_handler(void); @@ -46,8 +50,8 @@ static uint16_t coder_values[3]; #define I2C2_ClockSpeed 200000 #define MY_I2C2_BUF_LEN 4 -static uint8_t i2c2_idx; -static uint8_t i2c2_buf[MY_I2C2_BUF_LEN]; +//static uint8_t i2c2_idx; +//static uint8_t i2c2_buf[MY_I2C2_BUF_LEN]; uint16_t servos[4]; @@ -85,18 +89,28 @@ static inline void main_periodic( void ) { //RunOnceEvery(5, {DOWNLINK_SEND_ADC_GENERIC(DefaultChannel, &coder_values[0], &coder_values[1]);}); //RunOnceEvery(5, {DOWNLINK_SEND_ADC_GENERIC(DefaultChannel, &can1_status, &can1_pending);}); + + RunOnceEvery(5, {DOWNLINK_SEND_BETH(DefaultChannel, &bench_sensors.angle_1, + &bench_sensors.angle_2,&bench_sensors.angle_3, &bench_sensors.current);}); servos[0]=coder_values[0]; servos[1]=coder_values[1]; - can_transmit(2, 0, (uint8_t *)servos, 8); + //use id=1 for azimuth board + can_transmit(1, 0, (uint8_t *)servos, 8); } static inline void main_event( void ) { - + BenchSensorsEvent(main_on_bench_sensors); } + +static inline void main_on_bench_sensors( void ) { + +} + +#if 0 /* * * I2C2 : autopilot link @@ -189,7 +203,7 @@ void i2c2_er_irq_handler(void) { } - +#endif /* * diff --git a/sw/airborne/beth/main_overo.c b/sw/airborne/beth/main_overo.c index 65a510f17f..41f6d79471 100644 --- a/sw/airborne/beth/main_overo.c +++ b/sw/airborne/beth/main_overo.c @@ -27,52 +27,125 @@ #include #include - +#include "fms_network.h" +#include "fms_periodic.h" #include "fms_debug.h" #include "fms_spi_link.h" #include "fms_autopilot_msg.h" +#include -//static union AutopilotMessageBeth my_buffers[2]; -//static struct AutopilotMessageBethUp msg_in; -//static struct AutopilotMessageBethDown msg_out; -static struct AutopilotMessageFoo msg_in; -static struct AutopilotMessageFoo msg_out; +#include "downlink.h" +#include "udp_transport.h" + +#define GCS_HOST "10.31.4.104" +#define GCS_PORT 4242 +#define DATALINK_PORT 4243 + + +static void main_periodic(int); +static void on_datalink_event(int fd, short event, void *arg); + +static struct FmsNetwork* network; + +static struct AutopilotMessageBethUp msg_in; +static struct AutopilotMessageBethDown msg_out; static void send_message(void); +uint16_t az,elev,tilt; + +static uint32_t foo = 0; + +static void main_periodic(int my_sig_num) { + + DOWNLINK_SEND_ALIVE(DefaultChannel, 16, MD5SUM); + + send_message(); + + DOWNLINK_SEND_BETH(DefaultChannel,&msg_in.bench_sensor.x,&msg_in.bench_sensor.y, + &msg_in.bench_sensor.z,&msg_in.bench_sensor.x); + + DOWNLINK_SEND_BOOZ2_GYRO(DefaultChannel, + &msg_in.gyro.p, + &msg_in.gyro.q, + &msg_in.gyro.r); + + + DOWNLINK_SEND_BOOZ2_ACCEL(DefaultChannel, + &msg_in.accel.x, + &msg_in.accel.y, + &msg_in.accel.z); + + UdpTransportPeriodic(); + +} + + int main(int argc, char *argv[]) { if (spi_link_init()) { TRACE(TRACE_ERROR, "%s", "failed to open SPI link \n"); return -1; } - while (1) { - send_message(); - usleep(1953); - //usleep(500000); + + network = network_new(GCS_HOST, GCS_PORT, DATALINK_PORT, FALSE); + + /* Initalize the event library */ + event_init(); + + if (fms_periodic_init(main_periodic)) { + TRACE(TRACE_ERROR, "%s", "failed to start periodic generator\n"); + return -1; } + network = network_new(GCS_HOST, GCS_PORT, DATALINK_PORT, FALSE); + struct event datalink_event; + event_set(&datalink_event, network->socket_in, EV_READ, on_datalink_event, &datalink_event); + event_add(&datalink_event, NULL); + + event_dispatch(); + return 0; } -uint16_t az,elev,tilt; +static uint16_t foo1 = 0x12; -static uint32_t foo = 0; static void send_message() { + //uint8_t *fooptr; - msg_out.foo = 0x0123; - msg_out.bar = 0x4567; + msg_out.thrust = foo1+foo1; + msg_out.pitch = foo1; + foo1 = foo1+2; + //msg_out.cksum = msg_out.thrust + msg_out.pitch; - spi_link_send(&msg_out, sizeof(struct AutopilotMessageFoo) , &msg_in); -// if (msg_in.bli == "0xdead") { - az = msg_in.foo; - elev = msg_in.bar; - tilt = msg_in.blaa; -// } - if (!(foo%100)) { - printf("%d %d %d %x\r\n",az,elev,tilt,msg_in.bli); - - } + spi_link_send(&msg_out, sizeof(union AutopilotMessage) , &msg_in); + + /*if (msg_in.bench_sensor.x == 0){ + fooptr = &msg_in; + for (int i=0; i