diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2012-12-30 10:49:27 +0100 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2012-12-30 10:49:27 +0100 |
commit | abe1b9759a7b4f3114a91aa63b1e8caf8d37d9aa (patch) | |
tree | eb3efcfbcb9ce87dbf8708755bf5eef761a159bd /apps | |
parent | 142556b442b1c88ed2ede2cb9904a6a324051e71 (diff) | |
parent | f6ea42ab5e886b3475350c5dab95b5985bda26bc (diff) | |
download | px4-firmware-abe1b9759a7b4f3114a91aa63b1e8caf8d37d9aa.tar.gz px4-firmware-abe1b9759a7b4f3114a91aa63b1e8caf8d37d9aa.tar.bz2 px4-firmware-abe1b9759a7b4f3114a91aa63b1e8caf8d37d9aa.zip |
Merged IO mixing branch
Diffstat (limited to 'apps')
-rw-r--r-- | apps/drivers/px4io/px4io.cpp | 206 | ||||
-rw-r--r-- | apps/drivers/px4io/uploader.cpp | 1 | ||||
-rw-r--r-- | apps/px4io/comms.c | 15 | ||||
-rw-r--r-- | apps/px4io/mixer.cpp | 37 | ||||
-rw-r--r-- | apps/px4io/protocol.h | 7 | ||||
-rw-r--r-- | apps/px4io/px4io.h | 2 | ||||
-rw-r--r-- | apps/systemlib/mixer/mixer_group.cpp | 37 |
7 files changed, 183 insertions, 122 deletions
diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index c60772361..8001f1ff6 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -102,7 +102,8 @@ public: bool dump_one; private: - static const unsigned _max_actuators = PX4IO_OUTPUT_CHANNELS; + // XXX + static const unsigned _max_actuators = PX4IO_CONTROL_CHANNELS; int _update_rate; ///< serial send rate in Hz int _serial_fd; ///< serial interface to PX4IO @@ -129,7 +130,8 @@ private: orb_advert_t _t_outputs; ///< mixed outputs topic actuator_outputs_s _outputs; ///< mixed outputs - MixerGroup *_mixers; ///< loaded mixers + const char *volatile _mix_buf; ///< mixer text buffer + volatile unsigned _mix_buf_len; ///< size of the mixer text buffer bool _primary_pwm_device; ///< true if we are the default PWM output @@ -177,6 +179,11 @@ private: void config_send(); /** + * Send a buffer containing mixer text to PX4IO + */ + int mixer_send(const char *buf, unsigned buflen); + + /** * Mixer control callback; invoked to fetch a control from a specific * group/index during mixing. */ @@ -208,7 +215,8 @@ PX4IO::PX4IO() : _t_armed(-1), _t_vstatus(-1), _t_outputs(-1), - _mixers(nullptr), + _mix_buf(nullptr), + _mix_buf_len(0), _primary_pwm_device(false), _relays(0), _switch_armed(false), @@ -231,6 +239,7 @@ PX4IO::~PX4IO() /* give it another 100ms */ usleep(100000); } + /* well, kill it anyway, though this will probably crash */ if (_task != -1) task_delete(_task); @@ -261,6 +270,7 @@ PX4IO::init() /* start the IO interface task */ _task = task_create("px4io", SCHED_PRIORITY_ACTUATOR_OUTPUTS, 4096, (main_t)&PX4IO::task_main_trampoline, nullptr); + if (_task < 0) { debug("task start failed: %d", errno); return -errno; @@ -272,13 +282,16 @@ PX4IO::init() debug("PX4IO connected"); break; } + usleep(100000); } + if (!_connected) { /* error here will result in everything being torn down */ log("PX4IO not responding"); return -EIO; } + return OK; } @@ -325,10 +338,12 @@ PX4IO::task_main() /* protocol stream */ _io_stream = hx_stream_init(_serial_fd, &PX4IO::rx_callback_trampoline, this); + if (_io_stream == nullptr) { log("failed to allocate HX protocol stream"); goto out; } + hx_stream_set_counters(_io_stream, perf_alloc(PC_COUNT, "PX4IO frames transmitted"), perf_alloc(PC_COUNT, "PX4IO frames received"), @@ -375,13 +390,18 @@ PX4IO::task_main() fds[3].fd = _t_vstatus; fds[3].events = POLLIN; - log("ready"); + debug("ready"); + + /* lock against the ioctl handler */ + lock(); /* loop handling received serial bytes */ while (!_task_should_exit) { /* sleep waiting for data, but no more than 100ms */ + unlock(); int ret = ::poll(&fds[0], sizeof(fds) / sizeof(fds[0]), 100); + lock(); /* this would be bad... */ if (ret < 0) { @@ -398,47 +418,22 @@ PX4IO::task_main() if (fds[0].revents & POLLIN) io_recv(); - /* if we have new data from the ORB, go handle it */ + /* if we have new control data from the ORB, handle it */ if (fds[1].revents & POLLIN) { /* get controls */ orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : ORB_ID(actuator_controls_1), _t_actuators, &_controls); - /* mix */ - if (_mixers != nullptr) { - _outputs.timestamp = hrt_absolute_time(); - _outputs.noutputs = _mixers->mix(&_outputs.output[0], _max_actuators); - - // XXX output actual limited values - memcpy(&_controls_effective, &_controls, sizeof(_controls_effective)); - - orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1), _t_actuators_effective, &_controls_effective); - - /* convert to PWM values */ - for (unsigned i = 0; i < _max_actuators; i++) { - /* last resort: catch NaN, INF and out-of-band errors */ - if (i < _outputs.noutputs && - isfinite(_outputs.output[i]) && - _outputs.output[i] >= -1.0f && - _outputs.output[i] <= 1.0f) { - /* scale for PWM output 900 - 2100us */ - _outputs.output[i] = 1500 + (600 * _outputs.output[i]); - } else { - /* - * Value is NaN, INF or out of band - set to the minimum value. - * This will be clearly visible on the servo status and will limit the risk of accidentally - * spinning motors. It would be deadly in flight. - */ - _outputs.output[i] = 900; - } - } - - /* and flag for update */ - _send_needed = true; - } + /* scale controls to PWM (temporary measure) */ + for (unsigned i = 0; i < _max_actuators; i++) + _outputs.output[i] = 1500 + (600 * _controls.control[i]); + + /* and flag for update */ + _send_needed = true; } + /* if we have an arming state update, handle it */ if (fds[2].revents & POLLIN) { orb_copy(ORB_ID(actuator_armed), _t_armed, &_armed); @@ -462,14 +457,26 @@ PX4IO::task_main() _config_needed = false; config_send(); } + + /* send a mixer update if needed */ + if (_mix_buf != nullptr) { + mixer_send(_mix_buf, _mix_buf_len); + + /* clear the buffer record so the ioctl handler knows we're done */ + _mix_buf = nullptr; + _mix_buf_len = 0; + } } + unlock(); + out: debug("exiting"); /* kill the HX stream */ if (_io_stream != nullptr) hx_stream_free(_io_stream); + ::close(_serial_fd); /* clean up the alternate device node */ @@ -522,25 +529,27 @@ PX4IO::rx_callback(const uint8_t *buffer, size_t bytes_received) { const px4io_report *rep = (const px4io_report *)buffer; - lock(); +// lock(); /* sanity-check the received frame size */ if (bytes_received != sizeof(px4io_report)) { debug("got %u expected %u", bytes_received, sizeof(px4io_report)); goto out; } + if (rep->i2f_magic != I2F_MAGIC) { debug("bad magic"); goto out; } + _connected = true; /* publish raw rc channel values from IO if valid channels are present */ if (rep->channel_count > 0) { _input_rc.timestamp = hrt_absolute_time(); _input_rc.channel_count = rep->channel_count; - for (int i = 0; i < rep->channel_count; i++) - { + + for (int i = 0; i < rep->channel_count; i++) { _input_rc.values[i] = rep->rc_channel[i]; } @@ -557,13 +566,16 @@ PX4IO::rx_callback(const uint8_t *buffer, size_t bytes_received) dump_one = false; printf("IO: %s armed ", rep->armed ? "" : "not"); + for (unsigned i = 0; i < rep->channel_count; i++) printf("%d: %d ", i, rep->rc_channel[i]); + printf("\n"); } out: - unlock(); +// unlock(); + return; } void @@ -576,13 +588,13 @@ PX4IO::io_send() /* set outputs */ for (unsigned i = 0; i < _max_actuators; i++) - cmd.servo_command[i] = _outputs.output[i]; + cmd.output_control[i] = _outputs.output[i]; /* publish as we send */ _outputs.timestamp = hrt_absolute_time(); + /* XXX needs to be based off post-mix values from the IO side */ orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), _t_outputs, &_outputs); - /* update relays */ for (unsigned i = 0; i < PX4IO_RELAY_CHANNELS; i++) cmd.relay_state[i] = (_relays & (1<< i)) ? true : false; @@ -597,6 +609,7 @@ PX4IO::io_send() cmd.servo_rate = _update_rate; ret = hx_stream_send(_io_stream, &cmd, sizeof(cmd)); + if (ret) debug("send error %d", ret); } @@ -610,11 +623,47 @@ PX4IO::config_send() cfg.f2i_config_magic = F2I_CONFIG_MAGIC; ret = hx_stream_send(_io_stream, &cfg, sizeof(cfg)); + if (ret) debug("config error %d", ret); } int +PX4IO::mixer_send(const char *buf, unsigned buflen) +{ + uint8_t frame[HX_STREAM_MAX_FRAME]; + px4io_mixdata *msg = (px4io_mixdata *)&frame[0]; + + msg->f2i_mixer_magic = F2I_MIXER_MAGIC; + msg->action = F2I_MIXER_ACTION_RESET; + + do { + unsigned count = buflen; + + if (count > F2I_MIXER_MAX_TEXT) + count = F2I_MIXER_MAX_TEXT; + + if (count > 0) { + memcpy(&msg->text[0], buf, count); + buf += count; + buflen -= count; + } + + int ret = hx_stream_send(_io_stream, msg, sizeof(px4io_mixdata) + count); + + if (ret) { + log("mixer send error %d", ret); + return ret; + } + + msg->action = F2I_MIXER_ACTION_APPEND; + + } while (buflen > 0); + + return 0; +} + +int PX4IO::ioctl(file *filep, int cmd, unsigned long arg) { int ret = OK; @@ -637,7 +686,7 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg) case PWM_SERVO_SET(0) ... PWM_SERVO_SET(_max_actuators - 1): - /* fake an update to the selected servo channel */ + /* fake an update to the selected 'servo' channel */ if ((arg >= 900) && (arg <= 2100)) { _outputs.output[cmd - PWM_SERVO_SET(0)] = arg; _send_needed = true; @@ -678,66 +727,30 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg) break; case MIXERIOCGETOUTPUTCOUNT: - *(unsigned *)arg = _max_actuators; + *(unsigned *)arg = PX4IO_CONTROL_CHANNELS; break; case MIXERIOCRESET: - if (_mixers != nullptr) { - delete _mixers; - _mixers = nullptr; - } - + ret = 0; /* load always resets */ break; - case MIXERIOCADDSIMPLE: { - mixer_simple_s *mixinfo = (mixer_simple_s *)arg; + case MIXERIOCLOADBUF: - /* build the new mixer from the supplied argument */ - SimpleMixer *mixer = new SimpleMixer(control_callback, - (uintptr_t)&_controls, mixinfo); + /* set the buffer up for transfer */ + _mix_buf = (const char *)arg; + _mix_buf_len = strnlen(_mix_buf, 1024); - /* validate the new mixer */ - if (mixer->check()) { - delete mixer; - ret = -EINVAL; + /* drop the lock and wait for the thread to clear the transmit */ + unlock(); - } else { - /* if we don't have a group yet, allocate one */ - if (_mixers == nullptr) - _mixers = new MixerGroup(control_callback, - (uintptr_t)&_controls); + while (_mix_buf != nullptr) + usleep(1000); - /* add the new mixer to the group */ - _mixers->add_mixer(mixer); - } + lock(); - } + ret = 0; break; - case MIXERIOCLOADBUF: { - const char *buf = (const char *)arg; - unsigned buflen = strnlen(buf, 1024); - - if (_mixers == nullptr) - _mixers = new MixerGroup(control_callback, (uintptr_t)&_controls); - - if (_mixers == nullptr) { - ret = -ENOMEM; - - } else { - - ret = _mixers->load_from_buf(buf, buflen); - - if (ret != 0) { - debug("mixer load failed with %d", ret); - delete _mixers; - _mixers = nullptr; - ret = -EINVAL; - } - } - break; - } - default: /* not a recognised value */ ret = -ENOTTY; @@ -777,6 +790,13 @@ test(void) close(fd); + actuator_armed_s aa; + + aa.armed = true; + aa.lockdown = false; + + orb_advertise(ORB_ID(actuator_armed), &aa); + exit(0); } @@ -796,6 +816,7 @@ monitor(void) if (fds[0].revents == POLLIN) { int c; read(0, &c, 1); + if (cancels-- == 0) exit(0); } @@ -893,6 +914,7 @@ px4io_main(int argc, char *argv[]) if (!strcmp(argv[1], "test")) test(); + if (!strcmp(argv[1], "monitor")) monitor(); diff --git a/apps/drivers/px4io/uploader.cpp b/apps/drivers/px4io/uploader.cpp index 8b354ff60..6442e947c 100644 --- a/apps/drivers/px4io/uploader.cpp +++ b/apps/drivers/px4io/uploader.cpp @@ -190,6 +190,7 @@ PX4IO_Uploader::drain() do { ret = recv(c, 250); + if (ret == OK) { //log("discard 0x%02x", c); } diff --git a/apps/px4io/comms.c b/apps/px4io/comms.c index df07c34d8..c208db3ad 100644 --- a/apps/px4io/comms.c +++ b/apps/px4io/comms.c @@ -71,6 +71,8 @@ static struct px4io_report report; static void comms_handle_frame(void *arg, const void *buffer, size_t length); +perf_counter_t comms_rx_errors; + static void comms_init(void) { @@ -78,6 +80,9 @@ comms_init(void) fmu_fd = open("/dev/ttyS1", O_RDWR); stream = hx_stream_init(fmu_fd, comms_handle_frame, NULL); + comms_rx_errors = perf_alloc(PC_COUNT, "rx_err"); + hx_stream_set_counters(stream, 0, 0, comms_rx_errors); + /* default state in the report to FMU */ report.i2f_magic = I2F_MAGIC; @@ -173,14 +178,12 @@ comms_handle_command(const void *buffer, size_t length) irqstate_t flags = irqsave(); /* fetch new PWM output values */ - for (unsigned i = 0; i < PX4IO_OUTPUT_CHANNELS; i++) { - system_state.fmu_channel_data[i] = cmd->servo_command[i]; - } + for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++) + system_state.fmu_channel_data[i] = cmd->output_control[i]; - /* if IO is armed and FMU gets disarmed, IO must also disarm */ - if (system_state.arm_ok && !cmd->arm_ok) { + /* if the IO is armed and the FMU gets disarmed, the IO must also disarm */ + if (system_state.arm_ok && !cmd->arm_ok) system_state.armed = false; - } system_state.arm_ok = cmd->arm_ok; system_state.vector_flight_mode_ok = cmd->vector_flight_mode_ok; diff --git a/apps/px4io/mixer.cpp b/apps/px4io/mixer.cpp index b8ea308f3..2acd60ce3 100644 --- a/apps/px4io/mixer.cpp +++ b/apps/px4io/mixer.cpp @@ -56,6 +56,7 @@ #include <systemlib/mixer/mixer.h> extern "C" { +//#define DEBUG #include "px4io.h" } @@ -94,15 +95,17 @@ mixer_tick(void) /* * Decide which set of inputs we're using. */ + /* this is for planes, where manual override makes sense */ if(system_state.manual_override_ok) { /* if everything is ok */ if (!system_state.mixer_manual_override && system_state.mixer_fmu_available) { /* we have recent control data from the FMU */ - control_count = PX4IO_OUTPUT_CHANNELS; + control_count = PX4IO_CONTROL_CHANNELS; control_values = &system_state.fmu_channel_data[0]; - /* when override is on or the fmu is not available */ + } else if (system_state.rc_channels > 0) { + /* when override is on or the fmu is not available, but RC is present */ control_count = system_state.rc_channels; control_values = &system_state.rc_channel_data[0]; } else { @@ -116,7 +119,7 @@ mixer_tick(void) } else { /* if the fmu is available whe are good */ if(system_state.mixer_fmu_available) { - control_count = PX4IO_OUTPUT_CHANNELS; + control_count = PX4IO_CONTROL_CHANNELS; control_values = &system_state.fmu_channel_data[0]; /* we better shut everything off */ } else { @@ -183,20 +186,22 @@ mixer_callback(uintptr_t handle, return -1; /* scale from current PWM units (1000-2000) to mixer input values */ - /* XXX this presents some ugly problems w.r.t failsafe and R/C input scaling that have to be addressed */ control = ((float)control_values[control_index] - 1500.0f) / 500.0f; return 0; } +static char mixer_text[256]; +static unsigned mixer_text_length = 0; + void mixer_handle_text(const void *buffer, size_t length) { - static char mixer_text[256]; - static unsigned mixer_text_length = 0; px4io_mixdata *msg = (px4io_mixdata *)buffer; + debug("mixer text %u", length); + if (length < sizeof(px4io_mixdata)) return; @@ -204,11 +209,13 @@ mixer_handle_text(const void *buffer, size_t length) switch (msg->action) { case F2I_MIXER_ACTION_RESET: + debug("reset"); mixer_group.reset(); mixer_text_length = 0; /* FALLTHROUGH */ case F2I_MIXER_ACTION_APPEND: + debug("append %d", length); /* check for overflow - this is really fatal */ if ((mixer_text_length + text_length + 1) > sizeof(mixer_text)) @@ -218,14 +225,22 @@ mixer_handle_text(const void *buffer, size_t length) memcpy(&mixer_text[mixer_text_length], msg->text, text_length); mixer_text_length += text_length; mixer_text[mixer_text_length] = '\0'; + debug("buflen %u", mixer_text_length); /* process the text buffer, adding new mixers as their descriptions can be parsed */ - char *end = &mixer_text[mixer_text_length]; - mixer_group.load_from_buf(&mixer_text[0], mixer_text_length); + unsigned resid = mixer_text_length; + mixer_group.load_from_buf(&mixer_text[0], resid); - /* copy any leftover text to the base of the buffer for re-use */ - if (mixer_text_length > 0) - memcpy(&mixer_text[0], end - mixer_text_length, mixer_text_length); + /* if anything was parsed */ + if (resid != mixer_text_length) { + debug("used %u", mixer_text_length - resid); + + /* copy any leftover text to the base of the buffer for re-use */ + if (resid > 0) + memcpy(&mixer_text[0], &mixer_text[mixer_text_length - resid], resid); + + mixer_text_length = resid; + } break; } diff --git a/apps/px4io/protocol.h b/apps/px4io/protocol.h index 5b485e2ff..c53b94eb0 100644 --- a/apps/px4io/protocol.h +++ b/apps/px4io/protocol.h @@ -41,7 +41,7 @@ #pragma once -#define PX4IO_OUTPUT_CHANNELS 8 +#define PX4IO_CONTROL_CHANNELS 8 #define PX4IO_INPUT_CHANNELS 12 #define PX4IO_RELAY_CHANNELS 4 @@ -55,7 +55,8 @@ struct px4io_command { #define F2I_MAGIC 0x636d uint16_t servo_command[PX4IO_OUTPUT_CHANNELS]; /**< servo output channels */ - uint16_t servo_rate; /**< PWM output rate in Hz */ + uint16_t servo_rate; + uint16_t output_control[PX4IO_CONTROL_CHANNELS]; /**< PWM output rate in Hz */ bool relay_state[PX4IO_RELAY_CHANNELS]; /**< relay states as requested by FMU */ bool arm_ok; /**< FMU allows full arming */ bool vector_flight_mode_ok; /**< FMU aquired a valid position lock, ready for pos control */ @@ -102,6 +103,6 @@ struct px4io_mixdata { }; /* maximum size is limited by the HX frame size */ -#define F2I_MIXER_MAX_TEXT (sizeof(struct px4io_mixdata) - HX_STREAM_MAX_FRAME) +#define F2I_MIXER_MAX_TEXT (HX_STREAM_MAX_FRAME - sizeof(struct px4io_mixdata)) #pragma pack(pop)
\ No newline at end of file diff --git a/apps/px4io/px4io.h b/apps/px4io/px4io.h index 1c9d11e99..fef0a9ba4 100644 --- a/apps/px4io/px4io.h +++ b/apps/px4io/px4io.h @@ -82,7 +82,7 @@ struct sys_state_s { /** * Control signals from FMU. */ - uint16_t fmu_channel_data[PX4IO_OUTPUT_CHANNELS]; + uint16_t fmu_channel_data[PX4IO_CONTROL_CHANNELS]; /** * Mixed servo outputs diff --git a/apps/systemlib/mixer/mixer_group.cpp b/apps/systemlib/mixer/mixer_group.cpp index b98531c4d..9aeab1dcc 100644 --- a/apps/systemlib/mixer/mixer_group.cpp +++ b/apps/systemlib/mixer/mixer_group.cpp @@ -125,39 +125,58 @@ MixerGroup::load_from_buf(const char *buf, unsigned &buflen) int ret = -1; const char *end = buf + buflen; - /* loop until we have consumed the buffer */ + /* + * Loop until either we have emptied the buffer, or we have failed to + * allocate something when we expected to. + */ while (buflen > 0) { Mixer *m = nullptr; const char *p = end - buflen; + unsigned resid = buflen; - /* use the next character as a hint to decide which mixer class to construct */ + /* + * Use the next character as a hint to decide which mixer class to construct. + */ switch (*p) { case 'Z': - m = NullMixer::from_text(p, buflen); + m = NullMixer::from_text(p, resid); break; case 'M': - m = SimpleMixer::from_text(_control_cb, _cb_handle, p, buflen); + m = SimpleMixer::from_text(_control_cb, _cb_handle, p, resid); break; case 'R': - m = MultirotorMixer::from_text(_control_cb, _cb_handle, p, buflen); + m = MultirotorMixer::from_text(_control_cb, _cb_handle, p, resid); break; default: - /* it's probably junk or whitespace */ - break; + /* it's probably junk or whitespace, skip a byte and retry */ + buflen--; + continue; } + /* + * If we constructed something, add it to the group. + */ if (m != nullptr) { add_mixer(m); + + /* we constructed something */ ret = 0; + /* only adjust buflen if parsing was successful */ + buflen = resid; } else { - /* skip whitespace or junk in the buffer */ - buflen--; + + /* + * There is data in the buffer that we expected to parse, but it didn't, + * so give up for now. + */ + break; } } + /* nothing more in the buffer for us now */ return ret; } |