mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-09 22:49:53 +08:00
[logalizer] update ahrs2fg display
bind to more messages update Flight Gear compatibility more command line options
This commit is contained in:
Gautier Hattenberger
+130
-23
@@ -2,6 +2,7 @@
|
||||
#include <stdio.h>
|
||||
#include <math.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
|
||||
#include "flight_gear.h"
|
||||
#include "network.h"
|
||||
@@ -9,7 +10,26 @@
|
||||
#include <Ivy/ivy.h>
|
||||
#include <Ivy/ivyglibloop.h>
|
||||
|
||||
gfloat biases[3];
|
||||
// Default Host
|
||||
char default_fg_host[] = "127.0.0.1";
|
||||
char* fg_host;
|
||||
// Default TCP port
|
||||
int fg_port = 5501;
|
||||
|
||||
#ifdef __APPLE__
|
||||
char defaultIvyBus[] = "224.255.255.255:2010";
|
||||
#else
|
||||
char defaultIvyBus[] = "127.255.255.255:2010";
|
||||
#endif
|
||||
char* IvyBus;
|
||||
|
||||
int verbose = FALSE;
|
||||
|
||||
// quat int res
|
||||
const float quat_int_res = (float)(1<<15);
|
||||
// euler int res
|
||||
const float euler_int_res = (float)(1<<12);
|
||||
|
||||
gfloat quat[4];
|
||||
gfloat eulers[3];
|
||||
|
||||
@@ -17,7 +37,7 @@ gboolean timeout_callback(gpointer data) {
|
||||
|
||||
static double x = 0.;
|
||||
static double y = 0.;
|
||||
static double z = 10;
|
||||
static double z = 10.;
|
||||
const double earth_radius = 6372795.;
|
||||
|
||||
double lat = 0.656480 + asin(x/earth_radius);
|
||||
@@ -30,7 +50,10 @@ gboolean timeout_callback(gpointer data) {
|
||||
gui.longitude = lon;
|
||||
gui.altitude = z;
|
||||
|
||||
// printf("%f %f %f\n", eulers[0], eulers[1], eulers[2]);
|
||||
if (verbose) {
|
||||
printf("phi:%f, theta:%f, psi:%f\n", eulers[0], eulers[1], eulers[2]);
|
||||
fflush(stdout);
|
||||
}
|
||||
gui.phi = eulers[0];
|
||||
gui.theta = eulers[1];
|
||||
gui.psi = eulers[2];
|
||||
@@ -51,35 +74,119 @@ void quat_to_euler( gfloat* quat, gfloat* euler) {
|
||||
euler[2] = atan2( 2*(quat[1]*quat[2] + quat[0]*quat[3]),(1-2*(q22 + q32)) );
|
||||
}
|
||||
|
||||
void on_AHRS_STATE(IvyClientPtr app, void *user_data, int argc, char *argv[]){
|
||||
float q0 = atof(argv[0]);
|
||||
float q1 = atof(argv[1]);
|
||||
float q2 = atof(argv[2]);
|
||||
float q3 = atof(argv[3]);
|
||||
float bx = atof(argv[4]);
|
||||
float by = atof(argv[5]);
|
||||
float bz = atof(argv[6]);
|
||||
biases[0] = bx;
|
||||
biases[1] = by;
|
||||
biases[2] = bz;
|
||||
quat[0] = q0;
|
||||
quat[1] = q1;
|
||||
quat[2] = q2;
|
||||
quat[3] = q3;
|
||||
void on_ATTITUDE(IvyClientPtr app, void *user_data, int argc, char *argv[]){
|
||||
float phi = atof(argv[0]);
|
||||
float psi = atof(argv[1]);
|
||||
float theta = atof(argv[2]);
|
||||
eulers[0] = phi;
|
||||
eulers[1] = theta;
|
||||
eulers[2] = psi;
|
||||
}
|
||||
|
||||
void on_AHRS_EULER_INT(IvyClientPtr app, void *user_data, int argc, char *argv[]){
|
||||
float phi = atof(argv[3]);
|
||||
float theta = atof(argv[4]);
|
||||
float psi = atof(argv[5]);
|
||||
eulers[0] = phi / euler_int_res;
|
||||
eulers[1] = theta / euler_int_res;
|
||||
eulers[2] = psi / euler_int_res;
|
||||
}
|
||||
|
||||
void on_AHRS_QUAT_INT(IvyClientPtr app, void *user_data, int argc, char *argv[]){
|
||||
float q0 = atof(argv[5]);
|
||||
float q1 = atof(argv[6]);
|
||||
float q2 = atof(argv[7]);
|
||||
float q3 = atof(argv[8]);
|
||||
quat[0] = q0 / quat_int_res;
|
||||
quat[1] = q1 / quat_int_res;
|
||||
quat[2] = q2 / quat_int_res;
|
||||
quat[3] = q3 / quat_int_res;
|
||||
quat_to_euler(quat, eulers);
|
||||
}
|
||||
|
||||
void on_ROTORCRAFT_FP(IvyClientPtr app, void *user_data, int argc, char *argv[]){
|
||||
float phi = atof(argv[6]);
|
||||
float theta = atof(argv[7]);
|
||||
float psi = atof(argv[8]);
|
||||
eulers[0] = phi / euler_int_res;
|
||||
eulers[1] = theta / euler_int_res;
|
||||
eulers[2] = psi / euler_int_res;
|
||||
}
|
||||
|
||||
// Print help message
|
||||
void print_help() {
|
||||
printf("Usage: ahrs2fg [options] <AC_ID>\n");
|
||||
printf(" Options :\n");
|
||||
printf(" -fg_host <host>\tFlight Gear host address (default: %s)\n", fg_host);
|
||||
printf(" -fg_port <port>\tFlight Gear port (default: %d)\n", fg_port);
|
||||
printf(" -b <Ivy bus>\tdefault is %s\n", defaultIvyBus);
|
||||
printf(" -v\tverbose\n");
|
||||
printf(" -h --help show this help\n");
|
||||
}
|
||||
|
||||
|
||||
int main ( int argc, char** argv) {
|
||||
struct FgNetChannel* chan = open_out_channel("127.0.0.1", 5501);
|
||||
|
||||
g_timeout_add(16, timeout_callback, chan);
|
||||
int ac_id = -1;
|
||||
|
||||
IvyBus = getenv("IVYBUS");
|
||||
if (IvyBus == NULL) IvyBus = defaultIvyBus;
|
||||
|
||||
fg_host = default_fg_host;
|
||||
|
||||
if (argc < 1) {
|
||||
print_help();
|
||||
exit(0);
|
||||
}
|
||||
|
||||
// Parse options
|
||||
int i;
|
||||
for (i = 1; i < argc; ++i) {
|
||||
if (strcmp(argv[i], "--help") == 0 || strcmp(argv[i], "-h") == 0) {
|
||||
print_help();
|
||||
exit(0);
|
||||
}
|
||||
else if (strcmp(argv[i], "-fg_host") == 0) {
|
||||
fg_host = argv[++i];
|
||||
}
|
||||
else if (strcmp(argv[i], "-fg_port") == 0) {
|
||||
fg_port = atoi(argv[++i]);
|
||||
}
|
||||
else if (strcmp(argv[i], "-b") == 0) {
|
||||
IvyBus = argv[++i];
|
||||
}
|
||||
else if (strcmp(argv[i], "-v") == 0) {
|
||||
verbose = TRUE;
|
||||
}
|
||||
else {
|
||||
ac_id = atoi(argv[i]);
|
||||
}
|
||||
}
|
||||
if (ac_id == -1) {
|
||||
print_help();
|
||||
exit(0);
|
||||
}
|
||||
|
||||
if (verbose) {
|
||||
printf("FG options: %s:%d\n", fg_host, fg_port);
|
||||
fflush(stdout);
|
||||
}
|
||||
struct FgNetChannel* chan = open_out_channel(fg_host, fg_port);
|
||||
|
||||
g_timeout_add(50, timeout_callback, chan);
|
||||
|
||||
GMainLoop *ml = g_main_loop_new(NULL, FALSE);
|
||||
|
||||
IvyInit ("IvyGtkButton", "IvyGtkButton READY", NULL, NULL, NULL, NULL);
|
||||
IvyBindMsg(on_AHRS_STATE, chan, "^77 AHRS_STATE (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)");
|
||||
IvyStart("127.255.255.255");
|
||||
IvyInit ("ahrs2fg", "ahrs2fg READY", NULL, NULL, NULL, NULL);
|
||||
IvyBindMsg(on_ATTITUDE, chan, "^%d ATTITUDE (\\S*) (\\S*) (\\S*)", ac_id);
|
||||
IvyBindMsg(on_AHRS_EULER_INT, chan, "^%d AHRS_EULER_INT (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)", ac_id);
|
||||
IvyBindMsg(on_AHRS_QUAT_INT, chan, "^%d AHRS_QUAT_INT (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)", ac_id);
|
||||
IvyBindMsg(on_ROTORCRAFT_FP, chan, "^%d ROTORCRAFT_FP (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)", ac_id);
|
||||
if (verbose) {
|
||||
printf("Ivy options: %s\n", IvyBus);
|
||||
fflush(stdout);
|
||||
}
|
||||
IvyStart(IvyBus);
|
||||
|
||||
g_main_loop_run(ml);
|
||||
return 0;
|
||||
|
||||
@@ -114,6 +114,7 @@ struct FGNetCtrls {
|
||||
#define FG_NET_FDM_MAX_WHEELS 3
|
||||
#define FG_NET_FDM_MAX_TANKS 4
|
||||
|
||||
#ifndef _NET_FDM_HXX
|
||||
|
||||
struct FGNetFDM {
|
||||
|
||||
@@ -199,6 +200,8 @@ struct FGNetFDM {
|
||||
float spoilers;
|
||||
};
|
||||
|
||||
#endif
|
||||
|
||||
struct FGNetMiniFDM {
|
||||
uint32_t version; // increment when data values change
|
||||
|
||||
@@ -224,10 +227,33 @@ struct FGNetMiniFDM {
|
||||
int32_t warp; // offset in seconds to unix time
|
||||
};
|
||||
|
||||
#if FG_2_4
|
||||
#define FG_NET_GUI_VERSION 7
|
||||
#else
|
||||
#define FG_NET_GUI_VERSION 8
|
||||
#endif /*FG_2_4*/
|
||||
|
||||
#define FG_NET_GUI_MAX_TANKS 4
|
||||
|
||||
// Prior to FG_NET_GUI_VERSION 8, OS X needed #pragma pack(4) to
|
||||
// properly display FG visualization data. In version 8 they added
|
||||
// a padding1 element to ensure proper data alignment, so this is
|
||||
// no longer required. The rest of this struct is based on FG source
|
||||
// in src/Network/net_gui.hxx
|
||||
|
||||
#if FG_2_4
|
||||
#ifdef __x86_64__
|
||||
#pragma pack(push)
|
||||
#ifdef __APPLE__
|
||||
#pragma pack(4)
|
||||
#else
|
||||
#pragma pack(8)
|
||||
#endif /*__APPLE__*/
|
||||
#endif /*__x86_64__*/
|
||||
#endif /*FG_2_4*/
|
||||
struct FGNetGUI {
|
||||
uint32_t version; // increment when data values change
|
||||
uint32_t padding1;
|
||||
|
||||
// Positions
|
||||
double longitude; // geodetic (radians)
|
||||
@@ -260,6 +286,13 @@ struct FGNetGUI {
|
||||
float course_deviation_deg; // degrees off target course
|
||||
float gs_deviation_deg; // degrees off target glide slope
|
||||
};
|
||||
#if FG_2_4
|
||||
#ifdef __x86_64__
|
||||
#pragma pack(push)
|
||||
#pragma pack(pop)
|
||||
#endif /*__x86_64__*/
|
||||
#endif /*FG_2_4*/
|
||||
|
||||
|
||||
extern void net_fdm_dump (struct FGNetFDM* fdm);
|
||||
extern void net_fdm_ntoh (struct FGNetFDM* fdm);
|
||||
|
||||
Reference in New Issue
Block a user