PX4 visual update sample

#include <stdio.h>
#include <errno.h>
#include <string.h>
#include <sys/socket.h>
#include <sys/types.h>
#include <netinet/in.h>
#include <unistd.h>
#include <stdlib.h>
#include <fcntl.h>
#include <time.h>
#if (defined __QNX__) | (defined __QNXNTO__)
/* QNX specific headers */
#include <unix.h>
#else
/* Linux / MacOS POSIX timer headers */
#include <sys/time.h>
#include <time.h>
#include <arpa/inet.h>
#include <stdbool.h> /* required for the definition of bool in C99 */
#endif

/* This assumes you have the mavlink headers on your include path
 or in the same folder as this source file */
#include <mavlink.h>


#define BUFFER_LENGTH 2041 // minimum buffer size that can be used with qnx (I don't know why)

uint64_t microsSinceEpoch();

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;
    bool airborne = false;

    // 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(14540);

    /* 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 (defined __QNX__) | (defined __QNXNTO__)
    if (fcntl(sock, F_SETFL, O_NONBLOCK | FASYNC) < 0)
#else
    if (fcntl(sock, F_SETFL, O_NONBLOCK | O_ASYNC) < 0)
#endif

    {
        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(14557);

    int counter = 0;

    float reported_ned_x = 0;
    float reported_ned_y = 0;
    float reported_ned_z = 0;
    float reported_roll = 0;
    float reported_pitch = 0;
    float reported_yaw = 0;

    for (;;)
    {
        /*Send Heartbeat */
        mavlink_msg_heartbeat_pack(1, 200, &msg, MAV_TYPE_GENERIC, MAV_AUTOPILOT_INVALID, 0, 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 visual position opdate*/
        float covariance[] = { 1, 0, 0, 0, 0, 0,
                                  1, 0, 0, 0, 0,
                                     1, 0, 0, 0,
                                        1, 0, 0,
                                           1, 0,
                                              1 };

        float lat;
        float lon;
        float elev;
        float pitch;
        float roll;
        float yaw;

        lat = reported_ned_x;
        lon = reported_ned_y;
        elev = reported_ned_z;
        pitch = reported_pitch;
        roll = reported_roll;
        yaw = reported_yaw;

        printf("sending lat: %f lon: %f elev: %f yaw: %f pitch: %f roll: %f\n", lat, lon, elev, yaw, pitch, roll);

        mavlink_msg_vision_position_estimate_pack(1, MAV_COMP_ID_PERIPHERAL, &msg, microsSinceEpoch(),
                                                  lat, lon, elev, roll, pitch, yaw,
                                                  covariance);
        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)
            {
                if (mavlink_parse_char(MAVLINK_COMM_0, buf[i], &msg, &status))
                {
                    bool old_airborne = airborne;

                    switch (msg.msgid) {
                        case MAVLINK_MSG_ID_ATTITUDE:
                            reported_roll = mavlink_msg_attitude_get_roll(&msg);
                            reported_pitch = mavlink_msg_attitude_get_pitch(&msg);
                            reported_yaw = mavlink_msg_attitude_get_yaw(&msg);
                            printf("attitude, roll: %f, pitch: %f, yaw: %f, rollspeed: %f, pitchspeed: %f, yawspeed: %f\n",
                                                                            mavlink_msg_attitude_get_roll(&msg),
                                                                            mavlink_msg_attitude_get_pitch(&msg),
                                                                            mavlink_msg_attitude_get_yaw(&msg),
                                                                            mavlink_msg_attitude_get_rollspeed(&msg),
                                                                            mavlink_msg_attitude_get_pitchspeed(&msg),
                                                                            mavlink_msg_attitude_get_yawspeed(&msg));
                            break;
                        case MAVLINK_MSG_ID_LOCAL_POSITION_NED:
                            reported_ned_x = mavlink_msg_local_position_ned_get_x(&msg);
                            reported_ned_y = mavlink_msg_local_position_ned_get_y(&msg);
                            reported_ned_z = mavlink_msg_local_position_ned_get_z(&msg);
                            printf("local position NED, x: %f, y: %f, z: %f, vx: %f, vy: %f, vz: %f\n",
                                                                            mavlink_msg_local_position_ned_get_x(&msg),
                                                                            mavlink_msg_local_position_ned_get_y(&msg),
                                                                            mavlink_msg_local_position_ned_get_z(&msg),
                                                                            mavlink_msg_local_position_ned_get_vx(&msg),
                                                                            mavlink_msg_local_position_ned_get_vy(&msg),
                                                                            mavlink_msg_local_position_ned_get_vz(&msg));
                            break;
                        case MAVLINK_MSG_ID_GPS_RAW_INT:
                            printf("GPS raw int lat: %d lon: %d alt: %d\n", mavlink_msg_gps_raw_int_get_lat(&msg),
                                                                            mavlink_msg_gps_raw_int_get_lon(&msg),
                                                                            mavlink_msg_gps_raw_int_get_alt(&msg));
                            break;
                        default:
                            printf("message id: %d\n", msg.msgid);
                            break;
                    }
                }
            }
        }
        memset(buf, 0, BUFFER_LENGTH);
    }
}


/* QNX timer version */
#if (defined __QNX__) | (defined __QNXNTO__)
uint64_t microsSinceEpoch()
{

    struct timespec time;

    uint64_t micros = 0;

    clock_gettime(CLOCK_REALTIME, &time);
    micros = (uint64_t)time.tv_sec * 1000000 + time.tv_nsec/1000;

    return micros;
}
#else
uint64_t microsSinceEpoch()
{

    struct timeval tv;

    uint64_t micros = 0;

    gettimeofday(&tv, NULL);
    micros =  ((uint64_t)tv.tv_sec) * 1000000u + tv.tv_usec;

    return micros;
}
#endif