参数获取偏移量调整

d100.sh

@@ -1,3 +1,3 @@
 ./waf configure --board d100
 ./waf copter
-cp ./build/d100/bin/arducopter.apj  /mnt/f/_01-work/100\=data/固件/zr-v4/wsl/d100-v4.0.16-AVDv6.px4
+cp ./build/d100/bin/arducopter.apj  /mnt/f/_01-work/100\=data/固件/编译/中海达d100-v4.1.18.px4

libraries/AP_GPS/AP_GPS_NMEA.cpp

@@ -681,26 +681,11 @@ AP_GPS_NMEA::_detect(struct NMEA_detect_state &state, uint8_t data)
 
 bool AP_GPS_NMEA::get_offset_param()
 {
-    enum ap_var_type vtype;
-    AP_Float *offset_x = (AP_Float *)AP_Param::find("GPS_POS2_X", &vtype);
-    if (offset_x != NULL) {
-        _cam_offset.x = offset_x->get();
-    }else{
-        return false;
-    }
-    AP_Float *offset_y = (AP_Float *)AP_Param::find("GPS_POS2_Y", &vtype);
-    if (offset_y != NULL) {
-        _cam_offset.y = offset_y->get();
-    }else{
-        return false;
-    }
-    AP_Float *offset_z = (AP_Float *)AP_Param::find("GPS_POS2_Z", &vtype);
-    if (offset_z != NULL) {
-        _cam_offset.z = offset_z->get();
-    }else{
-        return false;
-    }
-
+    const Vector3f &off = gps._antenna_offset[1].get();
+    _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;
 }
 
@@ -708,7 +693,7 @@ 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,0,0};
+    Vector3f cam_pos_ned = {-0.02,-0.225,0.17};
     if(!get_param){
         get_param = get_offset_param();
         return cam_pos_ned;