diff --git a/sw/in_progress/ir_calibration/Makefile b/sw/in_progress/ir_calibration/Makefile index 01e623efa2..47a072344d 100644 --- a/sw/in_progress/ir_calibration/Makefile +++ b/sw/in_progress/ir_calibration/Makefile @@ -1,8 +1,11 @@ CC = gcc -CFLAGS=-g -O2 -Wall `pkg-config glib-2.0 --cflags` -LDFLAGS=`pkg-config glib-2.0 --libs` `pcre-config --libs` -lglibivy -lm +CFLAGS=-g -O2 -Wall `pkg-config gtk+-2.0 --cflags` +LDFLAGS=`pkg-config gtk+-2.0 --libs` `pcre-config --libs` -lglibivy -lm -lgtkdatabox -ir_calib : main.c - $(CC) $(CFLAGS) -g -o $@ $^ $(LDFLAGS) \ No newline at end of file +ir_calib : main.c calibrator.c gui.c + $(CC) $(CFLAGS) -g -o $@ $^ $(LDFLAGS) + +clean: + rm -f *~ ir_calib \ No newline at end of file diff --git a/sw/in_progress/ir_calibration/calibrator.c b/sw/in_progress/ir_calibration/calibrator.c new file mode 100644 index 0000000000..bb976d7736 --- /dev/null +++ b/sw/in_progress/ir_calibration/calibrator.c @@ -0,0 +1,163 @@ +#include "calibrator.h" + +#include +#include + +#include +#include +#include + +#define DEG_OF_RAD(a) ((a)/M_PI*180.) +#define RAD_OF_DEG(a) ((a)*M_PI/180.) +#define NORM_ANGLE_RAD(a) { while ((a) < -M_PI ) (a) += 2 * M_PI; while ((a) > M_PI ) (a) -= 2 * M_PI; } + +#define AC_ID 43 +#define IR_ROLL_NEUTRAL 0. +#define IR_360_LATERAL_CORRECTION 1. +#define IR_360_VERTICAL_CORRECTION 1. +#define IR_CORRECTION_RIGHT 1. +#define IR_CORRECTION_LEFT 1. + +#define DT 0.25 + +#define g 9.81 + +float est_airspeed; +float est_wind_dir; +float est_wind_speed; +float est_wind_east; +float est_wind_north; + +float gnd_ir_phi; +float estimator_phi; + + +float gps_gs_east; +float gps_gs_north; +float gps_gs_angle; +float gps_gs_norm; + +float gps_as_east; +float gps_as_north; +float gps_as_angle; +float gps_as_norm; + +float gps_phi; + +#define NB_POINTS 121 +float estimator_phi_by_degres[NB_POINTS]; +float alpha = 0.5; + +static void on_Attitude(IvyClientPtr app, void *user_data, int argc, char *argv[]); +static void on_GPS(IvyClientPtr app, void *user_data, int argc, char *argv[]); +static void on_Wind(IvyClientPtr app, void *user_data, int argc, char *argv[]); +static void on_IrSensors(IvyClientPtr app, void *user_data, int argc, char *argv[]); +static void reset_average(void); + + +static void on_Attitude(IvyClientPtr app, void *user_data, int argc, char *argv[]){ + //guint ac_id = atoi(argv[0]); + estimator_phi = RAD_OF_DEG(atof(argv[1])); + //g_message("attitude %d %f", ac_id, estimator_phi); + int gps_phi_deg = round(DEG_OF_RAD(gps_phi)); + if (fabs(gps_phi_deg) <= NB_POINTS/2) { + unsigned int idx = gps_phi_deg + NB_POINTS/2; + estimator_phi_by_degres[idx] = (1-alpha) * estimator_phi_by_degres[idx] + alpha * DEG_OF_RAD(estimator_phi); + } +} + +void on_GPS(IvyClientPtr app, void *user_data, int argc, char *argv[]){ + unsigned int ac_id = atoi(argv[0]); + float course = atof(argv[4]); + float speed = atof(argv[6]); + + gps_gs_norm = speed / 100.; + gps_gs_angle = RAD_OF_DEG(90. - course / 10.); + NORM_ANGLE_RAD( gps_gs_angle ); + + gps_gs_east = gps_gs_norm * cos(gps_gs_angle); + gps_gs_north = gps_gs_norm * sin(gps_gs_angle); + + gps_as_east = gps_gs_east - est_wind_east; + gps_as_north = gps_gs_north - est_wind_north; + + gps_as_angle = atan2(gps_as_north,gps_as_east); + gps_as_norm = sqrt(gps_as_east*gps_as_east + gps_as_north*gps_as_north); + + static float old_psi = 0.; + float delta_psi = gps_as_angle - old_psi; + old_psi = gps_as_angle; + NORM_ANGLE_RAD(delta_psi); + + /* tan(phi) = v^2 / (R*g) */ + /* R = (V * dt) / dpsi */ + + if (fabs(delta_psi) < 1e-6) + delta_psi = copysign(1e-6, delta_psi); + + float R = -gps_as_norm * DT / delta_psi; + + gps_phi = atan(gps_as_norm * gps_as_norm / R / g); + + printf("gps %d % 3.1f \t% 3.0f \t%.1f \t%.1f \t%.1f\n", ac_id, DEG_OF_RAD(delta_psi), R, DEG_OF_RAD(gps_phi), DEG_OF_RAD(estimator_phi), DEG_OF_RAD(gnd_ir_phi)); +} + +void on_Wind(IvyClientPtr app, void *user_data, int argc, char *argv[]){ + //guint ac_id = atoi(argv[1]); + est_wind_dir = atof(argv[2]); + est_wind_speed = atof(argv[3]); + est_airspeed = atof(argv[4]); + + float w_dir_rad =RAD_OF_DEG(270. - est_wind_dir); + NORM_ANGLE_RAD( w_dir_rad ); + + est_wind_east = est_wind_speed * cos( w_dir_rad ); + est_wind_north = est_wind_speed * sin( w_dir_rad ); + + //g_message("wind %d %f %f %f", ac_id, w_dir, w_speed, ac_aspeed); + //g_message("wind %f %f %f", w_dir_rad, est_wind_east, est_wind_north); +} + +void on_IrSensors(IvyClientPtr app, void *user_data, int argc, char *argv[]){ + //guint ac_id = atoi(argv[0]); + float lateral = atof(argv[2]); + float vertical = atof(argv[3]); + + float ir_roll = lateral * IR_360_LATERAL_CORRECTION; + float ir_top = vertical * IR_360_LATERAL_CORRECTION; + + gnd_ir_phi = atan2(ir_roll, ir_top) - IR_ROLL_NEUTRAL; + + if (gnd_ir_phi >= 0) + gnd_ir_phi *= IR_CORRECTION_RIGHT; + else + gnd_ir_phi *= IR_CORRECTION_LEFT; + + // g_message("ir_sensors %d %.0f %.0f (%.1f)", ac_id, lateral, vertical, DEG_OF_RAD(phi)); +} + +static void reset_average(void) { + + int i; + for (i=0; i +#include +#include +#include +#include +#include +#include + +#define NB_POINTS 121 + +gfloat *X; +gfloat *Y1; +gfloat *Y2; + +static GtkWidget* databox; + +GtkWidget* gui_init(void) { + + GtkWidget *window = gtk_window_new (GTK_WINDOW_TOPLEVEL); + gtk_widget_set_size_request (window, 640, 400); + + GtkWidget *vbox1 = gtk_vbox_new (FALSE, 0); + gtk_container_add (GTK_CONTAINER (window), vbox1); + + GtkWidget *frame = gtk_frame_new ("Foo"); + gtk_container_set_border_width (GTK_CONTAINER (frame), 10); + gtk_box_pack_start (GTK_BOX (vbox1), frame, TRUE, TRUE, 0); + + databox = gtk_databox_new (); + gtk_container_add (GTK_CONTAINER (frame), databox ); + + X = g_new0 (gfloat, NB_POINTS); + Y1 = g_new0 (gfloat, NB_POINTS); + Y2 = g_new0 (gfloat, NB_POINTS); + guint i; + for (i=0; i + +extern GtkWidget* gui_init(void); + +extern void gui_update(gfloat* values); + +#endif /* GUI_H */ diff --git a/sw/in_progress/ir_calibration/main.c b/sw/in_progress/ir_calibration/main.c index 38d82bbb96..7c3e03797c 100644 --- a/sw/in_progress/ir_calibration/main.c +++ b/sw/in_progress/ir_calibration/main.c @@ -3,113 +3,33 @@ #include #include -#include -#include - -#define DEG_OF_RAD(a) (a/M_PI*180.) -#define RAD_OF_DEG(a) (a*M_PI/180.) - -#define AC_ID 43 -#define IR_ROLL_NEUTRAL 0. -#define IR_360_LATERAL_CORRECTION 1. -#define IR_360_VERTICAL_CORRECTION 1. -#define IR_CORRECTION_RIGHT 1. -#define IR_CORRECTION_LEFT 1. - -#define AIRSPEED 12.6 -#define DT 0.25 - -#define g 9.81 - -float gnd_ir_phi; -float estimator_phi; -float gps_phi; +#include "calibrator.h" +#include "gui.h" gboolean timeout_callback(gpointer data) { + float* values = calibrator_get_values(); + gui_update(values); return TRUE; + } -void on_Attitude(IvyClientPtr app, void *user_data, int argc, char *argv[]){ - guint ac_id = atoi(argv[0]); - estimator_phi = RAD_OF_DEG(atof(argv[1])); - - // g_message("attitude %d %f", ac_id, estimator_phi); -} - -void on_GPS(IvyClientPtr app, void *user_data, int argc, char *argv[]){ - guint ac_id = atoi(argv[0]); - float course = atof(argv[4]); - - float angle_gspeed_deg = 90. - course / 10.; - if (angle_gspeed_deg < -180.) angle_gspeed_deg += 360.; - if (angle_gspeed_deg > 180.) angle_gspeed_deg -= 360.; - - float angle_gspeed_rad = RAD_OF_DEG(angle_gspeed_deg); - static float old_psi = 0.; - float delta_psi = angle_gspeed_rad - old_psi; - if (delta_psi < -M_PI ) delta_psi = 2 * M_PI - delta_psi; - if (delta_psi > M_PI ) delta_psi = delta_psi - 2 * M_PI; - - old_psi = angle_gspeed_rad; - - /* tan(phi) = v^2 / (R*g) */ - /* R = (V * dt) / dpsi */ - - if (fabs(delta_psi) < 1e-6) - delta_psi = copysign(1e-6, delta_psi); - - float R = -AIRSPEED * DT / delta_psi; - - gps_phi = atan(AIRSPEED * AIRSPEED / R / g); - - - - g_message("gps %d % 3.1f \t% 3.0f \t%.1f \t%.1f \t%.1f", ac_id, angle_gspeed_deg, R, DEG_OF_RAD(gps_phi), DEG_OF_RAD(estimator_phi), DEG_OF_RAD(gnd_ir_phi)); -} - -void on_Wind(IvyClientPtr app, void *user_data, int argc, char *argv[]){ - guint ac_id = atoi(argv[1]); - float w_dir = atof(argv[2]); // deg - float w_speed = atof(argv[3]); - float ac_aspeed = atof(argv[4]); - - // g_message("wind %d %f %f %f", ac_id, w_dir, w_speed, ac_aspeed); -} - -void on_IrSensors(IvyClientPtr app, void *user_data, int argc, char *argv[]){ - guint ac_id = atoi(argv[0]); - float lateral = atof(argv[2]); - float vertical = atof(argv[3]); - - float ir_roll = lateral * IR_360_LATERAL_CORRECTION; - float ir_top = vertical * IR_360_LATERAL_CORRECTION; - - gnd_ir_phi = atan2(ir_roll, ir_top) - IR_ROLL_NEUTRAL; - - if (gnd_ir_phi >= 0) - gnd_ir_phi *= IR_CORRECTION_RIGHT; - else - gnd_ir_phi *= IR_CORRECTION_LEFT; - - // g_message("ir_sensors %d %.0f %.0f (%.1f)", ac_id, lateral, vertical, DEG_OF_RAD(phi)); -} int main ( int argc, char** argv) { - GMainLoop *ml = g_main_loop_new(NULL, FALSE); + // GMainLoop *ml = g_main_loop_new(NULL, FALSE); + gtk_init(&argc, &argv); + g_timeout_add(500, timeout_callback, NULL); - IvyInit ("IrCalib", "IrCalib READY", NULL, NULL, NULL, NULL); - IvyBindMsg(on_Attitude, NULL, "^(\\S*) ATTITUDE (\\S*) (\\S*) (\\S*)"); - IvyBindMsg(on_GPS, NULL, "^(\\S*) GPS (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)"); - IvyBindMsg(on_Wind, NULL, "^(\\S*) WIND (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)"); - IvyBindMsg(on_IrSensors, NULL, "^(\\S*) IR_SENSORS (\\S*) (\\S*) (\\S*)"); - IvyStart("127.255.255.255"); + calibrator_init(); + + gui_init(); - g_main_loop_run(ml); + //g_main_loop_run(ml); + gtk_main(); return 0; }