pinxian wu
mavlink test
Dependencies: BSP_DISCO_F429ZI LCD_DISCO_F429ZI mavlink_bridge mbed
Fork of
DISCO-F429ZI_LCD_demo
by 
ST
Revision 4:c2de9e2123ac, committed 2018-08-29
- Comitter:
- wupinxian
- Date:
- Wed Aug 29 06:59:14 2018 +0000
- Parent:
- 3:e4d4cf43a2ca
- Commit message:
- mavlink test
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
| mavlink_bridge.lib | Show annotated file Show diff for this revision Revisions of this file |
--- 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, ¶m_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);
+ }
+}
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mavlink_bridge.lib Wed Aug 29 06:59:14 2018 +0000 @@ -0,0 +1,1 @@ +http://os.mbed.com/users/wupinxian/code/mavlink_bridge/#76345550efb4