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); } }
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); } }