diff --git a/conf/airframes/Poine/booz2_a8.xml b/conf/airframes/Poine/booz2_a8.xml index 49129e7fa1..8e28250b05 100644 --- a/conf/airframes/Poine/booz2_a8.xml +++ b/conf/airframes/Poine/booz2_a8.xml @@ -89,6 +89,42 @@ --> + + +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
@@ -160,7 +196,8 @@ USER = #HOST = 10.31.4.22 #HOST = overo -HOST = beth +HOST = auto7 +#HOST = 192.168.2.230 TARGET_DIR = ~ SRC_FMS=fms @@ -171,6 +208,8 @@ ARCHI=stm32 BOARD_CFG=\"boards/lisa_0.99.h\" FLASH_MODE = JTAG +PERIODIC_FREQ = 512 + include $(PAPARAZZI_SRC)/conf/autopilot/lisa_test_progs.makefile include $(PAPARAZZI_SRC)/conf/autopilot/lisa_passthrough.makefile diff --git a/conf/autopilot/lisa_passthrough.makefile b/conf/autopilot/lisa_passthrough.makefile index 366ebf2970..835aa0f73a 100644 --- a/conf/autopilot/lisa_passthrough.makefile +++ b/conf/autopilot/lisa_passthrough.makefile @@ -66,5 +66,6 @@ pt.CFLAGS += -DRADIO_CONTROL_LINK=Uart3 # PWM actuator pt.CFLAGS += -DSERVO_HZ=200 +#pt.CFLAGS += -DSERVO_HZ=50 pt.srcs += $(SRC_BOOZ)/actuators/booz_actuators_pwm.c pt.srcs += $(SRC_BOOZ_ARCH)/actuators/booz_actuators_pwm_hw.c diff --git a/conf/autopilot/lisa_test_progs.makefile b/conf/autopilot/lisa_test_progs.makefile index 4df653326d..7b67a3bdfd 100644 --- a/conf/autopilot/lisa_test_progs.makefile +++ b/conf/autopilot/lisa_test_progs.makefile @@ -42,13 +42,6 @@ overo_test_spi_link.srcs += $(SRC_FMS)/fms_spi_link.c -# test passthrough spi link between overo and stm32 -overo_test_passthrough.ARCHDIR = omap -overo_test_passthrough.CFLAGS += -I$(ACINCLUDE) -I. -I$(PAPARAZZI_HOME)/var/include -overo_test_passthrough.CFLAGS += -DOVERO_LINK_MSG_UP=AutopilotMessagePTUp -DOVERO_LINK_MSG_DOWN=AutopilotMessagePTDown -overo_test_passthrough.srcs = $(SRC_FMS)/overo_test_passthrough.c -overo_test_passthrough.srcs += $(SRC_FMS)/fms_spi_link.c - # test network based telemetry on overo overo_test_telemetry.ARCHDIR = omap overo_test_telemetry.CFLAGS += -I$(ACINCLUDE) -I. -I$(PAPARAZZI_HOME)/var/include @@ -94,6 +87,26 @@ overo_test_periodic.LDFLAGS += -levent overo_test_periodic.CFLAGS += -DOVERO_LINK_MSG_UP=AutopilotMessageBethUp -DOVERO_LINK_MSG_DOWN=AutopilotMessageBethDown overo_test_periodic.srcs += $(SRC_FMS)/fms_spi_link.c +# test passthrough , aka using stm32 as io processor +# this demonstrates +# -link with io processor +# -periodic event +# -telemetry and datalink +# +overo_test_passthrough.ARCHDIR = omap +overo_test_passthrough.LDFLAGS += -levent -lm +overo_test_passthrough.CFLAGS += -I$(ACINCLUDE) -I. -I$(PAPARAZZI_HOME)/var/include +overo_test_passthrough.CFLAGS += -DOVERO_LINK_MSG_UP=AutopilotMessagePTUp -DOVERO_LINK_MSG_DOWN=AutopilotMessagePTDown +overo_test_passthrough.srcs = $(SRC_FMS)/overo_test_passthrough.c +overo_test_passthrough.CFLAGS += -DFMS_PERIODIC_FREQ=512 +overo_test_passthrough.srcs += $(SRC_FMS)/fms_periodic.c +overo_test_passthrough.srcs += $(SRC_FMS)/fms_spi_link.c +overo_test_passthrough.srcs += $(SRC_FMS)/fms_gs_com.c +overo_test_passthrough.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=UdpTransport +overo_test_passthrough.srcs += $(SRC_FMS)/udp_transport2.c downlink.c +overo_test_passthrough.srcs += $(SRC_FMS)/fms_network.c + + ################################################################################ # diff --git a/sw/airborne/fms/fms_gs_com.c b/sw/airborne/fms/fms_gs_com.c new file mode 100644 index 0000000000..969eb470cb --- /dev/null +++ b/sw/airborne/fms/fms_gs_com.c @@ -0,0 +1,83 @@ +#include "fms/fms_gs_com.h" + +#include + +#include "udp_transport2.h" + +/* generated */ +#include "periodic.h" +/* holds the definitions of PERIODIC_SEND_XXX */ +#include "overo_test_passthrough_telemetry.h" +/* holds the definitions of DOWNLINK_SEND_XXX */ +#include "messages2.h" +#include "dl_protocol.h" +#include "settings.h" + +struct FmsGsCom fms_gs_com; +uint8_t telemetry_mode_Main_DefaultChannel; + + +static void on_datalink_event(int fd, short event __attribute__((unused)), void *arg); +static void on_datalink_message(void); + +uint8_t fms_gs_com_init(const char* gs_host, uint16_t gs_port, + uint16_t datalink_port, uint8_t broadcast) { + + fms_gs_com.network = network_new(gs_host, gs_port, datalink_port, broadcast); + fms_gs_com.udp_transport = udp_transport_new(fms_gs_com.network); + event_set(&fms_gs_com.datalink_event, fms_gs_com.network->socket_in, EV_READ | EV_PERSIST, + on_datalink_event, fms_gs_com.udp_transport); + event_add(&fms_gs_com.datalink_event, NULL); + + return 0; +} + +void fms_gs_com_periodic(void) { + + PeriodicSendMain(fms_gs_com.udp_transport); + + RunOnceEvery(10, {fms_gs_com.udp_transport->Periodic(fms_gs_com.udp_transport->impl);}); + +} + + +static void on_datalink_event(int fd, short event __attribute__((unused)), void *arg) { + char buf[512]; + int bytes_read = read(fd, buf, 512); + uint16_t i = 0; + struct udp_transport *tp = fms_gs_com.udp_transport->impl; + while (iudp_dl_msg_received) { + on_datalink_message(); + tp->udp_dl_msg_received = FALSE; + } + i++; + } + +} + +static void on_datalink_message(void) { + struct udp_transport *tp = fms_gs_com.udp_transport->impl; + uint8_t msg_id = tp->udp_dl_payload[1]; + + switch (msg_id) { + case DL_PING: + DOWNLINK_SEND_PONG(fms_gs_com.udp_transport); + break; + case DL_SETTING : { + uint8_t i = DL_SETTING_index(tp->udp_dl_payload); + float var = DL_SETTING_value(tp->udp_dl_payload); + DlSetting(i, var); + printf("datalink : %d %f\n",i,var); + DOWNLINK_SEND_DL_VALUE(fms_gs_com.udp_transport, &i, &var); + } + break; + + default : + printf("did nothing\n"); + break; + } + +} + diff --git a/sw/airborne/fms/fms_gs_com.h b/sw/airborne/fms/fms_gs_com.h new file mode 100644 index 0000000000..73795a368e --- /dev/null +++ b/sw/airborne/fms/fms_gs_com.h @@ -0,0 +1,34 @@ +/* + * This module handles communications with ground segment + * + * for now one channel of telemetry/datalink via wifi + * + */ + +#ifndef FMS_GS_COM_H +#define FMS_GS_COM_H + +#include +#include "fms_network.h" +#include "downlink_transport.h" + +#include "std.h" + +struct FmsGsCom { + + struct FmsNetwork* network; + struct DownlinkTransport *udp_transport; + struct event datalink_event; + +}; + + +extern struct FmsGsCom fms_gs_com; +/* remove me */ +extern uint8_t telemetry_mode_Main_DefaultChannel; + +extern uint8_t fms_gs_com_init(const char* gs_host, uint16_t gs_port, + uint16_t datalink_port, uint8_t broadcast); +extern void fms_gs_com_periodic(void); + +#endif /* FMS_GS_COM_H */ diff --git a/sw/airborne/fms/overo_test_passthrough.c b/sw/airborne/fms/overo_test_passthrough.c index 943de82506..df2df73e41 100644 --- a/sw/airborne/fms/overo_test_passthrough.c +++ b/sw/airborne/fms/overo_test_passthrough.c @@ -28,44 +28,69 @@ #include #include +#include + +#include "std.h" #include "fms_debug.h" +#include "fms_periodic.h" + +/* stuff for io processor link */ #include "fms_spi_link.h" #include "fms_autopilot_msg.h" -static struct AutopilotMessageCRCFrame msg_in; -static struct AutopilotMessageCRCFrame msg_out; +/* stuff for telemetry/datalink */ +#include "fms_gs_com.h" + +/* let's store those data somewhere */ +#include "booz/booz_imu.h" + +struct BoozImuFloat imu; + +static void main_periodic(int my_sig_num); + + + + + static void send_message(void); static void print_up_msg(struct AutopilotMessageCRCFrame * msg); static void print_down_msg(struct AutopilotMessageCRCFrame * msg); int main(int argc, char *argv[]) { - uint32_t us_delay; - + /* Initalize our SPI link to IO processor */ if (spi_link_init()) { TRACE(TRACE_ERROR, "%s", "failed to open SPI link \n"); return -1; } - if(argc > 1) { - us_delay = atoi(argv[1]); - } - else { - us_delay = 1953; - } - - printf("Delay: %dus\n", us_delay); - - - printf("AutopilotMessage size: %d\n", sizeof(union AutopilotMessage)); - printf("AutopilotMessageCRCFrame size: %d\n", sizeof(struct AutopilotMessageCRCFrame)); + /* Initalize the event library */ + event_init(); - while (1) { - send_message(); - usleep(us_delay); + /* Initalize our ô so accurate periodic timer */ + if (fms_periodic_init(main_periodic)) { + TRACE(TRACE_ERROR, "%s", "failed to start periodic generator\n"); + return -1; } + /* Initialize our communications with ground segment */ + fms_gs_com_init("10.31.4.7", 4242, 4243, FALSE); + + /* Enter our mainloop */ + event_dispatch(); + + printf("leaving... goodbye!\n"); + return 0; + +} + +static void main_periodic(int my_sig_num) { + + send_message(); + + fms_gs_com_periodic(); + } @@ -73,23 +98,37 @@ int main(int argc, char *argv[]) { static void send_message() { static uint32_t foo = 0; + struct AutopilotMessageCRCFrame msg_in; + struct AutopilotMessageCRCFrame msg_out; uint8_t crc_valid; - + + uint16_t val = 1500 + 500*sin(foo*0.001); + msg_out.payload.msg_down.pwm_outputs_usecs[0] = val; + msg_out.payload.msg_down.pwm_outputs_usecs[1] = val; + msg_out.payload.msg_down.pwm_outputs_usecs[2] = val; + spi_link_send(&msg_out, sizeof(struct AutopilotMessageCRCFrame), &msg_in, &crc_valid); - if (!(foo % 100)) { - printf("msg %d, CRC errors: %d\n", spi_link.msg_cnt, spi_link.crc_err_cnt); + + struct AutopilotMessagePTUp *in = &msg_in.payload.msg_up; + RATES_FLOAT_OF_BFP(imu.gyro, in->gyro); + ACCELS_FLOAT_OF_BFP(imu.accel, in->accel); + + if (!(foo % 200)) { + // printf("msg %d, CRC errors: %d\n", spi_link.msg_cnt, spi_link.crc_err_cnt); // print_up_msg(&msg_in); // print_down_msg(&msg_out); - printf("0x%08x -> gx%+02f gy%+02f gz%+02f ax%+02f ay%+02f az%+02f rs%02x | CRC errors: %d \n", + printf("%08d -> gx%+02.1f gy%+02.1f gz%+02.1f ax%+02.1f ay%+02.1f az%+02.1f rs%02x stm_msg %08d stm_err %d | CRC errors: %d\n", foo, - DegOfRad(RATE_FLOAT_OF_BFP(msg_in.payload.msg_up.gyro.p)), - DegOfRad(RATE_FLOAT_OF_BFP(msg_in.payload.msg_up.gyro.q)), - DegOfRad(RATE_FLOAT_OF_BFP(msg_in.payload.msg_up.gyro.r)), - ACCEL_FLOAT_OF_BFP(msg_in.payload.msg_up.accel.x), - ACCEL_FLOAT_OF_BFP(msg_in.payload.msg_up.accel.y), - ACCEL_FLOAT_OF_BFP(msg_in.payload.msg_up.accel.z), - msg_in.payload.msg_up.rc_status, + DegOfRad(RATE_FLOAT_OF_BFP(in->gyro.p)), + DegOfRad(RATE_FLOAT_OF_BFP(in->gyro.q)), + DegOfRad(RATE_FLOAT_OF_BFP(in->gyro.r)), + ACCEL_FLOAT_OF_BFP(in->accel.x), + ACCEL_FLOAT_OF_BFP(in->accel.y), + ACCEL_FLOAT_OF_BFP(in->accel.z), + in->rc_status, + in->stm_msg_cnt, + in->stm_crc_err_cnt, spi_link.crc_err_cnt); } foo++; diff --git a/sw/airborne/fms/overo_test_passthrough_telemetry.h b/sw/airborne/fms/overo_test_passthrough_telemetry.h new file mode 100644 index 0000000000..7cc57dd581 --- /dev/null +++ b/sw/airborne/fms/overo_test_passthrough_telemetry.h @@ -0,0 +1,13 @@ +#ifndef OVERO_TEST_PASSTHROUGH_TELEMETRY_H +#define OVERO_TEST_PASSTHROUGH_TELEMETRY_H + +#define PERIODIC_SEND_ALIVE(_transport) DOWNLINK_SEND_ALIVE(_transport, 16, MD5SUM) + +#include "booz/booz_imu.h" +extern struct BoozImuFloat imu; +#define PERIODIC_SEND_IMU_GYRO(_transport) \ + DOWNLINK_SEND_IMU_GYRO(_transport, &imu.gyro.p, &imu.gyro.q, &imu.gyro.r) + + + +#endif /* OVERO_TEST_PASSTHROUGH_TELEMETRY_H */