Rover:增加zr_app文件，执行1hz,10hz,50hz循环,

parameter增加zr_app参数
GCS_MAVLink:增加发送设备ID
AC_ZR_App:增加获取版本号、ID号

Rover/GCS_Mavlink.cpp

@@ -1133,3 +1133,15 @@ uint8_t GCS_MAVLINK_Rover::high_latency_wind_direction() const
     return 0;
 }
 #endif // HAL_HIGH_LATENCY2_ENABLED
+
+/**
+ * @brief send device id
+ * @
+ * @return char* 
+ */
+char* GCS_MAVLINK_Rover::gcs_get_sysid_id()
+{
+    static char  buf[50];
+    memcpy(buf,rover.zr_app.get_sysid_board_id(),50);
+    return buf;
+}

Rover/GCS_Mavlink.h

@@ -31,6 +31,8 @@ protected:
     void send_nav_controller_output() const override;
     void send_pid_tuning() override;
     
+    char* gcs_get_sysid_id() override ;
+
 private:
 
     void handleMessage(const mavlink_message_t &msg) override;

Rover/Parameters.cpp

@@ -382,6 +382,7 @@ const AP_Param::Info Rover::var_info[] = {
     // @Path: ../libraries/AP_OSD/AP_OSD.cpp
     GOBJECT(osd, "OSD", AP_OSD),
 #endif
+    GOBJECT(zr_app, "ZR",  AC_ZR_App),
 
     // @Group:
     // @Path: ../libraries/AP_Vehicle/AP_Vehicle.cpp

Rover/Parameters.h

@@ -226,6 +226,8 @@ public:
         // 254,255: reserved
 
         k_param_vehicle = 257, // vehicle common block of parameters
+        
+        k_param_zr_app = 258, // 253 - Logging Group
         };
 
     AP_Int16    format_version;

Rover/Rover.cpp

@@ -129,6 +129,11 @@ const AP_Scheduler::Task Rover::scheduler_tasks[] = {
 #if ADVANCED_FAILSAFE == ENABLED
     SCHED_TASK(afs_fs_check,           10,    200, 129),
 #endif
+
+
+    SCHED_TASK(zr_app_50hz, 50,     200, 132),
+    SCHED_TASK(zr_app_10hz, 10,     200, 135),
+    SCHED_TASK(zr_app_1hz, 1,     200, 138),
 };
 
 

Rover/Rover.h

@@ -37,6 +37,7 @@
 #include <AP_Logger/AP_Logger.h>
 #include <AP_OSD/AP_OSD.h>
 #include <AR_Motors/AP_MotorsUGV.h>
+#include <AC_ZR_APP/AC_ZR_App.h>
 
 #if AP_SCRIPTING_ENABLED
 #include <AP_Scripting/AP_Scripting.h>
@@ -374,6 +375,7 @@ private:
                   "_failsafe_priorities is missing the sentinel");
 
 
+    AC_ZR_App zr_app;
 public:
     void failsafe_check();
     // Motor test
@@ -388,6 +390,10 @@ public:
 
     // Simple mode
     float simple_sin_yaw;
+
+    void zr_app_50hz();
+    void zr_app_10hz();
+    void zr_app_1hz();
 };
 
 extern Rover rover;

Rover/wscript

@@ -25,6 +25,7 @@ def build(bld):
             'AP_WindVane',
             'AR_Motors',
             'AP_Torqeedo',
+            'AC_ZR_APP',
         ],
     )
 

Rover/zr_app.cpp

@@ -0,0 +1,27 @@
+#include "Rover.h"
+
+
+void Rover::zr_app_1hz()
+{
+    static bool before_fly = true;
+    if(arming.is_armed()){
+         if(before_fly){
+            before_fly = false;
+            gcs().send_text(MAV_SEVERITY_INFO,"%s",zr_app.get_sysid_board_id());
+        }
+        relay.on(3);    // 电子开关，保持通路
+        // camera.set_in_arm_mode(true);
+    }else{
+        relay.off(3);
+    }
+
+}
+
+void Rover::zr_app_10hz()
+{
+
+
+}
+void Rover::zr_app_50hz(){
+
+}

libraries/AC_ZR_APP/AC_ZR_App.cpp

@@ -13,7 +13,9 @@
    along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 #include "AC_ZR_App.h"
+#include<stdio.h>
 
+#include <AP_Common/AP_FWVersion.h>
 
 AC_ZR_App *AC_ZR_App::_singleton;
 
@@ -253,3 +255,27 @@ uint8_t AC_ZR_App::get_battery_capacity(uint8_t type,float volt)
     }   
     return bat_cnt;
 }
+
+
+const char* AC_ZR_App::get_sysid_board_id(void)
+{
+    static char  buf[50];
+    uint8_t type = sysid_type;
+    int32_t boad_id =(int32_t) sysid_board_id;
+    switch (type)
+    {
+    case 1:
+        snprintf(buf, sizeof(buf),  "Version: %u.%u.%u ,ID: M%07d",AP::fwversion().major,AP::fwversion().minor,AP::fwversion().patch,(int)boad_id);
+        break;
+    case 2:
+        snprintf(buf, sizeof(buf),  "Version: %u.%u.%u ,ID: H%07d",AP::fwversion().major,AP::fwversion().minor,AP::fwversion().patch,(int)boad_id);
+        break;
+    
+    default:
+        snprintf(buf, sizeof(buf),  "Version: %u.%u.%u ,ID: M%07d",AP::fwversion().major,AP::fwversion().minor,AP::fwversion().patch,(int)boad_id);
+        break;
+    }
+    
+
+    return buf;
+}

libraries/AC_ZR_APP/AC_ZR_App.h

@@ -44,6 +44,7 @@ public:
     uint16_t get_land_deceleration(int32_t alt_cm,int16_t sys_speed_dn);
     uint8_t get_battery_capacity(uint8_t type,float volt);
 
+    const char* get_sysid_board_id(void);
     static const struct AP_Param::GroupInfo var_info[];
 
 protected:

libraries/GCS_MAVLink/GCS.h

@@ -398,6 +398,8 @@ public:
 
     MAV_RESULT set_message_interval(uint32_t msg_id, int32_t interval_us);
 
+    virtual char* gcs_get_sysid_id() { return nullptr; }
+    
 protected:
 
     bool mavlink_coordinate_frame_to_location_alt_frame(MAV_FRAME coordinate_frame,

libraries/GCS_MAVLink/GCS_Common.cpp

@@ -3957,6 +3957,7 @@ void GCS_MAVLINK::send_banner()
         send_text(MAV_SEVERITY_INFO, "%s", banner_msg);
     }
     
+   send_text(MAV_SEVERITY_INFO, " %s", gcs_get_sysid_id());
 #if HAL_INS_ENABLED
     // output any fast sampling status messages
     for (uint8_t i = 0; i < INS_MAX_BACKENDS; i++) {