Пример #1
0
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);
      }
   }
Пример #2
0
int main_i2c(void)
{
   THROW_BEGIN();

   void *gps_socket = scl_get_socket("gps");
   THROW_IF(gps_socket == NULL, -EIO);
   int64_t hwm = 1;
   zmq_setsockopt(gps_socket, ZMQ_SNDHWM, &hwm, sizeof(hwm));

   void *sats_socket = scl_get_socket("sats");
   THROW_IF(sats_socket == NULL, -EIO);
 
   i2c_bus_t bus;
   i2c_dev_t device;

   uint8_t data_w[128];
   uint8_t data_r[128];   

   if (i2c_bus_open(&bus, "/dev/i2c-1"))
   {
      syslog(LOG_CRIT, "could not open i2c device");   
      exit(EXIT_FAILURE);
   }
   i2c_dev_init(&device, &bus, I2C_GPS_ADDRESS);
   
   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);

   while (1)
   {
      msleep(200);

      data_w[0] = I2C_GPS_STATUS;
      i2c_xfer(&device, 1, data_w, 1, data_r);
      
      msgpack_sbuffer_clear(msgpack_buf);
      int status = data_r[0];
      int fix = 0;
      if (status & I2C_GPS_STATUS_2DFIX)
         fix = 2;
      if (status & I2C_GPS_STATUS_3DFIX)
         fix = 3;

      if (fix == 2)
         msgpack_pack_array(pk, 7); /* 2d fix */
      else if (fix == 3)
         msgpack_pack_array(pk, 9); /* 3d fix */
      else
         msgpack_pack_array(pk, 1); /* no fix */
 
      data_w[0] = I2C_GPS_TIME;
      i2c_xfer(&device, 1, data_w, 4, data_r);
      long time = (((long) data_r[3]) << 24)
                | (((long) data_r[2]) << 16) 
                | (((long) data_r[1]) << 8) 
                | (data_r[0]);
      time /= 1000;
      char *time_str = ctime(&time);
      size_t len = strlen(time_str);
      msgpack_pack_raw(pk, len);
      msgpack_pack_raw_body(pk, time_str, len); /* gps array index 0 */

      if (fix == 2 || fix == 3)
      {
         data_w[0] = I2C_GPS_LOCATION;
         i2c_xfer(&device, 1, data_w, 8, data_r);
         PACKD(( (((long) data_r[3]) << 24) 
               | (((long) data_r[2]) << 16) 
               | (((long) data_r[1]) << 8) 
               | (data_r[0])) / 10000000.0); /* latitude, gps array index 1 */

         PACKD(( (((long) data_r[7]) << 24) 
               | (((long) data_r[6]) << 16) 
               | (((long) data_r[5]) << 8) 
               | (data_r[4])) / 10000000.0); /* logitude, gps array index 2 */

         PACKI((status & I2C_GPS_STATUS_NUMSATS) >> 4); /* gps array index 3 */
         
         data_w[0] = I2C_GPS_GROUND_SPEED;
         i2c_xfer(&device, 1, data_w, 2, data_r);
         PACKF(((float)((data_r[1] << 8) | data_r[0])) / 100.0 * 1.94384); /* gps array index 4 */
         
         data_w[0] = I2C_GPS_GROUND_SPEED;
         i2c_xfer(&device, 1, data_w, 2, data_r);
         PACKF((data_r[1] << 8) | data_r[0]); /* gps array index 5 */
         
         PACKF(0 /* HDOP */); /* gps array index 6 */
      }

      if (fix == 3)
      {
         data_w[0] = I2C_GPS_ALTITUDE;
         i2c_xfer(&device, 1, data_w, 2, data_r);
         PACKF(((data_r[1]) << 8) | data_r[0]);  /* gps array index 7 */
         PACKF(0 /* VDOP */); /* gps array index 8 */
      }
            
      scl_copy_send_dynamic(gps_socket, msgpack_buf->data, msgpack_buf->size);
   }