ArduCopterNG: added TraditionalHeli support, changed G_dt to work in microseconds (Issue #37),

git-svn-id: https://arducopter.googlecode.com/svn/trunk@909 f9c3cf11-9bcb-44bc-f272-b75c42450872

ArducopterNG/ArduUser.h

@@ -33,6 +33,13 @@ TODO:
 
 * ************************************************************** */
 
+/*************************************************************/
+// Airframe
+#define QUAD 0
+#define HELI 1
+
+#define AIRFRAME QUAD
+
 /*************************************************************/
 // Safety & Security 
 
@@ -138,5 +145,3 @@ TODO:
 #define ACCELZ 5
 #define LASTSENSOR 6
 
-
-

ArducopterNG/Arducopter.h

@@ -264,6 +264,7 @@ float err_pitch;
 float yaw_I=0;
 float yaw_D;
 float err_yaw;
+float heading_I=0;  // used only by heli
 
 //Position control
 long target_longitude;

ArducopterNG/ArducopterNG.pde

@@ -182,6 +182,9 @@
 //#include <GPS_NMEA.h>   // ArduPilot NMEA GPS library
 #endif
 
+#if AIRFRAME == HELI
+#include "Heli.h"
+#endif
 
 /* Software version */
 #define VER 1.52    // Current software version (only numeric values)
@@ -197,7 +200,8 @@ APM_Compass_Class	APM_Compass;
 
 byte flightMode;
 
-unsigned long currentTime, previousTime;
+unsigned long currentTime;  // current time in milliseconds
+unsigned long currentTimeMicros = 0, previousTimeMicros = 0;  // current and previous loop time in microseconds
 unsigned long mainLoop = 0;
 unsigned long mediumLoop = 0;
 unsigned long slowLoop = 0;
@@ -252,13 +256,16 @@ void loop()
   //int i;
   //float aux_float;
 
-  currentTime = millis();
+  currentTimeMicros = micros();
+  currentTime = currentTimeMicros / 1000;
 
   // Main loop at 200Hz (IMU + control)
   if ((currentTime-mainLoop) > 5)    // about 200Hz (every 5ms)
   {
-    G_Dt = (currentTime-mainLoop)*0.001;   // Microseconds!!!
+    //G_Dt = (currentTime-mainLoop)*0.001;   // Microseconds!!!
+    G_Dt = (currentTimeMicros-previousTimeMicros) * 0.000001;   // Microseconds!!!
     mainLoop = currentTime;
+    previousTimeMicros = currentTimeMicros;
 
     //IMU DCM Algorithm
     Read_adc_raw();       // Read sensors raw data
@@ -268,16 +275,33 @@ void loop()
     Euler_angles();
 
     // Read radio values (if new data is available)
-    if (APM_RC.GetState() == 1)   // New radio frame?
+    if (APM_RC.GetState() == 1) {  // New radio frame?
+#if AIRFRAME == QUAD    
       read_radio();
+#endif
+#if AIRFRAME == HELI
+      heli_read_radio();
+#endif
+    }
 
     // Attitude control
     if(flightMode == STABLE_MODE) {    // STABLE Mode
       gled_speed = 1200;
-      if (AP_mode == AP_NORMAL_MODE)    // Normal mode
+      if (AP_mode == AP_NORMAL_MODE) {   // Normal mode
+#if AIRFRAME == QUAD
         Attitude_control_v3(command_rx_roll,command_rx_pitch,command_rx_yaw);
-      else                              // Automatic mode : GPS position hold mode
+#endif        
+#if AIRFRAME == HELI
+        heli_attitude_control(command_rx_roll,command_rx_pitch,command_rx_collective,command_rx_yaw);
+#endif
+      }else{                        // Automatic mode : GPS position hold mode
+#if AIRFRAME == QUAD      
         Attitude_control_v3(command_rx_roll+command_gps_roll,command_rx_pitch+command_gps_pitch,command_rx_yaw);
+#endif        
+#if AIRFRAME == HELI
+        heli_attitude_control(command_rx_roll+command_gps_roll,command_rx_pitch+command_gps_pitch,command_rx_collective,command_rx_yaw);
+#endif
+      }
     }
     else {                 // ACRO Mode
       gled_speed = 400;
@@ -287,7 +311,9 @@ void loop()
     }
 
     // Send output commands to motor ESCs...
+#if AIRFRAME == QUAD     // we update the heli swashplate at about 60hz
     motor_output();
+#endif    
 
 #ifdef IsCAM
   // Do we have cameras stabilization connected and in use?
@@ -355,6 +381,11 @@ void loop()
 #ifdef IsGPS
     GPS.Read();     // Read GPS data 
 #endif
+    
+#if AIRFRAME == HELI    
+    // Send output commands to heli swashplate...
+    heli_moveSwashPlate();
+#endif
     // Each of the six cases executes at 10Hz
     switch (medium_loopCounter){
     case 0:   // Magnetometer reading (10Hz)

ArducopterNG/GCS.pde

@@ -134,6 +134,9 @@ void readSerialCommand() {
       break;
     case 'Y': // Initialize EEPROM with default values
       defaultUserConfig();
+#if AIRFRAME == HELI
+      heli_defaultUserConfig();
+#endif      
       break;
     case '1': // Receive transmitter calibration values
       ch_roll_slope = readFloatSerial();

ArducopterNG/Heli.h

@@ -0,0 +1,167 @@
+/*
+ www.ArduCopter.com - www.DIYDrones.com
+ Copyright (c) 2010.  All rights reserved.
+ An Open Source Arduino based multicopter.
+ 
+ File     : Arducopter.h
+ Version  : v1.0, Aug 27, 2010
+ Author(s): ArduCopter Team
+             Ted Carancho (aeroquad), Jose Julio, Jordi Muñoz,
+             Jani Hirvinen, Ken McEwans, Roberto Navoni,          
+             Sandro Benigno, Chris Anderson
+ 
+ This program is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+ 
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+ 
+ You should have received a copy of the GNU General Public License
+ along with this program. If not, see <http://www.gnu.org/licenses/>.
+
+* ************************************************************** *
+ChangeLog:
+
+
+* ************************************************************** *
+TODO:
+
+
+* ************************************************************** */
+
+#ifndef HELI_H
+#define HELI_H
+
+#include <avr/interrupt.h>
+#include "WProgram.h"
+#include <Wire.h>
+#include <EEPROM.h>   // added by Randy
+#include <APM_ADC.h>                                   // ArduPilot Mega Analog to Digital Converter Library
+#include <APM_RC.h>                                    // ArduPilot Mega RC Library
+#include <APM_Compass.h>                               // ArduPilot Mega Compass Library
+#include <DataFlash.h>                                 // ArduPilot Mega DataFlash Library.
+#include "../AP_Math/AP_Math.h"
+
+/**********************************************************************/
+// Channel definitions
+#define CHANNEL_FRONT_LEFT 0
+#define CHANNEL_FRONT_RIGHT 1
+#define CHANNEL_REAR 2
+#define CHANNEL_YAW 3
+
+/**********************************************************************/
+// EEPROM locations
+#define EEPROM_BASE_ADDRESS 300
+#define EEPROM_MAGIC_NUMBER_ADDR EEPROM_BASE_ADDRESS
+#define FRONT_LEFT_CCPM_MIN_ADDR EEPROM_BASE_ADDRESS+4
+#define FRONT_LEFT_CCPM_MAX_ADDR EEPROM_BASE_ADDRESS+8
+#define FRONT_RIGHT_CCPM_MIN_ADDR EEPROM_BASE_ADDRESS+12
+#define FRONT_RIGHT_CCPM_MAX_ADDR EEPROM_BASE_ADDRESS+16
+#define REAR_CCPM_MIN_ADDR EEPROM_BASE_ADDRESS+20
+#define REAR_CCPM_MAX_ADDR EEPROM_BASE_ADDRESS+24
+#define YAW_MIN_ADDR EEPROM_BASE_ADDRESS+28
+#define YAW_MAX_ADDR EEPROM_BASE_ADDRESS+32
+#define THROTTLE_MIN_ADDR EEPROM_BASE_ADDRESS+36
+#define THROTTLE_MAX_ADDR EEPROM_BASE_ADDRESS+40
+
+#define EEPROM_MAGIC_NUMBER 12345.0
+
+#define YAW_MODE_HEADING_HOLD 0
+#define YAW_MODE_RATE 1
+
+#define HELI_STICK_TO_ANGLE_FACTOR 2.0   // To convert ccpm values (-50 ~ 50 ) to absolute angles.  larger number means less lean
+#define HELI_YAW_STICK_TO_ANGLE_FACTOR 0.5  // convert yaw (-50 ~ 50) to turn rate in degrees per second.  larger number means slower turn rate
+
+// roll and pitch adjustment - attitude close to what's required to keep heli in one place when hovering.  This will be added to the roll/pitch commands from the pilot
+#define HELI_ADJUST_ROLL 4
+#define HELI_ADJUST_PITCH -3
+
+// CCPM Types
+#define HELI_CCPM_120_TWO_FRONT_ONE_BACK 0
+#define HELI_CCPM_120_ONE_FRONT_TWO_BACK 1
+
+// define which CCPM we have
+#define HELI_CCPM HELI_CCPM_120_TWO_FRONT_ONE_BACK
+
+// define DeAllocation matrix(converts radio inputs to roll, pitch and collective
+//   for example roll = (inputCh0*Row1Col1) + (inputCh1*Row1Col2) + (inputCh2*Row1Col3)
+//               pitch = (inputCh0*Row2Col1) + (inputCh1*Row2Col2) + (inputCh2*Row2Col3)
+//               collective = (inputCh0*Row3Col1) + (inputCh1*Row3Col2) + (inputCh2*Row3Col3)
+// and Allocation matrix (converts roll, pitch, collective to servo outputs)
+//   for example servo0 = (roll*Row1Col1) + (pitch*Row1Col2) + (collective*Row1Col3)
+//               servo1 = (roll*Row2Col1) + (pitch*Row2Col2) + (collective*Row2Col3)
+//               servo2 = (roll*Row3Col1) + (pitch*Row3Col2) + (collective*Row3Col3)
+#if HELI_CCPM == HELI_CCPM_120_TWO_FRONT_ONE_BACK
+  #define CCPM_DEALLOCATION   0.5774, -0.5774, 0.0000,  \
+                              0.3333, 0.3333, -0.6667,  \
+                              0.3333, 0.3333, 0.3333
+  #define CCPM_ALLOCATION     0.8660,0.5000,  1.0000,   \
+                             -0.8660, 0.5000, 1.0000,   \
+                              0.0000, -1.0000, 1.0000
+#endif
+
+#if HELI_CCPM == HELI_CCPM_120_ONE_FRONT_TWO_BACK
+  #define CCPM_DEALLOCATION   0.5774, -0.5774, 0.0000,  \
+                              -0.3333,-0.3333, 0.6667,  \
+                              0.3333, 0.3333,  0.3333
+  #define CCPM_ALLOCATION     0.8660, -0.5000, 1.0000,  \
+                             -0.8660, -0.5000, 1.0000,  \
+                              0.0000,  1.0000, 1.0000
+#endif
+
+const Matrix3f ccpmDeallocation(CCPM_DEALLOCATION);
+const Matrix3f ccpmAllocation(CCPM_ALLOCATION);
+
+/**********************************************************************/
+// time variables - we run at a different hertz than quads
+unsigned long heli_previousTimeMicros = 0;
+
+//  PWM Input Processing - Variable Definitions
+float frontLeftCCPMmin;
+float frontLeftCCPMmax;
+float frontLeftCCPMslope;
+float frontLeftCCPMintercept;
+
+float frontRightCCPMmin;
+float frontRightCCPMmax;
+float frontRightCCPMslope;
+float frontRightCCPMintercept;
+
+float rearCCPMmin;
+float rearCCPMmax;
+float rearCCPMslope;
+float rearCCPMintercept;
+
+float yawMin;
+float yawMax;
+float yawSlope;
+float yawIntercept;                            
+
+Vector3f ccpmPercents;                // Array of ccpm input values, converted to percents
+Vector3f rollPitchCollPercent;        // Array containing deallocated roll, pitch and collective percent commands
+float ch_collective;
+int collective_mid;
+float control_collective;
+float command_rx_collective;
+float yawPercent;
+float targetHeading;
+
+/// for sending values out to servos
+Vector3f ccpmPercents_out;            // Array of ccpm input values, converted to percents
+Vector3f pitchRollCollPercent_out;    // Array containing deallocated pitch, roll, and collective percent commands
+
+// heli debug
+int heli_debug = 0;
+
+/**********************************************************************/
+//  Output to Servos
+int leftOut;
+int rightOut;
+int rearOut;
+int yawOut;
+
+#endif HELI_H

ArducopterNG/Heli.pde

@@ -0,0 +1,333 @@
+/*
+ www.ArduCopter.com - www.DIYDrones.com
+ Copyright (c) 2010.  All rights reserved.
+ An Open Source Arduino based multicopter.
+ 
+ File     : Heli.pde
+ Desc     : code specific to traditional helicopters
+ Version  : v1.0, Aug 27, 2010
+ Author(s): ArduCopter Team
+             Ted Carancho (aeroquad), Jose Julio, Jordi Muñoz,
+             Jani Hirvinen, Ken McEwans, Roberto Navoni,          
+             Sandro Benigno, Chris Anderson
+ 
+ This program is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+ 
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+ 
+ You should have received a copy of the GNU General Public License
+ along with this program. If not, see <http://www.gnu.org/licenses/>.
+
+* ************************************************************** *
+ChangeLog:
+
+
+* ************************************************************** *
+TODO:
+
+
+* ************************************************************** */
+
+#if AIRFRAME == HELI
+
+/**********************************************************************/
+// heli_readUserConfig - reads values in from EEPROM
+void heli_readUserConfig() 
+{ 
+    float magicNum = 0;
+    magicNum = readEEPROM(EEPROM_MAGIC_NUMBER_ADDR);
+    if( magicNum != EEPROM_MAGIC_NUMBER ) {
+        SerPri("No heli settings found in EEPROM.  Using defaults");
+        heli_defaultUserConfig();
+    }else{
+        frontLeftCCPMmin = readEEPROM(FRONT_LEFT_CCPM_MIN_ADDR);
+        frontLeftCCPMmax = readEEPROM(FRONT_LEFT_CCPM_MAX_ADDR);
+        frontRightCCPMmin = readEEPROM(FRONT_RIGHT_CCPM_MIN_ADDR);
+        frontRightCCPMmax = readEEPROM(FRONT_RIGHT_CCPM_MAX_ADDR);
+        rearCCPMmin = readEEPROM(REAR_CCPM_MIN_ADDR);
+        rearCCPMmax = readEEPROM(REAR_CCPM_MAX_ADDR);
+        yawMin = readEEPROM(YAW_MIN_ADDR);
+        yawMax = readEEPROM(YAW_MAX_ADDR);
+    }
+}
+
+/**********************************************************************/
+// default the heli specific values to defaults
+void heli_defaultUserConfig() 
+{
+  // default CCPM values.
+  frontLeftCCPMmin =        1200;
+  frontLeftCCPMmax =        1800;
+  frontRightCCPMmin  =      1900;
+  frontRightCCPMmax =       1100;
+  rearCCPMmin =             1200;
+  rearCCPMmax =             1800;
+  yawMin =                  1200;
+  yawMax =                  1800; 
+  
+  // default PID values - Roll
+  KP_QUAD_ROLL               = 1.000;
+  KI_QUAD_ROLL               = 0.150;
+  STABLE_MODE_KP_RATE_ROLL   = 0.000;
+  
+  // default PID values - Pitch
+  KP_QUAD_PITCH              = 1.200;
+  KI_QUAD_PITCH              = 0.120;
+  STABLE_MODE_KP_RATE_PITCH  = 0.000;
+  
+  // default PID values - Yaw
+  Kp_RateYaw                 = 2.000;  // heading P term
+  Ki_RateYaw                 = 0.100;  // heading I term
+  KP_QUAD_YAW                = 0.150;  // yaw rate P term
+  KI_QUAD_YAW                = 0.030;  // yaw rate I term
+  STABLE_MODE_KP_RATE_YAW    = 0.000;  // not used
+}
+
+/**********************************************************************/
+// displaySettings - displays heli specific user settings
+void heli_displaySettings() 
+{
+    SerPri("frontLeftCCPM min: ");
+    SerPri(frontLeftCCPMmin);
+    SerPri("\tmax:");
+    SerPri(frontLeftCCPMmax);
+    
+    if( abs(frontLeftCCPMmin-frontLeftCCPMmax)<50 || frontLeftCCPMmin < 900 || frontLeftCCPMmin > 2100 || frontLeftCCPMmax < 900 || frontLeftCCPMmax > 2100 )
+        SerPrln("\t\t<-- check");
+    else
+        SerPrln();
+    
+    SerPri("frontRightCCPM min: ");
+    SerPri(frontRightCCPMmin);
+    SerPri("\tmax:");
+    SerPri(frontRightCCPMmax);
+    if( abs(frontRightCCPMmin-frontRightCCPMmax)<50 || frontRightCCPMmin < 900 || frontRightCCPMmin > 2100 || frontRightCCPMmax < 900 || frontRightCCPMmax > 2100 )
+        SerPrln("\t\t<-- check");
+    else
+        SerPrln();    
+    
+    SerPri("rearCCPM min: ");
+    SerPri(rearCCPMmin);
+    SerPri("\tmax:");
+    SerPri(rearCCPMmax);
+    if( abs(rearCCPMmin-rearCCPMmax)<50 || rearCCPMmin < 900 || rearCCPMmin > 2100 || rearCCPMmax < 900 || rearCCPMmax > 2100 )
+        SerPrln("\t\t<-- check");
+    else
+        SerPrln();
+    
+    SerPri("yaw min: ");
+    SerPri(yawMin);
+    SerPri("\tmax:");
+    SerPri(yawMax);
+    if( abs(yawMin-yawMax)<50 || yawMin < 900 || yawMin > 2100 || yawMax < 900 || yawMax > 2100 )
+        SerPrln("\t\t<-- check");
+    else
+        SerPrln();   
+
+    SerPrln();
+}
+
+////////////////////////////////////////////////////////////////////////////////
+//  Setup Procedure
+////////////////////////////////////////////////////////////////////////////////
+void heli_setup()
+{
+  // read heli specific settings (like CCPM values) from EEPROM
+  heli_readUserConfig();
+  
+  // update CCPM values
+  frontLeftCCPMslope =      100 / (frontLeftCCPMmax - frontLeftCCPMmin);
+  frontLeftCCPMintercept =  100 - (frontLeftCCPMslope * frontLeftCCPMmax);
+  frontRightCCPMslope =     100 / (frontRightCCPMmax - frontRightCCPMmin);
+  frontRightCCPMintercept = 100 - (frontRightCCPMslope * frontRightCCPMmax);
+  rearCCPMslope =           100 / (rearCCPMmax - rearCCPMmin);
+  rearCCPMintercept =       100 - (rearCCPMslope * rearCCPMmax);
+  yawSlope =                100 / (yawMax - yawMin);
+  yawIntercept =            50 - (yawSlope * yawMax);
+  
+  // hardcode mids because we will use ccpm
+  roll_mid = ROLL_MID;
+  pitch_mid = PITCH_MID;
+  collective_mid = 1500;
+  yaw_mid = (yawMin+yawMax)/2;
+}
+
+/**********************************************************************/
+// Radio decoding
+void heli_read_radio()
+{
+    static int count = 0;
+    // left channel
+    ccpmPercents.x  = frontLeftCCPMslope * APM_RC.InputCh(CHANNEL_FRONT_LEFT) + frontLeftCCPMintercept;
+    
+    // right channel
+    ccpmPercents.y = frontRightCCPMslope * APM_RC.InputCh(CHANNEL_FRONT_RIGHT) + frontRightCCPMintercept;
+    
+    // rear channel
+    ccpmPercents.z = rearCCPMslope * APM_RC.InputCh(CHANNEL_REAR) + rearCCPMintercept;
+    
+    // decode the ccpm
+    rollPitchCollPercent = ccpmDeallocation * ccpmPercents;
+    
+    // get the yaw (not coded)
+    yawPercent = yawSlope * APM_RC.InputCh(CHANNEL_YAW) + yawIntercept;
+    
+    // we add 1500 to make it fit in with rest of arduCopter code..
+    ch_roll = rollPitchCollPercent.x;
+    ch_pitch = rollPitchCollPercent.y;
+    ch_collective = rollPitchCollPercent.z;
+    
+    // allow a bit of a dead zone for the yaw
+    if( abs(yawPercent) < 2 )
+        ch_yaw = 0;
+    else
+        ch_yaw = yawPercent;
+    
+    // convert to absolute angles
+    command_rx_roll = ch_roll / HELI_STICK_TO_ANGLE_FACTOR + HELI_ADJUST_ROLL;        // Convert stick position to absolute angles
+    command_rx_pitch = ch_pitch / HELI_STICK_TO_ANGLE_FACTOR + HELI_ADJUST_PITCH;      // Convert stick position to absolute angles
+    command_rx_collective = ch_collective;
+    command_rx_yaw = ch_yaw / HELI_YAW_STICK_TO_ANGLE_FACTOR;      // Convert stick position to turn rate
+    
+    // hardcode flight mode
+    flightMode = STABLE_MODE;
+}
+
+/**********************************************************************/
+// output to swash plate based on control variables
+// Uses these global variables:
+// control_roll       : -50 ~ 50
+// control_pitch      : -50 ~ 50
+// control_collective :   0 ~ 100
+// control_yaw        : -50 ~ 50
+void heli_moveSwashPlate()
+{
+    static int count = 0;
+    // turn pitch, roll, collective commands into ccpm values (i.e. values for each servo)
+    ccpmPercents_out = ccpmAllocation * Vector3f(control_roll, control_pitch, control_collective);
+
+    // calculate values to be sent out to RC Output channels
+    leftOut =  (ccpmPercents_out.x - frontLeftCCPMintercept) / frontLeftCCPMslope;
+    rightOut = (ccpmPercents_out.y - frontRightCCPMintercept) / frontRightCCPMslope;
+    rearOut =  (ccpmPercents_out.z - rearCCPMintercept) / rearCCPMslope;
+    yawOut =   (control_yaw - yawIntercept) / yawSlope;
+      
+    APM_RC.OutputCh(CHANNEL_FRONT_LEFT,leftOut);
+    APM_RC.OutputCh(CHANNEL_FRONT_RIGHT,rightOut);
+    APM_RC.OutputCh(CHANNEL_REAR,rearOut);
+    APM_RC.OutputCh(CHANNEL_YAW,yawOut);
+    // InstantPWM
+    APM_RC.Force_Out0_Out1();
+    APM_RC.Force_Out2_Out3();
+}
+
+/**********************************************************************/
+// ROLL, PITCH and YAW PID controls... 
+// Input : desired Roll, Pitch absolute angles
+//         collective as a percentage from 0~100
+//         yaw as a rate of rotation
+// Output : control_roll - roll servo as a percentage (-50 to 50)
+//          control_pitch - pitch servo as a percentage (-50 to 50)
+//          control_collective - collective servo as a percentage (0 to 100)
+//          control_yaw - yaw servo as a percentage (0 to 100)
+void heli_attitude_control(int command_roll, int command_pitch, int command_collective, int command_yaw)
+{
+    static float firstIteration = 1;
+    static float command_yaw_previous = 0;
+    static float previousYawRate = 0;
+    float stable_roll, stable_pitch;
+    float currentYawRate;
+    float control_yaw_rate;
+    float err_heading;
+    static int aCounter = 0;
+    float heli_G_Dt;
+    
+    // get current time
+    heli_G_Dt = (currentTimeMicros-heli_previousTimeMicros) * 0.000001;   // Microseconds!!!
+    heli_previousTimeMicros = currentTimeMicros;
+    
+    // always pass through collective command
+    control_collective = command_rx_collective;
+
+    // ROLL CONTROL -- TWO PIDS - ANGLE CONTROL + RATE CONTROL
+    // P term
+    err_roll = command_roll - ToDeg(roll);    
+    err_roll = constrain(err_roll,-25,25);  // to limit max roll command...
+    // I term
+    roll_I += err_roll*heli_G_Dt*KI_QUAD_ROLL;
+    roll_I = constrain(roll_I,-10,10);
+    // D term
+    roll_D = ToDeg(Omega[0]) * STABLE_MODE_KP_RATE_ROLL;  // Take into account Angular velocity
+    roll_D = constrain(roll_D,-25,25);
+  
+    // PID control
+    control_roll = KP_QUAD_ROLL*err_roll + roll_I + roll_D;
+    control_roll = constrain(control_roll,-50,50);
+  
+    // PITCH CONTROL -- TWO PIDS - ANGLE CONTROL + RATE CONTROL
+    // P term
+    err_pitch = command_pitch - ToDeg(pitch);
+    err_pitch = constrain(err_pitch,-25,25);  // to limit max pitch command...
+    // I term
+    pitch_I += err_pitch * heli_G_Dt * KI_QUAD_PITCH;
+    pitch_I = constrain(pitch_I,-10,10);
+    // D term
+    pitch_D = ToDeg(Omega[1]) * STABLE_MODE_KP_RATE_PITCH; // Take into account Angular velocity
+    pitch_D = constrain(pitch_D,-25,25);
+    // PID control
+    control_pitch = KP_QUAD_PITCH*err_pitch + pitch_I + pitch_D;
+    control_pitch = constrain(control_pitch,-50,50);
+
+
+    // YAW CONTROL
+    if( command_yaw == 0 )  // heading hold mode 
+    {
+        // check we don't need to reset targetHeading
+        if( command_yaw_previous != 0 )
+            targetHeading = ToDeg(yaw);
+
+        // ensure reasonable targetHeading
+        if( firstIteration || targetHeading > 180 || targetHeading < -180 )
+        {
+            firstIteration = 0;
+            targetHeading = ToDeg(yaw);
+        }
+            
+        err_heading = Normalize_angle(targetHeading - ToDeg(yaw));     
+        err_heading = constrain(err_heading,-90,90);  // don't make it travel any faster beyond 90 degrees
+      
+        heading_I += err_heading * heli_G_Dt * Ki_RateYaw;
+        heading_I = constrain(heading_I,-20,20);
+
+        // PID control - a bit bad - we're using the acro mode's PID values because there's not PID for heading
+        control_yaw_rate = Kp_RateYaw*err_heading + heading_I;
+        control_yaw_rate = constrain(control_yaw_rate,-100,100);  // to limit max yaw command
+    }else{      // rate mode
+        err_heading = 0;
+        control_yaw_rate = command_yaw;
+    }
+    command_yaw_previous = command_yaw;
+
+    // YAW RATE CONTROL
+    currentYawRate = ToDeg(Gyro_Scaled_Z(read_adc(2)));
+    //currentYawRate = ToDeg(Omega_Vector[2]);  <-- makes things very unstable!!
+    err_yaw = control_yaw_rate-currentYawRate;
+  
+    yaw_I += err_yaw * heli_G_Dt * KI_QUAD_YAW;
+    yaw_I = constrain(yaw_I, -20, 20);
+  
+    //yaw_D = currentYawRate - previousYawRate;
+    previousYawRate = currentYawRate;
+  
+    // PID control
+    control_yaw = (KP_QUAD_YAW*err_yaw + yaw_I);
+    control_yaw = constrain(control_yaw,-50,50);
+}
+
+#endif  // #if AIRFRAME == HELI

ArducopterNG/Navigation.pde

@@ -55,6 +55,7 @@ void read_GPS_data()
 /* GPS based Position control */
 void Position_control(long lat_dest, long lon_dest)
 {
+#ifdef IsGPS
   long Lon_diff;
   long Lat_diff;
 
@@ -93,6 +94,7 @@ void Position_control(long lat_dest, long lon_dest)
   command_gps_pitch = constrain(command_gps_pitch, -GPS_MAX_ANGLE, GPS_MAX_ANGLE); // Limit max command
 
   //Log_Write_PID(2,KP_GPS_PITCH*gps_err_pitch*10,KI_GPS_PITCH*gps_pitch_I*10,KD_GPS_PITCH*gps_pitch_D*10,command_gps_pitch*10);
+#endif  
 }
 
 void Reset_I_terms_navigation()

ArducopterNG/System.pde

@@ -58,12 +58,21 @@ void APM_Init() {
 
   APM_RC.Init();             // APM Radio initialization
 
+#if AIRFRAME == QUAD
   // RC channels Initialization (Quad motors)  
   APM_RC.OutputCh(0,MIN_THROTTLE);  // Motors stoped
   APM_RC.OutputCh(1,MIN_THROTTLE);
   APM_RC.OutputCh(2,MIN_THROTTLE);
   APM_RC.OutputCh(3,MIN_THROTTLE);
+#endif  
 
+#if AIRFRAME == HELI
+  // RC channels Initialization (heli servos)  
+  APM_RC.OutputCh(0,CHANN_CENTER);  // mid position
+  APM_RC.OutputCh(1,CHANN_CENTER);
+  APM_RC.OutputCh(2,CHANN_CENTER);
+  APM_RC.OutputCh(3,CHANN_CENTER);
+#endif 
   // Make sure that Relay is switched off.
   digitalWrite(RELAY,LOW);
 
@@ -119,11 +128,21 @@ void APM_Init() {
   if(pitch_mid < 1400 || pitch_mid > 1600) pitch_mid = 1500;
   if(yaw_mid < 1400 || yaw_mid > 1600) yaw_mid = 1500;
 
+#if AIRFRAME == QUAD
   // RC channels Initialization (Quad motors)  
   APM_RC.OutputCh(0,MIN_THROTTLE);  // Motors stoped
   APM_RC.OutputCh(1,MIN_THROTTLE);
   APM_RC.OutputCh(2,MIN_THROTTLE);
   APM_RC.OutputCh(3,MIN_THROTTLE);
+#endif  
+
+#if AIRFRAME == HELI
+  // RC channels Initialization (heli servos)  
+  APM_RC.OutputCh(0,CHANN_CENTER);  // mid position
+  APM_RC.OutputCh(1,CHANN_CENTER);
+  APM_RC.OutputCh(2,CHANN_CENTER);
+  APM_RC.OutputCh(3,CHANN_CENTER);
+#endif 
 
   // Initialise Wire library used by Magnetometer and Barometer
   Wire.begin();
@@ -181,6 +200,11 @@ void APM_Init() {
   //timer = millis();
   //tlmTimer = millis();
 
+  // initialise helicopter
+#if AIRFRAME == HELI
+  heli_setup();
+#endif
+
 #ifdef IsAM
   // Switch Left & Right lights on
   digitalWrite(RI_LED, HIGH);