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