From 20cd40f740c21e9ab464656b7eb9e75dae5eba2d Mon Sep 17 00:00:00 2001 From: NageZeng Date: Sat, 11 Jun 2022 16:06:19 +0800 Subject: [PATCH] =?UTF-8?q?=E5=A4=A9=E7=BA=BF=E5=81=8F=E7=A7=BB=E9=87=8F?= =?UTF-8?q?=E4=BF=AE=E6=AD=A3=E6=B5=8B=E8=AF=95?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- ArduCopter/APM_Config.h | 16 +++---- ArduCopter/Copter.h | 3 ++ ArduCopter/UserCode.cpp | 83 ++++++++++++++++++++++++++++++++++++ Tools/autotest/locations.txt | 1 + 4 files changed, 95 insertions(+), 8 deletions(-) diff --git a/ArduCopter/APM_Config.h b/ArduCopter/APM_Config.h index c28a027b9b..4e08458efd 100644 --- a/ArduCopter/APM_Config.h +++ b/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_FASTLOOP userhook_FastLoop(); // for code to be run at 100hz -//#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_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_AUXSWITCH ENABLED // for code to handle user aux switches -//#define USER_PARAMS_ENABLED ENABLED // to enable user parameters +#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_50HZLOOP userhook_50Hz(); // for code to be run at 50hz +#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_SUPERSLOWLOOP userhook_SuperSlowLoop(); // for code to be run at 1hz +#define USERHOOK_AUXSWITCH ENABLED // for code to handle user aux switches +#define USER_PARAMS_ENABLED ENABLED // to enable user parameters diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index b2da1d8e2f..97e827f889 100644 --- a/ArduCopter/Copter.h +++ b/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; diff --git a/ArduCopter/UserCode.cpp b/ArduCopter/UserCode.cpp index 46070d47fa..1fa1b74478 100644 --- a/ArduCopter/UserCode.cpp +++ b/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) diff --git a/Tools/autotest/locations.txt b/Tools/autotest/locations.txt index fe12ec0816..611e343802 100644 --- a/Tools/autotest/locations.txt +++ b/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