Exemplo n.º 1
0
int main(void)
{
   i2c_bus_t bus;
   int ret = i2c_bus_open(&bus, "/dev/i2c-3");
   if (ret < 0)
   {
      fatal("could not open i2c bus", ret);
      return EXIT_FAILURE;
   }

   /* ITG: */
   itg3200_dev_t itg;
itg_again:
   ret = itg3200_init(&itg, &bus, ITG3200_DLPF_42HZ);
   if (ret < 0)
   {
      fatal("could not inizialize ITG3200", ret);
      if (ret == -EAGAIN)
      {
         goto itg_again;   
      }
      return EXIT_FAILURE;
   }

   /* BMA: */
   bma180_dev_t bma;
   bma180_init(&bma, &bus, BMA180_RANGE_4G, BMA180_BW_10HZ);

   /* HMC: */
   hmc5883_dev_t hmc;
   hmc5883_init(&hmc, &bus);
   
   /* MS: */
   ms5611_dev_t ms;
   ret = ms5611_init(&ms, &bus, MS5611_OSR4096, MS5611_OSR4096);
   if (ret < 0)
   {
      fatal("could not inizialize MS5611", ret);
      return EXIT_FAILURE;
   }
   pthread_t thread;
   pthread_create(&thread, NULL, ms5611_reader, &ms);

   /* initialize AHRS filter: */
   madgwick_ahrs_t madgwick_ahrs;
   madgwick_ahrs_init(&madgwick_ahrs, STANDARD_BETA);

   interval_t interval;
   interval_init(&interval);
   float init = START_BETA;
   udp_socket_t *socket = udp_socket_create("10.0.0.100", 5005, 0, 0);

   /* kalman filter: */
   kalman_t kalman1, kalman2, kalman3;
   kalman_init(&kalman1, 1.0e-6, 1.0e-2, 0, 0);
   kalman_init(&kalman2, 1.0e-6, 1.0e-2, 0, 0);
   kalman_init(&kalman3, 1.0e-6, 1.0e-2, 0, 0);
   vec3_t global_acc; /* x = N, y = E, z = D */
   int init_done = 0;
   int converged = 0;
   sliding_avg_t *avg[3];
   avg[0] = sliding_avg_create(1000, 0.0);
   avg[1] = sliding_avg_create(1000, 0.0);
   avg[2] = sliding_avg_create(1000, -9.81);
   float alt_rel_last = 0.0;
   int udp_cnt = 0;
   while (1)
   {
      int i;
      float dt = interval_measure(&interval);
      init -= BETA_STEP;
      if (init < FINAL_BETA)
      {
         init = FINAL_BETA;
         init_done = 1;
      }
      madgwick_ahrs.beta = init;
      
      /* sensor data acquisition: */
      itg3200_read_gyro(&itg);
      bma180_read_acc(&bma);
      hmc5883_read(&hmc);
      
      /* state estimates and output: */
      euler_t euler;
      madgwick_ahrs_update(&madgwick_ahrs, itg.gyro.x, itg.gyro.y, itg.gyro.z, bma.raw.x, bma.raw.y, bma.raw.z, hmc.raw.x, hmc.raw.y, hmc.raw.z, 11.0, dt);
      
      quat_t q_body_to_world;
      quat_copy(&q_body_to_world, &madgwick_ahrs.quat);
      quat_rot_vec(&global_acc, &bma.raw, &q_body_to_world);
      for (i = 0; i < 3; i++)
      {
         global_acc.vec[i] -= sliding_avg_calc(avg[i], global_acc.vec[i]);
      }
      if (init_done)
      {
         kalman_in_t kalman_in;
         kalman_in.dt = dt;
         kalman_in.pos = 0;
         kalman_out_t kalman_out;

         kalman_in.acc = global_acc.x;
         kalman_run(&kalman_out, &kalman1, &kalman_in);
         kalman_in.acc = global_acc.y;
         kalman_run(&kalman_out, &kalman2, &kalman_in);
         kalman_in.acc = -global_acc.z;
         pthread_mutex_lock(&mutex);
         kalman_in.pos = alt_rel;
         pthread_mutex_unlock(&mutex);
         kalman_run(&kalman_out, &kalman3, &kalman_in);
         if (!converged)
         {
            if (fabs(kalman_out.pos - alt_rel) < 0.1)
            {
               converged = 1;   
               fprintf(stderr, "init done\n");
            }
         }
         if (converged) // && udp_cnt++ > 10)
         {
            if (udp_cnt++ == 10)
            {
               char buffer[1024];
               udp_cnt = 0;
               int len = sprintf(buffer, "%f %f %f %f %f %f %f", madgwick_ahrs.quat.q0, madgwick_ahrs.quat.q1, madgwick_ahrs.quat.q2, madgwick_ahrs.quat.q3,
                                                                 global_acc.x, global_acc.y, global_acc.z);
               udp_socket_send(socket, buffer, len);
            }
            printf("%f %f %f\n", -global_acc.z, alt_rel, kalman_out.pos);
            fflush(stdout);
         }
      }

      
   }
   return 0;
}
Exemplo n.º 2
0
void _main(int argc, char *argv[])
{
   (void)argc;
   (void)argv;
   syslog(LOG_INFO, "initializing core");
   
   /* init SCL subsystem: */
   syslog(LOG_INFO, "initializing signaling and communication link (SCL)");
   if (scl_init("core") != 0)
   {
      syslog(LOG_CRIT, "could not init scl module");
      exit(EXIT_FAILURE);
   }
   
   /* init params subsystem: */
   syslog(LOG_INFO, "initializing opcd interface");
   opcd_params_init("core.", 1);
   
   /* initialize logger: */
   syslog(LOG_INFO, "opening logger");
   if (logger_open() != 0)
   {
      syslog(LOG_CRIT, "could not open logger");
      exit(EXIT_FAILURE);
   }
   syslog(LOG_CRIT, "logger opened");
   sleep(1); /* give scl some time to establish
                a link between publisher and subscriber */

   LOG(LL_INFO, "+------------------+");
   LOG(LL_INFO, "|   core startup   |");
   LOG(LL_INFO, "+------------------+");

   LOG(LL_INFO, "initializing system");

   /* set-up real-time scheduling: */
   struct sched_param sp;
   sp.sched_priority = sched_get_priority_max(SCHED_FIFO);
   sched_setscheduler(getpid(), SCHED_FIFO, &sp);
   if (mlockall(MCL_CURRENT | MCL_FUTURE))
   {
      LOG(LL_ERROR, "mlockall() failed");
      exit(EXIT_FAILURE);
   }

   /* initialize hardware/drivers: */
   omap_i2c_bus_init();
   baro_altimeter_init();
   ultra_altimeter_init();
   ahrs_init();
   motors_init();
   voltage_reader_start();
   //gps_init();
   
   LOG(LL_INFO, "initializing model/controller");
   model_init();
   ctrl_init();
   
   /* initialize command interface */
   LOG(LL_INFO, "initializing cmd interface");
   cmd_init();
   
   /* prepare main loop: */
   for (int i = 0; i < NUM_AVG; i++)
   {
      output_avg[i] = sliding_avg_create(OUTPUT_RATIO, 0.0f);
   }

   LOG(LL_INFO, "system up and running");
   struct timespec ts_curr;
   struct timespec ts_prev;
   struct timespec ts_diff;
   clock_gettime(CLOCK_REALTIME, &ts_curr);
 
   /* run model and controller: */
   while (1)
   {
      /* calculate dt: */
      ts_prev = ts_curr;
      clock_gettime(CLOCK_REALTIME, &ts_curr);
      TIMESPEC_SUB(ts_diff, ts_curr, ts_prev);
      float dt = (float)ts_diff.tv_sec + (float)ts_diff.tv_nsec / (float)NSEC_PER_SEC;

      /* read sensor values into model input structure: */
      model_input_t model_input;
      model_input.dt = dt;
      ahrs_read(&model_input.ahrs_data);
      gps_read(&model_input.gps_data);
      model_input.ultra_z = ultra_altimeter_read();
      model_input.baro_z = baro_altimeter_read();

      /* execute model step: */
      model_state_t model_state;
      model_step(&model_state, &model_input);

      /* execute controller step: */
      mixer_in_t mixer_in;
      ctrl_step(&mixer_in, dt, &model_state);
 
      /* set up mixer input: */
      mixer_in.pitch = sliding_avg_calc(output_avg[AVG_PITCH], mixer_in.pitch);
      mixer_in.roll = sliding_avg_calc(output_avg[AVG_ROLL], mixer_in.roll);
      mixer_in.yaw = sliding_avg_calc(output_avg[AVG_YAW], mixer_in.yaw);
      mixer_in.gas = sliding_avg_calc(output_avg[AVG_GAS], mixer_in.gas);

      /* write data to motor mixer: */
      EVERY_N_TIMES(OUTPUT_RATIO, motors_write(&mixer_in));
   }
}