当前位置：首页 > news >正文

继续记录无人机SITL的大循环

news 2026/6/8 17:08:05

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
加起来....