static void
do_pull_range_post (GstTracer * self, guint64 ts, GstPad * pad)
{
GstElement *parent = get_real_pad_parent (pad);
calculate_latency (parent, pad, ts);
}
static void
do_push_buffer_post (GstTracer * self, guint64 ts, GstPad * pad)
{
GstPad *peer_pad = GST_PAD_PEER (pad);
GstElement *parent = get_real_pad_parent (peer_pad);
calculate_latency (parent, peer_pad, ts);
}
static void
handoff_cb (GstElement * fakesink, GstBuffer * buff, GstPad * pad,
gpointer user_data)
{
calculate_latency (pad, buff);
g_idle_add ((GSourceFunc) quit_main_loop, user_data);
g_signal_handlers_disconnect_by_data (fakesink, user_data);
}
/*
* main()
*/
int main(int argc, char *argv[])
{
enum { NSAMPLES = 160, COUNT=100 };
pj_caching_pool cp;
pj_pool_t *pool;
pjmedia_port *wav;
pj_status_t status;
/* Verify cmd line arguments. */
if (argc != 2) {
puts("Error: missing argument(s)");
puts("Usage: latency REV.WAV");
return 1;
}
pj_log_set_level(0);
status = pj_init();
PJ_ASSERT_RETURN(status == PJ_SUCCESS, 1);
pj_caching_pool_init(&cp, &pj_pool_factory_default_policy, 0);
pool = pj_pool_create( &cp.factory, /* pool factory */
"wav", /* pool name. */
4000, /* init size */
4000, /* increment size */
NULL /* callback on error */
);
status = pj_register_strerror(PJMEDIA_ERRNO_START, PJ_ERRNO_SPACE_SIZE,
&pjmedia_strerror);
pj_assert(status == PJ_SUCCESS);
/* Wav */
status = pjmedia_wav_player_port_create( pool, /* memory pool */
argv[1], /* file to play */
0, /* use default ptime*/
0, /* flags */
0, /* default buffer */
&wav /* returned port */
);
if (status != PJ_SUCCESS) {
app_perror(THIS_FILE, argv[1], status);
return 1;
}
status = calculate_latency(pool, wav);
if (status != PJ_SUCCESS)
return 1;
status = pjmedia_port_destroy( wav );
PJ_ASSERT_RETURN(status == PJ_SUCCESS, 1);
pj_pool_release( pool );
pj_caching_pool_destroy( &cp );
pj_shutdown();
/* Done. */
return 0;
}
static void systest_latency_test(void)
{
const char *ref_wav_paths[] = { add_path(res_path, WAV_TOCK8_PATH), ALT_PATH1 WAV_TOCK8_PATH };
pj_str_t rec_wav_file;
pjsua_player_id play_id = PJSUA_INVALID_ID;
pjsua_conf_port_id play_slot = PJSUA_INVALID_ID;
pjsua_recorder_id rec_id = PJSUA_INVALID_ID;
pjsua_conf_port_id rec_slot = PJSUA_INVALID_ID;
pj_pool_t *pool = NULL;
pjmedia_port *wav_port = NULL;
unsigned lat_sum=0, lat_cnt=0, lat_min=0, lat_max=0;
enum gui_key key;
test_item_t *ti;
const char *title = "Audio Latency Test";
pj_status_t status;
ti = systest_alloc_test_item(title);
if (!ti)
return;
key = gui_msgbox(title,
"This test will try to find the audio device's "
"latency. We will play a special WAV file to the "
"speaker for ten seconds, then at the end "
"calculate the latency. Please don't do anything "
"until the test is done.", WITH_OKCANCEL);
if (key != KEY_OK) {
ti->skipped = PJ_TRUE;
return;
}
key = gui_msgbox(title,
"For this test to work, we must be able to capture "
"the audio played in the speaker (the echo), and only"
" that audio (i.e. you must be in relatively quiet "
"place to run this test). "
"Press OK to start, or CANCEL to skip.",
WITH_OKCANCEL);
if (key != KEY_OK) {
ti->skipped = PJ_TRUE;
return;
}
PJ_LOG(3,(THIS_FILE, "Running %s", title));
status = create_player(PJ_ARRAY_SIZE(ref_wav_paths), ref_wav_paths,
&play_id);
if (status != PJ_SUCCESS)
goto on_return;
play_slot = pjsua_player_get_conf_port(play_id);
rec_wav_file = pj_str(add_path(doc_path, WAV_LATENCY_OUT_PATH));
status = pjsua_recorder_create(&rec_wav_file, 0, NULL, -1, 0, &rec_id);
if (status != PJ_SUCCESS)
goto on_return;
rec_slot = pjsua_recorder_get_conf_port(rec_id);
/* Setup the test */
//status = pjsua_conf_connect(0, 0);
status = pjsua_conf_connect(play_slot, 0);
status = pjsua_conf_connect(0, rec_slot);
status = pjsua_conf_connect(play_slot, rec_slot);
/* We're running */
PJ_LOG(3,(THIS_FILE, "Please wait while test is running (~10 sec)"));
gui_sleep(10);
/* Done with the test */
//status = pjsua_conf_disconnect(0, 0);
status = pjsua_conf_disconnect(play_slot, rec_slot);
status = pjsua_conf_disconnect(0, rec_slot);
status = pjsua_conf_disconnect(play_slot, 0);
pjsua_recorder_destroy(rec_id);
rec_id = PJSUA_INVALID_ID;
pjsua_player_destroy(play_id);
play_id = PJSUA_INVALID_ID;
/* Confirm that echo is heard */
gui_msgbox(title,
"Test is done. Now we need to confirm that we indeed "
"captured the echo. We will play the captured audio "
"and please confirm that you can hear the 'tock' echo.",
WITH_OK);
status = pjsua_player_create(&rec_wav_file, 0, &play_id);
if (status != PJ_SUCCESS)
goto on_return;
play_slot = pjsua_player_get_conf_port(play_id);
status = pjsua_conf_connect(play_slot, 0);
if (status != PJ_SUCCESS)
goto on_return;
key = gui_msgbox(title,
"The captured audio is being played back now. "
"Can you hear the 'tock' echo?",
WITH_YESNO);
pjsua_player_destroy(play_id);
play_id = PJSUA_INVALID_ID;
if (key != KEY_YES)
goto on_return;
/* Now analyze the latency */
pool = pjsua_pool_create("latency", 512, 512);
status = pjmedia_wav_player_port_create(pool, rec_wav_file.ptr, 0, 0, 0, &wav_port);
if (status != PJ_SUCCESS)
goto on_return;
status = calculate_latency(pool, wav_port, &lat_sum, &lat_cnt,
&lat_min, &lat_max);
if (status != PJ_SUCCESS)
goto on_return;
on_return:
if (wav_port)
pjmedia_port_destroy(wav_port);
if (pool)
pj_pool_release(pool);
if (play_id != PJSUA_INVALID_ID)
pjsua_player_destroy(play_id);
if (rec_id != PJSUA_INVALID_ID)
pjsua_recorder_destroy(rec_id);
if (status != PJ_SUCCESS) {
systest_perror("Sorry we encountered an error: ", status);
ti->success = PJ_FALSE;
pj_strerror(status, ti->reason, sizeof(ti->reason));
} else if (key != KEY_YES) {
ti->success = PJ_FALSE;
if (!ti->success) {
pj_ansi_strcpy(ti->reason, USER_ERROR);
}
} else {
char msg[200];
pj_size_t msglen;
pj_ansi_snprintf(msg, sizeof(msg),
"The sound device latency:\r\n"
" Min=%u, Max=%u, Avg=%u\r\n",
lat_min, lat_max, lat_sum/lat_cnt);
msglen = strlen(msg);
if (lat_sum/lat_cnt > 500) {
pj_ansi_snprintf(msg+msglen, sizeof(msg)-msglen,
"The latency is huge!\r\n");
msglen = strlen(msg);
} else if (lat_sum/lat_cnt > 200) {
pj_ansi_snprintf(msg+msglen, sizeof(msg)-msglen,
"The latency is quite high\r\n");
msglen = strlen(msg);
}
key = gui_msgbox(title, msg, WITH_OK);
ti->success = PJ_TRUE;
pj_ansi_strncpy(ti->reason, msg, sizeof(ti->reason));
ti->reason[sizeof(ti->reason)-1] = '\0';
}
}
void
madara::cid::ga_naive (Settings & settings, double duration)
{
ACE_High_Res_Timer timer;
ACE_Time_Value max_tv (0, 0);
ACE_hrtime_t elapsed (0);
ACE_hrtime_t maximum (0);
timer.reset ();
timer.start ();
max_tv.set (duration);
maximum = max_tv.sec () * 1000000000;
maximum += max_tv.usec () * 1000;
#ifdef ENABLE_CID_LOGGING
MADARA_DEBUG (MADARA_LOG_EVENT_TRACE, (LM_DEBUG,
DLINFO "madara::cid::ga_naive:" \
" creating an exact copy of the solution vector\n"));
#endif
// create an exact copy of the solution vector
Deployment current;
current.resize (settings.solution.size ());
std::copy (settings.solution.begin (), settings.solution.end (),
current.begin ());
#ifdef ENABLE_CID_LOGGING
MADARA_DEBUG (MADARA_LOG_EVENT_TRACE, (LM_DEBUG,
DLINFO "madara::cid::ga_naive:" \
" calculating latency for the solution so far\n"));
#endif
uint64_t orig_latency = calculate_latency (settings);
uint64_t new_latency;
while (maximum > elapsed)
{
// generate some candidates for mutating
unsigned int candidate1 = rand () % settings.solution.size ();
unsigned int candidate2 = rand () % settings.solution.size ();
#ifdef ENABLE_CID_LOGGING
MADARA_DEBUG (MADARA_LOG_DETAILED_TRACE, (LM_DEBUG,
DLINFO "madara::cid::ga_naive:" \
" initial candidates: %u, %u\n",
candidate1, candidate2));
#endif
// loop until we have two different candidates
while (candidate1 == candidate2)
candidate1 = rand () % settings.solution.size ();
#ifdef ENABLE_CID_LOGGING
MADARA_DEBUG (MADARA_LOG_DETAILED_TRACE, (LM_DEBUG,
DLINFO "madara::cid::ga_naive:" \
" final candidates: %u, %u\n",
candidate1, candidate2));
#endif
// attempt the swap
std::swap (current[candidate1], current[candidate2]);
new_latency = calculate_latency (
settings.network_latencies, settings.target_deployment, current);
if (new_latency < orig_latency)
{
#ifdef ENABLE_CID_LOGGING
MADARA_DEBUG (MADARA_LOG_EVENT_TRACE, (LM_DEBUG,
DLINFO "madara::cid::ga_naive:" \
" latency improvement: %Q->%Q. Copying solution.\n",
orig_latency, new_latency));
#endif
std::copy (current.begin (), current.end (), settings.solution.begin ());
orig_latency = new_latency;
}
timer.stop ();
timer.elapsed_time (elapsed);
}
for (unsigned int i = 0; i < settings.solution.size (); ++i)
{
if (settings.solution_lookup[settings.solution[i]] != i)
settings.solution_lookup[settings.solution[i]] = i;
}
}
void madara::cid::ga_degree (Settings & settings, double duration)
{
if (settings.solution.size () < 2)
return;
ACE_High_Res_Timer timer;
ACE_Time_Value max_tv (0, 0);
ACE_hrtime_t elapsed (0);
ACE_hrtime_t maximum (0);
timer.reset ();
timer.start ();
max_tv.set (duration);
maximum = max_tv.sec () * 1000000000;
maximum += max_tv.usec () * 1000;
#ifdef ENABLE_CID_LOGGING
MADARA_DEBUG (MADARA_LOG_EVENT_TRACE, (LM_DEBUG,
DLINFO "madara::cid::ga_degree:" \
" entering main function\n"));
#endif
// create an exact copy of the solution vector
Deployment current;
current.resize (settings.solution.size ());
std::copy (settings.solution.begin (), settings.solution.end (),
current.begin ());
uint64_t orig_latency = calculate_latency (settings);
unsigned int num_degreed_nodes = 0;
Workflow & deployment = settings.target_deployment;
SummationsMap & averages = settings.network_summations;
SolutionMap solution_lookup;
#ifdef ENABLE_CID_LOGGING
MADARA_DEBUG (MADARA_LOG_EVENT_TRACE, (LM_DEBUG,
DLINFO "madara::cid::ga_degree:" \
" copying initial solution\n"));
#endif
for (unsigned int i = 0; i < current.size (); ++i)
{
solution_lookup[current[i]] = i;
}
#ifdef ENABLE_CID_LOGGING
MADARA_DEBUG (MADARA_LOG_EVENT_TRACE, (LM_DEBUG,
DLINFO "madara::cid::ga_degree:" \
" counting the degreed nodes in the deployment\n"));
#endif
// count the number of degreed nodes in the deployment.
for (unsigned int i = 0; i < deployment.size (); ++i)
{
if (deployment[i].size () == 0)
break;
++num_degreed_nodes;
}
#ifdef ENABLE_CID_LOGGING
MADARA_DEBUG (MADARA_LOG_EVENT_TRACE, (LM_DEBUG,
DLINFO "madara::cid::ga_degree:" \
" Attempting up to %f seconds worth of mutations to find better solution\n",
duration));
#endif
while (maximum > elapsed)
{
/**
* 1/5 times, choose a naive solution.
* 4/5 times, choose to swap someone from the high degreed candidates
**/
int tactic = rand () % 5;
unsigned int candidate1;
unsigned int candidate2;
if (tactic == 4)
{
#ifdef ENABLE_CID_LOGGING
MADARA_DEBUG (MADARA_LOG_DETAILED_TRACE, (LM_DEBUG,
DLINFO "madara::cid::ga_degree:" \
" naively choosing candidates\n"));
#endif
// go with a naive solution
candidate1 = rand () % settings.solution.size ();
candidate2 = rand () % settings.solution.size ();
if (settings.solution.size () > 0)
{
// loop until we have two different candidates
while (candidate1 == candidate2)
candidate1 = rand () % settings.solution.size ();
}
}
else
{
#ifdef ENABLE_CID_LOGGING
MADARA_DEBUG (MADARA_LOG_DETAILED_TRACE, (LM_DEBUG,
DLINFO "madara::cid::ga_degree:" \
" choosing degree-informed candidates\n"));
#endif
unsigned int choice = rand () % num_degreed_nodes;
// use degree and latency information to form ideal candidates
candidate1 = deployment[choice][0].first;
LatencyVector & cur_summations =
settings.network_summations[deployment[choice].size ()];
/**
* the other candidate can be from a range:
* 0-2 : top num_degreed_nodes
* 3 : top 10%
* 4 : top 25%
**/
int candidate_type = rand () % 5;
unsigned int range;
if (cur_summations.size () < 50)
{
range = cur_summations.size () / 10 + 1;
candidate2 = solution_lookup[cur_summations[rand () % range].first];
}
else if (candidate_type <= 2)
{
if (num_degreed_nodes < 5)
{
range = cur_summations.size () / 20;
}
else
{
range = num_degreed_nodes;
}
#ifdef ENABLE_CID_LOGGING
MADARA_DEBUG (MADARA_LOG_DETAILED_TRACE, (LM_DEBUG,
DLINFO "madara::cid::ga_degree:" \
" choosing from top %u candidates\n",
range));
#endif
candidate2 = solution_lookup[cur_summations[rand () % range].first];
}
else if (candidate_type == 3)
{
// choose candidate2 from the top 10%
range = cur_summations.size () / 10;
#ifdef ENABLE_CID_LOGGING
MADARA_DEBUG (MADARA_LOG_DETAILED_TRACE, (LM_DEBUG,
DLINFO "madara::cid::ga_degree:" \
" choosing from top %u candidates\n",
range));
#endif
candidate2 = solution_lookup[cur_summations[rand () % range].first];
}
else
{
// choose candidate2 from the top 25%
range = cur_summations.size () / 4;
#ifdef ENABLE_CID_LOGGING
MADARA_DEBUG (MADARA_LOG_DETAILED_TRACE, (LM_DEBUG,
DLINFO "madara::cid::ga_degree:" \
" choosing from top %u candidates\n",
range));
#endif
candidate2 = solution_lookup[cur_summations[rand () % range].first];
}
#ifdef ENABLE_CID_LOGGING
MADARA_DEBUG (MADARA_LOG_DETAILED_TRACE, (LM_DEBUG,
DLINFO "madara::cid::ga_degree:" \
" initial candidates: %u, %u\n",
candidate1, candidate2));
#endif
// loop until we have two different candidates
while (range >= 2 && candidate1 == candidate2)
candidate2 = solution_lookup[cur_summations[rand () % range].first];
}
#ifdef ENABLE_CID_LOGGING
MADARA_DEBUG (MADARA_LOG_DETAILED_TRACE, (LM_DEBUG,
DLINFO "madara::cid::ga_degree:" \
" final candidates: %u, %u\n",
candidate1, candidate2));
#endif
// attempt the swap
if (candidate1 != candidate2)
{
std::swap (current[candidate1], current[candidate2]);
solution_lookup[current[candidate1]] = candidate1;
solution_lookup[current[candidate2]] = candidate2;
uint64_t new_latency = calculate_latency (
settings.network_latencies, deployment, current);
#ifdef ENABLE_CID_LOGGING
MADARA_DEBUG (MADARA_LOG_DETAILED_TRACE, (LM_DEBUG,
DLINFO "madara::cid::ga_degree:" \
" latency: %Q->%Q\n",
orig_latency, new_latency));
#endif
if (new_latency < orig_latency)
{
#ifdef ENABLE_CID_LOGGING
MADARA_DEBUG (MADARA_LOG_DETAILED_TRACE, (LM_DEBUG,
DLINFO "madara::cid::ga_degree:" \
" updating solution to current\n",
orig_latency, new_latency));
#endif
std::copy (current.begin (), current.end (), settings.solution.begin ());
for (unsigned int j = 0; j < settings.solution.size (); ++j)
{
if (settings.solution_lookup[settings.solution[j]] != j)
settings.solution_lookup[settings.solution[j]] = j;
}
orig_latency = new_latency;
}
}
timer.stop ();
timer.elapsed_time (elapsed);
}
}
/***********************************
* MAIN
* *********************************/
int main(int argc, char *argv[]){
Connection connection;
write_error_ptr = &write_error; //initialize the function pointer to write error
//parse arguments
if(argc == 4){
//first argument is always name of program or empty string
connection.planebone_ip=argv[1];
connection.port_number_server_to_planebone=atoi(argv[2]);
connection.port_number_planebone_to_server=atoi(argv[3]);
}else{
printf("wrong parameters: planebone ip - send port number - receive port number\n");
exit(EXIT_FAILURE);
}
pthread_t thread_server_to_planebone;
//create a second thread which executes server_to_planebone
if(pthread_create(&thread_server_to_planebone, NULL, server_to_planebone,&connection)) {
error_write(FILENAME,"error creating lisa thread");
exit(EXIT_FAILURE);
}
/*-------------------------START OF FIRST THREAD: PLANEBONE TO SERVER------------------------*/
static UDP udp_server;
uint8_t input_stream[MAX_INPUT_STREAM_SIZE];
timeval tv_now;
UDP_err_handler(openUDPServerSocket(&udp_server,connection.port_number_planebone_to_server,UDP_SOCKET_TIMEOUT),write_error_ptr);
//init the data decode pointers
init_decoding();
/*
* WHAT WE EXPECT:
* IMU_ACCEL_RAW 204
* IMU_GYRO_RAW 203
* IMU_MAG_RAW 205
* BARO_RAW 221
* GPS_INT 155
* AIRSPEED_ETS 57
* SYSMON 33
* UART_ERROR 208
* ACTUATORS_received 105
* */
int IMU_ACCEL_RAW_received=0;
int IMU_GYRO_RAW_received=0;
int IMU_MAG_RAW_received=0;
int BARO_RAW_received=0;
int GPS_INT_received=0;
int AIRSPEED_received=0;
int SVINFO_received=0;
int SYSMON_received=0;
int UART_ERROR_received=0;
int ACTUATORS_received=0;
int NMEA_IIMWV_received = 0;
int NMEA_WIXDR_received = 0;
int err;
#if ANALYZE
Analyze an_imu_accel_raw_freq,an_imu_accel_raw_lat;
int an_imu_accel_freq_done=0,an_imu_accel_lat_done=0;
init_analyze(&an_imu_accel_raw_freq,2000);
init_analyze(&an_imu_accel_raw_lat,2000);
Analyze an_imu_gyro_raw_freq,an_imu_gyro_raw_lat;
int an_imu_gyro_freq_done=0,an_imu_gyro_lat_done=0;
init_analyze(&an_imu_gyro_raw_freq,2000);
init_analyze(&an_imu_gyro_raw_lat,2000);
Analyze an_imu_mag_raw_freq,an_imu_mag_raw_lat;
int an_imu_mag_freq_done=0,an_imu_mag_lat_done=0;
init_analyze(&an_imu_mag_raw_freq,2000);
init_analyze(&an_imu_mag_raw_lat,2000);
Analyze an_baro_raw_freq,an_baro_raw_lat;
int an_baro_raw_freq_done=0,an_baro_raw_lat_done=0;
init_analyze(&an_baro_raw_freq,2000);
init_analyze(&an_baro_raw_lat,2000);
Analyze an_gps_int_freq,an_gps_int_lat;
int an_gps_int_freq_done=0,an_gps_int_lat_done=0;
init_analyze(&an_gps_int_freq,40);
init_analyze(&an_gps_int_lat,40);
Analyze an_airspeed_ets_freq,an_airspeed_ets_lat;
int an_airspeed_ets_freq_done=0,an_airspeed_ets_lat_done=0;
init_analyze(&an_airspeed_ets_freq,100);
init_analyze(&an_airspeed_ets_lat,100);
Analyze an_actuators_freq,an_actuators_lat;
int an_actuators_freq_done=0,an_actuators_lat_done=0;
init_analyze(&an_actuators_freq,500);
init_analyze(&an_actuators_lat,500);
Analyze an_UART_errors_freq,an_UART_errors_lat;
int an_UART_errors_freq_done=0,an_UART_errors_lat_done=0;
init_analyze(&an_UART_errors_freq,50);
init_analyze(&an_UART_errors_lat,50);
Analyze an_sys_mon_freq,an_sys_mon_lat;
int an_sys_mon_freq_done=0,an_sys_mon_lat_done=0;
init_analyze(&an_sys_mon_freq,50);
init_analyze(&an_sys_mon_lat,50);
#endif
int recv_len;
size_t data_len = sizeof(input_stream);
while (1){
// err = receiveUDPServerData(&udp_server,(void *)&input_stream, sizeof(input_stream)); //blocking !!!
//1. retreive UDP data form planebone from ethernet port.
err = receiveUDPServerData(&udp_server,(void *)&input_stream, data_len, &recv_len); //blocking !!!
if (recv_len != 30) {
printf("Wrong number of bytes in received UDP packet!\n");
printf("Expected 30 bytes, Received %d bytes!\n", recv_len);
err = UDP_ERR_RECV;
}
UDP_err_handler(err,write_error_ptr);
if(err == UDP_ERR_NONE){
gettimeofday(&tv_now,NULL); //timestamp from receiving to calculate latency
#if DEBUG > 0
printf("message raw: ");
int i;
for(i=0;i<input_stream[1];i++){
printf("%d ",input_stream[i]);
}
printf("\n");
printf("start hex: %x\n", input_stream[0]);
printf("length: %d\n", input_stream[1]);
printf("send id: %d\n", input_stream[2]);
printf("message id: %d\n", input_stream[3]);
printf("checksum1: %d\n", input_stream[input_stream[1]-2]);
printf("checksum2: %d\n", input_stream[input_stream[1]-1]);
// printf("%d", input_stream[3]);
printf("\n");
#endif
//2. decode data
int err = data_decode(input_stream);
DEC_err_handler(err,write_error_ptr);
if(err==DEC_ERR_NONE){
switch_read_write(); //only switch read write if data decoding was succesfull
Data* data = get_read_pointer();
if(input_stream[3]==IMU_GYRO_RAW){
IMU_GYRO_RAW_received=1;
}else if(input_stream[3]==IMU_ACCEL_RAW){
IMU_ACCEL_RAW_received=1;
}else if(input_stream[3]==IMU_MAG_RAW){
IMU_MAG_RAW_received=1;
}else if(input_stream[3]==BARO_RAW){
BARO_RAW_received=1;
}else if(input_stream[3]==GPS_INT){
GPS_INT_received=1;
}else if(input_stream[3]==AIRSPEED_ETS){
AIRSPEED_received=1;
}else if(input_stream[3]==SVINFO){
SVINFO_received=1;
}else if(input_stream[3]==SYSMON){
SYSMON_received=1;
}else if(input_stream[3]==UART_ERRORS){
UART_ERROR_received=1;
}else if(input_stream[3]==ACTUATORS){
ACTUATORS_received=1;
}else if(input_stream[3]==NMEA_IIMWV_ID){
NMEA_IIMWV_received=1;
}else if(input_stream[3]==NMEA_WIXDR_ID){
NMEA_WIXDR_received=1;
}
/*printf("IMU_GYRO_RAW_received %d\n",IMU_GYRO_RAW_received);
printf("IMU_ACCEL_RAW_received %d\n",IMU_ACCEL_RAW_received);
printf("IMU_MAG_RAW_received %d\n",IMU_MAG_RAW_received);
printf("BARO_RAW_received %d\n",BARO_RAW_received);
printf("GPS_INT_received %d\n",GPS_INT_received);
printf("AIRSPEED_received %d\n",AIRSPEED_received);
printf("SVINFO_received %d\n",SVINFO_received);
printf("SYSMON_received %d\n",SYSMON_received);
printf("UART_ERROR_received %d\n",UART_ERROR_received);
printf("ACTUATORS_received %d\n",ACTUATORS_received);
printf("NMEA_IIMWV_received %d\n",NMEA_IIMWV_received);
printf("NMEA_WIXDR_received %d\n",NMEA_WIXDR_received);
printf("\n");*/
if(input_stream[3]==BARO_RAW){
#if ANALYZE
if(calculate_frequency(&an_baro_raw_freq,data->lisa_plane.baro_raw.tv)==1){
an_baro_raw_freq_done=1;
}
if(calculate_latency(&an_baro_raw_lat,data->lisa_plane.baro_raw.tv,tv_now)==1){
an_baro_raw_lat_done=1;
}
#endif
/*int i;
printf("Baro_raw content:");
print_mem((void *)&data->lisa_plane.baro_raw,sizeof(Baro_raw));
printf("abs %d\n",data->lisa_plane.baro_raw.abs);
printf("diff %d\n",data->lisa_plane.baro_raw.diff);
printf("\n\n\n");*/
}
if(input_stream[3]==IMU_GYRO_RAW){
#if ANALYZE
if(calculate_frequency(&an_imu_gyro_raw_freq,data->lisa_plane.imu_gyro_raw.tv)==1){
an_imu_gyro_freq_done=1;
}
if(calculate_latency(&an_imu_gyro_raw_lat,data->lisa_plane.imu_gyro_raw.tv,tv_now)==1){
an_imu_gyro_lat_done=1;
}
#endif
/* int i;
printf("Imu_gyro_raw content:");
print_mem((void *)&data->lisa_plane.imu_gyro_raw,sizeof(Imu_gyro_raw));
printf("\n");
printf("gp %d\n",data->lisa_plane.imu_gyro_raw.gp);
printf("gq %d\n",data->lisa_plane.imu_gyro_raw.gq);
printf("gr %d\n",data->lisa_plane.imu_gyro_raw.gr);
print_latency(data->lisa_plane.imu_gyro_raw.tv);
printf("\n\n\n");*/
}
if(input_stream[3]==IMU_ACCEL_RAW){
#if ANALYZE
if(calculate_frequency(&an_imu_accel_raw_freq,data->lisa_plane.imu_accel_raw.tv)==1){
an_imu_accel_freq_done=1;
}
if(calculate_latency(&an_imu_accel_raw_lat,data->lisa_plane.imu_accel_raw.tv,tv_now)==1){
an_imu_accel_lat_done=1;
}
#endif
/*int i;
printf("Imu_accel_raw content:");
print_mem((void *)&data->lisa_plane.imu_accel_raw,sizeof(Imu_accel_raw));
printf("\n");
printf("ax %d\n",data->lisa_plane.imu_accel_raw.ax);
printf("ay %d\n",data->lisa_plane.imu_accel_raw.ay);
printf("az %d\n",data->lisa_plane.imu_accel_raw.az);
printf("\n");
printf("\n\n\n");*/
}
if(input_stream[3]==IMU_MAG_RAW){
#if ANALYZE
if(calculate_frequency(&an_imu_mag_raw_freq,data->lisa_plane.imu_mag_raw.tv)==1){
an_imu_mag_freq_done=1;
}
if(calculate_latency(&an_imu_mag_raw_lat,data->lisa_plane.imu_mag_raw.tv,tv_now)==1){
an_imu_mag_lat_done=1;
}
#endif
}
if(input_stream[3]==AIRSPEED_ETS){
#if ANALYZE
if(calculate_frequency(&an_airspeed_ets_freq,data->lisa_plane.airspeed_ets.tv)==1){
an_airspeed_ets_freq_done=1;
}
if(calculate_latency(&an_airspeed_ets_lat,data->lisa_plane.airspeed_ets.tv,tv_now)==1){
an_airspeed_ets_lat_done=1;
}
#endif
/*int i;
printf("airspeed content:");
print_mem((void *)&data->lisa_plane.airspeed_ets,sizeof(Airspeed_ets));
char temp[64];
timestamp_to_timeString(data->lisa_plane.airspeed_ets.tv,temp);
printf("send time %s\n",temp);
printf("adc %d\n",data->lisa_plane.airspeed_ets.adc);
printf("offset %d\n",data->lisa_plane.airspeed_ets.offset);
printf("scaled %f\n",data->lisa_plane.airspeed_ets.scaled);*/
}
if(input_stream[3]==GPS_INT){
#if ANALYZE
if(calculate_frequency(&an_gps_int_freq,data->lisa_plane.gps_int.tv)==1){
an_gps_int_freq_done=1;
}
if(calculate_latency(&an_gps_int_lat,data->lisa_plane.gps_int.tv,tv_now)==1){
an_gps_int_lat_done=1;
}
#endif
/*int i;
printf("Gps_int_message content:");
print_mem((void *)&data->lisa_plane.gps_int,sizeof(Gps_int));
printf("\n");
printf("ecef_x %d\n",data->lisa_plane.gps_int.ecef_x);
printf("ecef_y %d\n",data->lisa_plane.gps_int.ecef_y);
printf("ecef_z %d\n",data->lisa_plane.gps_int.ecef_z);
printf("lat %d\n",data->lisa_plane.gps_int.lat);
printf("lon %d\n",data->lisa_plane.gps_int.lon);
printf("alt %d\n",data->lisa_plane.gps_int.alt);
printf("hmsl %d\n",data->lisa_plane.gps_int.hmsl);
printf("ecef_xd %d\n",data->lisa_plane.gps_int.ecef_xd);
printf("ecef_yd %d\n",data->lisa_plane.gps_int.ecef_yd);
printf("ecef_zd %d\n",data->lisa_plane.gps_int.ecef_zd);
printf("pacc %d\n",data->lisa_plane.gps_int.pacc);
printf("sacc %d\n",data->lisa_plane.gps_int.sacc);
printf("tow %d\n",data->lisa_plane.gps_int.tow);
printf("pdop %d\n",data->lisa_plane.gps_int.pdop);
printf("numsv %d\n",data->lisa_plane.gps_int.numsv);
printf("fix %d\n",data->lisa_plane.gps_int.fix);
print_latency(data->lisa_plane.gps_int.tv);*/
}
if(input_stream[3]==SYSMON){
#if ANALYZE
if(calculate_frequency(&an_sys_mon_freq,data->lisa_plane.sys_mon.tv)==1){
an_sys_mon_freq_done=1;
}
if(calculate_latency(&an_sys_mon_lat,data->lisa_plane.sys_mon.tv,tv_now)==1){
an_sys_mon_lat_done=1;
}
#endif
/* int i;
printf("sysmon content:");
print_mem((void *)&data->lisa_plane.sys_mon,sizeof(Sys_mon));
printf("\n");
printf("periodic_time %d\n",data->lisa_plane.sys_mon.periodic_time);
printf("periodic_cycle %d\n",data->lisa_plane.sys_mon.periodic_cycle);
printf("periodic_cycle_min %d\n",data->lisa_plane.sys_mon.periodic_cycle_min);
printf("periodic_cycle_max %d\n",data->lisa_plane.sys_mon.periodic_cycle_max);
printf("event_number %d\n",data->lisa_plane.sys_mon.event_number);
printf("cpu_load %d\n",data->lisa_plane.sys_mon.cpu_load);
print_latency(data->lisa_plane.sys_mon.tv);*/
}
if(input_stream[3]==UART_ERRORS){
#if ANALYZE
if(calculate_frequency(&an_UART_errors_freq,data->lisa_plane.uart_errors.tv)==1){
an_UART_errors_freq_done=1;
}
if(calculate_latency(&an_UART_errors_lat,data->lisa_plane.uart_errors.tv,tv_now)==1){
an_UART_errors_lat_done=1;
}
#endif
/* int i;
printf("uart error content:");
print_mem((void *)&data->lisa_plane.uart_errors,sizeof(UART_errors));
printf("overrun_cnt %d\n",data->lisa_plane.uart_errors.overrun_cnt);
printf("noise_err_cnt %d\n",data->lisa_plane.uart_errors.noise_err_cnt);
printf("framing_err_cnt %d\n",data->lisa_plane.uart_errors.framing_err_cnt);
printf("bus_number %d\n",data->lisa_plane.uart_errors.bus_number);
print_latency(data->lisa_plane.uart_errors.tv);*/
}
if(input_stream[3]==ACTUATORS){
#if ANALYZE
if(calculate_frequency(&an_actuators_freq,data->lisa_plane.actuators.tv)==1){
an_actuators_freq_done=1;
}
if(calculate_latency(&an_actuators_lat,data->lisa_plane.actuators.tv,tv_now)==1){
an_actuators_lat_done=1;
}
#endif
/*int i;
printf("actuators content:");
print_mem((void *)&data->lisa_plane.actuators,sizeof(Actuators));
printf("arr_length %d\n",data->lisa_plane.actuators.arr_length);
for(i=0;i<data->lisa_plane.actuators.arr_length;i++){
printf("servo_%d %d\n",i,data->lisa_plane.actuators.values[i]);
}
print_latency(data->lisa_plane.actuators.tv);*/
}
if(input_stream[3]==BEAGLE_ERROR){
//printf("beagle bone error content:");
//print_mem((void *)&data->bone_plane.error,sizeof(Beagle_error));
switch(data->bone_plane.error.library){
case UDP_L:
UDP_err_handler(data->bone_plane.error.error_code,write_error_ptr);
break;
case UART_L:
UART_err_handler(data->bone_plane.error.error_code,write_error_ptr);
break;
case DECODE_L:
DEC_err_handler(data->bone_plane.error.error_code,write_error_ptr);
break;
case LOG_L:
LOG_err_handler(data->bone_plane.error.error_code,write_error_ptr);
break;
}
}
if(input_stream[3]==NMEA_IIMWV_ID){
printf("NMEA_IIMWV_ID content:");
print_mem((void *)&data->bone_wind.nmea_iimmwv,sizeof(NMEA_IIMWV));
printf("wind angle %lf\n",data->bone_wind.nmea_iimmwv.wind_angle);
printf("relative %c\n",data->bone_wind.nmea_iimmwv.relative);
printf("wind speed %lf\n",data->bone_wind.nmea_iimmwv.wind_speed);
printf("wind speed unit %c\n",data->bone_wind.nmea_iimmwv.wind_speed_unit);
printf("status %c\n",data->bone_wind.nmea_iimmwv.status);
char temp[64];
timestamp_to_timeString16(data->bone_wind.nmea_iimmwv.tv,temp);
printf("send time: %s\n",temp);
printf("\n");
}
if(input_stream[3]==NMEA_WIXDR_ID){
printf("NMEA_WIXDR_ID content:");
print_mem((void *)&data->bone_wind.nmea_wixdr,sizeof(NMEA_WIXDR));
printf("Temperature %lf\n",data->bone_wind.nmea_wixdr.temperature);
printf("unit %c\n",data->bone_wind.nmea_wixdr.unit);
char temp[64];
timestamp_to_timeString16(data->bone_wind.nmea_wixdr.tv,temp);
printf("send time: %s\n",temp);
printf("\n");
}
if(input_stream[3]==LINE_ANGLE_ID){
// Send a character (to gpio of arduino)
// to stop the arduino-timer
/*
FILE *myFile;
myFile = fopen("/dev/ttyUSB0", "w");
fputs ("a", myFile);
fclose (myFile);
*/
printf("LINE_ANGLE_ID content:");
print_mem((void *)&data->bone_arm.line_angle,sizeof(LINE_ANGLE));
printf("Azimuth %i\n",data->bone_arm.line_angle.azimuth_raw);
printf("Elevation %i\n",data->bone_arm.line_angle.elevation_raw);
// printf("unit %c\n",data->bone_wind.nmea_wixdr.unit);
//char temp[64];
//timestamp_to_timeString16(data->bone_arm.line_angle.tv,temp);
//printf("send time: %s\n",temp);
printf("\n");
}
}else{
printf("UNKNOW PACKAGE with id %d\n",input_stream[3]);
exit(1);
}
}
#if ANALYZE
if(an_imu_accel_freq_done==1 &&
an_imu_accel_lat_done==1 &&
an_imu_gyro_freq_done==1 &&
an_imu_gyro_lat_done==1 &&
an_imu_mag_freq_done==1 &&
an_imu_mag_lat_done==1 &&
an_baro_raw_lat_done==1 &&
an_baro_raw_freq_done==1 &&
an_airspeed_ets_lat_done==1 &&
an_airspeed_ets_freq_done==1 &&
an_actuators_lat_done==1 &&
an_actuators_freq_done==1 &&
an_UART_errors_lat_done==1 &&
an_UART_errors_freq_done==1 &&
an_sys_mon_lat_done==1 &&
an_sys_mon_freq_done==1)
{
printf("ANALYZE RESULTS - IMU_ACCEL_RAW:\n");
printf("avg period:\t %0.4f ms\n",(get_avg(&an_imu_accel_raw_freq)));
printf("avg freq:\t %0.4f hz\n",1/(get_avg(&an_imu_accel_raw_freq))*1e3);
printf("avg latency\t %0.4f ms\n",get_avg(&an_imu_accel_raw_lat));
printf("\n");
dump_buffer_to_file(&an_imu_accel_raw_freq,"analyze/imu_accel_raw_per.csv");
dump_buffer_to_file(&an_imu_accel_raw_lat,"analyze/imu_accel_raw_lat.csv");
destroy_analyze(&an_imu_accel_raw_freq);
destroy_analyze(&an_imu_accel_raw_lat);
printf("ANALYZE RESULTS - IMU_GYRO_RAW:\n");
printf("avg period:\t %0.4f ms\n",(get_avg(&an_imu_gyro_raw_freq)));
printf("avg freq:\t %0.4f hz\n",1/(get_avg(&an_imu_gyro_raw_freq))*1e3);
printf("avg latency\t %0.4f ms\n",get_avg(&an_imu_gyro_raw_lat));
printf("\n");
dump_buffer_to_file(&an_imu_gyro_raw_freq,"analyze/imu_gyro_raw_per.csv");
dump_buffer_to_file(&an_imu_gyro_raw_lat,"analyze/imu_gyro_raw_lat.csv");
destroy_analyze(&an_imu_gyro_raw_freq);
destroy_analyze(&an_imu_gyro_raw_lat);
printf("ANALYZE RESULTS - IMU_MAG_RAW:\n");
printf("avg period:\t %0.4f ms\n",(get_avg(&an_imu_mag_raw_freq)));
printf("avg freq:\t %0.4f hz\n",1/(get_avg(&an_imu_mag_raw_freq))*1e3);
printf("avg latency\t %0.4f ms\n",get_avg(&an_imu_mag_raw_lat));
printf("\n");
dump_buffer_to_file(&an_imu_mag_raw_freq,"analyze/imu_mag_raw_per.csv");
dump_buffer_to_file(&an_imu_mag_raw_lat,"analyze/imu_mag_raw_lat.csv");
destroy_analyze(&an_imu_mag_raw_freq);
destroy_analyze(&an_imu_mag_raw_lat);
printf("ANALYZE RESULTS - BARO_RAW:\n");
printf("avg period:\t %0.4f ms\n",(get_avg(&an_baro_raw_freq)));
printf("avg freq:\t %0.4f hz\n",1/(get_avg(&an_baro_raw_freq))*1e3);
printf("avg latency\t %0.4f ms\n",get_avg(&an_baro_raw_lat));
printf("\n");
dump_buffer_to_file(&an_baro_raw_freq,"analyze/baro_raw_per.csv");
dump_buffer_to_file(&an_baro_raw_lat,"analyze/baro_raw_lat.csv");
destroy_analyze(&an_baro_raw_freq);
destroy_analyze(&an_baro_raw_lat);
printf("ANALYZE RESULTS - GPS_INT:\n");
printf("avg period:\t %0.4f ms\n",get_avg(&an_gps_int_freq));
printf("avg freq:\t %0.4f hz\n",1/(get_avg(&an_gps_int_freq))*1e3);
printf("avg latency\t %0.4f ms\n",get_avg(&an_gps_int_lat));
printf("\n");
dump_buffer_to_file(&an_gps_int_freq,"analyze/gps_int_per.csv");
dump_buffer_to_file(&an_gps_int_lat,"analyze/gps_int_lat.csv");
destroy_analyze(&an_gps_int_freq);
destroy_analyze(&an_gps_int_lat);
printf("ANALYZE RESULTS - AIRSPEED_ETS:\n");
printf("avg period:\t %0.4f ms\n",get_avg(&an_airspeed_ets_freq));
printf("avg freq:\t %0.4f hz\n",1/(get_avg(&an_airspeed_ets_freq))*1e3);
printf("avg latency\t %0.4f ms\n",get_avg(&an_airspeed_ets_lat));
printf("\n");
dump_buffer_to_file(&an_airspeed_ets_freq,"analyze/airspeed_ets_per.csv");
dump_buffer_to_file(&an_airspeed_ets_lat,"analyze/airspeed_ets_lat.csv");
destroy_analyze(&an_airspeed_ets_freq);
destroy_analyze(&an_airspeed_ets_lat);
printf("ANALYZE RESULTS - ACTUATORS:\n");
printf("avg period:\t %0.4f ms\n",get_avg(&an_actuators_freq));
printf("avg freq:\t %0.4f hz\n",1/(get_avg(&an_actuators_freq))*1e3);
printf("avg latency\t %0.4f ms\n",get_avg(&an_actuators_lat));
printf("\n");
dump_buffer_to_file(&an_actuators_freq,"analyze/actuators_per.csv");
dump_buffer_to_file(&an_actuators_lat,"analyze/actuators_lat.csv");
destroy_analyze(&an_actuators_freq);
destroy_analyze(&an_actuators_lat);
printf("ANALYZE RESULTS - UART_ERRORS:\n");
printf("avg period:\t %0.4f ms\n",get_avg(&an_UART_errors_freq));
printf("avg freq:\t %0.4f hz\n",1/(get_avg(&an_UART_errors_freq))*1e3);
printf("avg latency\t %0.4f ms\n",get_avg(&an_UART_errors_lat));
printf("\n");
dump_buffer_to_file(&an_UART_errors_freq,"analyze/UART_errors_per.csv");
dump_buffer_to_file(&an_UART_errors_lat,"analyze/UART_errors_lat.csv");
destroy_analyze(&an_UART_errors_freq);
destroy_analyze(&an_UART_errors_lat);
printf("ANALYZE RESULTS - SYS_MON:\n");
printf("avg period:\t %0.4f ms\n",get_avg(&an_sys_mon_freq));
printf("avg freq:\t %0.4f hz\n",1/(get_avg(&an_sys_mon_freq))*1e3);
printf("avg latency\t %0.4f ms\n",get_avg(&an_sys_mon_lat));
printf("\n");
dump_buffer_to_file(&an_sys_mon_freq,"analyze/sys_mon_per.csv");
dump_buffer_to_file(&an_sys_mon_lat,"analyze/sys_mon_lat.csv");
destroy_analyze(&an_sys_mon_freq);
destroy_analyze(&an_sys_mon_lat);
exit(1);
}
#endif
}
UDP_err_handler(closeUDPServerSocket(&udp_server),write_error_ptr);
/*------------------------END OF FIRST THREAD------------------------*/
//wait for the second thread to finish
if(pthread_join(thread_server_to_planebone, NULL)) {
error_write(FILENAME,"error joining thread_lisa_to_pc");
exit(EXIT_FAILURE);
}
return 0;
}
