add sysid

ArduCopter/Copter.h

@@ -1003,6 +1003,10 @@ private:
 public:
     void mavlink_delay_cb();    // GCS_Mavlink.cpp
     void failsafe_check();      // failsafe.cpp
+
+    const char* get_sysid_board_id(void); // modify by @Brown
+    const char* get_sysid_board_name_and_id(void); // modify by @Binsir
+    const char*    parse_sysid_board_name(void); // modify by @Binsir
 };
 
 extern Copter copter;

ArduCopter/GCS_Mavlink.cpp

@@ -244,6 +244,13 @@ bool GCS_MAVLINK_Copter::sysid_enforce() const
     return copter.g2.sysid_enforce;
 }
 
+char* GCS_MAVLINK_Copter::gcs_get_sysid_id() const
+{
+    static char  buf[50];
+    memcpy(buf,copter.get_sysid_board_id(),50);
+    return buf;
+}
+
 uint32_t GCS_MAVLINK_Copter::telem_delay() const
 {
     return (uint32_t)(copter.g.telem_delay);

ArduCopter/GCS_Mavlink.h

@@ -16,6 +16,7 @@ protected:
     MAV_RESULT handle_flight_termination(const mavlink_command_long_t &packet) override;
 
     uint8_t sysid_my_gcs() const override;
+    char* gcs_get_sysid_id() const override;
     bool sysid_enforce() const override;
 
     bool params_ready() const override;

ArduCopter/Parameters.cpp

@@ -43,6 +43,25 @@ const AP_Param::Info Copter::var_info[] = {
     // @ReadOnly: True
     GSCALAR(format_version, "SYSID_SW_MREV",   0),
 
+    // @Param: BOARD_ID
+    // @DisplayName: Board ID
+    // @Description: For mark board id
+    // @User: @Brown
+    GSCALAR(sysid_board_id,   "SYSID_BOARD_ID",     100),
+
+    // @Param: SYSID_BD_NAME1
+    // @DisplayName: Board Name 1st
+    // @Description: For mark board name first
+    // @Range: 1-4294967269
+    // @User: @Binsir
+    GSCALAR(sysid_board_name_1st,   "SYSID_BDNAME_P1",   12848 ),
+
+    // @Param: SYSID_BD_NAME2
+    // @DisplayName: Board Name 2nd
+    // @Description: For mark board name last
+  // @User: @Binsir
+    GSCALAR(sysid_board_name_2nd,   "SYSID_BDNAME_P2",     5329970),
+
     // @Param: SYSID_THISMAV
     // @DisplayName: MAVLink system ID of this vehicle
     // @Description: Allows setting an individual MAVLink system id for this vehicle to distinguish it from others on the same network
@@ -1594,3 +1613,26 @@ void Copter::convert_fs_options_params(void)
     // AP_Param::set_and_save_by_name("FS_OPTIONS", fs_options_converted);
     fs_opt->set_and_save(fs_options_converted);
 }
+
+
+// modify by @Brown
+const char* Copter::get_sysid_board_id(void)
+{
+
+    static char  buf[50];
+    // snprintf(buf, sizeof(buf),  "Version: v3.5.6 ,Board ID: ZRZK.19QT3.%d",(int)g.sysid_board_id);
+
+     int32_t nameValue1 =(int32_t) g.sysid_board_name_1st;
+     int32_t nameValue2 = (int32_t)g.sysid_board_name_2nd;   
+     char  name[6] = {' ',' ',' ',' ',' ','.'};   
+    //  memset(name,0,6);
+     nameValue2 = nameValue2 & 0xffffff;
+    //  name[5] = "a";
+     name[4] = nameValue2&0xFF;
+     name[3] = nameValue2>>8&0xFF; 
+     name[2] = nameValue2>>16& 0xFF;   
+     name[1] = nameValue1&0xFF; 
+     name[0] = nameValue1>>8 & 0xFF;  
+    snprintf(buf, sizeof(buf),  "Version: zr-v4.0 ,Board ID: ZRZK.%5s%d",name,(int)g.sysid_board_id);
+    return buf;
+}

ArduCopter/Parameters.h

@@ -202,6 +202,10 @@ public:
         k_param_circle_nav,
         k_param_loiter_nav,     // 105
 
+        k_param_sysid_board_name_1st =106, //add by @Binsir
+        k_param_sysid_board_name_2nd=107, //add by @Binsir
+        k_param_sysid_board_id=108, // modify by @Brown
+
         // 110: Telemetry control
         //
         k_param_gcs0 = 110,
@@ -379,6 +383,10 @@ public:
 
     AP_Int16        format_version;
 
+    AP_Int16        sysid_board_id; // modify by @Brown
+    AP_Int32        sysid_board_name_1st;// modify by @Binsir
+    AP_Int32         sysid_board_name_2nd;// modify by @Binsir
+
     // Telemetry control
     //
     AP_Int16        sysid_this_mav;

ArduCopter/UserCode.cpp

@@ -64,6 +64,21 @@ void Copter::userhook_SuperSlowLoop()
     // {
     //     camera.set_trigger_distance(0);  //  @Brown , stop take photos
     // }
+
+    //     float proximity_alt_diff;
+    // AP_Proximity *proximity = AP::proximity();
+    // if (proximity && proximity->get_horizontal_distance(0,proximity_alt_diff)) {
+
+    //     if(proximity_alt_diff < 40.0)
+    //     {
+
+    //         if(control_mode != Mode::Number::BRAKE)
+    //             gcs().send_text(MAV_SEVERITY_INFO, "Dist: %f, stop",proximity_alt_diff);
+    //         control_mode = Mode::Number::BRAKE;
+            
+    //     }
+
+    // }
 }
 #endif
 

ArduCopter/mode_auto.cpp

@@ -57,6 +57,21 @@ bool ModeAuto::init(bool ignore_checks)
 void ModeAuto::run()
 {
     // call the correct auto controller
+
+
+    //     float proximity_alt_diff;
+    // AP_Proximity *proximity = AP::proximity();
+    // if (proximity && proximity->get_horizontal_distance(0,proximity_alt_diff)) {
+
+    //     if(proximity_alt_diff < 30.0)
+    //     {
+    //         if(_mode != Auto_Loiter)
+    //             gcs().send_text(MAV_SEVERITY_INFO, "Dist: %f, stop",proximity_alt_diff);
+
+    //         _mode = Auto_Loiter;
+            
+    //     }
+    // }
     switch (_mode) {
 
     case Auto_TakeOff:

ArduCopter/version.cpp

@@ -29,6 +29,7 @@ const AP_FWVersion AP_FWVersion::fwver{
     .fw_hash_str = "",
 #else
     .fw_string = THISFIRMWARE " (" GIT_VERSION ")",
+    // .fw_string = copter.get_sysid_board_id(),
     .fw_hash_str = GIT_VERSION,
 #endif
     .middleware_name = nullptr,

Tools/autotest/locations.txt

@@ -1,6 +1,7 @@
 #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,20,0
+CELIU=24.6054997,118.0614525,1,0
 OSRF0_PILOTSBOX=37.4003371,-122.0800351,2,270
 CMAC=-35.363261,149.165230,584,353
 CMAC2=-35.362889,149.165221,584,270

libraries/GCS_MAVLink/GCS.h

@@ -26,7 +26,7 @@
 
 // check if a message will fit in the payload space available
 #define PAYLOAD_SIZE(chan, id) (GCS_MAVLINK::packet_overhead_chan(chan)+MAVLINK_MSG_ID_ ## id ## _LEN)
-#define HAVE_PAYLOAD_SPACE(chan, id) (comm_get_txspace(chan) >= PAYLOAD_SIZE(chan, id))
+#define HAVE_PAYLOAD_SPACE(chan, id) (comm_get_txspace(chan) >= PAYLOAD_SIZE(chan, id)     )
 #define CHECK_PAYLOAD_SIZE(id) if (comm_get_txspace(chan) < packet_overhead()+MAVLINK_MSG_ID_ ## id ## _LEN) return false
 #define CHECK_PAYLOAD_SIZE2(id) if (!HAVE_PAYLOAD_SPACE(chan, id)) return false
 
@@ -115,6 +115,8 @@ public:
     AP_HAL::UARTDriver *get_uart() { return _port; }
 
     virtual uint8_t sysid_my_gcs() const = 0;
+    virtual char* gcs_get_sysid_id() const = 0;
+
     virtual bool sysid_enforce() const { return false; }
 
     void send_parameter_value(const char *param_name,

libraries/GCS_MAVLink/GCS_Common.cpp

@@ -3259,6 +3259,7 @@ void GCS_MAVLINK::handle_send_autopilot_version(const mavlink_message_t &msg)
 
 void GCS_MAVLINK::send_banner()
 {
+#if 0
     // mark the firmware version in the tlog
     const AP_FWVersion &fwver = AP::fwversion();
 
@@ -3278,6 +3279,13 @@ void GCS_MAVLINK::send_banner()
     if (hal.util->get_system_id(sysid)) {
         send_text(MAV_SEVERITY_INFO, "%s", sysid);
     }
+#else
+    send_text(MAV_SEVERITY_INFO, gcs_get_sysid_id());
+    char sysid[40];
+    if (hal.util->get_system_id(sysid)) {
+        send_text(MAV_SEVERITY_INFO, sysid);
+    }
+#endif
 }
 
 

libraries/GCS_MAVLink/GCS_Dummy.h

@@ -38,6 +38,7 @@ private:
 protected:
 
     uint8_t sysid_my_gcs() const override { return 1; }
+    char* gcs_get_sysid_id() const override { return " "; }
  
     // dummy information:
     MAV_MODE base_mode() const override { return (MAV_MODE)MAV_MODE_FLAG_CUSTOM_MODE_ENABLED; }