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