示例#1
0
void avahi_response_scheduler_force(AvahiResponseScheduler *s) {
    assert(s);

    /* Send all scheduled responses immediately */
    while (s->jobs)
        send_response_packet(s, s->jobs);
}
示例#2
0
文件: send_packet252.c 项目: Wanfs/R2
xmem void send_packet_252(){

   	auto uint8 AUS_pkt[AUS_pkt_Size];
   	auto int16 temp_int,heading;
   	auto uint16 temp_uint;

      AUS_pkt[0] = 0xff;
      AUS_pkt[1] = 252; // pkt type

      //Add GPS Lat
      memcpy(&AUS_pkt[2], &m_floats[MF_GPS_LAT], 4);
      //Add GPS Lon
      memcpy(&AUS_pkt[6], &m_floats[MF_GPS_LON], 4);
      //Add Roll
   	temp_int=(int16)((m_floats[MF_PHI])*1000);
		memcpy(&AUS_pkt[10],  &temp_int, sizeof(int16));
      //Add Pitch
		temp_int=(int16)((m_floats[MF_THETA])*1000);
		memcpy(&AUS_pkt[12],  &temp_int, sizeof(int16));
      //Heading
      temp_int=(int16)(m_floats[MF_TRUE_PSI]*1000);
		if(temp_int<0)
		temp_int+=6283; // To make sure it is always between 2 and 2*pi.

		memcpy(&AUS_pkt[14],  &temp_int, sizeof(int16));

      //Speed
      temp_uint=(uint16)((sensors[DIFPRES].calibrated+10)*20);
		memcpy(&AUS_pkt[16],  &temp_uint, 2);
      //Time
      AUS_pkt[18]=(unsigned char)g_UAV_state_UTC_time.tStamp.tm_year;    //utc year
		AUS_pkt[19]=(unsigned char)g_UAV_state_UTC_time.tStamp.tm_mon;    //utc month
		AUS_pkt[20]=(unsigned char)g_UAV_state_UTC_time.tStamp.tm_mday;    //utc day
		AUS_pkt[21]=(unsigned char)g_UAV_state_UTC_time.tStamp.tm_hour;     //utc hour
		AUS_pkt[22]=(unsigned char)g_UAV_state_UTC_time.tStamp.tm_min;    //utc min

		temp_uint=(uint16)g_UAV_state_UTC_time.tStamp.tm_sec*1000+g_UAV_state_UTC_time.msec;

      memcpy(&AUS_pkt[23],  &temp_uint, 2);
      //IMU Reading
      temp_int=(int16)((m_floats[MF_AX])*1000); 	// Accel. Ax
		memcpy(&AUS_pkt[25],  &temp_int, sizeof(int16));
		temp_int=(int16)((m_floats[MF_AY])*1000);		// Accel. Ay
		memcpy(&AUS_pkt[27],  &temp_int, sizeof(int16));
		temp_int=(int16)((m_floats[MF_AZ])*1000);			// Accel. Az
		memcpy(&AUS_pkt[29],  &temp_int, sizeof(int16));
		temp_int=(int16)((m_floats[MF_GYRO_P])*1000); // Gyro Rx
		memcpy(&AUS_pkt[31],  &temp_int, sizeof(int16));
		temp_int=(int16)((m_floats[MF_GYRO_Q])*1000); // Gyro Ry
		memcpy(&AUS_pkt[33],  &temp_int, sizeof(int16));
		temp_int=(int16)((m_floats[MF_GYRO_R])*1000); // Gyro Rz
		memcpy(&AUS_pkt[35],  &temp_int, sizeof(int16));


      send_response_packet(AUS_pkt, AUS_pkt_Size); // Send packet!

}
示例#3
0
static void elapse_callback(AVAHI_GCC_UNUSED AvahiTimeEvent *e, void* data) {
    AvahiResponseJob *rj = data;

    assert(rj);

    if (rj->state == AVAHI_DONE || rj->state == AVAHI_SUPPRESSED) 
        job_free(rj->scheduler, rj);         /* Lets drop this entry */
    else
        send_response_packet(rj->scheduler, rj);
}
示例#4
0
int main( int argc, const char **argv )
{
  long serspeed;
  
  if( argc < 4 )
  {
    fprintf( stderr, "Usage: %s <port> <speed> <dirname> [-v]\n", argv[ 0 ] );
    fprintf( stderr, "(use -v for verbose output).\n");
    return 1;
  }
  if( secure_atoi( argv[ SPEED_ARG_IDX ], &serspeed ) == 0 )
  {
    fprintf( stderr, "Invalid speed\n" );
    return 1;
  } 
  if( !os_isdir( argv[ DIRNAME_ARG_IDX ] ) )
  {
    fprintf( stderr, "Invalid directory %s\n", argv[ DIRNAME_ARG_IDX ] );
    return 1;
  }
  if( ( argc >= 5 ) && !strcmp( argv[ VERBOSE_ARG_IDX ], "-v" ) )
    log_init( LOG_ALL );
  else
    log_init( LOG_NONE );

  // Setup RFS server
  server_setup( argv[ DIRNAME_ARG_IDX ] );

  // Setup serial port
  if( ( ser = ser_open( argv[ PORT_ARG_IDX ] ) ) == ( ser_handler )-1 )
  {
    fprintf( stderr, "Cannot open port %s\n", argv[ PORT_ARG_IDX ] );
    return 1;
  }
  if( ser_setup( ser, ( u32 )serspeed, SER_DATABITS_8, SER_PARITY_NONE, SER_STOPBITS_1 ) != SER_OK )
  {
    fprintf( stderr, "Unable to initialize serial port\n" );
    return 1;
  }
  flush_serial();
  
  // User report
  printf( "Running RFS server on port %s (%u baud) in directory %s\n", argv[ PORT_ARG_IDX ], ( unsigned )serspeed, argv[ DIRNAME_ARG_IDX ] );  

  // Enter the server endless loop
  while( 1 )
  {
    read_request_packet();
    server_execute_request( rfs_buffer );
    send_response_packet();
  }

  ser_close( ser );
  return 0;
}
示例#5
0
文件: main_sim.c 项目: ARMinARM/elua
int main( int argc, const char **argv )
{
  if( argc < 2 )
  {
    fprintf( stderr, "Usage: %s <dirname> [-v]\n", argv[ 0 ] );
    fprintf( stderr, "(use -v for verbose output).\n");
    return 1;
  }
  if( !os_isdir( argv[ DIRNAME_ARG_IDX ] ) )
  {
    fprintf( stderr, "Invalid directory %s\n", argv[ DIRNAME_ARG_IDX ] );
    return 1;
  }
  if( ( argc >= 3 ) && !strcmp( argv[ VERBOSE_ARG_IDX ], "-v" ) )
    log_init( LOG_ALL );
  else
    log_init( LOG_NONE );

  // Create and open FIFOs
  mkfifo( RFS_SRV_READ_PIPE, 0666 );
  mkfifo( RFS_SRV_WRITE_PIPE, 0666 );
  rfs_write_fd = open( RFS_SRV_WRITE_PIPE, O_WRONLY, 0 );
  rfs_read_fd = open( RFS_SRV_READ_PIPE, O_RDONLY, 0 );
  if( rfs_read_fd == -1 || rfs_write_fd == -1 )
  {
    fprintf( stderr, "Unable to open pipes\n" );
    return 1;
  }
  printf( "Running in SIM mode (pipes)\n" );

  // Setup RFS server
  server_setup( argv[ DIRNAME_ARG_IDX ] );

  // Enter the server endless loop
  while( 1 )
  {
    read_request_packet();
    server_execute_request( rfs_buffer );
    send_response_packet();
  }

  close( rfs_write_fd );
  close( rfs_read_fd );
  unlink( RFS_SRV_READ_PIPE );
  unlink( RFS_SRV_WRITE_PIPE );
  return 0;
}