fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Make it possible to run fmu and px4io simultaneously with full control over both sets of possible PWM outputs. First started wins.

Browse Source
This commit is contained in:
px4dev 2012-10-20 16:53:52 -07:00
parent d2ef2afb0b
commit bfbd17a2fa
5 changed files with 123 additions and 59 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
apps/drivers/device/cdev.cpp
View File

@ -74,7 +74,7 @@ static int cdev_poll(struct file *filp, struct pollfd *fds, bool setup);
* Note that we use the GNU extension syntax here because we don't get designated
* initialisers in gcc 4.6.
*/
static const struct file_operations cdev_fops = {
const struct file_operations CDev::fops = {
open : cdev_open,
close : cdev_close,
read : cdev_read,
@ -118,7 +118,7 @@ CDev::init()
goto out;
// now register the driver
ret = register_driver(_devname, &cdev_fops, 0666, (void *)this);
ret = register_driver(_devname, &fops, 0666, (void *)this);
if (ret != OK)
goto out;

6
apps/drivers/device/device.h
View File

@ -286,6 +286,12 @@ public:
bool is_open() { return _open_count > 0; }
protected:
/**
* Pointer to the default cdev file operations table; useful for
* registering clone devices etc.
*/
static const struct file_operations fops;
/**
* Check the current state of the device for poll events from the
* perspective of the file.

51
apps/px4/fmu/fmu.cpp
View File

@ -79,7 +79,7 @@ public:
FMUServo(Mode mode, int update_rate);
~FMUServo();
virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
virtual int ioctl(file *filp, int cmd, unsigned long arg);
virtual int init();
@ -93,6 +93,7 @@ private:
int _t_armed;
orb_advert_t _t_outputs;
unsigned _num_outputs;
bool _primary_pwm_device;
volatile bool _task_should_exit;
bool _armed;
@ -118,7 +119,7 @@ FMUServo *g_servo;
} // namespace
FMUServo::FMUServo(Mode mode, int update_rate) :
CDev("fmuservo", PWM_OUTPUT_DEVICE_PATH),
CDev("fmuservo", "/dev/px4fmu"),
_mode(mode),
_update_rate(update_rate),
_task(-1),
@ -126,6 +127,7 @@ FMUServo::FMUServo(Mode mode, int update_rate) :
_t_armed(-1),
_t_outputs(0),
_num_outputs(0),
_primary_pwm_device(false),
_task_should_exit(false),
_armed(false),
_mixers(nullptr)
@ -135,18 +137,16 @@ FMUServo::FMUServo(Mode mode, int update_rate) :
FMUServo::~FMUServo()
{
if (_task != -1) {
/* task should wake up every 100ms or so at least */
/* tell the task we want it to go away */
_task_should_exit = true;
unsigned i = 0;
unsigned i = 10;
do {
/* wait 20ms */
usleep(20000);
/* wait 50ms - it should wake every 100ms or so worst-case */
usleep(50000);
/* if we have given up, kill it */
if (++i > 10) {
if (--i == 0) {
task_delete(_task);
break;
}
@ -154,6 +154,10 @@ FMUServo::~FMUServo()
} while (_task != -1);
}
/* clean up the alternate device node */
if (_primary_pwm_device)
unregister_driver(PWM_OUTPUT_DEVICE_PATH);
g_servo = nullptr;
}
@ -170,6 +174,13 @@ FMUServo::init()
if (ret != OK)
return ret;
/* try to claim the generic PWM output device node as well - it's OK if we fail at this */
ret = register_driver(PWM_OUTPUT_DEVICE_PATH, &fops, 0666, (void *)this);
if (ret == OK) {
log("default PWM output device");
_primary_pwm_device = true;
}
/* start the IO interface task */
_task = task_spawn("fmuservo",
SCHED_DEFAULT,
@ -216,8 +227,12 @@ FMUServo::task_main()
break;
}
/* subscribe to objects that we are interested in watching */
_t_actuators = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
/*
* Subscribe to the appropriate PWM output topic based on whether we are the
* primary PWM output or not.
*/
_t_actuators = orb_subscribe(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS :
ORB_ID(actuator_controls_1));
/* convert the update rate in hz to milliseconds, rounding down if necessary */
int update_rate_in_ms = int(1000 / _update_rate);
orb_set_interval(_t_actuators, update_rate_in_ms);
@ -226,11 +241,13 @@ FMUServo::task_main()
orb_set_interval(_t_armed, 200); /* 5Hz update rate */
/* advertise the mixed control outputs */
struct actuator_outputs_s outputs;
actuator_outputs_s outputs;
memset(&outputs, 0, sizeof(outputs));
_t_outputs = orb_advertise(ORB_ID_VEHICLE_CONTROLS, &outputs);
/* advertise the mixed control outputs */
_t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1),
&outputs);
struct pollfd fds[2];
pollfd fds[2];
fds[0].fd = _t_actuators;
fds[0].events = POLLIN;
fds[1].fd = _t_armed;
@ -282,7 +299,7 @@ FMUServo::task_main()
/* how about an arming update? */
if (fds[1].revents & POLLIN) {
struct actuator_armed_s aa;
actuator_armed_s aa;
/* get new value */
orb_copy(ORB_ID(actuator_armed), _t_armed, &aa);
@ -320,7 +337,7 @@ FMUServo::control_callback(uintptr_t handle,
}
int
FMUServo::ioctl(struct file *filp, int cmd, unsigned long arg)
FMUServo::ioctl(file *filp, int cmd, unsigned long arg)
{
int ret = OK;
int channel;
@ -569,7 +586,7 @@ fake(int argc, char *argv[])
exit(1);
}
struct actuator_controls_s ac;
actuator_controls_s ac;
ac.control[0] = strtol(argv[1], 0, 0) / 100.0f;

119
apps/px4/px4io/driver/px4io.cpp
View File

@ -85,10 +85,10 @@ public:
virtual int init();
virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
virtual int ioctl(file *filp, int cmd, unsigned long arg);
private:
static const unsigned _max_actuators = 8;
static const unsigned _max_actuators = PX4IO_OUTPUT_CHANNELS;
int _serial_fd; ///< serial interface to PX4IO
hx_stream_t _io_stream; ///< HX protocol stream
@ -96,22 +96,23 @@ private:
int _task; ///< worker task
volatile bool _task_should_exit;
int _t_actuators; ///< ORB subscription for actuator outputs
int _t_armed;
orb_advert_t _t_outputs;
int _t_actuators; ///< actuator output topic
actuator_controls_s _controls; ///< actuator outputs
MixerGroup *_mixers;
actuator_controls_s _controls;
actuator_armed_s _armed;
actuator_outputs_s _outputs;
int _t_armed; ///< system armed control topic
actuator_armed_s _armed; ///< system armed state
orb_advert_t _t_input;
rc_input_values _input;
orb_advert_t _t_outputs; ///< mixed outputs topic
actuator_outputs_s _outputs; ///< mixed outputs
MixerGroup *_mixers; ///< loaded mixers
bool _primary_pwm_device; ///< true if we are the default PWM output
volatile bool _switch_armed; ///< PX4IO switch armed state
// XXX how should this work?
bool _send_needed;
bool _send_needed; ///< If true, we need to send a packet to IO
/**
* Trampoline to the worker task
@ -123,15 +124,30 @@ private:
*/
void task_main();
/**
* Handle receiving bytes from PX4IO
*/
void io_recv();
/**
* HX protocol callback.
* HX protocol callback trampoline.
*/
static void rx_callback_trampoline(void *arg, const void *buffer, size_t bytes_received);
/**
* Callback invoked when we receive a whole packet from PX4IO
*/
void rx_callback(const uint8_t *buffer, size_t bytes_received);
/**
* Send an update packet to PX4IO
*/
void io_send();
/**
* Mixer control callback; invoked to fetch a control from a specific
* group/index during mixing.
*/
static int control_callback(uintptr_t handle,
uint8_t control_group,
uint8_t control_index,
@ -156,7 +172,7 @@ PX4IO::PX4IO() :
_t_armed(-1),
_t_outputs(-1),
_mixers(nullptr),
_t_input(-1),
_primary_pwm_device(false),
_switch_armed(false),
_send_needed(false)
{
@ -169,22 +185,29 @@ PX4IO::PX4IO() :
PX4IO::~PX4IO()
{
if (_task != -1) {
/* task should wake up every 100ms or so at least */
/* tell the task we want it to go away */
_task_should_exit = true;
unsigned i = 0;
/* spin waiting for the thread to stop */
unsigned i = 10;
do {
/* wait 20ms */
usleep(20000);
/* wait 50ms - it should wake every 100ms or so worst-case */
usleep(50000);
/* if we have given up, kill it */
if (++i > 10) {
if (--i == 0) {
task_delete(_task);
break;
}
} while (_task != -1);
}
/* clean up the alternate device node */
if (_primary_pwm_device)
unregister_driver(PWM_OUTPUT_DEVICE_PATH);
/* kill the HX stream */
if (_io_stream != nullptr)
hx_stream_free(_io_stream);
@ -203,6 +226,13 @@ PX4IO::init()
if (ret != OK)
return ret;
/* try to claim the generic PWM output device node as well - it's OK if we fail at this */
ret = register_driver(PWM_OUTPUT_DEVICE_PATH, &fops, 0666, (void *)this);
if (ret == OK) {
log("default PWM output device");
_primary_pwm_device = true;
}
/* start the IO interface task */
_task = task_create("px4io", SCHED_PRIORITY_DEFAULT, 1024, (main_t)&PX4IO::task_main_trampoline, nullptr);
if (_task < 0) {
@ -244,8 +274,12 @@ PX4IO::task_main()
/* XXX verify firmware/protocol version */
/* subscribe to objects that we are interested in watching */
_t_actuators = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
/*
* Subscribe to the appropriate PWM output topic based on whether we are the
* primary PWM output or not.
*/
_t_actuators = orb_subscribe(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS :
ORB_ID(actuator_controls_1));
/* convert the update rate in hz to milliseconds, rounding down if necessary */
//int update_rate_in_ms = int(1000 / _update_rate);
orb_set_interval(_t_actuators, 20); /* XXX 50Hz hardcoded for now */
@ -254,13 +288,11 @@ PX4IO::task_main()
orb_set_interval(_t_armed, 200); /* 5Hz update rate */
/* advertise the mixed control outputs */
_t_outputs = orb_advertise(ORB_ID_VEHICLE_CONTROLS, &_outputs);
_t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1),
&_outputs);
/* advertise the PX4IO R/C input */
_t_input = orb_advertise(ORB_ID(input_rc), &_input);
/* poll descriptor(s) */
struct pollfd fds[3];
/* poll descriptor */
pollfd fds[3];
fds[0].fd = _serial_fd;
fds[0].events = POLLIN;
fds[1].fd = _t_actuators;
@ -358,10 +390,10 @@ PX4IO::rx_callback_trampoline(void *arg, const void *buffer, size_t bytes_receiv
void
PX4IO::rx_callback(const uint8_t *buffer, size_t bytes_received)
{
const struct px4io_report *rep = (const struct px4io_report *)buffer;
const px4io_report *rep = (const px4io_report *)buffer;
/* sanity-check the received frame size */
if (bytes_received != sizeof(struct px4io_report))
if (bytes_received != sizeof(px4io_report))
return;
/* XXX handle R/C inputs here ... needs code sharing/library */
@ -395,7 +427,7 @@ PX4IO::io_send()
}
int
PX4IO::ioctl(struct file *filep, int cmd, unsigned long arg)
PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
{
int ret = OK;
@ -416,6 +448,7 @@ PX4IO::ioctl(struct file *filep, int cmd, unsigned long arg)
break;
case PWM_SERVO_SET(0) ... PWM_SERVO_SET(_max_actuators - 1):
/* fake an update to the selected servo channel */
if ((arg >= 900) && (arg <= 2100)) {
_outputs.output[cmd - PWM_SERVO_SET(0)] = arg;
_send_needed = true;
@ -425,6 +458,7 @@ PX4IO::ioctl(struct file *filep, int cmd, unsigned long arg)
break;
case PWM_SERVO_GET(0) ... PWM_SERVO_GET(_max_actuators - 1):
/* copy the current output value from the channel */
*(servo_position_t *)arg = _outputs.output[cmd - PWM_SERVO_GET(0)];
break;
@ -442,18 +476,22 @@ PX4IO::ioctl(struct file *filep, int cmd, unsigned long arg)
case MIXERIOCADDSIMPLE: {
mixer_simple_s *mixinfo = (mixer_simple_s *)arg;
/* build the new mixer from the supplied argument */
SimpleMixer *mixer = new SimpleMixer(control_callback,
(uintptr_t)&_controls, mixinfo);
/* validate the new mixer */
if (mixer->check()) {
delete mixer;
ret = -EINVAL;
} else {
/* if we don't have a group yet, allocate one */
if (_mixers == nullptr)
_mixers = new MixerGroup(control_callback,
(uintptr_t)&_controls);
/* add the new mixer to the group */
_mixers->add_mixer(mixer);
}
@ -466,21 +504,22 @@ PX4IO::ioctl(struct file *filep, int cmd, unsigned long arg)
break;
case MIXERIOCLOADFILE: {
MixerGroup *newmixers;
const char *path = (const char *)arg;
if (_mixers != nullptr) {
delete _mixers;
_mixers = nullptr;
}
_mixers = new MixerGroup(control_callback, (uintptr_t)&_controls);
if (_mixers->load_from_file(path) != 0) {
delete _mixers;
_mixers = nullptr;
/* allocate a new mixer group and load it from the file */
newmixers = new MixerGroup(control_callback, (uintptr_t)&_controls);
if (newmixers->load_from_file(path) != 0) {
delete newmixers;
ret = -EINVAL;
}
/* swap the new mixers in for the old */
if (_mixers != nullptr) {
delete _mixers;
}
_mixers = newmixers;
}
break;

2
apps/px4io/protocol.h
View File

@ -39,6 +39,8 @@
* messages and the corresponding complexity involved.
*/
#pragma once
/*
* XXX MUST BE KEPT IN SYNC WITH THE VERSION IN PX4FMU UNTIL
* TREES ARE MERGED.