forked from Archive/PX4-Autopilot
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:
parent
f2af8b08ed
commit
0cea93a55c
|
@ -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
|
||||||
|
|
|
@ -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>
|
||||||
|
|
|
@ -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));
|
||||||
|
|
||||||
|
|
|
@ -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;
|
||||||
}
|
}
|
||||||
|
|
|
@ -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) {
|
||||||
|
|
|
@ -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>
|
||||||
|
|
|
@ -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__)
|
||||||
|
|
Loading…
Reference in New Issue