thank you for your contribution. For easier reading I made a diff against v2.3
Code: Select all
diff -BbwE -r -u MultiWii_2_3/MultiWii/MultiWii.cpp multiwii-helimod/MultiWii/MultiWii.cpp
--- MultiWii_2_3/MultiWii/MultiWii.cpp 2013-11-07 23:56:18.000000000 +0100
+++ multiwii-helimod/MultiWii/MultiWii.cpp 2014-10-06 20:26:31.000000000 +0200
@@ -1335,6 +1335,19 @@
//-----calculate total PID output
axisPID[axis] = PTerm + ITerm + DTerm;
}
+
+ // pirouette optimization ( rotate horizontal error term )
+ // rescale gyrodata to radian: gyrodata * (freq: cycletime/1000000) * (gyro2deg: 2000/8192) * (deg2rad: 2Pi/360)
+ double GyroToRad = cycleTime * 0.000000004259;
+ double Sint = sin(-imu.gyroData[YAW] * GyroToRad);
+ double Cost = cos(-imu.gyroData[YAW] * GyroToRad);
+
+ errorGyroI[ROLL] = errorGyroI[ROLL]*Cost + errorGyroI[PITCH]*Sint;
+ errorGyroI[PITCH] = -errorGyroI[ROLL]*Sint + errorGyroI[PITCH]*Cost;
+ axisPID[ROLL] = axisPID[ROLL]*Cost + axisPID[PITCH]*Sint;
+ axisPID[PITCH] = -axisPID[ROLL]*Sint + axisPID[PITCH]*Cost;
+
+
#else
#error "*** you must set PID_CONTROLLER to one existing implementation"
#endif
diff -BbwE -r -u MultiWii_2_3/MultiWii/Output.cpp multiwii-helimod/MultiWii/Output.cpp
--- MultiWii_2_3/MultiWii/Output.cpp 2013-09-09 23:41:40.000000000 +0200
+++ multiwii-helimod/MultiWii/Output.cpp 2014-10-06 20:26:31.000000000 +0200
@@ -1269,11 +1269,25 @@
/* Throttle & YAW
******************** */
// Yaw control is common for Heli 90 & 120
+ //servo[5] = (axisPID[YAW] * SERVODIR(5,1)) + conf.servoConf[5].middle + servo[7]<<7 + collective<<8;
+
+ // tail precomp from collective
+ if (abs(collective) > YAW_COLL_PRECOMP_DEADBAND ){
+ servo[5] = (axisPID[YAW] * SERVODIR(5,1)) + conf.servoConf[5].middle + (abs(collective)-YAW_COLL_PRECOMP_DEADBAND) * YAW_COLL_PRECOMP;
+ }else{
servo[5] = (axisPID[YAW] * SERVODIR(5,1)) + conf.servoConf[5].middle;
- #if YAWMOTOR
+ }
+
+ #if (YAWMOTOR || YAWSTOP)
servo[5] = constrain(servo[5], conf.servoConf[5].min, conf.servoConf[5].max); // limit the values
- if (rcCommand[THROTTLE]<conf.minthrottle || !f.ARMED) {servo[5] = MINCOMMAND;} // Kill YawMotor
+ if (rcCommand[THROTTLE] <= conf.minthrottle || !f.ARMED) {servo[5] = MINCOMMAND;} // Kill YawMotor
+ else{
+ if ( servo[5] < 1030) { // don't stop tail motor
+ servo[5] = 1030;
+ }
+ }
#endif
+
if (!f.ARMED){
servo[7] = MINCOMMAND; // Kill throttle when disarmed
} else {
diff -BbwE -r -u MultiWii_2_3/MultiWii/config.h multiwii-helimod/MultiWii/config.h
--- MultiWii_2_3/MultiWii/config.h 2013-11-08 00:00:00.000000000 +0100
+++ multiwii-helimod/MultiWii/config.h 2014-10-06 20:26:31.000000000 +0200
@@ -50,7 +50,7 @@
//#define AIRPLANE
//#define SINGLECOPTER
//#define DUALCOPTER
- //#define HELI_120_CCPM
+ #define HELI_120_CCPM
//#define HELI_90_DEG
/**************************** Motor minthrottle *******************************/
@@ -60,16 +60,16 @@
//#define MINTHROTTLE 1120 // for Super Simple ESCs 10A
//#define MINTHROTTLE 1064 // special ESC (simonk)
//#define MINTHROTTLE 1050 // for brushed ESCs like ladybird
- #define MINTHROTTLE 1150 // (*) (**)
+ #define MINTHROTTLE 0 // (*) (**)
/**************************** Motor maxthrottle *******************************/
/* this is the maximum value for the ESCs at full power, this value can be increased up to 2000 */
- #define MAXTHROTTLE 1850
+ #define MAXTHROTTLE 1950
/**************************** Mincommand *******************************/
/* this is the value for the ESCs when they are not armed
in some cases, this value must be lowered down to 900 for some specific ESCs, otherwise they failed to initiate */
- #define MINCOMMAND 1000
+ #define MINCOMMAND 0
/********************************** I2C speed ************************************/
#define I2C_SPEED 100000L //100kHz normal mode, this value must be used for a genuine WMP
@@ -96,7 +96,7 @@
//#define FREEIMUv035_BMP // FreeIMU v0.3.5_BMP
//#define FREEIMUv04 // FreeIMU v0.4 with MPU6050, HMC5883L, MS561101BA <- confirmed by Alex
//#define FREEIMUv043 // same as FREEIMUv04 with final MPU6050 (with the right ACC scale)
- //#define NANOWII // the smallest multiwii FC based on MPU6050 + pro micro based proc <- confirmed by Alex
+ #define NANOWII // the smallest multiwii FC based on MPU6050 + pro micro based proc <- confirmed by Alex
//#define PIPO // 9DOF board from erazz
//#define QUADRINO // full FC board 9DOF+baro board from witespy with BMP085 baro <- confirmed by Alex
//#define QUADRINO_ZOOM // full FC board 9DOF+baro board from witespy second edition
@@ -109,7 +109,7 @@
//#define SIRIUS600 // Sirius Navigator IMU using the WMP for the gyro
//#define SIRIUS_AIR // Sirius Navigator IMU 6050 32U4 from MultiWiiCopter.com <- confirmed by Alex
//#define SIRIUS_AIR_GPS // Sirius Navigator IMU 6050 32U4 from MultiWiiCopter.com with GPS/MAG remote located
- //#define SIRIUS_MEGAv5_OSD // Paris_Sirius‚Ñ¢ ITG3050,BMA280,MS5611,HMC5883,uBlox http://www.Multiwiicopter.com <- confirmed by Alex
+ //#define SIRIUS_MEGAv5_OSD // Paris_Sirius? ITG3050,BMA280,MS5611,HMC5883,uBlox http://www.Multiwiicopter.com <- confirmed by Alex
//#define MINIWII // Jussi's MiniWii Flight Controller <- confirmed by Alex
//#define MICROWII // MicroWii 10DOF with ATmega32u4, MPU6050, HMC5883L, MS561101BA from http://flyduino.net/
//#define CITRUSv2_1 // CITRUS from qcrc.ca
@@ -199,7 +199,7 @@
/* enforce your individual sensor orientation - even overrides board specific defaults */
//#define FORCE_ACC_ORIENTATION(X, Y, Z) {imu.accADC[ROLL] = Y; imu.accADC[PITCH] = -X; imu.accADC[YAW] = Z;}
- //#define FORCE_GYRO_ORIENTATION(X, Y, Z) {imu.gyroADC[ROLL] = -Y; imu.gyroADC[PITCH] = X; imu.gyroADC[YAW] = Z;}
+ #define FORCE_GYRO_ORIENTATION(X, Y, Z) {imu.gyroADC[ROLL] = Z; imu.gyroADC[PITCH] = Y; imu.gyroADC[YAW] = X;}
//#define FORCE_MAG_ORIENTATION(X, Y, Z) {imu.magADC[ROLL] = X; imu.magADC[PITCH] = Y; imu.magADC[YAW] = Z;}
/* Board orientation shift */
@@ -220,13 +220,13 @@
* 1 = evolved oldschool algorithm (similar to v2.2)
* 2 = new experimental algorithm from Alex Khoroshko - unsupported - http://www.multiwii.com/forum/viewtopic.php?f=8&t=3671&start=10#p37387
* */
- #define PID_CONTROLLER 1
+ #define PID_CONTROLLER 2
/* NEW: not used anymore for servo coptertypes <== NEEDS FIXING - MOVE TO WIKI */
#define YAW_DIRECTION 1
//#define YAW_DIRECTION -1 // if you want to reverse the yaw correction direction
- #define ONLYARMWHENFLAT //prevent the copter from arming when the copter is tilted
+// #define ONLYARMWHENFLAT //prevent the copter from arming when the copter is tilted
/******************************** ARM/DISARM *********************************/
/* optionally disable stick combinations to arm/disarm the motors.
@@ -281,7 +281,8 @@
/*********************** Heli ***********************/
/* Channel to control CollectivePitch */
- #define COLLECTIVE_PITCH THROTTLE
+ //#define COLLECTIVE_PITCH THROTTLE
+ #define COLLECTIVE_PITCH AUX2
/* Limit the range of Collective Pitch. 100% is Full Range each way and position for Zero Pitch */
#define COLLECTIVE_RANGE { 80, 0, 80 }// {Min%, ZeroPitch offset from 1500, Max%}.
@@ -300,6 +301,11 @@
if inactive, throttle output will be treated as a motor output, so it can drive an ESC */
//#define HELI_USE_SERVO_FOR_THROTTLE
+ // sachiho add for helicopter
+ #define YAWSTOP 1 // cut tail motor when throttle is zero :for brushless tail ESC driven by servo PWM signal
+ #define YAW_COLL_PRECOMP 1.5 // increase tail thrust with respect to abs(collective pitch)
+ #define YAW_COLL_PRECOMP_DEADBAND 120
+
/*********************** your individual mixing ***********************/
/* if you want to override an existing entry in the mixing table, you may want to avoid editing the
* mixTable() function for every version again and again.
@@ -343,7 +348,7 @@
Spektrum Satellites are 3V devices. DO NOT connect to 5V!
For MEGA boards, attach sat grey wire to RX1, pin 19. Sat black wire to ground. Sat orange wire to Mega board's 3.3V (or any other 3V to 3.3V source).
For PROMINI, attach sat grey to RX0. Attach sat black to ground. */
- //#define SPEKTRUM 1024
+ #define SPEKTRUM 1024
//#define SPEKTRUM 2048
//#define SPEK_SERIAL_PORT 1 // Forced to 0 on Pro Mini and single serial boards; Set to your choice of 0, 1, or 2 on any Mega based board (defaults to 1 on Mega).
//**************************
@@ -354,7 +359,7 @@
// Normally use 3.3V regulator is needed on the power pin!! If your satellite hangs during bind (blinks, but won't complete bind with a solid light), go direct 5V on all pins.
//**************************
// For Pro Mini, the connector for the Satellite that resides on the FTDI can be unplugged and moved to these three adjacent pins.
- //#define SPEK_BIND //Un-Comment for Spektrum Satellie Bind Support. Code is ~420 bytes smaller without it.
+ #define SPEK_BIND //Un-Comment for Spektrum Satellie Bind Support. Code is ~420 bytes smaller without it.
//#define SPEK_BIND_GROUND 4
//#define SPEK_BIND_POWER 5
//#define SPEK_BIND_DATA 6
@@ -502,7 +507,7 @@
//#define MPU6050_LPF_188HZ
//#define MPU6050_LPF_98HZ
//#define MPU6050_LPF_42HZ
- //#define MPU6050_LPF_20HZ
+ #define MPU6050_LPF_20HZ
//#define MPU6050_LPF_10HZ
//#define MPU6050_LPF_5HZ // Use this only in extreme cases, rather change motors and/or props
@@ -510,7 +515,7 @@
/* GYRO_SMOOTHING. In case you cannot reduce vibrations _and_ _after_ you have tried the low pass filter options, you
may try this gyro smoothing via averaging. Not suitable for multicopters!
Good results for helicopter, airplanes and flying wings (foamies) with lots of vibrations.*/
- //#define GYRO_SMOOTHING {20, 20, 3} // (*) separate averaging ranges for roll, pitch, yaw
+ #define GYRO_SMOOTHING {60, 60, 1} // (*) separate averaging ranges for roll, pitch, yaw
/************************ Moving Average Gyros **********************************/
//#define MMGYRO 10 // (*) Active Moving Average Function for Gyros
@@ -850,7 +855,7 @@
* + want altitude readout and/or variometer
* + do not use altitude hold feature
* + want to save memory space */
- //#define SUPPRESS_BARO_ALTHOLD
+ #define SUPPRESS_BARO_ALTHOLD
/********************************************************************/
/**** altitude variometer ****/
@@ -914,7 +919,7 @@
/*********************** Servo Refreshrates ***********************/
/* Default 50Hz Servo refresh rate*/
- #define SERVO_RFR_50HZ
+ //#define SERVO_RFR_50HZ
/* up to 160Hz servo refreshrate .. works with the most analog servos*/
//#define SERVO_RFR_160HZ
@@ -946,10 +951,10 @@
* servo[5] = yaw servo = pin 5
* servo[6] = right servo= pin 9
*/
- //#define A32U4_4_HW_PWM_SERVOS
+ #define A32U4_4_HW_PWM_SERVOS
- #define SERVO_RFR_RATE 50 // In Hz, you can set it from 20 to 400Hz, used only in HW PWM mode for mega and 32u4
- //#define SERVO_PIN5_RFR_RATE 200 // separate yaw pwm rate.
+ #define SERVO_RFR_RATE 60 // In Hz, you can set it from 20 to 400Hz, used only in HW PWM mode for mega and 32u4
+ #define SERVO_PIN5_RFR_RATE 100 // separate yaw pwm rate.
// In Hz, you can set it from 20 to 400Hz, used only in HW PWM mode for 32u4
In both cases, the second computation uses the result of xy[ROLL] from the previous line. This is not typical of applying rotation and maybe not intended?