联合体打包数组

ArduCopter/UserCode.cpp

@@ -42,30 +42,6 @@ void Copter::userhook_SlowLoop()
 void Copter::userhook_SuperSlowLoop()
 {
     // put your 1Hz code here
-    if(zr_serial_api.data_trans_init){
-        Vector3f pos_neu_cm;
-        
-        if (current_loc.get_vector_from_origin_NEU(pos_neu_cm)) {
-            Location my_loc;
-            if (ahrs.get_position(my_loc)) {
-                pos_neu_cm.x = my_loc.lat;
-                pos_neu_cm.y = my_loc.lng;
-                pos_neu_cm.z = my_loc.alt;
-                zr_serial_api.get_vehicle_NEU_pos(pos_neu_cm);
-                gcs().send_text(MAV_SEVERITY_INFO,"get loc:%.2f,%.2f,%.2f",pos_neu_cm.x,pos_neu_cm.y,pos_neu_cm.z);
-            }
-        }
-
-        Vector3f euler,euler_deg;
-        if(ahrs.get_secondary_attitude(euler)){
-            euler_deg.x = degrees(euler.x);
-            euler_deg.y = degrees(euler.y);
-            euler_deg.z = wrap_360_cd(degrees(euler.z));
-            zr_serial_api.get_vehicle_euler_angles(euler_deg);
-            gcs().send_text(MAV_SEVERITY_INFO,"get euler:%.2f,%.2f,%.2f",euler_deg.x,euler_deg.y,euler_deg.z);
-        }
-    }
-    
 }
 #endif
 
@@ -99,4 +75,36 @@ void Copter::zr_app_fast(){
             gcs().send_text(MAV_SEVERITY_INFO, "data_trans init");
         }
     }
+
+    
+    if(zr_serial_api.data_trans_init){
+        Vector3f pos_neu_cm;
+        
+        if (current_loc.get_vector_from_origin_NEU(pos_neu_cm)) {
+            Location my_loc;
+            if (ahrs.get_position(my_loc)) {
+                pos_neu_cm.x = my_loc.lat;
+                pos_neu_cm.y = my_loc.lng;
+                pos_neu_cm.z = my_loc.alt;
+                // zr_serial_api.get_vehicle_NEU_pos(pos_neu_cm);
+                // gcs().send_text(MAV_SEVERITY_INFO,"get loc:%.2f,%.2f,%.2f",pos_neu_cm.x,pos_neu_cm.y,pos_neu_cm.z);
+            }
+        }
+
+        Vector3f euler,euler_deg;
+        if(ahrs.get_secondary_attitude(euler)){
+            euler_deg.x = degrees(euler.x);
+            euler_deg.y = degrees(euler.y);
+            euler_deg.z = wrap_360_cd(degrees(euler.z));
+            // zr_serial_api.get_vehicle_euler_angles(euler_deg);
+            // gcs().send_text(MAV_SEVERITY_INFO,"get euler:%.2f,%.2f,%.2f",euler_deg.x,euler_deg.y,euler_deg.z);
+        }
+
+        Vector3f vel_ned;
+        if (ahrs.get_velocity_NED(vel_ned)) {
+            // vel_ned.length(); // 单位米
+        }
+        zr_serial_api.get_vehicle_flight_data(pos_neu_cm,euler,vel_ned,(uint8_t)flightmode->mode_number(),(uint8_t)flightmode->is_landing());
+    }
+    
 }

Tools/ardupilotwaf/boards.py

@@ -108,7 +108,7 @@ class Board:
 
             '-Wall',
             '-Wextra',
-            '-Werror=format',
+            # '-Werror=format',
             '-Wpointer-arith',
             '-Wcast-align',
             '-Wno-missing-field-initializers',
@@ -211,7 +211,7 @@ class Board:
             '-Werror=format-security',
             '-Werror=format-extra-args',
             '-Werror=enum-compare',
-            '-Werror=format',
+            # '-Werror=format',
             '-Werror=array-bounds',
             '-Werror=uninitialized',
             '-Werror=init-self',
@@ -226,7 +226,7 @@ class Board:
             '-Werror=unused-value',
             '-Werror=unused-variable',
             '-Werror=delete-non-virtual-dtor',
-            '-Wfatal-errors',
+            # '-Wfatal-errors',
             '-Wno-trigraphs',
             '-Werror=parentheses',
             '-DARDUPILOT_BUILD',
@@ -632,7 +632,7 @@ class chibios(Board):
             '-Wmissing-declarations',
             '-Wno-unused-parameter',
             '-Werror=array-bounds',
-            '-Wfatal-errors',
+            # '-Wfatal-errors',
             '-Werror=uninitialized',
             '-Werror=init-self',
             '-Werror=unused-but-set-variable',

libraries/AC_ZR_APP/AC_ZR_Serial_API.cpp

@@ -20,7 +20,7 @@
 #include <string.h>
 #include <stdio.h>
 
-#define ZR_API_DEBUG 1
+#define ZR_API_DEBUG 0
 #if ZR_API_DEBUG
  # define Debug(fmt, args ...)  gcs().send_text(MAV_SEVERITY_INFO, "{%d}," fmt , __LINE__, ## args)
 //  # define Debug(fmt, args ...)  gcs().send_text(MAV_SEVERITY_INFO, "NOTICE:" fmt , ## args)
@@ -78,7 +78,7 @@ void AC_ZR_Serial_API::write_data_to_port(AP_HAL::UARTDriver *_port, const uint8
         if (_port->txspace() > len) {
             _port->write(data, len);
         } else {
-            Debug("Data_Trans Not enough TXSPACE,%d < %d", _port->txspace(),len);
+            // Debug("Data_Trans Not enough TXSPACE,%d < %d", _port->txspace(),len);
         }
     }
 }
@@ -115,7 +115,7 @@ void AC_ZR_Serial_API::get_vehicle_NEU_pos(Vector3f vec_neu)
     static char  buf[50];
     snprintf(buf, sizeof(buf),  "pos:%.2f,%.2f,%.2f\n\r",vec_neu.x,vec_neu.y,vec_neu.z);
     mav_data_len = 50;
-    memset(mav_data_from_host,0,TRANS_DATA_LEN);
+    memset(mav_data_from_host,0,PARSER_DATA_LEN);
     memcpy(mav_data_from_host,buf,50);
     get_mav_data = true;
     write_data_to_port(_zr_api_port,mav_data_from_host, mav_data_len);
@@ -127,13 +127,48 @@ void AC_ZR_Serial_API::get_vehicle_euler_angles(Vector3f euler)
     static char  buf[50];
     snprintf(buf, sizeof(buf),  "rpy:%.2f,%.2f,%.2f\n\r",euler.x,euler.y,euler.z);
     mav_data_len = 50;
-    // memset(mav_data_from_host,0,TRANS_DATA_LEN);
+    // memset(mav_data_from_host,0,PARSER_DATA_LEN);
     // memcpy(mav_data_from_host,buf,50);
     get_mav_data = true;
     write_data_to_port(_zr_api_port,(const uint8_t *)buf, mav_data_len);
 
 }
 
+void AC_ZR_Serial_API::get_vehicle_flight_data(Vector3f pos,Vector3f euler,Vector3f vel,uint8_t mode,uint8_t in_air)
+{
+    uint8_t crc_sum = 0;
+    
+    flight_data_parser.data_msg.msg_head = 0xfe;
+    flight_data_parser.data_msg.msg_length = PARSER_DATA_LEN;
+
+    flight_data_parser.data_msg.time_ms = AP_HAL::millis();
+
+    flight_data_parser.data_msg.flight_data.lat = pos.x;
+    flight_data_parser.data_msg.flight_data.lng = pos.y;
+    flight_data_parser.data_msg.flight_data.alt = pos.z;
+
+    flight_data_parser.data_msg.flight_data.roll = euler.x;
+    flight_data_parser.data_msg.flight_data.pitch = euler.y;
+    flight_data_parser.data_msg.flight_data.yaw = euler.z;
+
+    flight_data_parser.data_msg.flight_data.vx = vel.x;
+    flight_data_parser.data_msg.flight_data.vy = vel.y;
+    flight_data_parser.data_msg.flight_data.vz = vel.z;
+
+    flight_data_parser.data_msg.flight_data.state = UINT16_VALUE(mode, in_air);
+    for (size_t i = 0; i < 40; i++)
+    {
+        crc_sum += flight_data_parser.data[i];
+    }
+    flight_data_parser.data_msg.crc = crc_sum;
+    write_data_to_port(_zr_api_port,flight_data_parser.data, PARSER_DATA_LEN);
+    if(0){
+        gcs().send_text(MAV_SEVERITY_INFO, "{%d},%.4f,%.4f,%.4f" , __LINE__,flight_data_parser.data_msg.flight_data.lat, \
+                                                    flight_data_parser.data_msg.flight_data.roll ,\
+                                                    flight_data_parser.data_msg.flight_data.vx );
+    }
+}
+
 void AC_ZR_Serial_API::update(void)
 {
     if(!data_trans_init || _zr_api_port == nullptr){
@@ -147,7 +182,7 @@ void AC_ZR_Serial_API::update(void)
         mav_len = serial_data_len;
         Debug("%d,device data len:%d",now_time,mav_len);
         serial_data_len = 0;
-        memset(serial_data_from_device,0,TRANS_DATA_LEN);
+        memset(serial_data_from_device,0,PARSER_DATA_LEN);
     }
 
     // if(get_mav_data){

libraries/AC_ZR_APP/AC_ZR_Serial_API.h

@@ -20,7 +20,7 @@
 #include <GCS_MAVLink/GCS.h>
 #include <GCS_MAVLink/GCS_MAVLink.h>
 
-#define TRANS_DATA_LEN 252
+#define PARSER_DATA_LEN 45
 
 class AC_ZR_Serial_API
 {
@@ -43,6 +43,7 @@ public:
     void get_vehicle_NEU_pos(Vector3f vec_neu);
     void get_vehicle_euler_angles(Vector3f euler);
 
+    void get_vehicle_flight_data(Vector3f pos,Vector3f euler,Vector3f vel,uint8_t mode,uint8_t in_air);
     bool data_trans_init;
 private:
 
@@ -50,12 +51,51 @@ private:
     AP_HAL::UARTDriver *_zr_api_port;
     uint32_t serial_last_data_time;
 
-    uint8_t serial_data_from_device[TRANS_DATA_LEN];
-    uint8_t mav_data_from_host[TRANS_DATA_LEN];
+    uint8_t serial_data_from_device[PARSER_DATA_LEN];
+    uint8_t mav_data_from_host[PARSER_DATA_LEN];
 
     uint8_t serial_data_len,mav_data_len,mav_len;
     bool get_mav_data;
 
+    struct PACKED zr_flight_data_s
+    {
+        /* data */
+        float lat;
+        float lng;
+        float alt;
+        // 12
+        float roll;
+        float pitch;
+        float yaw;
+        // 24
+        float vx;
+        float vy;
+        float vz;
+        // 36
+        uint16_t state;
+    };
+    union PACKED zr_flight_data_u
+    {
+        /* data */
+        zr_flight_data_s zr_flight_data;
+        uint8_t data[38];
+    };
+    
+    struct PACKED flight_data_parser_s
+    {
+        uint8_t msg_head;
+        uint8_t msg_length;
+        uint32_t time_ms;
+        zr_flight_data_s flight_data;
+        uint8_t crc;
+    };
+    
+    union flight_data_parser_u
+    {
+        flight_data_parser_s data_msg;
+        uint8_t data[PARSER_DATA_LEN];
+    }flight_data_parser;
+    
 
 };
     

sitl.sh

@@ -1 +1 @@
-./Tools/autotest/sim_vehicle.py  -j4 -L CYDS --console --map -v ArduCopter
+./Tools/autotest/sim_vehicle.py  -j4 -L CYDS --console --out 192.168.0.51:14552 --map -A --serial5=uart:/dev/ttyUSB0:115200 -v ArduCopter