Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-01 02:55:07 +08:00
Code Issues Actions 19 Packages Projects Releases Wiki Activity

px_generate_mixers.py: scale mixer so that sum(z_thrust_scale)==n_rotors

Browse Source
This commit is contained in:
Julien Lecoeur
2018-01-09 12:21:22 +01:00
committed by Daniel Agar
parent 262d9c790b
commit b7d7d1209b
1 changed files with 2 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/mixer/geometries/tools/px_generate_mixers.py
+2 -1
View File
@@ -195,6 +195,7 @@ def normalize_mix_px4(B):
''' '''
B_norm = np.linalg.norm(B, axis=0) B_norm = np.linalg.norm(B, axis=0)
B_max = np.abs(B).max(axis=0) B_max = np.abs(B).max(axis=0)
B_sum = np.sum(B, axis=0)
# Same scale on roll and pitch # Same scale on roll and pitch
B_norm[0] = max(B_norm[0], B_norm[1]) / np.sqrt(B.shape[0] / 2.0) B_norm[0] = max(B_norm[0], B_norm[1]) / np.sqrt(B.shape[0] / 2.0)
@@ -208,7 +209,7 @@ def normalize_mix_px4(B):
B_norm[4] = B_norm[3] B_norm[4] = B_norm[3]
# Scale z thrust separately # Scale z thrust separately
B_norm[5] = B_max[5] B_norm[5] = - B_sum[5] / np.count_nonzero(B[:,5])
# Normalize # Normalize
B_norm[np.abs(B_norm) < 1e-3] = 1 B_norm[np.abs(B_norm) < 1e-3] = 1