Merge remote-tracking branch 'upstream/master' into ros

src/modules/systemlib/mixer/mixer.h

@@ -441,7 +441,6 @@ private:
 	SimpleMixer operator=(const SimpleMixer&);
 };
 
-
 /**
  * Supported multirotor geometries.
  *
@@ -460,12 +459,14 @@ class __EXPORT MultirotorMixer : public Mixer
 {
 public:
 	/**
+
 	 * Precalculated rotor mix.
 	 */
 	struct Rotor {
 		float	roll_scale;	/**< scales roll for this rotor */
 		float	pitch_scale;	/**< scales pitch for this rotor */
 		float	yaw_scale;	/**< scales yaw for this rotor */
+		float	out_scale;	/**< scales total out for this rotor */
 	};
 
 	/**

src/modules/systemlib/mixer/mixer_multirotor.cpp

@@ -75,7 +75,8 @@ float constrain(float val, float min, float max)
 {
 	return (val < min) ? min : ((val > max) ? max : val);
 }
-}
+
+} // anonymous namespace
 
 MultirotorMixer::MultirotorMixer(ControlCallback control_cb,
 				 uintptr_t cb_handle,
@@ -89,6 +90,7 @@ MultirotorMixer::MultirotorMixer(ControlCallback control_cb,
 	_pitch_scale(pitch_scale),
 	_yaw_scale(yaw_scale),
 	_idle_speed(-1.0f + idle_speed * 2.0f),	/* shift to output range here to avoid runtime calculation */
+	_limits_pub(),
 	_rotor_count(_config_rotor_count[(MultirotorGeometryUnderlyingType)geometry]),
 	_rotors(_config_index[(MultirotorGeometryUnderlyingType)geometry])
 {
@@ -152,6 +154,9 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl
 	} else if (!strcmp(geomname, "4w")) {
 		geometry = MultirotorGeometry::QUAD_WIDE;
 
+	} else if (!strcmp(geomname, "4dc")) {
+		geometry = MultirotorGeometry::QUAD_DEADCAT;
+
 	} else if (!strcmp(geomname, "6+")) {
 		geometry = MultirotorGeometry::HEX_PLUS;
 
@@ -212,6 +217,8 @@ MultirotorMixer::mix(float *outputs, unsigned space)
 			    pitch * _rotors[i].pitch_scale +
 			    thrust;
 
+		out *= _rotors[i].out_scale;
+
 		/* limit yaw if it causes outputs clipping */
 		if (out >= 0.0f && out < -yaw * _rotors[i].yaw_scale) {
 			yaw = -out / _rotors[i].yaw_scale;

src/modules/systemlib/mixer/multi_tables.py

@@ -69,6 +69,13 @@ quad_plus = [
     [ 180, CW],
 ]
 
+quad_deadcat = [
+    [  63, CCW, 1.0],
+    [-135, CCW, 0.964],
+    [ -63, CW, 1.0],
+    [ 135, CW, 0.964],
+]
+
 quad_v = [
     [  18.8, 0.4242],
     [ -18.8, 1.0],
@@ -148,13 +155,18 @@ twin_engine = [
     [-90, 0.0],
 ]
 
+
+tables = [quad_x, quad_plus, quad_v, quad_wide, quad_deadcat, hex_x, hex_plus, hex_cox, octa_x, octa_plus, octa_cox, twin_engine]
+
 def variableName(variable):
     for variableName, value in list(globals().items()):
         if value is variable:
             return variableName
 
-tables = [quad_x, quad_plus, quad_v, quad_wide, hex_x, hex_plus, hex_cox, octa_x, octa_plus, octa_cox, twin_engine]
-
+def unpackScales(scalesList):
+    if len(scalesList) == 2:
+        scalesList += [1.0] #Add thrust scale
+    return scalesList
 
 def printEnum():
     print("enum class MultirotorGeometry : MultirotorGeometryUnderlyingType {")
@@ -167,10 +179,11 @@ def printEnum():
 def printScaleTables():
     for table in tables:
         print("const MultirotorMixer::Rotor _config_{}[] = {{".format(variableName(table)))
-        for (angle, yawScale) in table:
+        for row in table:
+            angle, yawScale, thrustScale = unpackScales(row)
             rollScale = rcos(angle + 90)
             pitchScale = rcos(angle)
-            print("\t{{ {:9f}, {:9f}, {:9f} }},".format(rollScale, pitchScale, yawScale))
+            print("\t{{ {:9f}, {:9f}, {:9f}, {:9f} }},".format(rollScale, pitchScale, yawScale, thrustScale))
         print("};\n")
 
 def printScaleTablesIndex():