Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-02 13:27:32 +08:00
Code Issues Actions 27 Packages Projects Releases Wiki Activity

[tools] another normalization fix for motor_mixing.py

Browse Source 
If the input matrix is not normalized (i.e. the max distance to the center is not 1.0), normalize that again first.
Also add some more comments...

Now the output should (tm) finally be correct for all cases.
This commit is contained in:
Felix Ruess
2015-07-09 23:59:58 +02:00
parent 3fb7080cb4
commit 0af38f7a8c
1 changed files with 8 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/tools/motor_mixing.py
+8 -4
View File
@@ -55,13 +55,17 @@ class MotorMixing(object):
# Moore-Penrose pseudoinverse of input matrix (A) # Moore-Penrose pseudoinverse of input matrix (A)
B = np.linalg.pinv(np.asarray(input_matrix)) B = np.linalg.pinv(np.asarray(input_matrix))
#print(B) #print(B)
# normalize roll/pitch to the largest of both # normalize inputs in xy plane to distance of 1.0 to center
# normalize yaw to 0.5 xy = input_matrix[0:2, :]
# and transpose xy_normalized = xy / np.linalg.norm(xy, axis=0)
max_lever = np.fabs(input_matrix[0:2, :]).max() # maximum distance to either x or y axis (effective lever arm for that axis)
max_lever = xy_normalized.max()
# normalize roll/pitch to the largest lever arm of both
rp_max = np.fabs(B[:, 0:2]).max() / max_lever rp_max = np.fabs(B[:, 0:2]).max() / max_lever
# normalize yaw to 0.5
y_max = 2 * np.fabs(B[:, 2]).max() y_max = 2 * np.fabs(B[:, 2]).max()
n = np.array([rp_max, rp_max, y_max]) n = np.array([rp_max, rp_max, y_max])
# normalize and transpose
B_nt = (B / n).T B_nt = (B / n).T
if scale is None: if scale is None:
return B_nt return B_nt