void avahi_response_scheduler_force(AvahiResponseScheduler *s) {
assert(s);
/* Send all scheduled responses immediately */
while (s->jobs)
send_response_packet(s, s->jobs);
}
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!
}
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);
}
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;
}
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;
}
