zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

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

sbg
px4dev 12 years ago
parent
d2ef2afb0b
commit
bfbd17a2fa
5 changed files with 121 additions and 57 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 4
      apps/drivers/device/cdev.cpp
  2. 6
      apps/drivers/device/device.h
  3. 51
      apps/px4/fmu/fmu.cpp
  4. 115
      apps/px4/px4io/driver/px4io.cpp
  5. 2
      apps/px4io/protocol.h

4
apps/drivers/device/cdev.cpp
Unescape 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 * Note that we use the GNU extension syntax here because we don't get designated
* initialisers in gcc 4.6. * initialisers in gcc 4.6.
*/ */
static const struct file_operations cdev_fops = { const struct file_operations CDev::fops = {
open : cdev_open, open : cdev_open,
close : cdev_close, close : cdev_close,
read : cdev_read, read : cdev_read,
@ -118,7 +118,7 @@ CDev::init()
goto out; goto out;
// now register the driver // now register the driver
ret = register_driver(_devname, &cdev_fops, 0666, (void *)this); ret = register_driver(_devname, &fops, 0666, (void *)this);
if (ret != OK) if (ret != OK)
goto out; goto out;

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

@ -286,6 +286,12 @@ public:
bool is_open() { return _open_count > 0; } bool is_open() { return _open_count > 0; }
protected: 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 * Check the current state of the device for poll events from the
* perspective of the file. * perspective of the file.

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

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

115
apps/px4/px4io/driver/px4io.cpp
Unescape View File

@ -85,10 +85,10 @@ public:
virtual int init(); 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: private:
static const unsigned _max_actuators = 8; static const unsigned _max_actuators = PX4IO_OUTPUT_CHANNELS;
int _serial_fd; ///< serial interface to PX4IO int _serial_fd; ///< serial interface to PX4IO
hx_stream_t _io_stream; ///< HX protocol stream hx_stream_t _io_stream; ///< HX protocol stream
@ -96,22 +96,23 @@ private:
int _task; ///< worker task int _task; ///< worker task
volatile bool _task_should_exit; volatile bool _task_should_exit;
int _t_actuators; ///< ORB subscription for actuator outputs int _t_actuators; ///< actuator output topic
int _t_armed; actuator_controls_s _controls; ///< actuator outputs
orb_advert_t _t_outputs;
MixerGroup *_mixers; int _t_armed; ///< system armed control topic
actuator_controls_s _controls; actuator_armed_s _armed; ///< system armed state
actuator_armed_s _armed;
actuator_outputs_s _outputs;
orb_advert_t _t_input; orb_advert_t _t_outputs; ///< mixed outputs topic
rc_input_values _input; 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 volatile bool _switch_armed; ///< PX4IO switch armed state
// XXX how should this work? // 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 * Trampoline to the worker task
@ -123,15 +124,30 @@ private:
*/ */
void task_main(); void task_main();
/**
* Handle receiving bytes from PX4IO
*/
void io_recv(); void io_recv();
/** /**
* HX protocol callback. * HX protocol callback trampoline.
*/ */
static void rx_callback_trampoline(void *arg, const void *buffer, size_t bytes_received); 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); void rx_callback(const uint8_t *buffer, size_t bytes_received);
/**
* Send an update packet to PX4IO
*/
void io_send(); 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, static int control_callback(uintptr_t handle,
uint8_t control_group, uint8_t control_group,
uint8_t control_index, uint8_t control_index,
@ -156,7 +172,7 @@ PX4IO::PX4IO() :
_t_armed(-1), _t_armed(-1),
_t_outputs(-1), _t_outputs(-1),
_mixers(nullptr), _mixers(nullptr),
_t_input(-1), _primary_pwm_device(false),
_switch_armed(false), _switch_armed(false),
_send_needed(false) _send_needed(false)
{ {
@ -169,22 +185,29 @@ PX4IO::PX4IO() :
PX4IO::~PX4IO() PX4IO::~PX4IO()
{ {
if (_task != -1) { 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; _task_should_exit = true;
unsigned i = 0; /* spin waiting for the thread to stop */
unsigned i = 10;
do { do {
/* wait 20ms */ /* wait 50ms - it should wake every 100ms or so worst-case */
usleep(20000); usleep(50000);
/* if we have given up, kill it */ /* if we have given up, kill it */
if (++i > 10) { if (--i == 0) {
task_delete(_task); task_delete(_task);
break; break;
} }
} while (_task != -1); } 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) if (_io_stream != nullptr)
hx_stream_free(_io_stream); hx_stream_free(_io_stream);
@ -203,6 +226,13 @@ PX4IO::init()
if (ret != OK) if (ret != OK)
return ret; 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 */ /* start the IO interface task */
_task = task_create("px4io", SCHED_PRIORITY_DEFAULT, 1024, (main_t)&PX4IO::task_main_trampoline, nullptr); _task = task_create("px4io", SCHED_PRIORITY_DEFAULT, 1024, (main_t)&PX4IO::task_main_trampoline, nullptr);
if (_task < 0) { if (_task < 0) {
@ -244,8 +274,12 @@ PX4IO::task_main()
/* XXX verify firmware/protocol version */ /* 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 */ /* convert the update rate in hz to milliseconds, rounding down if necessary */
//int update_rate_in_ms = int(1000 / _update_rate); //int update_rate_in_ms = int(1000 / _update_rate);
orb_set_interval(_t_actuators, 20); /* XXX 50Hz hardcoded for now */ 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 */ orb_set_interval(_t_armed, 200); /* 5Hz update rate */
/* advertise the mixed control outputs */ /* 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) */ /* poll descriptor */
struct pollfd fds[3]; pollfd fds[3];
fds[0].fd = _serial_fd; fds[0].fd = _serial_fd;
fds[0].events = POLLIN; fds[0].events = POLLIN;
fds[1].fd = _t_actuators; fds[1].fd = _t_actuators;
@ -358,10 +390,10 @@ PX4IO::rx_callback_trampoline(void *arg, const void *buffer, size_t bytes_receiv
void void
PX4IO::rx_callback(const uint8_t *buffer, size_t bytes_received) 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 */ /* sanity-check the received frame size */
if (bytes_received != sizeof(struct px4io_report)) if (bytes_received != sizeof(px4io_report))
return; return;
/* XXX handle R/C inputs here ... needs code sharing/library */ /* XXX handle R/C inputs here ... needs code sharing/library */
@ -395,7 +427,7 @@ PX4IO::io_send()
} }
int int
PX4IO::ioctl(struct file *filep, int cmd, unsigned long arg) PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
{ {
int ret = OK; int ret = OK;
@ -416,6 +448,7 @@ PX4IO::ioctl(struct file *filep, int cmd, unsigned long arg)
break; break;
case PWM_SERVO_SET(0) ... PWM_SERVO_SET(_max_actuators - 1): case PWM_SERVO_SET(0) ... PWM_SERVO_SET(_max_actuators - 1):
/* fake an update to the selected servo channel */
if ((arg >= 900) && (arg <= 2100)) { if ((arg >= 900) && (arg <= 2100)) {
_outputs.output[cmd - PWM_SERVO_SET(0)] = arg; _outputs.output[cmd - PWM_SERVO_SET(0)] = arg;
_send_needed = true; _send_needed = true;
@ -425,6 +458,7 @@ PX4IO::ioctl(struct file *filep, int cmd, unsigned long arg)
break; break;
case PWM_SERVO_GET(0) ... PWM_SERVO_GET(_max_actuators - 1): 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)]; *(servo_position_t *)arg = _outputs.output[cmd - PWM_SERVO_GET(0)];
break; break;
@ -442,18 +476,22 @@ PX4IO::ioctl(struct file *filep, int cmd, unsigned long arg)
case MIXERIOCADDSIMPLE: { case MIXERIOCADDSIMPLE: {
mixer_simple_s *mixinfo = (mixer_simple_s *)arg; mixer_simple_s *mixinfo = (mixer_simple_s *)arg;
/* build the new mixer from the supplied argument */
SimpleMixer *mixer = new SimpleMixer(control_callback, SimpleMixer *mixer = new SimpleMixer(control_callback,
(uintptr_t)&_controls, mixinfo); (uintptr_t)&_controls, mixinfo);
/* validate the new mixer */
if (mixer->check()) { if (mixer->check()) {
delete mixer; delete mixer;
ret = -EINVAL; ret = -EINVAL;
} else { } else {
/* if we don't have a group yet, allocate one */
if (_mixers == nullptr) if (_mixers == nullptr)
_mixers = new MixerGroup(control_callback, _mixers = new MixerGroup(control_callback,
(uintptr_t)&_controls); (uintptr_t)&_controls);
/* add the new mixer to the group */
_mixers->add_mixer(mixer); _mixers->add_mixer(mixer);
} }
@ -466,20 +504,21 @@ PX4IO::ioctl(struct file *filep, int cmd, unsigned long arg)
break; break;
case MIXERIOCLOADFILE: { case MIXERIOCLOADFILE: {
MixerGroup *newmixers;
const char *path = (const char *)arg; const char *path = (const char *)arg;
if (_mixers != nullptr) { /* allocate a new mixer group and load it from the file */
delete _mixers; newmixers = new MixerGroup(control_callback, (uintptr_t)&_controls);
_mixers = nullptr; if (newmixers->load_from_file(path) != 0) {
delete newmixers;
ret = -EINVAL;
} }
_mixers = new MixerGroup(control_callback, (uintptr_t)&_controls); /* swap the new mixers in for the old */
if (_mixers != nullptr) {
if (_mixers->load_from_file(path) != 0) {
delete _mixers; delete _mixers;
_mixers = nullptr;
ret = -EINVAL;
} }
_mixers = newmixers;
} }
break; break;

2
apps/px4io/protocol.h
Unescape View File

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

Write Preview
Loading…
Cancel
Save