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