From 8683616d8c87c2e54c28cd5fb8ee52500172c4fd Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 21 Apr 2016 14:51:29 +1000 Subject: [PATCH] Plane: redo scheduler table and improve perf logging The scheduler table was still setup for a worst case CPU of AVR2560. Adjust times for the stm32 and improve perf logging --- ArduPlane/ArduPlane.cpp | 106 +++++++++++++++++++------------------- ArduPlane/GCS_Mavlink.cpp | 4 +- ArduPlane/Log.cpp | 18 ++----- ArduPlane/Plane.h | 3 ++ ArduPlane/system.cpp | 3 +- 5 files changed, 66 insertions(+), 68 deletions(-) diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index 8d5b66b57d4f98..b1328674a3a36d 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -34,56 +34,56 @@ */ const AP_Scheduler::Task Plane::scheduler_tasks[] = { // Units: Hz us - SCHED_TASK(read_radio, 50, 700), - SCHED_TASK(check_short_failsafe, 50, 1000), - SCHED_TASK(ahrs_update, 400, 6400), - SCHED_TASK(update_speed_height, 50, 1600), - SCHED_TASK(update_flight_mode, 400, 1400), - SCHED_TASK(stabilize, 400, 3500), - SCHED_TASK(set_servos, 400, 1600), - SCHED_TASK(read_control_switch, 7, 1000), - SCHED_TASK(gcs_retry_deferred, 50, 1000), - SCHED_TASK(update_GPS_50Hz, 50, 2500), - SCHED_TASK(update_GPS_10Hz, 10, 2500), - SCHED_TASK(navigate, 10, 3000), - SCHED_TASK(update_compass, 10, 1200), - SCHED_TASK(read_airspeed, 10, 1200), - SCHED_TASK(update_alt, 10, 3400), - SCHED_TASK(adjust_altitude_target, 10, 1000), - SCHED_TASK(obc_fs_check, 10, 1000), - SCHED_TASK(gcs_update, 50, 1700), - SCHED_TASK(gcs_data_stream_send, 50, 3000), - SCHED_TASK(update_events, 50, 1500), - SCHED_TASK(check_usb_mux, 10, 300), - SCHED_TASK(read_battery, 10, 1000), - SCHED_TASK(compass_accumulate, 50, 1500), - SCHED_TASK(barometer_accumulate, 50, 900), + SCHED_TASK(ahrs_update, 400, 400), + SCHED_TASK(read_radio, 50, 100), + SCHED_TASK(check_short_failsafe, 50, 100), + SCHED_TASK(update_speed_height, 50, 200), + SCHED_TASK(update_flight_mode, 400, 100), + SCHED_TASK(stabilize, 400, 100), + SCHED_TASK(set_servos, 400, 100), + SCHED_TASK(read_control_switch, 7, 100), + SCHED_TASK(gcs_retry_deferred, 50, 500), + SCHED_TASK(update_GPS_50Hz, 50, 300), + SCHED_TASK(update_GPS_10Hz, 10, 400), + SCHED_TASK(navigate, 10, 150), + SCHED_TASK(update_compass, 10, 200), + SCHED_TASK(read_airspeed, 10, 100), + SCHED_TASK(update_alt, 10, 200), + SCHED_TASK(adjust_altitude_target, 10, 200), + SCHED_TASK(obc_fs_check, 10, 100), + SCHED_TASK(gcs_update, 50, 500), + SCHED_TASK(gcs_data_stream_send, 50, 500), + SCHED_TASK(update_events, 50, 150), + SCHED_TASK(check_usb_mux, 10, 100), + SCHED_TASK(read_battery, 10, 300), + SCHED_TASK(compass_accumulate, 50, 200), + SCHED_TASK(barometer_accumulate, 50, 150), SCHED_TASK(update_notify, 50, 300), - SCHED_TASK(read_rangefinder, 50, 500), - SCHED_TASK(compass_cal_update, 50, 100), - SCHED_TASK(accel_cal_update, 10, 100), + SCHED_TASK(read_rangefinder, 50, 100), + SCHED_TASK(compass_cal_update, 50, 50), + SCHED_TASK(accel_cal_update, 10, 50), #if OPTFLOW == ENABLED - SCHED_TASK(update_optical_flow, 50, 500), + SCHED_TASK(update_optical_flow, 50, 50), #endif - SCHED_TASK(one_second_loop, 1, 1000), - SCHED_TASK(check_long_failsafe, 3, 1000), - SCHED_TASK(read_receiver_rssi, 10, 1000), - SCHED_TASK(rpm_update, 10, 200), - SCHED_TASK(airspeed_ratio_update, 1, 1000), - SCHED_TASK(update_mount, 50, 1500), - SCHED_TASK(update_trigger, 50, 1500), - SCHED_TASK(log_perf_info, 0.1, 1000), - SCHED_TASK(compass_save, 0.016, 2500), - SCHED_TASK(update_logging1, 10, 1700), - SCHED_TASK(update_logging2, 10, 1700), - SCHED_TASK(parachute_check, 10, 500), + SCHED_TASK(one_second_loop, 1, 400), + SCHED_TASK(check_long_failsafe, 3, 400), + SCHED_TASK(read_receiver_rssi, 10, 100), + 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), + SCHED_TASK(log_perf_info, 0.2, 100), + SCHED_TASK(compass_save, 0.016, 200), + SCHED_TASK(update_logging1, 10, 300), + SCHED_TASK(update_logging2, 10, 300), + SCHED_TASK(parachute_check, 10, 200), #if FRSKY_TELEM_ENABLED == ENABLED SCHED_TASK(frsky_telemetry_send, 5, 100), #endif - SCHED_TASK(terrain_update, 10, 500), + SCHED_TASK(terrain_update, 10, 200), SCHED_TASK(update_is_flying_5Hz, 5, 100), - SCHED_TASK(dataflash_periodic, 50, 300), - SCHED_TASK(adsb_update, 1, 500), + SCHED_TASK(dataflash_periodic, 50, 400), + SCHED_TASK(adsb_update, 1, 400), }; void Plane::setup() @@ -107,6 +107,8 @@ void Plane::setup() void Plane::loop() { + uint32_t loop_us = 1000000UL / scheduler.get_loop_rate_hz(); + // wait for an INS sample ins.wait_for_sample(); @@ -115,6 +117,10 @@ void Plane::loop() perf.delta_us_fast_loop = timer - perf.fast_loopTimer_us; G_Dt = perf.delta_us_fast_loop * 1.0e-6f; + if (perf.delta_us_fast_loop > loop_us + 500) { + perf.num_long++; + } + if (perf.delta_us_fast_loop > perf.G_Dt_max && perf.fast_loopTimer_us != 0) { perf.G_Dt_max = perf.delta_us_fast_loop; } @@ -134,11 +140,7 @@ void Plane::loop() // in multiples of the main loop tick. So if they don't run on // the first call to the scheduler they won't run on a later // call until scheduler.tick() is called again - uint32_t remaining = (timer + 20000) - micros(); - if (remaining > 19500) { - remaining = 19500; - } - scheduler.run(remaining); + scheduler.run(loop_us); } // update AHRS system @@ -338,8 +340,10 @@ void Plane::one_second_loop() void Plane::log_perf_info() { if (scheduler.debug() != 0) { - gcs_send_text_fmt(MAV_SEVERITY_INFO, "G_Dt_max=%lu G_Dt_min=%lu\n", - (unsigned long)perf.G_Dt_max, + gcs_send_text_fmt(MAV_SEVERITY_INFO, "PERF: %u/%u %lu %lu\n", + (unsigned)perf.num_long, + (unsigned)perf.mainLoop_count, + (unsigned long)perf.G_Dt_max, (unsigned long)perf.G_Dt_min); } @@ -347,8 +351,6 @@ void Plane::log_perf_info() Log_Write_Performance(); } - perf.G_Dt_max = 0; - perf.G_Dt_min = 0; resetPerfData(); } diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 3e65f84009820c..d979c08ac23ab1 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -622,10 +622,10 @@ bool GCS_MAVLINK::try_send_message(enum ap_message id) return false; } - // if we don't have at least 1ms remaining before the main loop + // if we don't have at least 0.2ms remaining before the main loop // wants to fire then don't send a mavlink message. We want to // prioritise the main flight control loop over communications - if (!plane.in_mavlink_delay && plane.scheduler.time_available_usec() < 1200) { + if (!plane.in_mavlink_delay && plane.scheduler.time_available_usec() < 200) { plane.gcs_out_of_time = true; return false; } diff --git a/ArduPlane/Log.cpp b/ArduPlane/Log.cpp index 542259d5362687..e53b403e4b7cc5 100644 --- a/ArduPlane/Log.cpp +++ b/ArduPlane/Log.cpp @@ -194,14 +194,10 @@ void Plane::Log_Write_Attitude(void) struct PACKED log_Performance { LOG_PACKET_HEADER; uint64_t time_us; - uint32_t loop_time; + uint16_t num_long; uint16_t main_loop_count; uint32_t g_dt_max; - int16_t gyro_drift_x; - int16_t gyro_drift_y; - int16_t gyro_drift_z; - uint8_t i2c_lockup_count; - uint16_t ins_error_count; + uint32_t g_dt_min; }; // Write a performance monitoring packet. Total length : 19 bytes @@ -210,14 +206,10 @@ void Plane::Log_Write_Performance() struct log_Performance pkt = { LOG_PACKET_HEADER_INIT(LOG_PERFORMANCE_MSG), time_us : AP_HAL::micros64(), - loop_time : millis() - perf.start_ms, + num_long : perf.num_long, main_loop_count : perf.mainLoop_count, g_dt_max : perf.G_Dt_max, - gyro_drift_x : (int16_t)(ahrs.get_gyro_drift().x * 1000), - gyro_drift_y : (int16_t)(ahrs.get_gyro_drift().y * 1000), - gyro_drift_z : (int16_t)(ahrs.get_gyro_drift().z * 1000), - i2c_lockup_count: hal.i2c->lockup_count(), - ins_error_count : ins.error_count() + g_dt_min : perf.G_Dt_min }; DataFlash.WriteBlock(&pkt, sizeof(pkt)); } @@ -485,7 +477,7 @@ void Plane::Log_Write_Home_And_Origin() const struct LogStructure Plane::log_structure[] = { LOG_COMMON_STRUCTURES, { LOG_PERFORMANCE_MSG, sizeof(log_Performance), - "PM", "QIHIhhhBH", "TimeUS,LTime,MLC,gDt,GDx,GDy,GDz,I2CErr,INSErr" }, + "PM", "QHHII", "TimeUS,NLon,NLoop,MaxT,MinT" }, { LOG_STARTUP_MSG, sizeof(log_Startup), "STRT", "QBH", "TimeUS,SType,CTot" }, { LOG_CTUN_MSG, sizeof(log_Control_Tuning), diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 1ff165779feb4d..a52a73494429f0 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -703,6 +703,9 @@ class Plane : public AP_HAL::HAL::Callbacks { // Counter of main loop executions. Used for performance monitoring and failsafe processing uint16_t mainLoop_count; + + // number of long loops + uint16_t num_long; } perf; // Camera/Antenna mount tracking and stabilisation stuff diff --git a/ArduPlane/system.cpp b/ArduPlane/system.cpp index 45c65d4ac41005..9c7d9d1424ffd5 100644 --- a/ArduPlane/system.cpp +++ b/ArduPlane/system.cpp @@ -635,7 +635,8 @@ void Plane::resetPerfData(void) perf.mainLoop_count = 0; perf.G_Dt_max = 0; perf.G_Dt_min = 0; - perf.start_ms = millis(); + perf.num_long = 0; + perf.start_ms = millis(); }