diff --git a/ArduCopter/esc_calibration.pde b/ArduCopter/esc_calibration.pde index 620c9ee228..e7471339f9 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 8a70ca9b67..285c5939a9 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 23064c7551..d723f736d0 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; + } +}