mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-06-06 16:58:48 +08:00
*** empty log message ***
This commit is contained in:
Antoine Drouin
@@ -1,5 +1,10 @@
|
||||
<airframe name="MOTOR_BENCH">
|
||||
|
||||
<section name="DATALINK" prefix="DATALINK_">
|
||||
<define name="DEVICE_TYPE" value="PPRZ"/>
|
||||
<define name="DEVICE_ADDRESS" value="...."/>
|
||||
</section>
|
||||
|
||||
<makefile>
|
||||
|
||||
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
|
||||
|
||||
</makefile>
|
||||
|
||||
</airframe>
|
||||
|
||||
@@ -38,6 +38,8 @@
|
||||
#elif defined SITL
|
||||
#define GPS_NB_CHANNELS 16
|
||||
#define GpsFixValid() (gps_mode == 3)
|
||||
#else
|
||||
#define GPS_NB_CHANNELS 1
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -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
|
||||
|
||||
+181
-5
@@ -2,33 +2,209 @@
|
||||
#include <stdio.h>
|
||||
#include <math.h>
|
||||
#include <stdlib.h>
|
||||
#include <gtk/gtk.h>
|
||||
|
||||
#include <Ivy/ivy.h>
|
||||
#include <Ivy/ivyglibloop.h>
|
||||
|
||||
#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<MODE_NB; i++) {
|
||||
GtkWidget* radiobutton = gtk_radio_button_new_with_mnemonic (NULL, mode_names[i]);
|
||||
gtk_widget_show (radiobutton);
|
||||
gtk_table_attach (GTK_TABLE (table1), radiobutton, 0, 1, 1+i, 2+i,
|
||||
(GtkAttachOptions) (GTK_FILL),
|
||||
(GtkAttachOptions) (0), 0, 0);
|
||||
gtk_radio_button_set_group (GTK_RADIO_BUTTON (radiobutton), radiobutton_group);
|
||||
radiobutton_group = gtk_radio_button_get_group (GTK_RADIO_BUTTON (radiobutton));
|
||||
g_signal_connect ((gpointer) radiobutton, "toggled",
|
||||
G_CALLBACK (on_mode_changed),
|
||||
((gpointer)i));
|
||||
}
|
||||
|
||||
manual_throttle_scale = gtk_hscale_new (GTK_ADJUSTMENT (gtk_adjustment_new (0., 0, 100, 1, 1, 1)));
|
||||
gtk_widget_show (manual_throttle_scale);
|
||||
gtk_table_attach (GTK_TABLE (table1), manual_throttle_scale, 1, 1+PARAM_NB, 1+MODE_MANUAL, 2+MODE_MANUAL,
|
||||
(GtkAttachOptions) (GTK_EXPAND | GTK_FILL),
|
||||
(GtkAttachOptions) (GTK_FILL), 0, 0);
|
||||
gtk_range_set_update_policy (GTK_RANGE (manual_throttle_scale), GTK_UPDATE_DELAYED);
|
||||
g_signal_connect ((gpointer) manual_throttle_scale, "value_changed",
|
||||
G_CALLBACK (on_manual_throttle_value_changed),
|
||||
NULL);
|
||||
|
||||
|
||||
|
||||
for (i=0; i<PARAM_NB; i++) {
|
||||
GtkWidget* label = gtk_label_new (param_names[i]);
|
||||
gtk_table_attach (GTK_TABLE (table1), label, i, i+1, PARAM_NB+1, PARAM_NB + 2,
|
||||
(GtkAttachOptions) (GTK_FILL),
|
||||
(GtkAttachOptions) (0), 0, 0);
|
||||
param_label[i] = gtk_label_new ("unknown");
|
||||
gtk_table_attach (GTK_TABLE (table1), param_label[i], i, i+1, PARAM_NB+2, PARAM_NB + 3,
|
||||
(GtkAttachOptions) (GTK_FILL),
|
||||
(GtkAttachOptions) (0), 0, 0);
|
||||
}
|
||||
|
||||
GtkWidget *frame = gtk_frame_new ("Throttle");
|
||||
|
||||
gtk_container_set_border_width (GTK_CONTAINER (frame), 10);
|
||||
gtk_box_pack_start (GTK_BOX (vbox1), frame, TRUE, TRUE, 0);
|
||||
throttle_plot = sliding_plot_new(1);
|
||||
gtk_container_add (GTK_CONTAINER (frame), throttle_plot );
|
||||
|
||||
|
||||
frame = gtk_frame_new ("RPM");
|
||||
|
||||
gtk_container_set_border_width (GTK_CONTAINER (frame), 10);
|
||||
gtk_box_pack_start (GTK_BOX (vbox1), frame, TRUE, TRUE, 0);
|
||||
// rpm_plot = sliding_plot_new(1);
|
||||
// gtk_container_add (GTK_CONTAINER (frame), rpm_plot );
|
||||
|
||||
|
||||
gtk_box_pack_start (GTK_BOX (vbox1), frame, TRUE, TRUE, 0);
|
||||
|
||||
|
||||
return window;
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user