Diff: main.cpp

Revision:

4:c2de9e2123ac

Parent:

0:d4dacd8a359b

--- a/main.cpp	Tue Jul 04 15:45:36 2017 +0000
+++ b/main.cpp	Wed Aug 29 06:59:14 2018 +0000
@@ -1,51 +1,207 @@
 #include "mbed.h"
 #include "LCD_DISCO_F429ZI.h"
-
+#include "list"
+#include "iostream"
+#include "string.h"
+#include "mavlink.h"
 LCD_DISCO_F429ZI lcd;
 
+std::list<string> show_list;
+char buf[300];
+char re_buf[10];
 DigitalOut led1(LED1);
+DigitalOut led2(LED2);
+unsigned int len;
+void receive(void);
+std::string text;
+
+void show_text(){
+    lcd.Clear(LCD_COLOR_WHITE);
+    lcd.SetBackColor(LCD_COLOR_WHITE);
+    lcd.SetTextColor(LCD_COLOR_BLACK);
+    std::list<string> buffer;
+    buffer = show_list;
+for(int i=0;i<show_list.size();i++){
+     std::string bufs = buffer.front();
+     buffer.pop_front();
+     lcd.DisplayStringAt(0, LINE(i), (uint8_t *)bufs.c_str(), LEFT_MODE);
+     
+    }   
+}
+
+int sysid = 1;
+int compid=0;
+mavlink_message_t message;
+mavlink_heartbeat_t heartbeat;
+Serial pc(SERIAL_TX, SERIAL_RX);
 
-int main()
-{      
-    led1 = 1;
-  
-    BSP_LCD_SetFont(&Font20);
-    lcd.DisplayStringAt(0, LINE(1), (uint8_t *)"MBED EXAMPLE", CENTER_MODE);
-    wait(1);
-  
-    while(1)
-    {      
-      lcd.Clear(LCD_COLOR_BLUE);
-      lcd.SetBackColor(LCD_COLOR_BLUE);
-      lcd.SetTextColor(LCD_COLOR_WHITE);
-      wait(0.3);
-      lcd.DisplayStringAt(0, LINE(4), (uint8_t *)"DISCOVERY", CENTER_MODE);
-      lcd.DisplayStringAt(0, LINE(5), (uint8_t *)"STM32F429ZI", CENTER_MODE);
-      wait(1);
+void write_data( Serial& serial ,char* buf ,unsigned int len  ){
+ 
+for(int i =0 ; i<len;i++){
+    serial.putc(buf[i]);
+}
+}
+
+int j=0;
+char bufa[10];
+mavlink_att_pos_mocap_t mocap_pose;
+mavlink_message_t receive_msg;
+mavlink_status_t status;
+void receive(void){
+
+
+if(pc.readable()){
+    char c;
+    //led2 = !led2;
+     c  =  pc.getc();
+    //pc.read(&c,1);
+    
+    
+      if(mavlink_parse_char(MAVLINK_COMM_0, c, &receive_msg, &status)){
+          switch(receive_msg.msgid) {
+        case MAVLINK_MSG_ID_HEARTBEAT:  // #0: Heartbeat
+          {
+           
+          }
+          break;
 
-      lcd.Clear(LCD_COLOR_GREEN);
-      
-      lcd.SetTextColor(LCD_COLOR_BLUE);
-      lcd.FillRect(10, 20, 50, 50);
-      wait(0.1);
-      lcd.SetTextColor(LCD_COLOR_BROWN);
-      lcd.FillCircle(80, 80, 50);
-      wait(0.1);
-      lcd.SetTextColor(LCD_COLOR_YELLOW);
-      lcd.FillEllipse(150, 150, 50, 100);
-      wait(0.1);
-      lcd.SetTextColor(LCD_COLOR_RED);
-      lcd.FillCircle(200, 200, 40);
-      wait(1);
+        case MAVLINK_MSG_ID_ATT_POS_MOCAP:  // #1: SYS_STATUS
+          {
+            /* Message decoding: PRIMITIVE
+             *    mavlink_msg_sys_status_decode(const mavlink_message_t* msg, mavlink_sys_status_t* sys_status)
+             */
+             
+             
+           mavlink_msg_att_pos_mocap_decode(&receive_msg, &mocap_pose);
+            /*if(show_list.size()>15){
+                show_list.pop_front();
+                show_list.pop_front();
+                show_list.pop_front();
+             }*/
+            
+            /*
+          
+            
+            sprintf (bufa, " pos_y : %f", mocap_pose.y);
+            text =   bufa;
+            show_list.push_back(text);
+            show_text();
+            
+            sprintf (bufa, " pos_z : %f", mocap_pose.z);
+            text =   bufa;
+            show_list.push_back(text);
+            show_text();
+            */
+            
+            
+            
+          }
+          break;
 
-      lcd.SetBackColor(LCD_COLOR_ORANGE);
-      lcd.SetTextColor(LCD_COLOR_CYAN);
-      BSP_LCD_SetFont(&Font24);
-      lcd.DisplayStringAt(0, LINE(7), (uint8_t *)"HAVE FUN !!!", CENTER_MODE);
-      wait(1);
+        case MAV_CMD_COMPONENT_ARM_DISARM:  // #22: PARAM_VALUE
+          {
+            /* Message decoding: PRIMITIVE
+             *    mavlink_msg_param_value_decode(const mavlink_message_t* msg, mavlink_param_value_t* param_value)
+             */
+            //mavlink_message_t* msg;
+            //mavlink_param_value_t param_value;
+            //mavlink_msg_param_value_decode(&msg, &param_value);
+            
+            
+            
+          }
+          break;
 
-      led1 = !led1;
-      wait(0.5);
+        case MAVLINK_MSG_ID_RAW_IMU:  // #27: RAW_IMU
+          {
+            /* Message decoding: PRIMITIVE
+             *    static inline void mavlink_msg_raw_imu_decode(const mavlink_message_t* msg, mavlink_raw_imu_t* raw_imu)
+             */
+            //mavlink_raw_imu_t raw_imu;
+            //mavlink_msg_raw_imu_decode(&msg, &raw_imu);
+          }
+          break;
+
+        case MAVLINK_MSG_ID_ATTITUDE:  // #30
+          {
+            /* Message decoding: PRIMITIVE
+             *    mavlink_msg_attitude_decode(const mavlink_message_t* msg, mavlink_attitude_t* attitude)
+             */
+            //mavlink_attitude_t attitude;
+            //mavlink_msg_attitude_decode(&msg, &attitude);
+
+          
+          }
+          break;
+
+        
+       default:
+          break;
+      }
+          
+          
+          
+          }
+    
     }
 }
 
+void callback() {
+
+    receive();
+    led2 = !led2;
+}
+int main()
+{     
+    pc.baud(115200);
+     pc.attach(&callback);
+    heartbeat.custom_mode = 555;
+    heartbeat.type = 2;
+    heartbeat.autopilot = 12;
+    heartbeat.base_mode = 43;
+    heartbeat.system_status =32;
+    heartbeat.mavlink_version = 3; 
+    led1 = 1;
+  
+    BSP_LCD_SetFont(&Font12);
+    lcd.DisplayStringAt(0, LINE(1), (uint8_t *)"MAvLink EXAMPLE", LEFT_MODE);
+    wait(1);
+    lcd.Clear(LCD_COLOR_WHITE);
+    lcd.SetBackColor(LCD_COLOR_WHITE);
+    lcd.SetTextColor(LCD_COLOR_BLACK);
+    
+    while(1)
+    {
+     
+     if(show_list.size()>15){
+        show_list.pop_front();
+        show_list.pop_front();
+        show_list.pop_front();
+        
+     }
+     j++;
+    receive();
+        sprintf (re_buf, " pos_x : %f", mocap_pose.x);
+        text =  re_buf;
+        show_list.push_back(text);
+        show_text();
+        sprintf (re_buf, " pos_y : %f", mocap_pose.y);
+        text =  re_buf;
+        show_list.push_back(text);
+        show_text();
+        sprintf (re_buf, " pos_z : %f", mocap_pose.z);
+        text =  re_buf;
+        show_list.push_back(text);
+        show_text();
+     if(pc.writeable()){
+      
+      mavlink_msg_heartbeat_encode(1,0 ,&message ,&heartbeat  );
+    len = mavlink_msg_to_send_buffer((uint8_t*)buf, &message);
+     write_data(pc, buf , len);
+      }
+      //wait(1);
+      led1 = !led1;
+      wait_ms(100);
+    }
+}
+