该文件是APM的主文件。
#define SCHED_TASK(func, rate_hz, max_time_micros) SCHED_TASK_CLASS(Copter, &copter, func, rate_hz, max_time_micros)
/*
scheduler table for fast CPUs - all regular tasks apart from the fast_loop()
should be listed here, along with how often they should be called (in hz)
and the maximum time they are expected to take (in microseconds)
*/
const AP_Scheduler::Task Copter::scheduler_tasks[] = {
SCHED_TASK(rc_loop, 100, 130),
SCHED_TASK(throttle_loop, 50, 75),
SCHED_TASK(update_GPS, 50, 200),
#if OPTFLOW == ENABLED
SCHED_TASK(update_optical_flow, 200, 160),
#endif
SCHED_TASK(update_batt_compass, 10, 120),
SCHED_TASK(read_aux_switches, 10, 50),
SCHED_TASK(arm_motors_check, 10, 50),
SCHED_TASK(auto_disarm_check, 10, 50),
SCHED_TASK(auto_trim, 10, 75),
SCHED_TASK(read_rangefinder, 20, 100),
SCHED_TASK(update_altitude, 10, 100),
SCHED_TASK(run_nav_updates, 50, 100),
SCHED_TASK(update_throttle_hover,100, 90),
SCHED_TASK(three_hz_loop, 3, 75),
SCHED_TASK(compass_accumulate, 100, 100),
SCHED_TASK(barometer_accumulate, 50, 90),
#if PRECISION_LANDING == ENABLED
SCHED_TASK(update_precland, 400, 50),
#endif
#if FRAME_CONFIG == HELI_FRAME
SCHED_TASK(check_dynamic_flight, 50, 75),
#endif
SCHED_TASK(update_notify, 50, 90),
SCHED_TASK(one_hz_loop, 1, 100),
SCHED_TASK(ekf_check, 10, 75),
SCHED_TASK(landinggear_update, 10, 75),
SCHED_TASK(lost_vehicle_check, 10, 50),
SCHED_TASK(gcs_check_input, 400, 180),
SCHED_TASK(gcs_send_heartbeat, 1, 110),
SCHED_TASK(gcs_send_deferred, 50, 550),
SCHED_TASK(gcs_data_stream_send, 50, 550),
SCHED_TASK(update_mount, 50, 75),
SCHED_TASK(update_trigger, 50, 75),
SCHED_TASK(ten_hz_logging_loop, 10, 350),
SCHED_TASK(twentyfive_hz_logging, 25, 110),
SCHED_TASK(dataflash_periodic, 400, 300),
SCHED_TASK(perf_update, 0.1, 75),
SCHED_TASK(read_receiver_rssi, 10, 75),
SCHED_TASK(rpm_update, 10, 200),
SCHED_TASK(compass_cal_update, 100, 100),
SCHED_TASK(accel_cal_update, 10, 100),
#if ADSB_ENABLED == ENABLED
SCHED_TASK(avoidance_adsb_update, 10, 100),
#endif
#if ADVANCED_FAILSAFE == ENABLED
SCHED_TASK(afs_fs_check, 10, 100),
#endif
SCHED_TASK(terrain_update, 10, 100),
#if EPM_ENABLED == ENABLED
SCHED_TASK(epm_update, 10, 75),
#endif
#ifdef USERHOOK_FASTLOOP
SCHED_TASK(userhook_FastLoop, 100, 75),
#endif
#ifdef USERHOOK_50HZLOOP
SCHED_TASK(userhook_50Hz, 50, 75),
#endif
#ifdef USERHOOK_MEDIUMLOOP
SCHED_TASK(userhook_MediumLoop, 10, 75),
#endif
#ifdef USERHOOK_SLOWLOOP
SCHED_TASK(userhook_SlowLoop, 3.3, 75),
#endif
#ifdef USERHOOK_SUPERSLOWLOOP
SCHED_TASK(userhook_SuperSlowLoop, 1, 75),
#endif
SCHED_TASK(button_update, 5, 100),
};
注意，这些任务都是.cpp文件里的某个函数。
void Copter::loop()
{
// wait for an INS sample
ins.wait_for_sample(); 检查是否有陀螺仪和加速度计数据。其他传感器数据可以没有，飞机要运行这两个数据必须有。
uint32_t timer = micros();
// check loop time
perf_info_check_loop_time(timer - fast_loopTimer);
// used by PI Loops
G_Dt = (float)(timer - fast_loopTimer) / 1000000.0f;
fast_loopTimer = timer;
// for mainloop failure monitoring
mainLoop_count++;
// Execute the fast loop
// ---------------------
fast_loop(); 主循环。完成了读数据，根据飞行模式进行相应控制，输出给电机整个过程。
// tell the scheduler one tick has passed
scheduler.tick(); 调度器
// run all the tasks that are due to run. Note that we only
// have to call this once per loop, as the tasks are scheduled
// in multiples of the main loop tick. So if they don't run on
// the first call to the scheduler they won't run on a later
// call until scheduler.tick() is called again
uint32_t time_available = (timer + MAIN_LOOP_MICROS) - micros();
scheduler.run(time_available > MAIN_LOOP_MICROS ? 0u : time_available);所有任务执行一遍，也就是把上面列出的函数执行一遍。除了fast_loop()。
}
// Main loop - 400hz
void Copter::fast_loop()
{
// IMU DCM Algorithm
// --------------------
read_AHRS(); 使用余弦矩阵法，更新旋转矩阵，得到姿态角。
// run low level rate controllers that only require IMU data
attitude_control.rate_controller_run(); 角速度控制器，给出达到期望欧拉角,电机应该的PWM值。
#if FRAME_CONFIG == HELI_FRAME 硬件仿真
update_heli_control_dynamics();
#endif //HELI_FRAME
// send outputs to the motors library
motors_output();把上面的值进行处理，输出给电调
// Inertial Nav
// --------------------
read_inertia();注意，这里读的是当前点的经纬度，如果有HOME点则高度是高于HOME点高度，没有HOME点，则读的是地理坐标系高度（高于起飞点高度）
// check if ekf has reset target heading
check_ekf_yaw_reset();检查yaw是否重置，如果重置，计算yaw的改变量，并根据该改变量，更新旋转矩阵
// run the attitude controllers
update_flight_mode();更新飞行模式。根据不同的飞行模式，调用合适的姿态控制器。模式很多，不一一分析，但注意调用的是mode_run()。run函数里面，调用WP控制器，再调用POS控制器，再调用roll.pitch位置控制器，最终得到期望的姿态角速度。但没有调用roll，pitch，yaw速度控制器。
// update home from EKF if necessary
update_home_from_EKF(); 更新HOME点，如果HOME点已经设置，则返回，如果没有，则把当前点设为HOME点。HOME点可能是在飞行过程中的，不是非要起飞点。
// check if we've landed or crashed
update_land_and_crash_detectors();检查是否着陆或是坠毁
#if MOUNT == ENABLED
// camera mount's fast update
camera_mount.update_fast(); 相机
#endif
// log sensor health
if (should_log(MASK_LOG_ANY)) {
Log_Sensor_Health(); 记录。
}
}
void Copter::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();
}
AP_AHRS_DCM::update(void)
{
float delta_t;
if (_last_startup_ms == 0) {
_last_startup_ms = AP_HAL::millis();
}
// tell the IMU to grab some data
_ins.update(); 获取陀螺仪和加速度数据。
// ask the IMU how much time this sensor reading represents
delta_t = _ins.get_delta_time(); 两次获得陀螺仪和加速度数据的时间间隔。
// if the update call took more than 0.2 seconds then discard it,
// otherwise we may move too far. This happens when arming motors
// in ArduCopter
if (delta_t > 0.2f) {
memset(&_ra_sum[0], 0, sizeof(_ra_sum));
_ra_deltat = 0;
return;
}
// Integrate the DCM matrix using gyro inputs
matrix_update(delta_t); 算旋转矩阵
// Normalize the DCM matrix
normalize();强制正交化
// Perform drift correction
drift_correction(delta_t); 互补滤波，计算偏差。包括加速度计校正roll，pitch。磁力计和GPS校正yaw.
// paranoid check for bad values in the DCM matrix
check_matrix(); 检查旋转矩阵，看是否有无穷大值，如果有，重置
// Calculate pitch, roll, yaw for stabilization and navigation
euler_angles();根据余弦矩阵，计算欧拉角
// update trig values including _cos_roll, cos_pitch
update_trig(); 计算三角函数
}
void AC_AttitudeControl_Heli::rate_controller_run()
{
// call rate controllers and send output to motors object
// if using a flybar passthrough roll and pitch directly to motors
if (_flags_heli.flybar_passthrough) {
_motors.set_roll(_passthrough_roll/4500.0f);
_motors.set_pitch(_passthrough_pitch/4500.0f);
} else {
rate_bf_to_motor_roll_pitch(_rate_target_ang_vel.x, _rate_target_ang_vel.y); 执行该行，
角速度PID控制器 传入期望roll,pitch角速度。计算出rollout ，pitchout 给电机
}
if (_flags_heli.tail_passthrough) {
_motors.set_yaw(_passthrough_yaw/4500.0f);
} else {
_motors.set_yaw(rate_target_to_motor_yaw(_rate_target_ang_vel.z));执行该行，角速度PID控制器 传入期望yaw角速度。计算出yawout 给电机
}
}
// read_inertia - read inertia in from accelerometers
void Copter::read_inertia()
{
// inertial altitude estimates
inertial_nav.update(G_Dt);更新飞机当前点的在地理坐标系、WGS84坐标系下的位置速度信息，并求了垂直方向的位置导数。
// pull position from interial nav library
current_loc.lng = inertial_nav.get_longitude();获得当前位置经度
current_loc.lat = inertial_nav.get_latitude();获得当前位置纬度
// exit immediately if we do not have an altitude estimate
if (!inertial_nav.get_filter_status().flags.vert_pos) {
return; 如果没有高度信息，就退出
}
// without home return alt above the EKF origin
if (ap.home_state == HOME_UNSET) {
// with inertial nav we can update the altitude and climb rate at 50hz
current_loc.alt = inertial_nav.get_altitude();如果没有HOME点，则取地理坐标系高度
} else {
// with inertial nav we can update the altitude and climb rate at 50hz
current_loc.alt = pv_alt_above_home(inertial_nav.get_altitude());如果有HOME点，则取高于HOME点高度
}
// set flags and get velocity
current_loc.flags.relative_alt = true;
climb_rate = inertial_nav.get_velocity_z();获得垂直方向的速度
}
把EKF滤波得到的位置信息，转换到NEU坐标系下，供上层控制器使用。
void AP_InertialNav_NavEKF::update(float dt)
{
// get the NE position relative to the local earth frame origin
Vector2f posNE;
if (_ahrs_ekf.get_relative_position_NE(posNE)) {
_relpos_cm.x = posNE.x * 100; // convert from m to cm 把m转换成cm
_relpos_cm.y = posNE.y * 100; // convert from m to cm
}
// get the D position relative to the local earth frame origin地理坐标系，以起飞点为原点
float posD;
if (_ahrs_ekf.get_relative_position_D(posD)) {
_relpos_cm.z = - posD * 100; // convert from m in NED to cm in NEU
}
// get the absolute WGS-84 position 地心坐标系，为GPS建立的坐标系，坐标系的原点位于地球质心，z轴指向（国际时间局）BIH1984.0定义的协议地球极(CTP)方向，x轴指向BIH1984.0的零度子午面和CTP赤道的交点，y轴通过右手规则确定。
_haveabspos = _ahrs_ekf.get_position(_abspos);获得经纬度，绝对高度
// get the velocity relative to the local earth frame获得地理坐标系速度
Vector3f velNED;
if (_ahrs_ekf.get_velocity_NED(velNED)) {
_velocity_cm = velNED * 100; // convert to cm/s
_velocity_cm.z = -_velocity_cm.z; // convert from NED to NEU
}
// Get a derivative of the vertical position which is kinematically consistent with the vertical position is required by some control loops. 垂直方向的位置求导数
// This is different to the vertical velocity from the EKF which is not always consistent with the verical position due to the various errors that are being corrected for.它与EKF算出来的垂直速度不同，由于EKF校正各种错误，速度是不连续的。
if (_ahrs_ekf.get_vert_pos_rate(_pos_z_rate)) {
_pos_z_rate *= 100; // convert to cm/s
_pos_z_rate = - _pos_z_rate; // InertialNav is NEU 垂直方向位置求导，不等同于实时速度
}
}
void Copter::check_ekf_yaw_reset()
{
float yaw_angle_change_rad = 0.0f;
uint32_t new_ekfYawReset_ms = ahrs.getLastYawResetAngle(yaw_angle_change_rad);由于上一次yaw重置，yaw会改变
if (new_ekfYawReset_ms != ekfYawReset_ms) {
attitude_control.shift_ef_yaw_target(ToDeg(yaw_angle_change_rad) * 100.0f);把yaw的变化加到旋转矩阵中。
ekfYawReset_ms = new_ekfYawReset_ms;
Log_Write_Event(DATA_EKF_YAW_RESET);
}
}
// Shifts earth frame yaw target by yaw_shift_cd. yaw_shift_cd should be in centidegrees and is added to the current target heading
void AC_AttitudeControl::shift_ef_yaw_target(float yaw_shift_cd)
{
float yaw_shift = radians(yaw_shift_cd*0.01f); 把yaw改变的角度转换成弧度
Quaternion _attitude_target_update_quat;
_attitude_target_update_quat.from_axis_angle(Vector3f(0.0f, 0.0f, yaw_shift));
_attitude_target_quat = _attitude_target_update_quat*_attitude_target_quat; 把旋转矩阵加上yaw变化
}
void Copter::update_land_and_crash_detectors()
{
// update 1hz filtered acceleration
Vector3f accel_ef = ahrs.get_accel_ef_blended(); 获得地理坐标系下的加速度，blended是多个加速度计测量值的转换到地理坐标系后，的最优值
accel_ef.z += GRAVITY_MSS; 加上9.8
land_accel_ef_filter.apply(accel_ef, MAIN_LOOP_SECONDS);滤波 ，400HZ
update_land_detector();
#if PARACHUTE == ENABLED
// check parachute
parachute_check();
#endif
crash_check();
}
着陆检测，1.机体坐标系下加速度<1m/s^2;2.地理坐标系下垂直速度<1m/s;3.电机输出在最小值；4.测距仪测得对地高度小于2m
// update_land_detector - checks if we have landed and updates the ap.land_complete flag
// called at MAIN_LOOP_RATE
void Copter::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
判断着陆时，不能相信以下：气压计、EKF、姿态角或偏差、陀螺仪、测距仪、输入推力
if (!motors.armed()) { 判断是否解锁
// if disarmed, always landed.
set_land_complete(true); 如果没解锁，则一直在地上。
} else if (ap.land_complete) {
#if FRAME_CONFIG == HELI_FRAME
// if rotor speed and collective pitch are high then clear landing flag
if (motors.get_throttle() > get_non_takeoff_throttle() && !motors.limit.throttle_lower && motors.rotor_runup_complete()) {
#else
// if throttle output is high then clear landing flag
if (motors.get_throttle() > get_non_takeoff_throttle()) {
#endif
set_land_complete(false); 如果输出推力很大，肯定不是着陆。
}
} else {
#if FRAME_CONFIG == HELI_FRAME
// check that collective pitch is on lower limit (should be constrained by LAND_COL_MIN)
bool motor_at_lower_limit = motors.limit.throttle_lower;
#else
// check that the average throttle output is near minimum (less than 12.5% hover throttle)
bool motor_at_lower_limit = motors.limit.throttle_lower && attitude_control.is_throttle_mix_min();检查推力输出是不是接近最小值，小于0.125的盘旋推力
#endif
// check that the airframe is not accelerating (not falling or breaking after fast forward flight)
bool accel_stationary = (land_accel_ef_filter.get().length() <= LAND_DETECTOR_ACCEL_MAX);检查机体坐标系下，加速度是不是小于<1
// check that vertical speed is within 1m/s of zero
bool descent_rate_low = fabsf(inertial_nav.get_velocity_z()) < 100;检查垂直速度<1
测距仪是好的，也只能检查着陆是否小于2m
// if we have a healthy rangefinder only allow landing detection below 2 meters
bool rangefinder_check = (!rangefinder_alt_ok() || rangefinder_state.alt_cm_filt.get() < LAND_RANGEFINDER_MIN_ALT_CM);
如果以上条件都满足
if (motor_at_lower_limit && accel_stationary && descent_rate_low && rangefinder_check) {
// landed criteria met - increment the counter and check if we've triggered
if( land_detector_count < ((float)LAND_DETECTOR_TRIGGER_SEC)*scheduler.get_loop_rate_hz()) {
land_detector_count++; 主函数1s循环次数
} else {
set_land_complete(true); 且着陆检测次数已经达到，即已检测1s
}
} else {
// we've sensed movement up or down so reset land_detector
land_detector_count = 0;一旦有一次不满足，检测次数立马清零
}
}
set_land_complete_maybe(ap.land_complete || (land_detector_count >= LAND_DETECTOR_MAYBE_TRIGGER_SEC*scheduler.get_loop_rate_hz()));
}