ardupilot/libraries/AP_HAL_Linux/Perf.cpp

190 lines
5.1 KiB
C++

/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/*
* Copyright (C) 2015 Intel Corporation. All rights reserved.
*
* This file is free software: you can redistribute it and/or modify it
* under the terms of the GNU General Public License as published by the
* Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This file is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include <AP_HAL/AP_HAL.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX
#include <limits.h>
#include <time.h>
#include <AP_Math/AP_Math.h>
#include "AP_HAL_Linux.h"
#include "Util.h"
using namespace Linux;
struct perf_counter_base_t {
const char *name;
enum Util::perf_counter_type type;
};
struct perf_counter_count_t {
struct perf_counter_base_t base;
uint64_t count;
};
struct perf_counter_elapsed_t {
struct perf_counter_base_t base;
uint64_t count;
/* Everything below is in nanoseconds */
uint64_t start;
uint64_t total;
uint64_t least;
uint64_t most;
double mean;
double m2;
};
static const AP_HAL::HAL& hal = AP_HAL::get_HAL();
Util::perf_counter_t Util::perf_alloc(perf_counter_type type, const char *name)
{
struct perf_counter_base_t *base;
switch(type) {
case PC_COUNT: {
struct perf_counter_count_t *count;
count = (struct perf_counter_count_t *)calloc(1, sizeof(struct perf_counter_count_t));
if (!count) {
return nullptr;
}
base = &count->base;
break;
}
case PC_ELAPSED: {
struct perf_counter_elapsed_t *elapsed;
elapsed = (struct perf_counter_elapsed_t *)calloc(1, sizeof(struct perf_counter_elapsed_t));
if (!elapsed) {
return nullptr;
}
elapsed->least = ULONG_MAX;
base = &elapsed->base;
break;
}
default:
case PC_INTERVAL: {
/*
* Not implemented now because it is not used even on PX4 specific
* code and by looking at PX4 implementation without perf_reset()
* the average is broken.
*/
return nullptr;
}
}
base->name = name;
base->type = type;
return (perf_counter_t)base;
}
static inline uint64_t timespec_to_nsec(const struct timespec *ts)
{
return ts->tv_nsec + (ts->tv_sec * NSEC_PER_SEC);
}
void Util::perf_begin(perf_counter_t perf)
{
struct perf_counter_elapsed_t *perf_elapsed = (struct perf_counter_elapsed_t *)perf;
if (perf_elapsed == NULL) {
return;
}
if (perf_elapsed->base.type != PC_ELAPSED) {
hal.console->printf("perf_begin() called over a perf_counter_t(%s) that"
" is not of the PC_ELAPSED type.\n",
perf_elapsed->base.name);
return;
}
struct timespec ts;
clock_gettime(CLOCK_MONOTONIC, &ts);
perf_elapsed->start = timespec_to_nsec(&ts);
}
void Util::perf_end(perf_counter_t perf)
{
struct perf_counter_elapsed_t *perf_elapsed = (struct perf_counter_elapsed_t *)perf;
if (perf_elapsed == NULL) {
return;
}
if (perf_elapsed->base.type != PC_ELAPSED) {
hal.console->printf("perf_end() called over a perf_counter_t(%s) "
"that is not of the PC_ELAPSED type.\n",
perf_elapsed->base.name);
return;
}
if (perf_elapsed->start == 0) {
hal.console->printf("perf_end() called before an perf_begin() on %s.\n",
perf_elapsed->base.name);
return;
}
struct timespec ts;
clock_gettime(CLOCK_MONOTONIC, &ts);
const uint64_t elapsed = timespec_to_nsec(&ts) - perf_elapsed->start;
perf_elapsed->count++;
perf_elapsed->total += elapsed;
if (perf_elapsed->least > elapsed) {
perf_elapsed->least = elapsed;
}
if (perf_elapsed->most < elapsed) {
perf_elapsed->most = elapsed;
}
/*
* Maintain mean and variance of interval in nanoseconds
* Knuth/Welford recursive mean and variance of update intervals (via Wikipedia)
* Same implementation of PX4.
*/
const double delta_intvl = elapsed - perf_elapsed->mean;
perf_elapsed->mean += (delta_intvl / perf_elapsed->count);
perf_elapsed->m2 += (delta_intvl * (elapsed - perf_elapsed->mean));
perf_elapsed->start = 0;
}
void Util::perf_count(perf_counter_t perf)
{
struct perf_counter_count_t *perf_counter = (struct perf_counter_count_t *)perf;
if (perf_counter == NULL) {
return;
}
if (perf_counter->base.type != PC_COUNT) {
hal.console->printf("perf_count() called over a perf_counter_t(%s) "
"that is not of the PC_COUNT type.\n",
perf_counter->base.name);
return;
}
perf_counter->count++;
}
#endif