Exemple #1
0
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));
}
Exemple #2
0
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);
  }
}
Exemple #3
0
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      */