diff --git a/ArduSub/ArduSub.cpp b/ArduSub/ArduSub.cpp index dc8e0bebc3..b75d9b49a7 100644 --- a/ArduSub/ArduSub.cpp +++ b/ArduSub/ArduSub.cpp @@ -36,21 +36,23 @@ const AP_Scheduler::Task Sub::scheduler_tasks[] = { SCHED_TASK(three_hz_loop, 3, 75), SCHED_TASK(update_turn_counter, 10, 50), SCHED_TASK(compass_accumulate, 100, 100), - SCHED_TASK(barometer_accumulate, 50, 90), - SCHED_TASK(update_notify, 50, 90), + SCHED_TASK_CLASS(AP_Baro, &sub.barometer, accumulate, 50, 90), + SCHED_TASK_CLASS(AP_Notify, &sub.notify, update, 50, 90), SCHED_TASK(one_hz_loop, 1, 100), 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), +#if MOUNT == ENABLED + SCHED_TASK_CLASS(AP_Mount, &sub.camera_mount, update, 50, 75), +#endif #if CAMERA == ENABLED - SCHED_TASK(update_trigger, 50, 75), + SCHED_TASK_CLASS(AP_Camera, &sub.camera, update_trigger, 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, &sub.DataFlash, periodic_tasks, 400, 300), + SCHED_TASK_CLASS(AP_InertialSensor, &sub.ins, periodic, 400, 50), SCHED_TASK(perf_update, 0.1, 75), #if RPM_ENABLED == ENABLED SCHED_TASK(rpm_update, 10, 200), @@ -59,7 +61,7 @@ const AP_Scheduler::Task Sub::scheduler_tasks[] = { SCHED_TASK(accel_cal_update, 10, 100), SCHED_TASK(terrain_update, 10, 100), #if GRIPPER_ENABLED == ENABLED - SCHED_TASK(gripper_update, 10, 75), + SCHED_TASK_CLASS(AP_Gripper, &g2.gripper, update, 10, 75), #endif #ifdef USERHOOK_FASTLOOP SCHED_TASK(userhook_FastLoop, 100, 75), @@ -209,31 +211,6 @@ void Sub::fifty_hz_loop() SRV_Channels::output_ch_all(); } -// updates the status of notify -// should be called at 50hz -void Sub::update_notify() -{ - notify.update(); -} - -// update_mount - update camera mount position -// should be run at 50hz -void Sub::update_mount() -{ -#if MOUNT == ENABLED - // update camera mount's position - camera_mount.update(); -#endif -} - -#if CAMERA == ENABLED -// update camera trigger -void Sub::update_trigger(void) -{ - camera.update_trigger(); -} -#endif - // update_batt_compass - read battery and compass // should be called at 10hz void Sub::update_batt_compass(void) @@ -308,16 +285,6 @@ void Sub::twentyfive_hz_logging() } } -void Sub::dataflash_periodic(void) -{ - DataFlash.periodic_tasks(); -} - -void Sub::ins_periodic() -{ - ins.periodic(); -} - // three_hz_loop - 3.3hz loop void Sub::three_hz_loop() { diff --git a/ArduSub/GCS_Mavlink.cpp b/ArduSub/GCS_Mavlink.cpp index ce4ce7d134..5b329bc91a 100644 --- a/ArduSub/GCS_Mavlink.cpp +++ b/ArduSub/GCS_Mavlink.cpp @@ -1241,7 +1241,7 @@ void GCS_MAVLINK_Sub::handleMessage(mavlink_message_t* msg) mavlink_msg_command_ack_send_buf(msg, chan, packet.command, result); AP_Notify::flags.firmware_update = 1; - sub.update_notify(); + sub.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/ArduSub/Sub.h b/ArduSub/Sub.h index 08a622c7ef..f6ea60307b 100644 --- a/ArduSub/Sub.h +++ b/ArduSub/Sub.h @@ -464,7 +464,6 @@ private: void perf_update(void); void fast_loop(); void fifty_hz_loop(); - void update_mount(); void update_batt_compass(void); void ten_hz_logging_loop(); void twentyfive_hz_logging(); @@ -622,7 +621,6 @@ private: void update_surface_and_bottom_detector(); void set_surfaced(bool at_surface); void set_bottomed(bool at_bottom); - void update_notify(); bool init_arm_motors(bool arming_from_gcs); void init_disarm_motors(); void motors_output(); @@ -640,7 +638,6 @@ private: void clear_input_hold(); void init_barometer(bool save); void read_barometer(void); - void barometer_accumulate(void); void init_rangefinder(void); void read_rangefinder(void); bool rangefinder_alt_ok(void); @@ -650,9 +647,6 @@ private: void update_optical_flow(void); #endif void read_battery(void); -#if GRIPPER_ENABLED == ENABLED - void gripper_update(); -#endif void terrain_update(); void terrain_logging(); bool terrain_use(); @@ -690,7 +684,6 @@ private: #if CAMERA == ENABLED void do_digicam_configure(const AP_Mission::Mission_Command& cmd); void do_digicam_control(const AP_Mission::Mission_Command& cmd); - void update_trigger(); #endif #if GRIPPER_ENABLED == ENABLED @@ -710,8 +703,6 @@ private: void auto_spline_start(const Location_Class& destination, bool stopped_at_start, AC_WPNav::spline_segment_end_type seg_end_type, const Location_Class& next_destination); void log_init(void); void init_capabilities(void); - void dataflash_periodic(void); - void ins_periodic(); void accel_cal_update(void); void failsafe_leak_check(); diff --git a/ArduSub/motors.cpp b/ArduSub/motors.cpp index 40ce3c25ef..284258160a 100644 --- a/ArduSub/motors.cpp +++ b/ArduSub/motors.cpp @@ -33,9 +33,9 @@ bool Sub::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 CONFIG_HAL_BOARD == HAL_BOARD_SITL diff --git a/ArduSub/sensors.cpp b/ArduSub/sensors.cpp index c1e0d0f08b..4a8870a656 100644 --- a/ArduSub/sensors.cpp +++ b/ArduSub/sensors.cpp @@ -20,12 +20,6 @@ void Sub::read_barometer(void) } } -// try to accumulate a baro reading -void Sub::barometer_accumulate(void) -{ - barometer.accumulate(); -} - void Sub::init_rangefinder(void) { #if RANGEFINDER_ENABLED == ENABLED @@ -206,11 +200,3 @@ void Sub::accel_cal_update() ahrs.set_trim(Vector3f(trim_roll, trim_pitch, 0)); } } - -#if GRIPPER_ENABLED == ENABLED -// gripper update -void Sub::gripper_update() -{ - g2.gripper.update(); -} -#endif