CppExampleMonitoring Monitoring

From Skybotix

 #include <stdlib.h>
 #include <stdio.h>
 #include <math.h>
 #include <string.h>
 
 #ifndef M_PI
 #define M_PI 3.14159265358979323846
 #endif
 
 #ifdef WIN32
 #define sscanf sscanf_s
 #define strcpy strcpy_s
 #define strncpy strncpy_s
 #endif
 
 #include <string>
 #include <vector>
 
 #include <sbapi.h>
 #include <sbsimple.h>
 #include "PID.h"
 
 #define D2R(X) ((X)*M_PI/180.0)
 #define R2D(X) ((X)*180.0/M_PI)
 #define MAX(X,Y) (((X)<(Y))?(Y):(X))
 #define MIN(X,Y) (((X)<(Y))?(X):(Y))
 
 //#define DEBUG(c) printf("Executing "#c"\n")
 //#define DEBUG(c) res=0;printf("Executing "#c"\n");c;printf("Result %d\n",res)
 #define DEBUG(c) res=0;c;if (res) printf("Result of "#c": %d\n",res)
 
 //#define CRITICAL(c) res=0;printf("Executing "#c"\n");c;printf("Result %d\n",res); if(res) return res
 #define CRITICAL(c) res=0;c;if (res) {printf("Result %d\n",res); return res;}
 
 
 static int end = 0;
 
 void sighdl(int n) {
 	end ++;
 }
 
 class SBController
 {
 	protected:
 		SBApiSimpleContext *simple;
 		unsigned long sensorList;
 		int res;
 	public:
 		SBController(SBApiSimpleContext *s) : simple(s), sensorList(0), res(0) {}
 		~SBController() {}
 
 		int initialise(const std::string & devname,const std::string & port="") {
 			res = 0;
 			sbSimpleDefaultContext(simple);
 			simple->device[0] = 0;
 			simple->commPort = 5123; // that is the second port when using 
 									 // monitoring functions for instance
 			strncpy(simple->device,devname.c_str(),255);
 #ifdef WIN32
 			int comid;
 			simple->commType = SB_COMMTYPE_UDP;
 			if (sscanf(simple->device,"COM%d",&comid)==1) {
 				simple->commType = SB_COMMTYPE_SERIAL;
 			} else {
 				simple->commType = SB_COMMTYPE_UDP;
 			}
 #else
 			if (simple->device[0] == '/') {
 				simple->commType = SB_COMMTYPE_SERIAL;
 			} else {
 				simple->commType = SB_COMMTYPE_UDP;
 			}
 #endif
 			if (!port.empty()) {
 				sscanf(port.c_str(),"%d",&simple->commPort);
 			}
 			simple->device[255] = 0;
 			simple->initNavState = SB_NAV_STOP;
 			simple->cmdTimeout = 1000;
 			simple->ctrlTimeout =1000;
 			simple->sensorList = &sensorList;
 			simple->rollCtrlMode = SB_CTRL_POS;
 			simple->pitchCtrlMode = SB_CTRL_POS;
 			simple->masterMode = 0;
 
 			DEBUG(res = sbSimpleInitialise(simple));
 			printf("Channel connected, continuing\n");
 
 			return res;
 		}
 
 		int terminate() {
 			res = 0;
 			DEBUG(res = sbSimpleTerminate(simple));
 			return res;
 		}
 
 		void monitor() {
 			while (!*(simple->endP)) {
 				DEBUG(sbSimpleWaitState(simple,NULL,1.0));
 				printf("Packet count: %ld\n",simple->packetcount);
 				sbStatePrint(stdout,&(simple->state));
 			}
 		}
 
 
 
 };
 
 
 
 int main(int argc, const char *argv[])
 {
 	int res;
 	SBApiSimpleContext simple;
 	SBController api(&simple);
 
 	printf("Coax basic monitoring application \n");
 	CRITICAL(res = api.initialise(
 				(argc<2)?("localhost"):(argv[1]), // Host name (or serial port)
 				(argc<3)?("5123"):(argv[2])      // Port number
 			));
 	if (res) return -1;
 	api.monitor();
 	DEBUG(res = api.terminate());
 
 	return 0;
 }