diff --git a/src/modules/simulator/simulator.cpp b/src/modules/simulator/simulator.cpp
index bbfba4c4c5..96af6aa134 100644
--- a/src/modules/simulator/simulator.cpp
+++ b/src/modules/simulator/simulator.cpp
@@ -36,6 +36,7 @@
  * A device simulator
  */
 
+#include <px4_debug.h>
 #include <px4_tasks.h>
 #include <systemlib/err.h>
 #include <errno.h>
@@ -80,14 +81,14 @@ int Simulator::start(int argc, char *argv[])
 	int ret = 0;
 	_instance = new Simulator();
 	if (_instance) {
-		printf("Simulator started\n");
+		PX4_INFO("Simulator started\n");
 		drv_led_start();
 #ifndef __PX4_QURT
 		_instance->updateSamples();
 #endif
 	}
 	else {
-		printf("Simulator creation failed\n");
+		PX4_WARN("Simulator creation failed\n");
 		ret = 1;
 	}
 	return ret;
@@ -99,54 +100,54 @@ void Simulator::updateSamples()
 {
 	// get samples from external provider
 	struct sockaddr_in myaddr;
-        struct sockaddr_in srcaddr;
-        socklen_t addrlen = sizeof(srcaddr);
-        int len, fd;
+	struct sockaddr_in srcaddr;
+	socklen_t addrlen = sizeof(srcaddr);
+	int len, fd;
 	const int buflen = 200;
 	const int port = 9876;
-        unsigned char buf[buflen];
+	unsigned char buf[buflen];
 
-        if ((fd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) {
-                printf("create socket failed\n");
+	if ((fd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) {
+		PX4_WARN("create socket failed\n");
 		return;
-        }
+	}
 
-        memset((char *)&myaddr, 0, sizeof(myaddr));
-        myaddr.sin_family = AF_INET;
-        myaddr.sin_addr.s_addr = htonl(INADDR_ANY);
-        myaddr.sin_port = htons(port);
+	memset((char *)&myaddr, 0, sizeof(myaddr));
+	myaddr.sin_family = AF_INET;
+	myaddr.sin_addr.s_addr = htonl(INADDR_ANY);
+	myaddr.sin_port = htons(port);
 
-        if (bind(fd, (struct sockaddr *)&myaddr, sizeof(myaddr)) < 0) {
-                printf("bind failed\n");
+	if (bind(fd, (struct sockaddr *)&myaddr, sizeof(myaddr)) < 0) {
+		PX4_WARN("bind failed\n");
 		return;
-        }
+	}
 
-        for (;;) {
-                len = recvfrom(fd, buf, buflen, 0, (struct sockaddr *)&srcaddr, &addrlen);
-                if (len > 0) {
+	for (;;) {
+		len = recvfrom(fd, buf, buflen, 0, (struct sockaddr *)&srcaddr, &addrlen);
+		if (len > 0) {
 			if (len == sizeof(RawMPUData)) {
-                        	printf("received: MPU data\n");
+				PX4_DBG("received: MPU data\n");
 				_mpu.writeData(buf);
 			}
 			else if (len == sizeof(RawAccelData)) {
-                        	printf("received: accel data\n");
+				PX4_DBG("received: accel data\n");
 				_accel.writeData(buf);
 			}
 			else if (len == sizeof(RawBaroData)) {
-                        	printf("received: accel data\n");
+				PX4_DBG("received: accel data\n");
 				_baro.writeData(buf);
 			}
 			else {
-                        	printf("bad packet: len = %d\n", len);
+				PX4_DBG("bad packet: len = %d\n", len);
 			}
-                }
-        }
+		}
+	}
 }
 #endif
 
 static void usage()
 {
-	warnx("Usage: simulator {start|stop}");
+	PX4_WARN("Usage: simulator {start|stop}");
 }
 
 extern "C" {
@@ -172,7 +173,7 @@ int simulator_main(int argc, char *argv[])
 	}
 	else if (strcmp(argv[1], "stop") == 0) {
 		if (g_sim_task < 0) {
-			warnx("Simulator not running");
+			PX4_WARN("Simulator not running");
 		}
 		else {
 			px4_task_delete(g_sim_task);
@@ -196,37 +197,37 @@ extern void led_off(int led);
 extern void led_toggle(int led);
 __END_DECLS
 
-bool static _led_state = false;
+bool static _led_state[2] = { false , false };
 
 __EXPORT void led_init()
 {
-	printf("LED_ON\n");
+	PX4_DBG("LED_INIT\n");
 }
 
 __EXPORT void led_on(int led)
 {
-	if (led == 1)
+	if (led == 1 || led == 0)
 	{
-		printf("LED_ON\n");
-		_led_state = true;
+		PX4_DBG("LED%d_ON", led);
+		_led_state[led] = true;
 	}
 }
 
 __EXPORT void led_off(int led)
 {
-	if (led == 1)
+	if (led == 1 || led == 0)
 	{
-		printf("LED_OFF\n");
-		_led_state = false;
+		PX4_DBG("LED%d_OFF", led);
+		_led_state[led] = false;
 	}
 }
 
 __EXPORT void led_toggle(int led)
 {
-	if (led == 1)
+	if (led == 1 || led == 0)
 	{
-		_led_state = !_led_state;
-		printf("LED_TOGGLE: %s\n", _led_state ? "ON" : "OFF");
+		_led_state[led] = !_led_state[led];
+		PX4_DBG("LED%d_TOGGLE: %s\n", led, _led_state[led] ? "ON" : "OFF");
 
 	}
 }
diff --git a/src/platforms/posix/drivers/accelsim/accelsim.cpp b/src/platforms/posix/drivers/accelsim/accelsim.cpp
index b108f4969b..42e9eb0b1c 100644
--- a/src/platforms/posix/drivers/accelsim/accelsim.cpp
+++ b/src/platforms/posix/drivers/accelsim/accelsim.cpp
@@ -446,7 +446,7 @@ ACCELSIM::init()
 
 	/* do SIM init first */
 	if (VDev::init() != OK) {
-		warnx("SIM init failed");
+		PX4_WARN("SIM init failed");
 		goto out;
 	}
 
@@ -466,7 +466,7 @@ ACCELSIM::init()
 	/* do VDev init for the mag device node */
 	ret = _mag->init();
 	if (ret != OK) {
-		warnx("MAG init failed");
+		PX4_WARN("MAG init failed");
 		goto out;
 	}
 
@@ -482,7 +482,7 @@ ACCELSIM::init()
 		&_mag->_mag_orb_class_instance, ORB_PRIO_LOW);
 
 	if (_mag->_mag_topic < 0) {
-		warnx("ADVERT ERR");
+		PX4_WARN("ADVERT ERR");
 	}
 
 
@@ -497,7 +497,7 @@ ACCELSIM::init()
 		&_accel_orb_class_instance, ORB_PRIO_DEFAULT);
 
 	if (_accel_topic == (orb_advert_t)(-1)) {
-		warnx("ADVERT ERR");
+		PX4_WARN("ADVERT ERR");
 	}
 
 out:
@@ -1261,7 +1261,7 @@ start(enum Rotation rotation)
 {
 	int fd, fd_mag;
 	if (g_dev != nullptr) {
-		warnx( "already started");
+		PX4_WARN( "already started");
 		return 0;
 	}
 
@@ -1269,12 +1269,12 @@ start(enum Rotation rotation)
 	g_dev = new ACCELSIM(ACCELSIM_DEVICE_PATH_ACCEL, rotation);
 
 	if (g_dev == nullptr) {
-		warnx("failed instantiating ACCELSIM obj");
+		PX4_ERR("failed instantiating ACCELSIM obj");
 		goto fail;
 	}
 
 	if (OK != g_dev->init()) {
-		warnx("failed init of ACCELSIM obj");
+		PX4_ERR("failed init of ACCELSIM obj");
 		goto fail;
 	}
 
@@ -1282,12 +1282,12 @@ start(enum Rotation rotation)
 	fd = px4_open(ACCELSIM_DEVICE_PATH_ACCEL, O_RDONLY);
 
 	if (fd < 0) {
-		warnx("open %s failed", ACCELSIM_DEVICE_PATH_ACCEL);
+		PX4_WARN("open %s failed", ACCELSIM_DEVICE_PATH_ACCEL);
 		goto fail;
 	}
 
 	if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
-		warnx("ioctl SENSORIOCSPOLLRATE %s failed", ACCELSIM_DEVICE_PATH_ACCEL);
+		PX4_ERR("ioctl SENSORIOCSPOLLRATE %s failed", ACCELSIM_DEVICE_PATH_ACCEL);
         	px4_close(fd);
 		goto fail;
 	}
@@ -1297,12 +1297,12 @@ start(enum Rotation rotation)
 	/* don't fail if mag dev cannot be opened */
 	if (0 <= fd_mag) {
 		if (px4_ioctl(fd_mag, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
-			warnx("ioctl SENSORIOCSPOLLRATE %s failed", ACCELSIM_DEVICE_PATH_ACCEL);
+			PX4_ERR("ioctl SENSORIOCSPOLLRATE %s failed", ACCELSIM_DEVICE_PATH_ACCEL);
 		}
 	}
 	else
 	{
-		warnx("ioctl SENSORIOCSPOLLRATE %s failed", ACCELSIM_DEVICE_PATH_ACCEL);
+		PX4_ERR("ioctl SENSORIOCSPOLLRATE %s failed", ACCELSIM_DEVICE_PATH_ACCEL);
 	}
 
         px4_close(fd);
@@ -1316,7 +1316,7 @@ fail:
 		g_dev = nullptr;
 	}
 
-	warnx("driver start failed");
+	PX4_ERR("driver start failed");
 	return 1;
 }
 
@@ -1327,11 +1327,11 @@ int
 info()
 {
 	if (g_dev == nullptr) {
-		warnx("driver not running\n");
+		PX4_ERR("driver not running");
 		return 1;
 	}
 
-	printf("state @ %p\n", g_dev);
+	PX4_DBG("state @ %p", g_dev);
 	//g_dev->print_info();
 
 	return 0;
@@ -1340,9 +1340,9 @@ info()
 void
 usage()
 {
-	warnx("Usage: accelsim 'start', 'info'");
-	warnx("options:");
-	warnx("    -R rotation");
+	PX4_WARN("Usage: accelsim 'start', 'info'");
+	PX4_WARN("options:");
+	PX4_WARN("    -R rotation");
 }
 
 } // namespace
diff --git a/src/platforms/posix/drivers/adcsim/adcsim.cpp b/src/platforms/posix/drivers/adcsim/adcsim.cpp
index 893c30948c..fa6690ac21 100644
--- a/src/platforms/posix/drivers/adcsim/adcsim.cpp
+++ b/src/platforms/posix/drivers/adcsim/adcsim.cpp
@@ -241,7 +241,7 @@ test(void)
 
 	int fd = px4_open(ADCSIM0_DEVICE_PATH, O_RDONLY);
 	if (fd < 0) {
-		warnx("can't open ADCSIM device");
+		PX4_ERR("can't open ADCSIM device");
 		return 1;
 	}
 
@@ -250,17 +250,16 @@ test(void)
 		ssize_t count = px4_read(fd, data, sizeof(data));
 
 		if (count < 0) {
-			warnx("read error");
+			PX4_ERR("read error");
 			return 1;
 		}
 
 		unsigned channels = count / sizeof(data[0]);
 
 		for (unsigned j = 0; j < channels; j++) {
-			printf ("%d: %lu  ", data[j].am_channel, (unsigned long)data[j].am_data);
+			PX4_INFO("%d: %lu  ", data[j].am_channel, (unsigned long)data[j].am_data);
 		}
 
-		printf("\n");
 		usleep(500000);
 	}
 
@@ -279,13 +278,13 @@ adcsim_main(int argc, char *argv[])
 		g_adc = new ADCSIM((1 << 10) | (1 << 11) | (1 << 12) | (1 << 13));
 
 		if (g_adc == nullptr) {
-			warnx("couldn't allocate the ADCSIM driver");
+			PX4_ERR("couldn't allocate the ADCSIM driver");
 			return 1;
 		}
 
 		if (g_adc->init() != OK) {
 			delete g_adc;
-			warnx("ADCSIM init failed");
+			PX4_ERR("ADCSIM init failed");
 			return 1;
 		}
 	}
diff --git a/src/platforms/posix/drivers/airspeedsim/airspeedsim.cpp b/src/platforms/posix/drivers/airspeedsim/airspeedsim.cpp
index 2a500af794..a0933971a4 100644
--- a/src/platforms/posix/drivers/airspeedsim/airspeedsim.cpp
+++ b/src/platforms/posix/drivers/airspeedsim/airspeedsim.cpp
@@ -147,7 +147,7 @@ AirspeedSim::init()
 		_airspeed_pub = orb_advertise(ORB_ID(differential_pressure), &arp);
 
 		if (_airspeed_pub < 0)
-			warnx("uORB started?");
+			PX4_WARN("uORB started?");
 	}
 
 	ret = OK;
@@ -395,7 +395,7 @@ AirspeedSim::print_info()
 	perf_print_counter(_sample_perf);
 	perf_print_counter(_comms_errors);
 	perf_print_counter(_buffer_overflows);
-	warnx("poll interval:  %u ticks", _measure_ticks);
+	PX4_INFO("poll interval:  %u ticks", _measure_ticks);
 	_reports->print_info("report queue");
 }
 
diff --git a/src/platforms/posix/drivers/barosim/baro.cpp b/src/platforms/posix/drivers/barosim/baro.cpp
index eb6c6c29f7..751ffd43db 100644
--- a/src/platforms/posix/drivers/barosim/baro.cpp
+++ b/src/platforms/posix/drivers/barosim/baro.cpp
@@ -250,7 +250,7 @@ int
 BAROSIM::init()
 {
 	int ret;
-	warnx("BAROSIM::init");
+	PX4_WARN("BAROSIM::init");
 
 	ret = VDev::init();
 	if (ret != OK) {
@@ -280,7 +280,7 @@ BAROSIM::init()
 		/* do temperature first */
 		if (OK != measure()) {
 			ret = -EIO;
-			warnx("temp measure failed");
+			PX4_WARN("temp measure failed");
 			break;
 		}
 
@@ -288,14 +288,14 @@ BAROSIM::init()
 
 		if (OK != collect()) {
 			ret = -EIO;
-			warnx("temp collect failed");
+			PX4_WARN("temp collect failed");
 			break;
 		}
 
 		/* now do a pressure measurement */
 		if (OK != measure()) {
 			ret = -EIO;
-			warnx("pressure collect failed");
+			PX4_WARN("pressure collect failed");
 			break;
 		}
 
@@ -303,7 +303,7 @@ BAROSIM::init()
 
 		if (OK != collect()) {
 			ret = -EIO;
-			warnx("pressure collect failed");
+			PX4_WARN("pressure collect failed");
 			break;
 		}
 
@@ -316,9 +316,9 @@ BAROSIM::init()
 				&_orb_class_instance, (is_external()) ? ORB_PRIO_HIGH : ORB_PRIO_DEFAULT);
 
 		if (_baro_topic == (orb_advert_t)(-1)) {
-			warnx("failed to create sensor_baro publication");
+			PX4_WARN("failed to create sensor_baro publication");
 		}
-		//warnx("sensor_baro publication %ld", _baro_topic);
+		//PX4_WARN("sensor_baro publication %ld", _baro_topic);
 
 	} while (0);
 
@@ -728,7 +728,7 @@ BAROSIM::collect()
 				orb_publish(ORB_ID(sensor_baro), _baro_topic, &report);
 			}
 			else {
-				printf("BAROSIM::collect _baro_topic not initialized\n");
+				PX4_ERR("BAROSIM::collect _baro_topic not initialized");
 			}
 		}
 
@@ -754,23 +754,23 @@ BAROSIM::print_info()
 	perf_print_counter(_sample_perf);
 	perf_print_counter(_comms_errors);
 	perf_print_counter(_buffer_overflows);
-	printf("poll interval:  %u ticks\n", _measure_ticks);
+	PX4_INFO("poll interval:  %u ticks", _measure_ticks);
 	_reports->print_info("report queue");
-	printf("TEMP:           %ld\n", (long)_TEMP);
-	printf("SENS:           %lld\n", (long long)_SENS);
-	printf("OFF:            %lld\n", (long long)_OFF);
-	printf("P:              %.3f\n", (double)_P);
-	printf("T:              %.3f\n", (double)_T);
-	printf("MSL pressure:   %10.4f\n", (double)(_msl_pressure / 100.f));
-
-	printf("factory_setup             %u\n", _prom.factory_setup);
-	printf("c1_pressure_sens          %u\n", _prom.c1_pressure_sens);
-	printf("c2_pressure_offset        %u\n", _prom.c2_pressure_offset);
-	printf("c3_temp_coeff_pres_sens   %u\n", _prom.c3_temp_coeff_pres_sens);
-	printf("c4_temp_coeff_pres_offset %u\n", _prom.c4_temp_coeff_pres_offset);
-	printf("c5_reference_temp         %u\n", _prom.c5_reference_temp);
-	printf("c6_temp_coeff_temp        %u\n", _prom.c6_temp_coeff_temp);
-	printf("serial_and_crc            %u\n", _prom.serial_and_crc);
+	PX4_INFO("TEMP:           %ld", (long)_TEMP);
+	PX4_INFO("SENS:           %lld", (long long)_SENS);
+	PX4_INFO("OFF:            %lld", (long long)_OFF);
+	PX4_INFO("P:              %.3f", (double)_P);
+	PX4_INFO("T:              %.3f", (double)_T);
+	PX4_INFO("MSL pressure:   %10.4f", (double)(_msl_pressure / 100.f));
+
+	PX4_INFO("factory_setup             %u", _prom.factory_setup);
+	PX4_INFO("c1_pressure_sens          %u", _prom.c1_pressure_sens);
+	PX4_INFO("c2_pressure_offset        %u", _prom.c2_pressure_offset);
+	PX4_INFO("c3_temp_coeff_pres_sens   %u", _prom.c3_temp_coeff_pres_sens);
+	PX4_INFO("c4_temp_coeff_pres_offset %u", _prom.c4_temp_coeff_pres_offset);
+	PX4_INFO("c5_reference_temp         %u", _prom.c5_reference_temp);
+	PX4_INFO("c6_temp_coeff_temp        %u", _prom.c6_temp_coeff_temp);
+	PX4_INFO("serial_and_crc            %u", _prom.serial_and_crc);
 }
 
 /**
@@ -855,7 +855,7 @@ bool
 start_bus(struct barosim_bus_option &bus)
 {
 	if (bus.dev != nullptr) {
-		warnx("bus option already started");
+		PX4_ERR("bus option already started");
 		return false;
 	}
 
@@ -863,7 +863,7 @@ start_bus(struct barosim_bus_option &bus)
 	device::Device *interface = bus.interface_constructor(prom_buf, bus.busnum);
 	if (interface->init() != OK) {
 		delete interface;
-		warnx("no device on bus %u", (unsigned)bus.busid);
+		PX4_ERR("no device on bus %u", (unsigned)bus.busid);
 		return false;
 	}
 
@@ -871,7 +871,7 @@ start_bus(struct barosim_bus_option &bus)
 	if (bus.dev != nullptr && OK != bus.dev->init()) {
 		delete bus.dev;
 		bus.dev = NULL;
-		warnx("bus init failed %p", bus.dev);
+		PX4_ERR("bus init failed %p", bus.dev);
 		return false;
 	}
 			
@@ -879,12 +879,12 @@ start_bus(struct barosim_bus_option &bus)
 
 	/* set the poll rate to default, starts automatic data collection */
 	if (fd == -1) {
-		warnx("can't open baro device");
+		PX4_ERR("can't open baro device");
 		return false;
 	}
 	if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
 		px4_close(fd);
-		warnx("failed setting default poll rate");
+		PX4_ERR("failed setting default poll rate");
 		return false;
 	}
 
@@ -907,7 +907,7 @@ start()
 	started |= start_bus(bus_options[0]);
 
 	if (!started) {
-		warnx("driver start failed");
+		PX4_ERR("driver start failed");
 		return 1;
 	}
 
@@ -933,7 +933,7 @@ test()
 
 	fd = px4_open(bus.devpath, O_RDONLY);
 	if (fd < 0) {
-		warn("open failed (try 'barosim start' if the driver is not running)");
+		PX4_ERR("open failed (try 'barosim start' if the driver is not running)");
 		return 1;
 	}
 
@@ -941,25 +941,27 @@ test()
 	sz = px4_read(fd, &report, sizeof(report));
 
 	if (sz != sizeof(report)) {
-		warn("immediate read failed");
+		PX4_ERR("immediate read failed");
 		return 1;
 	}
 
-	warnx("single read");
-	warnx("pressure:    %10.4f", (double)report.pressure);
-	warnx("altitude:    %11.4f", (double)report.altitude);
-	warnx("temperature: %8.4f", (double)report.temperature);
-	warnx("time:        %lld", (long long)report.timestamp);
+	PX4_INFO("single read");
+	PX4_INFO("pressure:    %10.4f", (double)report.pressure);
+	PX4_INFO("altitude:    %11.4f", (double)report.altitude);
+	PX4_INFO("temperature: %8.4f", (double)report.temperature);
+	PX4_INFO("time:        %lld", (long long)report.timestamp);
 
 	/* set the queue depth to 10 */
 	if (OK != px4_ioctl(fd, SENSORIOCSQUEUEDEPTH, 10)) {
-		warnx("failed to set queue depth");
+		PX4_ERR("failed to set queue depth");
+		px4_close(fd);
 		return 1;
 	}
 
 	/* start the sensor polling at 2Hz */
 	if (OK != px4_ioctl(fd, SENSORIOCSPOLLRATE, 2)) {
-		warnx("failed to set 2Hz poll rate");
+		PX4_ERR("failed to set 2Hz poll rate");
+		px4_close(fd);
 		return 1;
 	}
 
@@ -973,26 +975,27 @@ test()
 		ret = px4_poll(&fds, 1, 2000);
 
 		if (ret != 1) {
-			warnx("timed out waiting for sensor data");
+			PX4_WARN("timed out waiting for sensor data");
 		}
 
 		/* now go get it */
 		sz = px4_read(fd, &report, sizeof(report));
 
 		if (sz != sizeof(report)) {
-			warn("periodic read failed");
+			PX4_ERR("periodic read failed");
+			px4_close(fd);
 			return 1;
 		}
 
-		warnx("periodic read %u", i);
-		warnx("pressure:    %10.4f", (double)report.pressure);
-		warnx("altitude:    %11.4f", (double)report.altitude);
-		warnx("temperature: %8.4f", (double)report.temperature);
-		warnx("time:        %lld", (long long)report.timestamp);
+		PX4_INFO("periodic read %u", i);
+		PX4_INFO("pressure:    %10.4f", (double)report.pressure);
+		PX4_INFO("altitude:    %11.4f", (double)report.altitude);
+		PX4_INFO("temperature: %8.4f", (double)report.temperature);
+		PX4_INFO("time:        %lld", (long long)report.timestamp);
 	}
 
 	px4_close(fd);
-	warnx("PASS");
+	PX4_INFO("PASS");
 	return 0;
 }
 
@@ -1007,17 +1010,17 @@ reset()
 
 	fd = px4_open(bus.devpath, O_RDONLY);
 	if (fd < 0) {
-		warn("failed ");
+		PX4_ERR("failed ");
 		return 1;
 	}
 
 	if (px4_ioctl(fd, SENSORIOCRESET, 0) < 0) {
-		warn("driver reset failed");
+		PX4_ERR("driver reset failed");
 		return 1;
 	}
 
 	if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
-		warn("driver poll restart failed");
+		PX4_ERR("driver poll restart failed");
 		return 1;
 	}
 	return 0;
@@ -1032,7 +1035,7 @@ info()
 	for (uint8_t i=0; i<NUM_BUS_OPTIONS; i++) {
 		struct barosim_bus_option &bus = bus_options[i];
 		if (bus.dev != nullptr) {
-			warnx("%s", bus.devpath);
+			PX4_INFO("%s", bus.devpath);
 			bus.dev->print_info();
 		}
 	}
@@ -1055,13 +1058,13 @@ calibrate(unsigned altitude)
 	fd = px4_open(bus.devpath, O_RDONLY);
 
 	if (fd < 0) {
-		warn("open failed (try 'barosim start' if the driver is not running)");
+		PX4_ERR("open failed (try 'barosim start' if the driver is not running)");
 		return 1;
 	}
 
 	/* start the sensor polling at max */
 	if (OK != px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX)) {
-		warnx("failed to set poll rate");
+		PX4_ERR("failed to set poll rate");
 		return 1;
 	}
 
@@ -1079,7 +1082,7 @@ calibrate(unsigned altitude)
 		ret = px4_poll(&fds, 1, 1000);
 
 		if (ret != 1) {
-			warnx("timed out waiting for sensor data");
+			PX4_ERR("timed out waiting for sensor data");
 			return 1;
 		}
 
@@ -1087,7 +1090,7 @@ calibrate(unsigned altitude)
 		sz = px4_read(fd, &report, sizeof(report));
 
 		if (sz != sizeof(report)) {
-			warn("sensor read failed");
+			PX4_ERR("sensor read failed");
 			return 1;
 		}
 
@@ -1103,17 +1106,17 @@ calibrate(unsigned altitude)
 	const float g  = 9.80665f;	/* gravity constant in m/s/s */
 	const float R  = 287.05f;	/* ideal gas constant in J/kg/K */
 
-	warnx("averaged pressure %10.4fkPa at %um", (double)pressure, altitude);
+	PX4_INFO("averaged pressure %10.4fkPa at %um", (double)pressure, altitude);
 
 	p1 = pressure * (powf(((T1 + (a * (float)altitude)) / T1), (g / (a * R))));
 
-	warnx("calculated MSL pressure %10.4fkPa", (double)p1);
+	PX4_INFO("calculated MSL pressure %10.4fkPa", (double)p1);
 
 	/* save as integer Pa */
 	p1 *= 1000.0f;
 
 	if (px4_ioctl(fd, BAROIOCSMSLPRESSURE, (unsigned long)p1) != OK) {
-		warn("BAROIOCSMSLPRESSURE");
+		PX4_WARN("BAROIOCSMSLPRESSURE");
 		return 1;
 	}
 
@@ -1124,7 +1127,7 @@ calibrate(unsigned altitude)
 void
 usage()
 {
-	warnx("missing command: try 'start', 'info', 'test', 'test2', 'reset', 'calibrate <altitude>'");
+	PX4_WARN("missing command: try 'start', 'info', 'test', 'test2', 'reset', 'calibrate <altitude>'");
 }
 
 } // namespace
@@ -1170,7 +1173,7 @@ barosim_main(int argc, char *argv[])
 	 */
 	else if (!strcmp(verb, "calibrate")) {
 		if (argc < 3) {
-			warnx("missing altitude");
+			PX4_WARN("missing altitude");
 			barosim::usage();
 			return 1;
 		}
diff --git a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp
index 44e85150c0..e0d50ce25d 100644
--- a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp
+++ b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp
@@ -566,25 +566,25 @@ GYROSIM::init()
 
 	/* if probe/setup failed, bail now */
 	if (ret != OK) {
-		warnx("VDev setup failed");
+		PX4_WARN("VDev setup failed");
 		return ret;
 	}
 
 	/* allocate basic report buffers */
 	_accel_reports = new RingBuffer(2, sizeof(accel_report));
 	if (_accel_reports == nullptr) {
-		warnx("_accel_reports creation failed");
+		PX4_WARN("_accel_reports creation failed");
 		goto out;
 	}
 
 	_gyro_reports = new RingBuffer(2, sizeof(gyro_report));
 	if (_gyro_reports == nullptr) {
-		warnx("_gyro_reports creation failed");
+		PX4_WARN("_gyro_reports creation failed");
 		goto out;
 	}
 
 	if (reset() != OK) {
-		warnx("reset failed");
+		PX4_WARN("reset failed");
 		goto out;
 	}
 
@@ -625,7 +625,7 @@ GYROSIM::init()
 		&_accel_orb_class_instance, ORB_PRIO_HIGH);
 
 	if (_accel_topic < 0) {
-		warnx("ADVERT FAIL");
+		PX4_WARN("ADVERT FAIL");
 	}
 
 
@@ -637,7 +637,7 @@ GYROSIM::init()
 		&_gyro->_gyro_orb_class_instance, ORB_PRIO_HIGH);
 
 	if (_gyro->_gyro_topic < 0) {
-		warnx("ADVERT FAIL");
+		PX4_WARN("ADVERT FAIL");
 	}
 
 out:
@@ -669,11 +669,11 @@ GYROSIM::transfer(uint8_t *send, uint8_t *recv, unsigned len)
 	}
 	else if (cmd & DIR_READ)
 	{
-		printf("Reading %u bytes from register %u\n", len-1, reg);
+		PX4_DBG("Reading %u bytes from register %u", len-1, reg);
 		memcpy(&_regdata[reg-MPUREG_PRODUCT_ID], &send[1], len-1);
 	}
 	else {
-		printf("Writing %u bytes to register %u\n", len-1, reg);
+		PX4_DBG("Writing %u bytes to register %u", len-1, reg);
 		if (recv)
 			memcpy(&recv[1], &_regdata[reg-MPUREG_PRODUCT_ID], len-1);
 	}
@@ -1509,31 +1509,38 @@ GYROSIM::print_info()
 	perf_print_counter(_reset_retries);
 	_accel_reports->print_info("accel queue");
 	_gyro_reports->print_info("gyro queue");
-        ::printf("checked_next: %u\n", _checked_next);
+        PX4_WARN("checked_next: %u", _checked_next);
         for (uint8_t i=0; i<GYROSIM_NUM_CHECKED_REGISTERS; i++) {
             uint8_t v = read_reg(_checked_registers[i], GYROSIM_HIGH_BUS_SPEED);
             if (v != _checked_values[i]) {
-                ::printf("reg %02x:%02x should be %02x\n",
+                PX4_WARN("reg %02x:%02x should be %02x",
                          (unsigned)_checked_registers[i],
                          (unsigned)v,
                          (unsigned)_checked_values[i]);
             }
         }
-	::printf("temperature: %.1f\n", (double)_last_temperature);
+	PX4_WARN("temperature: %.1f", (double)_last_temperature);
 }
 
 void
 GYROSIM::print_registers()
 {
-	printf("GYROSIM registers\n");
+	char buf[6*13+1];
+	int i=0;
+
+	buf[0] = '\0';
+	PX4_WARN("GYROSIM registers");
 	for (uint8_t reg=MPUREG_PRODUCT_ID; reg<=108; reg++) {
 		uint8_t v = read_reg(reg);
-		printf("%02x:%02x ",(unsigned)reg, (unsigned)v);
-		if ((reg - (MPUREG_PRODUCT_ID-1)) % 13 == 0) {
-			printf("\n");
+		sprintf(&buf[i*6], "%02x:%02x ",(unsigned)reg, (unsigned)v);
+		i++;
+		if ((i+1) % 13 == 0) {
+			PX4_WARN("%s", buf);
+			i=0;
+			buf[i] = '\0';
 		}
 	}
-	printf("\n");
+	PX4_WARN("%s",buf);
 }
 
 
@@ -1628,7 +1635,7 @@ start(enum Rotation rotation)
 
 	if (*g_dev_ptr != nullptr) {
 		/* if already started, the still command succeeded */
-		warnx("already started");
+		PX4_WARN("already started");
 		return 0;
 	}
 
@@ -1647,11 +1654,12 @@ start(enum Rotation rotation)
 	if (fd < 0)
 		goto fail;
 
-	if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
+	if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
+		px4_close(fd);
 		goto fail;
+	}
 
-        px4_close(fd);
-
+	px4_close(fd);
 	return 0;
 fail:
 
@@ -1660,7 +1668,7 @@ fail:
             *g_dev_ptr = nullptr;
 	}
 
-	warnx("driver start failed");
+	PX4_WARN("driver start failed");
 	return 1;
 }
 
@@ -1673,7 +1681,7 @@ stop()
 		*g_dev_ptr = nullptr;
 	} else {
 		/* warn, but not an error */
-		warnx("already stopped.");
+		PX4_WARN("already stopped.");
 	}
 	return 0;
 }
@@ -1695,64 +1703,74 @@ test()
 	/* get the driver */
 	int fd = px4_open(path_accel, O_RDONLY);
 
-	if (fd < 0)
-		err(1, "%s open failed (try 'gyrosim start')",
-		    path_accel);
+	if (fd < 0) {
+		PX4_ERR("%s open failed (try 'gyrosim start')", path_accel);
+		return 1;
+	}
 
 	/* get the driver */
 	int fd_gyro = px4_open(path_gyro, O_RDONLY);
 
-	if (fd_gyro < 0)
-		err(1, "%s open failed", path_gyro);
+	if (fd_gyro < 0) {
+		PX4_ERR("%s open failed", path_gyro);
+		return 1;
+	}
 
 	/* reset to manual polling */
-	if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0)
-		err(1, "reset to manual polling");
+	if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0) {
+		PX4_ERR("reset to manual polling");
+		return 1;
+	}
 
 	/* do a simple demand read */
 	sz = read(fd, &a_report, sizeof(a_report));
 
 	if (sz != sizeof(a_report)) {
-		warnx("ret: %zd, expected: %zd", sz, sizeof(a_report));
-		err(1, "immediate acc read failed");
+		PX4_WARN("ret: %zd, expected: %zd", sz, sizeof(a_report));
+		PX4_ERR("immediate acc read failed");
+		return 1;
 	}
 
-	warnx("single read");
-	warnx("time:     %lld", (long long)a_report.timestamp);
-	warnx("acc  x:  \t%8.4f\tm/s^2", (double)a_report.x);
-	warnx("acc  y:  \t%8.4f\tm/s^2", (double)a_report.y);
-	warnx("acc  z:  \t%8.4f\tm/s^2", (double)a_report.z);
-	warnx("acc  x:  \t%d\traw 0x%0x", (short)a_report.x_raw, (unsigned short)a_report.x_raw);
-	warnx("acc  y:  \t%d\traw 0x%0x", (short)a_report.y_raw, (unsigned short)a_report.y_raw);
-	warnx("acc  z:  \t%d\traw 0x%0x", (short)a_report.z_raw, (unsigned short)a_report.z_raw);
-	warnx("acc range: %8.4f m/s^2 (%8.4f g)", (double)a_report.range_m_s2,
+	PX4_INFO("single read");
+	PX4_INFO("time:     %lld", (long long)a_report.timestamp);
+	PX4_INFO("acc  x:  \t%8.4f\tm/s^2", (double)a_report.x);
+	PX4_INFO("acc  y:  \t%8.4f\tm/s^2", (double)a_report.y);
+	PX4_INFO("acc  z:  \t%8.4f\tm/s^2", (double)a_report.z);
+	PX4_INFO("acc  x:  \t%d\traw 0x%0x", (short)a_report.x_raw, (unsigned short)a_report.x_raw);
+	PX4_INFO("acc  y:  \t%d\traw 0x%0x", (short)a_report.y_raw, (unsigned short)a_report.y_raw);
+	PX4_INFO("acc  z:  \t%d\traw 0x%0x", (short)a_report.z_raw, (unsigned short)a_report.z_raw);
+	PX4_INFO("acc range: %8.4f m/s^2 (%8.4f g)", (double)a_report.range_m_s2,
 	      (double)(a_report.range_m_s2 / GYROSIM_ONE_G));
 
 	/* do a simple demand read */
 	sz = read(fd_gyro, &g_report, sizeof(g_report));
 
 	if (sz != sizeof(g_report)) {
-		warnx("ret: %zd, expected: %zd", sz, sizeof(g_report));
-		err(1, "immediate gyro read failed");
+		PX4_WARN("ret: %zd, expected: %zd", sz, sizeof(g_report));
+		PX4_ERR("immediate gyro read failed");
+		return 1;
 	}
 
-	warnx("gyro x: \t% 9.5f\trad/s", (double)g_report.x);
-	warnx("gyro y: \t% 9.5f\trad/s", (double)g_report.y);
-	warnx("gyro z: \t% 9.5f\trad/s", (double)g_report.z);
-	warnx("gyro x: \t%d\traw", (int)g_report.x_raw);
-	warnx("gyro y: \t%d\traw", (int)g_report.y_raw);
-	warnx("gyro z: \t%d\traw", (int)g_report.z_raw);
-	warnx("gyro range: %8.4f rad/s (%d deg/s)", (double)g_report.range_rad_s,
+	PX4_INFO("gyro x: \t% 9.5f\trad/s", (double)g_report.x);
+	PX4_INFO("gyro y: \t% 9.5f\trad/s", (double)g_report.y);
+	PX4_INFO("gyro z: \t% 9.5f\trad/s", (double)g_report.z);
+	PX4_INFO("gyro x: \t%d\traw", (int)g_report.x_raw);
+	PX4_INFO("gyro y: \t%d\traw", (int)g_report.y_raw);
+	PX4_INFO("gyro z: \t%d\traw", (int)g_report.z_raw);
+	PX4_INFO("gyro range: %8.4f rad/s (%d deg/s)", (double)g_report.range_rad_s,
 	      (int)((g_report.range_rad_s / M_PI_F) * 180.0f + 0.5f));
 
-	warnx("temp:  \t%8.4f\tdeg celsius", (double)a_report.temperature);
-	warnx("temp:  \t%d\traw 0x%0x", (short)a_report.temperature_raw, (unsigned short)a_report.temperature_raw);
+	PX4_INFO("temp:  \t%8.4f\tdeg celsius", (double)a_report.temperature);
+	PX4_INFO("temp:  \t%d\traw 0x%0x", (short)a_report.temperature_raw, (unsigned short)a_report.temperature_raw);
 
 
 	/* XXX add poll-rate tests here too */
 
+	px4_close(fd);
 	reset();
-	warnx("PASS");
+	PX4_INFO("PASS");
+
+	
 	return 0;
 }
 
@@ -1765,18 +1783,28 @@ reset()
 	const char *path_accel = MPU_DEVICE_PATH_ACCEL;
 	int fd = px4_open(path_accel, O_RDONLY);
 
-	if (fd < 0)
-		err(1, "failed ");
+	if (fd < 0) {
+		PX4_ERR("reset failed");
+		return 1;
+	}
+
 
-	if (px4_ioctl(fd, SENSORIOCRESET, 0) < 0)
-		err(1, "driver reset failed");
+	if (px4_ioctl(fd, SENSORIOCRESET, 0) < 0) {
+		PX4_ERR("driver reset failed");
+		goto reset_fail;
+	}
 
-	if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
-		err(1, "driver poll restart failed");
+	if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
+		PX4_ERR("driver poll restart failed");
+		goto reset_fail;
+	}
 
         px4_close(fd);
-
 	return 0;
+
+reset_fail:
+        px4_close(fd);
+	return 1;
 }
 
 /**
@@ -1787,11 +1815,11 @@ info()
 {
         GYROSIM **g_dev_ptr = &g_dev_sim;
 	if (*g_dev_ptr == nullptr) {
-		warnx("driver not running");
+		PX4_ERR("driver not running");
 		return 1;
 	}
 
-	printf("state @ %p\n", *g_dev_ptr);
+	PX4_INFO("state @ %p", *g_dev_ptr);
 	(*g_dev_ptr)->print_info();
 
 	return 0;
@@ -1805,11 +1833,11 @@ regdump()
 {
 	GYROSIM **g_dev_ptr = &g_dev_sim;
 	if (*g_dev_ptr == nullptr) {
-		warnx("driver not running");
+		PX4_ERR("driver not running");
 		return 1;
 	}
 
-	printf("regdump @ %p\n", *g_dev_ptr);
+	PX4_INFO("regdump @ %p", *g_dev_ptr);
 	(*g_dev_ptr)->print_registers();
 
 	return 0;
@@ -1818,9 +1846,9 @@ regdump()
 void
 usage()
 {
-	warnx("missing command: try 'start', 'info', 'test', 'stop',\n'reset', 'regdump'");
-	warnx("options:");
-	warnx("    -R rotation");
+	PX4_WARN("missing command: try 'start', 'info', 'test', 'stop', 'reset', 'regdump'");
+	PX4_WARN("options:");
+	PX4_WARN("    -R rotation");
 }
 
 } // namespace
diff --git a/src/platforms/posix/drivers/tonealrmsim/tone_alarm.cpp b/src/platforms/posix/drivers/tonealrmsim/tone_alarm.cpp
index 7c9e8cc912..def293ea4c 100644
--- a/src/platforms/posix/drivers/tonealrmsim/tone_alarm.cpp
+++ b/src/platforms/posix/drivers/tonealrmsim/tone_alarm.cpp
@@ -331,6 +331,10 @@ ToneAlarm::rest_duration(unsigned rest_length, unsigned dots)
 	return rest_period;
 }
 
+static void do_something(unsigned x)
+{
+}
+
 void
 ToneAlarm::start_note(unsigned note)
 {
@@ -344,7 +348,9 @@ ToneAlarm::start_note(unsigned note)
 	// calculate the timer period for the selected prescaler value
 	unsigned period = (divisor / (prescale + 1)) - 1;
 
-	printf("ToneAlarm::start_note %u\n", period);
+	// Silence warning of unused var
+	do_something(period);
+	PX4_DBG("ToneAlarm::start_note %u", period);
 }
 
 void
@@ -688,7 +694,7 @@ play_tune(unsigned tune)
 	fd = px4_open(TONEALARM0_DEVICE_PATH, 0);
 
 	if (fd < 0) {
-		warnx("Error: failed to open %s", TONEALARM0_DEVICE_PATH);
+		PX4_WARN("Error: failed to open %s", TONEALARM0_DEVICE_PATH);
 		return 1;
 	}
 
@@ -696,7 +702,7 @@ play_tune(unsigned tune)
 	px4_close(fd);
 
 	if (ret != 0) {
-		warn("TONE_SET_ALARM");
+		PX4_WARN("TONE_SET_ALARM");
 		return 1;
 	}
 
@@ -711,7 +717,7 @@ play_string(const char *str, bool free_buffer)
 	fd = px4_open(TONEALARM0_DEVICE_PATH, O_WRONLY);
 
 	if (fd < 0) {
-		warn("Error: failed to open %s", TONEALARM0_DEVICE_PATH);
+		PX4_WARN("Error: failed to open %s", TONEALARM0_DEVICE_PATH);
 		return 1;
 	}
 
@@ -722,7 +728,7 @@ play_string(const char *str, bool free_buffer)
 		free((void *)str);
 
 	if (ret < 0) {
-		warn("play tune");
+		PX4_WARN("play tune");
 		return 1;
 	}
 	return ret;
@@ -741,13 +747,13 @@ tone_alarm_main(int argc, char *argv[])
 		g_dev = new ToneAlarm;
 
 		if (g_dev == nullptr) {
-			warnx("couldn't allocate the ToneAlarm driver");
+			PX4_WARN("couldn't allocate the ToneAlarm driver");
 			return 1;
 		}
 
 		if (g_dev->init() != OK) {
 			delete g_dev;
-			warnx("ToneAlarm init failed");
+			PX4_WARN("ToneAlarm init failed");
 			return 1;
 		}
 	}
@@ -771,7 +777,7 @@ tone_alarm_main(int argc, char *argv[])
 			int sz;
 			char *buffer;
 			if (fd == nullptr) {
-				warnx("couldn't open '%s'", argv1);
+				PX4_WARN("couldn't open '%s'", argv1);
 				return 1;
 			}
 			fseek(fd, 0, SEEK_END);
@@ -779,7 +785,7 @@ tone_alarm_main(int argc, char *argv[])
 			fseek(fd, 0, SEEK_SET);
 			buffer = (char *)malloc(sz + 1);
 			if (buffer == nullptr) {
-				warnx("not enough memory memory");
+				PX4_WARN("not enough memory memory");
 				return 1;
 			}
 			fread(buffer, sz, 1, fd);
@@ -801,7 +807,7 @@ tone_alarm_main(int argc, char *argv[])
 					ret = play_tune(tune);
 					return ret;
 				}
-			warnx("unrecognized command, try 'start', 'stop', an alarm number or name, or a file name starting with a '/'");
+			PX4_WARN("unrecognized command, try 'start', 'stop', an alarm number or name, or a file name starting with a '/'");
 			ret = 1;
 		}
 	}
diff --git a/src/platforms/posix/px4_layer/px4_posix_tasks.cpp b/src/platforms/posix/px4_layer/px4_posix_tasks.cpp
index fd58637273..5b8f4ea666 100644
--- a/src/platforms/posix/px4_layer/px4_posix_tasks.cpp
+++ b/src/platforms/posix/px4_layer/px4_posix_tasks.cpp
@@ -37,6 +37,7 @@
  * Implementation of existing task API for Linux
  */
 
+#include <px4_debug.h>
 #include <unistd.h>
 #include <stdio.h>
 #include <stdlib.h>
@@ -82,9 +83,9 @@ static void *entry_adapter ( void *ptr )
 
 	data->entry(data->argc, data->argv);
 	free(ptr);
-	printf("Before px4_task_exit\n");
+	PX4_DBG("Before px4_task_exit");
 	px4_task_exit(0); 
-	printf("After px4_task_exit\n");
+	PX4_DBG("After px4_task_exit");
 
 	return NULL;
 } 
@@ -92,7 +93,7 @@ static void *entry_adapter ( void *ptr )
 void
 px4_systemreset(bool to_bootloader)
 {
-	printf("Called px4_system_reset\n");
+	PX4_WARN("Called px4_system_reset");
 }
 
 px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int stack_size, px4_main_t entry, char * const argv[])
@@ -138,17 +139,17 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int
 
 	rv = pthread_attr_init(&attr);
 	if (rv != 0) {
-		printf("px4_task_spawn_cmd: failed to init thread attrs\n");
+		PX4_WARN("px4_task_spawn_cmd: failed to init thread attrs");
 		return (rv < 0) ? rv : -rv;
 	}
 	rv = pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED);
 	if (rv != 0) {
-		printf("px4_task_spawn_cmd: failed to set inherit sched\n");
+		PX4_WARN("px4_task_spawn_cmd: failed to set inherit sched");
 		return (rv < 0) ? rv : -rv;
 	}
 	rv = pthread_attr_setschedpolicy(&attr, scheduler);
 	if (rv != 0) {
-		printf("px4_task_spawn_cmd: failed to set sched policy\n");
+		PX4_WARN("px4_task_spawn_cmd: failed to set sched policy");
 		return (rv < 0) ? rv : -rv;
 	}
 
@@ -156,7 +157,7 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int
 
 	rv = pthread_attr_setschedparam(&attr, &param);
 	if (rv != 0) {
-		printf("px4_task_spawn_cmd: failed to set sched param\n");
+		PX4_WARN("px4_task_spawn_cmd: failed to set sched param");
 		return (rv < 0) ? rv : -rv;
 	}
 
@@ -167,7 +168,7 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int
 			//printf("WARNING: NOT RUNING AS ROOT, UNABLE TO RUN REALTIME THREADS\n");
         		rv = pthread_create (&task, NULL, &entry_adapter, (void *) taskdata);
 			if (rv != 0) {
-				printf("px4_task_spawn_cmd: failed to create thread %d %d\n", rv, errno);
+				PX4_ERR("px4_task_spawn_cmd: failed to create thread %d %d\n", rv, errno);
 				return (rv < 0) ? rv : -rv;
 			}
 		}
@@ -194,7 +195,7 @@ int px4_task_delete(px4_task_t id)
 {
 	int rv = 0;
 	pthread_t pid;
-	printf("Called px4_task_delete\n");
+	PX4_WARN("Called px4_task_delete");
 
 	if (id < PX4_MAX_TASKS && taskmap[id].isused)
 		pid = taskmap[id].pid;
@@ -227,9 +228,9 @@ void px4_task_exit(int ret)
 		}
 	}
 	if (i>=PX4_MAX_TASKS) 
-		printf("px4_task_exit: self task not found!\n");
+		PX4_ERR("px4_task_exit: self task not found!");
 	else
-		printf("px4_task_exit: %s\n", taskmap[i].name.c_str());
+		PX4_DBG("px4_task_exit: %s", taskmap[i].name.c_str());
 
 	pthread_exit((void *)(unsigned long)ret);
 }
@@ -249,7 +250,7 @@ int px4_task_kill(px4_task_t id, int sig)
 {
 	int rv = 0;
 	pthread_t pid;
-	//printf("Called px4_task_delete\n");
+	PX4_DBG("Called px4_task_kill %d", sig);
 
 	if (id < PX4_MAX_TASKS && taskmap[id].pid != 0)
 		pid = taskmap[id].pid;
@@ -267,15 +268,15 @@ void px4_show_tasks()
 	int idx;
 	int count = 0;
 
-	printf("Active Tasks:\n");
+	PX4_INFO("Active Tasks:");
 	for (idx=0; idx < PX4_MAX_TASKS; idx++)
 	{
 		if (taskmap[idx].isused) {
-			printf("   %-10s %lu\n", taskmap[idx].name.c_str(), taskmap[idx].pid);
+			PX4_INFO("   %-10s %lu", taskmap[idx].name.c_str(), taskmap[idx].pid);
 			count++;
 		}
 	}
 	if (count == 0)
-		printf("   No running tasks\n");
+		PX4_INFO("   No running tasks");
 
 }
diff --git a/src/platforms/px4_debug.h b/src/platforms/px4_debug.h
new file mode 100644
index 0000000000..0f5b5ec34c
--- /dev/null
+++ b/src/platforms/px4_debug.h
@@ -0,0 +1,71 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2015 Mark Charlebois. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file px4_debug.h
+ * Platform dependant debug
+ */
+
+#pragma once
+
+#if defined(__PX4_LINUX) || defined(__PX4_QURT)
+#include <err.h>
+
+#define PX4_DBG(...)	
+#define PX4_INFO(...) 	warnx(__VA_ARGS__)
+#define PX4_WARN(...) 	warnx(__VA_ARGS__)
+#define PX4_ERR(...)	{ warnx("ERROR %s %s:", __FILE__, __LINE__); warnx(__VA_ARGS__); }
+
+#elif defined(__PX4_ROS)
+
+#define PX4_DBG(...) 
+#define PX4_INFO(...)	ROS_WARN(__VA_ARGS__)
+#define PX4_WARN(...) 	ROS_WARN(__VA_ARGS__)
+#define PX4_ERR(...) 	ROS_WARN(__VA_ARGS__)
+
+#elif defined(__PX4_NUTTX)
+#include <err.h>
+
+#define PX4_DBG(...) 
+#define PX4_INFO(...)	warnx(__VA_ARGS__)
+#define PX4_WARN(...)	warnx(__VA_ARGS__)
+#define PX4_ERR(...) 	warnx(__VA_ARGS__)
+
+#else
+
+#define PX4_DBG(...) 
+#define PX4_WARN(...) 
+#define PX4_INFO(...) 
+#define PX4_ERR(...) 
+
+#endif
diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h
index 8fcc9695ad..1b2cf1dea5 100644
--- a/src/platforms/px4_defines.h
+++ b/src/platforms/px4_defines.h
@@ -38,6 +38,9 @@
  */
 
 #pragma once
+
+#include <px4_debug.h>
+
 /* Get the name of the default value fiven the param name */
 #define PX4_PARAM_DEFAULT_VALUE_NAME(_name) PARAM_##_name##_DEFAULT
 
@@ -60,10 +63,6 @@
 /* Main entry point */
 #define PX4_MAIN_FUNCTION(_prefix) int main(int argc, char **argv)
 
-/* Print/output wrappers */
-#define PX4_WARN ROS_WARN
-#define PX4_INFO ROS_INFO
-
 /* Get value of parameter by name, which is equal to the handle for ros */
 #define PX4_PARAM_GET_BYNAME(_name, _destpt) ros::param::get(_name, *_destpt)
 
@@ -75,10 +74,6 @@
 /* Main entry point */
 #define PX4_MAIN_FUNCTION(_prefix) int _prefix##_task_main(int argc, char *argv[])
 
-/* Print/output wrappers */
-#define PX4_WARN warnx
-#define PX4_INFO warnx
-
 /* Parameter handle datatype */
 #include <systemlib/param/param.h>
 typedef param_t px4_param_t;