APM飞控程序解读

break;

case CH6_WP_SPEED:

// set waypoint navigation horizontal speed to 0 ~ 1000 cm/s wp_nav.set_horizontal_velocity(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;

case CH6_RELAY:

if (g.rc_6.control_in > 525) relay.on(); if (g.rc_6.control_in < 475) relay.off(); break;

#if FRAME_CONFIG == HELI_FRAME case CH6_HELI_EXTERNAL_GYRO:

motors.ext_gyro_gain = tuning_value; break; #endif

case CH6_OPTFLOW_KP:

g.pid_optflow_roll.kP(tuning_value); g.pid_optflow_pitch.kP(tuning_value); break;

case CH6_OPTFLOW_KI:

g.pid_optflow_roll.kI(tuning_value); g.pid_optflow_pitch.kI(tuning_value); break;

case CH6_OPTFLOW_KD:

g.pid_optflow_roll.kD(tuning_value); g.pid_optflow_pitch.kD(tuning_value); break; #if HIL_MODE != HIL_MODE_ATTITUDE // gains in HIL mode

case CH6_AHRS_YAW_KP:

ahrs._kp_yaw.set(tuning_value); break;

case CH6_AHRS_KP:

ahrs._kp.set(tuning_value); break; #endif

case CH6_INAV_TC:

// To-Do: allowing tuning TC for xy and z separately inertial_nav.set_time_constant_xy(tuning_value); inertial_nav.set_time_constant_z(tuning_value); break;

case CH6_DECLINATION:

// set declination to +-20degrees

do not allow modifying _kp or _kp_yaw 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

g.circle_rate.set(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; } }

AP_HAL_MAIN();