继续记录SITL的大循环

task列表在哪初始化？有点类似与kernel加了一推回调函数在大循环里面

const AP_Scheduler::Task Sub::scheduler_tasks\[\] = {

// update INS immediately to get current gyro data populated

FAST_TASK_CLASS(AP_InertialSensor, &sub.ins, update),

// run low level rate controllers that only require IMU data

FAST_TASK(run_rate_controller),

// send outputs to the motors library immediately

FAST_TASK(motors_output),

// run EKF state estimator (expensive)

FAST_TASK(read_AHRS),

// Inertial Nav

FAST_TASK(read_inertia),

// check if ekf has reset target heading

FAST_TASK(check_ekf_yaw_reset),

// run the attitude controllers

FAST_TASK(update_flight_mode),

// update home from EKF if necessary

FAST_TASK(update_home_from_EKF),

// check if we've reached the surface or bottom

FAST_TASK(update_surface_and_bottom_detector),

#if HAL_MOUNT_ENABLED

// camera mount's fast update

FAST_TASK_CLASS(AP_Mount, &sub.camera_mount, update_fast),

#endif

SCHED_TASK(fifty_hz_loop, 50, 75, 3),

#if AP_SUB_RC_ENABLED

SCHED_TASK(rc_loop, 50, 130, 3),

#endif

SCHED_TASK_CLASS(AP_GPS, &sub.gps, update, 50, 200, 6),

#if AP_OPTICALFLOW_ENABLED

SCHED_TASK_CLASS(AP_OpticalFlow, &sub.optflow, update, 200, 160, 9),

#endif

SCHED_TASK(update_batt_compass, 10, 120, 12),

SCHED_TASK(read_rangefinder, 20, 100, 15),

SCHED_TASK(update_altitude, 10, 100, 18),

#if AP_SUB_RC_ENABLED

SCHED_TASK_CLASS(RC_Channels, (RC_Channels*)&sub.g2.rc_channels, read_aux_all, 10, 50, 18),

#endif

SCHED_TASK(three_hz_loop, 3, 75, 21),

SCHED_TASK(update_turn_counter, 10, 50, 24),

SCHED_TASK(one_hz_loop, 1, 100, 33),

SCHED_TASK_CLASS(GCS, (GCS*)&sub._gcs, update_receive, 400, 180, 36),

SCHED_TASK_CLASS(GCS, (GCS*)&sub._gcs, update_send, 400, 550, 39),

#if HAL_MOUNT_ENABLED

SCHED_TASK_CLASS(AP_Mount, &sub.camera_mount, update, 50, 75, 45),

#endif

#if AP_CAMERA_ENABLED

SCHED_TASK_CLASS(AP_Camera, &sub.camera, update, 50, 75, 48),

#endif

#if HAL_LOGGING_ENABLED

SCHED_TASK(ten_hz_logging_loop, 10, 350, 51),

SCHED_TASK(twentyfive_hz_logging, 25, 110, 54),

SCHED_TASK(loop_rate_logging, LOOP_RATE, 50, 55),

SCHED_TASK_CLASS(AP_Logger, &sub.logger, periodic_tasks, 400, 300, 57),

#endif

SCHED_TASK_CLASS(AP_InertialSensor, &sub.ins, periodic, 400, 50, 60),

#if HAL_LOGGING_ENABLED

SCHED_TASK_CLASS(AP_Scheduler, &sub.scheduler, update_logging, 0.1, 75, 63),

#endif

SCHED_TASK(terrain_update, 10, 100, 72),

#if AP_STATS_ENABLED

SCHED_TASK(stats_update, 1, 200, 76),

#endif

#ifdef USERHOOK_FASTLOOP

SCHED_TASK(userhook_FastLoop, 100, 75, 78),

#endif

#ifdef USERHOOK_50HZLOOP

SCHED_TASK(userhook_50Hz, 50, 75, 81),

#endif

#ifdef USERHOOK_MEDIUMLOOP

SCHED_TASK(userhook_MediumLoop, 10, 75, 84),

#endif

#ifdef USERHOOK_SLOWLOOP

SCHED_TASK(userhook_SlowLoop, 3.3, 75, 87),

#endif

#ifdef USERHOOK_SUPERSLOWLOOP

SCHED_TASK(userhook_SuperSlowLoop, 1, 75, 90),

#endif

};

#0 AP_AHRS::update_state (this=0x555555b02cd8 <copter+11128>) at ../../libraries/AP_AHRS/AP_AHRS.cpp:444

#1 0x000055555561032f in AP_AHRS::update (this=0x555555b02cd8 <copter+11128>, skip_ins_update=true)

at ../../libraries/AP_AHRS/AP_AHRS.cpp:594

#2 0x00005555555d0bb5 in Copter::read_AHRS (this=0x555555b00160 <copter>) at ../../ArduCopter/Copter.cpp:919

#3 0x00005555555d2340 in Functor<void>::method_wrapper<Copter, &Copter::read_AHRS> (obj=0x555555b00160 <copter>)

at ../../libraries/AP_HAL/utility/functor.h:88

#4 0x000055555568d6b6 in Functor<void>::operator() (this=0x555555ac9ee0 <Copter::scheduler_tasks+128>)

at ../../libraries/AP_HAL/utility/functor.h:54

#5 0x00005555556ee4b3 in AP_Scheduler::run (this=0x555555b002b0 <copter+336>, time_available=2500)

at ../../libraries/AP_Scheduler/AP_Scheduler.cpp:270

#6 0x00005555556ee962 in AP_Scheduler::loop (this=0x555555b002b0 <copter+336>)

看一看这个电量咋算出来的

float Frame::get_current_amp(void)

{

float current = 0;

for (uint8_t i=0; i<num_motors; i++) {

current += motors $i$ .get_current();

}

return current;

}

(gdb) p frame->motors $0$ .current

$10 = 7.59481621

(gdb) p frame->motors $1$ .current

$11 = 7.59473801

(gdb) p frame->motors $2$ .current

$12 = 7.59512186

(gdb) p frame->motors $3$ .current

$13 = 7.59443188

加起来....

上一波理论知识点DCM

这个 dcm 矩阵描述了当前飞机的姿态。虽然它看起来只是一堆数字，但它实际上精确地包含了飞机的横滚 (Roll)、俯仰 (Pitch) 和偏航 (Yaw) 信息。

我们可以将它转换成人更容易理解的欧拉角。你给出的这个矩阵对应的姿态大致是：

偏航角 (Yaw): 约 -111.6° (或 248.4°)

俯仰角 (Pitch): 约 -26.4° (机头向下)

横滚角 (Roll): 约 -21.1° (向左倾斜)

//////////

如何直观地观察这个矩阵？

DCM 矩阵的每一列，代表了地球坐标系（北-东-地）的轴在飞机机体坐标系下的方向向量：

第一列 (a)：(-0.36, -0.91, 0.20)，这是地球的"北"轴在飞机里的指向。

第二列 (b)：(0.80, -0.41, -0.44)，这是地球的"东"轴在飞机里的指向。

第三列 (c)：(0.49, -0.00, 0.87)，这是地球的"地"轴（重力方向）在飞机里的指向。

从这个矩阵我们能直观地看出一些飞机状态：

第三列 (c) 严重偏离了 (0,0,1)：这说明飞机没有平飞。理想的平飞状态，第三列应该是接近 (0, 0, 1)，意味着重力在飞机的 Z 轴上。而你现在是 (0.49, -0.00, 0.87)，表明飞机有一个很大的俯仰角（机头向下或向上），导致重力被分解到了 X 轴上。

第一列 (a) 和第二列 (b) 不成单位正交：这表明飞机正在持续旋转，或者说你的 dcm 矩阵是瞬时的、动态变化的姿态。