断点续飞调整测试

libraries/AP_Camera/AP_Camera.cpp

@@ -338,17 +338,27 @@ void AP_Camera::send_feedback(mavlink_channel_t chan)
         altitude_rel = current_loc.alt - ahrs.get_home().alt;
     }
 
+    ant_ned = camera_pos_rotation(ant_offset);
     mavlink_msg_camera_feedback_send(
         chan,
         AP::gps().time_epoch_usec(),
         0, 0, _image_index,
         current_loc.lat, current_loc.lng,
         altitude*1e-2f, altitude_rel*1e-2f,
-        ahrs.roll_sensor*1e-2f, ahrs.pitch_sensor*1e-2f, ahrs.yaw_sensor*1e-2f,
+        ant_ned.x, ant_ned.y, ant_ned.z,
         0.0f, CAMERA_FEEDBACK_PHOTO, _camera_trigger_logged);
 }
 
+Vector3f AP_Camera::camera_pos_rotation(Vector3f offset)
+{
+    Matrix3f Tbn;                               // dcm rotation matrix to rotate body frame to north
+    Vector3f cam_pos_ned;
 
+    const AP_AHRS &ahrs = AP::ahrs();
+    Tbn = ahrs.get_rotation_body_to_ned();
+    cam_pos_ned = Tbn * (offset);
+    return cam_pos_ned;
+}
 /*  update; triggers by distance moved
 */
 void AP_Camera::update()
@@ -390,6 +400,7 @@ void AP_Camera::update()
     take_picture();
 
     _last_location = current_loc;
+    ant_ned = camera_pos_rotation(ant_offset);
     _last_photo_time = tnow;
 }
 
@@ -402,6 +413,7 @@ void AP_Camera::feedback_pin_isr(uint8_t pin, bool high, uint32_t timestamp_us)
     if(timestamp_us - last_timestamp_us > _min_time_interval * 1000){
         last_timestamp_us = timestamp_us;
         _feedback_timestamp_us = timestamp_us;
+        ant_ned = camera_pos_rotation(ant_offset);
         _camera_trigger_count++;
     }
 }
@@ -416,6 +428,7 @@ void AP_Camera::feedback_pin_timer(void)
     if (pin_state == trigger_polarity &&
         _last_pin_state != trigger_polarity) {
         _feedback_timestamp_us = AP_HAL::micros();
+        ant_ned = camera_pos_rotation(ant_offset);
         _camera_trigger_count++;
     }
     _last_pin_state = pin_state;
@@ -460,14 +473,14 @@ void AP_Camera::log_picture()
     if (!using_feedback_pin()) {
         gcs().send_message(MSG_CAMERA_FEEDBACK);
         if (logger->should_log(log_camera_bit)) {
-            logger->Write_Camera(current_loc);
+            logger->Write_Camera(current_loc,ant_ned);
             last_pos_lat = current_loc.lat;
             last_pos_lng = current_loc.lng;
             _image_index_log++;
         }
     } else {
         if (logger->should_log(log_camera_bit)) {
-            logger->Write_Trigger(current_loc);
+            logger->Write_Trigger(current_loc,ant_ned);
             last_pos_lat = current_loc.lat;
             last_pos_lng = current_loc.lng;
             _image_index_log++;
@@ -507,7 +520,7 @@ void AP_Camera::update_trigger()
             if (logger->should_log(log_camera_bit)) {
                 uint32_t tdiff = AP_HAL::micros() - timestamp32;
                 uint64_t timestamp = AP_HAL::micros64();
-                logger->Write_Camera(current_loc, timestamp - tdiff);
+                logger->Write_Camera(current_loc,ant_ned, timestamp - tdiff);
             }
         }
     }

libraries/AP_Camera/AP_Camera.h

@@ -138,6 +138,7 @@ public:
         float yaw;
         uint8_t mode;
     };
+    Vector3f camera_pos_rotation(Vector3f offset);
    struct Camera_State  &get_camera_state(){
        return camera_state;
    }
@@ -215,6 +216,8 @@ private:
     float _camera_yaw;     // 航线传入相机姿态yaw
    // void send_can_msg(void);
    bool new_att;
+   Vector3f ant_offset = {0.02,0.225,0.17};
+   Vector3f ant_ned;
 };
 namespace AP {
 AP_Camera *camera();

libraries/AP_Logger/AP_Logger.h

@@ -257,9 +257,9 @@ public:
     void Write_Radio(const mavlink_radio_t &packet);
     void Write_Message(const char *message);
     void Write_MessageF(const char *fmt, ...);
-    void Write_CameraInfo(enum LogMessages msg, const Location &current_loc, uint64_t timestamp_us=0);
-    void Write_Camera(const Location &current_loc, uint64_t timestamp_us=0);
-    void Write_Trigger(const Location &current_loc);
+    void Write_CameraInfo(enum LogMessages msg, const Location &current_loc,Vector3f offset, uint64_t timestamp_us=0);
+    void Write_Camera(const Location &current_loc, Vector3f offset, uint64_t timestamp_us=0);
+    void Write_Trigger(const Location &current_loc, Vector3f offset);
     void Write_ESC(uint8_t id, uint64_t time_us, int32_t rpm, uint16_t voltage, uint16_t current, int16_t temperature, uint16_t current_tot);
     void Write_Attitude(const Vector3f &targets);
     void Write_AttitudeView(AP_AHRS_View &ahrs, const Vector3f &targets);

libraries/AP_Logger/LogFile.cpp

@@ -554,7 +554,7 @@ void AP_Logger::Write_Radio(const mavlink_radio_t &packet)
 }
 
 // Write a Camera packet
-void AP_Logger::Write_CameraInfo(enum LogMessages msg, const Location &current_loc, uint64_t timestamp_us)
+void AP_Logger::Write_CameraInfo(enum LogMessages msg, const Location &current_loc, Vector3f offset, uint64_t timestamp_us)
 {
     const AP_AHRS &ahrs = AP::ahrs();
 
@@ -583,23 +583,26 @@ void AP_Logger::Write_CameraInfo(enum LogMessages msg, const Location &current_l
         altitude    : altitude,
         altitude_rel: altitude_rel,
         altitude_gps: altitude_gps,
-        roll        : (int16_t)ahrs.roll_sensor,
-        pitch       : (int16_t)ahrs.pitch_sensor,
-        yaw         : (uint16_t)ahrs.yaw_sensor
+        // roll        : (int16_t)ahrs.roll_sensor,
+        // pitch       : (int16_t)ahrs.pitch_sensor,
+        // yaw         : (uint16_t)ahrs.yaw_sensor
+        roll        : (int16_t)(offset.x * 100),
+        pitch       : (int16_t)(offset.y * 100),
+        yaw         : (uint16_t)(offset.z * 100)
     };
     WriteCriticalBlock(&pkt, sizeof(pkt));
 }
 
 // Write a Camera packet
-void AP_Logger::Write_Camera(const Location &current_loc, uint64_t timestamp_us)
+void AP_Logger::Write_Camera(const Location &current_loc, Vector3f offset, uint64_t timestamp_us)
 {
-    Write_CameraInfo(LOG_CAMERA_MSG, current_loc, timestamp_us);
+    Write_CameraInfo(LOG_CAMERA_MSG, current_loc, offset, timestamp_us);
 }
 
 // Write a Trigger packet
-void AP_Logger::Write_Trigger(const Location &current_loc)
+void AP_Logger::Write_Trigger(const Location &current_loc, Vector3f offset)
 {
-    Write_CameraInfo(LOG_TRIGGER_MSG, current_loc, 0);
+    Write_CameraInfo(LOG_TRIGGER_MSG, current_loc, offset, 0);
 }
 
 // Write an attitude packet

libraries/AP_Mission/AP_Mission.cpp

@@ -2062,14 +2062,14 @@ void AP_Mission::auto_resume_mission(uint8_t f_mode, bool is_arm,int32_t r_lat,i
     if(f_mode == 3 && need_change_cmd == 0)
     {
         need_change_cmd = 1;
-        // gcs_send_text_fmt(MAV_SEVERITY_DEBUG, "enter resume dec");
+        gcs().send_text(MAV_SEVERITY_DEBUG, "enter resume dec");
 
     }else if(need_change_cmd == 1 && _auto_resume && 6 == f_mode)
     {
         cmd_rtl_index = _nav_cmd.index;
 
         cmd_rtl_index -= 1;
-        // gcs_send_text_fmt(MAV_SEVERITY_DEBUG, "resume %d",cmd_rtl_index);
+        gcs().send_text(MAV_SEVERITY_DEBUG, "resume %d",cmd_rtl_index);
         if(cmd_rtl_index < 5 ||  num_commands() - cmd_rtl_index < 4)// 飛行的航點小於6則直接推出，否則讀取當前航點；飞完所有航点，也直接退出
         {
 		    need_change_cmd = 0;
@@ -2083,7 +2083,7 @@ void AP_Mission::auto_resume_mission(uint8_t f_mode, bool is_arm,int32_t r_lat,i
         {
             read_cmd_from_storage(i,cmd_rtl);   // 读取原本航点
 
-            // gcs_send_text_fmt(MAV_SEVERITY_DEBUG, "read[%d]: %d",i,cmd_rtl.id);
+            gcs().send_text(MAV_SEVERITY_DEBUG, "read[%d]: %d",i,cmd_rtl.id);
             if (is_nav_cmd(cmd_rtl)) {   // 是否是Waypoint
                 break;
             }
@@ -2106,14 +2106,15 @@ void AP_Mission::auto_resume_mission(uint8_t f_mode, bool is_arm,int32_t r_lat,i
 
     
     // 飞机上锁，则开始改写航点
-    if (!is_arm && need_change_cmd == 2) {
+    // if (!is_arm && need_change_cmd == 2) {
+    if (f_mode == 1  && need_change_cmd == 2) { // 测试用,模式ACRO重写航点
         cmd_total = num_commands();
         // gcs_send_text_fmt(MAV_SEVERITY_DEBUG, "[Resume] --- Star replace wp ,total:%d ---",cmd_total);
 
         for(cmd_i = 1; cmd_i < cmd_total; cmd_i++)
         {
             read_cmd_from_storage(cmd_i,cmd_temp);
-                // gcs_send_text_fmt(MAV_SEVERITY_DEBUG, "[%d] step:%d, id:%d ",cmd_i,cmd_step ,cmd_temp.id);
+                gcs().send_text(MAV_SEVERITY_DEBUG, "[%d] step:%d, id:%d ",cmd_i,cmd_step ,cmd_temp.id);
             switch (cmd_step)
             {
                 // gcs_send_text_fmt(MAV_SEVERITY_INFO, "[d]t:%d,i:%d \n ",cmd_step,cmd_rtl_i);
@@ -2144,7 +2145,7 @@ void AP_Mission::auto_resume_mission(uint8_t f_mode, bool is_arm,int32_t r_lat,i
 		  			}
 		  			break;
 	  			case 2:
-		  			if(cmd_temp.id == MAV_CMD_DO_SET_CAM_TRIGG_DIST)
+		  			if(cmd_temp.id == MAV_CMD_DO_SET_CAM_TRIGG_DIST || cmd_temp.id ==  MAV_CMD_DO_GIMBAL_CONTROL)
 					{
                         // cmd_rtl_i = cmd_i;
 		  				cmd_step = 3;

libraries/AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp

@@ -186,7 +186,7 @@ void NavEKF2_core::calcGpsGoodToAlign(void)
     }
 
     // fail if satellite geometry is poor
-    bool hdopFail = (gps.get_hdop() > 250)  && (frontend->_gpsCheck & MASK_GPS_HDOP);
+    bool hdopFail = (gps.get_hdop() > 800)  && (frontend->_gpsCheck & MASK_GPS_HDOP);
 
     // Report check result as a text string and bitmask
     if (hdopFail) {