调整偏移量修正方向

libraries/AP_GPS/AP_GPS_NMEA.cpp

@@ -335,7 +335,10 @@ bool AP_GPS_NMEA::_term_complete()
                                 cam_pos_ned,
                                 state.status);
                             // gcs().send_text(MAV_SEVERITY_INFO, "mrk:%d, %d, %d, %d", gps_mark.week, gps_mark.second, (int32_t)(gps_mark.lat * (int32_t)10000000UL), (int32_t)(gps_mark.lon * (int32_t)10000000UL));
-                            // gcs().send_text(MAV_SEVERITY_INFO,"rot:%dus,%.3f,%.3f,%.3f",(uint16_t)(tk2 - tk1),cam_pos_ned.x,cam_pos_ned.y,cam_pos_ned.z);
+                            // gcs().send_text(MAV_SEVERITY_INFO,"rot:%d,%.3f,%.3f,%.3f",(uint16_t)(tk2 - tk1),cam_pos_ned.x,cam_pos_ned.y,cam_pos_ned.z);
+                            // const AP_AHRS &ahrs = AP::ahrs();
+                            //     // integer Euler angles (Degrees * 100)
+                            // gcs().send_text(MAV_SEVERITY_INFO,"att:%.3f,%.3f,%.3f",ahrs.roll_sensor/100.0,ahrs.pitch_sensor/100.0,ahrs.yaw_sensor/100.0);
                         }
                     }
                 }
@@ -683,9 +686,9 @@ AP_GPS_NMEA::_detect(struct NMEA_detect_state &state, uint8_t data)
 bool AP_GPS_NMEA::get_offset_param()
 {
     const Vector3f &off = gps._antenna_offset[1].get();
-    _cam_offset.x = off.x;
-    _cam_offset.y = off.y;
-    _cam_offset.z = off.z;
+    _cam_offset.x = -off.x;
+    _cam_offset.y = -off.y;
+    _cam_offset.z = -off.z;
     gcs().send_text(MAV_SEVERITY_INFO,"ant offset:%.3f,%.3f,%.3f",_cam_offset.x,_cam_offset.y,_cam_offset.z);
     return true;
 }
@@ -693,11 +696,9 @@ bool AP_GPS_NMEA::get_offset_param()
 Vector3f AP_GPS_NMEA::camera_pos_rotation()
 {
     Matrix3f Tbn;                               // dcm rotation matrix to rotate body frame to north
-    Vector3f target_vec_unit_body;
-    Vector3f cam_pos_ned = {-0.02,-0.225,0.17};
+    Vector3f cam_pos_ned = {0.02,0.225,0.17};
     if(!get_param){
         get_param = get_offset_param();
-        return cam_pos_ned;
     }
     const AP_AHRS &ahrs = AP::ahrs();
     Tbn = ahrs.get_rotation_body_to_ned();