Commit ebb5be9a by raphael.deimel

Fix bug in signalsubscriptions being skipped when not perfectly aligned, limit…

Fix bug in signalsubscriptions being skipped when not perfectly aligned, limit number of packets/s being dispatched, make log messages more uniform, throttle flash memory in all possible ways to avoid skipping, make control loop use nominal execution time to compare against message timestamps to avoid jitter
parent 55788ea4
......@@ -6,7 +6,7 @@
#ARM=arm-linux-gnueabihf-
ARM=
CC=g++
CFLAGS=-c -Wall -g -std=c++11
CFLAGS=-c -Wall -g -std=c++11 -O3
BBBIO_LIB= ../3rdparty/BBBIOlib
......
......@@ -32,6 +32,17 @@ namespace AirserverController{
const float CONTROLRATE_S = 1e9 / (float)PERIOD_NS;
const ROStime_t RT_PERIOD=ConvertFromNS(PERIOD_NS); //2 ms loop
/**
* Aligns the given timestamp to the next possible absolute control period time
*/
inline ROStime_t alignWithNextControlLoop(ROStime_t t) {
t.nsecs = t.nsecs + (AirserverController::PERIOD_NS - t.nsecs % AirserverController::PERIOD_NS); //align to the next timepoint
return t;
}
/**
* Controller sub class
*/
......
......@@ -110,6 +110,9 @@ int main(int argc, char** argv){
ROStime_t now={0,0}, next_update=RT_PERIOD, end={0,0}, time_run = {0,0};
cout << std::endl << "Starting." << std::endl;
//init server
listener = new Server;
......@@ -178,7 +181,7 @@ int main(int argc, char** argv){
AirserverController::ControllerGroup* controllers = new AirserverController::ControllerGroup();
if (!IsClockAccessible()) {cout<<"Could not acquire realtime clock time.\n";exitHandler(0);}
if (!IsClockAccessible()) {cout<<"ERROR: Could not acquire realtime clock time.\n";exitHandler(0);}
now = GetNow();
ROStime_t last = now;
......@@ -188,7 +191,7 @@ int main(int argc, char** argv){
next_update.nsecs = 0;
next_update = rectify(next_update);
cout<<"State reset finished."<<endl;
//cout<<"State reset finished."<<endl;
//actual main loop
while(!signal_reset){
......@@ -203,24 +206,23 @@ int main(int argc, char** argv){
#endif
//Check if we need to skip some iterations because we are too late already
if (next_update < end) {
std::cout << "Info: Loop took too long! (" << usecs<<"us). Skipping to next possible control loop timepoint."<<std::endl;
next_update = end;
next_update.nsecs = next_update.nsecs + (AirserverController::PERIOD_NS - next_update.nsecs % AirserverController::PERIOD_NS); //align to the next timepoint
std::cout << "Warning: It took me too long to finish the loop! (" << usecs*1e-3 <<"ms). Skipping to next possible control loop timepoint."<<std::endl;
next_update = AirserverController::alignWithNextControlLoop(end); //align to the next timepoint
next_update = next_update + RT_PERIOD; //fast-forward and skip one iteration
}
Sleep(next_update - end); //wait until the next loop iteration should start
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
next_update = next_update + RT_PERIOD; //set the time of the next controller execution
/*entering the really time critical section*/
last = now;
now = GetNow();
/*the rest of the loop is for data handling, and setting up the signals for the next loop iteration:*/
if ( now > next_update ) {
ROStime_t delta = (now - last);
std::cout << "Info: System lag: " << delta.nsecs * 1e-3 <<"us"<<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
......@@ -228,13 +230,17 @@ int main(int argc, char** argv){
//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 more than 1500us! (" << usecs_coreloop<<" us)"<<std::endl;}
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
......@@ -249,7 +255,7 @@ int main(int argc, char** argv){
float usecs_network = ( GetNow() - t_network_start ).nsecs * 1e-3;
acc_network(usecs_network); //profile timing
if (max_recorded_period_network < usecs_network) {max_recorded_period_network = usecs_network;}
if (usecs_network > 1000) {std::cout << "Info: Processing incoming messages took more than 1000us! (" << usecs_network<<" us)"<<std::endl;}
if (usecs_network > 1500) {std::cout << "Info: Processing incoming messages took more than 1500us! (" << usecs_network<<" us)"<<std::endl;}
#endif
//TODO: ServerCheck only reads in a single message. iterate a few more times to not congest the connections
......@@ -317,7 +323,7 @@ int main(int argc, char** argv){
std::cout<<"Warning: MSGTYPE_SYNC_REPLY not implemented\n";
break;
case MSGTYPE_ADVERTISE_VERSION:
cout << "Client is advertising the use of API version " << ((MsgAdvertiseVersion*)d)->version << " (my version: " << PNEUMATICBOX_API_VERSION << ")" << std::endl;
cout << "Info: Client is advertising the use of API version " << ((MsgAdvertiseVersion*)d)->version << " (my version: " << PNEUMATICBOX_API_VERSION << ")" << std::endl;
if (!IsClientCompatible(((MsgAdvertiseVersion*)d)->version)) {
EmergencySoftStop("ERROR: Client tries to use an incompatible API version. Killing myself to avoid stupid mistakes.");
exitHandler(0);
......@@ -347,7 +353,7 @@ int main(int argc, char** argv){
/*****************/
}
EmergencySoftStop("Info: Resetting my internal state...");
EmergencySoftStop("Info: Doing a Reset now.");
//delete subscription_manager;
delete controllers; //TODO: does not delete individual controllers
delete MsgQueue;
......
......@@ -127,26 +127,21 @@ public:
// bool operator>= (const SignalSubscription& rhs) const {return !(rhs > *this);}
}; //class SignalSubscription
struct compareSignalSubscriptions {
bool operator() (const SignalSubscription* lhs, const SignalSubscription* rhs) const {
return (*lhs < *rhs);
}
};
typedef std::set<SignalSubscription*, compareSignalSubscriptions> SigSubSet;
typedef std::set<SignalSubscription*> SigSubSet;
class Subscriber {
public:
inline Subscriber() {this->client_id = -1;} //needed for SubMap access
inline Subscriber(int32_t client_id) {this->client_id = client_id;}
inline Subscriber() {this->client_id = -1;this->nextMessageDispatchTime={0,0};} //needed for SubMap access
inline Subscriber(int32_t client_id) {this->client_id = client_id;this->nextMessageDispatchTime={0,0};}
int32_t client_id;
SigSubSet subscriptions;
struct {
AirServerSignal signal;
AirServerSignalValue value;
} buffer[8];
ROStime_t nextMessageDispatchTime;
void clearBuffer();
int sendAwayMessage(ROStime_t& now);
int sendAwayMessage(ROStime_t& now, int flags);
}; //class Subscriber
typedef std::map<int32_t, Subscriber*> SubMap;
......@@ -155,6 +150,7 @@ public:
SubMap subscribers;
};
static const ROStime_t MIN_SILENT_TIME = ConvertFromNS(10000000); //number of periods that the airserver should not indicate a packet dispatch after having forced a message dispatch, in order to avoid swamping the network with packets
}
......
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