C++ PX4_DEBUG函数代码示例

int px4_task_kill(px4_task_t id, int sig)
{
	int rv = 0;
	pthread_t pid;
	PX4_DEBUG("Called px4_task_kill %d", sig);

	if (id < PX4_MAX_TASKS && taskmap[id].isused && taskmap[id].pid != 0)
		pid = taskmap[id].pid;
	else
		return -EINVAL;

	// If current thread then exit, otherwise cancel
	rv = pthread_kill(pid, sig);

	return rv;
}

int
VDev::remove_poll_waiter(px4_pollfd_struct_t *fds)
{
	PX4_DEBUG("VDev::remove_poll_waiter");
	for (unsigned i = 0; i < _max_pollwaiters; i++) {
		if (fds == _pollset[i]) {

			_pollset[i] = nullptr;
			return PX4_OK;

		}
	}

	PX4_WARN("poll: bad fd state");
	return -EINVAL;
}

int
AirspeedSim::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len) {
    if (recv_len > 0) {
        // this is equivalent to the collect phase
        Simulator *sim = Simulator::getInstance();
        if (sim == NULL) {
            PX4_ERR("Error BARO_SIM::transfer no simulator");
            return -ENODEV;
        }
        PX4_DEBUG("BARO_SIM::transfer getting sample");
        sim->getAirspeedSample(recv, recv_len);
    } else {
        // we don't need measure phase
    }

    return 0;
}

static void update_index_from_shmem(void)
{
	unsigned int i;

	if (get_shmem_lock(__FILE__, __LINE__) != 0) {
		PX4_ERR("Could not get shmem lock");
		return;
	}

	PX4_DEBUG("Updating index from shmem");

	for (i = 0; i < MAX_SHMEM_PARAMS / 8 + 1; i++) {
		adsp_changed_index[i] = shmem_info_p->adsp_changed_index[i];
	}

	release_shmem_lock();
}

int
MPU9250::select_register_bank(uint8_t bank)
{
	uint8_t ret;
	uint8_t buf;
	uint8_t retries = 3;

	if (_selected_bank != bank) {
		ret = _interface->write(MPU9250_LOW_SPEED_OP(ICMREG_20948_BANK_SEL), &bank, 1);

		if (ret != OK) {
			return ret;
		}
	}

	/*
	 * Making sure the right register bank is selected (even if it should be). Observed some
	 * unexpected changes to this, don't risk writing to the wrong register bank.
	 */
	_interface->read(MPU9250_LOW_SPEED_OP(ICMREG_20948_BANK_SEL), &buf, 1);

	while (bank != buf && retries > 0) {
		//PX4_WARN("user bank: expected %d got %d",bank,buf);
		ret = _interface->write(MPU9250_LOW_SPEED_OP(ICMREG_20948_BANK_SEL), &bank, 1);

		if (ret != OK) {
			return ret;
		}

		retries--;
		//PX4_WARN("BANK retries: %d", 4-retries);

		_interface->read(MPU9250_LOW_SPEED_OP(ICMREG_20948_BANK_SEL), &buf, 1);
	}


	_selected_bank = bank;

	if (bank != buf) {
		PX4_DEBUG("SELECT FAILED %d %d %d %d", retries, _selected_bank, bank, buf);
		return PX4_ERROR;

	} else {
		return PX4_OK;
	}
}

VDev::VDev(const char *name,
	   const char *devname) :
	// base class
	Device(name),
	// public
	// protected
	_pub_blocked(false),
	// private
	_devname(devname),
	_registered(false),
	_open_count(0)
{
	PX4_DEBUG("VDev::VDev");

	for (unsigned i = 0; i < _max_pollwaiters; i++) {
		_pollset[i] = nullptr;
	}
}

int px4_task_kill(px4_task_t id, int sig)
{
	int rv = 0;
	pthread_t pid;
	PX4_DEBUG("Called px4_task_kill %d, taskname %s", sig, taskmap[id].name.c_str());

	if (id < PX4_MAX_TASKS && taskmap[id].pid != 0) {
		pid = taskmap[id].pid;

	} else {
		return -EINVAL;
	}

	// If current thread then exit, otherwise cancel
	rv = pthread_kill(pid, sig);

	return rv;
}

/**
 * Print a little info about the driver.
 */
int
info()
{
	if (g_dev == nullptr) {
		PX4_ERR("driver not running");
		return 1;
	}

	PX4_DEBUG("state @ %p", g_dev);

	int ret = g_dev->print_info();

	if (ret != 0) {
		PX4_ERR("Unable to print info");
		return ret;
	}

	return 0;
}

int
VDev::store_poll_waiter(px4_pollfd_struct_t *fds)
{
	/*
	 * Look for a free slot.
	 */
	PX4_DEBUG("VDev::store_poll_waiter");
	for (unsigned i = 0; i < _max_pollwaiters; i++) {
		if (nullptr == _pollset[i]) {

			/* save the pollfd */
			_pollset[i] = fds;

			return PX4_OK;
		}
	}

	return -ENOMEM;
}

int
VDev::store_poll_waiter(px4_pollfd_struct_t *fds)
{
	/*
	 * Look for a free slot.
	 */
	PX4_DEBUG("VDev::store_poll_waiter");

	for (unsigned i = 0; i < _max_pollwaiters; i++) {
		if (nullptr == _pollset[i]) {

			/* save the pollfd */
			_pollset[i] = fds;

			return PX4_OK;
		}
	}

	/* No free slot found. Resize the pollset */

	if (_max_pollwaiters >= 256 / 2) { //_max_pollwaiters is uint8_t
		return -ENOMEM;
	}

	const uint8_t new_count = _max_pollwaiters > 0 ? _max_pollwaiters * 2 : 1;
	px4_pollfd_struct_t **new_pollset = new px4_pollfd_struct_t *[new_count];

	if (!new_pollset) {
		return -ENOMEM;
	}

	if (_max_pollwaiters > 0) {
		memset(new_pollset + _max_pollwaiters, 0, sizeof(px4_pollfd_struct_t *) * (new_count - _max_pollwaiters));
		memcpy(new_pollset, _pollset, sizeof(px4_pollfd_struct_t *) * _max_pollwaiters);
		delete[](_pollset);
	}

	_pollset = new_pollset;
	_pollset[_max_pollwaiters] = fds;
	_max_pollwaiters = new_count;
	return PX4_OK;
}

int
MPU9250::probe()
{
	int ret = PX4_ERROR;

	// Try first for mpu9250/6500
	_whoami = read_reg(MPUREG_WHOAMI);

	// If it's not an MPU it must be an ICM
	if ((_whoami != MPU_WHOAMI_9250) && (_whoami != MPU_WHOAMI_6500)) {
		// Make sure selected register bank is bank 0 (which contains WHOAMI)
		select_register_bank(REG_BANK(ICMREG_20948_WHOAMI));
		_whoami = read_reg(ICMREG_20948_WHOAMI);
	}

	if (_whoami == MPU_WHOAMI_9250 || _whoami == MPU_WHOAMI_6500) {

		_num_checked_registers = MPU9250_NUM_CHECKED_REGISTERS;
		_checked_registers = _mpu9250_checked_registers;
		memset(_checked_values, 0, MPU9250_NUM_CHECKED_REGISTERS);
		memset(_checked_bad, 0, MPU9250_NUM_CHECKED_REGISTERS);
		ret = PX4_OK;

	} else if (_whoami == ICM_WHOAMI_20948) {

		_num_checked_registers = ICM20948_NUM_CHECKED_REGISTERS;
		_checked_registers = _icm20948_checked_registers;
		memset(_checked_values, 0, ICM20948_NUM_CHECKED_REGISTERS);
		memset(_checked_bad, 0, ICM20948_NUM_CHECKED_REGISTERS);
		ret = PX4_OK;
	}

	_checked_values[0] = _whoami;
	_checked_bad[0] = _whoami;

	if (ret != PX4_OK) {
		PX4_DEBUG("unexpected whoami 0x%02x", _whoami);
	}

	return ret;
}

int
SF0X::measure()
{
	int ret;

	/*
	 * Send the command to begin a measurement.
	 */
	char cmd = SF0X_TAKE_RANGE_REG;
	ret = ::write(_fd, &cmd, 1);

	if (ret != sizeof(cmd)) {
		perf_count(_comms_errors);
		PX4_DEBUG("write fail %d", ret);
		return ret;
	}

	ret = OK;

	return ret;
}

void px4_task_exit(int ret)
{
	int i; 
	pthread_t pid = pthread_self();

	// Get pthread ID from the opaque ID
	for (i=0; i<PX4_MAX_TASKS; ++i) {
		if (taskmap[i].pid == pid) {
			taskmap[i].isused = false;
			break;
		}
	}
	if (i>=PX4_MAX_TASKS)  {
		PX4_ERR("px4_task_exit: self task not found!");
	}
	else {
		PX4_DEBUG("px4_task_exit: %s", taskmap[i].name.c_str());
	}

	pthread_exit((void *)(unsigned long)ret);
}

/**
 * worker callback method to save the parameters
 * @param arg unused
 */
static void
autosave_worker(void *arg)
{
	bool disabled = false;

	param_lock_writer();
	last_autosave_timestamp = hrt_absolute_time();
	autosave_scheduled = false;
	disabled = autosave_disabled;
	param_unlock_writer();

	if (disabled) {
		return;
	}

	PX4_DEBUG("Autosaving params");
	int ret = param_save_default();

	if (ret != 0) {
		PX4_ERR("param save failed (%i)", ret);
	}
}

static void *map_memory(off_t target)
{

	if ((mem_fd = open(MEMDEVICE, O_RDWR | O_SYNC)) == -1) {
		PX4_ERR("Cannot open %s", MEMDEVICE);
		exit(1);
	}

	/* Map one page */
	map_base = (unsigned char *) mmap(0, MAP_SIZE, PROT_READ | PROT_WRITE,
					  MAP_SHARED, mem_fd, target & ~MAP_MASK);

	if (map_base == (void *) - 1) {
		PX4_ERR("Cannot mmap /dev/atl_mem");
		exit(1);
	}

	PX4_DEBUG("Initializing map memory: mem_fd: %d, 0x%X", mem_fd, map_base + (target & MAP_MASK) + LOCK_SIZE);

	return (map_base + (target & MAP_MASK) + LOCK_SIZE);

}