修改状态码mavlink发送为主流方式,降落减少发送

APMrover2/GCS_Mavlink.h

@@ -20,6 +20,7 @@ protected:
     MAV_RESULT handle_command_long_packet(const mavlink_command_long_t &packet) override;
 
     void send_position_target_global_int() override;
+    void send_zr_flying_status() override;
 
     virtual bool in_hil_mode() const override;
 

ArduCopter/GCS_Mavlink.cpp

@@ -111,6 +111,19 @@ void GCS_MAVLINK_Copter::send_position_target_global_int()
         0.0f); // yaw_rate
 }
 
+void GCS_MAVLINK_Copter::send_zr_flying_status(){
+
+     if (!copter.ap.initialised) {
+        return;
+    }
+   
+    mavlink_msg_zr_flying_status_send(
+        chan,
+        (uint8_t)copter.updownStatus,
+        0//TODO  change this param
+    );
+}
+
 void GCS_MAVLINK_Copter::send_position_target_local_ned()
 {
 #if MODE_GUIDED_ENABLED == ENABLED

ArduCopter/GCS_Mavlink.h

@@ -27,6 +27,8 @@ protected:
     void send_position_target_global_int() override;
     void send_position_target_local_ned() override;
 
+    void send_zr_flying_status() override;
+    
     MAV_RESULT handle_command_do_set_roi(const Location &roi_loc) override;
     MAV_RESULT handle_preflight_reboot(const mavlink_command_long_t &packet) override;
     MAV_RESULT handle_command_mount(const mavlink_command_long_t &packet) override;

ArduCopter/UserCode.cpp

@@ -79,6 +79,8 @@ static float batt_mah_teb[] =
     19.88,19.73,19.53,19.34,19.19,19.00,18.80,18.51,18.00,17.40,
     16.80
 };
+
+//uint8_t flying_status_count =0;
 void Copter::userhook_SuperSlowLoop()
 {
     // put your 1Hz code here
@@ -96,10 +98,6 @@ void Copter::userhook_SuperSlowLoop()
         relay.off(3);
         // rc().set_enable_ch();
         updownStatus =UpDown_TakeOffStart;
-        mavlink_zr_flying_status_t  zr_flying_status_t; 
-        zr_flying_status_t.updown_status = updownStatus;
-        //TODO UPDATE COUNTDOWN
-        gcs().update_zr_fly_status(&zr_flying_status_t);
     }
     if(before_fly){
         uint8_t cnt,cacl_volt_pst;
@@ -114,6 +112,13 @@ void Copter::userhook_SuperSlowLoop()
         battery.reset_remaining(1, cacl_volt_pst);
     } 
  
+    //2s发送一个状态码
+   // flying_status_count++;
+   // if(flying_status_count>=2){
+   //     flying_status_count =0;
+        gcs().send_message(MSG_ZR_FLYING_STATUS);
+  //  }
+
 }
 #endif
 

ArduCopter/mode.cpp

@@ -824,10 +824,7 @@ void Copter::update_updownStatus2(uint8_t status)
     if (status == 3)
     {        
         set_mode(Mode::Number::AUTO, ModeReason::REQUEST_CMD);
-         mavlink_zr_flying_status_t  zr_flying_status_t;
+         gcs().send_message(MSG_ZR_FLYING_STATUS);
-        zr_flying_status_t.updown_status = copter.updownStatus;
-        zr_flying_status_t.countdown = 255;
-        gcs().update_zr_fly_status(&zr_flying_status_t);
          gcs().send_text(MAV_SEVERITY_WARNING,"NOTICE: 执行继续任务");
     }
 }

ArduCopter/mode_auto.cpp

@@ -238,10 +238,7 @@ void ModeAuto::wp_start(const Location& dest_loc)
     if (copter.updownStatus < UpDown_InMission)
     {
         copter.updownStatus = UpDown_InMission;
-        mavlink_zr_flying_status_t  zr_flying_status_t;
+        gcs().send_message(MSG_ZR_FLYING_STATUS);
-        zr_flying_status_t.updown_status = copter.updownStatus;
-        zr_flying_status_t.countdown = 255;
-        gcs().update_zr_fly_status(&zr_flying_status_t);
     }
 }
 
@@ -774,10 +771,9 @@ void ModeAuto::takeoff_run()
             {
                 copter.updownStatus = UpDown_RequestClimb;  
                 countdown = g.zr_tk_delay; 
-                mavlink_zr_flying_status_t  zr_flying_status_t;
+
-                zr_flying_status_t.updown_status = copter.updownStatus;
+                 gcs().send_message(MSG_ZR_FLYING_STATUS);  
-                zr_flying_status_t.countdown = countdown;
+
-                gcs().update_zr_fly_status(&zr_flying_status_t);          
                 last_takeoff_request_time = AP_HAL::millis();             
                 if(g.zr_tk_delay>0){
                     is_takeoff_delay_enable =true;
@@ -786,16 +782,14 @@ void ModeAuto::takeoff_run()
                 }
              set_mode(Mode::Number::BRAKE, ModeReason::REQUEST_CMD);
             } 
+
             else if (g.zr_tk_req_alt > 0 && alt_rev < g.zr_tk_req_alt 
                      && alt_rev > 0 
-                    &&(AP_HAL::millis()-last_time_send_updownload>500))
+                    &&(AP_HAL::millis()-last_time_send_updownload>1000))
             {
                 last_time_send_updownload = AP_HAL::millis();
                 copter.updownStatus = UpDown_TakeOffClimb;     
-                mavlink_zr_flying_status_t  zr_flying_status_t;
+              // gcs().send_message(MSG_ZR_FLYING_STATUS); //TODO
-                zr_flying_status_t.updown_status = copter.updownStatus;
-                zr_flying_status_t.countdown = 255;
-                gcs().update_zr_fly_status(&zr_flying_status_t);
             }     
         }   
     }

ArduCopter/mode_brake.cpp

@@ -82,10 +82,6 @@ void ModeBrake::run()
                 {
                     last_takeoff_request_time =  AP_HAL::millis();    
                      
-                    // mavlink_zr_flying_status_t  zr_flying_status_t;
-                    // zr_flying_status_t.updown_status = copter.updownStatus;
-                    // zr_flying_status_t.countdown = countdown;  
-                    // gcs().update_zr_fly_status(&zr_flying_status_t);
                     if(countdown>0){
                          gcs().send_text(MAV_SEVERITY_INFO,"NOTICE: 继续任务倒计时(秒)：%d",countdown);
                     }               

ArduCopter/mode_rtl.cpp

@@ -70,11 +70,8 @@ void ModeRTL::run(bool disarm_on_land)
             if (copter.updownStatus < UpDown_RequestDescent)
             {
                 copter.updownStatus = UpDown_RequestDescent;
-                mavlink_zr_flying_status_t  zr_flying_status_t;
-                zr_flying_status_t.updown_status = copter.updownStatus;
                countdown = g.zr_rtl_delay;  
-                zr_flying_status_t.countdown = countdown;  
+               gcs().send_message(MSG_ZR_FLYING_STATUS);
-                gcs().update_zr_fly_status(&zr_flying_status_t);
                 last_decent_time =  AP_HAL::millis();       
                 if(g.zr_rtl_delay>0){
                     is_rtl_delay_enable = true;
@@ -86,16 +83,13 @@ void ModeRTL::run(bool disarm_on_land)
             {
                  if((AP_HAL::millis()- last_decent_time)>=1000){
                     last_decent_time =  AP_HAL::millis();                     
-                    // mavlink_zr_flying_status_t  zr_flying_status_t;
-                    // zr_flying_status_t.updown_status = copter.updownStatus;
-                    // zr_flying_status_t.countdown = countdown;  
-                    // gcs().update_zr_fly_status(&zr_flying_status_t);
                     if(countdown>0){
                         gcs().send_text(MAV_SEVERITY_INFO ,"NOTICE: 着陆倒计时(秒): %d",countdown);
                     }
                     else if(countdown<=0){
                        gcs().send_text(MAV_SEVERITY_INFO,"NOTICE: 开始着陆");
                         copter.updownStatus = UpDown_ContinueDescent;
+                         gcs().send_message(MSG_ZR_FLYING_STATUS);
                         _state = RTL_Land;
                     }
                        countdown --; //TODO  shiji countdown+1 s
@@ -296,10 +290,7 @@ void ModeRTL::loiterathome_run()
 void ModeRTL::descent_start()
 {
     copter.updownStatus  = UpDown_RTLDescent;
-   mavlink_zr_flying_status_t  zr_flying_status_t;
+     gcs().send_message(MSG_ZR_FLYING_STATUS);
-    zr_flying_status_t.updown_status = copter.updownStatus;
-    zr_flying_status_t.countdown = 255;
-    gcs().update_zr_fly_status(&zr_flying_status_t);
     _state = RTL_FinalDescent;
     _state_complete = false;
 
@@ -439,13 +430,6 @@ void ModeRTL::land_run(bool disarm_on_land)
 
     land_run_horizontal_control();
     land_run_vertical_control();  
-    
-    if(AP_HAL::millis()-last_send_time>=500){
-         mavlink_zr_flying_status_t  zr_flying_status_t;
-        zr_flying_status_t.updown_status = copter.updownStatus;
-        zr_flying_status_t.countdown = 255;
-        gcs().update_zr_fly_status(&zr_flying_status_t);
-    }
 }
 
 void ModeRTL::build_path()

ArduPlane/GCS_Mavlink.h

@@ -25,7 +25,7 @@ protected:
     MAV_RESULT handle_command_long_packet(const mavlink_command_long_t &packet) override;
 
     void send_position_target_global_int() override;
-
+    void send_zr_flying_status() override;
     virtual bool in_hil_mode() const override;
 
     void send_attitude() const override;

libraries/AP_BattMonitor/AP_BattMonitor_Serial_BattGo.cpp

@@ -279,7 +279,7 @@ void AP_BattMonitor_Serial_BattGo::read()
 
 void AP_BattMonitor_Serial_BattGo::notice_to_send_battgo_param()
 {
-    gcs().send_message(MSG_BATTGAO_INFO);
+    gcs().send_message(MSG_BATTGO_INFO);
 }
 
 void AP_BattMonitor_Serial_BattGo::request_info()

libraries/GCS_MAVLink/GCS.h

@@ -211,6 +211,7 @@ public:
     void send_gps_global_origin() const;
     virtual void send_position_target_global_int() { };
     virtual void send_position_target_local_ned() { };
+    virtual void send_zr_flying_status(){};
     void send_servo_output_raw();
     void send_accelcal_vehicle_position(uint32_t position);
     void send_scaled_imu(uint8_t instance, void (*send_fn)(mavlink_channel_t chan, uint32_t time_ms, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag, int16_t temperature));
@@ -219,7 +220,6 @@ public:
     void send_rpm() const;
     bool send_battgo_info(const mavlink_battgo_info_t *mavlink_battgo_info_t);
     bool send_battgo_history(const mavlink_battgo_history_t *mavlink_battgo_info_t);
-    bool send_zr_flying_status(const mavlink_zr_flying_status_t *zr_flying_status_t);
     //void send_serial_battgo(mavlink_battgo_info_t &battgo_info_t );
     bool locked() const;
 
@@ -838,7 +838,6 @@ public:
 
    void   update_serial_battgo_info(mavlink_battgo_info_t *battgo_info_t );
    void   update_serial_battgo_history(mavlink_battgo_history_t *battgo_history_t );
-   void   update_zr_fly_status(mavlink_zr_flying_status_t *zr_flying_status_t );
     // minimum amount of time (in microseconds) that must remain in
     // the main scheduler loop before we are allowed to send any
     // mavlink messages.  We want to prioritise the main flight

libraries/GCS_MAVLink/GCS_Common.cpp

@@ -256,13 +256,13 @@ bool GCS_MAVLINK::send_battery_status() const
 }
 bool GCS_MAVLINK::send_battgo_info(const mavlink_battgo_info_t *battgo_info_t)
 {
-    mavlink_msg_battgo_info_send_struct(chan, battgo_info_t);
+    mavlink_msg_battgo_info_send_struct(chan, battgo_info_t); //TODO
     return true;
 }
 
 bool GCS_MAVLINK::send_battgo_history(const mavlink_battgo_history_t *battgo_history_t)
 {
-    mavlink_msg_battgo_history_send_struct(chan, battgo_history_t);
+   mavlink_msg_battgo_history_send_struct(chan, battgo_history_t);//TODO
     return true;
 }
 void GCS_MAVLINK::send_distance_sensor(const AP_RangeFinder_Backend *sensor, const uint8_t instance) const
@@ -795,6 +795,7 @@ ap_message GCS_MAVLINK::mavlink_id_to_ap_message_id(const uint32_t mavlink_id) c
         { MAVLINK_MSG_ID_DEEPSTALL,             MSG_LANDING},
         { MAVLINK_MSG_ID_EXTENDED_SYS_STATE,    MSG_EXTENDED_SYS_STATE},
         { MAVLINK_MSG_ID_AUTOPILOT_VERSION,     MSG_AUTOPILOT_VERSION},
+         { MAVLINK_MSG_ID_ZR_FLYING_STATUS,     MSG_ZR_FLYING_STATUS},
             };
 
     for (uint8_t i=0; i<ARRAY_SIZE(map); i++) {
@@ -1924,14 +1925,6 @@ void GCS::update_serial_battgo_history(mavlink_battgo_history_t *battgo_history_
     }          
 }
 
-void GCS::update_zr_fly_status(mavlink_zr_flying_status_t *zr_flying_status_t )
-{
-    for (uint8_t i=0; i<num_gcs(); i++) {
-    //  mavlink_msg_battgo_info_send_struct(chan,&battgo_info_t);
-    chan(i)->send_zr_flying_status(zr_flying_status_t);
-    }          
-}
-
 void GCS::send_mission_item_reached_message(uint16_t mission_index)
 {
     for (uint8_t i=0; i<num_gcs(); i++) {
@@ -4491,6 +4484,11 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id)
         send_autopilot_version();
         break;
 
+        case MSG_ZR_FLYING_STATUS:
+         CHECK_PAYLOAD_SIZE(ZR_FLYING_STATUS);
+         send_zr_flying_status();
+         break;
+
     case MSG_ESC_TELEMETRY: {
 #ifdef HAVE_AP_BLHELI_SUPPORT
         CHECK_PAYLOAD_SIZE(ESC_TELEMETRY_1_TO_4);
@@ -4836,12 +4834,6 @@ void GCS_MAVLINK::handle_zr_fly_status(const mavlink_message_t &msg)
     }
 }
 
-bool GCS_MAVLINK::send_zr_flying_status(const mavlink_zr_flying_status_t *zr_flying_status_t)
-{
-    mavlink_msg_zr_flying_status_send_struct(chan, zr_flying_status_t);
-    return true;
-}
-
 GCS &gcs()
 {
     return *GCS::get_singleton();

libraries/GCS_MAVLink/ap_message.h

@@ -73,6 +73,9 @@ enum ap_message : uint8_t {
     MSG_NAMED_FLOAT,
     MSG_EXTENDED_SYS_STATE,
     MSG_AUTOPILOT_VERSION,
-    MSG_BATTGAO_INFO,
+    MSG_ZR_FLYING_STATUS,
+    MSG_ZR_CAMERA_STATUS,
+    MSG_BATTGO_INFO,
+    MSG_BATTGO_HISTORY,
     MSG_LAST // MSG_LAST must be the last entry in this enum
 };