sdk控制调整，飞行测试成功

ArduCopter/Copter.h

@@ -681,7 +681,7 @@ private:
     // zr_app.cpp
     
     bool start_takeoff(float alt);
-    bool set_target_location(const Location& target_loc);
+    bool set_target_location(const Location& target_loc,int32_t yaw_cd);
     bool set_target_pos_NED(const Vector3f& target_pos, bool use_yaw, float yaw_deg, bool use_yaw_rate, float yaw_rate_degs, bool yaw_relative, bool terrain_alt);
     bool set_target_posvel_NED(const Vector3f& target_pos, const Vector3f& target_vel);
     bool set_target_posvelaccel_NED(const Vector3f& target_pos, const Vector3f& target_vel, const Vector3f& target_accel, bool use_yaw, float yaw_deg, bool use_yaw_rate, float yaw_rate_degs, bool yaw_relative);

ArduCopter/version.h

@@ -9,14 +9,14 @@
 #undef GIT_VERSION
 #define GIT_VERSION "3"
 #endif
-#define THISFIRMWARE "ZRUAV v4.1.16"  //"ArduCopter V4.0.0"
+#define THISFIRMWARE "ZRUAV v4.3.0-dev"  //"ArduCopter V4.0.0"
 
 // the following line is parsed by the autotest scripts
-#define FIRMWARE_VERSION 4,1,16,FIRMWARE_VERSION_TYPE_OFFICIAL
+#define FIRMWARE_VERSION 4,3,0,FIRMWARE_VERSION_TYPE_OFFICIAL
 
 #define FW_MAJOR 4
-#define FW_MINOR 1
-#define FW_PATCH 16
+#define FW_MINOR 3
+#define FW_PATCH 0
 #define FW_TYPE FIRMWARE_VERSION_TYPE_OFFICIAL
 /**
  * AVDv6: avoid.get_zr_mode() = 3时可以用Loiter模式测试自动避障

ArduCopter/zr_app.cpp

@@ -16,14 +16,15 @@ bool Copter::start_takeoff(float alt)
 }
 
 // set target location (for use by scripting)
-bool Copter::set_target_location(const Location& target_loc)
+bool Copter::set_target_location(const Location& target_loc,int32_t yaw_cd)
 {
     // exit if vehicle is not in Guided mode or Auto-Guided mode
     if (!flightmode->in_guided_mode()) {
         return false;
     }
 
-    return mode_guided.set_destination(target_loc);
+    // return mode_guided.set_destination(target_loc);
+    return mode_guided.set_destination(target_loc, true, (float)yaw_cd, true, 200.0, false);
 }
 
 // set target position (for use by scripting)
@@ -56,6 +57,14 @@ bool Copter::set_target_velocity_NED(const Vector3f& vel_ned)
 
 void Copter::zr_app_10hz()
 {
+    if(zr_serial_api.data_trans_init){
+// 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)
+        uint16_t now_volt = uint16_t(battery.voltage() * 100);
+        zr_serial_api.get_vehicle_status((uint8_t)control_mode,(uint8_t)flightmode->is_landing(),home_distance(),now_volt,battery.capacity_remaining_pct());
+    }
+
+}
+void Copter::zr_app_50hz(){
 
     if(zr_serial_api.data_trans_init){
         zr_serial_api.update();
@@ -69,15 +78,6 @@ void Copter::zr_app_10hz()
         }
     }
 
-    if(zr_serial_api.data_trans_init){
-// 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)
-        uint16_t now_volt = uint16_t(battery.voltage() * 100);
-        zr_serial_api.get_vehicle_status((uint8_t)control_mode,(uint8_t)flightmode->is_landing(),home_distance(),now_volt,battery.capacity_remaining_pct());
-    }
-
-}
-void Copter::zr_app_50hz(){
-
     if(zr_serial_api.data_trans_init){
         Vector3l pos_neu_cm;
         
@@ -117,6 +117,8 @@ void Copter::zr_app_50hz(){
         float yaw_deg;
         bool new_data = zr_serial_api.get_control_data((uint8_t)control_mode ,msg_type,data,yaw_deg);
         if(new_data){
+            
+            int32_t ahrs_yaw_cd = wrap_360_cd(int32_t(yaw_deg * 100));
             gcs().send_text(MAV_SEVERITY_INFO,"get msg_type:%d, data:%ld,%ld,%ld, y:%.2f",(int)msg_type,data.x,data.y,data.z,yaw_deg);
             switch (msg_type)
             {
@@ -140,21 +142,50 @@ void Copter::zr_app_50hz(){
                         target_loc.lng = data.y;
                         target_loc.alt = (data.z + 500);
                         // target_loc.alt = (data.z + 500)/100.0;
-                        set_target_location(target_loc);
+                        set_target_location(target_loc,ahrs_yaw_cd);
                         gcs().send_text(MAV_SEVERITY_INFO,"set location,lat %ld,%ld,%ld",target_loc.lat,target_loc.lng,target_loc.alt);
                     }
                     break;
                 case AC_ZR_Serial_API::ZR_Msg_Type::MSG_CONTROL_VEL:
                     if(motors->armed()){
                         Vector3f target_vel;
-                        target_vel.x = data.x / 100.0;
-                        target_vel.y = data.y / 100.0;
-                        target_vel.z = data.z / 100.0;
+                        target_vel.x = data.x / 1.0;
+                        target_vel.y = data.y / 1.0;
+                        target_vel.z = data.z / 1.0;
                         // target_loc.alt = (data.z + 500)/100.0;
-                        set_target_velocity_NED(target_vel);
+                        // set_target_velocity_NED(target_vel);
+                        mode_guided.set_velocity(target_vel,true,ahrs_yaw_cd,false,0.0f,false,true);
+
                         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();
+                        // 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:%ld",ne_x,ne_y,target_vel.z,ahrs_yaw_cd);
+                    }
+                    break;
             
                 default:
                     break;

libraries/AP_NavEKF2/AP_NavEKF2_core.cpp

@@ -337,7 +337,8 @@ void NavEKF2_core::InitialiseVariables()
     have_table_earth_field = false;
 
     // initialise pre-arm message
-    hal.util->snprintf(prearm_fail_string, sizeof(prearm_fail_string), "EKF2 still initialising");
+    // hal.util->snprintf(prearm_fail_string, sizeof(prearm_fail_string), "EKF2 still initialising");
+    hal.util->snprintf(prearm_fail_string, sizeof(prearm_fail_string), "EKF2 初始化");
 
     InitialiseVariablesMag();
 }

libraries/AP_RangeFinder/RangeFinder.cpp

@@ -700,7 +700,8 @@ bool RangeFinder::prearm_healthy(char *failure_msg, const uint8_t failure_msg_le
 {
     for (uint8_t i = 0; i < RANGEFINDER_MAX_INSTANCES; i++) {
         if ((params[i].type != RangeFinder_TYPE_NONE) && (drivers[i] == nullptr)) {
-          hal.util->snprintf(failure_msg, failure_msg_len, "Rangefinder %d was not detected", i + 1);
+        //   hal.util->snprintf(failure_msg, failure_msg_len, "Rangefinder %d was not detected", i + 1);
+          hal.util->snprintf(failure_msg, failure_msg_len, "未检测到测距传感器 %d", i + 1);
           return false;
         }
     }

modules/mavlink

@@ -1 +1 @@
-Subproject commit 5fc095d17043557589ecc6a28d62fb234081fc87
+Subproject commit 86cf7601a72f065f6c2dd8b10a602a73f827ac64

rs100.sh

@@ -1,3 +1,3 @@
 ./waf configure --board rs100
 ./waf copter
-cp ./build/rs100/bin/arducopter.apj  /mnt/f/_01-work/100\=data/固件/编译/rs100-v4.1.16.px4
+cp ./build/rs100/bin/arducopter.apj  /mnt/f/_01-work/100\=data/固件/编译/rs100-sdk-v4.3.0-dev-FE.px4