Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-25 23:46:04 +08:00
Code Issues Actions 28 Packages Projects Releases Wiki Activity

[motor mixing] update script and examples

Browse Source
This commit is contained in:
Felix Ruess
2015-05-06 22:14:23 +02:00
parent fec45ed7d0
commit 1d7ca52bb1
2 changed files with 65 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/subsystems/actuators/motor_mixing_types.h
+3 -3
View File
@@ -80,7 +80,7 @@
#define MOTOR_MIXING_NB_MOTOR 6
#define MOTOR_MIXING_SCALE 256
#define MOTOR_MIXING_ROLL_COEF { 128, -128, -256, -128, 128, 256 }
#define MOTOR_MIXING_PITCH_COEF { 221, 221, 0, -221, -221, 0 }
#define MOTOR_MIXING_PITCH_COEF { 222, 222, 0, -222, -222, 0 }
#define MOTOR_MIXING_YAW_COEF { -128, 128, -128, 128, -128, 128 }
#define MOTOR_MIXING_THRUST_COEF { 256, 256, 256, 256, 256, 256 }
@@ -97,8 +97,8 @@
#define MOTOR_FRONT_LEFT 5
#define MOTOR_MIXING_NB_MOTOR 6
#define MOTOR_MIXING_SCALE 256
#define MOTOR_MIXING_ROLL_COEF { 0, -181, -181, 0, 181, 181 }
#define MOTOR_MIXING_PITCH_COEF { 256, 181, -181, -256, -181, 181 }
#define MOTOR_MIXING_ROLL_COEF { 0, -222, -222, 0, 222, 222 }
#define MOTOR_MIXING_PITCH_COEF { 256, 128, -128, -256, -128, 128 }
#define MOTOR_MIXING_YAW_COEF { -128, 128, -128, 128, -128, 128 }
#define MOTOR_MIXING_THRUST_COEF { 256, 256, 256, 256, 256, 256 }
sw/tools/motor_mixing.py
+62 -13
View File
@@ -16,17 +16,31 @@ class MotorMixing(object):
def __init__(self):
self.rotors = []
def add_rotor(self, x, y, r):
def clear_rotors(self):
self.rotors = []
def add_rotor(self, x, y, d):
"""Add a rotor with x and y coordinates (body frame) and rotation direction"""
self.rotors.append([x, y, r])
self.rotors.append([x, y, d])
def add_rotors(self, nb, offset=0.0):
""" Add multiple rotors, placed symmetrically with optional angle offset.
Adds rotors in clockwise order, starting with CW rotation direction (and then alternating).
"""
direction = self.CW
for i in range(0, nb):
angle = i * 2 * np.pi / nb + offset
# adding in CW direction (negative mathematical angle)
self.add_rotor(np.cos(angle), np.sin(angle), direction)
direction *= -1
def calc_rotor_matrix(self, rotors):
""" convert a list of rotors to input matrix for coefficient calculation
arguments:
rotors -- list of rotors, each rotor as [x,y,r]
rotors -- list of rotors, each rotor as [x,y,d]
Rotor positions in body frame (x forward, y right) with rotation direction r (negative is CW, positive CCW)
Rotor positions in body frame (x forward, y right) with rotation direction d (negative is CW, positive CCW)
"""
m = np.asarray(rotors)
@@ -40,10 +54,14 @@ class MotorMixing(object):
input_matrix = self.calc_rotor_matrix(self.rotors)
# Moore-Penrose pseudoinverse of input matrix (A)
B = np.linalg.pinv(np.asarray(input_matrix))
#print(B)
# normalize roll/pitch to the largest of both
# normalize yaw to 0.5
# and transpose
rp_max = np.fabs(B[:, 0:2]).max()
#rp_max = np.fabs(np.linalg.norm(B[:, 0:2])).max()
#rp_max = np.fabs(np.sum(B[:, 0:2], axis=1)).max()
#rp_max = 2
y_max = 2 * np.fabs(B[:, 2]).max()
n = np.array([rp_max, rp_max, y_max])
B_nt = (B / n).T
@@ -74,16 +92,47 @@ class MotorMixing(object):
print(' <define name="THRUST_COEF" value="{' + string.join([fmt(scale)] * coeffs.shape[1], ', ') + '}"/>')
print('</section>')
def print_rotors(self, header=None):
if header is not None:
print(header)
for r in self.rotors:
print("x={: .3f}, y={: .3f}, d={}".format(r[0], r[1], "CW" if r[2] < 0 else "CCW"))
if __name__ == '__main__':
"""Example for hexa in X configuration"""
mm = MotorMixing()
mm.add_rotor(0.87, -0.5, mm.CW)
mm.add_rotor(0.87, 0.5, mm.CCW)
mm.add_rotor(0, 1, mm.CW)
mm.add_rotor(-0.87, 0.5, mm.CCW)
mm.add_rotor(-0.87, -0.5, mm.CW)
mm.add_rotor(0, -1, mm.CCW)
"""Print some example motor mixing configurations"""
print("Example for hexa in X configuration:\n")
mm = MotorMixing()
print("\nExample for quad in + configuration:")
mm.clear_rotors()
mm.add_rotors(4)
mm.print_xml()
print("\nExample for quad in x configuration:")
mm.clear_rotors()
# first rotor is front left
mm.add_rotors(4, np.radians(-45))
mm.print_xml()
print("Ahrg, normalization not correct if there is no rotor on x or y axis!")
print("\nExample for hexa in + configuration:")
mm.clear_rotors()
mm.add_rotors(6)
mm.print_xml()
mm.clear_rotors()
print("\nExample for hexa in x configuration:")
mm.add_rotors(6, np.radians(-30))
mm.print_xml()
print("\nExample for hexa in slight V configuration:")
mm.clear_rotors()
mm.add_rotor(-0.35, 0.17, mm.CW)
mm.add_rotor(-0.35, -0.17, mm.CCW)
mm.add_rotor(0, 0.25, mm.CCW)
mm.add_rotor(0, -0.25, mm.CW)
mm.add_rotor(0.35, 0.33, mm.CW)
mm.add_rotor(0.35, -0.33, mm.CCW)
mm.print_rotors("rotor positions in body frame:")
mm.print_xml()