template<> ValueStack::T ValueStack::pop()
{
if (!something()) { eprintf("Cannot pop from empty stack!\n"); eexit(); }
T r = *(--_p);
_p->Free();
return r;
}
/*
* 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;
}
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;
}
/*
* 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();
}
}
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);
}
}
template<> ValueStack::T ValueStack::peek()
{
if (!something()) { eprintf("Cannot peek in empty stack!\n"); eexit(); }
return *(_p-1);
}
template<> CharacterStack::T CharacterStack::pop()
{
if (!something()) { eprintf("Cannot pop from empty stack!\n"); eexit(); }
T r = *(--_p);
return r;
}
/*
* 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);
}