Commit 58af402c by Raphael Deimel

Merged branch api9 into master

parents 35c761c0 9a1a3589
......@@ -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