mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-25 23:46:04 +08:00
*** empty log message ***
This commit is contained in:
Antoine Drouin
@@ -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)
|
||||
ir_calib : main.c calibrator.c gui.c
|
||||
$(CC) $(CFLAGS) -g -o $@ $^ $(LDFLAGS)
|
||||
|
||||
clean:
|
||||
rm -f *~ ir_calib
|
||||
@@ -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;
|
||||
}
|
||||
@@ -0,0 +1,8 @@
|
||||
#ifndef CALIBRATOR_H
|
||||
#define CALIBRATOR_H
|
||||
|
||||
|
||||
extern void calibrator_init(void);
|
||||
extern float* calibrator_get_values(void);
|
||||
|
||||
#endif /* CALIBRATOR_H */
|
||||
@@ -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.);
|
||||
}
|
||||
@@ -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 */
|
||||
@@ -3,113 +3,33 @@
|
||||
#include <math.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) {
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user