diff --git a/src/asterx1_gps.cpp b/src/asterx1_gps.cpp index c3cbeec09ea0e4da98f26f2e1185b7156533c468..5ed0614ae986d4f64ac2a5655830d9095fb398dd 100644 --- a/src/asterx1_gps.cpp +++ b/src/asterx1_gps.cpp @@ -124,7 +124,8 @@ void CasteRx1::open_device() this->thread_server->start_thread(this->gps_process_data_thread_id); // configure the data streams this->configure_streams(); - }catch(CCommException &e){ + }catch(CException &e){ + this->close_device(); if(this->serial_port!=NULL) { delete this->serial_port; @@ -139,18 +140,25 @@ void CasteRx1::close_device() // stop the internal acquisition thread if necessary this->stop_acquisition(); // close the serial port - if(this->thread_server->get_thread_state(this->gps_process_data_thread_id)==active) - { + if(this->thread_server->get_thread_state(this->gps_process_data_thread_id)==active || + this->thread_server->get_thread_state(this->gps_process_data_thread_id)==starting) + { this->event_server->set_event(this->finish_thread_event_id); this->thread_server->end_thread(this->gps_process_data_thread_id); } + /* reset any pending events */ + if(this->event_server->event_is_set(this->new_reply_event_id)) + this->event_server->reset_event(this->new_reply_event_id); + if(this->event_server->event_is_set(this->finish_thread_event_id)) + this->event_server->reset_event(this->finish_thread_event_id); if(this->serial_port!=NULL) this->serial_port->close(); } void CasteRx1::start_acquisition() { - if(this->thread_server->get_thread_state(this->gps_process_data_thread_id)==active) + if(this->thread_server->get_thread_state(this->gps_process_data_thread_id)==active || + this->thread_server->get_thread_state(this->gps_process_data_thread_id)==starting) { if(!this->gps_running) { @@ -333,7 +341,7 @@ void *CasteRx1::gps_process_data_thread(void *params) CasteRx1 *gps=(CasteRx1 *)params; static gps_state_t state=header1; std::list<std::string> events; - unsigned char *data,*sbf_data; + unsigned char *data=NULL,*sbf_data=NULL; std::string reply; bool end=false; @@ -350,14 +358,20 @@ void *CasteRx1::gps_process_data_thread(void *params) gps->gps_access.enter(); num=gps->serial_port->get_num_data(); if(data!=NULL) + { delete[] data; + data=NULL; + } data=new unsigned char[num]; gps->serial_port->read(data,num); gps->gps_access.exit(); }catch(CException &e){ std::cout << e.what() << std::endl; if(data!=NULL) + { delete[] data; + data=NULL; + } gps->gps_access.exit(); } for(i=0;i<num;i++) @@ -1237,7 +1251,6 @@ void CasteRx1::send_command(const std::string &cmd) this->gps_access.exit(); }catch(CEventTimeoutException &e){ // no answer in the allowed time - this->gps_access.exit(); throw CasteRx1Exception(_HERE_,"Device did not answer in time"); } }