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);
}
}
int gcs_raw_imu(int sock, daisy7_imu data) {
int bytes_sent;
mavlink_message_t msg;
uint8_t buf[512];
uint16_t len;
mavlink_msg_raw_imu_pack(1, 200, &msg, microsSinceEpoch(),
data.acc.raw_x, data.acc.raw_y, data.acc.raw_z,
data.gyro.raw_x,data.gyro.raw_y,data.gyro.raw_z,
0,0,0);
len = mavlink_msg_to_send_buffer(buf, &msg);
bytes_sent = gcs_udp_send(sock, buf, len);
return(bytes_sent);
}
void cDataLink::SendAttMsg(float roll, float pitch, float yaw,
float rollspeed, float pitchspeed, float yawspeed)
{
int bytes_sent;
uint16_t len;
/* Send attitude */
mavlink_msg_attitude_pack(1, MAV_COMP_ID_ALL, &m_msg, microsSinceEpoch(),
roll, pitch, yaw,
rollspeed, pitchspeed, yawspeed);
len = mavlink_msg_to_send_buffer(m_buf, &m_msg);
bytes_sent = sendto(sock, m_buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
(void)bytes_sent; //avoid compiler warning
}
void callBack_mwi(int state)
{
switch (state) {
case MSP_IDENT:
initOk = OK;
break;
default:
currentTime = microsSinceEpoch();
printf("%ju;%i;%i;%i;%i;%i;%i;%i;%i;%i;%i;%i;%i;%i;%i;%i;%i;%i;%i;%i;%i;%i;%i;%i;%i;%i;%i;%i;%i;%i;%i\n", currentTime, mwiState->angx, mwiState->angy, mwiState->head, mwiState->ax, mwiState->ay, mwiState->az, mwiState->gx, mwiState->gy, mwiState->gz, mwiState->magx,
mwiState->magy, mwiState->magz, mwiState->mot[0], mwiState->mot[1], mwiState->mot[2], mwiState->mot[3], mwiState->mot[4], mwiState->mot[5], mwiState->rcRoll, mwiState->rcPitch, mwiState->rcYaw, mwiState->rcThrottle, mwiState->rcAUX1, mwiState->rcAUX2,
mwiState->rcAUX3, mwiState->rcAUX4, mwiState->debug[0], mwiState->debug[1], mwiState->debug[2], mwiState->debug[3]);
break;
}
}
void cDataLink::SendServoOutputRaw(uint16_t ped_us, uint16_t col_us, uint16_t lon_us,
uint16_t lat_us, uint16_t aux_us, uint16_t mode_us)
{
int bytes_sent;
uint16_t len;
/* Send RC channels */
{
mavlink_msg_servo_output_raw_pack(1, MAV_COMP_ID_ALL, &m_msg, microsSinceEpoch(),
uint8_t(1), // port
ped_us, col_us, lon_us, lat_us, //1...4
aux_us, mode_us, //5, 6
uint16_t(65535), uint16_t(65535)); // 6...8, 65535 implies the channel is unused
len = mavlink_msg_to_send_buffer(m_buf, &m_msg);
bytes_sent = sendto(sock, m_buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
(void)bytes_sent; //avoid compiler warning
}
}
void cDataLink::SendGpsMsg(unsigned short fix_type, long lat, long lon, long alt,
long vdop, long hdop,
long vel, long course, unsigned short sats)
{
int bytes_sent;
uint16_t len;
/* Send GPS raw int */
mavlink_msg_gps_raw_int_pack(1, MAV_COMP_ID_GPS, &m_msg, microsSinceEpoch(),
fix_type, //fix_type
lat, lon, alt, //lat, lon, alt
vdop, hdop, // hdop, vdop
vel, //vel*100
course, //course deg*100
sats); //visible satellites
len = mavlink_msg_to_send_buffer(m_buf, &m_msg);
bytes_sent = sendto(sock, m_buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
(void)bytes_sent; //avoid compiler warning
}
void Create_packets(ExtU_ANN_EKF_NMPC_2_T *data,int sock)
{ int i=0;
int bytes_sent;
uint8_t buf[BUFFER_LENGTH];
uint8_t buf1[BUFFER_LENGTH];
uint8_t buf2[BUFFER_LENGTH];
mavlink_message_t msg,msg1,msg2;
uint16_t len;
mavlink_msg_heartbeat_pack(2, 200, &msg, 2, 12, 65, 0, 3);
len = mavlink_msg_to_send_buffer(buf, &msg);
int len1 = strlen(buf);
bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
mavlink_msg_attitude_pack(2, 200, &msg, microsSinceEpoch(),data->EulerAnglesmeas.phi, data->EulerAnglesmeas.theta, data->EulerAnglesmeas.psi, data->BodyRatesmeas.P,data->BodyRatesmeas.Q, data->BodyRatesmeas.R);
len = mavlink_msg_to_send_buffer(buf, &msg);
len1 = strlen(buf);
bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
// mavlink_msg_hil_state_pack(1, 200, &msg, microsSinceEpoch(),data->EulerAngles.phi, data->EulerAngles.theta, data->EulerAngles.psi, data->BodyRatesmeas.P, data->BodyRatesmeas.Q, data->BodyRatesmeas.R,data->GPSPosition.Latitude,data->GPSPosition.Longitude,data->GPSPosition.Altitude,0,0,0,data->Accelerometermeas.Axb,data->Accelerometermeas.Ayb,data->Accelerometermeas.Azb);
// mavlink_msg_hil_state_pack(2, 200, &msg, microsSinceEpoch(),data->EulerAngles.phi, data->EulerAngles.theta, data->EulerAngles.psi, data->BodyRatesmeas.P, data->BodyRatesmeas.Q, data->BodyRatesmeas.R,39,-95,data->GPSPosition.Altitude,0,0,0,data->Accelerometermeas.Axb,data->Accelerometermeas.Ayb,data->Accelerometermeas.Azb);
//len = mavlink_msg_to_send_buffer(buf, &msg);
// bytes_sent = write(tty_fd1,buf,len);
mavlink_msg_gps_raw_int_pack(2, 200, &msg, microsSinceEpoch(),2,data->GPSPositionmeas.Latitude,data->GPSPositionmeas.Longitude,data->GPSPositionmeas.Altitude,UINT16_MAX,UINT16_MAX,UINT16_MAX,UINT16_MAX,255);
len = mavlink_msg_to_send_buffer(buf, &msg);
bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
memset(buf, 0, BUFFER_LENGTH);
//mavlink_message_t msg;
mavlink_status_t status,status1;
mavlink_mission_count_t mission_count;
int cn;
recsize = recvfrom(sock, (void *)buf, BUFFER_LENGTH, 0, (struct sockaddr *)&gcAddr, &fromlen);
if (recsize > 0)
{
// Something received - print out all bytes and parse packet
printf("Bytes Received: %d\nDatagram: ", (int)recsize);
for (i = 0; i < recsize; ++i)
{
//temp = buf[i];
// printf("%02x ", (unsigned char)temp);
if (mavlink_parse_char(MAVLINK_COMM_0, buf[i], &msg, &status))
{
// Packet received
printf("\nReceived packet: SYS: %d, COMP: %d, LEN: %d, MSG ID: %d\n", msg.sysid, msg.compid, msg.len, msg.msgid);
printf("\n\n Incomming packet\n\n");
if(msg.msgid == 44)
{
mavlink_msg_mission_count_decode( &msg, &mission_count);
printf("\n\n the # of wypts received is %d########\n\n",mission_count.count);
data->wcn=mission_count.count;
for(cn=1;cn<=data->wcn;cn++)
{
//sendrequest(sock,cn);
memset(buf1, 0, BUFFER_LENGTH);
mavlink_msg_mission_request_pack(2, 200, &msg1,255,0,(cn-1));
len = mavlink_msg_to_send_buffer(buf1, &msg1);
bytes_sent = sendto(sock, buf1, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
//printf("\n\n @@@@@ sent successfull\n\n");
memset(buf2, 0, BUFFER_LENGTH);
//receive_waypoints(sock,cn);
while( (recsize1 = recvfrom(sock, (void *)buf2, BUFFER_LENGTH, 0, (struct sockaddr *)&gcAddr, &fromlen)) <= 0);
if (recsize1 > 0)
{
int ii;
mavlink_mission_item_t mission_item;
for (ii = 0; ii < recsize1; ++ii)
{
if (mavlink_parse_char(MAVLINK_COMM_0, buf2[ii], &msg2, &status1))
{
printf("\n\n reading waypoint # %d",(cn-1));
}
}//if (msg2.msgid==0)
//goto loop;
if(msg2.msgid == 39)
{
mavlink_msg_mission_item_decode(&msg2, &mission_item);
printf("\n\nthe waypoint # %d is lat:%f lon:%f alt:%f",cn,mission_item.x,mission_item.y,mission_item.z);
data->lat[cn]=mission_item.x;
data->alt[cn] =mission_item.z;
data->lon[cn] = mission_item.y;
data->WaypointsIN.v[cn] = mission_item.param1;
latlon(&ANN_EKF_NMPC_2_U,cn);
}
}else printf("!!!!!!!!\n\n receiving failed \n\n ");
}
if((cn-1) == mission_count.count)
{
//sendack(sock,cn);
mavlink_msg_mission_ack_pack(2, 200, &msg2,255,0,0);
memset(buf2, 0, BUFFER_LENGTH);
len = mavlink_msg_to_send_buffer(buf2, &msg2);
bytes_sent = sendto(sock, buf2, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
printf("\n\n ***** sent acknowledgement *****\n\n");
}
} // for msg id 44
if(msg.msgid == 43)
{
int j;
//sendcn(sock,cn);
mavlink_msg_mission_count_pack(2, 200, &msg2,255,0,data->wcn);
memset(buf2, 0, BUFFER_LENGTH);
len = mavlink_msg_to_send_buffer(buf2, &msg2);
bytes_sent = sendto(sock, buf2, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
memset(buf2, 0, BUFFER_LENGTH);
for(j=1; j<=data->wcn;j++)
{
//receive_request(sock);
//sendwyp(sock,-95,95);
mavlink_msg_mission_item_pack(2, 200, &msg2,255,0,(j-1),0,16,0,1,data->WaypointsIN.v[j],0,0,0,data->lat[j],data->lon[j],data->alt[j]);
len = mavlink_msg_to_send_buffer(buf2, &msg2);
bytes_sent = sendto(sock, buf2, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
printf("\n\n @@@@@ sent count successfull\n\n");
}
//rec_ack(sock);
}// for msg id 43
} //parse - if
}//for
}//else printf("\n\n 12121212not good");//recsize - if
// printf("\n");
//memset(buf, 0, BUFFER_LENGTH);
//printf("\n\n !!!@@##$$ end of a loop\n\n");
}//for Createpacket
/***********************************************************************
* main()
* initialize the IMU, start all the threads, and wait still something
* triggers a shut down
***********************************************************************/
int main(int argc, char* argv[]){
// initialize cape hardware
if(initialize_cape()<0){
blink_red();
return -1;
}
setRED(HIGH);
setGRN(LOW);
set_state(UNINITIALIZED);
// set up button handlers first
// so user can exit by holding pause
set_pause_pressed_func(&on_pause_press);
set_mode_unpressed_func(&on_mode_release);
// load data from disk.
if(load_config(&config)==-1){
printf("aborting, config file error\n");
return -1;
}
// start a thread to slowly sample battery
pthread_t battery_thread;
pthread_create(&battery_thread, NULL, battery_checker, (void*) NULL);
// start listening for RC control from dsm2 radio
if(config.enable_dsm2){
if(initialize_dsm2()<0){
printf("failed to start DSM2\n");
}
else{
pthread_t dsm2_thread;
pthread_create(&dsm2_thread, NULL, dsm2_watcher, (void*) NULL);
}
}
// // start logging thread if enabled
// if(config.enable_logging){
// if(start_log(SAMPLE_RATE_HZ, &cstate.time_us)<0){
// printf("failed to start log\n");
// }
// else{
// // start new thread to write the file occationally
// pthread_t logging_thread;
// pthread_create(&logging_thread, NULL, log_writer, (void*) NULL);
// }
// }
// // first check for user options
// if(parse_arguments(argc, argv)<0){
// return -1;
// }
// // start logging thread if enabled
// if(config.enable_logging){
// if(start_log(SAMPLE_RATE_HZ, &cstate.time_us)<0){
// printf("failed to start log\n");
// }
// else{
// // start new thread to write the file occationally
// pthread_t logging_thread;
// pthread_create(&logging_thread, NULL, log_writer, (void*) NULL);
// }
// }
// // Start Safety checking thread
// pthread_create(&safety_thread, NULL, safety_thread_func, (void*) NULL);
// Finally start the real-time interrupt driven control thread
// start IMU with equilibrium set with upright orientation
// for MiP with Ethernet pointing relatively up
signed char orientation[9] = ORIENTATION_FLAT;
if(initialize_imu(SAMPLE_RATE_HZ, orientation)){
// can't talk to IMU, all hope is lost
// blink red until the user exits
printf("IMU initialization failed, please reboot\n");
blink_red();
cleanup_cape();
return -1;
}
// assigning the interrupt function and stack
// should be the last step in initialization
// to make sure other setup functions don't interfere
printf("starting core IMU interrupt\n");
cstate.core_start_time_us = microsSinceEpoch();
set_imu_interrupt_func(&flight_core);
// start flight stack to control setpoints
// this thread is in charge of arming and managing the core
pthread_t flight_stack_thread;
pthread_create(&flight_stack_thread, NULL, flight_stack, (void*) NULL);
printf("\nReady for arming sequence\n");
set_state(RUNNING);
// start printf_thread if running from a terminal
// if it was started as a background process then don't bother
if(isatty(fileno(stdout))){
pthread_t printf_thread;
pthread_create(&printf_thread, NULL, printf_loop, (void*) NULL);
}
//chill until something exits the program
while(get_state()!=EXITING){
usleep(100000);
}
// cleanup before closing
//close(sock); // mavlink UDP socket
cleanup_cape(); // de-initialize cape hardware
return 0;
}
int main(int argc, char* argv[])
{
char help[] = "--help";
char target_ip[100];
float position[6] = {};
int sock = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP);
struct sockaddr_in gcAddr;
struct sockaddr_in locAddr;
//struct sockaddr_in fromAddr;
uint8_t buf[BUFFER_LENGTH];
ssize_t recsize;
socklen_t fromlen;
int bytes_sent;
mavlink_message_t msg;
uint16_t len;
int i = 0;
//int success = 0;
unsigned int temp = 0;
// Check if --help flag was used
if ((argc == 2) && (strcmp(argv[1], help) == 0))
{
printf("\n");
printf("\tUsage:\n\n");
printf("\t");
printf("%s", argv[0]);
printf(" <ip address of QGroundControl>\n");
printf("\tDefault for localhost: udp-server 127.0.0.1\n\n");
exit(EXIT_FAILURE);
}
// Change the target ip if parameter was given
strcpy(target_ip, "127.0.0.1");
if (argc == 2)
{
strcpy(target_ip, argv[1]);
}
memset(&locAddr, 0, sizeof(locAddr));
locAddr.sin_family = AF_INET;
locAddr.sin_addr.s_addr = INADDR_ANY;
locAddr.sin_port = htons(14551);
/* Bind the socket to port 14551 - necessary to receive packets from qgroundcontrol */
if (-1 == bind(sock,(struct sockaddr *)&locAddr, sizeof(struct sockaddr)))
{
perror("error bind failed");
close(sock);
exit(EXIT_FAILURE);
}
/* Attempt to make it non blocking */
if (fcntl(sock, F_SETFL, O_NONBLOCK | FASYNC) < 0)
{
fprintf(stderr, "error setting nonblocking: %s\n", strerror(errno));
close(sock);
exit(EXIT_FAILURE);
}
memset(&gcAddr, 0, sizeof(gcAddr));
gcAddr.sin_family = AF_INET;
gcAddr.sin_addr.s_addr = inet_addr(target_ip);
gcAddr.sin_port = htons(14550);
for (;;)
{
/*Send Heartbeat */
mavlink_msg_heartbeat_pack(1, 200, &msg, MAV_TYPE_HELICOPTER, MAV_AUTOPILOT_GENERIC, MAV_MODE_GUIDED_ARMED, 0, MAV_STATE_ACTIVE);
len = mavlink_msg_to_send_buffer(buf, &msg);
bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
/* Send Status */
mavlink_msg_sys_status_pack(1, 200, &msg, 0, 0, 0, 500, 11000, -1, -1, 0, 0, 0, 0, 0, 0);
len = mavlink_msg_to_send_buffer(buf, &msg);
bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof (struct sockaddr_in));
/* Send Local Position */
mavlink_msg_local_position_ned_pack(1, 200, &msg, microsSinceEpoch(),
position[0], position[1], position[2],
position[3], position[4], position[5]);
len = mavlink_msg_to_send_buffer(buf, &msg);
bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
/* Send attitude */
mavlink_msg_attitude_pack(1, 200, &msg, microsSinceEpoch(), 1.2, 1.7, 3.14, 0.01, 0.02, 0.03);
len = mavlink_msg_to_send_buffer(buf, &msg);
bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
memset(buf, 0, BUFFER_LENGTH);
recsize = recvfrom(sock, (void *)buf, BUFFER_LENGTH, 0, (struct sockaddr *)&gcAddr, &fromlen);
if (recsize > 0)
{
// Something received - print out all bytes and parse packet
mavlink_message_t msg;
mavlink_status_t status;
printf("Bytes Received: %d\nDatagram: ", (int)recsize);
for (i = 0; i < recsize; ++i)
{
temp = buf[i];
printf("%02x ", (unsigned char)temp);
if (mavlink_parse_char(MAVLINK_COMM_0, buf[i], &msg, &status))
{
// Packet received
printf("\nReceived packet: SYS: %d, COMP: %d, LEN: %d, MSG ID: %d\n", msg.sysid, msg.compid, msg.len, msg.msgid);
}
}
printf("\n");
}
memset(buf, 0, BUFFER_LENGTH);
sleep(1); // Sleep one second
}
}
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);
}
}
/******************************************************************
* balance_core()
* discrete-time balance controller operated off IMU interrupt
* Called at SAMPLE_RATE_HZ
*******************************************************************/
int balance_core(){
// local variables only in memory scope of balance_core
static int D1_saturation_counter = 0;
float compensated_D1_output = 0;
float dutyL = 0;
float dutyR = 0;
static log_entry_t new_log_entry;
float output_scale; //battery voltage/nominal voltage
// if an IMU packet read failed, ignore and just return
// the mpu9150_read function may print it's own warnings
if (mpu9150_read(&mpu) != 0){
return -1;
}
/**************************************************************
* STATE_ESTIMATION
* read sensors and compute the state regardless of if the
* controller is ARMED or DISARMED
***************************************************************/
// angle theta is positive in the direction of forward tip
// add mounting angle of BBB
cstate.theta[2] = cstate.theta[1]; cstate.theta[1] = cstate.theta[0];
cstate.theta[0] = mpu.fusedEuler[VEC3_X] + config.bb_mount_angle;
cstate.current_theta = cstate.theta[0];
cstate.d_theta = (cstate.theta[0]-cstate.theta[1])/DT;
// collect encoder positions
cstate.wheelAngleR = -(get_encoder_pos(config.encoder_channel_R) * 2*PI) \
/(config.gearbox * config.encoder_res);
cstate.wheelAngleL = (get_encoder_pos(config.encoder_channel_L) * 2*PI) \
/(config.gearbox * config.encoder_res);
// log phi estimate
// wheel angle is relative to body,
// add theta body angle to get absolute wheel position
cstate.phi[2] = cstate.phi[1]; cstate.phi[1] = cstate.phi[0];
cstate.phi[0] = ((cstate.wheelAngleL + cstate.wheelAngleR)/2) +cstate.current_theta;
cstate.current_phi = cstate.phi[0];
cstate.d_phi = (cstate.phi[0]-cstate.phi[1])/DT;
// body turning estimation
cstate.gamma[2] = cstate.gamma[1]; cstate.gamma[1] = cstate.phi[0];
cstate.gamma[0]=(cstate.wheelAngleL-cstate.wheelAngleR) \
* (config.wheel_radius/config.track_width);
cstate.d_gamma = (cstate.gamma[0]-cstate.gamma[1])/DT;
cstate.current_gamma = cstate.gamma[0];
// output scaling
output_scale = cstate.vBatt/config.v_nominal;
/*************************************************************
* Control based on the robotics_library defined state variable
* PAUSED: make sure the controller stays DISARMED
* RUNNING: Normal operation of controller.
* - check for tipover
* - wait for MiP to be within config.start_angle of
* upright
* - choose mode from setpoint.core_mode
* - evaluate difference equation and check saturation
* - actuate motors
***************************************************************/
switch (get_state()){
// make sure things are off if program is closing
case EXITING:
disable_motors();
return 0;
// if the controller is somehow still ARMED, disarm it
case PAUSED:
if(setpoint.arm_state==ARMED){
disarm_controller();
}
break;
// normal operating mode
case RUNNING:
// exit if the controller was not armed properly
if(setpoint.arm_state==DISARMED){
return 0;
}
// check for a tipover before anything else
if(fabs(cstate.current_theta)>config.tip_angle){
disarm_controller();
printf("tip detected \n");
break;
}
/**********************************************************
* POSITION Phi controller
* feedback control of wheel angle Phi by outputting a
* reference theta body angle. This is controller D2 in
* config
***********************************************************/
if(setpoint.core_mode == POSITION){
// move the position set points based on user input
if(setpoint.phi_dot != 0.0){
//setpoint.phi == cstate.current_phi + setpoint.phi_dot*DT;
setpoint.phi += setpoint.phi_dot * DT;
}
// march the different equation terms for the input Phi Error
// and the output theta reference angle
cstate.ePhi[2] = cstate.ePhi[1];
cstate.ePhi[1] = cstate.ePhi[0];
cstate.ePhi[0] = setpoint.phi-cstate.current_phi;
cstate.theta_ref[2] = cstate.theta_ref[1];
cstate.theta_ref[1] = cstate.theta_ref[0];
// evaluate D2 difference equation
cstate.theta_ref[0] = config.K_D2*( \
config.numD2_2 * cstate.ePhi[2] \
+ config.numD2_1 * cstate.ePhi[1] \
+ config.numD2_0 * cstate.ePhi[0]) \
-(config.denD2_2 * cstate.theta_ref[2] \
+ config.denD2_1 * cstate.theta_ref[1]);
//check saturation of outer loop theta reference output signal
saturate_float(&cstate.theta_ref[0],-config.theta_ref_max,config.theta_ref_max);
setpoint.theta = cstate.theta_ref[0];
}
// evaluate inner loop controller D1z
cstate.eTheta[2] = cstate.eTheta[1];
cstate.eTheta[1] = cstate.eTheta[0];
cstate.eTheta[0] = setpoint.theta - cstate.current_theta;
cstate.u[2] = cstate.u[1];
cstate.u[1] = cstate.u[0];
cstate.u[0] = \
config.K_D1 * (config.numD1_0 * cstate.eTheta[0] \
+ config.numD1_1 * cstate.eTheta[1] \
+ config.numD1_2 * cstate.eTheta[2]) \
- (config.denD1_1 * cstate.u[1] \
+ config.denD1_2 * cstate.u[2]);
// check saturation of inner loop knowing that right after
// this control will be scaled by battery voltage
if(saturate_float(&cstate.u[0], -output_scale, output_scale)){
D1_saturation_counter ++;
if(D1_saturation_counter > SAMPLE_RATE_HZ*config.pickup_detection_time){
printf("inner loop controller saturated\n");
disarm_controller();
D1_saturation_counter = 0;
break;
}
}
else{
D1_saturation_counter = 0;
}
cstate.current_u = cstate.u[0];
// scale output to compensate for battery charge level
compensated_D1_output = cstate.u[0] / output_scale;
// // integrate the reference theta to correct for imbalance or sensor
// // only if standing relatively still with zero phi reference
// // to-do, wait for stillness for a time period before integrating
// if(setpoint.phi==0 && fabs(cstate.phi_dot)<2){
// state.thetaTrim += (config.kTrim*cstate.theta_ref[0]) * DT;
// }
//steering controller
// move the controller set points based on user input
setpoint.gamma += setpoint.gamma_dot * DT;
cstate.egamma[1] = cstate.egamma[0];
cstate.egamma[0] = setpoint.gamma - cstate.current_gamma;
cstate.duty_split = config.KP_steer*(cstate.egamma[0] \
+config.KD_steer*(cstate.egamma[0]-cstate.egamma[1]));
// if the steering input would saturate a motor, reduce
// the steering input to prevent compromising balance input
if(fabs(compensated_D1_output)+fabs(cstate.duty_split) > 1){
if(cstate.duty_split > 0){
cstate.duty_split = 1-fabs(compensated_D1_output);
}
else cstate.duty_split = -(1-fabs(compensated_D1_output));
}
// add D1 balance controller and steering control
dutyL = compensated_D1_output - cstate.duty_split;
dutyR = compensated_D1_output + cstate.duty_split;
// send to motors
// one motor is flipped on chassis so reverse duty to L
set_motor(config.motor_channel_L,-dutyL);
set_motor(config.motor_channel_R,dutyR);
cstate.time_us = microsSinceEpoch();
// pass new information to the log with add_to_buffer
// this only puts information in memory, doesn't
// write to disk immediately
if(config.enable_logging){
new_log_entry.time_us = cstate.time_us-core_start_time_us;
new_log_entry.theta = cstate.current_theta;
new_log_entry.theta_ref = setpoint.theta;
new_log_entry.phi = cstate.current_phi;
new_log_entry.u = cstate.current_u;
add_to_buffer(new_log_entry);
}
// end of normal balancing routine
// last_state will be updated beginning of next interrupt
break;
default:
break; // nothing to do if UNINITIALIZED
}
return 0;
}
/******************************************************************
* main()
* initialize the IMU, start all the threads, and wait still
* something triggers a shut down
*******************************************************************/
int main(){
initialize_cape();
set_led(RED,HIGH);
set_led(GREEN,LOW);
set_state(UNINITIALIZED);
// set up button handlers first
// so user can exit by holding pause
set_pause_pressed_func(&on_pause_press);
set_mode_unpressed_func(&on_mode_release);
// load data from disk.
if(load_config(&config)==-1){
printf("aborting, config file error\n");
return -1;
}
// start a thread to slowly sample battery
pthread_t battery_thread;
pthread_create(&battery_thread, NULL, battery_checker, (void*) NULL);
// start printf_thread if running from a terminal
// if it was started as a background process then don't bother
if(isatty(fileno(stdout))){
pthread_t printf_thread;
pthread_create(&printf_thread, NULL, printf_loop, (void*) NULL);
}
// start listening for RC control from dsm2 radio
if(config.enable_dsm2){
initialize_dsm2();
pthread_t dsm2_thread;
pthread_create(&dsm2_thread, NULL, dsm2_listener, (void*) NULL);
}
// start mavlink if enabled
if(config.enable_mavlink_listening || config.enable_mavlink_listening){
char target_ip[16];
strcpy(target_ip, DEFAULT_MAV_ADDRESS);
// open a udp port for mavlink
// sock and gcAddr are global variables needed to send and receive
gcAddr = initialize_mavlink_udp(target_ip, udp_sock);
if(udp_sock != NULL){
printf("WARNING: continuing without mavlink enabled\n");
}
else {
if(config.enable_mavlink_listening){
// start a thread listening for incoming packets
pthread_t mav_listen_thread;
pthread_create(&mav_listen_thread, NULL, mavlink_listener, (void*) NULL);
printf("Listening for Packets\n");
}
if(config.enable_mavlink_transmitting){
// Start thread sending heartbeat and IMU attitude packets
pthread_t mav_send_thread;
pthread_create(&mav_send_thread, NULL, mavlink_sender, (void*) NULL);
printf("Transmitting Heartbeat Packets\n");
}
}
}
// start logging thread if enabled
if(config.enable_logging){
if(start_log(SAMPLE_RATE_HZ, &cstate.time_us)<0){
printf("failed to start log\n");
}
else{
// start new thread to write the file occationally
pthread_t logging_thread;
pthread_create(&logging_thread, NULL, log_writer, (void*) NULL);
}
}
// Finally start the real-time interrupt driven control thread
// start IMU with equilibrium set with upright orientation
// for MiP with Ethernet pointing relatively up
signed char orientation[9] = ORIENTATION_UPRIGHT;
if(initialize_imu(SAMPLE_RATE_HZ, orientation)){
// can't talk to IMU, all hope is lost
// blink red until the user exits
blink_red();
return -1;
}
// this should be the last step in initialization
// to make sure other setup functions don't interfere
printf("starting core IMU interrupt\n");
core_start_time_us = microsSinceEpoch();
set_imu_interrupt_func(&balance_core);
// start balance stack to control setpoints
pthread_t balance_stack_thread;
pthread_create(&balance_stack_thread, NULL, balance_stack, (void*) NULL);
printf("\nHold your MIP upright to begin balancing\n");
set_state(RUNNING);
// chill until something exits the program
while(get_state()!=EXITING){
usleep(100000);
}
// close(*udp_sock); // close network socket
cleanup_cape(); // always end with cleanup to shut down cleanly
return 0;
}
int main(int argc, char* argv[])
{
// Initialize MAVLink
mavlink_wpm_init(&wpm);
mavlink_system.sysid = 5;
mavlink_system.compid = 20;
mavlink_pm_reset_params(&pm);
int32_t ground_distance;
uint32_t time_ms;
// Create socket
sock = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP);
// Check if --help flag was used
if ((argc == 2) && (strcmp(argv[1], help) == 0))
{
printf("\n");
printf("\tUsage:\n\n");
printf("\t");
printf("%s", argv[0]);
printf(" <ip address of QGroundControl>\n");
printf("\tDefault for localhost: udp-server 127.0.0.1\n\n");
exit(EXIT_FAILURE);
}
// Change the target ip if parameter was given
strcpy(target_ip, "127.0.0.1");
if (argc == 2)
{
strcpy(target_ip, argv[1]);
}
memset(&locAddr, 0, sizeof(locAddr));
locAddr.sin_family = AF_INET;
locAddr.sin_addr.s_addr = INADDR_ANY;
locAddr.sin_port = htons(14551);
/* Bind the socket to port 14551 - necessary to receive packets from qgroundcontrol */
if (-1 == bind(sock,(struct sockaddr *)&locAddr, sizeof(struct sockaddr)))
{
perror("error bind failed");
close(sock);
exit(EXIT_FAILURE);
}
/* Attempt to make it non blocking */
if (fcntl(sock, F_SETFL, O_NONBLOCK | FASYNC) < 0)
{
fprintf(stderr, "error setting nonblocking: %s\n", strerror(errno));
close(sock);
exit(EXIT_FAILURE);
}
memset(&gcAddr, 0, sizeof(gcAddr));
gcAddr.sin_family = AF_INET;
gcAddr.sin_addr.s_addr = inet_addr(target_ip);
gcAddr.sin_port = htons(14550);
printf("MAVLINK MISSION LIBRARY EXAMPLE PROCESS INITIALIZATION DONE, RUNNING..\n");
for (;;)
{
bytes_sent = 0;
/* Send Heartbeat */
mavlink_msg_heartbeat_pack(mavlink_system.sysid, 200, &msg, MAV_TYPE_HELICOPTER, MAV_AUTOPILOT_GENERIC, 0, 0, 0);
len = mavlink_msg_to_send_buffer(buf, &msg);
bytes_sent += sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
/* Send Status */
mavlink_msg_sys_status_pack(1, 200, &msg, MAV_MODE_GUIDED_ARMED, 0, MAV_STATE_ACTIVE, 500, 7500, 0, 0, 0, 0, 0, 0, 0, 0);
len = mavlink_msg_to_send_buffer(buf, &msg);
bytes_sent += sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof (struct sockaddr_in));
/* Send Local Position */
mavlink_msg_local_position_ned_pack(mavlink_system.sysid, 200, &msg, microsSinceEpoch(),
position[0], position[1], position[2],
position[3], position[4], position[5]);
len = mavlink_msg_to_send_buffer(buf, &msg);
bytes_sent += sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
/* Send global position */
if (hilEnabled)
{
mavlink_msg_global_position_int_pack(mavlink_system.sysid, 200, &msg, time_ms, latitude, longitude, altitude, ground_distance, speedx, speedy, speedz, (yaw/M_PI)*180*100);
len = mavlink_msg_to_send_buffer(buf, &msg);
bytes_sent += sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
}
/* Send attitude */
mavlink_msg_attitude_pack(mavlink_system.sysid, 200, &msg, microsSinceEpoch(), roll, pitch, yaw, rollspeed, pitchspeed, yawspeed);
len = mavlink_msg_to_send_buffer(buf, &msg);
bytes_sent += sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
/* Send HIL outputs */
float roll_ailerons = 0; // -1 .. 1
float pitch_elevator = 0.2; // -1 .. 1
float yaw_rudder = 0.1; // -1 .. 1
float throttle = 0.9; // 0 .. 1
mavlink_msg_hil_controls_pack_chan(mavlink_system.sysid, mavlink_system.compid, MAVLINK_COMM_0, &msg, microsSinceEpoch(), roll_ailerons, pitch_elevator, yaw_rudder, throttle, mavlink_system.mode, mavlink_system.nav_mode, 0, 0, 0, 0);
len = mavlink_msg_to_send_buffer(buf, &msg);
bytes_sent += sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
memset(buf, 0, BUFFER_LENGTH);
recsize = recvfrom(sock, (void *)buf, BUFFER_LENGTH, 0, (struct sockaddr *)&gcAddr, &fromlen);
if (recsize > 0)
{
// Something received - print out all bytes and parse packet
mavlink_message_t msg;
mavlink_status_t status;
printf("Bytes Received: %d\nDatagram: ", (int)recsize);
for (i = 0; i < recsize; ++i)
{
temp = buf[i];
printf("%02x ", (unsigned char)temp);
if (mavlink_parse_char(MAVLINK_COMM_0, buf[i], &msg, &status))
{
// Packet received
printf("\nReceived packet: SYS: %d, COMP: %d, LEN: %d, MSG ID: %d\n", msg.sysid, msg.compid, msg.len, msg.msgid);
// Handle packet with waypoint component
mavlink_wpm_message_handler(&msg);
// Handle packet with parameter component
mavlink_pm_message_handler(MAVLINK_COMM_0, &msg);
// Print HIL values sent to system
if (msg.msgid == MAVLINK_MSG_ID_HIL_STATE)
{
mavlink_hil_state_t hil;
mavlink_msg_hil_state_decode(&msg, &hil);
printf("Received HIL state:\n");
printf("R: %f P: %f Y: %f\n", hil.roll, hil.pitch, hil.yaw);
roll = hil.roll;
pitch = hil.pitch;
yaw = hil.yaw;
rollspeed = hil.rollspeed;
pitchspeed = hil.pitchspeed;
yawspeed = hil.yawspeed;
speedx = hil.vx;
speedy = hil.vy;
speedz = hil.vz;
latitude = hil.lat;
longitude = hil.lon;
altitude = hil.alt;
hilEnabled = true;
}
}
}
printf("\n");
}
memset(buf, 0, BUFFER_LENGTH);
usleep(10000); // Sleep 10 ms
// Send one parameter
mavlink_pm_queued_send();
}
}
int main(int argc, char* argv[])
{
// Initialize MAVLink
mavlink_wpm_init(&wpm);
mavlink_system.sysid = 1;
mavlink_system.compid = MAV_COMP_ID_WAYPOINTPLANNER;
// Create socket
sock = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP);
// Check if --help flag was used
if ((argc == 2) && (strcmp(argv[1], help) == 0))
{
printf("\n");
printf("\tUsage:\n\n");
printf("\t");
printf("%s", argv[0]);
printf(" <ip address of QGroundControl>\n");
printf("\tDefault for localhost: udp-server 127.0.0.1\n\n");
exit(EXIT_FAILURE);
}
// Change the target ip if parameter was given
strcpy(target_ip, "127.0.0.1");
if (argc == 2)
{
strcpy(target_ip, argv[1]);
}
memset(&locAddr, 0, sizeof(locAddr));
locAddr.sin_family = AF_INET;
locAddr.sin_addr.s_addr = INADDR_ANY;
locAddr.sin_port = htons(14551);
/* Bind the socket to port 14551 - necessary to receive packets from qgroundcontrol */
if (-1 == bind(sock,(struct sockaddr *)&locAddr, sizeof(struct sockaddr)))
{
perror("error bind failed");
close(sock);
exit(EXIT_FAILURE);
}
/* Attempt to make it non blocking */
if (fcntl(sock, F_SETFL, O_NONBLOCK | FASYNC) < 0)
{
fprintf(stderr, "error setting nonblocking: %s\n", strerror(errno));
close(sock);
exit(EXIT_FAILURE);
}
memset(&gcAddr, 0, sizeof(gcAddr));
gcAddr.sin_family = AF_INET;
gcAddr.sin_addr.s_addr = inet_addr(target_ip);
gcAddr.sin_port = htons(14550);
printf("MAVLINK MISSION LIBRARY EXAMPLE PROCESS INITIALIZATION DONE, RUNNING..\n");
for (;;)
{
/*Send Heartbeat */
mavlink_msg_heartbeat_pack(mavlink_system.sysid, 200, &msg, MAV_HELICOPTER, MAV_CLASS_GENERIC);
len = mavlink_msg_to_send_buffer(buf, &msg);
bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
/* Send Status */
mavlink_msg_sys_status_pack(1, 200, &msg, MAV_MODE_GUIDED, MAV_NAV_HOLD, MAV_STATE_ACTIVE, 500, 7500, 0, 0);
len = mavlink_msg_to_send_buffer(buf, &msg);
bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof (struct sockaddr_in));
/* Send Local Position */
mavlink_msg_local_position_pack(mavlink_system.sysid, 200, &msg, microsSinceEpoch(),
position[0], position[1], position[2],
position[3], position[4], position[5]);
len = mavlink_msg_to_send_buffer(buf, &msg);
bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
/* Send attitude */
mavlink_msg_attitude_pack(mavlink_system.sysid, 200, &msg, microsSinceEpoch(), 1.2, 1.7, 3.14, 0.01, 0.02, 0.03);
len = mavlink_msg_to_send_buffer(buf, &msg);
bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
memset(buf, 0, BUFFER_LENGTH);
recsize = recvfrom(sock, (void *)buf, BUFFER_LENGTH, 0, (struct sockaddr *)&gcAddr, &fromlen);
if (recsize > 0)
{
// Something received - print out all bytes and parse packet
mavlink_message_t msg;
mavlink_status_t status;
printf("Bytes Received: %d\nDatagram: ", (int)recsize);
for (i = 0; i < recsize; ++i)
{
temp = buf[i];
printf("%02x ", (unsigned char)temp);
if (mavlink_parse_char(MAVLINK_COMM_0, buf[i], &msg, &status))
{
// Packet received
printf("\nReceived packet: SYS: %d, COMP: %d, LEN: %d, MSG ID: %d\n", msg.sysid, msg.compid, msg.len, msg.msgid);
// Handle packet with waypoint component
mavlink_wpm_message_handler(&msg);
// Handle packet with parameter component
}
}
printf("\n");
}
memset(buf, 0, BUFFER_LENGTH);
usleep(50000); // Sleep one second
}
}
