POSIX and QuRT: fixed calls needing px4_ prefix

There were some missed calls to open and ioctl that need to be
px4_open and px4_ioctl.

QuRT also does not provide usleep() so px4_time.h has to be included
in files calling usleep.

Signed-off-by: Mark Charlebois <charlebm@gmail.com>
This commit is contained in:
Mark Charlebois 2015-05-05 12:13:39 -07:00
parent f2af8b08ed
commit 0cea93a55c
7 changed files with 19 additions and 11 deletions

View File

@ -90,6 +90,7 @@ ARCHDEFINES += -DCONFIG_ARCH_BOARD_$(CONFIG_BOARD) \
-I$(PX4_BASE)/src/platforms/qurt/include \ -I$(PX4_BASE)/src/platforms/qurt/include \
-I$(PX4_BASE)/../dspal/include \ -I$(PX4_BASE)/../dspal/include \
-I$(PX4_BASE)/../dspal/sys \ -I$(PX4_BASE)/../dspal/sys \
-I$(PX4_BASE)/mavlink/include/mavlink \
-Wno-error=shadow -Wno-error=shadow
# optimisation flags # optimisation flags

View File

@ -128,6 +128,7 @@
#include "commander_helper.h" #include "commander_helper.h"
#include <px4_posix.h> #include <px4_posix.h>
#include <px4_time.h>
#include <unistd.h> #include <unistd.h>
#include <stdio.h> #include <stdio.h>
#include <poll.h> #include <poll.h>

View File

@ -528,11 +528,11 @@ static void calibrate_answer_command(int mavlink_fd, struct vehicle_command_s &c
bool calibrate_cancel_check(int mavlink_fd, int cancel_sub) bool calibrate_cancel_check(int mavlink_fd, int cancel_sub)
{ {
struct pollfd fds[1]; px4_pollfd_struct_t fds[1];
fds[0].fd = cancel_sub; fds[0].fd = cancel_sub;
fds[0].events = POLLIN; fds[0].events = POLLIN;
if (poll(&fds[0], 1, 0) > 0) { if (px4_poll(&fds[0], 1, 0) > 0) {
struct vehicle_command_s cmd; struct vehicle_command_s cmd;
memset(&cmd, 0, sizeof(cmd)); memset(&cmd, 0, sizeof(cmd));

View File

@ -48,7 +48,8 @@
#include <sys/ioctl.h> #include <sys/ioctl.h>
#include <systemlib/err.h> #include <systemlib/err.h>
#include <fcntl.h> #include <fcntl.h>
#include <poll.h> #include <px4_posix.h>
#include <px4_time.h>
#include "drivers/drv_pwm_output.h" #include "drivers/drv_pwm_output.h"
#include <uORB/topics/battery_status.h> #include <uORB/topics/battery_status.h>
#include <uORB/topics/vehicle_command.h> #include <uORB/topics/vehicle_command.h>
@ -78,7 +79,7 @@ int check_if_batt_disconnected(int mavlink_fd) {
int do_esc_calibration(int mavlink_fd) { int do_esc_calibration(int mavlink_fd) {
int fd = open(PWM_OUTPUT0_DEVICE_PATH, 0); int fd = px4_open(PWM_OUTPUT0_DEVICE_PATH, 0);
int ret; int ret;
if(fd < 0) { if(fd < 0) {
@ -86,15 +87,15 @@ int do_esc_calibration(int mavlink_fd) {
} }
/* tell IO/FMU that its ok to disable its safety with the switch */ /* tell IO/FMU that its ok to disable its safety with the switch */
ret = ioctl(fd, PWM_SERVO_SET_ARM_OK, 0); ret = px4_ioctl(fd, PWM_SERVO_SET_ARM_OK, 0);
if (ret != OK) if (ret != OK)
err(1, "PWM_SERVO_SET_ARM_OK"); err(1, "PWM_SERVO_SET_ARM_OK");
/* tell IO/FMU that the system is armed (it will output values if safety is off) */ /* tell IO/FMU that the system is armed (it will output values if safety is off) */
ret = ioctl(fd, PWM_SERVO_ARM, 0); ret = px4_ioctl(fd, PWM_SERVO_ARM, 0);
if (ret != OK) if (ret != OK)
err(1, "PWM_SERVO_ARM"); err(1, "PWM_SERVO_ARM");
/* tell IO to switch off safety without using the safety switch */ /* tell IO to switch off safety without using the safety switch */
ret = ioctl(fd, PWM_SERVO_SET_FORCE_SAFETY_OFF, 0); ret = px4_ioctl(fd, PWM_SERVO_SET_FORCE_SAFETY_OFF, 0);
if(ret!=0) { if(ret!=0) {
err(1,"PWM_SERVO_SET_FORCE_SAFETY_OFF"); err(1,"PWM_SERVO_SET_FORCE_SAFETY_OFF");
} }
@ -143,10 +144,10 @@ int do_esc_calibration(int mavlink_fd) {
} }
/* disarm */ /* disarm */
ret = ioctl(fd, PWM_SERVO_DISARM, 0); ret = px4_ioctl(fd, PWM_SERVO_DISARM, 0);
if (ret != OK) if (ret != OK)
err(1, "PWM_SERVO_DISARM"); err(1, "PWM_SERVO_DISARM");
mavlink_log_info(mavlink_fd,"ESC calibration finished"); mavlink_log_info(mavlink_fd,"ESC calibration finished");
return OK; return OK;
} }

View File

@ -184,8 +184,8 @@ int do_gyro_calibration(int mavlink_fd)
sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s); sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s);
int fd = px4_open(str, 0); int fd = px4_open(str, 0);
if (fd >= 0) { if (fd >= 0) {
worker_data.device_id[s] = ioctl(fd, DEVIOCGDEVICEID, 0); worker_data.device_id[s] = px4_ioctl(fd, DEVIOCGDEVICEID, 0);
res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale_zero); res = px4_ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale_zero);
px4_close(fd); px4_close(fd);
if (res != OK) { if (res != OK) {

View File

@ -38,6 +38,7 @@
#include <px4_debug.h> #include <px4_debug.h>
#include <px4_tasks.h> #include <px4_tasks.h>
#include <px4_time.h>
#include <systemlib/err.h> #include <systemlib/err.h>
#include <errno.h> #include <errno.h>
#include <stdio.h> #include <stdio.h>

View File

@ -40,7 +40,11 @@
#if defined(__PX4_LINUX) || defined(__PX4_QURT) #if defined(__PX4_LINUX) || defined(__PX4_QURT)
#if defined(__PX4_LINUX)
#include <err.h> #include <err.h>
#else
#include <systemlib/err.h>
#endif
#define PX4_DBG(...) #define PX4_DBG(...)
#define PX4_INFO(...) warnx(__VA_ARGS__) #define PX4_INFO(...) warnx(__VA_ARGS__)