APM程序分析-ArduCopter.cpp

该文件是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()));

}