Radio functions in Radio.pde

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

ArducopterNG/ArduUser.h

@@ -60,10 +60,18 @@ TODO:
 /*************************************************************/
 // Radio related definitions
 
-// If you don't know these values, you can activate RADIO_TEST_MODE below
-// and check your mid values
-
-//#define RADIO_TEST_MODE
+#define CH_ROLL 0
+#define CH_PITCH 1
+#define CH_THROTTLE 2
+#define CH_RUDDER 3
+#define CH_1 0
+#define CH_2 1
+#define CH_3 2
+#define CH_4 3
+#define CH_5 4
+#define CH_6 5
+#define CH_7 6
+#define CH_8 7
 
 #define ROLL_MID 1500    // Radio Roll channel mid value
 #define PITCH_MID 1500    // Radio Pitch channel mid value

ArducopterNG/Attitude.pde

@@ -179,24 +179,3 @@ void Rate_control_v2()
   control_yaw = Kp_RateYaw*err_yaw + Kd_RateYaw*yaw_D + Ki_RateYaw*yaw_I; 
 }
 
-// Maximun slope filter for radio inputs... (limit max differences between readings)
-int channel_filter(int ch, int ch_old)
-{
-  int diff_ch_old;
-
-  if (ch_old==0)      // ch_old not initialized
-    return(ch);
-  diff_ch_old = ch - ch_old;      // Difference with old reading
-  if (diff_ch_old < 0)
-  {
-    if (diff_ch_old <- 60)
-      return(ch_old - 60);        // We limit the max difference between readings
-  }
-  else
-  {
-    if (diff_ch_old > 60)    
-      return(ch_old + 60);
-  }
-  return((ch + ch_old) >> 1);   // Small filtering
-  //return(ch);
-}

ArducopterNG/Functions.pde

@@ -79,3 +79,36 @@ void LightsOff() {
   digitalWrite(LED_Red, LOW);
  
 }
+
+// Funtion to normalize an angle in degrees to -180,180 degrees
+float Normalize_angle(float angle)
+{
+  if (angle > 180)         
+    return (angle - 360.0);
+  else if (angle < -180)
+    return (angle + 360.0);
+  else
+    return(angle);
+}
+
+// Maximun slope filter for radio inputs... (limit max differences between readings)
+int channel_filter(int ch, int ch_old)
+{
+  int diff_ch_old;
+
+  if (ch_old==0)      // ch_old not initialized
+    return(ch);
+  diff_ch_old = ch - ch_old;      // Difference with old reading
+  if (diff_ch_old < 0)
+  {
+    if (diff_ch_old <- 60)
+      return(ch_old - 60);        // We limit the max difference between readings
+  }
+  else
+  {
+    if (diff_ch_old > 60)    
+      return(ch_old + 60);
+  }
+  return((ch + ch_old) >> 1);   // Small filtering
+  //return(ch);
+}

ArducopterNG/Radio.pde

@@ -32,3 +32,110 @@ TODO:
 
 
 * ************************************************************** */
+
+#define STICK_TO_ANGLE_FACTOR 12.0        // To convert stick position to absolute angles
+#define YAW_STICK_TO_ANGLE_FACTOR 150.0
+
+void Read_radio()
+{
+  if (APM_RC.GetState() == 1)   // New radio frame?
+    {
+    // Commands from radio Rx
+    // We apply the Radio calibration parameters (from configurator) except for throttle
+    ch_roll = channel_filter(APM_RC.InputCh(CH_ROLL) * ch_roll_slope + ch_roll_offset, ch_roll);
+    ch_pitch = channel_filter(APM_RC.InputCh(CH_PITCH) * ch_pitch_slope + ch_pitch_offset, ch_pitch);
+    ch_throttle = channel_filter(APM_RC.InputCh(CH_THROTTLE), ch_throttle); // Transmiter calibration not used on throttle
+    ch_yaw = channel_filter(APM_RC.InputCh(CH_RUDDER) * ch_yaw_slope + ch_yaw_offset, ch_yaw);
+    ch_aux = APM_RC.InputCh(CH_5) * ch_aux_slope + ch_aux_offset;
+    ch_aux2 = APM_RC.InputCh(CH_6) * ch_aux2_slope + ch_aux2_offset;
+    
+    // Flight mode
+    if(ch_aux2 > 1800) 
+      flightMode = 1;  // Force to Acro mode from radio
+    else
+      flightMode = 0;  // Stable mode (default)
+
+    // Autopilot mode (only works on Stable mode)
+    if (flightMode == 0)
+      {
+      if(ch_aux > 1800)
+        AP_mode = 1;   // Automatic mode : GPS position hold mode + altitude hold
+      else 
+        AP_mode = 0;   // Normal mode
+      }
+      
+    if (flightMode==0)  // IN STABLE MODE we convert stick positions to absoulte angles
+      {
+      // In Stable mode stick position defines the desired angle in roll, pitch and yaw
+      #ifdef FLIGHT_MODE_X
+        // For X mode we make a mix in the input
+        float aux_roll = (ch_roll-roll_mid) / STICK_TO_ANGLE_FACTOR;
+        float aux_pitch = (ch_pitch-pitch_mid) / STICK_TO_ANGLE_FACTOR;
+        command_rx_roll = aux_roll - aux_pitch;
+        command_rx_pitch = aux_roll + aux_pitch;
+      #else
+        command_rx_roll = (ch_roll-roll_mid) / STICK_TO_ANGLE_FACTOR;       // Convert stick position to absolute angles
+        command_rx_pitch = (ch_pitch-pitch_mid) / STICK_TO_ANGLE_FACTOR;
+      #endif
+
+      // YAW
+      if (abs(ch_yaw-yaw_mid)<8)   // Take into account a bit of "dead zone" on yaw
+        aux_float = 0.0;
+      else
+        aux_float = (ch_yaw-yaw_mid) / YAW_STICK_TO_ANGLE_FACTOR;
+      command_rx_yaw += aux_float;
+      command_rx_yaw = Normalize_angle(command_rx_yaw);    // Normalize angle to [-180,180]
+      }
+    
+    // Write Radio data to DataFlash log
+    Log_Write_Radio(ch_roll,ch_pitch,ch_throttle,ch_yaw,int(K_aux*100),(int)AP_mode);
+    }  // END new radio data
+}
+
+
+// Send output commands to ESC´s
+void Motor_output()
+{
+  // Quadcopter mix
+  if (motorArmed == 1) 
+    {   
+  #ifdef IsAM
+    digitalWrite(FR_LED, HIGH);    // AM-Mode
+  #endif
+    // Quadcopter output mix
+    rightMotor = constrain(ch_throttle - control_roll + control_yaw, minThrottle, 2000);
+    leftMotor = constrain(ch_throttle + control_roll + control_yaw, minThrottle, 2000);
+    frontMotor = constrain(ch_throttle + control_pitch - control_yaw, minThrottle, 2000);
+    backMotor = constrain(ch_throttle - control_pitch - control_yaw, minThrottle, 2000);
+    }
+  else    // MOTORS DISARMED
+    {    
+  #ifdef IsAM
+    digitalWrite(FR_LED, LOW);    // AM-Mode
+  #endif
+    digitalWrite(LED_Green,HIGH); // Ready LED on
+
+    rightMotor = MIN_THROTTLE;
+    leftMotor = MIN_THROTTLE;
+    frontMotor = MIN_THROTTLE;
+    backMotor = MIN_THROTTLE;
+
+    // Reset_I_Terms();
+    roll_I = 0;     // reset I terms of PID controls
+    pitch_I = 0;
+    yaw_I = 0; 
+    // Initialize yaw command to actual yaw when throttle is down...
+    command_rx_yaw = ToDeg(yaw);
+    }
+
+  // Send commands to motors
+  APM_RC.OutputCh(0, rightMotor);
+  APM_RC.OutputCh(1, leftMotor);
+  APM_RC.OutputCh(2, frontMotor);
+  APM_RC.OutputCh(3, backMotor);
+
+  // InstantPWM => Force inmediate output on PWM signals
+  APM_RC.Force_Out0_Out1();
+  APM_RC.Force_Out2_Out3();
+}
+