diff --git a/conf/airframes/motor_bench.xml b/conf/airframes/motor_bench.xml index 892449134a..dd9bf18735 100644 --- a/conf/airframes/motor_bench.xml +++ b/conf/airframes/motor_bench.xml @@ -1,5 +1,10 @@ +
+ + +
+ ARCHI=arm7 @@ -14,7 +19,7 @@ LPC21ISP_XTAL = 12000 #FLASH_MODE = IAP FLASH_MODE = ISP -mb.CFLAGS += -DCONFIG=\"motor_bench.h\" -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(10e-3)' +mb.CFLAGS += -DCONFIG=\"conf_motor_bench.h\" -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(10e-3)' mb.srcs = main_motor_bench.c sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c mb.CFLAGS += -DLED @@ -25,6 +30,9 @@ mb.srcs += $(SRC_ARCH)/uart_hw.c mb.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart0 mb.srcs += downlink.c pprz_transport.c +mb.CFLAGS += -DDATALINK=PPRZ -DPPRZ_UART=Uart0 +#mb.srcs += datalink.c + mb.CFLAGS += -DADC -DUSE_AD0 -DUSE_AD0_1 -DUSE_AD0_2 -DUSE_AD0_3 -DUSE_AD0_4 mb.srcs += $(SRC_ARCH)/adc_hw.c @@ -34,6 +42,8 @@ mb.CFLAGS += -DICP_SCALE mb.CFLAGS += -DTACHO_MB mb.srcs += $(SRC_ARCH)/tacho_mb.c +mb.srcs += prbs.c motor_bench.c + diff --git a/conf/autopilot/motor_bench.h b/conf/autopilot/conf_motor_bench.h similarity index 100% rename from conf/autopilot/motor_bench.h rename to conf/autopilot/conf_motor_bench.h diff --git a/sw/airborne/gps.h b/sw/airborne/gps.h index 93d3944994..5a05461370 100644 --- a/sw/airborne/gps.h +++ b/sw/airborne/gps.h @@ -38,6 +38,8 @@ #elif defined SITL #define GPS_NB_CHANNELS 16 #define GpsFixValid() (gps_mode == 3) +#else +#define GPS_NB_CHANNELS 1 #endif diff --git a/sw/airborne/main_motor_bench.c b/sw/airborne/main_motor_bench.c index 1b54072bc4..7f99aa3599 100644 --- a/sw/airborne/main_motor_bench.c +++ b/sw/airborne/main_motor_bench.c @@ -16,22 +16,12 @@ #include "messages.h" #include "downlink.h" -#include "paparazzi.h" +#include "motor_bench.h" static inline void main_init( void ); static inline void main_periodic_task( void ); static inline void main_event_task( void); - -static inline void bench_periodic ( void ); -static inline bool_t prbs_10( void ) ; - -#define MODE_MANUAL 0 -#define MODE_RAMP 1 -#define MODE_STEP 2 -#define MODE_PRBS 3 - -uint8_t mode; -uint16_t throttle; +static inline void main_dl_parse_msg( void ); int main( void ) { main_init(); @@ -51,17 +41,28 @@ static inline void main_init( void ) { adc_init(); icp_scale_init(); tacho_mb_init(); - mode = MODE_PRBS; + motor_bench_init(); int_enable(); } static inline void main_event_task( 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(2); + } } static inline void main_periodic_task( void ) { - bench_periodic(); - DOWNLINK_SEND_MOTOR_BENCH_STATUS(&cpu_time_ticks, &cpu_time_sec, &throttle, &mode); + motor_bench_periodic(); + DOWNLINK_SEND_MOTOR_BENCH_STATUS(&cpu_time_ticks, &cpu_time_sec, &motor_bench_throttle, &motor_bench_mode); static uint8_t cnt; cnt++; @@ -71,31 +72,24 @@ static inline void main_periodic_task( void ) { } } -#define PRBS_VAL1 (0.55*MAX_PPRZ) -#define PRBS_VAL2 (0.65*MAX_PPRZ) +bool_t dl_msg_available; +/** Flag provided to control calls to ::dl_parse_msg. NOT used in this module*/ +#define MSG_SIZE 128 +uint8_t dl_buffer[MSG_SIZE] __attribute__ ((aligned)); -static inline void bench_periodic ( void ) { - switch (mode) { - case MODE_MANUAL: - throttle = 0; - break; - case MODE_PRBS: - throttle = prbs_10() ? PRBS_VAL1 : PRBS_VAL2; - break; - } -} +#include "settings.h" -static inline bool_t prbs_10( void ) { - static uint16_t lsr = 0xFFFF; - uint8_t feed = - bit_is_set(lsr,6) ^ - bit_is_set(lsr,9); - uint8_t val = bit_is_set(lsr,9); - lsr = (lsr << 1) | feed; - return val; +#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/logalizer/Makefile b/sw/logalizer/Makefile index 7969ca99ec..50e614fd67 100644 --- a/sw/logalizer/Makefile +++ b/sw/logalizer/Makefile @@ -38,7 +38,7 @@ CC = gcc CFLAGS=-g -O2 -Wall `pkg-config gtk+-2.0 --cflags` LDFLAGS=`pkg-config gtk+-2.0 --libs` -s -lgtkdatabox `pcre-config --libs` -lglibivy -motor_bench : motor_bench.c +motor_bench : motor_bench.c sliding_plot.c $(CC) $(CFLAGS) -g -o $@ $^ $(LDFLAGS) ahrsview : ahrsview.c sliding_plot.c diff --git a/sw/logalizer/motor_bench.c b/sw/logalizer/motor_bench.c index 23131bb602..48571ef65c 100644 --- a/sw/logalizer/motor_bench.c +++ b/sw/logalizer/motor_bench.c @@ -2,33 +2,209 @@ #include #include #include +#include #include #include +#include "sliding_plot.h" + +GtkWidget *throttle_plot; +GtkWidget *rpm_plot; + +GtkWidget *manual_throttle_scale; + +GSList *radiobutton_group = NULL; + +#define PARAM_TIME 0 +#define PARAM_THROTTLE 1 +#define PARAM_RPM 2 +#define PARAM_VOLTAGE 3 +#define PARAM_CURRENT 4 +#define PARAM_THRUST 5 +#define PARAM_TORQUE 6 +#define PARAM_NB 7 + +const gchar* param_names[] = { + "Time", + "Throttle", + "RPM", + "Voltage", + "Current", + "Thrust", + "Torque" +}; + +GtkWidget* param_label[PARAM_NB]; +gfloat param_value[PARAM_NB]; + +#define MODE_HALTED 0 +#define MODE_MANUAL 1 +#define MODE_RAMP 2 +#define MODE_STEP 3 +#define MODE_PRBS 4 +#define MODE_NB 5 + +guint mode; + +GtkWidget* build_gui ( void ); + +gboolean timeout_callback(gpointer data) { + GString* str = g_string_sized_new(64); + g_string_printf(str, "%.1f", param_value[PARAM_TIME]); + gtk_label_set_text(GTK_LABEL(param_label[PARAM_TIME]), str->str); + g_string_printf(str, "%.1f", param_value[PARAM_THROTTLE]); + gtk_label_set_text(GTK_LABEL(param_label[PARAM_THROTTLE]), str->str); + g_string_free(str, TRUE); + return TRUE; +} + +void on_mode_changed (GtkRadioButton *radiobutton, gpointer user_data) { + if (gtk_toggle_button_get_active(GTK_TOGGLE_BUTTON(radiobutton))) { + guint new_mode = (gint)user_data; + IvySendMsg("ME RAW_DATALINK 78 SETTING;0;0;%d", new_mode); + // IvySendMsg("ME DL_SETTINGS 78 0 %d.0", new_mode); + g_message("sending new mode %d" , new_mode); + } +} + +void on_manual_throttle_value_changed (GtkScale *scale, gpointer user_data) { + gfloat new_throttle = gtk_range_get_value(GTK_RANGE(scale)); + g_message("foo %f", new_throttle); + guint foo = (new_throttle/100.*9600.); + IvySendMsg("ME RAW_DATALINK 78 SETTING;3;0;%d", foo); +} + + #define TICK_PER_SEC 15000000. void on_MOTOR_BENCH_STATUS(IvyClientPtr app, void *user_data, int argc, char *argv[]){ guint time_tick = atoi(argv[0]); guint time_sec = atoi(argv[1]); guint throttle = atoi(argv[2]); - guint mode = atoi(argv[3]); + guint new_mode = atoi(argv[3]); float time = (float)time_sec + (float)time_tick / TICK_PER_SEC; - printf("%f %d\n", time, throttle); + param_value[PARAM_TIME] = time; + param_value[PARAM_THROTTLE] = (float)throttle / 9600. * 100.; + // printf("%f %d %d\n", time, throttle, mode); + gfloat foo[] = {(gfloat)throttle}; + sliding_plot_update(throttle_plot, foo); + if (new_mode != mode) { + g_message("mode changed %d->%d", mode, new_mode); + GtkRadioButton *radiobutton = g_slist_nth(radiobutton_group, MODE_NB-new_mode-1)->data; + gtk_toggle_button_set_active(GTK_TOGGLE_BUTTON(radiobutton), TRUE); + mode = new_mode; + } } int main ( int argc, char** argv) { - // g_timeout_add(16, timeout_callback, chan); + gtk_init(&argc, &argv); + + g_timeout_add(160, timeout_callback, NULL); - GMainLoop *ml = g_main_loop_new(NULL, FALSE); + // GMainLoop *ml = g_main_loop_new(NULL, FALSE); IvyInit ("MotorBench", "MotorBench READY", NULL, NULL, NULL, NULL); IvyBindMsg(on_MOTOR_BENCH_STATUS, NULL, "^\\S* MOTOR_BENCH_STATUS (\\S*) (\\S*) (\\S*) (\\S*)"); IvyStart("127.255.255.255"); - g_main_loop_run(ml); + // g_main_loop_run(ml); + + GtkWidget* window = build_gui(); + gtk_widget_show_all(window); + + gtk_main(); + return 0; } + + + +GtkWidget* build_gui ( void ) { + GtkWidget *window; + GtkWidget *vbox1; + GtkWidget *table1; + GtkWidget *label4; + + window = gtk_window_new (GTK_WINDOW_TOPLEVEL); + gtk_window_set_title (GTK_WINDOW (window), "motor bench"); + + vbox1 = gtk_vbox_new (FALSE, 0); + gtk_widget_show (vbox1); + gtk_container_add (GTK_CONTAINER (window), vbox1); + + table1 = gtk_table_new (4, 3, FALSE); + gtk_widget_show (table1); + gtk_box_pack_start (GTK_BOX (vbox1), table1, TRUE, TRUE, 0); + gtk_table_set_col_spacings (GTK_TABLE (table1), 5); + + label4 = gtk_label_new ("mode"); + gtk_widget_show (label4); + gtk_table_attach (GTK_TABLE (table1), label4, 0, 1, 0, 1, + (GtkAttachOptions) (GTK_FILL), + (GtkAttachOptions) (0), 0, 0); + gtk_misc_set_alignment (GTK_MISC (label4), 0, 0.5); + + gchar* mode_names[] = { "Halted", "Manual", "Ramp", "Step", "PRBS" }; + + gint i; + for (i=0; i