aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Plane: use AP:PerfInfo class

Browse Source
This commit is contained in:
Andrew Tridgell 2018-02-10 10:09:24 +11:00
parent 55358459c7
commit 2fb6113098
5 changed files with 33 additions and 71 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

45
ArduPlane/ArduPlane.cpp
View File

@ -108,6 +108,9 @@ void Plane::setup()
// initialise the main loop scheduler
scheduler.init(&scheduler_tasks[0], ARRAY_SIZE(scheduler_tasks));
perf_info.reset(scheduler.get_loop_rate_hz());
perf_info.ignore_this_loop();
}
void Plane::loop()
@ -119,23 +122,12 @@ void Plane::loop()
uint32_t timer = micros();
perf.delta_us_fast_loop = timer - perf.fast_loopTimer_us;
G_Dt = perf.delta_us_fast_loop * 1.0e-6f;
// check loop time
perf_info.check_loop_time(timer - fast_loopTimer);
G_Dt = (float)(timer - fast_loopTimer) * 1.0e-6;
fast_loopTimer = timer;
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;
}
if (perf.delta_us_fast_loop < perf.G_Dt_min || perf.G_Dt_min == 0) {
perf.G_Dt_min = perf.delta_us_fast_loop;
}
perf.fast_loopTimer_us = timer;
perf.mainLoop_count++;
mainLoop_count++;
// tell the scheduler one tick has passed
scheduler.tick();
@ -367,20 +359,19 @@ void Plane::one_second_loop()
void Plane::log_perf_info()
{
if (scheduler.debug() != 0) {
gcs().send_text(MAV_SEVERITY_INFO, "PERF: %u/%u Dt=%u/%u Log=%u",
(unsigned)perf.num_long,
(unsigned)perf.mainLoop_count,
(unsigned)perf.G_Dt_max,
(unsigned)perf.G_Dt_min,
(unsigned)(DataFlash.num_dropped() - perf.last_log_dropped));
}
if (should_log(MASK_LOG_PM)) {
Log_Write_Performance();
}
resetPerfData();
if (scheduler.debug()) {
gcs().send_text(MAV_SEVERITY_WARNING, "PERF: %u/%u max=%lu min=%lu avg=%lu sd=%lu",
(unsigned)perf_info.get_num_long_running(),
(unsigned)perf_info.get_num_loops(),
(unsigned long)perf_info.get_max_time(),
(unsigned long)perf_info.get_min_time(),
(unsigned long)perf_info.get_avg_time(),
(unsigned long)perf_info.get_stddev_time());
}
perf_info.reset(scheduler.get_loop_rate_hz());
}
void Plane::compass_save()

12
ArduPlane/Log.cpp
View File

@ -68,16 +68,18 @@ struct PACKED log_Performance {
// Write a performance monitoring packet. Total length : 19 bytes
void Plane::Log_Write_Performance()
{
uint32_t dropped = DataFlash.num_dropped();
struct log_Performance pkt = {
LOG_PACKET_HEADER_INIT(LOG_PERFORMANCE_MSG),
time_us : AP_HAL::micros64(),
num_long : perf.num_long,
main_loop_count : perf.mainLoop_count,
g_dt_max : perf.G_Dt_max,
g_dt_min : perf.G_Dt_min,
log_dropped : DataFlash.num_dropped() - perf.last_log_dropped,
num_long : perf_info.get_num_long_running(),
main_loop_count : (uint16_t)mainLoop_count,
g_dt_max : perf_info.get_max_time(),
g_dt_min : perf_info.get_min_time(),
log_dropped : dropped - last_log_dropped,
hal.util->available_memory()
};
last_log_dropped = dropped;
DataFlash.WriteCriticalBlock(&pkt, sizeof(pkt));
}

32
ArduPlane/Plane.h
View File

@ -95,6 +95,7 @@
#include <AP_Button/AP_Button.h>
#include <AP_ICEngine/AP_ICEngine.h>
#include <AP_Landing/AP_Landing.h>
#include <AP_Scheduler/PerfInfo.h> // loop perf monitoring
#include "GCS_Mavlink.h"
#include "GCS_Plane.h"
@ -734,31 +735,11 @@ private:
// This is the time between calls to the DCM algorithm and is the Integration time for the gyros.
float G_Dt = 0.02f;
struct {
// Performance monitoring
// Timer used to accrue data and trigger recording of the performanc monitoring log message
uint32_t start_ms;
// The maximum and minimum main loop execution time, in microseconds, recorded in the current performance monitoring interval
uint32_t G_Dt_max;
uint32_t G_Dt_min;
// System Timers
// Time in microseconds of start of main control loop
uint32_t fast_loopTimer_us;
// Number of milliseconds used in last main loop cycle
uint32_t delta_us_fast_loop;
// Counter of main loop executions. Used for performance monitoring and failsafe processing
uint16_t mainLoop_count;
// number of long loops
uint16_t num_long;
// accumulated lost log messages
uint32_t last_log_dropped;
} perf;
// loop performance monitoring:
AP::PerfInfo perf_info;
uint32_t mainLoop_count;
uint32_t fast_loopTimer;
uint32_t last_log_dropped;
struct {
uint32_t last_trim_check;
@ -972,7 +953,6 @@ private:
void check_short_failsafe();
void startup_INS_ground(void);
void update_notify();
void resetPerfData(void);
bool should_log(uint32_t mask);
int8_t throttle_percentage(void);
void change_arm_state(void);

4
ArduPlane/failsafe.cpp
View File

@ -21,9 +21,9 @@ void Plane::failsafe_check(void)
static bool in_failsafe;
uint32_t tnow = micros();
if (perf.mainLoop_count != last_mainLoop_count) {
if (perf_info.get_num_loops() != last_mainLoop_count) {
// the main loop is running, all is OK
last_mainLoop_count = perf.mainLoop_count;
last_mainLoop_count = perf_info.get_num_loops();
last_timestamp = tnow;
in_failsafe = false;
return;

11
ArduPlane/system.cpp
View File

@ -599,17 +599,6 @@ void Plane::update_notify()
notify.update();
}
void Plane::resetPerfData(void)
{
perf.mainLoop_count = 0;
perf.G_Dt_max = 0;
perf.G_Dt_min = 0;
perf.num_long = 0;
perf.start_ms = millis();
perf.last_log_dropped = DataFlash.num_dropped();
}
// sets notify object flight mode information
void Plane::notify_flight_mode(enum FlightMode mode)
{