diff --git a/assets/camera_calib/1.jpg b/assets/camera_calib/1.jpg index 2094d30..b273d74 100644 Binary files a/assets/camera_calib/1.jpg and b/assets/camera_calib/1.jpg differ diff --git a/assets/camera_calib/1.txt b/assets/camera_calib/1.txt index cad88ef..844ca77 100644 --- a/assets/camera_calib/1.txt +++ b/assets/camera_calib/1.txt @@ -1 +1 @@ -0.7177201718666837 0.09187038370024973 0.09699144216061435 -0.6833961132769422 \ No newline at end of file +0.7945099292803862 -0.0434575467108611 0.07481947984239669 0.601055288093252 \ No newline at end of file diff --git a/assets/camera_calib/10.jpg b/assets/camera_calib/10.jpg index cee12d3..b7a810f 100644 Binary files a/assets/camera_calib/10.jpg and b/assets/camera_calib/10.jpg differ diff --git a/assets/camera_calib/10.txt b/assets/camera_calib/10.txt index df5efed..ccf22b1 100644 --- a/assets/camera_calib/10.txt +++ b/assets/camera_calib/10.txt @@ -1 +1 @@ -0.6540393670774336 0.11673220229558735 0.104104730295333 -0.74011371044611 \ No newline at end of file +0.8320261217979533 -0.010711187380915883 0.034853447736525024 0.5535368463721503 \ No newline at end of file diff --git a/assets/camera_calib/11.jpg b/assets/camera_calib/11.jpg index 45274de..c5de466 100644 Binary files a/assets/camera_calib/11.jpg and b/assets/camera_calib/11.jpg differ diff --git a/assets/camera_calib/11.txt b/assets/camera_calib/11.txt index 2cdbe01..694c08d 100644 --- a/assets/camera_calib/11.txt +++ b/assets/camera_calib/11.txt @@ -1 +1 @@ -0.6695365068706467 0.11396525651605409 0.10689676647567184 -0.7261582937567594 \ No newline at end of file +0.8173712544780483 -0.011828605094770673 0.03328423249804994 0.5750273700629471 \ No newline at end of file diff --git a/assets/camera_calib/12.jpg b/assets/camera_calib/12.jpg index ddb8017..eb169ad 100644 Binary files a/assets/camera_calib/12.jpg and b/assets/camera_calib/12.jpg differ diff --git a/assets/camera_calib/12.txt b/assets/camera_calib/12.txt index bf07a54..d523189 100644 --- a/assets/camera_calib/12.txt +++ b/assets/camera_calib/12.txt @@ -1 +1 @@ -0.6865027206257972 0.11121980925498735 0.10959354583553703 -0.7101643635904696 \ No newline at end of file +0.7984894363309989 -0.013758457040363277 0.032468299086838105 0.6009751529656445 \ No newline at end of file diff --git a/assets/camera_calib/13.jpg b/assets/camera_calib/13.jpg index 171ee32..7685451 100644 Binary files a/assets/camera_calib/13.jpg and b/assets/camera_calib/13.jpg differ diff --git a/assets/camera_calib/13.txt b/assets/camera_calib/13.txt index add4a1c..ab4ee82 100644 --- a/assets/camera_calib/13.txt +++ b/assets/camera_calib/13.txt @@ -1 +1 @@ -0.7029618311310923 0.10910831122785473 0.114008892573956 -0.693499828989083 \ No newline at end of file +0.7800486898114236 -0.012651950562476097 0.03220614946153963 0.624761341319473 \ No newline at end of file diff --git a/assets/camera_calib/14.jpg b/assets/camera_calib/14.jpg index c0ea4c8..7712142 100644 Binary files a/assets/camera_calib/14.jpg and b/assets/camera_calib/14.jpg differ diff --git a/assets/camera_calib/14.txt b/assets/camera_calib/14.txt index c2fd6d4..f49bec7 100644 --- a/assets/camera_calib/14.txt +++ b/assets/camera_calib/14.txt @@ -1 +1 @@ -0.6846990959849547 0.11378336536460135 0.1095621509728064 -0.7115030771528001 \ No newline at end of file +0.7710456452265627 -0.02186493764130126 0.04513486486644217 0.6348018442416371 \ No newline at end of file diff --git a/assets/camera_calib/15.jpg b/assets/camera_calib/15.jpg index d648074..8e96eac 100644 Binary files a/assets/camera_calib/15.jpg and b/assets/camera_calib/15.jpg differ diff --git a/assets/camera_calib/15.txt b/assets/camera_calib/15.txt index 45eae7e..3eb6bf1 100644 --- a/assets/camera_calib/15.txt +++ b/assets/camera_calib/15.txt @@ -1 +1 @@ -0.7105886427866503 0.09454929951290965 0.09772990470338523 -0.6903427238922067 \ No newline at end of file +0.7916711278986903 -0.07209407556694548 0.11214836806575476 0.5962231235535496 \ No newline at end of file diff --git a/assets/camera_calib/16.jpg b/assets/camera_calib/16.jpg index f5a6d0f..3a5ea4e 100644 Binary files a/assets/camera_calib/16.jpg and b/assets/camera_calib/16.jpg differ diff --git a/assets/camera_calib/16.txt b/assets/camera_calib/16.txt index 182967c..a8ec915 100644 --- a/assets/camera_calib/16.txt +++ b/assets/camera_calib/16.txt @@ -1 +1 @@ -0.7082004773279313 0.09457995680211755 0.09737030792091227 -0.6928388981712751 \ No newline at end of file +0.8045946318607127 -0.06785936367345102 0.11352663362431434 0.5789078411981385 \ No newline at end of file diff --git a/assets/camera_calib/17.jpg b/assets/camera_calib/17.jpg index 09138de..f4078d1 100644 Binary files a/assets/camera_calib/17.jpg and b/assets/camera_calib/17.jpg differ diff --git a/assets/camera_calib/17.txt b/assets/camera_calib/17.txt index b3941ed..f2e33da 100644 --- a/assets/camera_calib/17.txt +++ b/assets/camera_calib/17.txt @@ -1 +1 @@ -0.6864091145945169 0.08696336166250927 0.08462036719328926 -0.7170211256204378 \ No newline at end of file +0.8215749957916847 -0.06455802999670587 0.11384191417076965 0.5548754866010687 \ No newline at end of file diff --git a/assets/camera_calib/18.jpg b/assets/camera_calib/18.jpg index d2c584d..5b0c250 100644 Binary files a/assets/camera_calib/18.jpg and b/assets/camera_calib/18.jpg differ diff --git a/assets/camera_calib/18.txt b/assets/camera_calib/18.txt index 41d3789..3454f9e 100644 --- a/assets/camera_calib/18.txt +++ b/assets/camera_calib/18.txt @@ -1 +1 @@ -0.6667428632979302 0.10364170157370939 0.0955403079117792 -0.7318363215229934 \ No newline at end of file +0.8368116765764088 -0.06352479955713666 0.11690362951903568 0.5310784868471715 \ No newline at end of file diff --git a/assets/camera_calib/19.jpg b/assets/camera_calib/19.jpg index 03dacd4..f965757 100644 Binary files a/assets/camera_calib/19.jpg and b/assets/camera_calib/19.jpg differ diff --git a/assets/camera_calib/19.txt b/assets/camera_calib/19.txt index 3f6eabb..0c90a2c 100644 --- a/assets/camera_calib/19.txt +++ b/assets/camera_calib/19.txt @@ -1 +1 @@ -0.6608952401030105 0.0924475886118082 0.08181877213214009 -0.7402544248404296 \ No newline at end of file +0.7659234104878251 -0.05152390626763256 0.07971188746430936 0.6358872788053714 \ No newline at end of file diff --git a/assets/camera_calib/2.jpg b/assets/camera_calib/2.jpg index 96417b3..5ffe180 100644 Binary files a/assets/camera_calib/2.jpg and b/assets/camera_calib/2.jpg differ diff --git a/assets/camera_calib/2.txt b/assets/camera_calib/2.txt index b281260..4ba4f9a 100644 --- a/assets/camera_calib/2.txt +++ b/assets/camera_calib/2.txt @@ -1 +1 @@ -0.7258014151886103 0.06893544090588102 0.07336676467256588 -0.6804980003929494 \ No newline at end of file +0.7711539361232197 -0.04474533855113315 0.07190790375467529 0.6309902652634334 \ No newline at end of file diff --git a/assets/camera_calib/20.jpg b/assets/camera_calib/20.jpg index d499cbc..483d0b0 100644 Binary files a/assets/camera_calib/20.jpg and b/assets/camera_calib/20.jpg differ diff --git a/assets/camera_calib/20.txt b/assets/camera_calib/20.txt index 9580033..209cc02 100644 --- a/assets/camera_calib/20.txt +++ b/assets/camera_calib/20.txt @@ -1 +1 @@ -0.6368114936681568 0.09604567438504773 0.07995934840288652 -0.7608237986334537 \ No newline at end of file +0.7843599785188415 -0.07036673754547611 0.10757796487882619 0.6068401171788649 \ No newline at end of file diff --git a/assets/camera_calib/21.jpg b/assets/camera_calib/21.jpg index ec742a6..067cfc0 100644 Binary files a/assets/camera_calib/21.jpg and b/assets/camera_calib/21.jpg differ diff --git a/assets/camera_calib/21.txt b/assets/camera_calib/21.txt index 9ea6f59..9bc2efd 100644 --- a/assets/camera_calib/21.txt +++ b/assets/camera_calib/21.txt @@ -1 +1 @@ -0.6572286496356463 0.07285829130720428 0.06370958053012925 -0.7474511762212797 \ No newline at end of file +0.8107296801983674 -0.06562048651309176 0.11368406086180097 0.5705149180356358 \ No newline at end of file diff --git a/assets/camera_calib/22.jpg b/assets/camera_calib/22.jpg index e02e245..4424037 100644 Binary files a/assets/camera_calib/22.jpg and b/assets/camera_calib/22.jpg differ diff --git a/assets/camera_calib/22.txt b/assets/camera_calib/22.txt index feca931..c376eee 100644 --- a/assets/camera_calib/22.txt +++ b/assets/camera_calib/22.txt @@ -1 +1 @@ -0.6350787937658589 0.07596458908813118 0.06184461820780068 -0.7662111654839704 \ No newline at end of file +0.7935983038966267 -0.033809170582879966 0.06190002705631246 0.6043401845710105 \ No newline at end of file diff --git a/assets/camera_calib/23.jpg b/assets/camera_calib/23.jpg new file mode 100644 index 0000000..8410a65 Binary files /dev/null and b/assets/camera_calib/23.jpg differ diff --git a/assets/camera_calib/23.txt b/assets/camera_calib/23.txt new file mode 100644 index 0000000..fd92349 --- /dev/null +++ b/assets/camera_calib/23.txt @@ -0,0 +1 @@ +0.8237228560732456 -0.030521906459115355 0.06638363742866517 0.5622653130777103 \ No newline at end of file diff --git a/assets/camera_calib/24.jpg b/assets/camera_calib/24.jpg new file mode 100644 index 0000000..83ac02e Binary files /dev/null and b/assets/camera_calib/24.jpg differ diff --git a/assets/camera_calib/24.txt b/assets/camera_calib/24.txt new file mode 100644 index 0000000..5cb19f4 --- /dev/null +++ b/assets/camera_calib/24.txt @@ -0,0 +1 @@ +0.8491398787146873 -0.025532247411769002 0.05836177108338319 0.5243123824539347 \ No newline at end of file diff --git a/assets/camera_calib/3.jpg b/assets/camera_calib/3.jpg index 0c13052..0492907 100644 Binary files a/assets/camera_calib/3.jpg and b/assets/camera_calib/3.jpg differ diff --git a/assets/camera_calib/3.txt b/assets/camera_calib/3.txt index 7532c1c..666396e 100644 --- a/assets/camera_calib/3.txt +++ b/assets/camera_calib/3.txt @@ -1 +1 @@ -0.7153160451022573 0.06679164379924944 0.06815692277445147 -0.6922546249869169 \ No newline at end of file +0.7548850619033333 -0.047418777216818586 0.07093119403444739 0.650283606279068 \ No newline at end of file diff --git a/assets/camera_calib/4.jpg b/assets/camera_calib/4.jpg index 34162c4..096cf90 100644 Binary files a/assets/camera_calib/4.jpg and b/assets/camera_calib/4.jpg differ diff --git a/assets/camera_calib/4.txt b/assets/camera_calib/4.txt index 6f79bd0..9ec0fc4 100644 --- a/assets/camera_calib/4.txt +++ b/assets/camera_calib/4.txt @@ -1 +1 @@ -0.6895526075414257 0.07227717802997588 0.06701324523260725 -0.7174973421081351 \ No newline at end of file +0.7591377331693356 -0.04763573374267206 0.07336706642951026 0.6450255905875732 \ No newline at end of file diff --git a/assets/camera_calib/5.jpg b/assets/camera_calib/5.jpg index 2274eff..246746e 100644 Binary files a/assets/camera_calib/5.jpg and b/assets/camera_calib/5.jpg differ diff --git a/assets/camera_calib/5.txt b/assets/camera_calib/5.txt index b2861a1..bad5122 100644 --- a/assets/camera_calib/5.txt +++ b/assets/camera_calib/5.txt @@ -1 +1 @@ -0.6755160976098915 0.06749445326901966 0.0605731820592963 -0.7317468074840031 \ No newline at end of file +0.7583547886588434 -0.022422481447733462 0.044038113363287834 0.6499660694338815 \ No newline at end of file diff --git a/assets/camera_calib/6.jpg b/assets/camera_calib/6.jpg index faaf6cc..cbc2e7e 100644 Binary files a/assets/camera_calib/6.jpg and b/assets/camera_calib/6.jpg differ diff --git a/assets/camera_calib/6.txt b/assets/camera_calib/6.txt index 770cb5c..14bb88d 100644 --- a/assets/camera_calib/6.txt +++ b/assets/camera_calib/6.txt @@ -1 +1 @@ -0.6554968212916839 0.0710136384754573 0.06108937801660896 -0.7493657773881318 \ No newline at end of file +0.7789208034949294 -0.021997736668215327 0.048119363872661865 0.624886396303184 \ No newline at end of file diff --git a/assets/camera_calib/7.jpg b/assets/camera_calib/7.jpg index 38e5689..2ddd923 100644 Binary files a/assets/camera_calib/7.jpg and b/assets/camera_calib/7.jpg differ diff --git a/assets/camera_calib/7.txt b/assets/camera_calib/7.txt index 7da118c..15ec492 100644 --- a/assets/camera_calib/7.txt +++ b/assets/camera_calib/7.txt @@ -1 +1 @@ -0.6453150221921371 0.08754614426431993 0.07488900209128711 -0.7551793377227582 \ No newline at end of file +0.8037386906226677 -0.020333318885384567 0.048369913784180284 0.5926643441103707 \ No newline at end of file diff --git a/assets/camera_calib/8.jpg b/assets/camera_calib/8.jpg index 4cac18f..ef3ea34 100644 Binary files a/assets/camera_calib/8.jpg and b/assets/camera_calib/8.jpg differ diff --git a/assets/camera_calib/8.txt b/assets/camera_calib/8.txt index 58de405..aaeab36 100644 --- a/assets/camera_calib/8.txt +++ b/assets/camera_calib/8.txt @@ -1 +1 @@ -0.6436936453206352 0.1018123504027431 0.08674315464388545 -0.7535040553319725 \ No newline at end of file +0.8256625130752344 -0.020650051549974013 0.04936930246923488 0.5616205675070786 \ No newline at end of file diff --git a/assets/camera_calib/9.jpg b/assets/camera_calib/9.jpg index 98f25ee..16faf75 100644 Binary files a/assets/camera_calib/9.jpg and b/assets/camera_calib/9.jpg differ diff --git a/assets/camera_calib/9.txt b/assets/camera_calib/9.txt index 6f84e3d..be80f2d 100644 --- a/assets/camera_calib/9.txt +++ b/assets/camera_calib/9.txt @@ -1 +1 @@ -0.6409116406794043 0.11242770296004105 0.09554048153994232 -0.7533022612709181 \ No newline at end of file +0.8377166207827541 -0.020123362855542434 0.049144700467202415 0.5435169840471376 \ No newline at end of file diff --git a/cali.MD b/cali.MD new file mode 100644 index 0000000..c9641f1 --- /dev/null +++ b/cali.MD @@ -0,0 +1,2 @@ +./build/capture configs/calibration.yaml -o assets/camera_calib +./build/calibrate_camera assets/camera_calib diff --git a/configs/calibration.yaml b/configs/calibration.yaml index 137720c..6821915 100644 --- a/configs/calibration.yaml +++ b/configs/calibration.yaml @@ -2,21 +2,23 @@ pattern_cols: 10 pattern_rows: 7 center_distance_mm: 39 -# 重投影误差: 0.0833px -camera_matrix: [1824.3445767434343, 0, 745.88898611634079, 0, 1828.9000780816298, 584.45884364277663, 0, 0, 1] -distort_coeffs: [-0.084922638867033884, 0.22051971879198334, -0.00021700148630802023, 0.00043485717343934131, 0] +# 重投影误差: 0.0840px +camera_matrix: [1834.1536403391224, 0, 748.34431374203518, 0, 1838.4254732917539, 592.07179931054554, 0, 0, 1] +distort_coeffs: [-0.089753587550662386, 0.25043670633576026, 0.00028701704204080067, 0.00026068590699182429, 0] R_gimbal2imubody: [1, 0, 0, 0, 1, 0, 0, 0, 1] -# 相机同理想情况的偏角: yaw174.40 pitch8.20 roll178.20 degree -R_camera2gimbal: [0.093296993396547734, 0.14503826005617809, -0.9850175501700672, -0.99514483056044212, -0.017561170025079953, -0.096841992524828474, -0.03134385477239883, 0.98927018980015091, 0.14269567036452102] -t_camera2gimbal: [-0.11903236791933375, -0.012143601357090483, -0.023682825149991701] +# 相机同理想情况的偏角: yaw0.65 pitch0.98 roll-4.71 degree +# 标定板到世界坐标系原点的水平距离: 1.20 m +# 标定板同竖直摆放时的偏角: yaw76.91 pitch5.90 roll-4.25 degree +R_camera2gimbal: [-0.012713492697510044, 0.016094161023931495, 0.99978965041870982, -0.9965458575215137, -0.082265286272855259, -0.011347974760117781, 0.082065345671248222, -0.99648050691189183, 0.017084448614971628] +t_camera2gimbal: [0.099890931342671643, 0.0015513845612699661, 0.048389617922529167] #####-----gimbal参数-----##### com_port: "/dev/ttyACM0" camera_name: "hikrobot" -exposure_ms: 50 +exposure_ms: 20 gain: 50.0 vid_pid: "2bdf:0001" diff --git a/configs/sentry.yaml b/configs/sentry.yaml index 6282e7a..5aced42 100644 --- a/configs/sentry.yaml +++ b/configs/sentry.yaml @@ -1,7 +1,6 @@ enemy_color: "red" # enemy_color: "blue" - #####-----神经网络参数-----##### yolo_name: yolov5 classify_model: assets/tiny_resnet.onnx @@ -9,8 +8,8 @@ yolo11_model_path: assets/yolo11.xml yolov8_model_path: assets/yolov8.xml yolov5_model_path: assets/yolov5.xml device: GPU -min_confidence: 0.8 -use_traditional: false +min_confidence: 0.75 +use_traditional: true #####-----ROI-----##### roi: @@ -21,24 +20,6 @@ roi: use_roi: false -#####-----USB相机参数-----##### -image_width: 1280 -image_height: 720 -fov_h: 57.7 #87.7 -fov_v: 56.7 -new_fov_h: 27 #67 -new_fov_v: 40.9 -usb_frame_rate: 120 -usb_exposure: 500 #1-80000______250 -usb_gamma: 160 -usb_gain: 10 #0-96 - -#####-----工业相机参数-----##### -camera_name: "hikrobot" -exposure_ms: 0.8 -gain: 16.9 -vid_pid: "2bdf:0001" - #####-----传统方法参数-----##### threshold: 150 max_angle_error: 45 # degree @@ -49,48 +30,71 @@ min_armor_ratio: 1 max_armor_ratio: 5 max_side_ratio: 1.5 max_rectangular_error: 25 # degree -min_confidence: 0.8 - -#####-----cboard参数-----##### -quaternion_canid: 0x01 -bullet_speed_canid: 0x110 -send_canid: 0xff -can_interface: "can0" #####-----tracker参数-----##### min_detect_count: 5 -max_temp_lost_count: 25 +max_temp_lost_count: 15 outpost_max_temp_lost_count: 75 #####-----aimer参数-----##### -yaw_offset: -0.8 # degree -2.5 -pitch_offset: -1 # degree 2 -comming_angle: 60 # degree +yaw_offset: 2 # degree -2.5 +pitch_offset: -6.5 # degree 2 +comming_angle: 55 # degree leaving_angle: 20 # degree -left_yaw_offset: -1 -right_yaw_offset: -0.6 -decision_speed: 10 # rad/s -high_speed_delay_time: 0.026 # s -low_speed_delay_time: 0.010 # s +decision_speed: 7 # rad/s +high_speed_delay_time: 0.0 # s +low_speed_delay_time: 0.0 # s planner use this value #####-----shooter参数-----##### -first_tolerance: 5 # 近距离射击容差,degree +first_tolerance: 3 # 近距离射击容差,degree second_tolerance: 2 # 远距离射击容差,degree -judge_distance: 3 #距离判断阈值 +judge_distance: 2 #距离判断阈值 auto_fire: true # 是否由自瞄控制射击 -#####-----decider参数-----##### -mode: 1 +camera_name: "hikrobot" +exposure_ms: 2.5 +gain: 16.9 +vid_pid: "2bdf:0001" + +# 重投影误差: 0.0840px +camera_matrix: [1834.1536403391224, 0, 748.34431374203518, 0, 1838.4254732917539, 592.07179931054554, 0, 0, 1] +distort_coeffs: [-0.089753587550662386, 0.25043670633576026, 0.00028701704204080067, 0.00026068590699182429, 0] -#####-----工业相机标定参数-----##### R_gimbal2imubody: [1, 0, 0, 0, 1, 0, 0, 0, 1] -# 重投影误差: 0.1833px -camera_matrix: [2414.9359264386621, 0, 717.26243105567414, 0, 2418.0489262208148, 582.68540529942845, 0, 0, 1] -distort_coeffs: [-0.0209453389287673, 0.15028138841073832, -0.0006517722113234505, -0.0016861906197686788, 0] - -# 相机同理想情况的偏角: yaw-1.11 pitch0.01 roll-0.06 degree +# 相机同理想情况的偏角: yaw0.65 pitch0.98 roll-4.71 degree # 标定板到世界坐标系原点的水平距离: 1.20 m -# 标定板同竖直摆放时的偏角: yaw123.89 pitch14.05 roll-0.86 degree -R_camera2gimbal: [0.01928451708725664, 0.00012696140743255463, 0.99981402834802846, -0.99981344688553653, -0.0010834913551969569, 0.019284643459122196, 0.0010857382619952124, -0.99999940496346484, 0.00010604311483339372] -t_camera2gimbal: [0.13089617453251859, 0.0038468007459533785, 0.094139945222010288] +# 标定板同竖直摆放时的偏角: yaw76.91 pitch5.90 roll-4.25 degree +R_camera2gimbal: [-0.012713492697510044, 0.016094161023931495, 0.99978965041870982, -0.9965458575215137, -0.082265286272855259, -0.011347974760117781, 0.082065345671248222, -0.99648050691189183, 0.017084448614971628] +t_camera2gimbal: [0.099890931342671643, 0.0015513845612699661, 0.048389617922529167] + +#####-----cboard参数-----##### +quaternion_canid: 0x100 +bullet_speed_canid: 0x101 +send_canid: 0xff +can_interface: "can0" + +#####-----gimbal参数-----##### +com_port: "/dev/ttyUSB0" +yaw_kp: 0 +yaw_kd: 0 +pitch_kp: 0 +pitch_kd: 0 + +#####-----planner-----##### +fire_thresh: 0.0035 + +max_yaw_acc: 50 +Q_yaw: [9e6, 0] +R_yaw: [1] + +max_pitch_acc: 100 +Q_pitch: [9e6, 0] +R_pitch: [1] + +#####-----buff_detector参数-----##### +model: "assets/yolo11_buff_int8.xml" + +#####-----buff_aimer参数-----##### +fire_gap_time: 0.700 # s +predict_time: 0.120 # s \ No newline at end of file diff --git a/io/gimbal/gimbal.cpp b/io/gimbal/gimbal.cpp index b1e9a24..8094192 100644 --- a/io/gimbal/gimbal.cpp +++ b/io/gimbal/gimbal.cpp @@ -199,11 +199,11 @@ void Gimbal::read_thread() // float bullet_speed = rx_data_.bullet_speed; // uint16_t bullet_count = rx_data_.bullet_count, crc16 = rx_data_.crc16; - tools::logger()->debug( - "[Gimbal] mode: {}, q: [{:.3f}, {:.3f}, {:.3f}, {:.3f}], yaw: {:.3f}, yaw_vel: {:.3f}, " - "pitch: {:.3f}, pitch_vel: {:.3f}, bullet_speed: {:.3f}, bullet_count: {}, crc16: {}", - rx_data_.mode, q0, q1, q2, q3, yaw, yaw_vel, pitch, pitch_vel, bullet_speed, - bullet_count, crc16); + // tools::logger()->debug( + // "[Gimbal] mode: {}, q: [{:.3f}, {:.3f}, {:.3f}, {:.3f}], yaw: {:.3f}, yaw_vel: {:.3f}, " + // "pitch: {:.3f}, pitch_vel: {:.3f}, bullet_speed: {:.3f}, bullet_count: {}, crc16: {}", + // rx_data_.mode, q0, q1, q2, q3, yaw, yaw_vel, pitch, pitch_vel, bullet_speed, + // bullet_count, crc16); error_count = 0; Eigen::Quaterniond q(rx_data_.q[0], rx_data_.q[1], rx_data_.q[2], rx_data_.q[3]); queue_.push({q, t}); diff --git a/src/auto_aim_debug_mpc.cpp b/src/auto_aim_debug_mpc.cpp index 8d7657f..1df22c7 100644 --- a/src/auto_aim_debug_mpc.cpp +++ b/src/auto_aim_debug_mpc.cpp @@ -62,6 +62,12 @@ int main(int argc, char * argv[]) plan.control, plan.fire, plan.yaw, plan.yaw_vel, plan.yaw_acc, plan.pitch, plan.pitch_vel, plan.pitch_acc); + tools::logger()->info( + "Target: yaw={:.3f}, pitch={:.3f} | Plan: yaw={:.3f}, pitch={:.3f} | Control={}, Fire={} | " + "Bullet_speed={:.2f}m/s", + plan.target_yaw, plan.target_pitch, plan.yaw, plan.pitch, plan.control, plan.fire, + gs.bullet_speed); + auto fired = gs.bullet_count > last_bullet_count; last_bullet_count = gs.bullet_count; diff --git a/true/1.txt b/true/1.txt deleted file mode 100644 index cad88ef..0000000 --- a/true/1.txt +++ /dev/null @@ -1 +0,0 @@ -0.7177201718666837 0.09187038370024973 0.09699144216061435 -0.6833961132769422 \ No newline at end of file diff --git a/true/10.jpg b/true/10.jpg deleted file mode 100644 index cee12d3..0000000 Binary files a/true/10.jpg and /dev/null differ diff --git a/true/10.txt b/true/10.txt deleted file mode 100644 index df5efed..0000000 --- a/true/10.txt +++ /dev/null @@ -1 +0,0 @@ -0.6540393670774336 0.11673220229558735 0.104104730295333 -0.74011371044611 \ No newline at end of file diff --git a/true/11.jpg b/true/11.jpg deleted file mode 100644 index 45274de..0000000 Binary files a/true/11.jpg and /dev/null differ diff --git a/true/11.txt b/true/11.txt deleted file mode 100644 index 2cdbe01..0000000 --- a/true/11.txt +++ /dev/null @@ -1 +0,0 @@ -0.6695365068706467 0.11396525651605409 0.10689676647567184 -0.7261582937567594 \ No newline at end of file diff --git a/true/12.jpg b/true/12.jpg deleted file mode 100644 index ddb8017..0000000 Binary files a/true/12.jpg and /dev/null differ diff --git a/true/12.txt b/true/12.txt deleted file mode 100644 index bf07a54..0000000 --- a/true/12.txt +++ /dev/null @@ -1 +0,0 @@ -0.6865027206257972 0.11121980925498735 0.10959354583553703 -0.7101643635904696 \ No newline at end of file diff --git a/true/13.jpg b/true/13.jpg deleted file mode 100644 index 171ee32..0000000 Binary files a/true/13.jpg and /dev/null differ diff --git a/true/13.txt b/true/13.txt deleted file mode 100644 index add4a1c..0000000 --- a/true/13.txt +++ /dev/null @@ -1 +0,0 @@ -0.7029618311310923 0.10910831122785473 0.114008892573956 -0.693499828989083 \ No newline at end of file diff --git a/true/14.jpg b/true/14.jpg deleted file mode 100644 index c0ea4c8..0000000 Binary files a/true/14.jpg and /dev/null differ diff --git a/true/14.txt b/true/14.txt deleted file mode 100644 index c2fd6d4..0000000 --- a/true/14.txt +++ /dev/null @@ -1 +0,0 @@ -0.6846990959849547 0.11378336536460135 0.1095621509728064 -0.7115030771528001 \ No newline at end of file diff --git a/true/15.jpg b/true/15.jpg deleted file mode 100644 index d648074..0000000 Binary files a/true/15.jpg and /dev/null differ diff --git a/true/15.txt b/true/15.txt deleted file mode 100644 index 45eae7e..0000000 --- a/true/15.txt +++ /dev/null @@ -1 +0,0 @@ -0.7105886427866503 0.09454929951290965 0.09772990470338523 -0.6903427238922067 \ No newline at end of file diff --git a/true/16.jpg b/true/16.jpg deleted file mode 100644 index f5a6d0f..0000000 Binary files a/true/16.jpg and /dev/null differ diff --git a/true/16.txt b/true/16.txt deleted file mode 100644 index 182967c..0000000 --- a/true/16.txt +++ /dev/null @@ -1 +0,0 @@ -0.7082004773279313 0.09457995680211755 0.09737030792091227 -0.6928388981712751 \ No newline at end of file diff --git a/true/17.jpg b/true/17.jpg deleted file mode 100644 index 09138de..0000000 Binary files a/true/17.jpg and /dev/null differ diff --git a/true/17.txt b/true/17.txt deleted file mode 100644 index b3941ed..0000000 --- a/true/17.txt +++ /dev/null @@ -1 +0,0 @@ -0.6864091145945169 0.08696336166250927 0.08462036719328926 -0.7170211256204378 \ No newline at end of file diff --git a/true/18.jpg b/true/18.jpg deleted file mode 100644 index d2c584d..0000000 Binary files a/true/18.jpg and /dev/null differ diff --git a/true/18.txt b/true/18.txt deleted file mode 100644 index 41d3789..0000000 --- a/true/18.txt +++ /dev/null @@ -1 +0,0 @@ -0.6667428632979302 0.10364170157370939 0.0955403079117792 -0.7318363215229934 \ No newline at end of file diff --git a/true/19.jpg b/true/19.jpg deleted file mode 100644 index 03dacd4..0000000 Binary files a/true/19.jpg and /dev/null differ diff --git a/true/19.txt b/true/19.txt deleted file mode 100644 index 3f6eabb..0000000 --- a/true/19.txt +++ /dev/null @@ -1 +0,0 @@ -0.6608952401030105 0.0924475886118082 0.08181877213214009 -0.7402544248404296 \ No newline at end of file diff --git a/true/2.jpg b/true/2.jpg deleted file mode 100644 index 96417b3..0000000 Binary files a/true/2.jpg and /dev/null differ diff --git a/true/2.txt b/true/2.txt deleted file mode 100644 index b281260..0000000 --- a/true/2.txt +++ /dev/null @@ -1 +0,0 @@ -0.7258014151886103 0.06893544090588102 0.07336676467256588 -0.6804980003929494 \ No newline at end of file diff --git a/true/20.jpg b/true/20.jpg deleted file mode 100644 index d499cbc..0000000 Binary files a/true/20.jpg and /dev/null differ diff --git a/true/20.txt b/true/20.txt deleted file mode 100644 index 9580033..0000000 --- a/true/20.txt +++ /dev/null @@ -1 +0,0 @@ -0.6368114936681568 0.09604567438504773 0.07995934840288652 -0.7608237986334537 \ No newline at end of file diff --git a/true/21.jpg b/true/21.jpg deleted file mode 100644 index ec742a6..0000000 Binary files a/true/21.jpg and /dev/null differ diff --git a/true/21.txt b/true/21.txt deleted file mode 100644 index 9ea6f59..0000000 --- a/true/21.txt +++ /dev/null @@ -1 +0,0 @@ -0.6572286496356463 0.07285829130720428 0.06370958053012925 -0.7474511762212797 \ No newline at end of file diff --git a/true/22.jpg b/true/22.jpg deleted file mode 100644 index e02e245..0000000 Binary files a/true/22.jpg and /dev/null differ diff --git a/true/22.txt b/true/22.txt deleted file mode 100644 index feca931..0000000 --- a/true/22.txt +++ /dev/null @@ -1 +0,0 @@ -0.6350787937658589 0.07596458908813118 0.06184461820780068 -0.7662111654839704 \ No newline at end of file diff --git a/true/3.jpg b/true/3.jpg deleted file mode 100644 index 0c13052..0000000 Binary files a/true/3.jpg and /dev/null differ diff --git a/true/3.txt b/true/3.txt deleted file mode 100644 index 7532c1c..0000000 --- a/true/3.txt +++ /dev/null @@ -1 +0,0 @@ -0.7153160451022573 0.06679164379924944 0.06815692277445147 -0.6922546249869169 \ No newline at end of file diff --git a/true/4.jpg b/true/4.jpg deleted file mode 100644 index 34162c4..0000000 Binary files a/true/4.jpg and /dev/null differ diff --git a/true/4.txt b/true/4.txt deleted file mode 100644 index 6f79bd0..0000000 --- a/true/4.txt +++ /dev/null @@ -1 +0,0 @@ -0.6895526075414257 0.07227717802997588 0.06701324523260725 -0.7174973421081351 \ No newline at end of file diff --git a/true/5.jpg b/true/5.jpg deleted file mode 100644 index 2274eff..0000000 Binary files a/true/5.jpg and /dev/null differ diff --git a/true/5.txt b/true/5.txt deleted file mode 100644 index b2861a1..0000000 --- a/true/5.txt +++ /dev/null @@ -1 +0,0 @@ -0.6755160976098915 0.06749445326901966 0.0605731820592963 -0.7317468074840031 \ No newline at end of file diff --git a/true/6.jpg b/true/6.jpg deleted file mode 100644 index faaf6cc..0000000 Binary files a/true/6.jpg and /dev/null differ diff --git a/true/6.txt b/true/6.txt deleted file mode 100644 index 770cb5c..0000000 --- a/true/6.txt +++ /dev/null @@ -1 +0,0 @@ -0.6554968212916839 0.0710136384754573 0.06108937801660896 -0.7493657773881318 \ No newline at end of file diff --git a/true/7.jpg b/true/7.jpg deleted file mode 100644 index 38e5689..0000000 Binary files a/true/7.jpg and /dev/null differ diff --git a/true/7.txt b/true/7.txt deleted file mode 100644 index 7da118c..0000000 --- a/true/7.txt +++ /dev/null @@ -1 +0,0 @@ -0.6453150221921371 0.08754614426431993 0.07488900209128711 -0.7551793377227582 \ No newline at end of file diff --git a/true/8.jpg b/true/8.jpg deleted file mode 100644 index 4cac18f..0000000 Binary files a/true/8.jpg and /dev/null differ diff --git a/true/8.txt b/true/8.txt deleted file mode 100644 index 58de405..0000000 --- a/true/8.txt +++ /dev/null @@ -1 +0,0 @@ -0.6436936453206352 0.1018123504027431 0.08674315464388545 -0.7535040553319725 \ No newline at end of file diff --git a/true/9.jpg b/true/9.jpg deleted file mode 100644 index 98f25ee..0000000 Binary files a/true/9.jpg and /dev/null differ diff --git a/true/9.txt b/true/9.txt deleted file mode 100644 index 6f84e3d..0000000 --- a/true/9.txt +++ /dev/null @@ -1 +0,0 @@ -0.6409116406794043 0.11242770296004105 0.09554048153994232 -0.7533022612709181 \ No newline at end of file