APM飞控程序解读

#if MOUNT == ENABLED

camera_mount.update_mount_type(); #endif

#if MOUNT2 == ENABLED

camera_mount2.update_mount_type(); #endif

check_usb_mux(); }

// called at 100hz but data from sensor only arrives at 20 Hz #if OPTFLOW == ENABLED

static void update_optical_flow(void) {

static uint32_t last_of_update = 0; static uint8_t of_log_counter = 0;

// if new data has arrived, process it

if( optflow.last_update != last_of_update ) { last_of_update = optflow.last_update;

optflow.update_position(ahrs.roll, ahrs.pitch, sin_yaw, cos_yaw, current_loc.alt); // updates internal lon and lat with estimation based on optical flow

// write to log at 5hz of_log_counter++;

if( of_log_counter >= 4 ) { of_log_counter = 0;

if (g.log_bitmask & MASK_LOG_OPTFLOW) { Log_Write_Optflow(); } } } }

#endif // OPTFLOW == ENABLED

// called at 50hz

static void update_GPS(void) {

// A counter that is used to grab at least 10 reads before commiting the Home location static uint8_t ground_start_count = 10;

g_gps->update();

if (g_gps->new_data && last_gps_time != g_gps->last_fix_time && g_gps->status() >= GPS::GPS_OK_FIX_2D) { // clear new data flag g_gps->new_data = false;

// save GPS time so we don't get duplicate reads last_gps_time = g_gps->last_fix_time;

// log location if we have at least a 2D fix

if (g.log_bitmask & MASK_LOG_GPS && motors.armed()) { DataFlash.Log_Write_GPS(g_gps, current_loc.alt); }

// for performance monitoring gps_fix_count++;

// run glitch protection and update AP_Notify gps_glitch.check_position();

if (AP_Notify::flags.gps_glitching != gps_glitch.glitching()) {

if (gps_glitch.glitching()) {

Log_Write_Error(ERROR_SUBSYSTEM_GPS, ERROR_CODE_GPS_GLITCH); }else{

Log_Write_Error(ERROR_SUBSYSTEM_GPS, ERROR_CODE_ERROR_RESOLVED); }

AP_Notify::flags.gps_glitching = gps_glitch.glitching(); }

// check if we can initialise home yet if (!ap.home_is_set) {

// if we have a 3d lock and valid location

if(g_gps->status() >= GPS::GPS_OK_FIX_3D && g_gps->latitude != 0) { if( ground_start_count > 0 ) { ground_start_count--; }else{

// after 10 successful reads store home location

// ap.home_is_set will be true so this will only happen once ground_start_count = 0; init_home();

// set system clock for log timestamps

hal.util->set_system_clock(g_gps->time_epoch_usec());

if (g.compass_enabled) {

// Set compass declination automatically

compass.set_initial_location(g_gps->latitude, g_gps->longitude); } } }else{

// start again if we lose 3d lock ground_start_count = 10; } }

#if CAMERA == ENABLED

if (camera.update_location(current_loc) == true) { do_take_picture(); }

#endif }

// check for loss of gps failsafe_gps_check(); }

// set_yaw_mode - update yaw mode and initialise any variables required bool set_yaw_mode(uint8_t new_yaw_mode) {

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

// return immediately if no change if( new_yaw_mode == yaw_mode ) { return true; }

switch( new_yaw_mode ) { case YAW_HOLD:

yaw_initialised = true; break; case YAW_ACRO:

yaw_initialised = true;

acro_yaw_rate = 0; break;

case YAW_LOOK_AT_NEXT_WP: if( ap.home_is_set ) {

yaw_initialised = true; }

break;

case YAW_LOOK_AT_LOCATION: if( ap.home_is_set ) {

// update bearing - assumes yaw_look_at_WP has been intialised before set_yaw_mode was called yaw_look_at_WP_bearing = pv_get_bearing_cd(inertial_nav.get_position(), yaw_look_at_WP); yaw_initialised = true; }

break;

case YAW_CIRCLE:

if( ap.home_is_set ) {

// set yaw to point to center of circle yaw_look_at_WP = circle_center;

// initialise bearing to current heading yaw_look_at_WP_bearing = ahrs.yaw_sensor; yaw_initialised = true; }

break;

case YAW_LOOK_AT_HEADING: yaw_initialised = true; break;

case YAW_LOOK_AT_HOME: if( ap.home_is_set ) {

yaw_initialised = true; }

break;

case YAW_LOOK_AHEAD:

if( ap.home_is_set ) {

yaw_initialised = true; }

break; case YAW_TOY:

yaw_initialised = true; break;

case YAW_RESETTOARMEDYAW:

nav_yaw = ahrs.yaw_sensor; // store current yaw so we can start rotating back to correct one yaw_initialised = true; break; }

// if initialisation has been successful update the yaw mode if( yaw_initialised ) {

yaw_mode = new_yaw_mode; }

// return success or failure return yaw_initialised; }

// update_yaw_mode - run high level yaw controllers // 100hz update rate

void update_yaw_mode(void) {

switch(yaw_mode) {

case YAW_HOLD:

// if we are landed reset yaw target to current heading

if (ap.land_complete) {

nav_yaw = ahrs.yaw_sensor; }

// heading hold at heading held in nav_yaw but allow input from pilot get_yaw_rate_stabilized_ef(g.rc_4.control_in); break;

case YAW_ACRO:

// pilot controlled yaw using rate controller get_yaw_rate_stabilized_bf(g.rc_4.control_in); break;

case YAW_LOOK_AT_NEXT_WP:

// if we are landed reset yaw target to current heading if (ap.land_complete) {

nav_yaw = ahrs.yaw_sensor; }else{

// point towards next waypoint (no pilot input accepted)

// we don't use wp_bearing because we don't want the copter to turn too much during flight nav_yaw = get_yaw_slew(nav_yaw, original_wp_bearing, AUTO_YAW_SLEW_RATE); }

get_stabilize_yaw(nav_yaw);

// if there is any pilot input, switch to YAW_HOLD mode for the next iteration if( g.rc_4.control_in != 0 ) { set_yaw_mode(YAW_HOLD); }

break;

case YAW_LOOK_AT_LOCATION:

// if we are landed reset yaw target to current heading if (ap.land_complete) {

nav_yaw = ahrs.yaw_sensor; }

// point towards a location held in yaw_look_at_WP get_look_at_yaw();

// if there is any pilot input, switch to YAW_HOLD mode for the next iteration if( g.rc_4.control_in != 0 ) { set_yaw_mode(YAW_HOLD); }

break;

case YAW_CIRCLE:

// if we are landed reset yaw target to current heading if (ap.land_complete) {

nav_yaw = ahrs.yaw_sensor; }

// points toward the center of the circle or does a panorama get_circle_yaw();

// if there is any pilot input, switch to YAW_HOLD mode for the next iteration if( g.rc_4.control_in != 0 ) { set_yaw_mode(YAW_HOLD); }

break;

case YAW_LOOK_AT_HOME:

// if we are landed reset yaw target to current heading if (ap.land_complete) {

nav_yaw = ahrs.yaw_sensor; }else{

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