mavlink c 编码解码demo

#include "common/mavlink.h"
#include "common/mavlink_msg_local_position_ned.h"
#include "common/mavlink_msg_sys_status.h"
#define BUFFER_LENGTH                                                          \
  2041 // minimum buffer size that can be used with qnx (I don't know why)

int main(int argc, char *argv[]) {

  mavlink_system_t mavlink_system = {
      1, // System ID (1-255)
      1  // Component ID (a MAV_COMPONENT value)
  };
  char help[] = "--help";

  char target_ip[100];

  float position[6] = {};
 
  uint8_t buf[BUFFER_LENGTH];
  ssize_t recsize;
  // socklen_t fromlen = sizeof(gcAddr);
  int bytes_sent;
  mavlink_message_t msg2;

  // mavlink_message_t msg;
  mavlink_status_t status;

  uint16_t len;
  int i = 0;
  // int success = 0;
  unsigned int temp = 0;
  {

    /*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(12, 234, &msg, 0, 0, 0, 500, 11000, -1, -1,
    // 0, 0, 0, 0, 0, 0);
    // len = mavlink_msg_to_send_buffer(buf, &msg);

    mavlink_msg_local_position_ned_pack(1, 200, &msg2, 123, 2, 33, 35, 43, 52,
                                        62);
    len = mavlink_msg_to_send_buffer(buf, &msg2);

    for (i = 0; i < len; i++)
      printf("%x ", buf[i]);
    printf("\n----\n");
    mavlink_message_t msg;
    for (i = 0; i < len; i++) {
      temp = buf[i];
      printf("%02x ", (unsigned char)temp);
      if (mavlink_parse_char(12, 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);
        if (msg.msgid == 32) {
          mavlink_local_position_ned_t local_position_ned;
          mavlink_msg_local_position_ned_decode(&msg, &local_position_ned);
          printf("ned x %f, y%f, z%f\nned vx %f, vy%f, vz%f\n",
                 local_position_ned.x, local_position_ned.y,
                 local_position_ned.z, local_position_ned.vx,
                 local_position_ned.vy, local_position_ned.vz);
        }
      }
    }

  }
}