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));
}
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_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);
}
#include "geometry.h"
//#include <math.h>
#include "math_papabench.h"
const struct ellipsoid ellipsoids[ELLIPSOID_NB] = {
{-168., -60., 320., 6378249.2, 0.0034075495234250643, 0.08248325676}, /* NTF */
{ 0., 0., 0., 6378137.0, 0.0033528106647474805, 0.08181919106}, /* WGS84 */
{-87.0, -98., -121.0, 6378388.0, 0.003367003367003367, 0.08199188998}, /* ED50 */
{ 0.0, 125.0, 194.0, 6378067.6, 0.0033155460257, 0.08181919106} /* NAD27 */
};
/* From http://www.tandt.be/wis/WiS/eqntf.html and http://www.ign.fr/MP/GEOD/geodesie/coordonnees.html */
const struct lambert lamberts[LAMBERT_NB] = {
{ /* LAMBERTI */
&ellipsoids[NTF],
RAD_OF_DEG(DECIMAL(49., 30., 0.)), /* phi0 */
0.991996665, /* lphi0 */
5457616.674, /* r0 */
RAD_OF_DEG(DECIMAL(2., 20., 14.025)), /* lambda0 */
600000, /* x0 */
200000, /* y0 */
5657617, /* ys */
0.99987734 /* k0 */
},
{ /* LAMBERTII */
&ellipsoids[NTF],
RAD_OF_DEG(DECIMAL(46., 48., 0.)), /* phi0 */
0.921557361, /* lphi0 */
5999695.77, /* r0 */
RAD_OF_DEG(DECIMAL(2., 20., 14.025)), /* lambda0 */
600000, /* x0 */