diff --git a/conf/settings/booz.xml b/conf/settings/booz.xml index f0e0a2a5b3..c1d69145e4 100644 --- a/conf/settings/booz.xml +++ b/conf/settings/booz.xml @@ -6,5 +6,7 @@ - + + + diff --git a/sw/airborne/main_coax.c b/sw/airborne/main_coax.c deleted file mode 100644 index 579ed83346..0000000000 --- a/sw/airborne/main_coax.c +++ /dev/null @@ -1,93 +0,0 @@ -#include "std.h" -#include "init_hw.h" -#include "sys_time.h" -#include "led.h" -#include "interrupt_hw.h" -#include "uart.h" - -#include "main_ap.h" -#include "airframe.h" - -#include "messages.h" -#include "downlink.h" -#include "spi.h" -#include "baro_MS5534A.h" - -#include "estimator.h" - -float ground_alt; - -#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE - -static bool_t dl_msg_available; -static inline void main_dl_parse_msg( void ); - -void init_ap( void ) { - /** init done in main_fbw */ - spi_init(); - - baro_MS5534A_init(); - - int_enable(); - - baro_MS5534A_init(); -} - -void periodic_task_ap( void ) { - // LED_TOGGLE(1); - // DOWNLINK_SEND_TAKEOFF(&cpu_time_sec); - - static uint8_t _20Hz = 0; - _20Hz++; - if (_20Hz>=3) _20Hz=0; - - if (!_20Hz) { - baro_MS5534A_send(); - } -} - -void event_task_ap( void ) { - if (PprzBuffer()) { - ReadPprzBuffer(); - if (pprz_msg_received) { - pprz_parse_payload(); - pprz_msg_received = FALSE; - } - } - if (dl_msg_available) { - main_dl_parse_msg(); - dl_msg_available = FALSE; - LED_TOGGLE(1); - } - - if (spi_message_received) { - /* Got a message on SPI. */ - spi_message_received = FALSE; - baro_MS5534A_event_task(); - if (baro_MS5534A_available) { - baro_MS5534A_available = FALSE; - - baro_MS5534A_z = ground_alt +((float)baro_MS5534A_ground_pressure - baro_MS5534A_pressure)*0.084; - if (alt_baro_enabled) { - EstimatorSetAlt(baro_MS5534A_z); - } - } - } -} - -#define MSG_SIZE 128 -uint8_t dl_buffer[MSG_SIZE] __attribute__ ((aligned)); - -#include "settings.h" - -#define IdOfMsg(x) (x[1]) - -static inline void main_dl_parse_msg(void) { - uint8_t msg_id = IdOfMsg(dl_buffer); - if (msg_id == DL_SETTING) { - uint8_t i = DL_SETTING_index(dl_buffer); - float var = DL_SETTING_value(dl_buffer); - DlSetting(i, var); - DOWNLINK_SEND_DL_VALUE(&i, &var); - } -} diff --git a/sw/simulator/Makefile b/sw/simulator/Makefile index 6b84dac655..7be2b40951 100644 --- a/sw/simulator/Makefile +++ b/sw/simulator/Makefile @@ -105,10 +105,21 @@ CFLAGS += -DRADIO_CONTROL BOOZ_AB_SRCS += ../airborne/radio_control.c CFLAGS += -DACTUATORS=\"servos_nil.h\" BOOZ_AB_SRCS += ../airborne/actuators.c -CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=IvyTransport + +CFLAGS += -DDOWNLINK +BOOZ_AB_SRCS += ../airborne/booz_telemetry.c ../airborne/downlink.c + +#CFLAGS += -DDOWNLINK_TRANSPORT=PprzTransport -DPPRZ_UART=SimUart -DDOWNLINK_DEVICE=SimUart -DSIM_UART +#BOOZ_AB_SRCS += ../airborne/pprz_transport.c ../airborne/sim/sim_uart.c + +CFLAGS += -DDOWNLINK_TRANSPORT=IvyTransport +BOOZ_AB_SRCS += ../airborne/sim/ivy_transport.c LDFLAGS += -lglibivy -BOOZ_AB_SRCS += ../airborne/booz_telemetry.c ../airborne/downlink.c ../airborne/sim/ivy_transport.c + #../airborne/link_imu.c +#CFLAGS += -DDATALINK=PPRZ +#BOOZ_AB_SRCS += ../airborne/datalink.c + BOOZ_AB_SRCS += ../airborne/booz_estimator.c BOOZ_AB_SRCS += ../airborne/booz_control.c BOOZ_AB_SRCS += ../airborne/booz_autopilot.c diff --git a/sw/simulator/main_booz_sim.c b/sw/simulator/main_booz_sim.c index 08af8f5c65..1e5d4cf4c6 100644 --- a/sw/simulator/main_booz_sim.c +++ b/sw/simulator/main_booz_sim.c @@ -23,8 +23,17 @@ double disp_time; double foo_commands[] = {0., 0., 0., 0.}; - static gboolean timeout_callback(gpointer data); +static gboolean on_js_data_received(GIOChannel *source, GIOCondition condition, gpointer data); + +static void joystick_init(void); +#ifdef SIM_UART +static void sim_uart_init(void); +#endif +#if defined DOWNLINK_TRANSPORT && DOWNLINK_TRANSPORT == IvyTransport +static void ivy_transport_init(void); +static void on_DL_SETTING(IvyClientPtr app, void *user_data, int argc, char *argv[]); +#endif static void airborne_init(void); static void airborne_periodic_task(void); @@ -34,9 +43,6 @@ static void airborne_event_task(void); static gboolean timeout_callback(gpointer data) { - - - booz_flight_model_run(DT, foo_commands); sim_time += DT; @@ -62,6 +68,18 @@ static gboolean timeout_callback(gpointer data) { + + + + + + + + + + + + int main ( int argc, char** argv) { sim_time = 0.; @@ -71,6 +89,15 @@ int main ( int argc, char** argv) { booz_flightgear_init(fg_host, fg_port); +#ifdef SIM_UART + sim_uart_init(); +#endif + +#if defined DOWNLINK_TRANSPORT && DOWNLINK_TRANSPORT == IvyTransport + ivy_transport_init(); +#endif + + joystick_init(); airborne_init(); @@ -79,8 +106,6 @@ int main ( int argc, char** argv) { g_timeout_add(TIMEOUT_PERIOD, timeout_callback, NULL); - IvyInit ("BoozSim", "BoozSim READY", NULL, NULL, NULL, NULL); - IvyStart("127.255.255.255"); g_main_loop_run(ml); @@ -88,11 +113,108 @@ int main ( int argc, char** argv) { } +///////////////////// +// Helpers +//////////////////// + +#include +#include +#include +#include +#include +#include + +static void joystick_init(void) { + const char* device = "/dev/input/js1"; + int fd = open(device, O_RDONLY | O_NONBLOCK); + if (fd == -1) { + printf("opening joystick serial device %s : %s\n", device, strerror(errno)); + return; + } + // if( ioctl( fd, JSIOCSCORR, corr ) != 0 ) + // perror( "Unable to turn off deadband correction" ); + + GIOChannel* channel = g_io_channel_unix_new(fd); + g_io_channel_set_encoding(channel, NULL, NULL); + g_io_add_watch (channel, G_IO_IN , on_js_data_received, NULL); + +} + + + + + + + + + + +#if defined DOWNLINK_TRANSPORT && DOWNLINK_TRANSPORT == IvyTransport + +static void ivy_transport_init(void) { + IvyInit ("BoozSim", "BoozSim READY", NULL, NULL, NULL, NULL); + IvyBindMsg(on_DL_SETTING, NULL, "^(\\S*) DL_SETTING (\\S*) (\\S*) (\\S*)"); + IvyStart("127.255.255.255"); +} + +#include "std.h" +#include "settings.h" +#include "booz_telemetry.h" +static void on_DL_SETTING(IvyClientPtr app, void *user_data, int argc, char *argv[]){ + uint8_t index = atoi(argv[2]); + float value = atof(argv[3]); + DlSetting(index, value); + DOWNLINK_SEND_DL_VALUE(&index, &value); + printf("setting %d %f\n", index, value); +} + +#endif + + + +#ifdef SIM_UART +#define AC_ID 148 +#include +#include +#include + +#include "sim_uart.h" + +static void sim_uart_init(void) { + char link_pipe_name[128]; + sprintf(link_pipe_name, "/tmp/pprz_link_%d", AC_ID); + struct stat st; + if (stat(link_pipe_name, &st)) { + if (mkfifo(link_pipe_name, 0644) == -1) { + perror("make pipe"); + exit (10); + } + } + if ( !(pipe_stream = fopen(link_pipe_name, "w")) ) { + perror("open pipe"); + exit (10); + } +} +#endif /* SIM_UART */ + + + + + + + +////////////////// +// AIRBORNE CODE +////////////////// + + +#include "ppm.h" #include "radio_control.h" #include "actuators.h" #include "commands.h" #include "booz_telemetry.h" +//#include "datalink.h" #include "booz_estimator.h" #include "booz_control.h" @@ -110,10 +232,8 @@ uint16_t ppm_pulses[PPM_NB_PULSES]; static void airborne_init(void) { - ppm_pulses[RADIO_THROTTLE] = 1223 + 0.615 * (2050-1223); - ppm_pulses[RADIO_ROLL] = 1500; - ppm_pulses[RADIO_PITCH] = 1498; - ppm_pulses[RADIO_YAW] = 1493; + ppm_init(); + radio_control_init(); booz_estimator_init(); booz_control_init(); @@ -124,12 +244,6 @@ static void airborne_init(void) { static void airborne_periodic_task(void) { - int foo = sim_time / 10; - if (!(foo%2)) - ppm_pulses[RADIO_PITCH] = 1600; - else - ppm_pulses[RADIO_PITCH] = 1400; - booz_autopilot_periodic_task(); SetActuatorsFromCommands(commands); @@ -160,6 +274,9 @@ static void airborne_periodic_task(void) { static void airborne_event_task(void) { + + // DlEventCheckAndHandle(); + // if (ppm_valid) { // ppm_valid = FALSE; radio_control_event_task(); @@ -171,3 +288,44 @@ static void airborne_event_task(void) { //} } + + + + + + + +#define JS_THROTTLE 6 +#define JS_ROLL 0 +#define JS_PITCH 1 +#define JS_YAW 5 + +static gboolean on_js_data_received(GIOChannel *source, GIOCondition condition, gpointer data) { + + struct js_event js; + gsize len; + GError *err = NULL; + g_io_channel_read_chars(source, (void*)(&js), sizeof(struct js_event), &len, &err); + + if (js.type == JS_EVENT_AXIS) { + //printf("%d %d\n", js.number, js.value); + switch (js.number) { + case JS_THROTTLE: + ppm_pulses[RADIO_THROTTLE] = 1223 + (js.value - 30) * (float)(2050-1223) / (float)(223 - 30); + break; + case JS_PITCH: + ppm_pulses[RADIO_PITCH] = 1498 + (js.value - 113) * (float)(2050-950) / (float)(224 - 1); + break; + case JS_ROLL: + ppm_pulses[RADIO_ROLL] = 1500 + (js.value - 112) * (float)(2050-950) / (float)(1 - 224); + break; + case JS_YAW: + ppm_pulses[RADIO_YAW] = 1500 + (js.value - 112) * (float)(2050-950) / (float)(224 - 1); + break; + } + } + + return TRUE; +} + +