zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

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
master
Andrew Tridgell 9 years ago
parent
ced4cce358
commit
8683616d8c
5 changed files with 66 additions and 68 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 106
      ArduPlane/ArduPlane.cpp
  2. 4
      ArduPlane/GCS_Mavlink.cpp
  3. 18
      ArduPlane/Log.cpp
  4. 3
      ArduPlane/Plane.h
  5. 3
      ArduPlane/system.cpp

106
ArduPlane/ArduPlane.cpp
Unescape View File

@ -34,56 +34,56 @@
*/ */
const AP_Scheduler::Task Plane::scheduler_tasks[] = { const AP_Scheduler::Task Plane::scheduler_tasks[] = {
// Units: Hz us // Units: Hz us
SCHED_TASK(read_radio, 50, 700), SCHED_TASK(ahrs_update, 400, 400),
SCHED_TASK(check_short_failsafe, 50, 1000), SCHED_TASK(read_radio, 50, 100),
SCHED_TASK(ahrs_update, 400, 6400), SCHED_TASK(check_short_failsafe, 50, 100),
SCHED_TASK(update_speed_height, 50, 1600), SCHED_TASK(update_speed_height, 50, 200),
SCHED_TASK(update_flight_mode, 400, 1400), SCHED_TASK(update_flight_mode, 400, 100),
SCHED_TASK(stabilize, 400, 3500), SCHED_TASK(stabilize, 400, 100),
SCHED_TASK(set_servos, 400, 1600), SCHED_TASK(set_servos, 400, 100),
SCHED_TASK(read_control_switch, 7, 1000), SCHED_TASK(read_control_switch, 7, 100),
SCHED_TASK(gcs_retry_deferred, 50, 1000), SCHED_TASK(gcs_retry_deferred, 50, 500),
SCHED_TASK(update_GPS_50Hz, 50, 2500), SCHED_TASK(update_GPS_50Hz, 50, 300),
SCHED_TASK(update_GPS_10Hz, 10, 2500), SCHED_TASK(update_GPS_10Hz, 10, 400),
SCHED_TASK(navigate, 10, 3000), SCHED_TASK(navigate, 10, 150),
SCHED_TASK(update_compass, 10, 1200), SCHED_TASK(update_compass, 10, 200),
SCHED_TASK(read_airspeed, 10, 1200), SCHED_TASK(read_airspeed, 10, 100),
SCHED_TASK(update_alt, 10, 3400), SCHED_TASK(update_alt, 10, 200),
SCHED_TASK(adjust_altitude_target, 10, 1000), SCHED_TASK(adjust_altitude_target, 10, 200),
SCHED_TASK(obc_fs_check, 10, 1000), SCHED_TASK(obc_fs_check, 10, 100),
SCHED_TASK(gcs_update, 50, 1700), SCHED_TASK(gcs_update, 50, 500),
SCHED_TASK(gcs_data_stream_send, 50, 3000), SCHED_TASK(gcs_data_stream_send, 50, 500),
SCHED_TASK(update_events, 50, 1500), SCHED_TASK(update_events, 50, 150),
SCHED_TASK(check_usb_mux, 10, 300), SCHED_TASK(check_usb_mux, 10, 100),
SCHED_TASK(read_battery, 10, 1000), SCHED_TASK(read_battery, 10, 300),
SCHED_TASK(compass_accumulate, 50, 1500), SCHED_TASK(compass_accumulate, 50, 200),
SCHED_TASK(barometer_accumulate, 50, 900), SCHED_TASK(barometer_accumulate, 50, 150),
SCHED_TASK(update_notify, 50, 300), SCHED_TASK(update_notify, 50, 300),
SCHED_TASK(read_rangefinder, 50, 500), SCHED_TASK(read_rangefinder, 50, 100),
SCHED_TASK(compass_cal_update, 50, 100), SCHED_TASK(compass_cal_update, 50, 50),
SCHED_TASK(accel_cal_update, 10, 100), SCHED_TASK(accel_cal_update, 10, 50),
#if OPTFLOW == ENABLED #if OPTFLOW == ENABLED
SCHED_TASK(update_optical_flow, 50, 500), SCHED_TASK(update_optical_flow, 50, 50),
#endif #endif
SCHED_TASK(one_second_loop, 1, 1000), SCHED_TASK(one_second_loop, 1, 400),
SCHED_TASK(check_long_failsafe, 3, 1000), SCHED_TASK(check_long_failsafe, 3, 400),
SCHED_TASK(read_receiver_rssi, 10, 1000), SCHED_TASK(read_receiver_rssi, 10, 100),
SCHED_TASK(rpm_update, 10, 200), SCHED_TASK(rpm_update, 10, 100),
SCHED_TASK(airspeed_ratio_update, 1, 1000), SCHED_TASK(airspeed_ratio_update, 1, 100),
SCHED_TASK(update_mount, 50, 1500), SCHED_TASK(update_mount, 50, 100),
SCHED_TASK(update_trigger, 50, 1500), SCHED_TASK(update_trigger, 50, 100),
SCHED_TASK(log_perf_info, 0.1, 1000), SCHED_TASK(log_perf_info, 0.2, 100),
SCHED_TASK(compass_save, 0.016, 2500), SCHED_TASK(compass_save, 0.016, 200),
SCHED_TASK(update_logging1, 10, 1700), SCHED_TASK(update_logging1, 10, 300),
SCHED_TASK(update_logging2, 10, 1700), SCHED_TASK(update_logging2, 10, 300),
SCHED_TASK(parachute_check, 10, 500), SCHED_TASK(parachute_check, 10, 200),
#if FRSKY_TELEM_ENABLED == ENABLED #if FRSKY_TELEM_ENABLED == ENABLED
SCHED_TASK(frsky_telemetry_send, 5, 100), SCHED_TASK(frsky_telemetry_send, 5, 100),
#endif #endif
SCHED_TASK(terrain_update, 10, 500), SCHED_TASK(terrain_update, 10, 200),
SCHED_TASK(update_is_flying_5Hz, 5, 100), SCHED_TASK(update_is_flying_5Hz, 5, 100),
SCHED_TASK(dataflash_periodic, 50, 300), SCHED_TASK(dataflash_periodic, 50, 400),
SCHED_TASK(adsb_update, 1, 500), SCHED_TASK(adsb_update, 1, 400),
}; };
void Plane::setup() void Plane::setup()
@ -107,6 +107,8 @@ void Plane::setup()
void Plane::loop() void Plane::loop()
{ {
uint32_t loop_us = 1000000UL / scheduler.get_loop_rate_hz();
// wait for an INS sample // wait for an INS sample
ins.wait_for_sample(); ins.wait_for_sample();
@ -115,6 +117,10 @@ void Plane::loop()
perf.delta_us_fast_loop = timer - perf.fast_loopTimer_us; perf.delta_us_fast_loop = timer - perf.fast_loopTimer_us;
G_Dt = perf.delta_us_fast_loop * 1.0e-6f; 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) { if (perf.delta_us_fast_loop > perf.G_Dt_max && perf.fast_loopTimer_us != 0) {
perf.G_Dt_max = perf.delta_us_fast_loop; 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 // 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 // the first call to the scheduler they won't run on a later
// call until scheduler.tick() is called again // call until scheduler.tick() is called again
uint32_t remaining = (timer + 20000) - micros(); scheduler.run(loop_us);
if (remaining > 19500) {
remaining = 19500;
}
scheduler.run(remaining);
} }
// update AHRS system // update AHRS system
@ -338,8 +340,10 @@ void Plane::one_second_loop()
void Plane::log_perf_info() void Plane::log_perf_info()
{ {
if (scheduler.debug() != 0) { if (scheduler.debug() != 0) {
gcs_send_text_fmt(MAV_SEVERITY_INFO, "G_Dt_max=%lu G_Dt_min=%lu\n", gcs_send_text_fmt(MAV_SEVERITY_INFO, "PERF: %u/%u %lu %lu\n",
(unsigned long)perf.G_Dt_max, (unsigned)perf.num_long,
(unsigned)perf.mainLoop_count,
(unsigned long)perf.G_Dt_max,
(unsigned long)perf.G_Dt_min); (unsigned long)perf.G_Dt_min);
} }
@ -347,8 +351,6 @@ void Plane::log_perf_info()
Log_Write_Performance(); Log_Write_Performance();
} }
perf.G_Dt_max = 0;
perf.G_Dt_min = 0;
resetPerfData(); resetPerfData();
} }

4
ArduPlane/GCS_Mavlink.cpp
Unescape View File

@ -622,10 +622,10 @@ bool GCS_MAVLINK::try_send_message(enum ap_message id)
return false; 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 // wants to fire then don't send a mavlink message. We want to
// prioritise the main flight control loop over communications // 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; plane.gcs_out_of_time = true;
return false; return false;
} }

18
ArduPlane/Log.cpp
Unescape View File

@ -194,14 +194,10 @@ void Plane::Log_Write_Attitude(void)
struct PACKED log_Performance { struct PACKED log_Performance {
LOG_PACKET_HEADER; LOG_PACKET_HEADER;
uint64_t time_us; uint64_t time_us;
uint32_t loop_time; uint16_t num_long;
uint16_t main_loop_count; uint16_t main_loop_count;
uint32_t g_dt_max; uint32_t g_dt_max;
int16_t gyro_drift_x; uint32_t g_dt_min;
int16_t gyro_drift_y;
int16_t gyro_drift_z;
uint8_t i2c_lockup_count;
uint16_t ins_error_count;
}; };
// Write a performance monitoring packet. Total length : 19 bytes // Write a performance monitoring packet. Total length : 19 bytes
@ -210,14 +206,10 @@ void Plane::Log_Write_Performance()
struct log_Performance pkt = { struct log_Performance pkt = {
LOG_PACKET_HEADER_INIT(LOG_PERFORMANCE_MSG), LOG_PACKET_HEADER_INIT(LOG_PERFORMANCE_MSG),
time_us : AP_HAL::micros64(), time_us : AP_HAL::micros64(),
loop_time : millis() - perf.start_ms, num_long : perf.num_long,
main_loop_count : perf.mainLoop_count, main_loop_count : perf.mainLoop_count,
g_dt_max : perf.G_Dt_max, g_dt_max : perf.G_Dt_max,
gyro_drift_x : (int16_t)(ahrs.get_gyro_drift().x * 1000), g_dt_min : perf.G_Dt_min
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()
}; };
DataFlash.WriteBlock(&pkt, sizeof(pkt)); DataFlash.WriteBlock(&pkt, sizeof(pkt));
} }
@ -485,7 +477,7 @@ void Plane::Log_Write_Home_And_Origin()
const struct LogStructure Plane::log_structure[] = { const struct LogStructure Plane::log_structure[] = {
LOG_COMMON_STRUCTURES, LOG_COMMON_STRUCTURES,
{ LOG_PERFORMANCE_MSG, sizeof(log_Performance), { 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), { LOG_STARTUP_MSG, sizeof(log_Startup),
"STRT", "QBH", "TimeUS,SType,CTot" }, "STRT", "QBH", "TimeUS,SType,CTot" },
{ LOG_CTUN_MSG, sizeof(log_Control_Tuning), { LOG_CTUN_MSG, sizeof(log_Control_Tuning),

3
ArduPlane/Plane.h
Unescape View File

@ -703,6 +703,9 @@ private:
// Counter of main loop executions. Used for performance monitoring and failsafe processing // Counter of main loop executions. Used for performance monitoring and failsafe processing
uint16_t mainLoop_count; uint16_t mainLoop_count;
// number of long loops
uint16_t num_long;
} perf; } perf;
// Camera/Antenna mount tracking and stabilisation stuff // Camera/Antenna mount tracking and stabilisation stuff

3
ArduPlane/system.cpp
Unescape View File

@ -635,7 +635,8 @@ void Plane::resetPerfData(void)
perf.mainLoop_count = 0; perf.mainLoop_count = 0;
perf.G_Dt_max = 0; perf.G_Dt_max = 0;
perf.G_Dt_min = 0; perf.G_Dt_min = 0;
perf.start_ms = millis(); perf.num_long = 0;
perf.start_ms = millis();
} }

Write Preview
Loading…
Cancel
Save