83 float local_psi = 0.0f;
89 float K[9] = {cam_intrinsics.focal_x, 0.0f, cam_intrinsics.center_x,
90 0.0f, cam_intrinsics.focal_y, cam_intrinsics.center_y,
95 struct FloatVect3 gate_vectors[4], vec_B, vec_E, p_vec, temp_vec;
117 printf(
"Undistortion not possible in PnPAHRS.c... why?\n");
118 return pos_drone_E_vec;
121 gate_vectors[i].
x = 1.0;
122 gate_vectors[i].
y = y_n;
123 gate_vectors[i].
z = -x_n;
132 printf(
"Determine world vector for corner %d\n", i);
133 printf(
"Distorted coordinates: (x,y) = (%d, %d)\n", x_corners[i], y_corners[i]);
134 printf(
"Normalized coordinates: (x_n, y_n) = (%f, %f)\n", x_n, y_n);
135 printf(
"Gate vector: (%f,%f,%f)\n", gate_vectors[i].x, gate_vectors[i].y, gate_vectors[i].z);
136 printf(
"Gate vector to Body: (%f,%f,%f)\n", vec_B.
x, vec_B.
y, vec_B.
z);
137 printf(
"Gate vector to World: (%f,%f,%f)\n", vec_E.
x, vec_E.
y, vec_E.
z);
154 #if CAMERA_ROTATED_90DEG_RIGHT
156 float temp = pos_drone_E_vec.
y;
157 pos_drone_E_vec.
y = -pos_drone_E_vec.
z;
158 pos_drone_E_vec.
z = -temp;
162 float y_threshold = 4;
163 if (pos_drone_E_vec.
y > y_threshold) { pos_drone_E_vec.
y = y_threshold; }
164 else if (pos_drone_E_vec.
y < -y_threshold) { pos_drone_E_vec.
y = -y_threshold; }
166 return pos_drone_E_vec;