/** * 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); }
/** * 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; }
/** * @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::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; }
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; }
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; }
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; }
/** * 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"); }
/** * 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"); } }
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; }
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); }
int led_off(int led) { if (leds < 0) { return leds; } return px4_ioctl(leds, LED_OFF, led); }
// 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"); }
int led_on(int led) { if (leds < 0) { return leds; } return px4_ioctl(leds, LED_ON, led); }
int led_toggle(int led) { if (leds < 0) { return leds; } return px4_ioctl(leds, LED_TOGGLE, led); }
orb_advert_t uORB::Manager::orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance, int priority) { int result, fd; orb_advert_t advertiser; //warnx("orb_advertise_multi meta = %p\n", meta); /* open the node as an advertiser */ fd = node_open(PUBSUB, meta, data, true, instance, priority); if (fd == ERROR) { warnx("node_open as advertiser failed."); return nullptr; } /* get the advertiser handle and close the node */ result = px4_ioctl(fd, ORBIOCGADVERTISER, (unsigned long)&advertiser); px4_close(fd); if (result == ERROR) { warnx("px4_ioctl ORBIOCGADVERTISER failed. fd = %d", fd); return nullptr; } /* the advertiser must perform an initial publish to initialise the object */ result = orb_publish(meta, advertiser, data); if (result == ERROR) { warnx("orb_publish failed"); return nullptr; } return advertiser; }
/** * 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 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; }
/** * 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; }
/** * 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"); } }
void rgbled_set_color(rgbled_color_t color) { if (rgbleds < 0) { return; } px4_ioctl(rgbleds, RGBLED_SET_COLOR, (unsigned long)color); }
void rgbled_set_pattern(rgbled_pattern_t *pattern) { if (rgbleds < 0) { return; } px4_ioctl(rgbleds, RGBLED_SET_PATTERN, (unsigned long)pattern); }
void rgbled_set_mode(rgbled_mode_t mode) { if (rgbleds < 0) { return; } px4_ioctl(rgbleds, RGBLED_SET_MODE, (unsigned long)mode); }
/** * 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; }
static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional) { bool success = true; char s[30]; sprintf(s, "%s%u", MAG_BASE_DEVICE_PATH, instance); int fd = px4_open(s, 0); if (fd < 0) { if (!optional) { mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: NO MAG SENSOR #%u", instance); } return false; } int calibration_devid; int ret; int devid = px4_ioctl(fd, DEVIOCGDEVICEID, 0); sprintf(s, "CAL_MAG%u_ID", instance); param_get(param_find(s), &(calibration_devid)); if (devid != calibration_devid) { mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: MAG #%u UNCALIBRATED", instance); success = false; goto out; } ret = px4_ioctl(fd, MAGIOCSELFTEST, 0); if (ret != OK) { mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: MAG #%u SELFTEST FAILED", instance); success = false; goto out; } out: px4_close(fd); return success; }
/** * Reset the driver. */ void reset() { int fd = px4_open(RANGE_FINDER0_DEVICE_PATH, O_RDONLY); if (fd < 0) { err(1, "failed "); } if (px4_ioctl(fd, SENSORIOCRESET, 0) < 0) { err(1, "driver reset failed"); } if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { err(1, "driver poll restart failed"); } exit(0); }
// reset the driver int reset() { int fd = px4_open(PATH_SDP3X, O_RDONLY); if (fd < 0) { PX4_ERR("failed "); return PX4_ERROR; } if (px4_ioctl(fd, SENSORIOCRESET, 0) < 0) { PX4_ERR("driver reset failed"); return PX4_ERROR; } if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { PX4_ERR("driver poll restart failed"); return PX4_ERROR; } return PX4_OK; }
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; }
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; }