Commit c961a14c by raphael.deimel

fix bug where the first signal sending is skipped, make histogram being printed…

fix bug where the first signal sending is skipped, make histogram being printed when a reset is done, syntax cleaning
parent ebb5be9a
......@@ -145,23 +145,23 @@ int main(int argc, char** argv){
//respawn point after a reset event, don't kill server but reallocate and initialize all other objects
while (1) {
// AirserverIO::AD7147* sensor = new AirserverIO::AD7147(1, 0x2F);
// AirserverIO::AD7147* sensor = new AirserverIO::AD7147(1, 0x2F);
// //read at least once
// int buffer[6];
// int i;
// cout << "\nTest\n" << sensor->get_success();
// if(sensor->get_success())
// sensor->readRegs(buffer);
//
// // I2C synchronous mode
// sensor->readRegs(buffer);
// std::cout << "\nTest\n";
// for( i=0; i<6; i++ )
// {
// std::cout << "buffer["<<i<<"]=" << buffer[i]<< "\n";
// }
// std::cout << "\n\n";
// //read at least once
// int buffer[6];
// int i;
// cout << "\nTest\n" << sensor->get_success();
// if(sensor->get_success())
// sensor->readRegs(buffer);
//
// // I2C synchronous mode
// sensor->readRegs(buffer);
// std::cout << "\nTest\n";
// for( i=0; i<6; i++ )
// {
// std::cout << "buffer["<<i<<"]=" << buffer[i]<< "\n";
// }
// std::cout << "\n\n";
......@@ -214,34 +214,28 @@ int main(int argc, char** argv){
last = end;
Sleep(next_update - end); //wait until the next loop iteration should start
ROStime_t now_real = GetNow();
now = next_update; //use the nominal time to compare messages against
now = next_update; //use the nominal time to compare messages against
next_update = next_update + RT_PERIOD; //set the time of the next controller execution
/*entering the really time critical section*/
/*the rest of the loop is for data handling, and setting up the signals for the next loop iteration:*/
if ( now_real > next_update ) {
ROStime_t delta = (now_real - last - RT_PERIOD);
std::cout << "Warning: System lag: got woken up " << delta.nsecs * 1e-6 <<"ms too late."<<std::endl;
}
if ( now_real > next_update ) {
ROStime_t delta = (now_real - last - RT_PERIOD);
std::cout << "Warning: System lag: got woken up " << delta.nsecs * 1e-6 <<"ms too late."<<std::endl;
}
AirserverIO::updateInputAdc(); //read from sensors
//std::cout << "updateInputI2C() took " << ( GetNow() - before ).nsecs * 1e-3 << "us\n";
//std::cout << "updateInputI2C() took " << ( GetNow() - before ).nsecs * 1e-3 << "us\n";
controllers->run(); //execute all controllers
AirserverIO::update_output(); //output to acutators
#ifdef MAKE_HISTOGRAM
float usecs_coreloop_presubscription = ( GetNow() - now ).nsecs * 1e-3;
#endif
subscription_manager->run(now); /*update the client with new values if requested*/
/*leaving the really time critical section*/
#ifdef MAKE_HISTOGRAM
float usecs_coreloop = ( GetNow() - now ).nsecs * 1e-3;
acc_coreloop(usecs_coreloop); //profile timing
if (max_recorded_period_coreloop < usecs_coreloop) {max_recorded_period_coreloop = usecs_coreloop;}
if (usecs_coreloop > 1500) {std::cout << "Info: Core Loop took much more than expected! (" << usecs_coreloop<<" us)"<<std::endl;}
if (usecs_coreloop - usecs_coreloop_presubscription> 300) {std::cout << "Info: Signal Subscription management took more than expected! (" << usecs_coreloop - usecs_coreloop_presubscription<<" us)"<<std::endl;}
#endif
/*the rest of the loop is for data handling, and setting up the signals for the next loop iteration:*/
......@@ -271,10 +265,10 @@ int main(int argc, char** argv){
subscription_manager->unsubscribeAllSignals(msg.getSender()); //implicitly cancel all signal subscriptions, but only for the client that requested the reset
break;
case MSGTYPE_EVENT:
if (LOG_INFO) {
MsgSignalEvent* event = (MsgSignalEvent*) d;
std::cout<< "MSGTYPE_EVENT: id:" << event->signal_id << ", value:" << event->value <<endl;
}
if (LOG_INFO) {
MsgSignalEvent* event = (MsgSignalEvent*) d;
std::cout<< "MSGTYPE_EVENT: id:" << event->signal_id << ", value:" << event->value <<endl;
}
Event( (MsgSignalEvent*)d ) && std::cout<<"Warning: Can't perform the event\n";
break;
case MSGTYPE_CONFIG_CONTROLLER_THRESHOLD:
......@@ -352,9 +346,23 @@ int main(int argc, char** argv){
}//end while
/*****************/
subscription_manager->run(now); /*update the client with new values if requested*/
}
EmergencySoftStop("Info: Doing a Reset now.");
//delete subscription_manager;
//EmergencySoftStop("Info: Doing a Reset now."); //not exactly necessary
cout << "Info: Doing a Reset now." <<endl;
#ifdef MAKE_HISTOGRAM
std::cout << "Maximum period length: " << max_recorded_period <<"us"<< std::endl;
std::cout << "Maximum period length coreloop: " << max_recorded_period_coreloop <<"us"<< std::endl;
std::cout << "Maximum period length network: " << max_recorded_period_network <<"us"<< std::endl;
plot_histogram(acc);
std::cout << "Core loop runtime:" <<std::endl;
plot_histogram(acc_coreloop);
std::cout << "Network runtime:" <<std::endl;
plot_histogram(acc_network);
#endif
//delete subscription_manager;
delete controllers; //TODO: does not delete individual controllers
delete MsgQueue;
AirserverIO::exit();
......@@ -370,11 +378,11 @@ void plot_histogram(boost::accumulators::accumulator_set<float, boost::accumulat
boost::iterator_range<std::vector<std::pair<float, float> >::iterator > histogram;
histogram = boost::accumulators::density(acc);
for (std::size_t i = 0; i < (unsigned int)histogram.size(); ++i) {
int count = (histogram[i].second * boost::accumulators::count(acc));
if (count > 30) {count=30;} //limit how much is printed
std::cout << "dt<" << histogram[i].first << "us: ";
int count = (histogram[i].second * boost::accumulators::count(acc));
if (count > 30) {count=30;} //limit how much is printed
std::cout << "dt<" << histogram[i].first << "us: ";
while (--count > 0) {std::cout << "#";}
std::cout << std::endl;
std::cout << std::endl;
}
}
#endif
......@@ -384,16 +392,6 @@ void exitHandler(int signal){
EmergencySoftStop("Server exiting");
AirserverIO::exit();
unlink(listener->name_unixdomainsocket.c_str()); //TODO: sanity check file descriptor
#ifdef MAKE_HISTOGRAM
std::cout << "Maximum period length: " << max_recorded_period <<"us"<< std::endl;
std::cout << "Maximum period length coreloop: " << max_recorded_period_coreloop <<"us"<< std::endl;
std::cout << "Maximum period length network: " << max_recorded_period_network <<"us"<< std::endl;
plot_histogram(acc);
std::cout << "Core loop runtime:" <<std::endl;
plot_histogram(acc_coreloop);
std::cout << "Network runtime:" <<std::endl;
plot_histogram(acc_network);
#endif
exit(EXIT_SUCCESS);
}
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment