From e4be8eccc9b8bf8f91f5a00638f419b39e4a3345 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Sat, 7 Jul 2018 11:17:46 -0400
Subject: [PATCH] bmi055 add measure interval perf counter

---
 src/drivers/imu/bmi055/BMI055_accel.cpp | 5 +++++
 src/drivers/imu/bmi055/BMI055_accel.hpp | 1 +
 src/drivers/imu/bmi055/BMI055_gyro.cpp  | 5 +++++
 src/drivers/imu/bmi055/BMI055_gyro.hpp  | 1 +
 4 files changed, 12 insertions(+)

diff --git a/src/drivers/imu/bmi055/BMI055_accel.cpp b/src/drivers/imu/bmi055/BMI055_accel.cpp
index eb4bd9697418..f06daea1e059 100644
--- a/src/drivers/imu/bmi055/BMI055_accel.cpp
+++ b/src/drivers/imu/bmi055/BMI055_accel.cpp
@@ -49,6 +49,7 @@ const uint8_t BMI055_accel::_checked_registers[BMI055_ACCEL_NUM_CHECKED_REGISTER
 BMI055_accel::BMI055_accel(int bus, const char *path_accel, uint32_t device, enum Rotation rotation) :
 	BMI055("BMI055_ACCEL", path_accel, bus, device, SPIDEV_MODE3, BMI055_BUS_SPEED, rotation),
 	_sample_perf(perf_alloc(PC_ELAPSED, "bmi055_accel_read")),
+	_measure_interval(perf_alloc(PC_INTERVAL, "bmi055_accel_measure_interval")),
 	_bad_transfers(perf_alloc(PC_COUNT, "bmi055_accel_bad_transfers")),
 	_bad_registers(perf_alloc(PC_COUNT, "bmi055_accel_bad_registers")),
 	_duplicates(perf_alloc(PC_COUNT, "bmi055_accel_duplicates")),
@@ -96,6 +97,7 @@ BMI055_accel::~BMI055_accel()
 
 	/* delete the perf counter */
 	perf_free(_sample_perf);
+	perf_free(_measure_interval);
 	perf_free(_bad_transfers);
 	perf_free(_bad_registers);
 	perf_free(_duplicates);
@@ -637,6 +639,8 @@ BMI055_accel::check_registers(void)
 void
 BMI055_accel::measure()
 {
+	perf_count(_measure_interval);
+
 	uint8_t index = 0, accel_data[7];
 	uint16_t lsb, msb, msblsb;
 	uint8_t status_x, status_y, status_z;
@@ -805,6 +809,7 @@ BMI055_accel::print_info()
 	PX4_INFO("Accel");
 
 	perf_print_counter(_sample_perf);
+	perf_print_counter(_measure_interval);
 	perf_print_counter(_bad_transfers);
 	perf_print_counter(_bad_registers);
 	perf_print_counter(_duplicates);
diff --git a/src/drivers/imu/bmi055/BMI055_accel.hpp b/src/drivers/imu/bmi055/BMI055_accel.hpp
index 0a38d3387703..3718fd172a20 100644
--- a/src/drivers/imu/bmi055/BMI055_accel.hpp
+++ b/src/drivers/imu/bmi055/BMI055_accel.hpp
@@ -176,6 +176,7 @@ class BMI055_accel : public BMI055
 private:
 
 	perf_counter_t      _sample_perf;
+	perf_counter_t      _measure_interval;
 	perf_counter_t      _bad_transfers;
 	perf_counter_t      _bad_registers;
 	perf_counter_t      _duplicates;
diff --git a/src/drivers/imu/bmi055/BMI055_gyro.cpp b/src/drivers/imu/bmi055/BMI055_gyro.cpp
index fcd4fdb0a9f2..30a4fd32a5ef 100644
--- a/src/drivers/imu/bmi055/BMI055_gyro.cpp
+++ b/src/drivers/imu/bmi055/BMI055_gyro.cpp
@@ -50,6 +50,7 @@ const uint8_t BMI055_gyro::_checked_registers[BMI055_GYRO_NUM_CHECKED_REGISTERS]
 BMI055_gyro::BMI055_gyro(int bus, const char *path_gyro, uint32_t device, enum Rotation rotation) :
 	BMI055("BMI055_GYRO", path_gyro, bus, device, SPIDEV_MODE3, BMI055_BUS_SPEED, rotation),
 	_sample_perf(perf_alloc(PC_ELAPSED, "bmi055_gyro_read")),
+	_measure_interval(perf_alloc(PC_INTERVAL, "bmi055_gyro_measure_interval")),
 	_bad_transfers(perf_alloc(PC_COUNT, "bmi055_gyro_bad_transfers")),
 	_bad_registers(perf_alloc(PC_COUNT, "bmi055_gyro_bad_registers")),
 	_gyro_reports(nullptr),
@@ -95,6 +96,7 @@ BMI055_gyro::~BMI055_gyro()
 
 	/* delete the perf counter */
 	perf_free(_sample_perf);
+	perf_free(_measure_interval);
 	perf_free(_bad_transfers);
 	perf_free(_bad_registers);
 }
@@ -637,6 +639,8 @@ BMI055_gyro::check_registers(void)
 void
 BMI055_gyro::measure()
 {
+	perf_count(_measure_interval);
+
 	if (hrt_absolute_time() < _reset_wait) {
 		// we're waiting for a reset to complete
 		return;
@@ -779,6 +783,7 @@ BMI055_gyro::print_info()
 	PX4_INFO("Gyro");
 
 	perf_print_counter(_sample_perf);
+	perf_print_counter(_measure_interval);
 	perf_print_counter(_bad_transfers);
 	perf_print_counter(_bad_registers);
 
diff --git a/src/drivers/imu/bmi055/BMI055_gyro.hpp b/src/drivers/imu/bmi055/BMI055_gyro.hpp
index 1814a42b12d9..1b8de7d41f21 100644
--- a/src/drivers/imu/bmi055/BMI055_gyro.hpp
+++ b/src/drivers/imu/bmi055/BMI055_gyro.hpp
@@ -168,6 +168,7 @@ class BMI055_gyro : public BMI055
 private:
 
 	perf_counter_t      _sample_perf;
+	perf_counter_t      _measure_interval;
 	perf_counter_t      _bad_transfers;
 	perf_counter_t      _bad_registers;