/**
* Disable all multirotor motors when in fw mode.
*/
void
Standard::set_max_mc(unsigned pwm_value)
{
	int ret;
	unsigned servo_count;
	const char *dev = PWM_OUTPUT0_DEVICE_PATH;
	int fd = px4_open(dev, 0);

	if (fd < 0) {
		PX4_WARN("can't open %s", dev);
	}

	ret = px4_ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count);
	struct pwm_output_values pwm_values;
	memset(&pwm_values, 0, sizeof(pwm_values));

	for (int i = 0; i < _params->vtol_motor_count; i++) {
		pwm_values.values[i] = pwm_value;
		pwm_values.channel_count = _params->vtol_motor_count;
	}

	ret = px4_ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_values);

	if (ret != OK) {
		PX4_WARN("failed setting max values");
	}

	px4_close(fd);
}

static int
load(const char *devname, const char *fname)
{
	// sleep a while to ensure device has been set up
	usleep(20000);
	
	int		dev;
	char		buf[2048];

	/* open the device */
	if ((dev = px4_open(devname, 0)) < 0) {
		warnx("can't open %s\n", devname);
		return 1;
	}

	/* reset mixers on the device */
	if (px4_ioctl(dev, MIXERIOCRESET, 0)) {
		warnx("can't reset mixers on %s", devname);
		return 1;
	}

	if (load_mixer_file(fname, &buf[0], sizeof(buf)) < 0) {
		warnx("can't load mixer: %s", fname);
		return 1;
	}

	/* XXX pass the buffer to the device */
	int ret = px4_ioctl(dev, MIXERIOCLOADBUF, (unsigned long)buf);

	if (ret < 0) {
		warnx("error loading mixers from %s", fname);
		return 1;
	}
	return 0;
}

// Start the driver.
// This function call only returns once the driver is up and running
// or failed to detect the sensor.
void
start(uint8_t i2c_bus)
{
	int fd = -1;

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

	g_dev = new MS5525(i2c_bus, I2C_ADDRESS_1_MS5525DSO, PATH_MS5525);

	/* check if the MS4525DO was instantiated */
	if (g_dev == nullptr) {
		goto fail;
	}

	/* try the next MS5525DSO if init fails */
	if (OK != g_dev->Airspeed::init()) {
		delete g_dev;

		PX4_WARN("trying MS5525 address 2");

		g_dev = new MS5525(i2c_bus, I2C_ADDRESS_2_MS5525DSO, PATH_MS5525);

		/* check if the MS5525DSO was instantiated */
		if (g_dev == nullptr) {
			PX4_WARN("MS5525 was not instantiated");
			goto fail;
		}

		/* both versions failed if the init for the MS5525DSO fails, give up */
		if (OK != g_dev->Airspeed::init()) {
			PX4_WARN("MS5525 init fail");
			goto fail;
		}
	}

	/* set the poll rate to default, starts automatic data collection */
	fd = px4_open(PATH_MS5525, 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 MS5525 airspeed sensor connected");
}

/**
 * @brief Resets the driver.
 */
void
reset()
{
	if (!instance) {
		PX4_WARN("No ll40ls driver running");
		return;
	}

	int fd = px4_open(instance->get_dev_name(), O_RDONLY);

	if (fd < 0) {
		PX4_ERR("Error opening fd");
		return;
	}

	if (px4_ioctl(fd, SENSORIOCRESET, 0) < 0) {
		PX4_ERR("driver reset failed");
		px4_close(fd);
		return;
	}

	if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
		PX4_ERR("driver poll restart failed");
		px4_close(fd);
		return;
	}
}

bool VtolType::init()
{
	const char *dev = PWM_OUTPUT0_DEVICE_PATH;
	int fd = px4_open(dev, 0);

	if (fd < 0) {
		PX4_ERR("can't open %s", dev);
		return false;
	}

	int ret = px4_ioctl(fd, PWM_SERVO_GET_MAX_PWM, (long unsigned int)&_max_mc_pwm_values);


	if (ret != PX4_OK) {
		PX4_ERR("failed getting max values");
		px4_close(fd);
		return false;
	}


	ret = px4_ioctl(fd, PWM_SERVO_GET_DISARMED_PWM, (long unsigned int)&_disarmed_pwm_values);

	if (ret != PX4_OK) {
		PX4_ERR("failed getting disarmed values");
		px4_close(fd);
		return false;
	}

	return true;

}

/**
* Adjust idle speed for fw mode.
*/
void VtolType::set_idle_fw()
{
	const char *dev = PWM_OUTPUT0_DEVICE_PATH;
	int fd = px4_open(dev, 0);

	if (fd < 0) {
		PX4_WARN("can't open %s", dev);
	}

	struct pwm_output_values pwm_values;

	memset(&pwm_values, 0, sizeof(pwm_values));

	for (int i = 0; i < _params->vtol_motor_count; i++) {

		pwm_values.values[i] = PWM_MOTOR_OFF;
		pwm_values.channel_count++;
	}

	int ret = px4_ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values);

	if (ret != OK) {
		PX4_WARN("failed setting min values");
	}

	px4_close(fd);
}

int test_gpio(int argc, char *argv[])
{
	int		ret = 0;

#ifdef PX4IO_DEVICE_PATH

	int fd = px4_open(PX4IO_DEVICE_PATH, 0);

	if (fd < 0) {
		printf("GPIO: open fail\n");
		return ERROR;
	}

	/* set all GPIOs to default state */
	px4_ioctl(fd, GPIO_RESET, ~0);


	/* XXX need to add some GPIO waving stuff here */


	/* Go back to default */
	px4_ioctl(fd, GPIO_RESET, ~0);

	px4_close(fd);
	printf("\t GPIO test successful.\n");

#endif


	return ret;
}

int uORB::Manager::node_advertise
(
    const struct orb_metadata *meta,
    int *instance,
    int priority
)
{
  int fd = -1;
  int ret = ERROR;

  /* fill advertiser data */
  const struct orb_advertdata adv = { meta, instance, priority };

  /* open the control device */
  fd = px4_open(TOPIC_MASTER_DEVICE_PATH, 0);

  if (fd < 0)
    goto out;

  /* advertise the object */
  ret = px4_ioctl(fd, ORBIOCADVERTISE, (unsigned long)(uintptr_t)&adv);

  /* it's PX4_OK if it already exists */
  if ((PX4_OK != ret) && (EEXIST == errno)) {
    ret = PX4_OK;
  }

out:

  if (fd >= 0)
    px4_close(fd);

  return ret;
}

/**
 * Perform some basic functional tests on the driver;
 * make sure we can collect data from the sensor in polled
 * and automatic modes.
 */
void
test()
{
	struct differential_pressure_s report;
	ssize_t sz;
	int ret;

	int fd = px4_open(ETS_PATH, O_RDONLY);

	if (fd < 0) {
		PX4_ERR("%s open failed (try 'ets_airspeed start' if the driver is not running", ETS_PATH);
	}

	/* do a simple demand read */
	sz = px4_read(fd, &report, sizeof(report));

	if (sz != sizeof(report)) {
		PX4_ERR("immediate read failed");
	}

	PX4_INFO("single read");
	PX4_INFO("diff pressure: %f pa", (double)report.differential_pressure_filtered_pa);

	/* start the sensor polling at 2Hz */
	if (OK != px4_ioctl(fd, SENSORIOCSPOLLRATE, 2)) {
		PX4_ERR("failed to set 2Hz poll rate");
	}

	/* read the sensor 5x and report each value */
	for (unsigned i = 0; i < 5; i++) {
		struct pollfd fds;

		/* wait for data to be ready */
		fds.fd = fd;
		fds.events = POLLIN;
		ret = poll(&fds, 1, 2000);

		if (ret != 1) {
			PX4_ERR("timed out waiting for sensor data");
		}

		/* now go get it */
		sz = px4_read(fd, &report, sizeof(report));

		if (sz != sizeof(report)) {
			err(1, "periodic read failed");
		}

		PX4_INFO("periodic read %u", i);
		PX4_INFO("diff pressure: %f pa", (double)report.differential_pressure_filtered_pa);
	}

	/* reset the sensor polling to its default rate */
	if (OK != px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) {
		PX4_ERR("failed to set default rate");
	}

	errx(0, "PASS");
}

/**
 * Start the driver on a specific bus.
 *
 * This function call only returns once the driver is up and running
 * or failed to detect the sensor.
 */
int
start_bus(uint8_t i2c_bus)
{
	int fd = -1;

	if (g_dev != nullptr) {
		PX4_WARN("already started");
		return PX4_ERROR;
	}

	g_dev = new SDP3X(i2c_bus, I2C_ADDRESS_1_SDP3X, PATH_SDP3X);

	/* check if the SDP3XDSO was instantiated */
	if (g_dev == nullptr) {
		goto fail;
	}

	/* try the next SDP3XDSO if init fails */
	if (g_dev->init() != PX4_OK) {
		delete g_dev;

		g_dev = new SDP3X(i2c_bus, I2C_ADDRESS_2_SDP3X, PATH_SDP3X);

		/* check if the SDP3XDSO was instantiated */
		if (g_dev == nullptr) {
			PX4_ERR("SDP3X was not instantiated (RAM)");
			goto fail;
		}

		/* both versions failed if the init for the SDP3XDSO fails, give up */
		if (g_dev->init() != PX4_OK) {
			goto fail;
		}
	}

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

	if (fd < 0) {
		goto fail;
	}

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

	return PX4_OK;

fail:

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

	PX4_WARN("not started on bus %d", i2c_bus);

	return PX4_ERROR;
}

/**
 * Perform some basic functional tests on the driver;
 * make sure we can collect data from the sensor in polled
 * and automatic modes.
 */
void
test()
{
	struct distance_sensor_s report;
	ssize_t sz;

	int fd = px4_open(RANGE_FINDER0_DEVICE_PATH, O_RDONLY);

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

	/* do a simple demand read */
	sz = px4_read(fd, &report, sizeof(report));

	if (sz != sizeof(report)) {
		err(1, "immediate read failed");
	}

	print_message(report);

	/* start the sensor polling at 2 Hz rate */
	if (OK != px4_ioctl(fd, SENSORIOCSPOLLRATE, 2)) {
		errx(1, "failed to set 2Hz poll rate");
	}

	/* read the sensor 5x and report each value */
	for (unsigned i = 0; i < 5; i++) {
		px4_pollfd_struct_t fds{};

		/* wait for data to be ready */
		fds.fd = fd;
		fds.events = POLLIN;
		int ret = px4_poll(&fds, 1, 2000);

		if (ret != 1) {
			warnx("timed out");
			break;
		}

		/* now go get it */
		sz = px4_read(fd, &report, sizeof(report));

		if (sz != sizeof(report)) {
			warnx("read failed: got %d vs exp. %d", sz, sizeof(report));
			break;
		}

		print_message(report);
	}

	/* reset the sensor polling to the default rate */
	if (OK != px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) {
		errx(1, "ERR: DEF RATE");
	}

	errx(0, "PASS");
}

// perform some basic functional tests on the driver;
// make sure we can collect data from the sensor in polled
// and automatic modes.
void test()
{
	int fd = px4_open(PATH_MS5525, O_RDONLY);

	if (fd < 0) {
		PX4_WARN("%s open failed (try 'ms5525_airspeed start' if the driver is not running", PATH_MS5525);
		return;
	}

	// do a simple demand read
	differential_pressure_s report;
	ssize_t sz = px4_read(fd, &report, sizeof(report));

	if (sz != sizeof(report)) {
		PX4_WARN("immediate read failed");
		return;
	}

	PX4_WARN("single read");
	PX4_WARN("diff pressure: %d pa", (int)report.differential_pressure_filtered_pa);

	/* start the sensor polling at 2Hz */
	if (OK != px4_ioctl(fd, SENSORIOCSPOLLRATE, 2)) {
		PX4_WARN("failed to set 2Hz poll rate");
		return;
	}

	/* read the sensor 5x and report each value */
	for (unsigned i = 0; i < 5; i++) {
		px4_pollfd_struct_t fds;

		/* wait for data to be ready */
		fds.fd = fd;
		fds.events = POLLIN;
		int ret = px4_poll(&fds, 1, 2000);

		if (ret != 1) {
			PX4_ERR("timed out");
		}

		/* now go get it */
		sz = px4_read(fd, &report, sizeof(report));

		if (sz != sizeof(report)) {
			PX4_ERR("periodic read failed");
		}

		PX4_WARN("periodic read %u", i);
		PX4_WARN("diff pressure: %d pa", (int)report.differential_pressure_filtered_pa);
		PX4_WARN("temperature: %d C (0x%02x)", (int)report.temperature, (unsigned) report.temperature);
	}

	/* reset the sensor polling to its default rate */
	if (PX4_OK != px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) {
		PX4_WARN("failed to set default rate");
	}
}

/**
 * Start the driver.
 *
 * This function call only returns once the driver is up and running
 * or failed to detect the sensor.
 */
int
start(int i2c_bus)
{
	int fd;

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

	/* create the driver, try the MS4525DO first */
	g_dev = new MEASAirspeedSim(i2c_bus, I2C_ADDRESS_MS4525DO, PATH_MS4525);

	/* check if the MS4525DO was instantiated */
	if (g_dev == nullptr) {
		goto fail;
	}

	/* try the MS5525DSO next if init fails */
	if (OK != g_dev->AirspeedSim::init()) {
		delete g_dev;
		g_dev = new MEASAirspeedSim(i2c_bus, I2C_ADDRESS_MS5525DSO, PATH_MS5525);

		/* check if the MS5525DSO was instantiated */
		if (g_dev == nullptr) {
			goto fail;
		}

		/* both versions failed if the init for the MS5525DSO fails, give up */
		if (OK != g_dev->AirspeedSim::init()) {
			goto fail;
		}
	}

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

	if (fd < 0) {
		goto fail;
	}

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

	return 0;

fail:

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

	PX4_ERR("no MS4525 airspeedSim sensor connected");
	return 1;
}

int test_tone(int argc, char *argv[])
{
	int fd, result;
	unsigned long tone;

	fd = px4_open(TONEALARM0_DEVICE_PATH, O_WRONLY);

	if (fd < 0) {
		printf("failed opening " TONEALARM0_DEVICE_PATH "\n");
		goto out;
	}

	tone = 1;

	if (argc == 2) {
		tone = atoi(argv[1]);
	}

	if (tone  == 0) {
		result = px4_ioctl(fd, TONE_SET_ALARM, TONE_STOP_TUNE);

		if (result < 0) {
			printf("failed clearing alarms\n");
			goto out;

		} else {
			printf("Alarm stopped.\n");
		}

	} else {
		result = px4_ioctl(fd, TONE_SET_ALARM, TONE_STOP_TUNE);

		if (result < 0) {
			printf("failed clearing alarms\n");
			goto out;
		}

		result = px4_ioctl(fd, TONE_SET_ALARM, tone);

		if (result < 0) {
			printf("failed setting alarm %lu\n", tone);

		} else {
			printf("Alarm %lu (disable with: tests tone 0)\n", tone);
		}
	}

out:

	if (fd >= 0) {
		px4_close(fd);
	}

	return 0;
}

int uORB::Manager::orb_exists(const struct orb_metadata *meta, int instance)
{
	/*
	 * Generate the path to the node and try to open it.
	 */
	char path[orb_maxpath];
	int inst = instance;
	int ret = uORB::Utils::node_mkpath(path, meta, &inst);

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

#if defined(__PX4_NUTTX)
	struct stat buffer;
	ret = stat(path, &buffer);
#else
	ret = px4_access(path, F_OK);

#ifdef ORB_COMMUNICATOR

	if (ret == -1 && meta != nullptr && !_remote_topics.empty()) {
		ret = (_remote_topics.find(meta->o_name) != _remote_topics.end()) ? OK : PX4_ERROR;
	}

#endif /* ORB_COMMUNICATOR */

#endif

	if (ret == 0) {
		// we know the topic exists, but it's not necessarily advertised/published yet (for example
		// if there is only a subscriber)
		// The open() will not lead to memory allocations.
		int fd = px4_open(path, 0);

		if (fd >= 0) {
			unsigned long is_published;

			if (px4_ioctl(fd, ORBIOCISPUBLISHED, (unsigned long)&is_published) == 0) {
				if (!is_published) {
					ret = PX4_ERROR;
				}
			}

			px4_close(fd);
		}
	}

	return ret;
}

void Tiltrotor::set_rear_motor_state(rear_motor_state state)
{
	int pwm_value = PWM_DEFAULT_MAX;

	// map desired rear rotor state to max allowed pwm signal
	switch (state) {
	case ENABLED:
		pwm_value = PWM_DEFAULT_MAX;
		_rear_motors = ENABLED;
		break;

	case DISABLED:
		pwm_value = PWM_LOWEST_MAX;
		_rear_motors = DISABLED;
		break;

	case IDLE:
		pwm_value = _params->idle_pwm_mc;
		_rear_motors = IDLE;
		break;
	}

	int ret;
	unsigned servo_count;
	char *dev = PWM_OUTPUT0_DEVICE_PATH;
	int fd = px4_open(dev, 0);

	if (fd < 0) {PX4_WARN("can't open %s", dev);}

	ret = px4_ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count);
	struct pwm_output_values pwm_values;
	memset(&pwm_values, 0, sizeof(pwm_values));

	for (int i = 0; i < _params->vtol_motor_count; i++) {
		if (is_motor_off_channel(i)) {
			pwm_values.values[i] = pwm_value;

		} else {
			pwm_values.values[i] = PWM_DEFAULT_MAX;
		}

		pwm_values.channel_count = _params->vtol_motor_count;
	}

	ret = px4_ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_values);

	if (ret != OK) {PX4_WARN("failed setting max values");}

	px4_close(fd);
}

static int
accel(int argc, char *argv[], const char *path)
{
	printf("\tACCEL: test start\n");
	fflush(stdout);

	int		fd;
	struct accel_report buf;
	int		ret;

	fd = px4_open(path, O_RDONLY);

	if (fd < 0) {
		printf("\tACCEL: open fail, run <mpu6000 start> or <lsm303 start> or <bma180 start> first.\n");
		return ERROR;
	}

	/* wait at least 100ms, sensor should have data after no more than 20ms */
	usleep(100000);

	/* read data - expect samples */
	ret = px4_read(fd, &buf, sizeof(buf));

	if (ret != sizeof(buf)) {
		printf("\tACCEL: read1 fail (%d)\n", ret);
		return ERROR;

	} else {
		printf("\tACCEL accel: x:%8.4f\ty:%8.4f\tz:%8.4f m/s^2\n", (double)buf.x, (double)buf.y, (double)buf.z);
	}

	if (fabsf(buf.x) > 30.0f || fabsf(buf.y) > 30.0f || fabsf(buf.z) > 30.0f) {
		warnx("ACCEL acceleration values out of range!");
		return ERROR;
	}

	float len = sqrtf(buf.x * buf.x + buf.y * buf.y + buf.z * buf.z);

	if (len < 8.0f || len > 12.0f) {
		warnx("ACCEL scale error!");
		return ERROR;
	}

	/* Let user know everything is ok */
	printf("\tOK: ACCEL passed all tests successfully\n");
	px4_close(fd);

	return OK;
}

int led_init()
{
	blink_msg_end = 0;

	/* first open normal LEDs */
	leds = px4_open(LED0_DEVICE_PATH, 0);

	if (leds < 0) {
		warnx("LED: px4_open fail\n");
		return ERROR;
	}

	/* the blue LED is only available on FMUv1 & AeroCore but not FMUv2 */
	(void)px4_ioctl(leds, LED_ON, LED_BLUE);

	/* switch blue off */
	led_off(LED_BLUE);

	/* we consider the amber led mandatory */
	if (px4_ioctl(leds, LED_ON, LED_AMBER)) {
		warnx("Amber LED: px4_ioctl fail\n");
		return ERROR;
	}

	/* switch amber off */
	led_off(LED_AMBER);

	/* then try RGB LEDs, this can fail on FMUv1*/
	rgbleds = px4_open(RGBLED0_DEVICE_PATH, 0);

	if (rgbleds < 0) {
		warnx("No RGB LED found at " RGBLED0_DEVICE_PATH);
	}

	return 0;
}

/**
 * Start the driver.
 *
 * This function call only returns once the driver is up and running
 * or failed to detect the sensor.
 */
int
start(int i2c_bus)
{
	int fd;

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

	/* create the driver, try the MS4525DO first */
	g_dev = new MEASAirspeed(i2c_bus, I2C_ADDRESS_MS4525DO, PATH_MS4525);

	/* check if the MS4525DO was instantiated */
	if (g_dev == nullptr) {
		goto fail;
	}

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

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

	if (fd < 0) {
		goto fail;
	}

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

	return PX4_OK;

fail:

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

	PX4_WARN("not started on bus %d", i2c_bus);

	return PX4_ERROR;
}

int test_led(int argc, char *argv[])
{
	int		fd;
	int		ret = 0;

	fd = px4_open(LED0_DEVICE_PATH, 0);

	if (fd < 0) {
		printf("\tLED: open fail\n");
		return ERROR;
	}

	if (px4_ioctl(fd, LED_ON, LED_BLUE) ||
	    px4_ioctl(fd, LED_ON, LED_AMBER)) {

		printf("\tLED: ioctl fail\n");
		return ERROR;
	}

	/* let them blink for fun */

	int i;
	uint8_t ledon = 1;

	for (i = 0; i < 10; i++) {
		if (ledon) {
			px4_ioctl(fd, LED_ON, LED_BLUE);
			px4_ioctl(fd, LED_OFF, LED_AMBER);

		} else {
			px4_ioctl(fd, LED_OFF, LED_BLUE);
			px4_ioctl(fd, LED_ON, LED_AMBER);
		}

		ledon = !ledon;
		usleep(60000);
	}

	/* Go back to default */
	px4_ioctl(fd, LED_ON, LED_BLUE);
	px4_ioctl(fd, LED_OFF, LED_AMBER);

	printf("\t LED test completed, no errors.\n");

	return ret;
}