diff --git a/sw/airborne/fms/overo_test_periodic.c b/sw/airborne/fms/overo_test_periodic.c new file mode 100644 index 0000000000..b9f20d039f --- /dev/null +++ b/sw/airborne/fms/overo_test_periodic.c @@ -0,0 +1,95 @@ + + +#include +#include +#include +#include + +//#define LINK_HOST "10.31.4.7" +//#define LINK_HOST "stripe" +#define LINK_HOST "192.168.1.0" +#define LINK_PORT 4242 +#define DATALINK_PORT 4243 + +#include "airframe.h" + +#include "fms_periodic.h" +#include "downlink.h" +#include "udp_transport.h" +#include "fms_network.h" +#include "fms_spi_link.h" +#include "fms_autopilot_msg.h" + +static void main_periodic(int); +static void main_send_to_stm(void); + +static void on_datalink_event(int fd, short event, void *arg); +static struct FmsNetwork* network; + +int main(int argc, char** argv) { + + /* Initalize event library */ + event_init(); + + if (fms_periodic_init(main_periodic)) { + TRACE(TRACE_ERROR, "%s", "failed to start periodic generator\n"); + return -1; + } + + if (spi_link_init()) { + TRACE(TRACE_ERROR, "%s", "failed to open SPI link\n"); + return -1; + } + + /* Add our datalink event */ + network = network_new(LINK_HOST, LINK_PORT, DATALINK_PORT, FMS_NETWORK_BROADCAST); + 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; +} + + +static void main_periodic(int my_sig_num) { + + DOWNLINK_SEND_ALIVE(DefaultChannel, 16, MD5SUM); + + main_send_to_stm(); + + UdpTransportPeriodic(); + +} + + +static void main_send_to_stm(void) { + + union AutopilotMessageTW msg_in, msg_out; + spi_link_send(&msg_out.down, sizeof(union AutopilotMessageTW), &msg_in.up); + + printf("spi telemetry got %d\n", msg_in.up.tw_len); + // for (int i=0; i