Helicopter aiframe and basic PID control (#5339)

* Helicopter aiframe and basic PID control

* codestyle

* Blade 130x gains working but not optimal

* Changed behaviour of feedforward for all multicopter

ROMFS/px4fmu_common/init.d/16001_helicopter

+#!nsh
+#
+# @name Blade 130X
+#
+# @type Helicopter
+#
+# @maintainer Bart Slinger <bartslinger@gmail.com>
+#
+
+sh /etc/init.d/rc.mc_defaults
+
+# Configure as helicopter (number 4 defined in commander_helper.cpp)
+set MAV_TYPE 4
+
+set MIXER heli_120deg
+
+set PWM_OUT 1234
+
+if [ $AUTOCNF == yes ]
+then
+	param set MC_ROLL_P 5.0
+	param set MC_ROLLRATE_P 0.0
+	param set MC_ROLLRATE_I 0.0
+	param set MC_ROLLRATE_D 0.0
+	param set MC_ROLLRATE_FF 0.15
+	param set MC_PITCH_P 6.5
+	param set MC_PITCHRATE_P 0.0
+	param set MC_PITCHRATE_I 0.0
+	param set MC_PITCHRATE_D 0.0
+	param set MC_PITCHRATE_FF 0.15
+	param set MC_YAW_P 3.0
+	param set MC_YAWRATE_P 0.1
+	param set MC_YAWRATE_I 0.0
+	param set MC_YAWRATE_D 0.0
+	param set MC_YAW_FF 0.0
+	param set MC_ROLLRATE_MAX 720.0
+	param set MC_PITCHRATE_MAX 720.0
+	param set MC_YAWRATE_MAX 400.0
+	param set MC_ACRO_R_MAX 360.0
+	param set MC_ACRO_P_MAX 360.0
+
+	param set PWM_MIN 1075
+
+	param set MPC_THR_MIN 0.06
+	param set MPC_MANTHR_MIN 0.06
+
+	param set ATT_BIAS_MAX 0.0
+
+	param set CBRK_IO_SAFETY 22027
+fi

ROMFS/px4fmu_common/mixers/heli_120deg.main.mix

+Helicopter 120 degree Cyclic-Collective-Pitch Mixing (CCPM) for PX4FMU
+Blade 130x helicopter has longer servo arms left and right. The front servo arm is shorter.
+Therefore it is not required to use the 0.866 factor.
+==================================================
+
+Output 0 - Left Servo Mixer
+-----------------
+Left Servo = Collective (Thurst - 3) - 0.5 * Elevator (Pitch - 1) + Aileron (Roll - 0)
+
+M: 3
+O:       10000   10000      0 -10000  10000
+S: 3 5   10000   10000      0 -10000  10000
+S: 0 1  -10000  -10000      0 -10000  10000
+S: 0 0   10000   10000      0 -10000  10000
+
+
+Output 1 - Front Servo Mixer
+----------------
+
+Rear Servo = Collective (Thrust - 3) + Elevator (Pitch - 1)
+
+M: 2
+O:      10000  10000      0 -10000  10000
+S: 3 5  10000  10000      0 -10000  10000
+S: 0 1  10000  10000      0 -10000  10000
+
+
+Output 2 - Right Servo Mixer
+----------------
+Right Servo = Collective (Thurst - 3) - 0.5 * Elevator (Pitch - 1) - Aileron (Roll - 0)
+
+M: 3
+O:       10000   10000      0 -10000  10000
+S: 3 5   10000   10000      0 -10000  10000
+S: 0 1  -10000  -10000      0 -10000  10000
+S: 0 0  -10000  -10000      0 -10000  10000
+
+
+Output 3 - Tail Servo Mixer
+----------------
+Tail Servo = Yaw (control index = 2)
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 2  10000  10000      0 -10000  10000
+
+
+Output 4 - Motor speed mixer
+-----------------
+This would be the motor speed control output from governor power demand- not sure what index to use here?
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 3      0  20000 -10000 -10000  10000

src/modules/mc_att_control/mc_att_control_main.cpp

@@ -788,8 +788,10 @@ MulticopterAttitudeControl::control_attitude_rates(float dt)
 
 	/* angular rates error */
 	math::Vector<3> rates_err = _rates_sp - rates;
+
 	_att_control = _params.rate_p.emult(rates_err * tpa) + _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int +
-		       _params.rate_ff.emult(_rates_sp - _rates_sp_prev) / dt;
+		       _params.rate_ff.emult(_rates_sp);
+
 	_rates_sp_prev = _rates_sp;
 	_rates_prev = rates;