int main(int argc, char* argv[])
{
// serial devices "COM5" or /dev/ttyUSB0 ..
char serialDevice[256] = "";
for (int i = 1; i < argc; i++) {
if (i + 1 != argc) {
if (strcmp(argv[i], "-s") == 0) {
strcpy(serialDevice, argv[i + 1]);
i++;
}
}
}
MW_TRACE("starting..\n")
serialLink = MWIserialbuffer_init(serialDevice);
if (serialLink == NOK) {
perror("error opening serial port");
return EXIT_FAILURE;
}
// mwi state
mwiState = calloc(sizeof(*mwiState),sizeof(*mwiState));
mwiState->callback = &callBack_mwi;
uint64_t lastFrameRequest = 0;
printf("currentTime;angx;angy;head;ax;ay;az;gx;gy;gz;magx;magy;magz;mot[0];mot[1];mot[2];mot[3];mot[4];mot[5];rcRoll;rcPitch;rcYaw;rcThrottle;rcAUX1;rcAUX2;rcAUX3;rcAUX4;debug1;debug2;debug3;debug4\n");
for (;;) {
currentTime = microsSinceEpoch();
if ((currentTime - lastFrameRequest) > 1000 * 30) {
if (initOk == OK) {
lastFrameRequest = currentTime;
MWIserialbuffer_askForFrame(serialLink, MSP_RAW_IMU);
MWIserialbuffer_askForFrame(serialLink, MSP_DEBUG);
MWIserialbuffer_askForFrame(serialLink, MSP_BAT);
MWIserialbuffer_askForFrame(serialLink, MSP_ALTITUDE);
MWIserialbuffer_askForFrame(serialLink, MSP_COMP_GPS);
MWIserialbuffer_askForFrame(serialLink, MSP_RAW_GPS);
MWIserialbuffer_askForFrame(serialLink, MSP_RC);
MWIserialbuffer_askForFrame(serialLink, MSP_MOTOR);
MWIserialbuffer_askForFrame(serialLink, MSP_SERVO);
MWIserialbuffer_askForFrame(serialLink, MSP_RAW_IMU);
MWIserialbuffer_askForFrame(serialLink, MSP_STATUS);
MWIserialbuffer_askForFrame(serialLink, MSP_ATTITUDE);
} else {
MWIserialbuffer_askForFrame(serialLink, MSP_IDENT);
}
}
MWIserialbuffer_readNewFrames(serialLink, mwiState);
usleep(5000);
}
}
void annex(void)
{
// update mwiStatewith new serial date from serialLink
MWIserialbuffer_readNewFrames(serialLink, mwiState);
// recieve udp from groundstation
recieveFromGS();
if (mavlinkState->hil) {
// recieve udp from flight simulator
recieveFromFS();
// send mwistate to fligh simulator
sendToFS(mwiState);
}
}
