fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-23 13:03:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

Changed device::px4_device_handle_t to device::file_t

Browse Source 
This change allowed the _posix.cpp file changes to be merged
back into the original files.

Signed-off-by: Mark Charlebois <charlebm@gmail.com>
This commit is contained in:
Mark Charlebois
2015-04-24 17:49:35 -07:00
parent 20d35e33da
commit d913ec8dc9
28 changed files with 600 additions and 1327 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/hil/hil.cpp
+112 -75
View File
@@ -49,6 +49,9 @@
*/
#include <px4_config.h>
#include <px4_tasks.h>
#include <px4_time.h>
#include <px4_common.h>
#include <sys/types.h>
#include <stdint.h>
@@ -60,11 +63,10 @@
#include <poll.h>
#include <errno.h>
#include <stdio.h>
#include <math.h>
#include <cmath>
#include <string.h>
#include <unistd.h>
#include <nuttx/arch.h>
#include <drivers/device/device.h>
#include <drivers/drv_pwm_output.h>
#include <drivers/drv_gpio.h>
@@ -82,21 +84,25 @@
#include <systemlib/err.h>
#ifdef __PX4_NUTTX
class HIL : public device::CDev
#else
class HIL : public device::VDev
#endif
{
public:
enum Mode {
MODE_2PWM,
MODE_4PWM,
MODE_8PWM,
MODE_12PWM,
MODE_16PWM,
MODE_8PWM,
MODE_12PWM,
MODE_16PWM,
MODE_NONE
};
HIL();
virtual ~HIL();
virtual int ioctl(file *filp, int cmd, unsigned long arg);
virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg);
virtual int init();
@@ -131,7 +137,7 @@ private:
uint8_t control_index,
float &input);
int pwm_ioctl(file *filp, int cmd, unsigned long arg);
int pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg);
struct GPIOConfig {
uint32_t input;
@@ -146,7 +152,7 @@ private:
void gpio_set_function(uint32_t gpios, int function);
void gpio_write(uint32_t gpios, int function);
uint32_t gpio_read(void);
int gpio_ioctl(file *filp, int cmd, unsigned long arg);
int gpio_ioctl(device::file_t *filp, int cmd, unsigned long arg);
};
@@ -158,7 +164,12 @@ HIL *g_hil;
} // namespace
HIL::HIL() :
CDev("hilservo", PWM_OUTPUT0_DEVICE_PATH/*"/dev/hil" XXXL*/),
#ifdef __PX4_NUTTX
CDev(
#else
VDev(
#endif
"hilservo", PWM_OUTPUT0_DEVICE_PATH/*"/dev/hil" XXXL*/),
_mode(MODE_NONE),
_update_rate(50),
_current_update_rate(0),
@@ -188,7 +199,7 @@ HIL::~HIL()
/* if we have given up, kill it */
if (--i == 0) {
task_delete(_task);
px4_task_delete(_task);
break;
}
@@ -211,7 +222,11 @@ HIL::init()
ASSERT(_task == -1);
/* do regular cdev init */
#ifdef __PX4_NUTTX
ret = CDev::init();
#else
ret = VDev::init();
#endif
if (ret != OK)
return ret;
@@ -228,11 +243,11 @@ HIL::init()
// gpio_reset();
/* start the HIL interface task */
_task = task_spawn_cmd("fmuhil",
_task = px4_task_spawn_cmd("fmuhil",
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT,
1200,
(main_t)&HIL::task_main_trampoline,
(px4_main_t)&HIL::task_main_trampoline,
nullptr);
if (_task < 0) {
@@ -273,22 +288,22 @@ HIL::set_mode(Mode mode)
break;
case MODE_8PWM:
debug("MODE_8PWM");
/* multi-port as 8 PWM outs */
_update_rate = 50; /* default output rate */
break;
debug("MODE_8PWM");
/* multi-port as 8 PWM outs */
_update_rate = 50; /* default output rate */
break;
case MODE_12PWM:
debug("MODE_12PWM");
/* multi-port as 12 PWM outs */
_update_rate = 50; /* default output rate */
break;
case MODE_12PWM:
debug("MODE_12PWM");
/* multi-port as 12 PWM outs */
_update_rate = 50; /* default output rate */
break;
case MODE_16PWM:
debug("MODE_16PWM");
/* multi-port as 16 PWM outs */
_update_rate = 50; /* default output rate */
break;
case MODE_16PWM:
debug("MODE_16PWM");
/* multi-port as 16 PWM outs */
_update_rate = 50; /* default output rate */
break;
case MODE_NONE:
debug("MODE_NONE");
@@ -336,7 +351,7 @@ HIL::task_main()
_t_outputs = orb_advertise_multi(ORB_ID(actuator_outputs),
&outputs, &dummy, ORB_PRIO_LOW);
pollfd fds[2];
px4_pollfd_struct_t fds[2];
fds[0].fd = _t_actuators;
fds[0].events = POLLIN;
fds[1].fd = _t_armed;
@@ -383,7 +398,7 @@ HIL::task_main()
}
/* sleep waiting for data, but no more than a second */
int ret = ::poll(&fds[0], 2, 1000);
int ret = px4_poll(&fds[0], 2, 1000);
/* this would be bad... */
if (ret < 0) {
@@ -410,7 +425,7 @@ HIL::task_main()
/* last resort: catch NaN, INF and out-of-band errors */
if (i < outputs.noutputs &&
isfinite(outputs.output[i]) &&
PX4_ISFINITE(outputs.output[i]) &&
outputs.output[i] >= -1.0f &&
outputs.output[i] <= 1.0f) {
/* scale for PWM output 900 - 2100us */
@@ -439,8 +454,8 @@ HIL::task_main()
}
}
::close(_t_actuators);
::close(_t_armed);
px4_close(_t_actuators);
px4_close(_t_armed);
/* make sure servos are off */
// up_pwm_servo_deinit();
@@ -449,7 +464,6 @@ HIL::task_main()
/* tell the dtor that we are exiting */
_task = -1;
_exit(0);
}
int
@@ -465,7 +479,7 @@ HIL::control_callback(uintptr_t handle,
}
int
HIL::ioctl(file *filp, int cmd, unsigned long arg)
HIL::ioctl(device::file_t *filp, int cmd, unsigned long arg)
{
int ret;
@@ -480,9 +494,9 @@ HIL::ioctl(file *filp, int cmd, unsigned long arg)
switch(_mode) {
case MODE_2PWM:
case MODE_4PWM:
case MODE_8PWM:
case MODE_12PWM:
case MODE_16PWM:
case MODE_8PWM:
case MODE_12PWM:
case MODE_16PWM:
ret = HIL::pwm_ioctl(filp, cmd, arg);
break;
default:
@@ -492,14 +506,19 @@ HIL::ioctl(file *filp, int cmd, unsigned long arg)
}
/* if nobody wants it, let CDev have it */
if (ret == -ENOTTY)
if (ret == -ENOTTY) {
#ifdef __PX4_NUTTX
ret = CDev::ioctl(filp, cmd, arg);
#else
ret = VDev::ioctl(filp, cmd, arg);
#endif
}
return ret;
}
int
HIL::pwm_ioctl(file *filp, int cmd, unsigned long arg)
HIL::pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg)
{
int ret = OK;
// int channel;
@@ -675,8 +694,8 @@ hil_new_mode(PortMode new_mode)
case PORT_MODE_UNDEFINED:
case PORT1_MODE_UNSET:
case PORT2_MODE_UNSET:
/* nothing more to do here */
break;
/* nothing more to do here */
break;
case PORT1_FULL_PWM:
/* select 4-pin PWM mode */
@@ -695,20 +714,20 @@ hil_new_mode(PortMode new_mode)
servo_mode = HIL::MODE_2PWM;
break;
case PORT2_8PWM:
/* select 8-pin PWM mode */
servo_mode = HIL::MODE_8PWM;
break;
case PORT2_8PWM:
/* select 8-pin PWM mode */
servo_mode = HIL::MODE_8PWM;
break;
case PORT2_12PWM:
/* select 12-pin PWM mode */
servo_mode = HIL::MODE_12PWM;
break;
case PORT2_12PWM:
/* select 12-pin PWM mode */
servo_mode = HIL::MODE_12PWM;
break;
case PORT2_16PWM:
/* select 16-pin PWM mode */
servo_mode = HIL::MODE_16PWM;
break;
case PORT2_16PWM:
/* select 16-pin PWM mode */
servo_mode = HIL::MODE_16PWM;
break;
}
// /* adjust GPIO config for serial mode(s) */
@@ -746,32 +765,32 @@ hil_start(void)
return ret;
}
void
int
test(void)
{
int fd;
fd = open(PWM_OUTPUT0_DEVICE_PATH, 0);
fd = px4_open(PWM_OUTPUT0_DEVICE_PATH, 0);
if (fd < 0) {
puts("open fail");
exit(1);
return -ENODEV;
}
ioctl(fd, PWM_SERVO_ARM, 0);
ioctl(fd, PWM_SERVO_SET(0), 1000);
px4_ioctl(fd, PWM_SERVO_ARM, 0);
px4_ioctl(fd, PWM_SERVO_SET(0), 1000);
close(fd);
px4_close(fd);
exit(0);
return OK;
}
void
int
fake(int argc, char *argv[])
{
if (argc < 5) {
puts("hil fake <roll> <pitch> <yaw> <thrust> (values -100 .. 100)");
exit(1);
return -EINVAL;
}
actuator_controls_s ac;
@@ -788,24 +807,39 @@ fake(int argc, char *argv[])
if (handle < 0) {
puts("advertise failed");
exit(1);
return 1;
}
exit(0);
return 0;
}
} // namespace
extern "C" __EXPORT int hil_main(int argc, char *argv[]);
static void
usage() {
fprintf(stderr, "HIL: unrecognized command, try:\n");
fprintf(stderr, " mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio, mode_port2_pwm8, mode_port2_pwm12, mode_port2_pwm16\n");
}
int
hil_main(int argc, char *argv[])
{
PortMode new_mode = PORT_MODE_UNDEFINED;
const char *verb = argv[1];
const char *verb;
int ret = OK;
if (hil_start() != OK)
errx(1, "failed to start the HIL driver");
if (hil_start() != OK) {
warnx("failed to start the HIL driver");
return 1;
}
if (argc < 2) {
usage();
return -EINVAL;
}
verb = argv[1];
/*
* Mode switches.
@@ -842,14 +876,17 @@ hil_main(int argc, char *argv[])
return hil_new_mode(new_mode);
}
if (!strcmp(verb, "test"))
test();
if (!strcmp(verb, "test")) {
ret = test();
}
if (!strcmp(verb, "fake"))
fake(argc - 1, argv + 1);
else if (!strcmp(verb, "fake")) {
ret = fake(argc - 1, argv + 1);
}
fprintf(stderr, "HIL: unrecognized command, try:\n");
fprintf(stderr, " mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio, mode_port2_pwm8, mode_port2_pwm12, mode_port2_pwm16\n");
return -EINVAL;
else {
usage();
ret = -EINVAL;
}
return ret;
}