From 0cea93a55cc2b2762475c5834b385c12c67ad902 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Tue, 5 May 2015 12:13:39 -0700 Subject: [PATCH] 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 --- makefiles/toolchain_hexagon.mk | 1 + .../commander/accelerometer_calibration.cpp | 1 + src/modules/commander/calibration_routines.cpp | 4 ++-- src/modules/commander/esc_calibration.cpp | 15 ++++++++------- src/modules/commander/gyro_calibration.cpp | 4 ++-- src/modules/simulator/simulator.cpp | 1 + src/platforms/px4_debug.h | 4 ++++ 7 files changed, 19 insertions(+), 11 deletions(-) diff --git a/makefiles/toolchain_hexagon.mk b/makefiles/toolchain_hexagon.mk index 57c5d1a3c8..e250f12b37 100644 --- a/makefiles/toolchain_hexagon.mk +++ b/makefiles/toolchain_hexagon.mk @@ -90,6 +90,7 @@ ARCHDEFINES += -DCONFIG_ARCH_BOARD_$(CONFIG_BOARD) \ -I$(PX4_BASE)/src/platforms/qurt/include \ -I$(PX4_BASE)/../dspal/include \ -I$(PX4_BASE)/../dspal/sys \ + -I$(PX4_BASE)/mavlink/include/mavlink \ -Wno-error=shadow # optimisation flags diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index f7aace7753..44b5a43e87 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -128,6 +128,7 @@ #include "commander_helper.h" #include +#include #include #include #include diff --git a/src/modules/commander/calibration_routines.cpp b/src/modules/commander/calibration_routines.cpp index cecf22a0c3..68e094f7c4 100644 --- a/src/modules/commander/calibration_routines.cpp +++ b/src/modules/commander/calibration_routines.cpp @@ -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) { - struct pollfd fds[1]; + px4_pollfd_struct_t fds[1]; fds[0].fd = cancel_sub; fds[0].events = POLLIN; - if (poll(&fds[0], 1, 0) > 0) { + if (px4_poll(&fds[0], 1, 0) > 0) { struct vehicle_command_s cmd; memset(&cmd, 0, sizeof(cmd)); diff --git a/src/modules/commander/esc_calibration.cpp b/src/modules/commander/esc_calibration.cpp index 541aca0531..2e8f1e6f34 100644 --- a/src/modules/commander/esc_calibration.cpp +++ b/src/modules/commander/esc_calibration.cpp @@ -48,7 +48,8 @@ #include #include #include -#include +#include +#include #include "drivers/drv_pwm_output.h" #include #include @@ -78,7 +79,7 @@ int check_if_batt_disconnected(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; 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 */ - ret = ioctl(fd, PWM_SERVO_SET_ARM_OK, 0); + ret = px4_ioctl(fd, PWM_SERVO_SET_ARM_OK, 0); if (ret != OK) err(1, "PWM_SERVO_SET_ARM_OK"); /* 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) err(1, "PWM_SERVO_ARM"); /* 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) { err(1,"PWM_SERVO_SET_FORCE_SAFETY_OFF"); } @@ -143,10 +144,10 @@ int do_esc_calibration(int mavlink_fd) { } /* disarm */ - ret = ioctl(fd, PWM_SERVO_DISARM, 0); + ret = px4_ioctl(fd, PWM_SERVO_DISARM, 0); if (ret != OK) err(1, "PWM_SERVO_DISARM"); mavlink_log_info(mavlink_fd,"ESC calibration finished"); return OK; - } \ No newline at end of file + } diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index d444553b31..71e44c3d1e 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -184,8 +184,8 @@ int do_gyro_calibration(int mavlink_fd) sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s); int fd = px4_open(str, 0); if (fd >= 0) { - worker_data.device_id[s] = ioctl(fd, DEVIOCGDEVICEID, 0); - res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale_zero); + worker_data.device_id[s] = px4_ioctl(fd, DEVIOCGDEVICEID, 0); + res = px4_ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale_zero); px4_close(fd); if (res != OK) { diff --git a/src/modules/simulator/simulator.cpp b/src/modules/simulator/simulator.cpp index 2520f8a28a..e6cecbb08b 100644 --- a/src/modules/simulator/simulator.cpp +++ b/src/modules/simulator/simulator.cpp @@ -38,6 +38,7 @@ #include #include +#include #include #include #include diff --git a/src/platforms/px4_debug.h b/src/platforms/px4_debug.h index 0331875096..6cac039b28 100644 --- a/src/platforms/px4_debug.h +++ b/src/platforms/px4_debug.h @@ -40,7 +40,11 @@ #if defined(__PX4_LINUX) || defined(__PX4_QURT) +#if defined(__PX4_LINUX) #include +#else +#include +#endif #define PX4_DBG(...) #define PX4_INFO(...) warnx(__VA_ARGS__)