Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-31 12:23:23 +08:00
Code Issues Actions 26 Packages Projects Releases Wiki Activity

*** empty log message ***

Browse Source
This commit is contained in:
Antoine Drouin
2007-10-21 16:55:37 +00:00
parent aa501591e5
commit f7c98dbda5
6 changed files with 269 additions and 97 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/in_progress/ir_calibration/Makefile
+7 -4
View File
@@ -1,8 +1,11 @@
CC = gcc CC = gcc
CFLAGS=-g -O2 -Wall `pkg-config glib-2.0 --cflags` CFLAGS=-g -O2 -Wall `pkg-config gtk+-2.0 --cflags`
LDFLAGS=`pkg-config glib-2.0 --libs` `pcre-config --libs` -lglibivy -lm LDFLAGS=`pkg-config gtk+-2.0 --libs` `pcre-config --libs` -lglibivy -lm -lgtkdatabox
ir_calib : main.c ir_calib : main.c calibrator.c gui.c
$(CC) $(CFLAGS) -g -o $@ $^ $(LDFLAGS) $(CC) $(CFLAGS) -g -o $@ $^ $(LDFLAGS)
clean:
rm -f *~ ir_calib
sw/in_progress/ir_calibration/calibrator.c
+163
View File
@@ -0,0 +1,163 @@
#include "calibrator.h"
#include <Ivy/ivy.h>
#include <Ivy/ivyglibloop.h>
#include <stdlib.h>
#include <math.h>
#include <stdio.h>
#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<NB_POINTS; i++) {
estimator_phi_by_degres[i] = i-NB_POINTS/2;
}
}
void calibrator_init(void) {
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");
reset_average();
}
float* calibrator_get_values(void) {
return estimator_phi_by_degres;
}
sw/in_progress/ir_calibration/calibrator.h
+8
View File
@@ -0,0 +1,8 @@
#ifndef CALIBRATOR_H
#define CALIBRATOR_H
extern void calibrator_init(void);
extern float* calibrator_get_values(void);
#endif /* CALIBRATOR_H */
sw/in_progress/ir_calibration/gui.c
+68
View File
@@ -0,0 +1,68 @@
#include "gui.h"
#include <gtkdatabox.h>
#include <gtkdatabox_points.h>
#include <gtkdatabox_lines.h>
#include <gtkdatabox_bars.h>
#include <gtkdatabox_grid.h>
#include <gtkdatabox_cross_simple.h>
#include <gtkdatabox_marker.h>
#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<NB_POINTS; i++){
X[i] = (gfloat)i - NB_POINTS/2;
Y1[i] = (gfloat)i -NB_POINTS/2;
Y2[i] = (gfloat)i -NB_POINTS/2;
}
GdkColor cblack = {0, 0, 0, 0};
GdkColor cred = {0, 65535, 0, 0};
GtkDataboxGraph *graph1 = gtk_databox_lines_new (NB_POINTS, X, Y1, &cblack, 2);
gtk_databox_graph_add (GTK_DATABOX (databox), graph1);
GtkDataboxGraph *graph2 = gtk_databox_lines_new (NB_POINTS, X, Y2, &cred, 2);
gtk_databox_graph_add (GTK_DATABOX (databox), graph2);
GtkDataboxGraph *grid = gtk_databox_grid_new (11, 11, &cblack, 1);
gtk_databox_graph_add (GTK_DATABOX (databox), grid);
gtk_databox_auto_rescale (GTK_DATABOX(databox), 0.);
gtk_widget_show_all(window);
return window;
}
void gui_update(gfloat* values) {
guint i;
for (i=0; i<NB_POINTS; i++){
Y2[i] = values[i];
}
gtk_databox_auto_rescale (GTK_DATABOX(databox), 0.);
}
sw/in_progress/ir_calibration/gui.h
+10
View File
@@ -0,0 +1,10 @@
#ifndef GUI_H
#define GUI_H
#include <gtk/gtk.h>
extern GtkWidget* gui_init(void);
extern void gui_update(gfloat* values);
#endif /* GUI_H */
sw/in_progress/ir_calibration/main.c
+13 -93
View File
@@ -3,113 +3,33 @@
#include <math.h> #include <math.h>
#include <stdlib.h> #include <stdlib.h>
#include <Ivy/ivy.h>
#include <Ivy/ivyglibloop.h>
#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) { gboolean timeout_callback(gpointer data) {
float* values = calibrator_get_values();
gui_update(values);
return TRUE; 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) { 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); g_timeout_add(500, timeout_callback, NULL);
IvyInit ("IrCalib", "IrCalib READY", NULL, NULL, NULL, NULL); calibrator_init();
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*)"); gui_init();
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");
g_main_loop_run(ml); //g_main_loop_run(ml);
gtk_main();
return 0; return 0;
} }