diff --git a/ArduCopter/ArduCopter.cpp b/ArduCopter/ArduCopter.cpp index 24c821a7e3..41d9e7e8c7 100644 --- a/ArduCopter/ArduCopter.cpp +++ b/ArduCopter/ArduCopter.cpp @@ -93,21 +93,23 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = { SCHED_TASK(read_aux_switches, 10, 50), SCHED_TASK(arm_motors_check, 10, 50), #if TOY_MODE_ENABLED == ENABLED - SCHED_TASK(toy_mode_update, 10, 50), + SCHED_TASK_CLASS(ToyMode, &copter.g2.toy_mode, update, 10, 50), #endif SCHED_TASK(auto_disarm_check, 10, 50), SCHED_TASK(auto_trim, 10, 75), SCHED_TASK(read_rangefinder, 20, 100), - SCHED_TASK(update_proximity, 100, 50), - SCHED_TASK(update_beacon, 400, 50), +#if PROXIMITY_ENABLED == ENABLED + SCHED_TASK_CLASS(AP_Proximity, &copter.g2.proximity, update, 100, 50), +#endif + SCHED_TASK_CLASS(AP_Beacon, &copter.g2.beacon, update, 400, 50), SCHED_TASK(update_visual_odom, 400, 50), SCHED_TASK(update_altitude, 10, 100), SCHED_TASK(run_nav_updates, 50, 100), SCHED_TASK(update_throttle_hover,100, 90), - SCHED_TASK(smart_rtl_save_position, 3, 100), + SCHED_TASK_CLASS(Copter::ModeSmartRTL, &copter.mode_smartrtl, save_position, 3, 100), SCHED_TASK(three_hz_loop, 3, 75), SCHED_TASK(compass_accumulate, 100, 100), - SCHED_TASK(barometer_accumulate, 50, 90), + SCHED_TASK_CLASS(AP_Baro, &copter.barometer, accumulate, 50, 90), #if PRECISION_LANDING == ENABLED SCHED_TASK(update_precland, 400, 50), #endif @@ -115,7 +117,7 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = { SCHED_TASK(check_dynamic_flight, 50, 75), #endif SCHED_TASK(fourhundred_hz_logging,400, 50), - SCHED_TASK(update_notify, 50, 90), + SCHED_TASK_CLASS(AP_Notify, &copter.notify, update, 50, 90), SCHED_TASK(one_hz_loop, 1, 100), SCHED_TASK(ekf_check, 10, 75), SCHED_TASK(gpsglitch_check, 10, 50), @@ -125,18 +127,22 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = { 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), +#if MOUNT == ENABLED + SCHED_TASK_CLASS(AP_Mount, &copter.camera_mount, update, 50, 75), +#endif +#if CAMERA == ENABLED + SCHED_TASK_CLASS(AP_Camera, &copter.camera, update, 50, 75), +#endif SCHED_TASK(ten_hz_logging_loop, 10, 350), SCHED_TASK(twentyfive_hz_logging, 25, 110), - SCHED_TASK(dataflash_periodic, 400, 300), - SCHED_TASK(ins_periodic, 400, 50), + SCHED_TASK_CLASS(DataFlash_Class, &copter.DataFlash, periodic_tasks, 400, 300), + SCHED_TASK_CLASS(AP_InertialSensor, &copter.ins, periodic, 400, 50), 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), - SCHED_TASK(temp_cal_update, 10, 100), + SCHED_TASK_CLASS(AP_TempCalibration, &copter.g2.temp_calibration, update, 10, 100), #if ADSB_ENABLED == ENABLED SCHED_TASK(avoidance_adsb_update, 10, 100), #endif @@ -145,7 +151,7 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = { #endif SCHED_TASK(terrain_update, 10, 100), #if GRIPPER_ENABLED == ENABLED - SCHED_TASK(gripper_update, 10, 75), + SCHED_TASK_CLASS(AP_Gripper, &copter.g2.gripper, update, 10, 75), #endif SCHED_TASK(winch_update, 10, 50), #ifdef USERHOOK_FASTLOOP @@ -163,8 +169,8 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = { #ifdef USERHOOK_SUPERSLOWLOOP SCHED_TASK(userhook_SuperSlowLoop, 1, 75), #endif - SCHED_TASK(button_update, 5, 100), - SCHED_TASK(stats_update, 1, 100), + SCHED_TASK_CLASS(AP_Button, &copter.g2.button, update, 5, 100), + SCHED_TASK_CLASS(AP_Stats, &copter.g2.stats, update, 1, 100), }; @@ -203,14 +209,6 @@ void Copter::perf_update(void) pmTest1 = 0; } -/* - update AP_Stats - */ -void Copter::stats_update(void) -{ - g2.stats.update(); -} - void Copter::loop() { // wait for an INS sample @@ -325,24 +323,6 @@ void Copter::throttle_loop() update_ground_effect_detector(); } -// update_mount - update camera mount position -// should be run at 50hz -void Copter::update_mount() -{ -#if MOUNT == ENABLED - // update camera mount's position - camera_mount.update(); -#endif -} - -// update camera trigger -void Copter::update_trigger(void) -{ -#if CAMERA == ENABLED - camera.update_trigger(); -#endif -} - // update_batt_compass - read battery and compass // should be called at 10hz void Copter::update_batt_compass(void) @@ -433,16 +413,6 @@ void Copter::twentyfive_hz_logging() #endif } -void Copter::dataflash_periodic(void) -{ - DataFlash.periodic_tasks(); -} - -void Copter::ins_periodic(void) -{ - ins.periodic(); -} - // three_hz_loop - 3.3hz loop void Copter::three_hz_loop() { @@ -540,11 +510,6 @@ void Copter::update_GPS(void) } } -void Copter::smart_rtl_save_position() -{ - mode_smartrtl.save_position(); -} - void Copter::init_simple_bearing() { // capture current cos_yaw and sin_yaw values diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 526201a55e..7e0be89936 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -615,22 +615,16 @@ private: // ArduCopter.cpp void perf_update(void); - void stats_update(); void fast_loop(); void rc_loop(); void throttle_loop(); - void update_mount(); - void update_trigger(void); void update_batt_compass(void); void fourhundred_hz_logging(); void ten_hz_logging_loop(); void twentyfive_hz_logging(); - void dataflash_periodic(void); - void ins_periodic(); void three_hz_loop(); void one_hz_loop(); void update_GPS(void); - void smart_rtl_save_position(); void init_simple_bearing(); void update_simple_mode(void); void update_super_simple_bearing(bool force_update); @@ -761,9 +755,6 @@ private: // landing_gear.cpp void landinggear_update(); - // leds.cpp - void update_notify(); - // Log.cpp void Log_Write_Optflow(); void Log_Write_Nav_Tuning(); @@ -828,9 +819,6 @@ private: void motors_output(); void lost_vehicle_check(); - // toy_mode.cpp - void toy_mode_update(void); - // navigation.cpp void run_nav_updates(void); int32_t home_bearing(); @@ -864,7 +852,6 @@ private: // sensors.cpp void init_barometer(bool full_calibration); void read_barometer(void); - void barometer_accumulate(void); void init_rangefinder(void); void read_rangefinder(void); bool rangefinder_alt_ok(); @@ -877,13 +864,10 @@ private: void read_receiver_rssi(void); void compass_cal_update(void); void accel_cal_update(void); - void gripper_update(); - void button_update(); void init_proximity(); void update_proximity(); void update_sensor_status_flags(void); void init_beacon(); - void update_beacon(); void init_visual_odom(); void update_visual_odom(); void winch_init(); @@ -989,8 +973,6 @@ private: Mode *mode_from_mode_num(const uint8_t mode); void exit_mode(Mode *&old_flightmode, Mode *&new_flightmode); - void temp_cal_update(void); - public: void mavlink_delay_cb(); // GCS_Mavlink.cpp void failsafe_check(); // failsafe.cpp diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index 6cfbfab371..a810c79ef5 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -1126,7 +1126,7 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg) case MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN: if (is_equal(packet.param1,1.0f) || is_equal(packet.param1,3.0f)) { AP_Notify::flags.firmware_update = 1; - copter.update_notify(); + copter.notify.update(); hal.scheduler->delay(200); // when packet.param1 == 3 we reboot to hold in bootloader hal.scheduler->reboot(is_equal(packet.param1,3.0f)); diff --git a/ArduCopter/esc_calibration.cpp b/ArduCopter/esc_calibration.cpp index 7696063232..5dd9767024 100644 --- a/ArduCopter/esc_calibration.cpp +++ b/ArduCopter/esc_calibration.cpp @@ -202,6 +202,6 @@ void Copter::esc_calibration_notify() uint32_t now = AP_HAL::millis(); if (now - esc_calibration_notify_update_ms > 20) { esc_calibration_notify_update_ms = now; - update_notify(); + notify.update(); } } diff --git a/ArduCopter/leds.cpp b/ArduCopter/leds.cpp index 38f65998ae..d452858e51 100644 --- a/ArduCopter/leds.cpp +++ b/ArduCopter/leds.cpp @@ -1,10 +1 @@ #include "Copter.h" - - -// updates the status of notify -// should be called at 50hz -void Copter::update_notify() -{ - notify.update(); -} - diff --git a/ArduCopter/motors.cpp b/ArduCopter/motors.cpp index 78e37060ea..c2adba9a33 100644 --- a/ArduCopter/motors.cpp +++ b/ArduCopter/motors.cpp @@ -161,9 +161,9 @@ bool Copter::init_arm_motors(bool arming_from_gcs) // notify that arming will occur (we do this early to give plenty of warning) AP_Notify::flags.armed = true; - // call update_notify a few times to ensure the message gets out + // call notify update a few times to ensure the message gets out for (uint8_t i=0; i<=10; i++) { - update_notify(); + notify.update(); } #if HIL_MODE != HIL_MODE_DISABLED || CONFIG_HAL_BOARD == HAL_BOARD_SITL diff --git a/ArduCopter/sensors.cpp b/ArduCopter/sensors.cpp index cfb1fe5323..4287fa04f3 100644 --- a/ArduCopter/sensors.cpp +++ b/ArduCopter/sensors.cpp @@ -24,12 +24,6 @@ void Copter::read_barometer(void) motors->set_air_density_ratio(barometer.get_air_density_ratio()); } -// try to accumulate a baro reading -void Copter::barometer_accumulate(void) -{ - barometer.accumulate(); -} - void Copter::init_rangefinder(void) { #if RANGEFINDER_ENABLED == ENABLED @@ -260,22 +254,6 @@ void Copter::accel_cal_update() #endif } -#if GRIPPER_ENABLED == ENABLED -// gripper update -void Copter::gripper_update() -{ - g2.gripper.update(); -} -#endif - -/* - update AP_Button - */ -void Copter::button_update(void) -{ - g2.button.update(); -} - // initialise proximity sensor void Copter::init_proximity(void) { @@ -285,14 +263,6 @@ void Copter::init_proximity(void) #endif } -// update proximity sensor -void Copter::update_proximity(void) -{ -#if PROXIMITY_ENABLED == ENABLED - g2.proximity.update(); -#endif -} - // update error mask of sensors and subsystems. The mask // uses the MAV_SYS_STATUS_* values from mavlink. If a bit is set // then it indicates that the sensor or subsystem is present but @@ -503,12 +473,6 @@ void Copter::init_beacon() g2.beacon.init(); } -// update beacons -void Copter::update_beacon() -{ - g2.beacon.update(); -} - // init visual odometry sensor void Copter::init_visual_odom() { @@ -553,8 +517,3 @@ void Copter::winch_update() g2.wheel_encoder.update(); g2.winch.update(); } - -void Copter::temp_cal_update(void) -{ - g2.temp_calibration.update(); -} diff --git a/ArduCopter/toy_mode.cpp b/ArduCopter/toy_mode.cpp index 47750f8dc6..36d536e910 100644 --- a/ArduCopter/toy_mode.cpp +++ b/ArduCopter/toy_mode.cpp @@ -967,16 +967,6 @@ void ToyMode::send_named_int(const char *name, int32_t value) mavlink_msg_named_value_int_send(MAVLINK_COMM_1, AP_HAL::millis(), name, value); } -#if TOY_MODE_ENABLED == ENABLED -/* - called from scheduler at 10Hz - */ -void Copter::toy_mode_update(void) -{ - g2.toy_mode.update(); -} -#endif - /* limit maximum thrust based on voltage */