TradHeli - updated Heli.h to use AP_Compass instead of the outdated APM_Compass. Also changed autopilot on/off switch to be consistent with Quad.

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

ArducopterNG/Heli.h

@@ -42,7 +42,7 @@ TODO:
 #include <EEPROM.h>   // added by Randy
 #include <AP_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 <AP_Compass.h>                               // ArduPilot Mega Compass Library
 #include <DataFlash.h>                                 // ArduPilot Mega DataFlash Library.
 #include "../AP_Math/AP_Math.h"
 
@@ -156,9 +156,9 @@ int roll_control_switch = 1;
 int pitch_control_switch = 1;
 int yaw_control_switch = 1;
 int collective_control_switch = 0;
-int position_control_switch = 0;
-int position_control_engaged = 0;  // we don't have enough buttons so we will turn this on and off with roll + pitch positions
-int position_control_safety = 0;   // if 0 then safety is off.  if 1 then safety is on and position control will not operate
+//int position_control_switch = 0;
+//int position_control_engaged = 0;  // we don't have enough buttons so we will turn this on and off with roll + pitch positions
+//int position_control_safety = 0;   // if 0 then safety is off.  if 1 then safety is on and position control will not operate
 
 /// for sending values out to servos
 Vector3f ccpmPercents_out;            // Array of ccpm input values, converted to percents

ArducopterNG/Heli.pde

@@ -164,8 +164,8 @@ void heli_setup()
   roll_control_switch = !SW_DIP1;
   pitch_control_switch = !SW_DIP2;
   yaw_control_switch = !SW_DIP3; 
-  collective_control_switch = 0; //!SW_DIP4;
-  position_control_switch = !SW_DIP4;  // switch 4 controls whether we will do GPS hold or not
+  collective_control_switch = !SW_DIP4;
+  //position_control_switch = !SW_DIP4;  // switch 4 controls whether we will do GPS hold or not
 }
 
 /*****************************************************************************************************/
@@ -231,7 +231,7 @@ void heli_read_radio()
     // get the yaw (not coded)
     yawPercent = (yawSlope * APM_RC.InputCh(CHANNEL_YAW) + yawIntercept) - trim_yaw;
     
-    // we add 1500 to make it fit in with rest of arduCopter code..
+    // put decoded values into the global variables
     ch_roll = rollPitchCollPercent.x;
     ch_pitch = rollPitchCollPercent.y;
     ch_collective = rollPitchCollPercent.z;
@@ -242,42 +242,32 @@ void heli_read_radio()
     else
         ch_yaw = yawPercent;
     
+    // get the aux channel (for tuning on/off autopilot)
+    ch_aux = APM_RC.InputCh(CH_5) * ch_aux_slope + ch_aux_offset;        
+    
     // convert to absolute angles
     command_rx_roll = ch_roll / HELI_STICK_TO_ANGLE_FACTOR;        // Convert stick position to absolute angles
     command_rx_pitch = ch_pitch / HELI_STICK_TO_ANGLE_FACTOR;      // 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
     
+    // for use by non-heli parts of code
+    ch_throttle = 1000 + (ch_collective * 10);
+    
     // hardcode flight mode
     flightMode = STABLE_MODE;
     
-    // if position_control could be active
-    if( position_control_switch == 1 && position_control_safety == 0 ) {
-      
-        //if roll or pitch ever > 40% or < -40% turn off autopilot
-        if( AP_mode == AP_AUTOMATIC_MODE && (fabs(ch_roll) >= 40 || fabs(ch_pitch) >= 40 )) {
-            position_control_safety = 1;
-            target_position = 0; // force it to reset to current position
-            Serial.println("autopilot OFF!  safety!");
-        }
-            
-        // switch on auto pilot if elevator goes over 50%
-        if( AP_mode == AP_NORMAL_MODE && ch_collective >= 50 ) {
-            AP_mode = AP_AUTOMATIC_MODE;   
-            Serial.println("autopilot on!");
-        }
-        
-        // switch off auto pilot if elevator goes below 20%
-        if( AP_mode == AP_AUTOMATIC_MODE && ch_collective <= 20 ) {
-            AP_mode = AP_NORMAL_MODE;
-            target_position = 0; // force it to reset to current position
-            Serial.println("autopilot off!");
-        }        
+    // Autopilot mode (only works on Stable mode)
+    if (flightMode == STABLE_MODE)
+    {
+      if(ch_aux < 1300) {
+        AP_mode = AP_AUTOMATIC_MODE;   // Automatic mode : GPS position hold mode + altitude hold
+        //SerPrln("autopilot ON!");
       }else{ 
-        AP_mode = AP_NORMAL_MODE;
+        AP_mode = AP_NORMAL_MODE;   // Normal mode
+        //SerPrln("autopilot OFF!");
+      }
     }    
-    // turn off AP_AUTOMATIC_MODE if roll or pitch is > 40% or < -40%
-    
 }
 
 /**********************************************************************/
@@ -334,7 +324,7 @@ void heli_attitude_control(int command_roll, int command_pitch, int command_coll
     heli_previousTimeMicros = currentTimeMicros;
     
     // always pass through collective command
-    control_collective = command_rx_collective;
+    control_collective = command_collective;
 
     // ROLL CONTROL -- ONE PID
     if( roll_control_switch ) 
@@ -428,6 +418,11 @@ void heli_attitude_control(int command_roll, int command_pitch, int command_coll
         // straight pass through
         control_yaw = ch_yaw;
     }
+    
+    Log_Write_PID(7,KP_QUAD_ROLL*err_roll,roll_I,roll_D,control_roll);
+    Log_Write_PID(8,KP_QUAD_PITCH*err_pitch,pitch_I,pitch_D,control_pitch);
+    Log_Write_PID(9,Kp_RateYaw*err_heading,heading_I,0,control_yaw_rate);
+    Log_Write_PID(10,KP_QUAD_YAW*err_yaw,yaw_I,yaw_D,control_yaw);
 }
 
 #endif  // #if AIRFRAME == HELI