add body frame speed control

ArduCopter/zr_app.cpp

@@ -103,6 +103,34 @@ void Copter::zr_app_50hz(){
                         gcs().send_text(MAV_SEVERITY_INFO,"set vel %.2f,%.2f,%.2f",target_vel.x,target_vel.y,target_vel.z);
                     }
                     break;
+                case AC_ZR_Serial_API::ZR_Msg_Type::MSG_CONTROL_VEL_P:
+                    if(motors->armed()){
+                        Vector3f target_vel;
+                        target_vel.x = data.x / 1.0;
+                        target_vel.y = data.y / 1.0;
+                        target_vel.z = data.z / 1.0;
+                        // float speed_forward = target_vel.x * ahrs.cos_yaw() + target_vel.y * ahrs.sin_yaw();
+                        // float speed_right   = -target_vel.x * ahrs.sin_yaw() + target_vel.y * ahrs.cos_yaw();
+
+                        // rotate from body-frame to NE frame
+                        const float ne_x = target_vel.x * ahrs.cos_yaw() - target_vel.y * ahrs.sin_yaw();
+                        const float ne_y = target_vel.x * ahrs.sin_yaw() + target_vel.y * ahrs.cos_yaw();
+                        int32_t ahrs_yaw_cd = wrap_360_cd(int32_t(yaw_deg * 100));
+                        // target_loc.alt = (data.z + 500)/100.0;
+                        // set_target_velocity_NED(target_vel);
+
+                        // exit if vehicle is not in Guided mode or Auto-Guided mode
+                        if (!flightmode->in_guided_mode()) {
+                            return;
+                        }
+
+                        // convert vector to neu in cm
+                        const Vector3f vel_neu_cms(ne_x, ne_y, -target_vel.z);
+                        mode_guided.set_velocity(vel_neu_cms,true,ahrs_yaw_cd,false,0.0f,false,true);
+
+                        gcs().send_text(MAV_SEVERITY_INFO,"set speed %.2f,%.2f,%.2f,yaw:%.2f,cd:%d",ne_x,ne_y,target_vel.z,ahrs_yaw_cd,wrap_360_cd(ahrs.yaw_sensor));
+                    }
+                    break;
             
                 default:
                     break;

Tools/ardupilotwaf/boards.py

@@ -629,7 +629,7 @@ class chibios(Board):
             '-Wno-sign-compare',
             '-Wfloat-equal',
             '-Wpointer-arith',
-            '-Wmissing-declarations',
+            # '-Wmissing-declarations',
             '-Wno-unused-parameter',
             '-Werror=array-bounds',
             '-Wfatal-errors',

Tools/autotest/locations.txt

@@ -1,6 +1,6 @@
 #NAME=latitude,longitude,absolute-altitude,heading
 OSRF0=37.4003371,-122.0800351,0,353
-CYDS=24.6122682,118.0672663,1,0
+CYDS=24.6122682,118.0672663,1,90
 OSRF0_PILOTSBOX=37.4003371,-122.0800351,2,270
 CMAC=-35.363261,149.165230,584,353
 CMAC2=-35.362889,149.165221,584,270

libraries/AC_ZR_APP/AC_ZR_Serial_API.cpp

@@ -154,14 +154,14 @@ bool AC_ZR_Serial_API::check_uart(AP_HAL::UARTDriver *_port)
         uint8_t newlen = pktoffset - (p - pktbuf);
         memmove(&pktbuf[0], p, newlen);
         pktoffset = newlen;
-        Debug("move msg:%d",newlen);
+        // Debug("move msg:%d",newlen);
     } else {
         // pktoffset = 0;
         goto reset;
     }
 
     if (pktbuf[0] != 0xFE) {
-        Debug("no head:%d",pktoffset);
+        // Debug("no head:%d",pktoffset);
         return false;
     }
 
@@ -170,7 +170,7 @@ bool AC_ZR_Serial_API::check_uart(AP_HAL::UARTDriver *_port)
         // got pkt1
         memcpy(flight_control_parser.data,pktbuf,CONTROL_DATA_LEN); // 收到一帧数据
         control_data_last_time = AP_HAL::millis();
-        Debug(" %ld-get msg,ofs:%d ",control_data_last_time,pktoffset);
+        // Debug(" %ld-get msg,ofs:%d ",control_data_last_time,pktoffset);
         // process_packet(pktbuf);
         memmove(&pktbuf[0], &pktbuf[CONTROL_DATA_LEN], pktoffset-CONTROL_DATA_LEN);
         pktoffset -= CONTROL_DATA_LEN;
@@ -266,6 +266,7 @@ void AC_ZR_Serial_API::get_vehicle_euler_angles(Vector3f euler)
 
 void AC_ZR_Serial_API::get_vehicle_status(uint8_t mode,uint8_t in_air,uint32_t home_distance,uint16_t volt_mv,uint8_t bat_remaining)
 {
+    return; // for test ,don't send
     flight_mode = mode;
   
     static uint8_t delay_cnt;
@@ -370,7 +371,7 @@ bool AC_ZR_Serial_API::get_control_data(uint8_t mode, ZR_Msg_Type &type,Vector3l
         return false; // 校验失败
     }
     type = flight_control_parser.msg.type;
-    if(type < ZR_Msg_Type::MSG_CONTROL_TKOFF || type > ZR_Msg_Type::MSG_CONTROL_VEL ){
+    if(type < ZR_Msg_Type::MSG_CONTROL_TKOFF || type > ZR_Msg_Type::MSG_CONTROL_VEL_P ){
         set_control_ask(flight_control_parser.msg.msg_id,flight_control_parser.msg.type,ZR_Msg_ASK::MSG_ASK_ERRTYPE); // 应答控制类型错误
         return false; // 控制类型错误
     }

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.10.137:14552 --map -A --serial5=uart:/dev/ttyS1:115200 -v ArduCopter