/**
* 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);
}
bool VtolType::apply_pwm_limits(struct pwm_output_values &pwm_values, pwm_limit_type type)
{
const char *dev = PWM_OUTPUT0_DEVICE_PATH;
int fd = px4_open(dev, 0);
if (fd < 0) {
PX4_WARN("can't open %s", dev);
return false;
}
int ret;
if (type == pwm_limit_type::TYPE_MINIMUM) {
ret = px4_ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values);
} else {
ret = px4_ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_values);
}
px4_close(fd);
if (ret != OK) {
PX4_ERR("failed setting max values");
return false;
}
return true;
}
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;
}
/**
* Adjust idle speed for mc mode.
*/
void VtolType::set_idle_mc()
{
const char *dev = PWM_OUTPUT0_DEVICE_PATH;
int fd = px4_open(dev, 0);
if (fd < 0) {
PX4_WARN("can't open %s", dev);
}
unsigned servo_count;
int ret = px4_ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count);
unsigned pwm_value = _params->idle_pwm_mc;
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++;
}
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);
flag_idle_mc = true;
}
/**
* 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;
}
/**
* Reset the driver.
*/
void
reset()
{
int fd = px4_open(ETS_PATH, O_RDONLY);
if (fd < 0) {
PX4_ERR("failed ");
}
if (px4_ioctl(fd, SENSORIOCRESET, 0) < 0) {
PX4_ERR("driver reset failed");
}
if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
PX4_ERR("driver poll restart failed");
}
}
/**
* Start the driver.
*/
int
start(const char *port, uint8_t rotation)
{
int fd;
if (g_dev != nullptr) {
PX4_ERR("already started");
return 1;
}
/* create the driver */
g_dev = new TFMINI(port, rotation);
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(RANGE_FINDER0_DEVICE_PATH, O_RDONLY);
if (fd < 0) {
PX4_ERR("Opening device '%s' failed", port);
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("driver start failed");
return 1;
}
/**
* Start the driver on a specific bus.
*
* This function only returns if the sensor is up and running
* or could not be detected successfully.
*/
int
start_bus(uint8_t rotation, int i2c_bus)
{
int fd = -1;
if (g_dev != nullptr) {
PX4_ERR("already started");
return PX4_ERROR;
}
/* create the driver */
g_dev = new SF1XX(rotation, i2c_bus);
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(SF1XX_DEVICE_PATH, O_RDONLY);
if (fd < 0) {
goto fail;
}
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
px4_close(fd);
goto fail;
}
px4_close(fd);
return PX4_OK;
fail:
if (g_dev != nullptr) {
delete g_dev;
g_dev = nullptr;
}
return PX4_ERROR;
}
// 1M 8N1 serial connection to NRF51
int
Syslink::open_serial(const char *dev)
{
#ifndef B1000000
#define B1000000 1000000
#endif
int rate = B1000000;
// open uart
int fd = px4_open(dev, O_RDWR | O_NOCTTY);
int termios_state = -1;
if (fd < 0) {
PX4_ERR("failed to open uart device!");
return -1;
}
// set baud rate
struct termios config;
tcgetattr(fd, &config);
// clear ONLCR flag (which appends a CR for every LF)
config.c_oflag = 0;
config.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG);
// Disable hardware flow control
config.c_cflag &= ~CRTSCTS;
/* Set baud rate */
if (cfsetispeed(&config, rate) < 0 || cfsetospeed(&config, rate) < 0) {
warnx("ERR SET BAUD %s: %d\n", dev, termios_state);
px4_close(fd);
return -1;
}
if ((termios_state = tcsetattr(fd, TCSANOW, &config)) < 0) {
PX4_WARN("ERR SET CONF %s\n", dev);
px4_close(fd);
return -1;
}
return fd;
}
static int
gyro(int argc, char *argv[], const char *path)
{
printf("\tGYRO: test start\n");
fflush(stdout);
int fd;
struct gyro_report buf;
int ret;
fd = px4_open(path, O_RDONLY);
if (fd < 0) {
printf("\tGYRO: open fail, run <l3gd20 start> or <mpu6000 start> first.\n");
return ERROR;
}
/* wait at least 5 ms, sensor should have data after that */
usleep(5000);
/* read data - expect samples */
ret = px4_read(fd, &buf, sizeof(buf));
if (ret != sizeof(buf)) {
printf("\tGYRO: read fail (%d)\n", ret);
return ERROR;
} else {
printf("\tGYRO rates: x:%8.4f\ty:%8.4f\tz:%8.4f rad/s\n", (double)buf.x, (double)buf.y, (double)buf.z);
}
float len = sqrtf(buf.x * buf.x + buf.y * buf.y + buf.z * buf.z);
if (len > 0.3f) {
warnx("GYRO scale error!");
return ERROR;
}
/* Let user know everything is ok */
printf("\tOK: GYRO passed all tests successfully\n");
px4_close(fd);
return OK;
}
static int
mag(int argc, char *argv[], const char *path)
{
printf("\tMAG: test start\n");
fflush(stdout);
int fd;
struct mag_report buf;
int ret;
fd = px4_open(path, O_RDONLY);
if (fd < 0) {
printf("\tMAG: open fail, run <hmc5883 start> or <lsm303 start> first.\n");
return ERROR;
}
/* wait at least 5 ms, sensor should have data after that */
usleep(5000);
/* read data - expect samples */
ret = px4_read(fd, &buf, sizeof(buf));
if (ret != sizeof(buf)) {
printf("\tMAG: read fail (%d)\n", ret);
return ERROR;
} else {
printf("\tMAG values: x:%8.4f\ty:%8.4f\tz:%8.4f\n", (double)buf.x, (double)buf.y, (double)buf.z);
}
float len = sqrtf(buf.x * buf.x + buf.y * buf.y + buf.z * buf.z);
if (len < 0.25f || len > 3.0f) {
warnx("MAG scale error!");
return ERROR;
}
/* Let user know everything is ok */
printf("\tOK: MAG passed all tests successfully\n");
px4_close(fd);
return OK;
}
/**
* 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_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 to initialize */
if (g_dev->init() != PX4_OK) {
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 PX4_OK;
fail:
if (g_dev != nullptr) {
delete g_dev;
g_dev = nullptr;
}
return PX4_ERROR;
}
int
I2C::init()
{
int ret = PX4_OK;
// Assume the driver set the desired bus frequency. There is no standard
// way to set it from user space.
// do base class init, which will create device node, etc
ret = VDev::init();
if (ret != PX4_OK) {
DEVICE_DEBUG("VDev::init failed");
return ret;
}
_fd = px4_open(get_devname(), PX4_F_RDONLY | PX4_F_WRONLY);
if (_fd < 0) {
DEVICE_DEBUG("px4_open failed of device %s", get_devname());
return PX4_ERROR;
}
#ifdef __PX4_QURT
simulate = true;
#endif
if (simulate) {
_fd = 10000;
}
else {
#ifndef __PX4_QURT
// Open the actual I2C device and map to the virtual dev name
_fd = ::open(get_devname(), O_RDWR);
if (_fd < 0) {
warnx("could not open %s", get_devname());
px4_errno = errno;
return PX4_ERROR;
}
#endif
}
return ret;
}
