diff --git a/src/drivers/vesc/VescSerialDevice.cpp b/src/drivers/vesc/VescSerialDevice.cpp index 6961adfe58..f46e515370 100644 --- a/src/drivers/vesc/VescSerialDevice.cpp +++ b/src/drivers/vesc/VescSerialDevice.cpp @@ -54,6 +54,11 @@ VescDevice::VescDevice(const char *port) : _port[sizeof(_port) - 1] = '\0'; } +VescDevice::~VescDevice() +{ + perf_free(_cycle_perf); +} + int VescDevice::init() { int ret = setBaudrate(115200); @@ -69,16 +74,19 @@ void VescDevice::print_info() { printf("Using port '%s'\n", _port); printf("VESC version: %d.%d\n", _vesc_driver.getVersionMajor(), _vesc_driver.getVersionMinor()); + perf_print_counter(_cycle_perf); } void VescDevice::Run() { + perf_begin(_cycle_perf); + if (_serial_fd < 0) { // Reopen fd from the work queue context _serial_fd = ::open(_port, O_RDWR | O_NOCTTY); + _vesc_driver.requestFirmwareVersion(); } - _vesc_driver.requestFirmwareVersion(); _vesc_driver.commandDutyCycle(.05f); // Check the number of bytes available in the buffer @@ -107,6 +115,7 @@ void VescDevice::Run() } // TODO: ::close(_serial_fd); on exit + perf_end(_cycle_perf); } int VescDevice::setBaudrate(const unsigned baudrate) diff --git a/src/drivers/vesc/VescSerialDevice.hpp b/src/drivers/vesc/VescSerialDevice.hpp index 6e8452dbee..dc9030f77b 100644 --- a/src/drivers/vesc/VescSerialDevice.hpp +++ b/src/drivers/vesc/VescSerialDevice.hpp @@ -44,7 +44,7 @@ class VescDevice : public px4::ScheduledWorkItem, public VescWritable { public: VescDevice(const char *port); - ~VescDevice() = default; + ~VescDevice(); int init(); void print_info(); @@ -57,4 +57,5 @@ private: char _port[20] {}; int _serial_fd{-1}; VescDriver _vesc_driver; + perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; };