APM飞控程序解读

} } }

set_target_alt_for_reporting(0); break;

case THROTTLE_HOLD: if(ap.auto_armed) {

// alt hold plus pilot input of climb rate

pilot_climb_rate = get_pilot_desired_climb_rate(g.rc_3.control_in);

// special handling if we have landed if (ap.land_complete) {

if (pilot_climb_rate > 0) {

// indicate we are taking off set_land_complete(false);

// clear i term when we're taking off set_throttle_takeoff(); }else{

// move throttle to minimum to keep us on the ground set_throttle_out(0, false);

// deactivate accel based throttle controller (it will be automatically re-enabled when alt-hold controller next runs)

throttle_accel_deactivate(); } }

// check land_complete flag again in case it was changed above if (!ap.land_complete) {

if( sonar_alt_health >= SONAR_ALT_HEALTH_MAX ) { // if sonar is ok, use surface tracking

get_throttle_surface_tracking(pilot_climb_rate); // this function calls set_target_alt_for_reporting for us }else{

// if no sonar fall back stabilize rate controller

get_throttle_rate_stabilized(pilot_climb_rate); // this function calls set_target_alt_for_reporting for us } } }else{

// pilot's throttle must be at zero so keep motors off set_throttle_out(0, false);

// deactivate accel based throttle controller throttle_accel_deactivate();

set_target_alt_for_reporting(0); }

break;

case THROTTLE_AUTO:

// auto pilot altitude controller with target altitude held in wp_nav.get_desired_alt() if(ap.auto_armed) {

// special handling if we are just taking off if (ap.land_complete) {

// tell motors to do a slow start. motors.slow_start(true); }

get_throttle_althold_with_slew(wp_nav.get_desired_alt(), -wp_nav.get_descent_velocity(), wp_nav.get_climb_velocity());

set_target_alt_for_reporting(wp_nav.get_desired_alt()); // To-Do: return get_destination_alt if we are flying to a waypoint }else{

// pilot's throttle must be at zero so keep motors off set_throttle_out(0, false);

// deactivate accel based throttle controller throttle_accel_deactivate();

set_target_alt_for_reporting(0); }

break;

case THROTTLE_LAND:

// landing throttle controller get_throttle_land();

set_target_alt_for_reporting(0); break; } }

// set_target_alt_for_reporting - set target altitude in cm for reporting purposes (logs and gcs) static void set_target_alt_for_reporting(float alt_cm) {

target_alt_for_reporting = alt_cm; }

// get_target_alt_for_reporting - returns target altitude in cm for reporting purposes (logs and gcs) static float get_target_alt_for_reporting() {

return target_alt_for_reporting; }

static void read_AHRS(void) {

// Perform IMU calculations and get attitude info //----------------------------------------------- #if HIL_MODE != HIL_MODE_DISABLED // update hil before ahrs update gcs_check_input(); #endif

ahrs.update();

omega = ins.get_gyro(); }

static void update_trig(void){ Vector2f yawvector;

const Matrix3f &temp = ahrs.get_dcm_matrix();

yawvector.x = temp.a.x; // sin yawvector.y = temp.b.x; // cos yawvector.normalize();

cos_pitch_x = safe_sqrt(1 - (temp.c.x * temp.c.x)); // level = 1

cos_roll_x = temp.c.z / cos_pitch_x; // level = 1

cos_pitch_x = constrain_float(cos_pitch_x, 0, 1.0);

// this relies on constrain_float() of infinity doing the right thing, // which it does do in avr-libc

cos_roll_x = constrain_float(cos_roll_x, -1.0, 1.0);

sin_yaw = constrain_float(yawvector.y, -1.0, 1.0); cos_yaw = constrain_float(yawvector.x, -1.0, 1.0);

// added to convert earth frame to body frame for rate controllers sin_pitch = -temp.c.x;

sin_roll = temp.c.y / cos_pitch_x;

// update wp_nav controller with trig values

wp_nav.set_cos_sin_yaw(cos_yaw, sin_yaw, cos_pitch_x);

//flat:

// 0 ° = cos_yaw: 1.00, sin_yaw: 0.00, // 90° = cos_yaw: 0.00, sin_yaw: 1.00, // 180 = cos_yaw: -1.00, sin_yaw: 0.00, // 270 = cos_yaw: 0.00, sin_yaw: -1.00, }

// read baro and sonar altitude at 20hz static void update_altitude() {

#if HIL_MODE == HIL_MODE_ATTITUDE

// we are in the SIM, fake out the baro and Sonar baro_alt = g_gps->altitude_cm;

if(g.sonar_enabled) {

sonar_alt = baro_alt; } #else

// read in baro altitude

baro_alt = read_barometer();

// read in sonar altitude

sonar_alt = read_sonar(); #endif // HIL_MODE == HIL_MODE_ATTITUDE

// write altitude info to dataflash logs

if ((g.log_bitmask & MASK_LOG_CTUN) && motors.armed()) { Log_Write_Control_Tuning(); } }

static void tuning(){

// exit immediately when radio failsafe is invoked so tuning values are not set to zero if (failsafe.radio || failsafe.radio_counter != 0) { return; }

tuning_value = (float)g.rc_6.control_in / 1000.0f;

g.rc_6.set_range(g.radio_tuning_low,g.radio_tuning_high); // 0 to 1

switch(g.radio_tuning) {

// Roll, Pitch tuning

case CH6_STABILIZE_ROLL_PITCH_KP:

g.pi_stabilize_roll.kP(tuning_value); g.pi_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.pi_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.pi_alt_hold.kP(tuning_value); break;

case CH6_THROTTLE_RATE_KP:

g.pid_throttle_rate.kP(tuning_value); break;

case CH6_THROTTLE_RATE_KD:

g.pid_throttle_rate.kD(tuning_value); break;

case CH6_THROTTLE_ACCEL_KP:

g.pid_throttle_accel.kP(tuning_value); break;

case CH6_THROTTLE_ACCEL_KI:

g.pid_throttle_accel.kI(tuning_value); break;

case CH6_THROTTLE_ACCEL_KD:

g.pid_throttle_accel.kD(tuning_value); break;

// Loiter and navigation tuning case CH6_LOITER_POSITION_KP:

g.pi_loiter_lat.kP(tuning_value); g.pi_loiter_lon.kP(tuning_value); break;

case CH6_LOITER_RATE_KP:

g.pid_loiter_rate_lon.kP(tuning_value); g.pid_loiter_rate_lat.kP(tuning_value); break;

case CH6_LOITER_RATE_KI:

g.pid_loiter_rate_lon.kI(tuning_value); g.pid_loiter_rate_lat.kI(tuning_value); break;

case CH6_LOITER_RATE_KD:

g.pid_loiter_rate_lon.kD(tuning_value); g.pid_loiter_rate_lat.kD(tuning_value);

联系客服:779662525#qq.com(#替换为@)