int main(){
mtsl_init();
#ifndef __SIMULATION_MODE__
pthread_t th0, th1;
pthread_mutex_init( &mutex_lock, NULL );
pthread_create( &th0, NULL, (void*)th_advert, NULL );
sleep( ADVERT_BASE_PERIOD );
pthread_create( &th1, NULL, (void*)th_recv, NULL );
#endif
while(1){ // this is the main thread
led3_toggle();
#ifdef __SIMULATION_MODE__
do_step();
report();
#else
if( my_info.level < MTSL_LEVEL3 ) continue;
pthread_mutex_lock( &mutex_lock );
mtsl_ranging();
mtsl_dist_filtering();
fill_anchor();
self_pos();
report();
pthread_mutex_unlock( &mutex_lock );
#endif
sleep( RANGING_BASE_PERIOD );
}
return 0;
}
/*******************************************************************************
* @fn uint8_t process_rx( uint8_t* buffer, uint8_t size )
* @brief callback function called when new message is received
* ****************************************************************************/
uint8_t process_rx( uint8_t* buffer, uint8_t size )
{
packet_header_t* header;
static uint8_t counter = 0;
header = (packet_header_t*)(buffer);
//packet_footer_t* footer;
// Add one to account for the byte with the packet length
//footer = (packet_footer_t*)(buffer + header->length + 1 );
//uart_write( , 1 );
uart_write_escaped( buffer, header->length + 1 );
// Erase buffer just for fun
memset( buffer, 0x00, size );
led3_toggle();
return 1;
}