diff --git a/ROMFS/px4fmu_common/init.d/16001_helicopter b/ROMFS/px4fmu_common/init.d/16001_helicopter new file mode 100644 index 0000000000..fba8f9ebf0 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/16001_helicopter @@ -0,0 +1,50 @@ +#!nsh +# +# @name Blade 130X +# +# @type Helicopter +# +# @maintainer Bart Slinger +# + +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 diff --git a/ROMFS/px4fmu_common/mixers/heli_120deg.main.mix b/ROMFS/px4fmu_common/mixers/heli_120deg.main.mix new file mode 100644 index 0000000000..a5e57e5952 --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/heli_120deg.main.mix @@ -0,0 +1,54 @@ +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 diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index dd486c5347..10335a1223 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/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;