APM飞控程序解读 下载本文

get_stabilize_pitch(nav_pitch); break;

case ROLL_PITCH_SPORT:

// apply SIMPLE mode transform update_simple_mode();

// copy user input for reporting purposes control_roll = g.rc_1.control_in; control_pitch = g.rc_2.control_in;

get_roll_rate_stabilized_ef(g.rc_1.control_in); get_pitch_rate_stabilized_ef(g.rc_2.control_in); break;

#if AUTOTUNE == ENABLED

case ROLL_PITCH_AUTOTUNE:

// apply SIMPLE mode transform

if(ap.simple_mode && ap.new_radio_frame) { update_simple_mode(); }

// convert pilot input to lean angles

get_pilot_desired_lean_angles(g.rc_1.control_in, g.rc_2.control_in, control_roll, control_pitch);

// pass desired roll, pitch to stabilize attitude controllers get_stabilize_roll(control_roll); get_stabilize_pitch(control_pitch);

// copy user input for reporting purposes

get_autotune_roll_pitch_controller(g.rc_1.control_in, g.rc_2.control_in); break; #endif } #if FRAME_CONFIG != HELI_FRAME

if(g.rc_3.control_in == 0 && control_mode <= ACRO) { reset_rate_I(); } #endif //HELI_FRAME

if(ap.new_radio_frame) {

// clear new radio frame info ap.new_radio_frame = false; } }

static void

init_simple_bearing() {

// capture current cos_yaw and sin_yaw values simple_cos_yaw = cos_yaw; simple_sin_yaw = sin_yaw;

// initialise super simple heading (i.e. heading towards home) to be 180 deg from simple mode heading super_simple_last_bearing = wrap_360_cd(ahrs.yaw_sensor+18000); super_simple_cos_yaw = simple_cos_yaw; super_simple_sin_yaw = simple_sin_yaw;

// log the simple bearing to dataflash if (g.log_bitmask != 0) {

Log_Write_Data(DATA_INIT_SIMPLE_BEARING, ahrs.yaw_sensor); }

}

// update_simple_mode - rotates pilot input if we are in simple mode void update_simple_mode(void) {

float rollx, pitchx;

// exit immediately if no new radio frame or not in simple mode if (ap.simple_mode == 0 || !ap.new_radio_frame) { return; }

if (ap.simple_mode == 1) {

// rotate roll, pitch input by -initial simple heading (i.e. north facing) rollx = g.rc_1.control_in*simple_cos_yaw - g.rc_2.control_in*simple_sin_yaw; pitchx = g.rc_1.control_in*simple_sin_yaw + g.rc_2.control_in*simple_cos_yaw; }else{

// rotate roll, pitch input by -super simple heading (reverse of heading to home)

rollx = g.rc_1.control_in*super_simple_cos_yaw - g.rc_2.control_in*super_simple_sin_yaw; pitchx = g.rc_1.control_in*super_simple_sin_yaw + g.rc_2.control_in*super_simple_cos_yaw; }

// rotate roll, pitch input from north facing to vehicle's perspective g.rc_1.control_in = rollx*cos_yaw + pitchx*sin_yaw; g.rc_2.control_in = -rollx*sin_yaw + pitchx*cos_yaw; }

// update_super_simple_bearing - adjusts simple bearing based on location // should be called after home_bearing has been updated void update_super_simple_bearing(bool force_update) {

// check if we are in super simple mode and at least 10m from home

if(force_update || (ap.simple_mode == 2 && home_distance > SUPER_SIMPLE_RADIUS)) { // check the bearing to home has changed by at least 5 degrees if (labs(super_simple_last_bearing - home_bearing) > 500) { super_simple_last_bearing = home_bearing;

float angle_rad = radians((super_simple_last_bearing+18000)/100); super_simple_cos_yaw = cosf(angle_rad); super_simple_sin_yaw = sinf(angle_rad); } } }

// set_throttle_mode - sets the throttle mode and initialises any variables as required bool set_throttle_mode( uint8_t new_throttle_mode ) {

// boolean to ensure proper initialisation of throttle modes bool throttle_initialised = false;

// return immediately if no change

if( new_throttle_mode == throttle_mode ) { return true; }

// initialise any variables required for the new throttle mode switch(new_throttle_mode) { case THROTTLE_MANUAL:

case THROTTLE_MANUAL_TILT_COMPENSATED:

throttle_accel_deactivate(); // this controller does not use accel based throttle controller

altitude_error = 0; // clear altitude error reported to GCS throttle_initialised = true;

break;

case THROTTLE_HOLD: case THROTTLE_AUTO:

controller_desired_alt = get_initial_alt_hold(current_loc.alt, climb_rate); // reset controller desired altitude to current altitude

wp_nav.set_desired_alt(controller_desired_alt); // same as above but for loiter controller

if ( throttle_mode <= THROTTLE_MANUAL_TILT_COMPENSATED ) { // reset the alt hold I terms if previous throttle mode was manual reset_throttle_I();

set_accel_throttle_I_from_pilot_throttle(get_pilot_desired_throttle(g.rc_3.control_in)); }

throttle_initialised = true; break;

case THROTTLE_LAND:

reset_land_detector(); // initialise land detector

controller_desired_alt = get_initial_alt_hold(current_loc.alt, climb_rate); // reset controller desired altitude to current altitude throttle_initialised = true; break; }

// update the throttle mode if( throttle_initialised ) {

throttle_mode = new_throttle_mode;

// reset some variables used for logging desired_climb_rate = 0; nav_throttle = 0; }

// return success or failure return throttle_initialised; }

// update_throttle_mode - run high level throttle controllers // 50 hz update rate

void update_throttle_mode(void) {

int16_t pilot_climb_rate;

int16_t pilot_throttle_scaled;

if(ap.do_flip) // this is pretty bad but needed to flip in AP modes. return;

#if FRAME_CONFIG == HELI_FRAME if (control_mode == STABILIZE){ motors.stab_throttle = true; } else { motors.stab_throttle = false; }

// allow swash collective to move if we are in manual throttle modes, even if disarmed if( !motors.armed() ) {

if ( !(throttle_mode == THROTTLE_MANUAL) && !(throttle_mode == THROTTLE_MANUAL_TILT_COMPENSATED)){ throttle_accel_deactivate(); // do not allow the accel based throttle to override our command return; } }

#else // HELI_FRAME

// do not run throttle controllers if motors disarmed if( !motors.armed() ) {

set_throttle_out(0, false);

throttle_accel_deactivate(); // do not allow the accel based throttle to override our command set_target_alt_for_reporting(0); return; }

#endif // HELI_FRAME

switch(throttle_mode) {

case THROTTLE_MANUAL:

// completely manual throttle if(g.rc_3.control_in <= 0){ set_throttle_out(0, false); }else{

// send pilot's output directly to motors

pilot_throttle_scaled = get_pilot_desired_throttle(g.rc_3.control_in); set_throttle_out(pilot_throttle_scaled, false);

// update estimate of throttle cruise #if FRAME_CONFIG == HELI_FRAME

update_throttle_cruise(motors.coll_out); #else update_throttle_cruise(pilot_throttle_scaled); #endif //HELI_FRAME

// check if we've taken off yet

if (!ap.takeoff_complete && motors.armed()) {

if (pilot_throttle_scaled > g.throttle_cruise) { // we must be in the air by now set_takeoff_complete(true); } } }

set_target_alt_for_reporting(0); break;

case THROTTLE_MANUAL_TILT_COMPENSATED:

// manual throttle but with angle boost if (g.rc_3.control_in <= 0) {

set_throttle_out(0, false); // no need for angle boost with zero throttle }else{

pilot_throttle_scaled = get_pilot_desired_throttle(g.rc_3.control_in); set_throttle_out(pilot_throttle_scaled, true);

// update estimate of throttle cruise #if FRAME_CONFIG == HELI_FRAME

update_throttle_cruise(motors.coll_out); #else update_throttle_cruise(pilot_throttle_scaled); #endif //HELI_FRAME

if (!ap.takeoff_complete && motors.armed()) {

if (pilot_throttle_scaled > g.throttle_cruise) { // we must be in the air by now set_takeoff_complete(true);