예제 #1
0
template<> ValueStack::T ValueStack::pop()
{
	if (!something()) { eprintf("Cannot pop from empty stack!\n"); eexit(); }
	T r = *(--_p);
	_p->Free();
	return r;
}
예제 #2
0
/*
 * ParseDirective
 * Note: macro directives are handled before recording
 */
void ParseDirective(const char *cline)
{
	char *line = strdup(strltrim(cline) + 1);	// skip '.' and allow strtok
//printf("'%s'\n", file->line);
//printf("'%s'\n", line);
	bool valid_directive = false;
	//bool done_directive = true;
//printf("Line: [%s]\n", cline);
	
	/*
	 * macro ending directives
	 */

	if (DIRECTIVE("endm", PARSE_MACRO_DIRECTIVES))
	{
		current_macro = 0;
		parse_directives = PARSE_ALL_DIRECTIVES;
	}
	else if (DIRECTIVE("endr", PARSE_REPT_DIRECTIVES))
	{
		current_macro = 0;
		parse_directives = PARSE_ALL_DIRECTIVES;
		while (repeat-- > 0) MacroExecute("_rept");
	}

	/*
	 * record to macro
	 */
	if (current_macro)
	{
		MacroLine(cline);		// record full line
		goto exit;				// only process macro ending directives
	}

	/*
	 * macro starting directives
	 */
	if (DIRECTIVE("macro", PARSE_MACRO_DIRECTIVES) || DIRECTIVE("macroicase", PARSE_MACRO_DIRECTIVES))
	{
		char *name = strtok((char *)strskipspace(line), delim_chars);
		current_macro = FindMacro(name);
		if (pass != PASS_ASM)
		{
			if (current_macro) { eprintf("Macro name already defined.\n"); eexit(); }
			current_macro = NewMacro(name, DIRECTIVE("macroicase", PARSE_MACRO_DIRECTIVES));
EEKS{printf("new macro at %p\n", current_macro);}
			char *paramname;
			while ((paramname = strtok(0, delim_chars)))
			{
				if (isspace2(paramname[0])) paramname = strskipspace(paramname);
				if (strchr(endline_chars, *paramname)) break;

				current_macro->AddParameter(paramname);
			}
		}
		parse_directives = PARSE_MACRO_DIRECTIVES;
	}
예제 #3
0
int creat_cd_unit(FILE* file, cd_unit* unit)
{
	double summ = 0;
	double temp = 0;
	char string[100];
	if (fgets(string, 100, file) != NULL)
	{
		if ( 1 != sscanf(string, "%d", (int*)&(unit->rows)) )
			{ eexit("Cannot read file\n") }	
	}	
	unit->data = malloc(sizeof(double) * unit->rows);

	for (int i = 0; i < unit->rows; i++) {
		fgets(string,100,file);
		if ( 1 != sscanf(string, "%lf", &temp) )
			{ eexit("Cannot read file\n") } 
		unit->data[i] = summ;
		summ += temp;
		if (summ > 1.0) { eexit("Wrong distribution file containg\n") }
	}
	return 0;
}
예제 #4
0
/*
 * eprintf
 * Print error
 */
void eprintf(const char *fmt, ...)
{
	char errormsg[TMPSIZE];
	va_list marker;
	va_start(marker, fmt);
	/*int r =*/ vsprintf(errormsg, fmt, marker);
	va_end(marker);
	char *p;
	if ((p = strchr(file->origline, '\r'))) *p = 0;
	if ((p = strchr(file->origline, '\n'))) *p = 0;
	while ((p = strchr(file->origline, '\t'))) *p = ' ';
	fprintf(stderr, "%s, line %d: \'%s\' : %s",
		file->filename, file->line_num, file->origline, errormsg
	);
	if (++errors >= MAX_ERRORS)
	{
		fprintf(stderr, "Maximum number of errors reached.\n"); eexit();
	}
}
예제 #5
0
int main(int argc, char* argv[])
{
#if defined( _WINDOZ )
    WSADATA WSAData;
    WSAStartup(MAKEWORD(2,0), &WSAData);
#endif

    // msp paylaod
    payload = calloc(sizeof(*payload), sizeof(*payload));

    // mwi state
    mwiState = calloc(sizeof(*mwiState), sizeof(*mwiState));
    mwiState->callback = &callBack_mwi;

    // mavlink state
    mavlinkState = calloc(sizeof(*mavlinkState), sizeof(*mavlinkState));
    mavlinkState->mwiUavID = 1;
    mavlinkState->mwiAutoPilotType = MAV_AUTOPILOT_GENERIC;
    mavlinkState->mwiFlightMode = MAV_STATE_UNINIT; // initial mode is unknown
    mavlinkState->mwiAirFrametype = MAV_TYPE_GENERIC;
    mavlinkState->autoTelemtry = NOK;
    mavlinkState->baudrate = SERIAL_115200_BAUDRATE;
    mavlinkState->hertz = 30;
    strcpy(mavlinkState->targetIp, "127.0.0.1");
    strcpy(mavlinkState->serialDevice, "/dev/ttyO2");

    // some counters
    uint64_t lastFrameRequest = 0;
    uint64_t lastHeartBeat = 0;
    uint64_t lastReaquestLowPriority = 0;
    uint64_t currentTime = microsSinceEpoch();

    // create a configuration from command line
    if (config(mavlinkState, argc, argv) == NOK) {
        eexit(serialLink);
    }
    // translate argument to something mavlink knows about
    if (mavlinkState->mwiAutoPilotType == TYPE_PX4) {
        mavlinkState->mwiAutoPilotType = MAV_AUTOPILOT_PX4;
    }

    // create our socket
    sock = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP);
    sockFSin = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP);
    sockFSout = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP);

    memset(&locAddr, 0, sizeof(locAddr));
    locAddr.sin_family = AF_INET;
    locAddr.sin_port = htons(14551);
    locAddr.sin_addr.s_addr = htonl(INADDR_LOOPBACK);

    memset(&locGSAddr, 0, sizeof(locGSAddr));
    locGSAddr.sin_family = AF_INET;
    locGSAddr.sin_addr.s_addr = htonl(INADDR_LOOPBACK);
    locGSAddr.sin_port = htons(14550); // TODO port number option

    // Bind the socket to port 14551 - necessary to receive packets from qgroundcontrol
    if (NOK != bind(sock, (struct sockaddr *)&locAddr, sizeof(struct sockaddr))) {
        perror("error bind to port 14551 failed");
        eexit(serialLink);
    }

    memset(&locFSAddr, 0, sizeof(locFSAddr));
    locFSAddr.sin_family = AF_INET;
    locFSAddr.sin_addr.s_addr = htonl(INADDR_LOOPBACK);
    locFSAddr.sin_port = htons(15500); // TODO port number option

    memset(&locAddr, 0, sizeof(locAddr));
    locAddr.sin_family = AF_INET;
    locAddr.sin_port = htons(15501);
    locAddr.sin_addr.s_addr = htonl(INADDR_LOOPBACK);

    // Bind the socket to port 15501 - necessary to receive packets from simulator
    if (NOK != bind(sockFSin, (struct sockaddr *)&locAddr, sizeof(struct sockaddr))) {
        perror("error bind to port 15501 failed");
        eexit(serialLink);
    }

    // Attempt to make it non blocking
#if defined(_WINDOZ)
    u_long argi = 1;
    if(ioctlsocket(sock, FIONBIO, &argi )<0) {
        fprintf(stderr, "error setting nonblocking: %s\n", strerror(errno));
        eexit(serialLink);
    }

    argi = 1;
    if(ioctlsocket(sockFSin, FIONBIO, &argi )<0) {
        fprintf(stderr, "error setting nonblocking: %s\n", strerror(errno));
        eexit(serialLink);
    }
    argi = 1;
    if(ioctlsocket(sockFSout, FIONBIO, &argi )<0) {
        fprintf(stderr, "error setting nonblocking: %s\n", strerror(errno));
        eexit(serialLink);
    }
#define SOCKLEN_T_INT(fromlen) ((int*)fromlen)
    int opt = 1;
       if (setsockopt(sock, SOL_SOCKET, SO_REUSEADDR, &opt, sizeof(opt)) < 0){
           printf("IMU setsockopt SO_REUSEADDR");
           eexit(serialLink);
       }
       opt = 1;
       if (setsockopt(sockFSin, SOL_SOCKET, SO_REUSEADDR, &opt, sizeof(opt)) < 0){
           printf("IMU setsockopt SO_REUSEADDR");
           eexit(serialLink);
       }
       opt = 1;
       if (setsockopt(sockFSout, SOL_SOCKET, SO_REUSEADDR, &opt, sizeof(opt)) < 0){
           printf("IMU setsockopt SO_REUSEADDR");
           eexit(serialLink);
       }
#else
    if (fcntl(sock, F_SETFL, O_NONBLOCK | FASYNC) < 0) {
        fprintf(stderr, "error setting nonblocking: %s\n", strerror(errno));
        close(sock);
        eexit(serialLink);
    }
    if (fcntl(sockFSin, F_SETFL, O_NONBLOCK | FASYNC) < 0) {
        fprintf(stderr, "error setting nonblocking: %s\n", strerror(errno));
        close(sockFSin);
        eexit(serialLink);
    }
    if (fcntl(sockFSout, F_SETFL, O_NONBLOCK | FASYNC) < 0) {
        fprintf(stderr, "error setting nonblocking: %s\n", strerror(errno));
        close(sockFSout);
        eexit(serialLink);
    }
#define SOCKLEN_T_INT(fromlen) fromlen

    int opt = 1;
    if (setsockopt(sock, SOL_SOCKET, SO_REUSEADDR, &opt, sizeof(opt)) < 0)
        err(1, "IMU setsockopt SO_REUSEADDR");
    opt = 1;
    if (setsockopt(sockFSin, SOL_SOCKET, SO_REUSEADDR, &opt, sizeof(opt)) < 0)
        err(1, "IMU setsockopt SO_REUSEADDR");
    opt = 1;
    if (setsockopt(sockFSout, SOL_SOCKET, SO_REUSEADDR, &opt, sizeof(opt)) < 0)
        err(1, "IMU setsockopt SO_REUSEADDR");
    // end socket
#endif
    // init serial
    serialLink = MWIserialbuffer_init(mavlinkState->serialDevice, mavlinkState->baudrate);
    if (serialLink == NOK) {
        perror("error opening serial port");
        eexit(serialLink);
    }

    MW_TRACE("starting..\n")
    for (;;) {

        currentTime = microsSinceEpoch();

        if (!mavlinkState->autoTelemtry && ((currentTime - lastFrameRequest) > (1000 * (1000 / (uint32_t)(mavlinkState->hertz))))) {
            lastFrameRequest = currentTime;
            if (mwiState->init == OK) {
                if ((currentTime - lastHeartBeat) > 1000 * 500) {
                    // ~ 2hz
                    lastHeartBeat = currentTime;
                    MWIserialbuffer_askForFrame(serialLink, MSP_IDENT, payload);
                    MWIserialbuffer_askForFrame(serialLink, MSP_STATUS, payload);
                }
                if ((currentTime - lastReaquestLowPriority) > 1000 * 90) {
                    // ~10hz
                    lastReaquestLowPriority = currentTime;
                    MWIserialbuffer_askForFrame(serialLink, MSP_ANALOG, payload);
                    MWIserialbuffer_askForFrame(serialLink, MSP_COMP_GPS, payload);
                    MWIserialbuffer_askForFrame(serialLink, MSP_RAW_GPS, payload);
                }
                // ~30 hz
                MWIserialbuffer_askForFrame(serialLink, MSP_ATTITUDE, payload);
                MWIserialbuffer_askForFrame(serialLink, MSP_RAW_IMU, payload);
                MWIserialbuffer_askForFrame(serialLink, MSP_ALTITUDE, payload);
                MWIserialbuffer_askForFrame(serialLink, MSP_RC, payload);
                MWIserialbuffer_askForFrame(serialLink, MSP_MOTOR, payload);
                MWIserialbuffer_askForFrame(serialLink, MSP_SERVO, payload);
                MWIserialbuffer_askForFrame(serialLink, MSP_DEBUG, payload);
            } else {
                // we need boxnames for ARM BOX, HORIZON , ANGLE , GPS ..
                MWIserialbuffer_askForFrame(serialLink, MSP_IDENT, payload);
                MWIserialbuffer_askForFrame(serialLink, MSP_BOXNAMES, payload);
                MWIserialbuffer_askForFrame(serialLink, MSP_RC_TUNING, payload);
                MWIserialbuffer_askForFrame(serialLink, MSP_PID, payload);
            }
            //TODO others if required
            //MSP_COMP_GPS
            //MSP_BAT

            // the ground station is sending rc data
            if (mavlinkState->rcdata.toSend == OK) {
                mavlinkState->rcdata.toSend = NOK;
                payload->length = 0;
                MWIserialbuffer_Payloadwrite16(payload, mavlinkState->rcdata.y);
                MWIserialbuffer_Payloadwrite16(payload, mavlinkState->rcdata.x);
                MWIserialbuffer_Payloadwrite16(payload, mavlinkState->rcdata.r);
                MWIserialbuffer_Payloadwrite16(payload, mavlinkState->rcdata.z);
                MWIserialbuffer_Payloadwrite16(payload, 1500);
                MWIserialbuffer_Payloadwrite16(payload, 1500);
                MWIserialbuffer_Payloadwrite16(payload, 1500);
                MWIserialbuffer_Payloadwrite16(payload, 1500);

                MWIserialbuffer_askForFrame(serialLink, MSP_SET_RAW_RC, payload);
            }

        }

        // fast rate
        annex();

        // we dont use thread ..
        usleep(1);
    }
}
예제 #6
0
template<> ValueStack::T ValueStack::peek()
{
	if (!something()) { eprintf("Cannot peek in empty stack!\n"); eexit(); }
	return *(_p-1);
}
예제 #7
0
template<> CharacterStack::T CharacterStack::pop()
{
	if (!something()) { eprintf("Cannot pop from empty stack!\n"); eexit(); }
	T r = *(--_p);
	return r;
}
예제 #8
0
파일: peochk.c 프로젝트: rhaamo/sosyslog
/*
 * check: read logfile and check it
 */
void
check(void)
{
	FILE *finput;
	int   i;
	int   input;
	int   mfd;
	unsigned char  key[41];
	int   keylen;
	unsigned char  lastkey[21];
	int   lastkeylen;
	int   line;
	unsigned char  mkey1[21];
	int   mkey1len;
	unsigned char  mkey2[21];
	int   mkey2len;
	char  msg[MAXLINE];
	int   msglen;

	/* open logfile */
	if (actionf & ST_IN)
		input = STDIN_FILENO;
	else if ( (input = open(logfile, O_RDONLY, 0)) == -1) {
		perror(logfile);
		exit(-1);
	}

	mfd = 0;	/* shutup gcc */

	/* open macfile */
	if (macfile)
		if ( (mfd = open(macfile, O_RDONLY, 0)) == -1) {
			perror(macfile);
			exit(-1);
		}

	/* read initial key (as ascii string) and tranlate it to binary */
	if ( (i = open(key0file, O_RDONLY, 0)) == -1) {
		perror(key0file);
		exit(-1);
	}
	if ( (keylen = read(i, key, 40)) == -1) {
		perror(key0file);
		exit(-1);
	}
	if (!keylen) {
		if (actionf & QUIET)
			eexit(1, "1\n");
		else
			eexit(1, "(1) %s: %s\n", key0file, corrupted);
	}
	key[keylen] = 0;
	asc2bin(key, key);
	keylen >>= 1;
	close(i);

	/* read last key */
	if ( (i = open(keyfile, O_RDONLY, 0)) == -1) {
		perror(keyfile);
		exit(-1);
	}
	if ( (lastkeylen = read(i, lastkey, 20)) == -1) {
		perror(keyfile);
		exit(-1);
	}
	if (!lastkeylen) {
		if (actionf & QUIET)
			eexit(1, "1\n");
		else
			eexit(1, "(1) %s: %s\n", keyfile, corrupted);
	}
	close(i);

	/* test both key lenghts */
	if (lastkeylen != keylen) {
		if (actionf & QUIET)
			eexit(1, "1\n");
		else
			eexit(1, "(1) %s and/or %s %s\n", key0file, keyfile,
			    corrupted);
	}

	/* check it */
	line = 1;
	finput = NULL;
	while ( (msglen = readline(input, msg, MAXLINE, &finput)) > 0) {
		if (macfile) {
			if ( ((mkey1len = mac2(key, keylen,
			    (unsigned char *) msg, msglen, mkey1)) < 0) ||
			    ((mkey2len = read(mfd, mkey2,
			    mkey1len)) < 0) ) {
				perror(macfile);
				exit(-1);
			}
			if ((mkey2len != mkey1len) || memcmp(mkey2, mkey1,
			    mkey1len)) {
				if (actionf & QUIET)
					eexit(1, "%i\n", line);
				else
					eexit(1, "(%i) %s %s on line %i\n",
					    line, logfile, corrupted, line);
			}
			line++;
		}
		if ( (keylen = mac(method, key, keylen,
		    (unsigned char *) msg, msglen, key)) == -1) {
			perror("fatal");
			exit(-1);
		}
	}

	if (finput != NULL)
		fclose(finput);

	if (macfile != NULL)
		close(mfd);

	if (i < 0) {
		fprintf(stderr, "error reading logs form %s : %s\n",
		    (actionf & ST_IN) ? "standard input" : logfile,
		    strerror(errno));
		exit(-1);
	}

	if (memcmp(lastkey, key, keylen)) {
		if (actionf & QUIET) 
			eexit(1, "1\n");
		else
			eexit(1, "(1) %s %s\n", logfile, corrupted);
	}
	if (actionf & QUIET)
		eexit(0, "0\n");
	else
		eexit(0, "(0) %s file is ok\n", logfile);
}