pinxian wu / Mbed 2 deprecated mavlink

mavlink heart beat test

Dependencies:   mavlink_bridge mbed

main.cpp@1:2d06621b9863, 2018-07-23 (annotated)

Committer:
wupinxian
Date:
Mon Jul 23 16:36:47 2018 +0000
Revision:
1:2d06621b9863
Parent:
0:cf75b2b10606 
mavlink test for stm32 nucleo board

Who changed what in which revision?

UserRevisionLine numberNew contents of line
wupinxian 0:cf75b2b10606 1 #include "mbed.h"
wupinxian 0:cf75b2b10606 2 #include "mavlink.h"
wupinxian 0:cf75b2b10606 3
wupinxian 0:cf75b2b10606 4 char buf[300];
wupinxian 0:cf75b2b10606 5 DigitalOut led(LED1);
wupinxian 0:cf75b2b10606 6 int sysid = 0;
wupinxian 0:cf75b2b10606 7 int compid=0;
wupinxian 0:cf75b2b10606 8 mavlink_message_t message;
wupinxian 0:cf75b2b10606 9 mavlink_heartbeat_t heartbeat;
wupinxian 0:cf75b2b10606 10
wupinxian 0:cf75b2b10606 11 Serial pc(SERIAL_TX, SERIAL_RX);
wupinxian 0:cf75b2b10606 12
wupinxian 0:cf75b2b10606 13
wupinxian 0:cf75b2b10606 14 int write_data(char * data){
wupinxian 0:cf75b2b10606 15
wupinxian 0:cf75b2b10606 16
wupinxian 0:cf75b2b10606 17
wupinxian 0:cf75b2b10606 18 }
wupinxian 0:cf75b2b10606 19
wupinxian 0:cf75b2b10606 20
wupinxian 0:cf75b2b10606 21
wupinxian 0:cf75b2b10606 22 void write_data( Serial& serial ,char* buf ,unsigned int len ){
wupinxian 0:cf75b2b10606 23
wupinxian 0:cf75b2b10606 24 for(int i =0 ; i<len;i++){
wupinxian 0:cf75b2b10606 25 serial.putc(buf[i]);
wupinxian 0:cf75b2b10606 26 }
wupinxian 0:cf75b2b10606 27 }
wupinxian 0:cf75b2b10606 28 int light = 0;
wupinxian 0:cf75b2b10606 29 unsigned len = 0;
wupinxian 0:cf75b2b10606 30 int main()
wupinxian 0:cf75b2b10606 31 {
wupinxian 0:cf75b2b10606 32 pc.baud(921600);
wupinxian 0:cf75b2b10606 33 heartbeat.custom_mode = 555;
wupinxian 0:cf75b2b10606 34 heartbeat.type = 2;
wupinxian 0:cf75b2b10606 35 heartbeat.autopilot = 12;
wupinxian 0:cf75b2b10606 36 heartbeat.base_mode = 43;
wupinxian 0:cf75b2b10606 37 heartbeat.system_status =32;
wupinxian 0:cf75b2b10606 38 heartbeat.mavlink_version = 3;
wupinxian 0:cf75b2b10606 39
wupinxian 0:cf75b2b10606 40 int j=0;
wupinxian 0:cf75b2b10606 41
wupinxian 0:cf75b2b10606 42 while(1){
wupinxian 0:cf75b2b10606 43
wupinxian 0:cf75b2b10606 44 mavlink_msg_heartbeat_encode(3,255 ,&message ,&heartbeat );
wupinxian 0:cf75b2b10606 45 len = mavlink_msg_to_send_buffer((uint8_t*)buf, &message);
wupinxian 0:cf75b2b10606 46 write_data(pc, buf , len);
wupinxian 0:cf75b2b10606 47
wupinxian 0:cf75b2b10606 48 mavlink_msg_sys_status_pack(3, 255, &message, 0, 0, 0, 500, 11000, -1, -1, 0, 0, 0, 0, 0, 0);
wupinxian 0:cf75b2b10606 49 len = mavlink_msg_to_send_buffer((uint8_t*)buf, &message);
wupinxian 0:cf75b2b10606 50 write_data(pc, buf , len);
wupinxian 0:cf75b2b10606 51
wupinxian 0:cf75b2b10606 52 mavlink_msg_local_position_ned_pack(3, 255, &message, j,
wupinxian 0:cf75b2b10606 53 0, 1, 2,
wupinxian 0:cf75b2b10606 54 3, 4, 5);
wupinxian 0:cf75b2b10606 55 len = mavlink_msg_to_send_buffer((uint8_t*)buf, &message);
wupinxian 0:cf75b2b10606 56 write_data(pc, buf , len);
wupinxian 0:cf75b2b10606 57
wupinxian 0:cf75b2b10606 58
wupinxian 0:cf75b2b10606 59 mavlink_msg_attitude_pack(3, 255, &message, j, 1.2, 1.7, 3.14, 0.01, 0.02, 0.03);
wupinxian 0:cf75b2b10606 60 len = mavlink_msg_to_send_buffer((uint8_t*)buf, &message);
wupinxian 0:cf75b2b10606 61
wupinxian 0:cf75b2b10606 62 write_data(pc, buf , len);
wupinxian 0:cf75b2b10606 63
wupinxian 0:cf75b2b10606 64
wupinxian 0:cf75b2b10606 65
wupinxian 0:cf75b2b10606 66 if(led.read()){
wupinxian 0:cf75b2b10606 67 led.write(0);
wupinxian 0:cf75b2b10606 68 }else{
wupinxian 0:cf75b2b10606 69 led.write(1);
wupinxian 0:cf75b2b10606 70 }
wupinxian 0:cf75b2b10606 71 wait(1);
wupinxian 0:cf75b2b10606 72 j+=1;
wupinxian 0:cf75b2b10606 73 }
wupinxian 0:cf75b2b10606 74 }