int px4_prctl(int option, const char *arg2, unsigned pid)
{
	int rv;

	switch (option) {
	case PR_SET_NAME:
		// set the threads name - Not supported
		// rv = pthread_setname_np(pthread_self(), arg2);
		rv = -1;
		break;

	default:
		rv = -1;
		PX4_WARN("FAILED SETTING TASK NAME");
		break;
	}

	return rv;
}

int Ekf2::start()
{
	ASSERT(_control_task == -1);

	/* start the task */
	_control_task = px4_task_spawn_cmd("ekf2",
					   SCHED_DEFAULT,
					   SCHED_PRIORITY_MAX - 5,
					   9000,
					   (px4_main_t)&Ekf2::task_main_trampoline,
					   nullptr);

	if (_control_task < 0) {
		PX4_WARN("task start failed");
		return -errno;
	}

	return OK;
}

/**
 * Start the driver.
 *
 * This function only returns if the sensor is up and running
 * or could not be detected successfully.
 */
void
start(int i2c_bus)
{
	int fd;

	if (g_dev != nullptr) {
		PX4_ERR("already started");
	}

	/* create the driver */
	g_dev = new ETSAirspeed(i2c_bus);

	if (g_dev == nullptr) {
		goto fail;
	}

	if (OK != g_dev->Airspeed::init()) {
		goto fail;
	}

	/* set the poll rate to default, starts automatic data collection */
	fd = px4_open(AIRSPEED0_DEVICE_PATH, O_RDONLY);

	if (fd < 0) {
		goto fail;
	}

	if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
		goto fail;
	}

	return;

fail:

	if (g_dev != nullptr) {
		delete g_dev;
		g_dev = nullptr;
	}

	PX4_WARN("no ETS airspeed sensor connected");
}

int
AirspeedSim::init()
{
	int ret = ERROR;

	/* init base class */
	if (CDev::init() != OK) {
		DEVICE_DEBUG("CDev init failed");
		goto out;
	}

	/* allocate basic report buffers */
	_reports = new ringbuffer::RingBuffer(2, sizeof(differential_pressure_s));

	if (_reports == nullptr) {
		goto out;
	}

	/* register alternate interfaces if we have to */
	_class_instance = register_class_devname(AIRSPEED_BASE_DEVICE_PATH);

	/* publication init */
	if (_class_instance == CLASS_DEVICE_PRIMARY) {

		/* advertise sensor topic, measure manually to initialize valid report */
		struct differential_pressure_s arp;
		measure();
		_reports->get(&arp);

		/* measurement will have generated a report, publish */
		_airspeed_pub = orb_advertise(ORB_ID(differential_pressure), &arp);

		if (_airspeed_pub == nullptr) {
			PX4_WARN("uORB started?");
		}
	}

	ret = OK;

out:
	return ret;
}

/**
 * Start the driver.
 */
void
start(const char *uart_path, bool fake_gps, bool enable_sat_info, int gps_num)
{
	if (g_dev[gps_num - 1] != nullptr) {
		PX4_WARN("GPS %i already started", gps_num);
		return;
	}

	/* create the driver */
	g_dev[gps_num - 1] = new GPS(uart_path, fake_gps, enable_sat_info, gps_num);

	if (!g_dev[gps_num - 1] || OK != g_dev[gps_num - 1]->init()) {
		if (g_dev[gps_num - 1] != nullptr) {
			delete g_dev[gps_num - 1];
			g_dev[gps_num - 1] = nullptr;
		}

		PX4_ERR("start of GPS %i failed", gps_num);
	}
}

int RcInput::start()
{
	int result = 0;

	result = navio_rc_init();

	if (result != 0) {
		PX4_WARN("error: RC initialization failed");
		return -1;
	}

	_isRunning = true;
	result = work_queue(HPWORK, &_work, (worker_t)&RcInput::cycle_trampoline, this, 0);

	if (result == -1) {
		_isRunning = false;
	}

	return result;
}

Device::Device(const char *name) :
	// public
	// protected
	_name(name),
	_debug_enabled(false)
{
	int ret = px4_sem_init(&_lock, 0, 1);

	if (ret != 0) {
		PX4_WARN("SEM INIT FAIL: ret %d, %s", ret, strerror(errno));
	}

	/* setup a default device ID. When bus_type is UNKNOWN the
	   other fields are invalid */
	_device_id.devid = 0;
	_device_id.devid_s.bus_type = DeviceBusType_UNKNOWN;
	_device_id.devid_s.bus = 0;
	_device_id.devid_s.address = 0;
	_device_id.devid_s.devtype = 0;
}

int
VtolAttitudeControl::start()
{
	ASSERT(_control_task == -1);

	/* start the task */
	_control_task = px4_task_spawn_cmd("vtol_att_control",
					   SCHED_DEFAULT,
					   SCHED_PRIORITY_MAX - 10,
					   2048,
					   (px4_main_t)&VtolAttitudeControl::task_main_trampoline,
					   nullptr);

	if (_control_task < 0) {
		PX4_WARN("task start failed");
		return -errno;
	}

	return OK;
}

int TAP_ESC::send_packet(EscPacket &packet, int responder)
{
	if (responder >= 0) {

		if (responder > _channels_count) {
			return -EINVAL;
		}

		select_responder(responder);
	}

	int packet_len = crc_packet(packet);
	int ret = ::write(_uart_fd, &packet.head, packet_len);

	if (ret != packet_len) {
		PX4_WARN("TX ERROR: ret: %d, errno: %d", ret, errno);
	}

	return ret;
}

/**
 * Automatic scale calibration.
 *
 * Basic idea:
 *
 *   output = (ext field +- 1.1 Ga self-test) * scale factor
 *
 * and consequently:
 *
 *   1.1 Ga = (excited - normal) * scale factor
 *   scale factor = (excited - normal) / 1.1 Ga
 *
 *   sxy = (excited - normal) / 766 | for conf reg. B set to 0x60 / Gain = 3
 *   sz  = (excited - normal) / 713 | for conf reg. B set to 0x60 / Gain = 3
 *
 * By subtracting the non-excited measurement the pure 1.1 Ga reading
 * can be extracted and the sensitivity of all axes can be matched.
 *
 * SELF TEST OPERATION
 * To check the IST8310L for proper operation, a self test feature in incorporated
 * in which the sensor will change the polarity on all 3 axis. The values with and
 * with and without selftest on shoult be compared and if the absolete value are equal
 * the IC is functional.
 */
int calibrate(enum IST8310_BUS busid)
{
	int ret;
	struct ist8310_bus_option &bus = find_bus(busid);
	const char *path = bus.devpath;

	int fd = open(path, O_RDONLY);

	if (fd < 0) {
		err(1, "%s open failed (try 'ist8310 start' if the driver is not running", path);
	}

	if (OK != (ret = ioctl(fd, MAGIOCCALIBRATE, fd))) {
		PX4_WARN("failed to enable sensor calibration mode");
	}

	close(fd);

	return ret;
}

int micrortps_client_main(int argc, char *argv[])
{
	if (argc < 2) {
		usage(argv[0]);
		return -1;
	}

	if (!strcmp(argv[1], "start")) {
		PX4_WARN("PX4 built without RTPS bridge support, EXITING...\n");
		return -1;
	}

	if (!strcmp(argv[1], "stop")) {
		PX4_INFO("Not running");
		return -1;
	}

	usage(argv[0]);

	return -1;
}

/**
 * Start the driver.
 */
void
start(const char *path, gps_driver_mode_t mode, GPSHelper::Interface interface, bool fake_gps, bool enable_sat_info,
      int gps_num)
{
	if (g_dev[gps_num - 1] != nullptr) {
		PX4_WARN("GPS %i already started", gps_num);
		return;
	}

	/* create the driver */
	g_dev[gps_num - 1] = new GPS(path, mode, interface, fake_gps, enable_sat_info, gps_num);

	if (!g_dev[gps_num - 1] || OK != g_dev[gps_num - 1]->init()) {
		if (g_dev[gps_num - 1] != nullptr) {
			delete g_dev[gps_num - 1];
			g_dev[gps_num - 1] = nullptr;
		}

		PX4_ERR("start of GPS %i failed", gps_num);
	}
}

int px4_prctl(int option, const char *arg2, px4_task_t pid)
{
	int rv;

	switch (option) {
	case PR_SET_NAME:
		// set the threads name
#ifdef __PX4_DARWIN
		rv = pthread_setname_np(arg2);
#else
		rv = pthread_setname_np(pthread_self(), arg2);
#endif
		break;

	default:
		rv = -1;
		PX4_WARN("FAILED SETTING TASK NAME");
		break;
	}

	return rv;
}

int initialise_uart()
{
	// open uart
	_uart_fd = open(_device, O_RDWR | O_NOCTTY | O_NONBLOCK);
	int termios_state = -1;

	if (_uart_fd < 0) {
		PX4_ERR("failed to open uart device!");
		return -1;
	}

	// set baud rate
	int speed = B250000;
	struct termios uart_config;
	tcgetattr(_uart_fd, &uart_config);

	// clear ONLCR flag (which appends a CR for every LF)
	uart_config.c_oflag &= ~ONLCR;

	// set baud rate
	if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
		PX4_ERR("failed to set baudrate for %s: %d\n", _device, termios_state);
		close(_uart_fd);
		return -1;
	}

	if ((termios_state = tcsetattr(_uart_fd, TCSANOW, &uart_config)) < 0) {
		PX4_ERR("tcsetattr failed for %s\n", _device);
		close(_uart_fd);
		return -1;
	}

	// setup output flow control
	if (enable_flow_control(false)) {
		PX4_WARN("hardware flow disable failed");
	}

	return _uart_fd;
}

void TAP_ESC:: send_esc_outputs(const float *pwm, const unsigned num_pwm)
{

	uint16_t rpm[TAP_ESC_MAX_MOTOR_NUM];
	memset(rpm, 0, sizeof(rpm));
	uint8_t motor_cnt = num_pwm;
	static uint8_t which_to_respone = 0;

	for (uint8_t i = 0; i < motor_cnt; i++) {
		rpm[i] = pwm[i];

		if (rpm[i] > RPMMAX) {
			rpm[i] = RPMMAX;

		} else if (rpm[i] < RPMSTOPPED) {
			rpm[i] = RPMSTOPPED;
		}
	}

	rpm[which_to_respone] |= (RUN_FEEDBACK_ENABLE_MASK | RUN_BLUE_LED_ON_MASK);


	EscPacket packet = {0xfe, _channels_count, ESCBUS_MSG_ID_RUN};
	packet.len *= sizeof(packet.d.reqRun.rpm_flags[0]);

	for (uint8_t i = 0; i < _channels_count; i++) {
		packet.d.reqRun.rpm_flags[i] = rpm[i];
	}

	int ret = send_packet(packet, which_to_respone);

	if (++which_to_respone == _channels_count) {
		which_to_respone = 0;
	}

	if (ret < 1) {
		PX4_WARN("TX ERROR: ret: %d, errno: %d", ret, errno);
	}
}

void
PrecLand::on_activation()
{
	// We need to subscribe here and not in the constructor because constructor is called before the navigator task is spawned
	if (!_targetPoseSub) {
		_targetPoseSub = orb_subscribe(ORB_ID(landing_target_pose));
	}

	_state = PrecLandState::Start;
	_search_cnt = 0;
	_last_slewrate_time = 0;

	vehicle_local_position_s *vehicle_local_position = _navigator->get_local_position();

	if (!map_projection_initialized(&_map_ref)) {
		map_projection_init(&_map_ref, vehicle_local_position->ref_lat, vehicle_local_position->ref_lon);
	}

	position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();

	pos_sp_triplet->next.valid = false;

	// Check that the current position setpoint is valid, otherwise land at current position
	if (!pos_sp_triplet->current.valid) {
		PX4_WARN("Resetting landing position to current position");
		pos_sp_triplet->current.lat = _navigator->get_global_position()->lat;
		pos_sp_triplet->current.lon = _navigator->get_global_position()->lon;
		pos_sp_triplet->current.alt = _navigator->get_global_position()->alt;
		pos_sp_triplet->current.valid = true;
	}

	_sp_pev = matrix::Vector2f(0, 0);
	_sp_pev_prev = matrix::Vector2f(0, 0);
	_last_slewrate_time = 0;

	switch_to_state_start();

}

int px4_task_delete(px4_task_t id)
{
	int rv = 0;
	pthread_t pid;
	PX4_WARN("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;
}

void
PrecLand::run_state_descend_above_target()
{
	position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();

	// check if target visible
	if (!check_state_conditions(PrecLandState::DescendAboveTarget)) {
		if (!switch_to_state_final_approach()) {
			PX4_WARN("Lost landing target while landing (descending).");

			// Stay at current position for searching for the target
			pos_sp_triplet->current.lat = _navigator->get_global_position()->lat;
			pos_sp_triplet->current.lon = _navigator->get_global_position()->lon;
			pos_sp_triplet->current.alt = _navigator->get_global_position()->alt;

			if (!switch_to_state_start()) {
				if (!switch_to_state_fallback()) {
					PX4_ERR("Can't switch to fallback landing");
				}
			}
		}

		return;
	}

	// XXX need to transform to GPS coords because mc_pos_control only looks at that
	double lat, lon;
	map_projection_reproject(&_map_ref, _target_pose.x_abs, _target_pose.y_abs, &lat, &lon);

	pos_sp_triplet->current.lat = lat;
	pos_sp_triplet->current.lon = lon;

	pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_LAND;

	_navigator->set_position_setpoint_triplet_updated();

}

/**
* Main entry point for this module
**/
int land_detector_main(int argc, char *argv[])
{

	if (argc < 2) {
		goto exiterr;
	}

	if (argc >= 2 && !strcmp(argv[1], "start")) {
		if (land_detector_start(argv[2]) != 0) {
			PX4_WARN("land_detector start failed");
			return 1;
		}

		return 0;
	}

	if (!strcmp(argv[1], "stop")) {
		land_detector_stop();
		return 0;
	}

	if (!strcmp(argv[1], "status")) {
		if (land_detector_task) {

			if (land_detector_task->isRunning()) {
				PX4_WARN("running (%s): %s", _currentMode, (land_detector_task->isLanded()) ? "LANDED" : "IN AIR");

			} else {
				PX4_WARN("exists, but not running (%s)", _currentMode);
			}

			return 0;

		} else {
			PX4_WARN("not running");
			return 1;
		}
	}

exiterr:
	PX4_WARN("usage: land_detector {start|stop|status} [mode]");
	PX4_WARN("mode can either be 'fixedwing' or 'multicopter'");
	return 1;
}

int
MPU9250_SPI::probe()
{
	uint8_t whoami = 0;

	int ret = read(MPUREG_WHOAMI, &whoami, 1);

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

	switch (whoami) {
	case MPU_WHOAMI_9250:
	case MPU_WHOAMI_6500:
		ret = 0;
		break;

	default:
		PX4_WARN("probe failed! %u", whoami);
		ret = -EIO;
	}

	return ret;
}