From 7daab1ee62f3a9597ada9222867c9b57a5a07afb Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 26 Dec 2015 15:05:34 +1100 Subject: [PATCH] Rover: convert to Hz based task table --- APMrover2/APMrover2.cpp | 65 +++++++++++++++++++---------------------- APMrover2/Rover.cpp | 1 - APMrover2/Rover.h | 3 -- APMrover2/system.cpp | 2 +- APMrover2/test.cpp | 4 +-- 5 files changed, 33 insertions(+), 42 deletions(-) diff --git a/APMrover2/APMrover2.cpp b/APMrover2/APMrover2.cpp index 2734360dec..59fb392377 100644 --- a/APMrover2/APMrover2.cpp +++ b/APMrover2/APMrover2.cpp @@ -37,12 +37,7 @@ const AP_HAL::HAL& hal = AP_HAL::get_HAL(); Rover rover; -#define SCHED_TASK(func, _interval_ticks, _max_time_micros) {\ - .function = FUNCTOR_BIND(&rover, &Rover::func, void),\ - AP_SCHEDULER_NAME_INITIALIZER(func)\ - .interval_ticks = _interval_ticks,\ - .max_time_micros = _max_time_micros,\ -} +#define SCHED_TASK(func, _interval_ticks, _max_time_micros) SCHED_TASK_CLASS(Rover, &rover, func, _interval_ticks, _max_time_micros) /* scheduler table - all regular tasks should be listed here, along @@ -50,38 +45,38 @@ Rover rover; time they are expected to take (in microseconds) */ const AP_Scheduler::Task Rover::scheduler_tasks[] = { - SCHED_TASK(read_radio, 1, 1000), - SCHED_TASK(ahrs_update, 1, 6400), - SCHED_TASK(read_sonars, 1, 2000), - SCHED_TASK(update_current_mode, 1, 1500), - SCHED_TASK(set_servos, 1, 1500), - SCHED_TASK(update_GPS_50Hz, 1, 2500), - SCHED_TASK(update_GPS_10Hz, 5, 2500), - SCHED_TASK(update_alt, 5, 3400), - SCHED_TASK(navigate, 5, 1600), - SCHED_TASK(update_compass, 5, 2000), - SCHED_TASK(update_commands, 5, 1000), - SCHED_TASK(update_logging1, 5, 1000), - SCHED_TASK(update_logging2, 5, 1000), - SCHED_TASK(gcs_retry_deferred, 1, 1000), - SCHED_TASK(gcs_update, 1, 1700), - SCHED_TASK(gcs_data_stream_send, 1, 3000), + SCHED_TASK(read_radio, 50, 1000), + SCHED_TASK(ahrs_update, 50, 6400), + SCHED_TASK(read_sonars, 50, 2000), + SCHED_TASK(update_current_mode, 50, 1500), + SCHED_TASK(set_servos, 50, 1500), + SCHED_TASK(update_GPS_50Hz, 50, 2500), + SCHED_TASK(update_GPS_10Hz, 10, 2500), + SCHED_TASK(update_alt, 10, 3400), + SCHED_TASK(navigate, 10, 1600), + SCHED_TASK(update_compass, 10, 2000), + SCHED_TASK(update_commands, 10, 1000), + SCHED_TASK(update_logging1, 10, 1000), + SCHED_TASK(update_logging2, 10, 1000), + SCHED_TASK(gcs_retry_deferred, 50, 1000), + SCHED_TASK(gcs_update, 50, 1700), + SCHED_TASK(gcs_data_stream_send, 50, 3000), SCHED_TASK(read_control_switch, 7, 1000), - SCHED_TASK(read_trim_switch, 5, 1000), - SCHED_TASK(read_battery, 5, 1000), - SCHED_TASK(read_receiver_rssi, 5, 1000), - SCHED_TASK(update_events, 1, 1000), - SCHED_TASK(check_usb_mux, 15, 1000), - SCHED_TASK(mount_update, 1, 600), - SCHED_TASK(gcs_failsafe_check, 5, 600), - SCHED_TASK(compass_accumulate, 1, 900), - SCHED_TASK(update_notify, 1, 300), - SCHED_TASK(one_second_loop, 50, 3000), - SCHED_TASK(compass_cal_update, 1, 100), + SCHED_TASK(read_trim_switch, 10, 1000), + SCHED_TASK(read_battery, 10, 1000), + SCHED_TASK(read_receiver_rssi, 10, 1000), + SCHED_TASK(update_events, 50, 1000), + SCHED_TASK(check_usb_mux, 3, 1000), + SCHED_TASK(mount_update, 50, 600), + SCHED_TASK(gcs_failsafe_check, 10, 600), + SCHED_TASK(compass_accumulate, 50, 900), + SCHED_TASK(update_notify, 50, 300), + SCHED_TASK(one_second_loop, 1, 3000), + SCHED_TASK(compass_cal_update, 50, 100), #if FRSKY_TELEM_ENABLED == ENABLED - SCHED_TASK(frsky_telemetry_send, 10, 100), + SCHED_TASK(frsky_telemetry_send, 5, 100), #endif - SCHED_TASK(dataflash_periodic, 1, 300), + SCHED_TASK(dataflash_periodic, 50, 300), }; /* diff --git a/APMrover2/Rover.cpp b/APMrover2/Rover.cpp index 3a2250c2ff..b482097560 100644 --- a/APMrover2/Rover.cpp +++ b/APMrover2/Rover.cpp @@ -21,7 +21,6 @@ Rover::Rover(void) : param_loader(var_info), - ins_sample_rate(AP_InertialSensor::RATE_50HZ), channel_steer(NULL), channel_throttle(NULL), channel_learn(NULL), diff --git a/APMrover2/Rover.h b/APMrover2/Rover.h index a06642f1fe..3fe5d42026 100644 --- a/APMrover2/Rover.h +++ b/APMrover2/Rover.h @@ -112,9 +112,6 @@ private: // variables AP_Param param_loader; - // the rate we run the main loop at - const AP_InertialSensor::Sample_rate ins_sample_rate; - // all settable parameters Parameters g; diff --git a/APMrover2/system.cpp b/APMrover2/system.cpp index 6d1b7fa684..5b97be253a 100644 --- a/APMrover2/system.cpp +++ b/APMrover2/system.cpp @@ -396,7 +396,7 @@ void Rover::startup_INS_ground(void) ahrs.set_fly_forward(true); ahrs.set_vehicle_class(AHRS_VEHICLE_GROUND); - ins.init(ins_sample_rate); + ins.init(scheduler.get_loop_rate_hz()); ahrs.reset(); } diff --git a/APMrover2/test.cpp b/APMrover2/test.cpp index ef201102d3..0421f00f73 100644 --- a/APMrover2/test.cpp +++ b/APMrover2/test.cpp @@ -308,7 +308,7 @@ int8_t Rover::test_ins(uint8_t argc, const Menu::arg *argv) //cliSerial->printf("Calibrating."); ahrs.init(); ahrs.set_fly_forward(true); - ins.init(ins_sample_rate); + ins.init(scheduler.get_loop_rate_hz()); ahrs.reset(); print_hit_enter(); @@ -371,7 +371,7 @@ int8_t Rover::test_mag(uint8_t argc, const Menu::arg *argv) ahrs.set_compass(&compass); // we need the AHRS initialised for this test - ins.init(ins_sample_rate); + ins.init(scheduler.get_loop_rate_hz()); ahrs.reset(); int counter = 0;