天线偏移量修正测试

ArduCopter/APM_Config.h

@@ -64,11 +64,11 @@
 // Put your variable definitions into the UserVariables.h file (or another file name and then change the #define below).
 //#define USERHOOK_VARIABLES "UserVariables.h"
 // Put your custom code into the UserCode.cpp with function names matching those listed below and ensure the appropriate #define below is uncommented below
-//#define USERHOOK_INIT userhook_init();                      // for code to be run once at startup
+#define USERHOOK_INIT userhook_init();                      // for code to be run once at startup
-//#define USERHOOK_FASTLOOP userhook_FastLoop();            // for code to be run at 100hz
+#define USERHOOK_FASTLOOP userhook_FastLoop();            // for code to be run at 100hz
-//#define USERHOOK_50HZLOOP userhook_50Hz();                  // for code to be run at 50hz
+#define USERHOOK_50HZLOOP userhook_50Hz();                  // for code to be run at 50hz
-//#define USERHOOK_MEDIUMLOOP userhook_MediumLoop();        // for code to be run at 10hz
+#define USERHOOK_MEDIUMLOOP userhook_MediumLoop();        // for code to be run at 10hz
-//#define USERHOOK_SLOWLOOP userhook_SlowLoop();            // for code to be run at 3.3hz
+#define USERHOOK_SLOWLOOP userhook_SlowLoop();            // for code to be run at 3.3hz
-//#define USERHOOK_SUPERSLOWLOOP userhook_SuperSlowLoop();  // for code to be run at 1hz
+#define USERHOOK_SUPERSLOWLOOP userhook_SuperSlowLoop();  // for code to be run at 1hz
-//#define USERHOOK_AUXSWITCH ENABLED                        // for code to handle user aux switches
+#define USERHOOK_AUXSWITCH ENABLED                        // for code to handle user aux switches
-//#define USER_PARAMS_ENABLED ENABLED                       // to enable user parameters
+#define USER_PARAMS_ENABLED ENABLED                       // to enable user parameters

ArduCopter/Copter.h

@@ -1001,6 +1001,9 @@ private:
 public:
     void mavlink_delay_cb();    // GCS_Mavlink.cpp
     void failsafe_check();      // failsafe.cpp
+    void camera_pos_rotation();
+    bool get_vector_xyz_from_NEU(Vector3f &vec_ne,Location lla) const;
+    bool get_NEU_from_xyz(Location &neu,Vector3f vec_ne);
 };
 
 extern Copter copter;

ArduCopter/UserCode.cpp

@@ -40,13 +40,96 @@ void Copter::userhook_SlowLoop()
 void Copter::userhook_SuperSlowLoop()
 {
     // put your 1Hz code here
+    static uint8_t cnt;
+    cnt +=1;
+
+    if (cnt > 5)
+    {
+        camera_pos_rotation();
+        cnt = 0;
+    }
+    
+    
+    
 }
 #endif
 
+void Copter::camera_pos_rotation()
+{
+    Matrix3f Tbn;                               // dcm rotation matrix to rotate body frame to north
+    Vector3f target_vec_unit_body;
+    Vector3f _cam_offset = {0,240.0,130.0};
+    Tbn = ahrs.get_rotation_body_to_ned();
+    Vector3f cam_pos_ned = Tbn * (_cam_offset);
+    Vector3f euler,euler_deg;
+    if(ahrs.get_secondary_attitude(euler)){
+        euler_deg.x = degrees(euler.x);
+        euler_deg.y = degrees(euler.y);
+        euler_deg.z = wrap_360(degrees(euler.z));
+    }
+    // Vector3f cam_pos_delt = Tbn.mul_transpose(_cam_offset);
+    gcs().send_text(MAV_SEVERITY_INFO,"org:%.2f,%.2f,%.2f,euler:%.2f,%.2f,%.2f",_cam_offset.x,_cam_offset.y,_cam_offset.z,euler_deg.x,euler_deg.y,euler_deg.z);
+    gcs().send_text(MAV_SEVERITY_INFO,"rot:%.2f,%.2f,%.2f",cam_pos_ned.x,cam_pos_ned.y,cam_pos_ned.z);
+    // gcs().send_text(MAV_SEVERITY_INFO,"del:%.2f,%.2f,%.2f",cam_pos_delt.x,cam_pos_delt.y,cam_pos_delt.z);
+
+
+    Vector3f vec_ne;
+        const Location &loc = gps.location();
+    get_vector_xyz_from_NEU(vec_ne,loc);
+
+    gcs().send_text(MAV_SEVERITY_INFO,"pos:%d,%d,%d",loc.lat,loc.lng,loc.alt);
+    gcs().send_text(MAV_SEVERITY_INFO,"xyz:%.2f,%.2f,%.2f",vec_ne.x,vec_ne.y,vec_ne.z);
+    Location neu;
+    get_NEU_from_xyz(neu,vec_ne);
+    gcs().send_text(MAV_SEVERITY_INFO,"neu:%d,%d,%d",neu.lat,neu.lng,neu.alt);
+
+}
+
+bool Copter::get_vector_xyz_from_NEU(Vector3f &vec_ne,Location lla) const
+{
+    Location ekf_origin;
+    if (!AP::ahrs().get_origin(ekf_origin)) {
+        return false;
+    }
+    vec_ne.x = (lla.lat-0) * LATLON_TO_CM;
+    vec_ne.y = (lla.lng-0) * LATLON_TO_CM * ekf_origin.longitude_scale();
+    vec_ne.z = (lla.alt-0);
+
+    return true;
+}
+
+bool Copter::get_NEU_from_xyz(Location &neu,Vector3f vec_ne)
+{
+    Location ekf_origin;
+    if (!AP::ahrs().get_origin(ekf_origin)) {
+        return false;
+    }
+    neu.lat = vec_ne.x/ LATLON_TO_CM;
+    neu.lng = vec_ne.y/(LATLON_TO_CM * ekf_origin.longitude_scale());
+    neu.alt = vec_ne.z;
+
+    return true;
+}
+
 #ifdef USERHOOK_AUXSWITCH
 void Copter::userhook_auxSwitch1(uint8_t ch_flag)
 {
     // put your aux switch #1 handler here (CHx_OPT = 47)
+    switch (ch_flag)
+    {
+    case 0:
+        /* code */
+        break;
+    case 1:
+        /* code */
+        break;
+    case 2:
+        camera_pos_rotation();
+        break;
+    
+    default:
+        break;
+    }
 }
 
 void Copter::userhook_auxSwitch2(uint8_t ch_flag)

Tools/autotest/locations.txt

@@ -1,4 +1,5 @@
 #NAME=latitude,longitude,absolute-altitude,heading
+CYDS=24.6122682,118.0672663,1,90
 OSRF0=37.4003371,-122.0800351,0,353
 OSRF0_PILOTSBOX=37.4003371,-122.0800351,2,270
 CMAC=-35.363261,149.165230,584,353