Plane: use AP:PerfInfo class
This commit is contained in:
parent
55358459c7
commit
2fb6113098
@ -108,6 +108,9 @@ void Plane::setup()
|
|||||||
|
|
||||||
// initialise the main loop scheduler
|
// initialise the main loop scheduler
|
||||||
scheduler.init(&scheduler_tasks[0], ARRAY_SIZE(scheduler_tasks));
|
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()
|
void Plane::loop()
|
||||||
@ -119,23 +122,12 @@ void Plane::loop()
|
|||||||
|
|
||||||
uint32_t timer = micros();
|
uint32_t timer = micros();
|
||||||
|
|
||||||
perf.delta_us_fast_loop = timer - perf.fast_loopTimer_us;
|
// check loop time
|
||||||
G_Dt = perf.delta_us_fast_loop * 1.0e-6f;
|
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) {
|
mainLoop_count++;
|
||||||
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++;
|
|
||||||
|
|
||||||
// tell the scheduler one tick has passed
|
// tell the scheduler one tick has passed
|
||||||
scheduler.tick();
|
scheduler.tick();
|
||||||
@ -367,20 +359,19 @@ void Plane::one_second_loop()
|
|||||||
|
|
||||||
void Plane::log_perf_info()
|
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)) {
|
if (should_log(MASK_LOG_PM)) {
|
||||||
Log_Write_Performance();
|
Log_Write_Performance();
|
||||||
}
|
}
|
||||||
|
if (scheduler.debug()) {
|
||||||
resetPerfData();
|
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()
|
void Plane::compass_save()
|
||||||
|
@ -68,16 +68,18 @@ struct PACKED log_Performance {
|
|||||||
// Write a performance monitoring packet. Total length : 19 bytes
|
// Write a performance monitoring packet. Total length : 19 bytes
|
||||||
void Plane::Log_Write_Performance()
|
void Plane::Log_Write_Performance()
|
||||||
{
|
{
|
||||||
|
uint32_t dropped = DataFlash.num_dropped();
|
||||||
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(),
|
||||||
num_long : perf.num_long,
|
num_long : perf_info.get_num_long_running(),
|
||||||
main_loop_count : perf.mainLoop_count,
|
main_loop_count : (uint16_t)mainLoop_count,
|
||||||
g_dt_max : perf.G_Dt_max,
|
g_dt_max : perf_info.get_max_time(),
|
||||||
g_dt_min : perf.G_Dt_min,
|
g_dt_min : perf_info.get_min_time(),
|
||||||
log_dropped : DataFlash.num_dropped() - perf.last_log_dropped,
|
log_dropped : dropped - last_log_dropped,
|
||||||
hal.util->available_memory()
|
hal.util->available_memory()
|
||||||
};
|
};
|
||||||
|
last_log_dropped = dropped;
|
||||||
DataFlash.WriteCriticalBlock(&pkt, sizeof(pkt));
|
DataFlash.WriteCriticalBlock(&pkt, sizeof(pkt));
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -95,6 +95,7 @@
|
|||||||
#include <AP_Button/AP_Button.h>
|
#include <AP_Button/AP_Button.h>
|
||||||
#include <AP_ICEngine/AP_ICEngine.h>
|
#include <AP_ICEngine/AP_ICEngine.h>
|
||||||
#include <AP_Landing/AP_Landing.h>
|
#include <AP_Landing/AP_Landing.h>
|
||||||
|
#include <AP_Scheduler/PerfInfo.h> // loop perf monitoring
|
||||||
|
|
||||||
#include "GCS_Mavlink.h"
|
#include "GCS_Mavlink.h"
|
||||||
#include "GCS_Plane.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.
|
// This is the time between calls to the DCM algorithm and is the Integration time for the gyros.
|
||||||
float G_Dt = 0.02f;
|
float G_Dt = 0.02f;
|
||||||
|
|
||||||
struct {
|
// loop performance monitoring:
|
||||||
// Performance monitoring
|
AP::PerfInfo perf_info;
|
||||||
// Timer used to accrue data and trigger recording of the performanc monitoring log message
|
uint32_t mainLoop_count;
|
||||||
uint32_t start_ms;
|
uint32_t fast_loopTimer;
|
||||||
|
uint32_t last_log_dropped;
|
||||||
// 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;
|
|
||||||
|
|
||||||
struct {
|
struct {
|
||||||
uint32_t last_trim_check;
|
uint32_t last_trim_check;
|
||||||
@ -972,7 +953,6 @@ private:
|
|||||||
void check_short_failsafe();
|
void check_short_failsafe();
|
||||||
void startup_INS_ground(void);
|
void startup_INS_ground(void);
|
||||||
void update_notify();
|
void update_notify();
|
||||||
void resetPerfData(void);
|
|
||||||
bool should_log(uint32_t mask);
|
bool should_log(uint32_t mask);
|
||||||
int8_t throttle_percentage(void);
|
int8_t throttle_percentage(void);
|
||||||
void change_arm_state(void);
|
void change_arm_state(void);
|
||||||
|
@ -21,9 +21,9 @@ void Plane::failsafe_check(void)
|
|||||||
static bool in_failsafe;
|
static bool in_failsafe;
|
||||||
uint32_t tnow = micros();
|
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
|
// 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;
|
last_timestamp = tnow;
|
||||||
in_failsafe = false;
|
in_failsafe = false;
|
||||||
return;
|
return;
|
||||||
|
@ -599,17 +599,6 @@ void Plane::update_notify()
|
|||||||
notify.update();
|
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
|
// sets notify object flight mode information
|
||||||
void Plane::notify_flight_mode(enum FlightMode mode)
|
void Plane::notify_flight_mode(enum FlightMode mode)
|
||||||
{
|
{
|
||||||
|
Loading…
Reference in New Issue
Block a user