diff --git a/ArduCopter/esc_calibration.pde b/ArduCopter/esc_calibration.pde
index 620c9ee2284dc9b6e43316a7e4f6290f0557bf9a..e7471339f978214127ff4857316aeceb8f3e0e22 100644
--- a/ArduCopter/esc_calibration.pde
+++ b/ArduCopter/esc_calibration.pde
@@ -1,139 +1,139 @@
-// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
-
-/*****************************************************************************
-*  esc_calibration.pde : functions to check and perform ESC calibration
-*****************************************************************************/
-
-#define ESC_CALIBRATION_HIGH_THROTTLE   950
-
-// enum for ESC CALIBRATION
-enum ESCCalibrationModes {
-    ESCCAL_NONE = 0,
-    ESCCAL_PASSTHROUGH_IF_THROTTLE_HIGH = 1,
-    ESCCAL_PASSTHROUGH_ALWAYS = 2,
-    ESCCAL_AUTO = 3
-};
-
-// check if we should enter esc calibration mode
-static void esc_calibration_startup_check()
-{
-    // exit immediately if pre-arm rc checks fail
-    pre_arm_rc_checks();
-    if (!ap.pre_arm_rc_check) {
-        // clear esc flag for next time
-        if (g.esc_calibrate != ESCCAL_NONE) {
-            g.esc_calibrate.set_and_save(ESCCAL_NONE);
-        }
-        return;
-    }
-
-    // check ESC parameter
-    switch (g.esc_calibrate) {
-        case ESCCAL_NONE:
-            // check if throttle is high
-            if (g.rc_3.control_in >= ESC_CALIBRATION_HIGH_THROTTLE) {
-                // we will enter esc_calibrate mode on next reboot
-                g.esc_calibrate.set_and_save(ESCCAL_PASSTHROUGH_IF_THROTTLE_HIGH);
-                // send message to gcs
-                gcs_send_text_P(SEVERITY_HIGH,PSTR("ESC Calibration: restart board"));
-                // turn on esc calibration notification
-                AP_Notify::flags.esc_calibration = true;
-                // block until we restart
-                while(1) { delay(5); }
-            }
-            break;
-        case ESCCAL_PASSTHROUGH_IF_THROTTLE_HIGH:
-            // check if throttle is high
-            if (g.rc_3.control_in >= ESC_CALIBRATION_HIGH_THROTTLE) {
-                // pass through pilot throttle to escs
-                esc_calibration_passthrough();
-            }
-            break;
-        case ESCCAL_PASSTHROUGH_ALWAYS:
-            // pass through pilot throttle to escs
-            esc_calibration_passthrough();
-            break;
-        case ESCCAL_AUTO:
-            // perform automatic ESC calibration
-            esc_calibration_auto();
-            break;
-        default:
-            // do nothing
-            break;
-    }
-    // clear esc flag for next time
-    g.esc_calibrate.set_and_save(ESCCAL_NONE);
-}
-
-// esc_calibration_passthrough - pass through pilot throttle to escs
-static void esc_calibration_passthrough()
-{
-    // clear esc flag for next time
-    g.esc_calibrate.set_and_save(ESCCAL_NONE);
-
-    // reduce update rate to motors to 50Hz
-    motors.set_update_rate(50);
-
-    // send message to GCS
-    gcs_send_text_P(SEVERITY_HIGH,PSTR("ESC Calibration: passing pilot throttle to ESCs"));
-
-    while(1) {
-        // arm motors
-        motors.armed(true);
-        motors.enable();
-
-        // flash LEDS
-        AP_Notify::flags.esc_calibration = true;
-
-        // read pilot input
-        read_radio();
-        delay(10);
-
-        // pass through to motors
-        motors.throttle_pass_through(g.rc_3.radio_in);
-    }
-}
-
-// esc_calibration_auto - calibrate the ESCs automatically using a timer and no pilot input
-static void esc_calibration_auto()
-{
-    bool printed_msg = false;
-
-    // reduce update rate to motors to 50Hz
-    motors.set_update_rate(50);
-
-    // send message to GCS
-    gcs_send_text_P(SEVERITY_HIGH,PSTR("ESC Calibration: auto calibration"));
-
-    // arm and enable motors
-    motors.armed(true);
-    motors.enable();
-
-    // flash LEDS
-    AP_Notify::flags.esc_calibration = true;
-
-    // raise throttle to maximum
-    delay(10);
-    motors.throttle_pass_through(g.rc_3.radio_max);
-
-    // wait for safety switch to be pressed
-    while (hal.util->safety_switch_state() == AP_HAL::Util::SAFETY_DISARMED) {
-        if (!printed_msg) {
-            gcs_send_text_P(SEVERITY_HIGH,PSTR("ESC Calibration: push safety switch"));
-            printed_msg = true;
-        }
-        delay(10);
-    }
-
-    // delay for 5 seconds
-    delay(5000);
-
-    // reduce throttle to minimum
-    motors.throttle_pass_through(g.rc_3.radio_min);
-
-    // clear esc parameter
-    g.esc_calibrate.set_and_save(ESCCAL_NONE);
-
-    // block until we restart
-    while(1) { delay(5); }
-}
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+/*****************************************************************************
+*  esc_calibration.pde : functions to check and perform ESC calibration
+*****************************************************************************/
+
+#define ESC_CALIBRATION_HIGH_THROTTLE   950
+
+// enum for ESC CALIBRATION
+enum ESCCalibrationModes {
+    ESCCAL_NONE = 0,
+    ESCCAL_PASSTHROUGH_IF_THROTTLE_HIGH = 1,
+    ESCCAL_PASSTHROUGH_ALWAYS = 2,
+    ESCCAL_AUTO = 3
+};
+
+// check if we should enter esc calibration mode
+static void esc_calibration_startup_check()
+{
+    // exit immediately if pre-arm rc checks fail
+    pre_arm_rc_checks();
+    if (!ap.pre_arm_rc_check) {
+        // clear esc flag for next time
+        if (g.esc_calibrate != ESCCAL_NONE) {
+            g.esc_calibrate.set_and_save(ESCCAL_NONE);
+        }
+        return;
+    }
+
+    // check ESC parameter
+    switch (g.esc_calibrate) {
+        case ESCCAL_NONE:
+            // check if throttle is high
+            if (g.rc_3.control_in >= ESC_CALIBRATION_HIGH_THROTTLE) {
+                // we will enter esc_calibrate mode on next reboot
+                g.esc_calibrate.set_and_save(ESCCAL_PASSTHROUGH_IF_THROTTLE_HIGH);
+                // send message to gcs
+                gcs_send_text_P(SEVERITY_HIGH,PSTR("ESC Calibration: restart board"));
+                // turn on esc calibration notification
+                AP_Notify::flags.esc_calibration = true;
+                // block until we restart
+                while(1) { delay(5); }
+            }
+            break;
+        case ESCCAL_PASSTHROUGH_IF_THROTTLE_HIGH:
+            // check if throttle is high
+            if (g.rc_3.control_in >= ESC_CALIBRATION_HIGH_THROTTLE) {
+                // pass through pilot throttle to escs
+                esc_calibration_passthrough();
+            }
+            break;
+        case ESCCAL_PASSTHROUGH_ALWAYS:
+            // pass through pilot throttle to escs
+            esc_calibration_passthrough();
+            break;
+        case ESCCAL_AUTO:
+            // perform automatic ESC calibration
+            esc_calibration_auto();
+            break;
+        default:
+            // do nothing
+            break;
+    }
+    // clear esc flag for next time
+    g.esc_calibrate.set_and_save(ESCCAL_NONE);
+}
+
+// esc_calibration_passthrough - pass through pilot throttle to escs
+static void esc_calibration_passthrough()
+{
+    // clear esc flag for next time
+    g.esc_calibrate.set_and_save(ESCCAL_NONE);
+
+    // reduce update rate to motors to 50Hz
+    motors.set_update_rate(50);
+
+    // send message to GCS
+    gcs_send_text_P(SEVERITY_HIGH,PSTR("ESC Calibration: passing pilot throttle to ESCs"));
+
+    while(1) {
+        // arm motors
+        motors.armed(true);
+        motors.enable();
+
+        // flash LEDS
+        AP_Notify::flags.esc_calibration = true;
+
+        // read pilot input
+        read_radio();
+        delay(10);
+
+        // pass through to motors
+        motors.throttle_pass_through(g.rc_3.radio_in);
+    }
+}
+
+// esc_calibration_auto - calibrate the ESCs automatically using a timer and no pilot input
+static void esc_calibration_auto()
+{
+    bool printed_msg = false;
+
+    // reduce update rate to motors to 50Hz
+    motors.set_update_rate(50);
+
+    // send message to GCS
+    gcs_send_text_P(SEVERITY_HIGH,PSTR("ESC Calibration: auto calibration"));
+
+    // arm and enable motors
+    motors.armed(true);
+    motors.enable();
+
+    // flash LEDS
+    AP_Notify::flags.esc_calibration = true;
+
+    // raise throttle to maximum
+    delay(10);
+    motors.throttle_pass_through(g.rc_3.radio_max);
+
+    // wait for safety switch to be pressed
+    while (hal.util->safety_switch_state() == AP_HAL::Util::SAFETY_DISARMED) {
+        if (!printed_msg) {
+            gcs_send_text_P(SEVERITY_HIGH,PSTR("ESC Calibration: push safety switch"));
+            printed_msg = true;
+        }
+        delay(10);
+    }
+
+    // delay for 5 seconds
+    delay(5000);
+
+    // reduce throttle to minimum
+    motors.throttle_pass_through(g.rc_3.radio_min);
+
+    // clear esc parameter
+    g.esc_calibrate.set_and_save(ESCCAL_NONE);
+
+    // block until we restart
+    while(1) { delay(5); }
+}
diff --git a/ArduCopter/land_detector.pde b/ArduCopter/land_detector.pde
index 8a70ca9b670da4dd41227db05b8e37d121f424e4..285c5939a969b471bdd63d344047690548772c23 100644
--- a/ArduCopter/land_detector.pde
+++ b/ArduCopter/land_detector.pde
@@ -1,90 +1,90 @@
-/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
-
-// counter to verify landings
-static uint16_t land_detector = LAND_DETECTOR_TRIGGER;  // we assume we are landed
-
-// land_complete_maybe - return true if we may have landed (used to reset loiter targets during landing)
-static bool land_complete_maybe()
-{
-    return (ap.land_complete || ap.land_complete_maybe);
-}
-
-// update_land_detector - checks if we have landed and updates the ap.land_complete flag
-// called at 50hz
-static void update_land_detector()
-{
-    // land detector can not use the following sensors because they are unreliable during landing
-    // barometer altitude :                 ground effect can cause errors larger than 4m
-    // EKF vertical velocity or altitude :  poor barometer and large acceleration from ground impact
-    // earth frame angle or angle error :   landing on an uneven surface will force the airframe to match the ground angle
-    // gyro output :                        on uneven surface the airframe may rock back an forth after landing
-    // range finder :                       tend to be problematic at very short distances
-    // input throttle :                     in slow land the input throttle may be only slightly less than hover
-
-    // check that the average throttle output is near minimum (less than 12.5% hover throttle)
-    bool motor_at_lower_limit = motors.limit.throttle_lower && (motors.get_throttle_low_comp() < 0.125f);
-    Vector3f accel_ef = ahrs.get_accel_ef_blended();
-    accel_ef.z += GRAVITY_MSS;
-
-    // check that the airframe is not accelerating (not falling or breaking after fast forward flight)
-    bool accel_stationary = (accel_ef.length() < 1.0f);
-
-    if ( motor_at_lower_limit && accel_stationary) {
-        if (!ap.land_complete) {
-            // increase counter until we hit the trigger then set land complete flag
-            if( land_detector < LAND_DETECTOR_TRIGGER) {
-                land_detector++;
-            }else{
-                set_land_complete(true);
-                land_detector = LAND_DETECTOR_TRIGGER;
-            }
-        }
-    } else {
-        // we've sensed movement up or down so reset land_detector
-        land_detector = 0;
-        // if throttle output is high then clear landing flag
-        if (motors.get_throttle_out() > get_non_takeoff_throttle()) {
-            set_land_complete(false);
-        }
-    }
-
-    // set land maybe flag
-    set_land_complete_maybe(land_detector >= LAND_DETECTOR_MAYBE_TRIGGER);
-}
-
-// update_throttle_low_comp - sets motors throttle_low_comp value depending upon vehicle state
-//  low values favour pilot/autopilot throttle over attitude control, high values favour attitude control over throttle
-//  has no effect when throttle is above hover throttle
-static void update_throttle_low_comp()
-{
-    if (mode_has_manual_throttle(control_mode)) {
-        // manual throttle
-        if(!motors.armed() || g.rc_3.control_in <= 0) {
-            motors.set_throttle_low_comp(0.1f);
-        } else {
-            motors.set_throttle_low_comp(0.5f);
-        }
-    } else {
-        // autopilot controlled throttle
-
-        // check for aggressive flight requests
-        const Vector3f angle_target = attitude_control.angle_ef_targets();
-        bool large_angle_request = (pythagorous2(angle_target.x, angle_target.y) > 1500.0f);
-
-        // check for large external disturbance
-        const Vector3f angle_error = attitude_control.angle_bf_error();
-        bool large_angle_error = (pythagorous2(angle_error.x, angle_error.y) > 3000.0f);
-
-        // check for large acceleration ( falling )
-        Vector3f accel_ef = ahrs.get_accel_ef_blended();
-        accel_ef.z += GRAVITY_MSS;
-        bool accel_moving = (accel_ef.length() > 3.0f);
-
-        if ( large_angle_request || large_angle_error || accel_moving) {
-            // if target lean angle is over 15 degrees set high
-            motors.set_throttle_low_comp(0.9f);
-        } else {
-            motors.set_throttle_low_comp(0.1f);
-        }
-    }
-}
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+// counter to verify landings
+static uint16_t land_detector = LAND_DETECTOR_TRIGGER;  // we assume we are landed
+
+// land_complete_maybe - return true if we may have landed (used to reset loiter targets during landing)
+static bool land_complete_maybe()
+{
+    return (ap.land_complete || ap.land_complete_maybe);
+}
+
+// update_land_detector - checks if we have landed and updates the ap.land_complete flag
+// called at 50hz
+static void update_land_detector()
+{
+    // land detector can not use the following sensors because they are unreliable during landing
+    // barometer altitude :                 ground effect can cause errors larger than 4m
+    // EKF vertical velocity or altitude :  poor barometer and large acceleration from ground impact
+    // earth frame angle or angle error :   landing on an uneven surface will force the airframe to match the ground angle
+    // gyro output :                        on uneven surface the airframe may rock back an forth after landing
+    // range finder :                       tend to be problematic at very short distances
+    // input throttle :                     in slow land the input throttle may be only slightly less than hover
+
+    // check that the average throttle output is near minimum (less than 12.5% hover throttle)
+    bool motor_at_lower_limit = motors.limit.throttle_lower && (motors.get_throttle_low_comp() < 0.125f);
+    Vector3f accel_ef = ahrs.get_accel_ef_blended();
+    accel_ef.z += GRAVITY_MSS;
+
+    // check that the airframe is not accelerating (not falling or breaking after fast forward flight)
+    bool accel_stationary = (accel_ef.length() < 1.0f);
+
+    if ( motor_at_lower_limit && accel_stationary) {
+        if (!ap.land_complete) {
+            // increase counter until we hit the trigger then set land complete flag
+            if( land_detector < LAND_DETECTOR_TRIGGER) {
+                land_detector++;
+            }else{
+                set_land_complete(true);
+                land_detector = LAND_DETECTOR_TRIGGER;
+            }
+        }
+    } else {
+        // we've sensed movement up or down so reset land_detector
+        land_detector = 0;
+        // if throttle output is high then clear landing flag
+        if (motors.get_throttle_out() > get_non_takeoff_throttle()) {
+            set_land_complete(false);
+        }
+    }
+
+    // set land maybe flag
+    set_land_complete_maybe(land_detector >= LAND_DETECTOR_MAYBE_TRIGGER);
+}
+
+// update_throttle_low_comp - sets motors throttle_low_comp value depending upon vehicle state
+//  low values favour pilot/autopilot throttle over attitude control, high values favour attitude control over throttle
+//  has no effect when throttle is above hover throttle
+static void update_throttle_low_comp()
+{
+    if (mode_has_manual_throttle(control_mode)) {
+        // manual throttle
+        if(!motors.armed() || g.rc_3.control_in <= 0) {
+            motors.set_throttle_low_comp(0.1f);
+        } else {
+            motors.set_throttle_low_comp(0.5f);
+        }
+    } else {
+        // autopilot controlled throttle
+
+        // check for aggressive flight requests
+        const Vector3f angle_target = attitude_control.angle_ef_targets();
+        bool large_angle_request = (pythagorous2(angle_target.x, angle_target.y) > 1500.0f);
+
+        // check for large external disturbance
+        const Vector3f angle_error = attitude_control.angle_bf_error();
+        bool large_angle_error = (pythagorous2(angle_error.x, angle_error.y) > 3000.0f);
+
+        // check for large acceleration ( falling )
+        Vector3f accel_ef = ahrs.get_accel_ef_blended();
+        accel_ef.z += GRAVITY_MSS;
+        bool accel_moving = (accel_ef.length() > 3.0f);
+
+        if ( large_angle_request || large_angle_error || accel_moving) {
+            // if target lean angle is over 15 degrees set high
+            motors.set_throttle_low_comp(0.9f);
+        } else {
+            motors.set_throttle_low_comp(0.1f);
+        }
+    }
+}
diff --git a/ArduCopter/tuning.pde b/ArduCopter/tuning.pde
index 23064c7551f0b47342e3b4b037f7dc9a2ece93dd..d723f736d0add186918ac0d34346e84172bb344b 100644
--- a/ArduCopter/tuning.pde
+++ b/ArduCopter/tuning.pde
@@ -1,201 +1,201 @@
-/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
-
-/*
- * tuning.pde - function to update various parameters in flight using the ch6 tuning knob
- *      This should not be confused with the AutoTune feature which can bve found in control_autotune.pde
- */
-
-// tuning - updates parameters based on the ch6 tuning knob's position
-//  should be called at 3.3hz
-static void tuning() {
-
-    // exit immediately if not tuning of when radio failsafe is invoked so tuning values are not set to zero
-    if ((g.radio_tuning <= 0) || failsafe.radio || failsafe.radio_counter != 0) {
-        return;
-    }
-
-    float tuning_value = (float)g.rc_6.control_in / 1000.0f;
-    g.rc_6.set_range(g.radio_tuning_low,g.radio_tuning_high);
-
-    switch(g.radio_tuning) {
-
-    // Roll, Pitch tuning
-    case CH6_STABILIZE_ROLL_PITCH_KP:
-        g.p_stabilize_roll.kP(tuning_value);
-        g.p_stabilize_pitch.kP(tuning_value);
-        break;
-
-    case CH6_RATE_ROLL_PITCH_KP:
-        g.pid_rate_roll.kP(tuning_value);
-        g.pid_rate_pitch.kP(tuning_value);
-        break;
-
-    case CH6_RATE_ROLL_PITCH_KI:
-        g.pid_rate_roll.kI(tuning_value);
-        g.pid_rate_pitch.kI(tuning_value);
-        break;
-
-    case CH6_RATE_ROLL_PITCH_KD:
-        g.pid_rate_roll.kD(tuning_value);
-        g.pid_rate_pitch.kD(tuning_value);
-        break;
-
-    // Yaw tuning
-    case CH6_STABILIZE_YAW_KP:
-        g.p_stabilize_yaw.kP(tuning_value);
-        break;
-
-    case CH6_YAW_RATE_KP:
-        g.pid_rate_yaw.kP(tuning_value);
-        break;
-
-    case CH6_YAW_RATE_KD:
-        g.pid_rate_yaw.kD(tuning_value);
-        break;
-
-    // Altitude and throttle tuning
-    case CH6_ALTITUDE_HOLD_KP:
-        g.p_alt_hold.kP(tuning_value);
-        break;
-
-    case CH6_THROTTLE_RATE_KP:
-        g.p_vel_z.kP(tuning_value);
-        break;
-
-    case CH6_ACCEL_Z_KP:
-        g.pid_accel_z.kP(tuning_value);
-        break;
-
-    case CH6_ACCEL_Z_KI:
-        g.pid_accel_z.kI(tuning_value);
-        break;
-
-    case CH6_ACCEL_Z_KD:
-        g.pid_accel_z.kD(tuning_value);
-        break;
-
-    // Loiter and navigation tuning
-    case CH6_LOITER_POSITION_KP:
-        g.p_pos_xy.kP(tuning_value);
-        break;
-
-    case CH6_VEL_XY_KP:
-        g.pi_vel_xy.kP(tuning_value);
-        break;
-
-    case CH6_VEL_XY_KI:
-        g.pi_vel_xy.kI(tuning_value);
-        break;
-
-    case CH6_WP_SPEED:
-        // set waypoint navigation horizontal speed to 0 ~ 1000 cm/s
-        wp_nav.set_speed_xy(g.rc_6.control_in);
-        break;
-
-    // Acro roll pitch gain
-    case CH6_ACRO_RP_KP:
-        g.acro_rp_p = tuning_value;
-        break;
-
-    // Acro yaw gain
-    case CH6_ACRO_YAW_KP:
-        g.acro_yaw_p = tuning_value;
-        break;
-
-#if FRAME_CONFIG == HELI_FRAME
-    case CH6_HELI_EXTERNAL_GYRO:
-        motors.ext_gyro_gain(g.rc_6.control_in);
-        break;
-
-    case CH6_RATE_PITCH_FF:
-        g.pid_rate_pitch.ff(tuning_value);
-        break;
-
-    case CH6_RATE_ROLL_FF:
-        g.pid_rate_roll.ff(tuning_value);
-        break;
-
-    case CH6_RATE_YAW_FF:
-        g.pid_rate_yaw.ff(tuning_value);
-        break;
-#endif
-
-    case CH6_AHRS_YAW_KP:
-        ahrs._kp_yaw.set(tuning_value);
-        break;
-
-    case CH6_AHRS_KP:
-        ahrs._kp.set(tuning_value);
-        break;
-
-    case CH6_DECLINATION:
-        // set declination to +-20degrees
-        compass.set_declination(ToRad((2.0f * g.rc_6.control_in - g.radio_tuning_high)/100.0f), false);     // 2nd parameter is false because we do not want to save to eeprom because this would have a performance impact
-        break;
-
-    case CH6_CIRCLE_RATE:
-        // set circle rate
-        circle_nav.set_rate(g.rc_6.control_in/25-20);   // allow approximately 45 degree turn rate in either direction
-        break;
-
-    case CH6_SONAR_GAIN:
-        // set sonar gain
-        g.sonar_gain.set(tuning_value);
-        break;
-
-#if 0
-        // disabled for now - we need accessor functions
-    case CH6_EKF_VERTICAL_POS:
-        // EKF's baro vs accel (higher rely on accels more, baro impact is reduced)
-        ahrs.get_NavEKF()._gpsVertPosNoise = tuning_value;
-        break;
-
-    case CH6_EKF_HORIZONTAL_POS:
-        // EKF's gps vs accel (higher rely on accels more, gps impact is reduced)
-        ahrs.get_NavEKF()._gpsHorizPosNoise = tuning_value;
-        break;
-
-    case CH6_EKF_ACCEL_NOISE:
-        // EKF's accel noise (lower means trust accels more, gps & baro less)
-        ahrs.get_NavEKF()._accNoise = tuning_value;
-        break;
-#endif
-
-    case CH6_RC_FEEL_RP:
-        // roll-pitch input smoothing
-        g.rc_feel_rp = g.rc_6.control_in / 10;
-        break;
-
-    case CH6_RATE_PITCH_KP:
-        g.pid_rate_pitch.kP(tuning_value);
-        break;
-
-    case CH6_RATE_PITCH_KI:
-        g.pid_rate_pitch.kI(tuning_value);
-        break;
-
-    case CH6_RATE_PITCH_KD:
-        g.pid_rate_pitch.kD(tuning_value);
-        break;
-
-    case CH6_RATE_ROLL_KP:
-        g.pid_rate_roll.kP(tuning_value);
-        break;
-
-    case CH6_RATE_ROLL_KI:
-        g.pid_rate_roll.kI(tuning_value);
-        break;
-
-    case CH6_RATE_ROLL_KD:
-        g.pid_rate_roll.kD(tuning_value);
-        break;
-
-    case CH6_RATE_MOT_YAW_HEADROOM:
-        motors.set_yaw_headroom(tuning_value*1000);
-        break;
-
-     case CH6_RATE_YAW_FILT:
-         g.pid_rate_yaw.filt_hz(tuning_value);
-         break;
-    }
-}
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+/*
+ * tuning.pde - function to update various parameters in flight using the ch6 tuning knob
+ *      This should not be confused with the AutoTune feature which can bve found in control_autotune.pde
+ */
+
+// tuning - updates parameters based on the ch6 tuning knob's position
+//  should be called at 3.3hz
+static void tuning() {
+
+    // exit immediately if not tuning of when radio failsafe is invoked so tuning values are not set to zero
+    if ((g.radio_tuning <= 0) || failsafe.radio || failsafe.radio_counter != 0) {
+        return;
+    }
+
+    float tuning_value = (float)g.rc_6.control_in / 1000.0f;
+    g.rc_6.set_range(g.radio_tuning_low,g.radio_tuning_high);
+
+    switch(g.radio_tuning) {
+
+    // Roll, Pitch tuning
+    case CH6_STABILIZE_ROLL_PITCH_KP:
+        g.p_stabilize_roll.kP(tuning_value);
+        g.p_stabilize_pitch.kP(tuning_value);
+        break;
+
+    case CH6_RATE_ROLL_PITCH_KP:
+        g.pid_rate_roll.kP(tuning_value);
+        g.pid_rate_pitch.kP(tuning_value);
+        break;
+
+    case CH6_RATE_ROLL_PITCH_KI:
+        g.pid_rate_roll.kI(tuning_value);
+        g.pid_rate_pitch.kI(tuning_value);
+        break;
+
+    case CH6_RATE_ROLL_PITCH_KD:
+        g.pid_rate_roll.kD(tuning_value);
+        g.pid_rate_pitch.kD(tuning_value);
+        break;
+
+    // Yaw tuning
+    case CH6_STABILIZE_YAW_KP:
+        g.p_stabilize_yaw.kP(tuning_value);
+        break;
+
+    case CH6_YAW_RATE_KP:
+        g.pid_rate_yaw.kP(tuning_value);
+        break;
+
+    case CH6_YAW_RATE_KD:
+        g.pid_rate_yaw.kD(tuning_value);
+        break;
+
+    // Altitude and throttle tuning
+    case CH6_ALTITUDE_HOLD_KP:
+        g.p_alt_hold.kP(tuning_value);
+        break;
+
+    case CH6_THROTTLE_RATE_KP:
+        g.p_vel_z.kP(tuning_value);
+        break;
+
+    case CH6_ACCEL_Z_KP:
+        g.pid_accel_z.kP(tuning_value);
+        break;
+
+    case CH6_ACCEL_Z_KI:
+        g.pid_accel_z.kI(tuning_value);
+        break;
+
+    case CH6_ACCEL_Z_KD:
+        g.pid_accel_z.kD(tuning_value);
+        break;
+
+    // Loiter and navigation tuning
+    case CH6_LOITER_POSITION_KP:
+        g.p_pos_xy.kP(tuning_value);
+        break;
+
+    case CH6_VEL_XY_KP:
+        g.pi_vel_xy.kP(tuning_value);
+        break;
+
+    case CH6_VEL_XY_KI:
+        g.pi_vel_xy.kI(tuning_value);
+        break;
+
+    case CH6_WP_SPEED:
+        // set waypoint navigation horizontal speed to 0 ~ 1000 cm/s
+        wp_nav.set_speed_xy(g.rc_6.control_in);
+        break;
+
+    // Acro roll pitch gain
+    case CH6_ACRO_RP_KP:
+        g.acro_rp_p = tuning_value;
+        break;
+
+    // Acro yaw gain
+    case CH6_ACRO_YAW_KP:
+        g.acro_yaw_p = tuning_value;
+        break;
+
+#if FRAME_CONFIG == HELI_FRAME
+    case CH6_HELI_EXTERNAL_GYRO:
+        motors.ext_gyro_gain(g.rc_6.control_in);
+        break;
+
+    case CH6_RATE_PITCH_FF:
+        g.pid_rate_pitch.ff(tuning_value);
+        break;
+
+    case CH6_RATE_ROLL_FF:
+        g.pid_rate_roll.ff(tuning_value);
+        break;
+
+    case CH6_RATE_YAW_FF:
+        g.pid_rate_yaw.ff(tuning_value);
+        break;
+#endif
+
+    case CH6_AHRS_YAW_KP:
+        ahrs._kp_yaw.set(tuning_value);
+        break;
+
+    case CH6_AHRS_KP:
+        ahrs._kp.set(tuning_value);
+        break;
+
+    case CH6_DECLINATION:
+        // set declination to +-20degrees
+        compass.set_declination(ToRad((2.0f * g.rc_6.control_in - g.radio_tuning_high)/100.0f), false);     // 2nd parameter is false because we do not want to save to eeprom because this would have a performance impact
+        break;
+
+    case CH6_CIRCLE_RATE:
+        // set circle rate
+        circle_nav.set_rate(g.rc_6.control_in/25-20);   // allow approximately 45 degree turn rate in either direction
+        break;
+
+    case CH6_SONAR_GAIN:
+        // set sonar gain
+        g.sonar_gain.set(tuning_value);
+        break;
+
+#if 0
+        // disabled for now - we need accessor functions
+    case CH6_EKF_VERTICAL_POS:
+        // EKF's baro vs accel (higher rely on accels more, baro impact is reduced)
+        ahrs.get_NavEKF()._gpsVertPosNoise = tuning_value;
+        break;
+
+    case CH6_EKF_HORIZONTAL_POS:
+        // EKF's gps vs accel (higher rely on accels more, gps impact is reduced)
+        ahrs.get_NavEKF()._gpsHorizPosNoise = tuning_value;
+        break;
+
+    case CH6_EKF_ACCEL_NOISE:
+        // EKF's accel noise (lower means trust accels more, gps & baro less)
+        ahrs.get_NavEKF()._accNoise = tuning_value;
+        break;
+#endif
+
+    case CH6_RC_FEEL_RP:
+        // roll-pitch input smoothing
+        g.rc_feel_rp = g.rc_6.control_in / 10;
+        break;
+
+    case CH6_RATE_PITCH_KP:
+        g.pid_rate_pitch.kP(tuning_value);
+        break;
+
+    case CH6_RATE_PITCH_KI:
+        g.pid_rate_pitch.kI(tuning_value);
+        break;
+
+    case CH6_RATE_PITCH_KD:
+        g.pid_rate_pitch.kD(tuning_value);
+        break;
+
+    case CH6_RATE_ROLL_KP:
+        g.pid_rate_roll.kP(tuning_value);
+        break;
+
+    case CH6_RATE_ROLL_KI:
+        g.pid_rate_roll.kI(tuning_value);
+        break;
+
+    case CH6_RATE_ROLL_KD:
+        g.pid_rate_roll.kD(tuning_value);
+        break;
+
+    case CH6_RATE_MOT_YAW_HEADROOM:
+        motors.set_yaw_headroom(tuning_value*1000);
+        break;
+
+     case CH6_RATE_YAW_FILT:
+         g.pid_rate_yaw.filt_hz(tuning_value);
+         break;
+    }
+}