예제 #1
0
int drotek_marg2_read(marg_data_t *data, drotek_marg2_t *marg2)
{
   THROW_BEGIN();
   THROW_ON_ERR(mpu6050_read(&marg2->mpu, &data->gyro, &data->acc, NULL));
   THROW_ON_ERR(hmc5883_read_mag(&data->mag, &marg2->hmc));
   THROW_END();
}
예제 #2
0
int drotek_marg2_init(drotek_marg2_t *marg2, i2c_bus_t *bus)
{
   THROW_BEGIN();
   THROW_ON_ERR(mpu6050_init(&marg2->mpu, bus, MPU6050_DLPF_CFG_94_98Hz, MPU6050_FS_SEL_500, MPU6050_AFS_SEL_4G));
   THROW_ON_ERR(hmc5883_init(&marg2->hmc, bus));
   THROW_END();
}
예제 #3
0
int drotek_marg_read(marg_data_t *data, drotek_marg_t *marg)
{
   THROW_BEGIN();
   THROW_ON_ERR(itg3200_read_gyro(data->gyro.ve, &marg->itg));
   THROW_ON_ERR(bma180_read_acc(data->acc.ve, &marg->bma));
   THROW_ON_ERR(hmc5883_read_mag(&data->mag, &marg->hmc));
   THROW_END();
}
예제 #4
0
int drotek_marg_init(drotek_marg_t *marg, i2c_bus_t *bus)
{
   THROW_BEGIN();
   THROW_ON_ERR(itg3200_init(&marg->itg, bus, ITG3200_DLPF_98HZ));
   THROW_ON_ERR(bma180_init(&marg->bma, bus, BMA180_RANGE_4G, BMA180_BW_40HZ));
   THROW_ON_ERR(hmc5883_init(&marg->hmc, bus));
   THROW_END();
}
예제 #5
0
static int ms5611_read_adc(uint32_t *val, ms5611_t *ms5611)
{
   THROW_BEGIN();
   uint8_t raw[3]; /* 24-bit adc data */
   THROW_ON_ERR(i2c_read_block_reg(&ms5611->i2c_dev, MS5611_ADC, raw, sizeof(raw)));
   *val = raw[2] | (raw[1] << 8) | (raw[0] << 16);
   THROW_END();
}
예제 #6
0
/* reads prom register into val and returns 0
 * if the read failed, val remains untouched
 * and a negative error code is returned */
static int ms5611_read_prom(ms5611_t *ms5611, uint8_t reg)
{
   THROW_BEGIN();
   uint8_t raw[2];
   THROW_ON_ERR(i2c_read_block_reg(&ms5611->i2c_dev, MS5611_PROM_READ(reg), raw, sizeof(raw)));
   ms5611->prom[reg] = raw[1] | (raw[0] << 8);
   THROW_END();
}
예제 #7
0
int generic_platform_init(platform_t *plat)
{
   ASSERT_ONCE();
   THROW_BEGIN();
   LOG(LL_INFO, "initializing power reader");
   THROW_ON_ERR(scl_power_init());
   plat->read_power = scl_power_read;
   
   LOG(LL_INFO, "initializing remote control reader");
   THROW_ON_ERR(scl_rc_init());
   plat->read_rc = scl_rc_read;
 
   LOG(LL_INFO, "initializing GPS reader");
   THROW_ON_ERR(scl_gps_init());
   plat->read_gps = scl_gps_read;

   THROW_END();
}
예제 #8
0
파일: main.c 프로젝트: f599gtb/PenguPilot
int _main(void)
{
   THROW_BEGIN()

   /* init scl and get sockets:: */
   THROW_ON_ERR(scl_init("arduino"));
   void *rc_socket = scl_get_socket("rc_raw");
   THROW_IF(rc_socket == NULL, -ENODEV);
   void *power_socket = scl_get_socket("power");
   THROW_IF(power_socket == NULL, -ENODEV);

   /* allocate msgpack buffers: */
   msgpack_sbuffer *msgpack_buf = msgpack_sbuffer_new();
   THROW_IF(msgpack_buf == NULL, -ENOMEM);
   msgpack_packer *pk = msgpack_packer_new(msgpack_buf, msgpack_sbuffer_write);
   THROW_IF(pk == NULL, -ENOMEM);
 
   /* fetch parameters: */
   char *dev_path;
   tsint_t dev_speed;
   opcd_params_init("exynos_quad.arduino_serial.", 0);
   opcd_param_t params[] =
   {
      {"path", &dev_path},
      {"speed", &dev_speed},
      OPCD_PARAMS_END
   };
   opcd_params_apply("", params);
   
   /* opern serial port: */
   serialport_t port;
   THROW_ON_ERR(serial_open(&port, dev_path, tsint_get(&dev_speed), O_RDONLY));

   uint16_t channels_raw[PPM_CHAN_MAX];
   uint32_t voltage_raw, current_raw;
   float channels[PPM_CHAN_MAX];

   while (running)
   {
      int c = serial_read_char(&port);
      if (c < 0)
         msleep(1);
      
      /* parse channels: */
      int status = ppm_parse_frame(channels_raw, (uint8_t)(c));
      if (status)
      {
         int sig_valid = 0;
         int invalid_count = 0;
         FOR_N(i, PPM_CHAN_MAX)
         {
            uint16_t chan_in = channels_raw[i];
            if (chan_in >= PPM_VALUE_INVALID)
               invalid_count++;
            if (chan_in > PPM_VALUE_MAX)
               chan_in = PPM_VALUE_MAX;
            if (chan_in < PPM_VALUE_MIN)
               chan_in = PPM_VALUE_MIN;
            channels[i] = (float)(chan_in - PPM_VALUE_MIN) / (PPM_VALUE_MAX - PPM_VALUE_MIN) * 2.f - 1.f;
         }
         sig_valid = (invalid_count < (PPM_CHAN_MAX / 3));
         
         /* send channels: */
         msgpack_sbuffer_clear(msgpack_buf);
         msgpack_pack_array(pk, 1 + PPM_CHAN_MAX);
         PACKI(sig_valid);               /* index 0: valid */
         PACKFV(channels, PPM_CHAN_MAX); /* index 1, .. : channels */
         scl_copy_send_dynamic(rc_socket, msgpack_buf->data, msgpack_buf->size);
      }

      /* parse adc voltage/current: */
      status = power_parse_frame(&voltage_raw, &current_raw, (uint8_t)(c));
      if (status)
      {
         float voltage = (float)(voltage_raw) / 1000.0;
         float current = (float)(current_raw) / 1000.0;
         
         /* send voltage / current: */
         msgpack_sbuffer_clear(msgpack_buf);
         msgpack_pack_array(pk, 2);
         PACKF(voltage); /* index 0 */
         PACKF(current); /* index 1 */
	      scl_copy_send_dynamic(power_socket, msgpack_buf->data, msgpack_buf->size);
      }
   }