mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 06:28:27 -04:00
Copter: fixup Windows newlines
This commit is contained in:
Jonathan Challinger
Randy Mackay
parent
ee91be66cf
commit
0cff3a4292
@ -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); }
|
||||
}
|
||||
|
||||
@ -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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -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;
|
||||
}
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user