diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index f8d7233967..3173740c01 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -54,7 +54,7 @@ const AP_Scheduler::Task Plane::scheduler_tasks[] = { SCHED_TASK(update_events, 50, 150), SCHED_TASK_CLASS(AP_BattMonitor, &plane.battery, read, 10, 300), SCHED_TASK(compass_accumulate, 50, 200), - SCHED_TASK(barometer_accumulate, 50, 150), + SCHED_TASK_CLASS(AP_Baro, &plane.barometer, accumulate, 50, 150), SCHED_TASK(update_notify, 50, 300), SCHED_TASK(read_rangefinder, 50, 100), SCHED_TASK(ice_update, 10, 100), @@ -67,8 +67,12 @@ const AP_Scheduler::Task Plane::scheduler_tasks[] = { SCHED_TASK(check_long_failsafe, 3, 400), SCHED_TASK(rpm_update, 10, 100), SCHED_TASK(airspeed_ratio_update, 1, 100), - SCHED_TASK(update_mount, 50, 100), - SCHED_TASK(update_trigger, 50, 100), +#if MOUNT == ENABLED + SCHED_TASK_CLASS(AP_Mount, &plane.camera_mount, update, 50, 100), +#endif // MOUNT == ENABLED +#if CAMERA == ENABLED + SCHED_TASK_CLASS(AP_Camera, &plane.camera, update_trigger, 50, 100), +#endif // CAMERA == ENABLED SCHED_TASK_CLASS(AP_Scheduler, &plane.scheduler, update_logging, 0.2, 100), SCHED_TASK(compass_save, 0.1, 200), SCHED_TASK(Log_Write_Fast, 25, 300), @@ -76,16 +80,18 @@ const AP_Scheduler::Task Plane::scheduler_tasks[] = { SCHED_TASK(update_logging2, 25, 300), SCHED_TASK(update_soaring, 50, 400), SCHED_TASK(parachute_check, 10, 200), - SCHED_TASK(terrain_update, 10, 200), +#if AP_TERRAIN_AVAILABLE + SCHED_TASK_CLASS(AP_Terrain, &plane.terrain, update, 10, 200), +#endif // AP_TERRAIN_AVAILABLE SCHED_TASK(update_is_flying_5Hz, 5, 100), #if LOGGING_ENABLED == ENABLED - SCHED_TASK(dataflash_periodic, 50, 400), + SCHED_TASK_CLASS(DataFlash_Class, &plane.DataFlash, periodic_tasks, 50, 400), #endif - SCHED_TASK(ins_periodic, 50, 50), + SCHED_TASK_CLASS(AP_InertialSensor, &plane.ins, periodic, 50, 50), SCHED_TASK(avoidance_adsb_update, 10, 100), SCHED_TASK(button_update, 5, 100), #if STATS_ENABLED == ENABLED - SCHED_TASK(stats_update, 1, 100), + SCHED_TASK_CLASS(AP_Stats, &plane.g2.stats, update, 1, 100), #endif #if GRIPPER_ENABLED == ENABLED SCHED_TASK_CLASS(AP_Gripper, &plane.g2.gripper, update, 10, 75), @@ -94,23 +100,11 @@ const AP_Scheduler::Task Plane::scheduler_tasks[] = { constexpr int8_t Plane::_failsafe_priorities[5]; -#if STATS_ENABLED == ENABLED -/* - update AP_Stats - */ -void Plane::stats_update(void) -{ - g2.stats.update(); -} -#endif - void Plane::setup() { // load the default values of variables listed in var_info[] AP_Param::setup_sketch_defaults(); - AP_Notify::flags.failsafe_battery = false; - rssi.init(); init_ardupilot(); @@ -183,26 +177,6 @@ void Plane::update_speed_height(void) } -/* - update camera mount - */ -void Plane::update_mount(void) -{ -#if MOUNT == ENABLED - camera_mount.update(); -#endif -} - -/* - update camera trigger - */ -void Plane::update_trigger(void) -{ -#if CAMERA == ENABLED - camera.update_trigger(); -#endif -} - /* read and update compass */ @@ -226,14 +200,6 @@ void Plane::compass_accumulate(void) } } -/* - try to accumulate a baro reading - */ -void Plane::barometer_accumulate(void) -{ - barometer.accumulate(); -} - /* do 10Hz logging */ @@ -355,24 +321,6 @@ void Plane::compass_save() } } -void Plane::terrain_update(void) -{ -#if AP_TERRAIN_AVAILABLE - terrain.update(); -#endif -} - - -void Plane::ins_periodic(void) -{ - ins.periodic(); -} - -void Plane::dataflash_periodic(void) -{ - DataFlash.periodic_tasks(); -} - /* once a second update the airspeed calibration ratio */ diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 6b689335cd..d364c5fa79 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -912,7 +912,6 @@ private: void read_airspeed(void); void rpm_update(void); void button_update(void); - void stats_update(); void ice_update(void); void init_ardupilot(); void startup_ground(void); @@ -943,16 +942,12 @@ private: void afs_fs_check(void); void compass_accumulate(void); void compass_cal_update(); - void barometer_accumulate(void); void update_optical_flow(void); void one_second_loop(void); void airspeed_ratio_update(void); - void update_mount(void); - void update_trigger(void); void compass_save(void); void update_logging1(void); void update_logging2(void); - void terrain_update(void); void avoidance_adsb_update(void); void update_flight_mode(void); void stabilize(); @@ -1025,8 +1020,6 @@ private: void notify_flight_mode(enum FlightMode mode); void log_init(); void init_capabilities(void); - void ins_periodic(); - void dataflash_periodic(void); void parachute_check(); #if PARACHUTE == ENABLED void do_parachute(const AP_Mission::Mission_Command& cmd);