diff --git a/APMrover2/APMrover2.cpp b/APMrover2/APMrover2.cpp index efb8eb6262..c798eaa290 100644 --- a/APMrover2/APMrover2.cpp +++ b/APMrover2/APMrover2.cpp @@ -52,8 +52,8 @@ const AP_Scheduler::Task Rover::scheduler_tasks[] = { SCHED_TASK(update_GPS_50Hz, 50, 2500), SCHED_TASK(update_GPS_10Hz, 10, 2500), SCHED_TASK(update_alt, 10, 3400), - SCHED_TASK(update_beacon, 50, 50), - SCHED_TASK(update_proximity, 50, 50), + SCHED_TASK_CLASS(AP_Beacon, &rover.g2.beacon, update, 50, 50), + SCHED_TASK_CLASS(AP_Proximity, &rover.g2.proximity, update, 50, 50), SCHED_TASK(update_visual_odom, 50, 50), SCHED_TASK(update_wheel_encoder, 20, 50), SCHED_TASK(update_compass, 10, 2000), @@ -65,23 +65,27 @@ const AP_Scheduler::Task Rover::scheduler_tasks[] = { SCHED_TASK(gcs_data_stream_send, 50, 3000), SCHED_TASK(read_control_switch, 7, 1000), SCHED_TASK(read_aux_switch, 10, 100), - SCHED_TASK(read_battery, 10, 1000), + SCHED_TASK_CLASS(AP_BattMonitor, &rover.battery, read, 10, 1000), SCHED_TASK(read_receiver_rssi, 10, 1000), - SCHED_TASK(update_events, 50, 1000), + SCHED_TASK_CLASS(AP_ServoRelayEvents, &rover.ServoRelayEvents, update_events, 50, 1000), SCHED_TASK(check_usb_mux, 3, 1000), - SCHED_TASK(mount_update, 50, 600), - SCHED_TASK(update_trigger, 50, 600), +#if MOUNT == ENABLED + SCHED_TASK_CLASS(AP_Mount, &rover.camera_mount, update, 50, 600), +#endif +#if CAMERA == ENABLED + SCHED_TASK_CLASS(AP_Camera, &rover.camera, update_trigger, 50, 600), +#endif SCHED_TASK(gcs_failsafe_check, 10, 600), SCHED_TASK(fence_check, 10, 100), SCHED_TASK(compass_accumulate, 50, 900), - SCHED_TASK(smart_rtl_update, 3, 100), - SCHED_TASK(update_notify, 50, 300), + SCHED_TASK_CLASS(ModeSmartRTL, &rover.mode_smartrtl, save_position, 3, 100), + SCHED_TASK_CLASS(AP_Notify, &rover.notify, update, 50, 300), SCHED_TASK(one_second_loop, 1, 3000), SCHED_TASK(compass_cal_update, 50, 100), SCHED_TASK(accel_cal_update, 10, 100), - SCHED_TASK(dataflash_periodic, 50, 300), - SCHED_TASK(ins_periodic, 50, 50), - SCHED_TASK(button_update, 5, 100), + SCHED_TASK_CLASS(DataFlash_Class, &rover.DataFlash, periodic_tasks, 50, 300), + SCHED_TASK_CLASS(AP_InertialSensor, &rover.ins, periodic, 50, 50), + SCHED_TASK_CLASS(AP_Button, &rover.button, update, 5, 100), SCHED_TASK(stats_update, 1, 100), SCHED_TASK(crash_check, 10, 1000), SCHED_TASK(cruise_learn_update, 50, 50), @@ -193,26 +197,6 @@ void Rover::ahrs_update() } } -/* - update camera mount - 50Hz - */ -void Rover::mount_update(void) -{ -#if MOUNT == ENABLED - camera_mount.update(); -#endif -} - -/* - update camera trigger - 50Hz - */ -void Rover::update_trigger(void) -{ -#if CAMERA == ENABLED - camera.update_trigger(); -#endif -} - void Rover::update_alt() { barometer.update(); @@ -355,16 +339,6 @@ void Rover::one_second_loop(void) update_sensor_status_flags(); } -void Rover::dataflash_periodic(void) -{ - DataFlash.periodic_tasks(); -} - -void Rover::ins_periodic() -{ - ins.periodic(); -} - void Rover::update_GPS_50Hz(void) { static uint32_t last_gps_reading[GPS_MAX_INSTANCES]; diff --git a/APMrover2/Rover.h b/APMrover2/Rover.h index 977f130208..dcae72208a 100644 --- a/APMrover2/Rover.h +++ b/APMrover2/Rover.h @@ -403,8 +403,6 @@ private: // APMrover2.cpp void stats_update(); void ahrs_update(); - void mount_update(void); - void update_trigger(void); void update_alt(); void gcs_failsafe_check(void); void update_compass(void); @@ -474,9 +472,6 @@ private: void cruise_learn_update(); void cruise_learn_complete(); - // events.cpp - void update_events(void); - // failsafe.cpp void failsafe_trigger(uint8_t failsafe_type, bool on); #if ADVANCED_FAILSAFE == ENABLED @@ -544,18 +539,14 @@ private: void init_barometer(bool full_calibration); void init_rangefinder(void); void init_beacon(); - void update_beacon(); void init_visual_odom(); void update_visual_odom(); void update_wheel_encoder(); - void read_battery(void); void read_receiver_rssi(void); void compass_cal_update(void); void accel_cal_update(void); void read_rangefinders(void); - void button_update(void); void init_proximity(); - void update_proximity(); void update_sensor_status_flags(void); // Steering.cpp @@ -569,7 +560,6 @@ private: bool set_mode(Mode &new_mode, mode_reason_t reason); bool mavlink_set_mode(uint8_t mode); void startup_INS_ground(void); - void update_notify(); void resetPerfData(void); void check_usb_mux(void); void print_mode(AP_HAL::BetterStream *port, uint8_t mode); @@ -579,15 +569,12 @@ private: void change_arm_state(void); bool arm_motors(AP_Arming::ArmingMethod method); bool disarm_motors(void); - void smart_rtl_update(); bool is_boat() const; public: void mavlink_delay_cb(); void failsafe_check(); - void dataflash_periodic(void); - void ins_periodic(); void update_soft_armed(); // Motor test void motor_test_output(); diff --git a/APMrover2/events.cpp b/APMrover2/events.cpp deleted file mode 100644 index 14252811cb..0000000000 --- a/APMrover2/events.cpp +++ /dev/null @@ -1,6 +0,0 @@ -#include "Rover.h" - -void Rover::update_events(void) -{ - ServoRelayEvents.update_events(); -} diff --git a/APMrover2/sensors.cpp b/APMrover2/sensors.cpp index 278a6bab79..3d9c9080fa 100644 --- a/APMrover2/sensors.cpp +++ b/APMrover2/sensors.cpp @@ -61,12 +61,6 @@ void Rover::init_beacon() g2.beacon.init(); } -// update beacons -void Rover::update_beacon() -{ - g2.beacon.update(); -} - // init visual odometry sensor void Rover::init_visual_odom() { @@ -162,13 +156,6 @@ void Rover::update_wheel_encoder() wheel_encoder_last_ekf_update_ms = now; } -// read_battery - reads battery voltage and current and invokes failsafe -// should be called at 10hz -void Rover::read_battery(void) -{ - battery.read(); -} - // read the receiver RSSI as an 8 bit number for MAVLink // RC_CHANNELS_SCALED message void Rover::read_receiver_rssi(void) @@ -267,14 +254,6 @@ void Rover::read_rangefinders(void) } } -/* - update AP_Button - */ -void Rover::button_update(void) -{ - button.update(); -} - // initialise proximity sensor void Rover::init_proximity(void) { @@ -282,12 +261,6 @@ void Rover::init_proximity(void) g2.proximity.set_rangefinder(&rangefinder); } -// update proximity sensor -void Rover::update_proximity(void) -{ - g2.proximity.update(); -} - // 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 diff --git a/APMrover2/system.cpp b/APMrover2/system.cpp index f1403330b6..049d6618da 100644 --- a/APMrover2/system.cpp +++ b/APMrover2/system.cpp @@ -262,13 +262,6 @@ void Rover::startup_INS_ground(void) ahrs.reset(); } -// updates the notify state -// should be called at 50hz -void Rover::update_notify() -{ - notify.update(); -} - void Rover::resetPerfData(void) { mainLoop_count = 0; G_Dt_max = 0; @@ -365,12 +358,6 @@ bool Rover::disarm_motors(void) return true; } -// save current position for use by the smart_rtl mode -void Rover::smart_rtl_update() -{ - mode_smartrtl.save_position(); -} - // returns true if vehicle is a boat // this affects whether the vehicle tries to maintain position after reaching waypoints bool Rover::is_boat() const