fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Revamped debug macros

Browse Source 
Created px4_debug,h to define:

PX4_DBG
PX4_INFO
PX4_WARN
PX4_ERR

These enable OS specific mappings to be made, filtering, etc.

Signed-off-by: Mark Charlebois <charlebm@gmail.com>
This commit is contained in:
Mark Charlebois 2015-04-27 21:58:54 -07:00
parent 6ab25ae890
commit 09718fa324
10 changed files with 325 additions and 221 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

77
src/modules/simulator/simulator.cpp
View File

@ -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");
}
}

34
src/platforms/posix/drivers/accelsim/accelsim.cpp
View File

@ -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

11
src/platforms/posix/drivers/adcsim/adcsim.cpp
View File

@ -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;
}
}

4
src/platforms/posix/drivers/airspeedsim/airspeedsim.cpp
View File

@ -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");
}

121
src/platforms/posix/drivers/barosim/baro.cpp
View File

@ -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));
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));
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("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;
}

160
src/platforms/posix/drivers/gyrosim/gyrosim.cpp
View File

@ -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, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
err(1, "driver poll restart 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) {
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

26
src/platforms/posix/drivers/tonealrmsim/tone_alarm.cpp
View File

@ -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;
}
}

31
src/platforms/posix/px4_layer/px4_posix_tasks.cpp
View File

@ -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");
}

71
src/platforms/px4_debug.h Normal file
View File

@ -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

11
src/platforms/px4_defines.h
View File

@ -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;