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

}

int px4_task_delete(px4_task_t id)
{
    int rv = 0;
    pthread_t pid;
    PX4_DEBUG("Called px4_task_delete");

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

    // If current thread then exit, otherwise cancel
    if (pthread_self() == pid) {
        taskmap[id].isused = false;
        pthread_exit(0);
    } else {
        rv = pthread_cancel(pid);
    }

    taskmap[id].isused = false;

    return rv;
}

int16_t uORB::Manager::process_received_message(const char *messageName, int32_t length, uint8_t *data)
{
	int16_t rc = -1;
	char nodepath[orb_maxpath];
	int ret = uORB::Utils::node_mkpath(nodepath, messageName);
	DeviceMaster *device_master = get_device_master();

	if (ret == OK && device_master) {
		uORB::DeviceNode *node = device_master->getDeviceNode(nodepath);

		// get the node name.
		if (node == nullptr) {
			PX4_DEBUG("No existing subscriber found for message: [%s] nodepath:[%s]", messageName, nodepath);

		} else {
			// node is present.
			node->process_received_message(length, data);
			rc = 0;
		}
	}

	return rc;
}

int16_t uORB::FastRpcChannel::is_subscriber_present(const char *messageName, int32_t *status)
{
	int16_t rc = 0;

	if (std::find(_Subscribers.begin(), _Subscribers.end(), messageName) != _Subscribers.end()) {
		*status = 1;
		//PX4_DEBUG("******* Found subscriber for message[%s]....", messageName);

	} else {
		*status = 0;
		//PX4_WARN("@@@@@ Subscriber not found for[%s]...numSubscribers[%d]", messageName, _Subscribers.size());
		int i = 0;

		for (std::list<std::string>::iterator it = _Subscribers.begin(); it != _Subscribers.end(); ++it) {
			if (*it == messageName) {
				PX4_DEBUG("##### Found the message[%s] in the subscriber list-index[%d]", messageName, i);
			}

			++i;
		}
	}

	return rc;
}

static void
hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime interval, hrt_callout callout, void *arg)
{
	PX4_DEBUG("hrt_call_internal deadline=%lu interval = %lu", deadline, interval);
	hrt_lock();

	//PX4_INFO("hrt_call_internal after lock");
	/* if the entry is currently queued, remove it */
	/* note that we are using a potentially uninitialised
	   entry->link here, but it is safe as sq_rem() doesn't
	   dereference the passed node unless it is found in the
	   list. So we potentially waste a bit of time searching the
	   queue for the uninitialised entry->link but we don't do
	   anything actually unsafe.
	*/
	if (entry->deadline != 0) {
		sq_rem(&entry->link, &callout_queue);
	}

#if 1

	// Use this to debug busy CPU that keeps rescheduling with 0 period time
	/*if (interval < HRT_INTERVAL_MIN) {*/
	/*PX4_ERR("hrt_call_internal interval too short: %" PRIu64, interval);*/
	/*PX4_BACKTRACE();*/
	/*}*/

#endif
	entry->deadline = deadline;
	entry->period = interval;
	entry->callout = callout;
	entry->arg = arg;

	hrt_call_enter(entry);
	hrt_unlock();
}

int get_shmem_lock(const char *caller_file_name, int caller_line_number)
{
	unsigned char *lock = (unsigned char *)(MAP_ADDRESS + LOCK_OFFSET);
	unsigned int i = 0;

	while (!atomic_compare_and_set(lock, 1, 0)) {
		PX4_INFO("Could not get lock, file name: %s, line number: %d.\n",
			 caller_file_name, caller_line_number);
		i++;
		usleep(1000);

		if (i > 100) { break; }
	}

	if (i > 100) {
		return -1;

	} else {
		PX4_DEBUG("Lock acquired, file name: %s, line number: %d\n", caller_file_name, caller_line_number);
	}

	return 0; //got the lock

}