void recieveFromFS()
{
int recsize = recvfrom(sockFSin, &fsMsg, sizeof(fsMsg), 0, NULL, NULL);
if (recsize > 0) {
MW_TRACE("\n")MW_TRACE(" <-- udp in FS <--\n")
swap32(&fsMsg.ctrl);
if (fsMsg.ctrl != FS_OUTCTRL) {
if (mavlinkState->verbose) {
printf("%d\n", fsMsg.ctrl);
}
MW_TRACE(" <-- udp in FS Checksum Failed <--\n")
} else {
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 recieveFromGS(void)
{
ssize_t recsize;
socklen_t fromlen;
uint8_t udpInBuf[BUFFER_LENGTH];
memset(udpInBuf, 0, BUFFER_LENGTH);
recsize = recvfrom(sock, (void *)udpInBuf, BUFFER_LENGTH, 0, (SOCKADDR *)&locGSAddr, SOCKLEN_T_INT(&fromlen));
if (recsize > 0) {
MW_TRACE("\n")MW_TRACE(" <-- udp in GS <--\n")
mavlink_message_t msgIn;
mavlink_status_t status;
for (int i = 0; i < recsize; ++i) {
if (mavlink_parse_char(MAVLINK_COMM_0, udpInBuf[i], &msgIn, &status)) {
// Packet received
if (mavlinkState->verbose) {
printf("\nReceived packet: SYS: %d, COMP: %d, LEN: %d, MSG ID: %d\n", msgIn.sysid, msgIn.compid, msgIn.len, msgIn.msgid);
}
handleMessage(&msgIn);
}
}
}
}
