From a589d15c5212c3249599932080f656ce2b7a0044 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Tue, 11 Aug 2015 12:07:06 -0700 Subject: [PATCH] Refactored debug() and log() in CDev These functions used vprintf which is not available on all platforms. They also do not enable line and file debug output. Changed to macros that preserve the output format. Uses new macro that can be used to implement per object, runtime selectable logging Signed-off-by: Mark Charlebois --- readme.md | 0 src/drivers/blinkm/blinkm.cpp | 2 +- src/drivers/device/device_nuttx.cpp | 29 +-------------- src/drivers/device/device_nuttx.h | 19 ++-------- src/drivers/device/device_posix.cpp | 28 --------------- src/drivers/device/i2c_nuttx.cpp | 14 ++++---- src/drivers/device/i2c_posix.cpp | 6 ++-- src/drivers/device/spi.cpp | 8 ++--- src/drivers/device/vdev.h | 23 +++--------- src/drivers/ets_airspeed/ets_airspeed.cpp | 4 +-- src/drivers/gimbal/gimbal.cpp | 4 +-- src/drivers/gps/gps.cpp | 2 +- src/drivers/hil/hil.cpp | 24 ++++++------- src/drivers/hmc5883/hmc5883.cpp | 14 ++++---- src/drivers/hmc5883/hmc5883_i2c.cpp | 4 +-- src/drivers/hmc5883/hmc5883_spi.cpp | 6 ++-- src/drivers/l3gd20/l3gd20.cpp | 4 +-- src/drivers/led/led.cpp | 2 +- src/drivers/ll40ls/LidarLiteI2C.cpp | 12 +++---- src/drivers/ll40ls/LidarLitePWM.cpp | 4 +-- src/drivers/mb12xx/mb12xx.cpp | 16 ++++----- src/drivers/meas_airspeed/meas_airspeed.cpp | 2 +- src/drivers/mkblctrl/mkblctrl.cpp | 12 +++---- src/drivers/mpu6000/mpu6000.cpp | 10 +++--- src/drivers/ms5611/ms5611_nuttx.cpp | 6 ++-- src/drivers/ms5611/ms5611_posix.cpp | 8 ++--- src/drivers/ms5611/ms5611_spi.cpp | 13 ++++--- src/drivers/oreoled/oreoled.cpp | 4 +-- src/drivers/pca9685/pca9685.cpp | 8 ++--- src/drivers/px4flow/px4flow.cpp | 10 +++--- src/drivers/px4fmu/fmu.cpp | 28 +++++++-------- src/drivers/px4io/px4io.cpp | 36 +++++++++---------- src/drivers/rgbled/rgbled.cpp | 4 +-- src/drivers/sf0x/sf0x.cpp | 12 +++---- src/drivers/stm32/adc/adc.cpp | 6 ++-- src/drivers/stm32/tone_alarm/tone_alarm.cpp | 6 ++-- src/drivers/trone/trone.cpp | 12 +++---- src/modules/uavcan/sensors/baro.cpp | 6 ++-- src/modules/uavcan/sensors/mag.cpp | 2 +- src/modules/uavcan/sensors/sensor_bridge.cpp | 10 +++--- src/modules/uavcan/uavcan_main.cpp | 2 +- src/platforms/posix/drivers/adcsim/adcsim.cpp | 2 +- src/platforms/posix/drivers/barosim/baro.cpp | 8 ++--- .../posix/drivers/gyrosim/gyrosim.cpp | 4 +-- .../posix/drivers/tonealrmsim/tone_alarm.cpp | 4 +-- src/platforms/px4_log.h | 19 ++++++++++ src/platforms/px4_workqueue.h | 4 +++ 47 files changed, 202 insertions(+), 261 deletions(-) create mode 100644 readme.md diff --git a/readme.md b/readme.md new file mode 100644 index 0000000000..e69de29bb2 diff --git a/src/drivers/blinkm/blinkm.cpp b/src/drivers/blinkm/blinkm.cpp index 185565d8b6..17cd868cdc 100644 --- a/src/drivers/blinkm/blinkm.cpp +++ b/src/drivers/blinkm/blinkm.cpp @@ -356,7 +356,7 @@ BlinkM::probe() ret = get_firmware_version(version); if (ret == OK) - log("found BlinkM firmware version %c%c", version[1], version[0]); + DEVICE_DEBUG("found BlinkM firmware version %c%c", version[1], version[0]); return ret; } diff --git a/src/drivers/device/device_nuttx.cpp b/src/drivers/device/device_nuttx.cpp index d46901683f..9146acb5da 100644 --- a/src/drivers/device/device_nuttx.cpp +++ b/src/drivers/device/device_nuttx.cpp @@ -38,6 +38,7 @@ */ #include "device.h" +#include "px4_log.h" #include #include @@ -156,34 +157,6 @@ Device::interrupt(void *context) interrupt_disable(); } -void -Device::log(const char *fmt, ...) -{ - va_list ap; - - printf("[%s] ", _name); - va_start(ap, fmt); - vprintf(fmt, ap); - va_end(ap); - printf("\n"); - fflush(stdout); -} - -void -Device::debug(const char *fmt, ...) -{ - va_list ap; - - if (_debug_enabled) { - printf("<%s> ", _name); - va_start(ap, fmt); - vprintf(fmt, ap); - va_end(ap); - printf("\n"); - fflush(stdout); - } -} - static int register_interrupt(int irq, Device *owner) { diff --git a/src/drivers/device/device_nuttx.h b/src/drivers/device/device_nuttx.h index a3a0640d2d..974d94ee45 100644 --- a/src/drivers/device/device_nuttx.h +++ b/src/drivers/device/device_nuttx.h @@ -54,6 +54,9 @@ #include +#define DEVICE_LOG(FMT, ...) PX4_LOG_NAMED(_name, FMT, ##__VA_ARGS__) +#define DEVICE_DEBUG(FMT, ...) PX4_LOG_NAMED_COND(_name, _debug_enabled, FMT, ##__VA_ARGS__) + /** * Namespace encapsulating all device framework classes, functions and data. */ @@ -197,22 +200,6 @@ protected: sem_post(&_lock); } - /** - * Log a message. - * - * The message is prefixed with the driver name, and followed - * by a newline. - */ - void log(const char *fmt, ...); - - /** - * Print a debug message. - * - * The message is prefixed with the driver name, and followed - * by a newline. - */ - void debug(const char *fmt, ...); - private: int _irq; bool _irq_attached; diff --git a/src/drivers/device/device_posix.cpp b/src/drivers/device/device_posix.cpp index 771bee2471..5fc6a595ad 100644 --- a/src/drivers/device/device_posix.cpp +++ b/src/drivers/device/device_posix.cpp @@ -78,34 +78,6 @@ Device::init() return ret; } -void -Device::log(const char *fmt, ...) -{ - va_list ap; - - PX4_INFO("[%s] ", _name); - va_start(ap, fmt); - vprintf(fmt, ap); - va_end(ap); - printf("\n"); - fflush(stdout); -} - -void -Device::debug(const char *fmt, ...) -{ - va_list ap; - - if (_debug_enabled) { - printf("<%s> ", _name); - va_start(ap, fmt); - vprintf(fmt, ap); - va_end(ap); - printf("\n"); - fflush(stdout); - } -} - int Device::dev_read(unsigned offset, void *data, unsigned count) { diff --git a/src/drivers/device/i2c_nuttx.cpp b/src/drivers/device/i2c_nuttx.cpp index 092ecd1440..ad74f60c47 100644 --- a/src/drivers/device/i2c_nuttx.cpp +++ b/src/drivers/device/i2c_nuttx.cpp @@ -90,7 +90,7 @@ I2C::set_bus_clock(unsigned bus, unsigned clock_hz) } if (_bus_clocks[index] > 0) { - // debug("overriding clock of %u with %u Hz\n", _bus_clocks[index], clock_hz); + // DEVICE_DEBUG("overriding clock of %u with %u Hz\n", _bus_clocks[index], clock_hz); } _bus_clocks[index] = clock_hz; @@ -107,7 +107,7 @@ I2C::init() _dev = up_i2cinitialize(_bus); if (_dev == nullptr) { - debug("failed to init I2C"); + DEVICE_DEBUG("failed to init I2C"); ret = -ENOENT; goto out; } @@ -121,7 +121,7 @@ I2C::init() if (_bus_clocks[bus_index] > _frequency) { (void)up_i2cuninitialize(_dev); _dev = nullptr; - log("FAIL: too slow for bus #%u: %u KHz, device max: %u KHz)", + DEVICE_LOG("FAIL: too slow for bus #%u: %u KHz, device max: %u KHz)", _bus, _bus_clocks[bus_index] / 1000, _frequency / 1000); ret = -EINVAL; goto out; @@ -148,7 +148,7 @@ I2C::init() ret = probe(); if (ret != OK) { - debug("probe failed"); + DEVICE_DEBUG("probe failed"); goto out; } @@ -156,12 +156,12 @@ I2C::init() ret = CDev::init(); if (ret != OK) { - debug("cdev init failed"); + DEVICE_DEBUG("cdev init failed"); goto out; } // tell the world where we are - log("on I2C bus %d at 0x%02x (bus: %u KHz, max: %u KHz)", + DEVICE_LOG("on I2C bus %d at 0x%02x (bus: %u KHz, max: %u KHz)", _bus, _address, _bus_clocks[bus_index] / 1000, _frequency / 1000); out: @@ -188,7 +188,7 @@ I2C::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned re unsigned retry_count = 0; do { - // debug("transfer out %p/%u in %p/%u", send, send_len, recv, recv_len); + // DEVICE_DEBUG("transfer out %p/%u in %p/%u", send, send_len, recv, recv_len); msgs = 0; diff --git a/src/drivers/device/i2c_posix.cpp b/src/drivers/device/i2c_posix.cpp index 159366c168..9ccd6c29ca 100644 --- a/src/drivers/device/i2c_posix.cpp +++ b/src/drivers/device/i2c_posix.cpp @@ -100,13 +100,13 @@ I2C::init() ret = VDev::init(); if (ret != PX4_OK) { - debug("VDev::init failed"); + DEVICE_DEBUG("VDev::init failed"); return ret; } _fd = px4_open(get_devname(), PX4_F_RDONLY | PX4_F_WRONLY); if (_fd < 0) { - debug("px4_open failed of device %s", get_devname()); + DEVICE_DEBUG("px4_open failed of device %s", get_devname()); return PX4_ERROR; } @@ -150,7 +150,7 @@ I2C::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned re } do { - // debug("transfer out %p/%u in %p/%u", send, send_len, recv, recv_len); + // DEVICE_DEBUG("transfer out %p/%u in %p/%u", send, send_len, recv, recv_len); msgs = 0; if (send_len > 0) { diff --git a/src/drivers/device/spi.cpp b/src/drivers/device/spi.cpp index 14f6d5706c..698fa63996 100644 --- a/src/drivers/device/spi.cpp +++ b/src/drivers/device/spi.cpp @@ -98,7 +98,7 @@ SPI::init() _dev = up_spiinitialize(_bus); if (_dev == nullptr) { - debug("failed to init SPI"); + DEVICE_DEBUG("failed to init SPI"); ret = -ENOENT; goto out; } @@ -110,7 +110,7 @@ SPI::init() ret = probe(); if (ret != OK) { - debug("probe failed"); + DEVICE_DEBUG("probe failed"); goto out; } @@ -118,12 +118,12 @@ SPI::init() ret = CDev::init(); if (ret != OK) { - debug("cdev init failed"); + DEVICE_DEBUG("cdev init failed"); goto out; } /* tell the workd where we are */ - log("on SPI bus %d at %d (%u KHz)", _bus, _device, _frequency / 1000); + DEVICE_LOG("on SPI bus %d at %d (%u KHz)", _bus, _device, _frequency / 1000); out: return ret; diff --git a/src/drivers/device/vdev.h b/src/drivers/device/vdev.h index 7d5b8811b2..3fd28c82ca 100644 --- a/src/drivers/device/vdev.h +++ b/src/drivers/device/vdev.h @@ -51,6 +51,9 @@ #include #include +#define DEVICE_LOG(FMT, ...) PX4_LOG_NAMED(_name, FMT, ##__VA_ARGS__) +#define DEVICE_DEBUG(FMT, ...) PX4_LOG_NAMED_COND(_name, _debug_enabled, FMT, ##__VA_ARGS__) + /** * Namespace encapsulating all device framework classes, functions and data. */ @@ -176,7 +179,7 @@ protected: * Note that we must loop as the wait may be interrupted by a signal. */ void lock() { - debug("lock"); + DEVICE_DEBUG("lock"); do {} while (sem_wait(&_lock) != 0); } @@ -184,26 +187,10 @@ protected: * Release the driver lock. */ void unlock() { - debug("unlock"); + DEVICE_DEBUG("unlock"); sem_post(&_lock); } - /** - * Log a message. - * - * The message is prefixed with the driver name, and followed - * by a newline. - */ - void log(const char *fmt, ...); - - /** - * Print a debug message. - * - * The message is prefixed with the driver name, and followed - * by a newline. - */ - void debug(const char *fmt, ...); - private: sem_t _lock; diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp index a5a3fd9b20..b4517d66e7 100644 --- a/src/drivers/ets_airspeed/ets_airspeed.cpp +++ b/src/drivers/ets_airspeed/ets_airspeed.cpp @@ -161,7 +161,7 @@ ETSAirspeed::collect() // caller could end up using this value as part of an // average perf_count(_comms_errors); - log("zero value from sensor"); + DEVICE_LOG("zero value from sensor"); return -1; } @@ -240,7 +240,7 @@ ETSAirspeed::cycle() /* measurement phase */ ret = measure(); if (OK != ret) { - debug("measure error"); + DEVICE_DEBUG("measure error"); } _sensor_ok = (ret == OK); diff --git a/src/drivers/gimbal/gimbal.cpp b/src/drivers/gimbal/gimbal.cpp index 8085c43a1f..249fc22dac 100644 --- a/src/drivers/gimbal/gimbal.cpp +++ b/src/drivers/gimbal/gimbal.cpp @@ -358,7 +358,7 @@ Gimbal::cycle() if (_control_cmd_set) { unsigned mountMode = _control_cmd.param7; - debug("control_cmd: %d, mountMode %d | param1: %8.4f param2: %8.4f", _control_cmd.command, mountMode, (double)_control_cmd.param1, (double)_control_cmd.param2); + DEVICE_DEBUG("control_cmd: %d, mountMode %d | param1: %8.4f param2: %8.4f", _control_cmd.command, mountMode, (double)_control_cmd.param1, (double)_control_cmd.param2); if (_control_cmd.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL && mountMode == vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING) { @@ -388,7 +388,7 @@ Gimbal::cycle() struct actuator_controls_s controls; - // debug("publishing | roll: %8.4f pitch: %8.4f yaw: %8.4f", (double)roll, (double)pitch, (double)yaw); + // DEVICE_DEBUG("publishing | roll: %8.4f pitch: %8.4f yaw: %8.4f", (double)roll, (double)pitch, (double)yaw); /* fill in the final control values */ controls.timestamp = hrt_absolute_time(); diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 2158af5bfc..41947f27e7 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -279,7 +279,7 @@ GPS::task_main() _serial_fd = ::open(_port, O_RDWR); if (_serial_fd < 0) { - log("failed to open serial port: %s err: %d", _port, errno); + DEVICE_LOG("failed to open serial port: %s err: %d", _port, errno); /* tell the dtor that we are exiting, set error code */ _task = -1; _exit(1); diff --git a/src/drivers/hil/hil.cpp b/src/drivers/hil/hil.cpp index b6586856cc..dcf0233508 100644 --- a/src/drivers/hil/hil.cpp +++ b/src/drivers/hil/hil.cpp @@ -237,7 +237,7 @@ HIL::init() ///* try to claim the generic PWM output device node as well - it's OK if we fail at this */ //ret = register_driver(PWM_OUTPUT_DEVICE_PATH, &fops, 0666, (void *)this); if (ret == OK) { - log("default PWM output device"); + DEVICE_LOG("default PWM output device"); _primary_pwm_device = true; } @@ -253,7 +253,7 @@ HIL::init() nullptr); if (_task < 0) { - debug("task start failed: %d", errno); + DEVICE_DEBUG("task start failed: %d", errno); return -errno; } @@ -278,42 +278,42 @@ HIL::set_mode(Mode mode) */ switch (mode) { case MODE_2PWM: - debug("MODE_2PWM"); + DEVICE_DEBUG("MODE_2PWM"); /* multi-port with flow control lines as PWM */ _update_rate = 50; /* default output rate */ _num_outputs = 2; break; case MODE_4PWM: - debug("MODE_4PWM"); + DEVICE_DEBUG("MODE_4PWM"); /* multi-port as 4 PWM outs */ _update_rate = 50; /* default output rate */ _num_outputs = 4; break; case MODE_8PWM: - debug("MODE_8PWM"); + DEVICE_DEBUG("MODE_8PWM"); /* multi-port as 8 PWM outs */ _update_rate = 50; /* default output rate */ _num_outputs = 8; break; case MODE_12PWM: - debug("MODE_12PWM"); + DEVICE_DEBUG("MODE_12PWM"); /* multi-port as 12 PWM outs */ _update_rate = 50; /* default output rate */ _num_outputs = 12; break; case MODE_16PWM: - debug("MODE_16PWM"); + DEVICE_DEBUG("MODE_16PWM"); /* multi-port as 16 PWM outs */ _update_rate = 50; /* default output rate */ _num_outputs = 16; break; case MODE_NONE: - debug("MODE_NONE"); + DEVICE_DEBUG("MODE_NONE"); /* disable servo outputs and set a very low update rate */ _update_rate = 10; _num_outputs = 0; @@ -389,7 +389,7 @@ HIL::task_main() break; } - log("starting"); + DEVICE_LOG("starting"); /* loop until killed */ while (!_task_should_exit) { @@ -409,7 +409,7 @@ HIL::task_main() /* this would be bad... */ if (ret < 0) { - log("poll error %d", errno); + DEVICE_LOG("poll error %d", errno); continue; } @@ -504,7 +504,7 @@ HIL::ioctl(device::file_t *filp, int cmd, unsigned long arg) break; default: ret = -ENOTTY; - debug("not in a PWM mode"); + DEVICE_DEBUG("not in a PWM mode"); break; } @@ -712,7 +712,7 @@ HIL::pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg) ret = _mixers->load_from_buf(buf, buflen); if (ret != 0) { - debug("mixer load failed with %d", ret); + DEVICE_DEBUG("mixer load failed with %d", ret); delete _mixers; _mixers = nullptr; ret = -EINVAL; diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 794ba760b6..77f26e0ca4 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -418,7 +418,7 @@ HMC5883::init() ret = CDev::init(); if (ret != OK) { - debug("CDev init failed"); + DEVICE_DEBUG("CDev init failed"); goto out; } @@ -734,7 +734,7 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg) return check_calibration(); case MAGIOCGEXTERNAL: - debug("MAGIOCGEXTERNAL in main driver"); + DEVICE_DEBUG("MAGIOCGEXTERNAL in main driver"); return _interface->ioctl(cmd, dummy); case MAGIOCSTEMPCOMP: @@ -789,7 +789,7 @@ HMC5883::cycle() /* perform collection */ if (OK != collect()) { - debug("collection error"); + DEVICE_DEBUG("collection error"); /* restart the measurement state machine */ start(); return; @@ -816,7 +816,7 @@ HMC5883::cycle() /* measurement phase */ if (OK != measure()) - debug("measure error"); + DEVICE_DEBUG("measure error"); /* next phase is collection */ _collect_phase = true; @@ -886,7 +886,7 @@ HMC5883::collect() if (ret != OK) { perf_count(_comms_errors); - debug("data/status read error"); + DEVICE_DEBUG("data/status read error"); goto out; } @@ -936,7 +936,7 @@ HMC5883::collect() and can't do temperature. Disable it */ _temperature_error_count = 0; - debug("disabling temperature compensation"); + DEVICE_DEBUG("disabling temperature compensation"); set_temperature_compensation(0); } } @@ -994,7 +994,7 @@ HMC5883::collect() &_orb_class_instance, (sensor_is_onboard) ? ORB_PRIO_HIGH : ORB_PRIO_MAX); if (_mag_topic == nullptr) - debug("ADVERT FAIL"); + DEVICE_DEBUG("ADVERT FAIL"); } } diff --git a/src/drivers/hmc5883/hmc5883_i2c.cpp b/src/drivers/hmc5883/hmc5883_i2c.cpp index 880610bfd9..6d5d36864b 100644 --- a/src/drivers/hmc5883/hmc5883_i2c.cpp +++ b/src/drivers/hmc5883/hmc5883_i2c.cpp @@ -145,7 +145,7 @@ HMC5883_I2C::probe() if (read(ADDR_ID_A, &data[0], 1) || read(ADDR_ID_B, &data[1], 1) || read(ADDR_ID_C, &data[2], 1)) { - debug("read_reg fail"); + DEVICE_DEBUG("read_reg fail"); return -EIO; } @@ -154,7 +154,7 @@ HMC5883_I2C::probe() if ((data[0] != ID_A_WHO_AM_I) || (data[1] != ID_B_WHO_AM_I) || (data[2] != ID_C_WHO_AM_I)) { - debug("ID byte mismatch (%02x,%02x,%02x)", data[0], data[1], data[2]); + DEVICE_DEBUG("ID byte mismatch (%02x,%02x,%02x)", data[0], data[1], data[2]); return -EIO; } diff --git a/src/drivers/hmc5883/hmc5883_spi.cpp b/src/drivers/hmc5883/hmc5883_spi.cpp index 0eec3d8d18..2aae792f7c 100644 --- a/src/drivers/hmc5883/hmc5883_spi.cpp +++ b/src/drivers/hmc5883/hmc5883_spi.cpp @@ -107,7 +107,7 @@ HMC5883_SPI::init() ret = SPI::init(); if (ret != OK) { - debug("SPI init failed"); + DEVICE_DEBUG("SPI init failed"); return -EIO; } @@ -117,13 +117,13 @@ HMC5883_SPI::init() if (read(ADDR_ID_A, &data[0], 1) || read(ADDR_ID_B, &data[1], 1) || read(ADDR_ID_C, &data[2], 1)) { - debug("read_reg fail"); + DEVICE_DEBUG("read_reg fail"); } if ((data[0] != ID_A_WHO_AM_I) || (data[1] != ID_B_WHO_AM_I) || (data[2] != ID_C_WHO_AM_I)) { - debug("ID byte mismatch (%02x,%02x,%02x)", data[0], data[1], data[2]); + DEVICE_DEBUG("ID byte mismatch (%02x,%02x,%02x)", data[0], data[1], data[2]); return -EIO; } diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 5960414e36..7dcd5a0004 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -493,7 +493,7 @@ L3GD20::init() &_orb_class_instance, (is_external()) ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT); if (_gyro_topic == nullptr) { - debug("failed to create sensor_gyro publication"); + DEVICE_DEBUG("failed to create sensor_gyro publication"); } ret = OK; @@ -875,7 +875,7 @@ L3GD20::disable_i2c(void) return; } } - debug("FAILED TO DISABLE I2C"); + DEVICE_DEBUG("FAILED TO DISABLE I2C"); } void diff --git a/src/drivers/led/led.cpp b/src/drivers/led/led.cpp index 69c72b5b53..7ff53fc423 100644 --- a/src/drivers/led/led.cpp +++ b/src/drivers/led/led.cpp @@ -89,7 +89,7 @@ LED::~LED() int LED::init() { - debug("LED::init"); + DEVICE_DEBUG("LED::init"); #ifdef __PX4_NUTTX CDev::init(); #else diff --git a/src/drivers/ll40ls/LidarLiteI2C.cpp b/src/drivers/ll40ls/LidarLiteI2C.cpp index f3fb0dda45..fb13203516 100644 --- a/src/drivers/ll40ls/LidarLiteI2C.cpp +++ b/src/drivers/ll40ls/LidarLiteI2C.cpp @@ -133,7 +133,7 @@ int LidarLiteI2C::init() &_orb_class_instance, ORB_PRIO_LOW); if (_distance_sensor_topic == nullptr) { - debug("failed to create distance_sensor object. Did you start uOrb?"); + DEVICE_DEBUG("failed to create distance_sensor object. Did you start uOrb?"); } } @@ -177,7 +177,7 @@ int LidarLiteI2C::probe() goto ok; } - debug("probe failed reg11=0x%02x reg2=0x%02x\n", + DEVICE_DEBUG("probe failed reg11=0x%02x reg2=0x%02x\n", (unsigned)who_am_i, (unsigned)max_acq_count); } @@ -307,7 +307,7 @@ int LidarLiteI2C::measure() if (OK != ret) { perf_count(_comms_errors); - debug("i2c::transfer returned %d", ret); + DEVICE_DEBUG("i2c::transfer returned %d", ret); // if we are getting lots of I2C transfer errors try // resetting the sensor @@ -387,7 +387,7 @@ int LidarLiteI2C::collect() read before it is ready, so only consider it an error if more than 100ms has elapsed. */ - debug("error reading from sensor: %d", ret); + DEVICE_DEBUG("error reading from sensor: %d", ret); perf_count(_comms_errors); if (perf_event_count(_comms_errors) % 10 == 0) { @@ -488,7 +488,7 @@ void LidarLiteI2C::cycle() /* try a collection */ if (OK != collect()) { - debug("collection error"); + DEVICE_DEBUG("collection error"); /* if we've been waiting more than 200ms then send a new acquire */ @@ -520,7 +520,7 @@ void LidarLiteI2C::cycle() if (_collect_phase == false) { /* measurement phase */ if (OK != measure()) { - debug("measure error"); + DEVICE_DEBUG("measure error"); } else { /* next phase is collection. Don't switch to diff --git a/src/drivers/ll40ls/LidarLitePWM.cpp b/src/drivers/ll40ls/LidarLitePWM.cpp index 406aa111b8..3d17d5b36b 100644 --- a/src/drivers/ll40ls/LidarLitePWM.cpp +++ b/src/drivers/ll40ls/LidarLitePWM.cpp @@ -117,7 +117,7 @@ int LidarLitePWM::init() &_orb_class_instance, ORB_PRIO_LOW); if (_distance_sensor_topic == nullptr) { - debug("failed to create distance_sensor object. Did you start uOrb?"); + DEVICE_DEBUG("failed to create distance_sensor object. Did you start uOrb?"); } } @@ -175,7 +175,7 @@ int LidarLitePWM::measure() perf_begin(_sample_perf); if (OK != collect()) { - debug("collection error"); + DEVICE_DEBUG("collection error"); perf_count(_read_errors); perf_end(_sample_perf); return ERROR; diff --git a/src/drivers/mb12xx/mb12xx.cpp b/src/drivers/mb12xx/mb12xx.cpp index 45ce7200ba..ecd2ea04a6 100644 --- a/src/drivers/mb12xx/mb12xx.cpp +++ b/src/drivers/mb12xx/mb12xx.cpp @@ -277,7 +277,7 @@ MB12XX::init() &_orb_class_instance, ORB_PRIO_LOW); if (_distance_sensor_topic == nullptr) { - log("failed to create distance_sensor object. Did you start uOrb?"); + DEVICE_LOG("failed to create distance_sensor object. Did you start uOrb?"); } } @@ -294,7 +294,7 @@ MB12XX::init() if (ret2 == 0) { /* sonar is present -> store address_index in array */ addr_ind.push_back(_index_counter); - debug("sonar added"); + DEVICE_DEBUG("sonar added"); _latest_sonar_measurements.push_back(200); } } @@ -312,10 +312,10 @@ MB12XX::init() /* show the connected sonars in terminal */ for (unsigned i = 0; i < addr_ind.size(); i++) { - log("sonar %d with address %d added", (i + 1), addr_ind[i]); + DEVICE_LOG("sonar %d with address %d added", (i + 1), addr_ind[i]); } - debug("Number of sonars connected: %d", addr_ind.size()); + DEVICE_DEBUG("Number of sonars connected: %d", addr_ind.size()); ret = OK; /* sensor is ok, but we don't really know if it is within range */ @@ -545,7 +545,7 @@ MB12XX::measure() if (OK != ret) { perf_count(_comms_errors); - debug("i2c::transfer returned %d", ret); + DEVICE_DEBUG("i2c::transfer returned %d", ret); return ret; } @@ -567,7 +567,7 @@ MB12XX::collect() ret = transfer(nullptr, 0, &val[0], 2); if (ret < 0) { - debug("error reading from sensor: %d", ret); + DEVICE_DEBUG("error reading from sensor: %d", ret); perf_count(_comms_errors); perf_end(_sample_perf); return ret; @@ -660,7 +660,7 @@ MB12XX::cycle() /* perform collection */ if (OK != collect()) { - debug("collection error"); + DEVICE_DEBUG("collection error"); /* if error restart the measurement state machine */ start(); return; @@ -699,7 +699,7 @@ MB12XX::cycle() /* Perform measurement */ if (OK != measure()) { - debug("measure error sonar adress %d", _index_counter); + DEVICE_DEBUG("measure error sonar adress %d", _index_counter); } /* next phase is collection */ diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index 7b42f82061..a72bba77a2 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -305,7 +305,7 @@ MEASAirspeed::cycle() /* measurement phase */ ret = measure(); if (OK != ret) { - debug("measure error"); + DEVICE_DEBUG("measure error"); } _sensor_ok = (ret == OK); diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index 3448b570c9..de37f0d089 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -295,7 +295,7 @@ MK::init(unsigned motors) ret = register_driver(_device, &fops, 0666, (void *)this); if (ret == OK) { - log("creating alternate output device"); + DEVICE_LOG("creating alternate output device"); _primary_pwm_device = true; } @@ -311,7 +311,7 @@ MK::init(unsigned motors) if (_task < 0) { - debug("task start failed: %d", errno); + DEVICE_DEBUG("task start failed: %d", errno); return -errno; } @@ -499,7 +499,7 @@ MK::task_main() up_pwm_servo_set_rate(_update_rate); /* unnecessary ? */ - log("starting"); + DEVICE_LOG("starting"); /* loop until killed */ while (!_task_should_exit) { @@ -516,7 +516,7 @@ MK::task_main() /* this would be bad... */ if (ret < 0) { - log("poll error %d", errno); + DEVICE_LOG("poll error %d", errno); usleep(1000000); continue; } @@ -648,7 +648,7 @@ MK::task_main() /* make sure servos are off */ up_pwm_servo_deinit(); - log("stopping"); + DEVICE_LOG("stopping"); /* note - someone else is responsible for restoring the GPIO config */ @@ -1075,7 +1075,7 @@ MK::pwm_ioctl(file *filp, int cmd, unsigned long arg) ret = _mixers->load_from_buf(buf, buflen); if (ret != 0) { - debug("mixer load failed with %d", ret); + DEVICE_DEBUG("mixer load failed with %d", ret); delete _mixers; _mixers = nullptr; ret = -EINVAL; diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 818a045a9f..288ad579ab 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -606,7 +606,7 @@ MPU6000::init() /* if probe/setup failed, bail now */ if (ret != OK) { - debug("SPI setup failed"); + DEVICE_DEBUG("SPI setup failed"); return ret; } @@ -642,7 +642,7 @@ MPU6000::init() ret = _gyro->init(); /* if probe/setup failed, bail now */ if (ret != OK) { - debug("gyro init failed"); + DEVICE_DEBUG("gyro init failed"); return ret; } @@ -773,12 +773,12 @@ MPU6000::probe() case MPU6000_REV_D8: case MPU6000_REV_D9: case MPU6000_REV_D10: - debug("ID 0x%02x", _product); + DEVICE_DEBUG("ID 0x%02x", _product); _checked_values[0] = _product; return OK; } - debug("unexpected ID 0x%02x", _product); + DEVICE_DEBUG("unexpected ID 0x%02x", _product); return -EIO; } @@ -1873,7 +1873,7 @@ MPU6000_gyro::init() /* if probe/setup failed, bail now */ if (ret != OK) { - debug("gyro init failed"); + DEVICE_DEBUG("gyro init failed"); return ret; } diff --git a/src/drivers/ms5611/ms5611_nuttx.cpp b/src/drivers/ms5611/ms5611_nuttx.cpp index 76a4213f5b..4a304e130b 100644 --- a/src/drivers/ms5611/ms5611_nuttx.cpp +++ b/src/drivers/ms5611/ms5611_nuttx.cpp @@ -266,7 +266,7 @@ MS5611::init() ret = CDev::init(); if (ret != OK) { - debug("CDev init failed"); + DEVICE_DEBUG("CDev init failed"); goto out; } @@ -274,7 +274,7 @@ MS5611::init() _reports = new ringbuffer::RingBuffer(2, sizeof(baro_report)); if (_reports == nullptr) { - debug("can't get memory for reports"); + DEVICE_DEBUG("can't get memory for reports"); ret = -ENOMEM; goto out; } @@ -562,7 +562,7 @@ MS5611::cycle() * spam the console with a message for this. */ } else { - //log("collection error %d", ret); + //DEVICE_LOG("collection error %d", ret); } /* issue a reset command to the sensor */ _interface->ioctl(IOCTL_RESET, dummy); diff --git a/src/drivers/ms5611/ms5611_posix.cpp b/src/drivers/ms5611/ms5611_posix.cpp index 2b675af175..8225ec007a 100644 --- a/src/drivers/ms5611/ms5611_posix.cpp +++ b/src/drivers/ms5611/ms5611_posix.cpp @@ -264,7 +264,7 @@ MS5611::init() ret = VDev::init(); if (ret != OK) { - debug("VDev init failed"); + DEVICE_DEBUG("VDev init failed"); goto out; } @@ -272,7 +272,7 @@ MS5611::init() _reports = new RingBuffer(2, sizeof(baro_report)); if (_reports == nullptr) { - debug("can't get memory for reports"); + DEVICE_DEBUG("can't get memory for reports"); ret = -ENOMEM; goto out; } @@ -561,7 +561,7 @@ MS5611::cycle() * spam the console with a message for this. */ } else { - //log("collection error %d", ret); + //DEVICE_LOG("collection error %d", ret); } /* issue a reset command to the sensor */ _interface->dev_ioctl(IOCTL_RESET, dummy); @@ -595,7 +595,7 @@ MS5611::cycle() /* measurement phase */ ret = measure(); if (ret != OK) { - //log("measure error %d", ret); + //DEVICE_LOG("measure error %d", ret); /* issue a reset command to the sensor */ _interface->dev_ioctl(IOCTL_RESET, dummy); /* reset the collection state machine and try again */ diff --git a/src/drivers/ms5611/ms5611_spi.cpp b/src/drivers/ms5611/ms5611_spi.cpp index 7ec7718157..37c9fb4c12 100644 --- a/src/drivers/ms5611/ms5611_spi.cpp +++ b/src/drivers/ms5611/ms5611_spi.cpp @@ -44,7 +44,6 @@ #include #include #include -//#include #include #include @@ -146,21 +145,21 @@ MS5611_SPI::init() ret = SPI::init(); if (ret != OK) { - debug("SPI init failed"); + DEVICE_DEBUG("SPI init failed"); goto out; } /* send reset command */ ret = _reset(); if (ret != OK) { - debug("reset failed"); + DEVICE_DEBUG("reset failed"); goto out; } /* read PROM */ ret = _read_prom(); if (ret != OK) { - debug("prom readout failed"); + DEVICE_DEBUG("prom readout failed"); goto out; } @@ -251,16 +250,16 @@ MS5611_SPI::_read_prom() _prom.c[i] = _reg16(cmd); if (_prom.c[i] != 0) all_zero = false; - //debug("prom[%u]=0x%x", (unsigned)i, (unsigned)_prom.c[i]); + //DEVICE_DEBUG("prom[%u]=0x%x", (unsigned)i, (unsigned)_prom.c[i]); } /* calculate CRC and return success/failure accordingly */ int ret = ms5611::crc4(&_prom.c[0]) ? OK : -EIO; if (ret != OK) { - debug("crc failed"); + DEVICE_DEBUG("crc failed"); } if (all_zero) { - debug("prom all zero"); + DEVICE_DEBUG("prom all zero"); ret = -EIO; } return ret; diff --git a/src/drivers/oreoled/oreoled.cpp b/src/drivers/oreoled/oreoled.cpp index 3918a6575e..5ee8dc030b 100644 --- a/src/drivers/oreoled/oreoled.cpp +++ b/src/drivers/oreoled/oreoled.cpp @@ -202,10 +202,10 @@ OREOLED::info() /* print health info on each LED */ for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { if (!_healthy[i]) { - log("oreo %u: BAD", (int)i); + DEVICE_LOG("oreo %u: BAD", (int)i); } else { - log("oreo %u: OK", (int)i); + DEVICE_LOG("oreo %u: OK", (int)i); } } diff --git a/src/drivers/pca9685/pca9685.cpp b/src/drivers/pca9685/pca9685.cpp index c26e1b8fed..7f0a3de031 100644 --- a/src/drivers/pca9685/pca9685.cpp +++ b/src/drivers/pca9685/pca9685.cpp @@ -326,7 +326,7 @@ PCA9685::i2cpwm() * the control[i] values are on the range -1 ... 1 */ uint16_t new_value = PCA9685_PWMCENTER + (_actuator_controls.control[i] * M_PI_F * PCA9685_SCALE); - debug("%d: current: %u, new %u, control %.2f", i, _current_values[i], new_value, + DEVICE_DEBUG("%d: current: %u, new %u, control %.2f", i, _current_values[i], new_value, (double)_actuator_controls.control[i]); if (new_value != _current_values[i] && isfinite(new_value) && @@ -368,7 +368,7 @@ PCA9685::setPWM(uint8_t num, uint16_t on, uint16_t off) if (OK != ret) { perf_count(_comms_errors); - log("i2c::transfer returned %d", ret); + DEVICE_LOG("i2c::transfer returned %d", ret); } return ret; @@ -469,7 +469,7 @@ PCA9685::read8(uint8_t addr, uint8_t &value) fail_read: perf_count(_comms_errors); - log("i2c::transfer returned %d", ret); + DEVICE_LOG("i2c::transfer returned %d", ret); return ret; } @@ -489,7 +489,7 @@ PCA9685::write8(uint8_t addr, uint8_t value) { ret = transfer(_msg, 2, nullptr, 0); if (ret != OK) { perf_count(_comms_errors); - log("i2c::transfer returned %d", ret); + DEVICE_LOG("i2c::transfer returned %d", ret); } return ret; } diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index 9712bb1ab5..db0f45ec1f 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -245,7 +245,7 @@ PX4FLOW::init() &_orb_class_instance, ORB_PRIO_HIGH); if (_distance_sensor_topic == nullptr) { - log("failed to create distance_sensor object. Did you start uOrb?"); + DEVICE_LOG("failed to create distance_sensor object. Did you start uOrb?"); } } @@ -452,7 +452,7 @@ PX4FLOW::measure() if (OK != ret) { perf_count(_comms_errors); - debug("i2c::transfer returned %d", ret); + DEVICE_DEBUG("i2c::transfer returned %d", ret); return ret; } @@ -480,7 +480,7 @@ PX4FLOW::collect() } if (ret < 0) { - debug("error reading from sensor: %d", ret); + DEVICE_DEBUG("error reading from sensor: %d", ret); perf_count(_comms_errors); perf_end(_sample_perf); return ret; @@ -598,12 +598,12 @@ void PX4FLOW::cycle() { if (OK != measure()) { - debug("measure error"); + DEVICE_DEBUG("measure error"); } /* perform collection */ if (OK != collect()) { - debug("collection error"); + DEVICE_DEBUG("collection error"); /* restart the measurement state machine */ start(); return; diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index ed6d7f36d5..19cb4cafce 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -359,7 +359,7 @@ PX4FMU::init() nullptr); if (_task < 0) { - debug("task start failed: %d", errno); + DEVICE_DEBUG("task start failed: %d", errno); return -errno; } @@ -384,7 +384,7 @@ PX4FMU::set_mode(Mode mode) */ switch (mode) { case MODE_2PWM: // v1 multi-port with flow control lines as PWM - debug("MODE_2PWM"); + DEVICE_DEBUG("MODE_2PWM"); /* default output rates */ _pwm_default_rate = 50; @@ -398,7 +398,7 @@ PX4FMU::set_mode(Mode mode) break; case MODE_4PWM: // v1 multi-port as 4 PWM outs - debug("MODE_4PWM"); + DEVICE_DEBUG("MODE_4PWM"); /* default output rates */ _pwm_default_rate = 50; @@ -412,7 +412,7 @@ PX4FMU::set_mode(Mode mode) break; case MODE_6PWM: // v2 PWMs as 6 PWM outs - debug("MODE_6PWM"); + DEVICE_DEBUG("MODE_6PWM"); /* default output rates */ _pwm_default_rate = 50; @@ -427,7 +427,7 @@ PX4FMU::set_mode(Mode mode) #ifdef CONFIG_ARCH_BOARD_AEROCORE case MODE_8PWM: // AeroCore PWMs as 8 PWM outs - debug("MODE_8PWM"); + DEVICE_DEBUG("MODE_8PWM"); /* default output rates */ _pwm_default_rate = 50; _pwm_alt_rate = 50; @@ -440,7 +440,7 @@ PX4FMU::set_mode(Mode mode) #endif case MODE_NONE: - debug("MODE_NONE"); + DEVICE_DEBUG("MODE_NONE"); _pwm_default_rate = 10; /* artificially reduced output rate */ _pwm_alt_rate = 10; @@ -462,7 +462,7 @@ PX4FMU::set_mode(Mode mode) int PX4FMU::set_pwm_rate(uint32_t rate_map, unsigned default_rate, unsigned alt_rate) { - debug("set_pwm_rate %x %u %u", rate_map, default_rate, alt_rate); + DEVICE_DEBUG("set_pwm_rate %x %u %u", rate_map, default_rate, alt_rate); for (unsigned pass = 0; pass < 2; pass++) { for (unsigned group = 0; group < _max_actuators; group++) { @@ -536,11 +536,11 @@ PX4FMU::subscribe() _poll_fds_num = 0; for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { if (sub_groups & (1 << i)) { - debug("subscribe to actuator_controls_%d", i); + DEVICE_DEBUG("subscribe to actuator_controls_%d", i); _control_subs[i] = orb_subscribe(_control_topics[i]); } if (unsub_groups & (1 << i)) { - debug("unsubscribe from actuator_controls_%d", i); + DEVICE_DEBUG("unsubscribe from actuator_controls_%d", i); ::close(_control_subs[i]); _control_subs[i] = -1; } @@ -646,7 +646,7 @@ PX4FMU::task_main() update_rate_in_ms = 100; } - debug("adjusted actuator update interval to %ums", update_rate_in_ms); + DEVICE_DEBUG("adjusted actuator update interval to %ums", update_rate_in_ms); for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { if (_control_subs[i] > 0) { orb_set_interval(_control_subs[i], update_rate_in_ms); @@ -663,7 +663,7 @@ PX4FMU::task_main() /* this would be bad... */ if (ret < 0) { - log("poll error %d", errno); + DEVICE_LOG("poll error %d", errno); continue; } else if (ret == 0) { @@ -815,7 +815,7 @@ PX4FMU::task_main() /* make sure servos are off */ up_pwm_servo_deinit(); - log("stopping"); + DEVICE_LOG("stopping"); /* note - someone else is responsible for restoring the GPIO config */ @@ -888,7 +888,7 @@ PX4FMU::ioctl(file *filp, int cmd, unsigned long arg) break; default: - debug("not in a PWM mode"); + DEVICE_DEBUG("not in a PWM mode"); break; } @@ -1309,7 +1309,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) ret = _mixers->load_from_buf(buf, buflen); if (ret != 0) { - debug("mixer load failed with %d", ret); + DEVICE_DEBUG("mixer load failed with %d", ret); delete _mixers; _mixers = nullptr; _groups_required = 0; diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 1259daceb1..cdfce1e9c8 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -592,10 +592,10 @@ PX4IO::detect() if (protocol != PX4IO_PROTOCOL_VERSION) { if (protocol == _io_reg_get_error) { - log("IO not installed"); + DEVICE_LOG("IO not installed"); } else { - log("IO version error"); + DEVICE_LOG("IO version error"); mavlink_log_emergency(_mavlink_fd, "IO VERSION MISMATCH, PLEASE UPGRADE SOFTWARE!"); } @@ -603,7 +603,7 @@ PX4IO::detect() } } - log("IO found"); + DEVICE_LOG("IO found"); return 0; } @@ -667,7 +667,7 @@ PX4IO::init() (_max_transfer < 16) || (_max_transfer > 255) || (_max_rc_input < 1) || (_max_rc_input > 255)) { - log("config read error"); + DEVICE_LOG("config read error"); mavlink_log_emergency(_mavlink_fd, "[IO] config read fail, abort."); return -1; } @@ -796,7 +796,7 @@ PX4IO::init() /* re-send if necessary */ if (!safety.armed) { orb_publish(ORB_ID(vehicle_command), pub, &cmd); - log("re-sending arm cmd"); + DEVICE_LOG("re-sending arm cmd"); } /* keep waiting for state change for 2 s */ @@ -822,7 +822,7 @@ PX4IO::init() ret = io_disable_rc_handling(); if (ret != OK) { - log("failed disabling RC handling"); + DEVICE_LOG("failed disabling RC handling"); return ret; } @@ -851,7 +851,7 @@ PX4IO::init() ret = register_driver(PWM_OUTPUT0_DEVICE_PATH, &fops, 0666, (void *)this); if (ret == OK) { - log("default PWM output device"); + DEVICE_LOG("default PWM output device"); _primary_pwm_device = true; } @@ -864,7 +864,7 @@ PX4IO::init() nullptr); if (_task < 0) { - debug("task start failed: %d", errno); + DEVICE_DEBUG("task start failed: %d", errno); return -errno; } @@ -1162,7 +1162,7 @@ PX4IO::task_main() unlock(); out: - debug("exiting"); + DEVICE_DEBUG("exiting"); /* clean up the alternate device node */ if (_primary_pwm_device) @@ -1473,7 +1473,7 @@ PX4IO::io_set_rc_config() ret = io_reg_set(PX4IO_PAGE_RC_CONFIG, offset, regs, PX4IO_P_RC_CONFIG_STRIDE); if (ret != OK) { - log("rc config upload failed"); + DEVICE_LOG("rc config upload failed"); break; } @@ -1851,14 +1851,14 @@ PX4IO::io_reg_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned { /* range check the transfer */ if (num_values > ((_max_transfer) / sizeof(*values))) { - debug("io_reg_set: too many registers (%u, max %u)", num_values, _max_transfer / 2); + DEVICE_DEBUG("io_reg_set: too many registers (%u, max %u)", num_values, _max_transfer / 2); return -EINVAL; } int ret = _interface->write((page << 8) | offset, (void *)values, num_values); if (ret != (int)num_values) { - debug("io_reg_set(%u,%u,%u): error %d", page, offset, num_values, ret); + DEVICE_DEBUG("io_reg_set(%u,%u,%u): error %d", page, offset, num_values, ret); return -1; } @@ -1876,14 +1876,14 @@ PX4IO::io_reg_get(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_v { /* range check the transfer */ if (num_values > ((_max_transfer) / sizeof(*values))) { - debug("io_reg_get: too many registers (%u, max %u)", num_values, _max_transfer / 2); + DEVICE_DEBUG("io_reg_get: too many registers (%u, max %u)", num_values, _max_transfer / 2); return -EINVAL; } int ret = _interface->read((page << 8) | offset, reinterpret_cast(values), num_values); if (ret != (int)num_values) { - debug("io_reg_get(%u,%u,%u): data error %d", page, offset, num_values, ret); + DEVICE_DEBUG("io_reg_get(%u,%u,%u): data error %d", page, offset, num_values, ret); return -1; } @@ -2030,7 +2030,7 @@ PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries) } if (ret) { - log("mixer send error %d", ret); + DEVICE_LOG("mixer send error %d", ret); return ret; } @@ -2060,7 +2060,7 @@ PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries) retries--; - log("mixer sent"); + DEVICE_LOG("mixer sent"); } while (retries > 0 && (!(io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS) & PX4IO_P_STATUS_FLAGS_MIXER_OK))); @@ -2070,7 +2070,7 @@ PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries) return 0; } - log("mixer rejected by IO"); + DEVICE_LOG("mixer rejected by IO"); mavlink_log_info(_mavlink_fd, "[IO] mixer upload fail"); /* load must have failed for some reason */ @@ -2702,7 +2702,7 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg) if (ret != OK) return ret; if (io_crc != arg) { - debug("crc mismatch 0x%08x 0x%08x", (unsigned)io_crc, arg); + DEVICE_DEBUG("crc mismatch 0x%08x 0x%08x", (unsigned)io_crc, arg); return -EINVAL; } break; diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp index 6491253017..8b5be151de 100644 --- a/src/drivers/rgbled/rgbled.cpp +++ b/src/drivers/rgbled/rgbled.cpp @@ -212,8 +212,8 @@ RGBLED::info() if (ret == OK) { /* we don't care about power-save mode */ - log("state: %s", on ? "ON" : "OFF"); - log("red: %u, green: %u, blue: %u", (unsigned)r, (unsigned)g, (unsigned)b); + DEVICE_LOG("state: %s", on ? "ON" : "OFF"); + DEVICE_LOG("red: %u, green: %u, blue: %u", (unsigned)r, (unsigned)g, (unsigned)b); } else { warnx("failed to read led"); diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp index b331aff08b..ec5795025c 100644 --- a/src/drivers/sf0x/sf0x.cpp +++ b/src/drivers/sf0x/sf0x.cpp @@ -301,7 +301,7 @@ SF0X::init() &_orb_class_instance, ORB_PRIO_HIGH); if (_distance_sensor_topic == nullptr) { - log("failed to create distance_sensor object. Did you start uOrb?"); + DEVICE_LOG("failed to create distance_sensor object. Did you start uOrb?"); } } @@ -532,7 +532,7 @@ SF0X::measure() if (ret != sizeof(cmd)) { perf_count(_comms_errors); - log("write fail %d", ret); + DEVICE_LOG("write fail %d", ret); return ret; } @@ -559,7 +559,7 @@ SF0X::collect() ret = ::read(_fd, &readbuf[0], readlen); if (ret < 0) { - debug("read err: %d", ret); + DEVICE_DEBUG("read err: %d", ret); perf_count(_comms_errors); perf_end(_sample_perf); @@ -589,7 +589,7 @@ SF0X::collect() return -EAGAIN; } - debug("val (float): %8.4f, raw: %s, valid: %s", (double)distance_m, _linebuf, ((valid) ? "OK" : "NO")); + DEVICE_DEBUG("val (float): %8.4f, raw: %s, valid: %s", (double)distance_m, _linebuf, ((valid) ? "OK" : "NO")); struct distance_sensor_s report; @@ -689,7 +689,7 @@ SF0X::cycle() /* we know the sensor needs about four seconds to initialize */ if (hrt_absolute_time() > 5 * 1000 * 1000LL && _consecutive_fail_count < 5) { - log("collection error #%u", _consecutive_fail_count); + DEVICE_LOG("collection error #%u", _consecutive_fail_count); } _consecutive_fail_count++; @@ -722,7 +722,7 @@ SF0X::cycle() /* measurement phase */ if (OK != measure()) { - log("measure error"); + DEVICE_LOG("measure error"); } /* next phase is collection */ diff --git a/src/drivers/stm32/adc/adc.cpp b/src/drivers/stm32/adc/adc.cpp index 77b6c247b1..a6be9d5c24 100644 --- a/src/drivers/stm32/adc/adc.cpp +++ b/src/drivers/stm32/adc/adc.cpp @@ -233,13 +233,13 @@ ADC::init() /* don't wait for more than 500us, since that means something broke - should reset here if we see this */ if ((hrt_absolute_time() - now) > 500) { - log("sample timeout"); + DEVICE_LOG("sample timeout"); return -1; } } - debug("init done"); + DEVICE_DEBUG("init done"); /* create the device node */ return CDev::init(); @@ -356,7 +356,7 @@ ADC::_sample(unsigned channel) /* don't wait for more than 50us, since that means something broke - should reset here if we see this */ if ((hrt_absolute_time() - now) > 50) { - log("sample timeout"); + DEVICE_LOG("sample timeout"); return 0xffff; } } diff --git a/src/drivers/stm32/tone_alarm/tone_alarm.cpp b/src/drivers/stm32/tone_alarm/tone_alarm.cpp index 98804a4840..17f4c9362b 100644 --- a/src/drivers/stm32/tone_alarm/tone_alarm.cpp +++ b/src/drivers/stm32/tone_alarm/tone_alarm.cpp @@ -398,7 +398,7 @@ ToneAlarm::init() /* make sure the timer is running */ rCR1 = GTIM_CR1_CEN; - debug("ready"); + DEVICE_DEBUG("ready"); return OK; } @@ -741,14 +741,14 @@ ToneAlarm::ioctl(file *filp, int cmd, unsigned long arg) { int result = OK; - debug("ioctl %i %u", cmd, arg); + DEVICE_DEBUG("ioctl %i %u", cmd, arg); // irqstate_t flags = irqsave(); /* decide whether to increase the alarm level to cmd or leave it alone */ switch (cmd) { case TONE_SET_ALARM: - debug("TONE_SET_ALARM %u", arg); + DEVICE_DEBUG("TONE_SET_ALARM %u", arg); if (arg < TONE_NUMBER_OF_TUNES) { if (arg == TONE_STOP_TUNE) { diff --git a/src/drivers/trone/trone.cpp b/src/drivers/trone/trone.cpp index 606f37ba21..83834b7f84 100644 --- a/src/drivers/trone/trone.cpp +++ b/src/drivers/trone/trone.cpp @@ -304,7 +304,7 @@ TRONE::init() &_orb_class_instance, ORB_PRIO_LOW); if (_distance_sensor_topic == nullptr) { - log("failed to create distance_sensor object. Did you start uOrb?"); + DEVICE_LOG("failed to create distance_sensor object. Did you start uOrb?"); } } @@ -332,7 +332,7 @@ TRONE::probe() } } - debug("WHO_AM_I byte mismatch 0x%02x should be 0x%02x\n", + DEVICE_DEBUG("WHO_AM_I byte mismatch 0x%02x should be 0x%02x\n", (unsigned)who_am_i, TRONE_WHO_AM_I_REG_VAL); @@ -551,7 +551,7 @@ TRONE::measure() if (OK != ret) { perf_count(_comms_errors); - log("i2c::transfer returned %d", ret); + DEVICE_LOG("i2c::transfer returned %d", ret); return ret; } @@ -573,7 +573,7 @@ TRONE::collect() ret = transfer(nullptr, 0, &val[0], 3); if (ret < 0) { - log("error reading from sensor: %d", ret); + DEVICE_LOG("error reading from sensor: %d", ret); perf_count(_comms_errors); perf_end(_sample_perf); return ret; @@ -665,7 +665,7 @@ TRONE::cycle() /* perform collection */ if (OK != collect()) { - log("collection error"); + DEVICE_LOG("collection error"); /* restart the measurement state machine */ start(); return; @@ -691,7 +691,7 @@ TRONE::cycle() /* measurement phase */ if (OK != measure()) { - log("measure error"); + DEVICE_LOG("measure error"); } /* next phase is collection */ diff --git a/src/modules/uavcan/sensors/baro.cpp b/src/modules/uavcan/sensors/baro.cpp index ea7fa09bf7..98ff1e5a72 100644 --- a/src/modules/uavcan/sensors/baro.cpp +++ b/src/modules/uavcan/sensors/baro.cpp @@ -67,14 +67,14 @@ int UavcanBarometerBridge::init() res = _sub_air_pressure_data.start(AirPressureCbBinder(this, &UavcanBarometerBridge::air_pressure_sub_cb)); if (res < 0) { - log("failed to start uavcan sub: %d", res); + DEVICE_LOG("failed to start uavcan sub: %d", res); return res; } res = _sub_air_temperature_data.start(AirTemperatureCbBinder(this, &UavcanBarometerBridge::air_temperature_sub_cb)); if (res < 0) { - log("failed to start uavcan sub: %d", res); + DEVICE_LOG("failed to start uavcan sub: %d", res); return res; } @@ -111,7 +111,7 @@ int UavcanBarometerBridge::ioctl(struct file *filp, int cmd, unsigned long arg) return -EINVAL; } else { - log("new msl pressure %u", _msl_pressure); + DEVICE_LOG("new msl pressure %u", _msl_pressure); _msl_pressure = arg; return OK; } diff --git a/src/modules/uavcan/sensors/mag.cpp b/src/modules/uavcan/sensors/mag.cpp index 48c4d20dff..6ef94090c9 100644 --- a/src/modules/uavcan/sensors/mag.cpp +++ b/src/modules/uavcan/sensors/mag.cpp @@ -61,7 +61,7 @@ int UavcanMagnetometerBridge::init() res = _sub_mag.start(MagCbBinder(this, &UavcanMagnetometerBridge::mag_sub_cb)); if (res < 0) { - log("failed to start uavcan sub: %d", res); + DEVICE_LOG("failed to start uavcan sub: %d", res); return res; } return 0; diff --git a/src/modules/uavcan/sensors/sensor_bridge.cpp b/src/modules/uavcan/sensors/sensor_bridge.cpp index 75451ce848..fa97b3c89e 100644 --- a/src/modules/uavcan/sensors/sensor_bridge.cpp +++ b/src/modules/uavcan/sensors/sensor_bridge.cpp @@ -85,7 +85,7 @@ void UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report) return; // Give up immediately - saves some CPU time } - log("adding channel %d...", node_id); + DEVICE_LOG("adding channel %d...", node_id); // Search for the first free channel for (unsigned i = 0; i < _max_channels; i++) { @@ -98,7 +98,7 @@ void UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report) // No free channels left if (channel == nullptr) { _out_of_channels = true; - log("out of channels"); + DEVICE_LOG("out of channels"); return; } @@ -109,7 +109,7 @@ void UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report) const int class_instance = register_class_devname(_class_devname); if (class_instance < 0 || class_instance >= int(_max_channels)) { _out_of_channels = true; - log("out of class instances"); + DEVICE_LOG("out of class instances"); (void)unregister_class_devname(_class_devname, class_instance); return; } @@ -120,13 +120,13 @@ void UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report) channel->orb_advert = orb_advertise_multi(_orb_topic, report, &channel->orb_instance, ORB_PRIO_HIGH); if (channel->orb_advert == nullptr) { - log("ADVERTISE FAILED"); + DEVICE_LOG("ADVERTISE FAILED"); (void)unregister_class_devname(_class_devname, class_instance); *channel = Channel(); return; } - log("channel %d class instance %d ok", channel->node_id, channel->class_instance); + DEVICE_LOG("channel %d class instance %d ok", channel->node_id, channel->class_instance); } assert(channel != nullptr); diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index f51d60a8ab..63ccea49bc 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -489,7 +489,7 @@ int UavcanNode::run() // this would be bad... if (poll_ret < 0) { - log("poll error %d", errno); + DEVICE_LOG("poll error %d", errno); continue; } else { diff --git a/src/platforms/posix/drivers/adcsim/adcsim.cpp b/src/platforms/posix/drivers/adcsim/adcsim.cpp index d81b4a4552..b7d4071111 100644 --- a/src/platforms/posix/drivers/adcsim/adcsim.cpp +++ b/src/platforms/posix/drivers/adcsim/adcsim.cpp @@ -150,7 +150,7 @@ ADCSIM::~ADCSIM() int ADCSIM::init() { - debug("init done"); + DEVICE_DEBUG("init done"); /* create the device node */ return VDev::init(); diff --git a/src/platforms/posix/drivers/barosim/baro.cpp b/src/platforms/posix/drivers/barosim/baro.cpp index bcfbe965d8..1d4e63f94f 100644 --- a/src/platforms/posix/drivers/barosim/baro.cpp +++ b/src/platforms/posix/drivers/barosim/baro.cpp @@ -250,11 +250,11 @@ int BAROSIM::init() { int ret; - debug("BAROSIM::init"); + DEVICE_DEBUG("BAROSIM::init"); ret = VDev::init(); if (ret != OK) { - debug("VDev init failed"); + DEVICE_DEBUG("VDev init failed"); goto out; } @@ -262,7 +262,7 @@ BAROSIM::init() _reports = new ringbuffer::RingBuffer(2, sizeof(baro_report)); if (_reports == nullptr) { - debug("can't get memory for reports"); + DEVICE_DEBUG("can't get memory for reports"); ret = -ENOMEM; goto out; } @@ -576,7 +576,7 @@ BAROSIM::cycle() /* measurement phase */ ret = measure(); if (ret != OK) { - //log("measure error %d", ret); + //DEVICE_LOG("measure error %d", ret); /* issue a reset command to the sensor */ _interface->dev_ioctl(IOCTL_RESET, dummy); /* reset the collection state machine and try again */ diff --git a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp index 0fde8b293a..5eb8ff7fea 100644 --- a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp +++ b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp @@ -457,7 +457,7 @@ GYROSIM::init() ret = _gyro->init(); /* if probe/setup failed, bail now */ if (ret != OK) { - debug("gyro init failed"); + DEVICE_DEBUG("gyro init failed"); return ret; } @@ -1175,7 +1175,7 @@ GYROSIM_gyro::init() /* if probe/setup failed, bail now */ if (ret != OK) { - debug("gyro init failed"); + DEVICE_DEBUG("gyro init failed"); return ret; } diff --git a/src/platforms/posix/drivers/tonealrmsim/tone_alarm.cpp b/src/platforms/posix/drivers/tonealrmsim/tone_alarm.cpp index b127128a66..d070017a36 100644 --- a/src/platforms/posix/drivers/tonealrmsim/tone_alarm.cpp +++ b/src/platforms/posix/drivers/tonealrmsim/tone_alarm.cpp @@ -260,7 +260,7 @@ ToneAlarm::init() if (ret != OK) return ret; - debug("ready"); + DEVICE_DEBUG("ready"); return OK; } @@ -599,7 +599,7 @@ ToneAlarm::ioctl(device::file_t *filp, int cmd, unsigned long arg) { int result = OK; - debug("ioctl %i %u", cmd, arg); + DEVICE_DEBUG("ioctl %i %lu", cmd, arg); // irqstate_t flags = irqsave(); diff --git a/src/platforms/px4_log.h b/src/platforms/px4_log.h index 153985efa9..ab6e0f287c 100644 --- a/src/platforms/px4_log.h +++ b/src/platforms/px4_log.h @@ -87,6 +87,7 @@ __EXPORT extern int __px4_log_level_current; * Additional behavior can be added using "{\" for __px4__log_startline and * "}" for __px4__log_endline and any other required setup or teardown steps ****************************************************************************/ +#define __px4__log_startcond(cond) if (cond) printf( #define __px4__log_startline(level) if (level <= __px4_log_level_current) printf( #define __px4__log_timestamp_fmt "%-10" PRIu64 " " @@ -112,6 +113,22 @@ __EXPORT extern int __px4_log_level_current; ****************************************************************************/ #define __px4_log_omit(level, FMT, ...) do_nothing(level, ##__VA_ARGS__) +/**************************************************************************** + * __px4_log_named_cond: + * Convert a message in the form: + * PX4_LOG_COND(__dbg_enabled, "val is %d", val); + * to + * printf("%-5s val is %d\n", "LOG", val); + * if the first arg/condition is true. + ****************************************************************************/ +#define __px4_log_named_cond(name, cond, FMT, ...) \ + __px4__log_startcond(cond)\ + "%s " \ + FMT\ + __px4__log_end_fmt \ + ,name, ##__VA_ARGS__\ + __px4__log_endline + /**************************************************************************** * __px4_log: * Convert a message in the form: @@ -305,5 +322,7 @@ __EXPORT extern int __px4_log_level_current; #define PX4_DEBUG(FMT, ...) __px4_log_omit(_PX4_LOG_LEVEL_DEBUG, FMT, ##__VA_ARGS__) #endif +#define PX4_LOG_NAMED(name, FMT, ...) __px4_log_named_cond(name, true, FMT, ##__VA_ARGS__) +#define PX4_LOG_NAMED_COND(name, cond, FMT, ...) __px4_log_named_cond(name, cond, FMT, ##__VA_ARGS__) __END_DECLS #endif diff --git a/src/platforms/px4_workqueue.h b/src/platforms/px4_workqueue.h index 79162e3f6b..76e1218ba5 100644 --- a/src/platforms/px4_workqueue.h +++ b/src/platforms/px4_workqueue.h @@ -47,6 +47,10 @@ #include #include +#ifdef __PX4_QURT + #include +#endif + __BEGIN_DECLS #define HPWORK 0