控制模式调整

ArduCopter/Parameters.cpp

@@ -1728,7 +1728,7 @@ const char* Copter::get_sysid_board_id(void)
     
         int32_t deadline = 0;
         get_deadline_params(deadline);
-        gcs().send_text(MAV_SEVERITY_INFO, "Date:%ld",deadline);
+        gcs().send_text(MAV_SEVERITY_INFO, "使用期限:%ld",deadline);
         if((int32_t)g.zr_reg_date != deadline){
             // gcs().send_text(MAV_SEVERITY_INFO, "reload date:%ld->%ld",(int32_t)g.zr_reg_date,deadline);
             g.zr_reg_date.set_and_save_ifchanged(deadline);
@@ -1742,7 +1742,7 @@ const char* Copter::get_sysid_board_id(void)
             {
                 if (ptype == AP_PARAM_INT16)
                 {
-                    gcs().send_text(MAV_SEVERITY_INFO, "Battery cycle: %d", (int16_t)g.zr_bat_cycled);
+                    gcs().send_text(MAV_SEVERITY_INFO, "电池循环: %d", (int16_t)g.zr_bat_cycled);
                 }
             }
             else

ArduCopter/UserCode.cpp

@@ -33,7 +33,6 @@ void Copter::userhook_FastLoop()
 {
     // put your 100Hz code here
     takeoff_crash_detect();
-    zr_app_10hz();
 }
 #endif
 
@@ -49,6 +48,7 @@ void Copter::userhook_50Hz()
 void Copter::userhook_MediumLoop()
 {
     // put your 10Hz code here
+    zr_app_10hz();
 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
 if(current_loc.alt > avoid.get_zr_avd_alt())
     do_avoid_action();

ArduCopter/mode_auto.cpp

@@ -888,7 +888,7 @@ void ModeAuto::wp_run()
     }
     // process pilot's yaw input
     float target_yaw_rate = 0;
-    if (!copter.failsafe.radio) {
+    if (!copter.failsafe.radio && !copter.zr_serial_api.data_trans_init) {   //copter.zr_serial_api.data_trans_init 仿线模式启动，禁用遥控控制
         // get pilot's desired yaw rate
         target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
         if (!is_zero(target_yaw_rate)) {
@@ -935,7 +935,7 @@ void ModeAuto::spline_run()
 
     // process pilot's yaw input
     float target_yaw_rate = 0;
-    if (!copter.failsafe.radio) {
+    if (!copter.failsafe.radio && !copter.zr_serial_api.data_trans_init) {   //copter.zr_serial_api.data_trans_init 仿线模式启动，禁用遥控控制
         // get pilot's desired yaw rat
         target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
         if (!is_zero(target_yaw_rate)) {

ArduCopter/mode_guided.cpp

@@ -386,7 +386,7 @@ void Mode::auto_takeoff_run()
 
     // process pilot's yaw input
     float target_yaw_rate = 0;
-    if (!copter.failsafe.radio) {
+    if (!copter.failsafe.radio && !copter.zr_serial_api.data_trans_init) {   //copter.zr_serial_api.data_trans_init 仿线模式启动，禁用遥控控制
         // get pilot's desired yaw rate
         target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
     }
@@ -417,7 +417,7 @@ void ModeGuided::pos_control_run()
 {
     // process pilot's yaw input
     float target_yaw_rate = 0;
-    if (!copter.failsafe.radio) {
+    if (!copter.failsafe.radio && !copter.zr_serial_api.data_trans_init) {   //copter.zr_serial_api.data_trans_init 仿线模式启动，禁用遥控控制
         // get pilot's desired yaw rate
         target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
         if (!is_zero(target_yaw_rate)) {
@@ -459,7 +459,7 @@ void ModeGuided::vel_control_run()
 {
     // process pilot's yaw input
     float target_yaw_rate = 0;
-    if (!copter.failsafe.radio) {
+    if (!copter.failsafe.radio && !copter.zr_serial_api.data_trans_init) {   //copter.zr_serial_api.data_trans_init 仿线模式启动，禁用遥控控制
         // get pilot's desired yaw rate
         target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
         if (!is_zero(target_yaw_rate)) {
@@ -512,7 +512,7 @@ void ModeGuided::posvel_control_run()
     // process pilot's yaw input
     float target_yaw_rate = 0;
 
-    if (!copter.failsafe.radio) {
+    if (!copter.failsafe.radio && !copter.zr_serial_api.data_trans_init) {   //copter.zr_serial_api.data_trans_init 仿线模式启动，禁用遥控控制
         // get pilot's desired yaw rate
         target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
         if (!is_zero(target_yaw_rate)) {

ArduCopter/version.h

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

ArduCopter/zr_app.cpp

@@ -60,7 +60,7 @@ 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());
+        zr_serial_api.get_vehicle_status((uint8_t)control_mode,!ap.land_complete,home_distance(),now_volt,battery.capacity_remaining_pct());
     }
 
 }
@@ -96,7 +96,7 @@ void Copter::zr_app_50hz(){
         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));
+            euler_deg.z = wrap_360(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);
         }
@@ -117,9 +117,15 @@ 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){
-            
+            static char  buf[50];
             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);
+            // 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);
+            // zr_serial_api.print_data("get msg_type:%d, data:%ld,%ld,%ld, y:%.2f",(int)msg_type,data.x,data.y,data.z,yaw_deg);
+            if(zr_serial_api.get_param_debug()){
+            snprintf(buf, sizeof(buf), "type:%d, data:%ld,%ld,%ld, y:%.2f",(int)msg_type,data.x,data.y,data.z,yaw_deg);
+            zr_serial_api.print_msg(buf);
+            memset(buf,0,50);
+            }
             switch (msg_type)
             {
                 case AC_ZR_Serial_API::ZR_Msg_Type::MSG_CONTROL_TKOFF:
@@ -132,7 +138,13 @@ void Copter::zr_app_50hz(){
                     }
                     float tk_alt = (data.z)/100.0;
                     start_takeoff(tk_alt);
-                    gcs().send_text(MAV_SEVERITY_INFO,"do takeoff,alt %.2f",tk_alt);
+                    if(zr_serial_api.get_param_debug()){
+
+                        // zr_serial_api.print_data("do takeoff,alt %.2f",tk_alt);
+                        snprintf(buf, sizeof(buf), "do takeoff,alt %.2f",tk_alt);
+                        zr_serial_api.print_msg(buf);
+                        memset(buf,0,50);
+                    }
                 }
                     break;
                 case AC_ZR_Serial_API::ZR_Msg_Type::MSG_CONTROL_POS:
@@ -143,7 +155,12 @@ void Copter::zr_app_50hz(){
                         target_loc.alt = (data.z + 500);
                         // target_loc.alt = (data.z + 500)/100.0;
                         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);
+                        if(zr_serial_api.get_param_debug()){
+                            // zr_serial_api.print_data("set location,lat %ld,%ld,%ld",target_loc.lat,target_loc.lng,target_loc.alt);
+                            snprintf(buf, sizeof(buf), "set location,lat %ld,%ld,%ld",target_loc.lat,target_loc.lng,target_loc.alt);
+                            zr_serial_api.print_msg(buf);
+                            memset(buf,0,50);
+                        }
                     }
                     break;
                 case AC_ZR_Serial_API::ZR_Msg_Type::MSG_CONTROL_VEL:
@@ -155,8 +172,13 @@ void Copter::zr_app_50hz(){
                         // target_loc.alt = (data.z + 500)/100.0;
                         // set_target_velocity_NED(target_vel);
                         mode_guided.set_velocity(target_vel,true,ahrs_yaw_cd,false,0.0f,false,true);
+                        if(zr_serial_api.get_param_debug()){
 
-                        gcs().send_text(MAV_SEVERITY_INFO,"set vel %.2f,%.2f,%.2f",target_vel.x,target_vel.y,target_vel.z);
+                            // zr_serial_api.print_data("set vel %.2f,%.2f,%.2f",target_vel.x,target_vel.y,target_vel.z);
+                            snprintf(buf, sizeof(buf), "set vel %.2f,%.2f,%.2f",target_vel.x,target_vel.y,target_vel.z);
+                            zr_serial_api.print_msg(buf);
+                            memset(buf,0,50);
+                        }
                     }
                     break;
                 case AC_ZR_Serial_API::ZR_Msg_Type::MSG_CONTROL_VEL_P:
@@ -182,11 +204,61 @@ void Copter::zr_app_50hz(){
                         // 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);
+                        if(zr_serial_api.get_param_debug()){
+                            // zr_serial_api.print_data("set speed %.2f,%.2f,%.2f,yaw:%ld",ne_x,ne_y,target_vel.z,ahrs_yaw_cd);
+                            snprintf(buf, sizeof(buf), "set speed %.2f,%.2f,%.2f,yaw:%ld",ne_x,ne_y,target_vel.z,ahrs_yaw_cd);
+                            zr_serial_api.print_msg(buf);
+                            memset(buf,0,50);
+                        }
+                    }
+                    break;
+                case AC_ZR_Serial_API::ZR_Msg_Type::MSG_CONTROL_VEL_LOCK_YAW:
+                    if(motors->armed()){
+                        Vector3f target_vel;
+                        target_vel.x = 0;
+                        target_vel.y = 0;
+                        target_vel.z = 0;
+                        // target_loc.alt = (data.z + 500)/100.0;
+                        // set_target_velocity_NED(target_vel);
+                        mode_guided.set_velocity(target_vel,false,ahrs_yaw_cd,false,0.0f,false,true);
+                        if(zr_serial_api.get_param_debug()){
+
+                            // zr_serial_api.print_data("set vel %.2f,%.2f,%.2f",target_vel.x,target_vel.y,target_vel.z);
+                            snprintf(buf, sizeof(buf), "set vel %.2f,%.2f,%.2f,lock yaw",target_vel.x,target_vel.y,target_vel.z);
+                            zr_serial_api.print_msg(buf);
+                            memset(buf,0,50);
+                        }
+                    }
+                    break;
+                case AC_ZR_Serial_API::ZR_Msg_Type::MSG_CONTROL_MODE:
+                {
+
+                    if(control_mode != Mode::Number::GUIDED){
+                        set_mode(Mode::Number::GUIDED, ModeReason::SCRIPTING);
+                    }
+                    uint8_t user_mode = (data.x);
+                    if(user_mode == 3 || user_mode == 4 || user_mode == 6 || user_mode == 17){
+                        bool change_ok = set_mode(user_mode, ModeReason::SCRIPTING);
+    
+                        if(zr_serial_api.get_param_debug() && change_ok){
+                            // zr_serial_api.print_data("do takeoff,alt %.2f",tk_alt);
+                            snprintf(buf, sizeof(buf), "Change mode %d",user_mode);
+                            zr_serial_api.print_msg(buf);
+                            memset(buf,0,50);
+                        }
+                    }else{
+                        if(zr_serial_api.get_param_debug()){
+                            // zr_serial_api.print_data("do takeoff,alt %.2f",tk_alt);
+                            snprintf(buf, sizeof(buf), "Unsupported mode %d",user_mode);
+                            zr_serial_api.print_msg(buf);
+                            memset(buf,0,50);
+                        }
+                    }
                     
-                        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;
             }

hrs100h.sh

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

libraries/AC_ZR_APP/AC_ZR_Serial_API.cpp

@@ -20,6 +20,7 @@
 #include <string.h>
 #include <stdio.h>
 #include <AP_Math/crc.h>
+#include <AP_Logger/AP_Logger.h>
 
 
 #define ZR_API_DEBUG 1
@@ -44,8 +45,8 @@ const AP_Param::GroupInfo AC_ZR_Serial_API::var_info[] = {
     // @User: zrzk
     AP_GROUPINFO("_SER_DBG",           0,  AC_ZR_Serial_API,  parm_msg_debug,   1),
 
-    AP_GROUPINFO("_SER_DAT_FQ",        1,  AC_ZR_Serial_API,  parm_data_freq,     1 ),
+    AP_GROUPINFO("_SER_DAT_FQ",        1,  AC_ZR_Serial_API,  parm_data_freq,     10 ),
-    AP_GROUPINFO("_SER_STU_FQ",          2,  AC_ZR_Serial_API,  parm_status_freq, 1),
+    AP_GROUPINFO("_SER_STU_FQ",          2,  AC_ZR_Serial_API,  parm_status_freq, 10),
 
     AP_GROUPEND
 };
@@ -84,13 +85,29 @@ void AC_ZR_Serial_API::init(const AP_SerialManager& serial_manager)
     data_trans_init =  init_host;
 }
 
-void AC_ZR_Serial_API::get_param_value(uint8_t &debug,uint8_t &data_freq,uint8_t &status_freq)
+void AC_ZR_Serial_API::print_data( const char *fmt, ...) const
 {
-    debug = parm_msg_debug; 
+    if (!parm_msg_debug) {
-    data_freq = parm_data_freq; 
+        return;
-    status_freq = parm_status_freq; 
+    }
+    gcs().send_text(MAV_SEVERITY_INFO, fmt );
+    // char taggedfmt[51];
+    // // hal.util->snprintf(taggedfmt, sizeof(taggedfmt), "PreArm: %s", fmt);
+    // snprintf(taggedfmt, sizeof(taggedfmt), "%s", fmt);
+    // va_list arg_list;
+    // va_start(arg_list, fmt);
+    // gcs().send_textv(MAV_SEVERITY_CRITICAL, fmt, arg_list);
+    // va_end(arg_list);
 }
 
+
+void AC_ZR_Serial_API::print_msg( const char *msg) const
+{
+    if (!parm_msg_debug) {
+        return;
+    }
+    gcs().send_text(MAV_SEVERITY_INFO, msg );
+}
 /**
  * @brief 数据写入串口
  * 
@@ -196,7 +213,7 @@ void AC_ZR_Serial_API::get_vehicle_status(uint8_t mode,uint8_t in_air,uint32_t h
     if(freq == 0){
         return;
     }else{
-        freq_cnt = 100/freq;
+        freq_cnt = 10/freq;
     }
 
     if(delay_cnt < freq_cnt){
@@ -233,7 +250,7 @@ void AC_ZR_Serial_API::get_vehicle_flight_data(Vector3l pos,Vector3l vel,Vector3
     if(freq == 0){
         return;
     }else{
-        freq_cnt = 100/freq;
+        freq_cnt = 50/freq;
     }
 
     if(delay_cnt < freq_cnt){
@@ -284,30 +301,50 @@ bool AC_ZR_Serial_API::get_control_data(uint8_t mode, ZR_Msg_Type &type,Vector3l
     if(control_data_last_time == last_time){    // 数据没更新，直接退出
         return false;
     }
+    AP::logger().Write("APID", "TimeUS,Mode,Type,X,Y,Z,Yaw", "QBBiiif",
+                            AP_HAL::micros64(), //Q
+                            mode, //B
+                            flight_control_parser.msg.type, //B
+                            flight_control_parser.msg.x,    //i
+                            flight_control_parser.msg.y,    //i
+                            flight_control_parser.msg.z,    //i
+                            flight_control_parser.msg.yaw); //f
+
     last_time = control_data_last_time;
     WITH_SEMAPHORE(sem);
     if(flight_control_parser.msg.head != MSG_HEAD){ // 帧头
+        if(get_param_debug()){
             Debug("head error: %02x",flight_control_parser.msg.head );
+        }
+        
         return false;
     }
     // crc 校验
     crc_sum = crc_crc8(flight_control_parser.data,CONTROL_DATA_LEN-1);
 
     if(crc_sum != flight_control_parser.data[CONTROL_DATA_LEN - 1]){
+        // Debug("crc err: %02x ,clc: %02x",flight_control_parser.msg.crc,crc_sum);
+        if(get_param_debug()){
+
             Debug("crc err: %02x ,clc: %02x",flight_control_parser.msg.crc,crc_sum);
+        }
         set_control_ask(flight_control_parser.msg.msg_id,flight_control_parser.msg.type,ZR_Msg_ASK::MSG_ASK_ERRCRC); // 应答CRC错误
         return false; // 校验失败
     }
     type = flight_control_parser.msg.type;
-    if(type < ZR_Msg_Type::MSG_CONTROL_TKOFF || type > ZR_Msg_Type::MSG_CONTROL_VEL_P ){
+    if(type < ZR_Msg_Type::MSG_CONTROL_TKOFF || type > ZR_Msg_Type::MSG_CONTROL_MODE ){
         set_control_ask(flight_control_parser.msg.msg_id,flight_control_parser.msg.type,ZR_Msg_ASK::MSG_ASK_ERRTYPE); // 应答控制类型错误
-        Debug("type error: %d",(int)flight_control_parser.msg.type);
+        if(get_param_debug()){
 
+            Debug("type error: %d",(int)flight_control_parser.msg.type);
+        }
         return false; // 控制类型错误
     }
-    if(type != ZR_Msg_Type::MSG_CONTROL_TKOFF && mode != 4){
+    if(type != ZR_Msg_Type::MSG_CONTROL_MODE && type != ZR_Msg_Type::MSG_CONTROL_TKOFF && mode != 4 ){
         set_control_ask(flight_control_parser.msg.msg_id,flight_control_parser.msg.type,ZR_Msg_ASK::MSG_ASK_ERRMODE); 
+        if(get_param_debug()){
             Debug("mode error: %d",mode);
+        }
 
         return false; // 飞行模式错误
     }
@@ -367,11 +404,11 @@ void AC_ZR_Serial_API::update(void)
 }
 
 
-namespace AP {
+// namespace AP {
 
-    AC_ZR_Serial_API *zr_data_trans()
+//     AC_ZR_Serial_API *zr_data_trans()
-    {
+//     {
-        return AC_ZR_Serial_API::get_singleton();
+//         return AC_ZR_Serial_API::get_singleton();
-    }
+//     }
 
-}
+// }

libraries/AC_ZR_APP/AC_ZR_Serial_API.h

@@ -45,8 +45,10 @@ public:
         MSG_VEHICLE_STATUS,  //  飞机状态
         MSG_CONTROL_TKOFF , // 飞行控制：起飞
         MSG_CONTROL_POS ,   // 飞行控制：位置控制模式
-        MSG_CONTROL_VEL ,   // 飞行控制：速度控制模式
+        MSG_CONTROL_VEL ,   // 飞行控制：速度控制模式，大地坐标系
-        MSG_CONTROL_VEL_P   // 飞行控制：速度控制模式
+        MSG_CONTROL_VEL_P,   // 飞行控制：速度控制模式
+        MSG_CONTROL_VEL_LOCK_YAW ,   // 悬停，锁定航向
+        MSG_CONTROL_MODE,   // 飞行控制：速度控制模式
         
     };
 
@@ -79,7 +81,9 @@ public:
 
     bool data_trans_init;
 
-    void get_param_value(uint8_t &debug,uint8_t &data_freq,uint8_t &status_freq);
+    uint8_t get_param_debug(){ return parm_msg_debug; };
+    void print_data(const char *fmt, ...)  const;
+    void print_msg(const char *msg)  const;
     
     static const struct AP_Param::GroupInfo var_info[];
 protected:
@@ -205,4 +209,3 @@ private:
     uint16_t bufsize;
 
 };
-    

modules/mavlink

@@ -1 +1 @@
-Subproject commit 86cf7601a72f065f6c2dd8b10a602a73f827ac64
+Subproject commit 959dea22dafbae0e5682cd2191e608b45a7566a9

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-sdk-v4.3.0-dev-FE.px4
+cp ./build/rs100/bin/arducopter.apj  /mnt/f/_01-work/100\=data/固件/编译/rs100f-sdk-v4.3.2-beta.px4