btl143/QDAC-firmware
4
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

pref: 换用卡马克的快速开方求倒函数

Browse Source 
虽然用的地方不是很多()
This commit is contained in:
bmy
2024-03-10 20:02:40 +08:00
parent c1e2b75aa5
commit ae70d9388e
5 changed files with 39 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
app/gl_tracking.c
View File

@@ -86,7 +86,7 @@ void MidLineTrack()
for (int i = 0; i < mid_track_count; i++) {
float dx = mid_track[i][1] - cx;
float dy = mid_track[i][0] - cy;
float dist = sqrt(dx * dx + dy * dy);
float dist = Q_sqrt(dx * dx + dy * dy);
if (dist < min_dist) {
min_dist = dist;
begin_id = i;
@@ -108,14 +108,14 @@ void MidLineTrack()
// 计算远锚点偏差值
float dx = rptsn[aim_idx][1] - cx;
float dy = cy - rptsn[aim_idx][0] + 0.2 * PIXPERMETER;
float dn = sqrt(dx * dx + dy * dy);
float dy = cy - rptsn[aim_idx][0] + 0.2f * PIXPERMETER;
float dn = (float)Q_sqrt(dx * dx + dy * dy);
// float error = -atan2f(dx, dy) * 180 / PI32;
// 计算近锚点偏差值
dx_near = rptsn[aim_idx_near][1] - cx;
// float dy_near = cy - rptsn[aim_idx_near][0] + 0.2 * PIXPERMETER;
// float dn_near = sqrt(dx_near * dx_near + dy_near * dy_near);
// float dn_near = Q_sqrt(dx_near * dx_near + dy_near * dy_near);
// float error_near = -atan2f(dx_near, dy_near) * 180 / PI32;
// //考虑近点