mavlink heart beat test

Dependencies:   mavlink_bridge mbed

main.cpp

Committer:
wupinxian
Date:
2018-07-23
Revision:
0:cf75b2b10606

File content as of revision 0:cf75b2b10606:

#include "mbed.h"
#include "mavlink.h"

char buf[300];
DigitalOut led(LED1);
int sysid = 0;
int compid=0;
mavlink_message_t message;
mavlink_heartbeat_t heartbeat;

Serial pc(SERIAL_TX, SERIAL_RX);


int write_data(char * data){


    
}



void write_data( Serial& serial ,char* buf ,unsigned int len  ){

for(int i =0 ; i<len;i++){
    serial.putc(buf[i]);
}
}
int light = 0;
unsigned len = 0;
int main()
{
     pc.baud(921600);
    heartbeat.custom_mode = 555;
    heartbeat.type = 2;
    heartbeat.autopilot = 12;
    heartbeat.base_mode = 43;
    heartbeat.system_status =32;
    heartbeat.mavlink_version = 3;
  
int j=0;

  while(1){
    
    mavlink_msg_heartbeat_encode(3,255 ,&message ,&heartbeat  );
    len = mavlink_msg_to_send_buffer((uint8_t*)buf, &message);
    write_data(pc, buf , len);
    
    mavlink_msg_sys_status_pack(3, 255, &message, 0, 0, 0, 500, 11000, -1, -1, 0, 0, 0, 0, 0, 0);
    len = mavlink_msg_to_send_buffer((uint8_t*)buf, &message);
     write_data(pc, buf , len);
    
    mavlink_msg_local_position_ned_pack(3, 255, &message, j, 
                                    0, 1, 2,
                                    3, 4, 5);
    len = mavlink_msg_to_send_buffer((uint8_t*)buf, &message);
    write_data(pc, buf , len);


    mavlink_msg_attitude_pack(3, 255, &message, j, 1.2, 1.7, 3.14, 0.01, 0.02, 0.03);
    len = mavlink_msg_to_send_buffer((uint8_t*)buf, &message);
    
    write_data(pc, buf , len);
    
    
   
    if(led.read()){
    led.write(0);    
    }else{
    led.write(1);
        }
    wait(1);  
      j+=1;
    }  
}