/** * 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; }