From 85375c2201e6b9fe4737cb85b462c0aef8d271ca Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 29 Dec 2012 17:15:48 -0800 Subject: Rename the FMU->IO output controls to reflect the fact that they are controls, not servo values. --- apps/px4io/comms.c | 4 ++-- apps/px4io/mixer.cpp | 3 +-- apps/px4io/protocol.h | 4 ++-- apps/px4io/px4io.h | 2 +- 4 files changed, 6 insertions(+), 7 deletions(-) (limited to 'apps/px4io') diff --git a/apps/px4io/comms.c b/apps/px4io/comms.c index b815eda3c..b87f21dba 100644 --- a/apps/px4io/comms.c +++ b/apps/px4io/comms.c @@ -171,8 +171,8 @@ 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 the IO is armed and the FMU gets disarmed, the IO must also disarm */ if (system_state.arm_ok && !cmd->arm_ok) { diff --git a/apps/px4io/mixer.cpp b/apps/px4io/mixer.cpp index 0fd4ac717..52666ae39 100644 --- a/apps/px4io/mixer.cpp +++ b/apps/px4io/mixer.cpp @@ -85,7 +85,7 @@ mixer_tick(void) */ if (system_state.mixer_use_fmu) { /* 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]; /* check that we are receiving fresh data from the FMU */ @@ -170,7 +170,6 @@ 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; diff --git a/apps/px4io/protocol.h b/apps/px4io/protocol.h index 979f2f8cb..18d49a6be 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 @@ -54,7 +54,7 @@ struct px4io_command { uint16_t f2i_magic; #define F2I_MAGIC 0x636d - uint16_t servo_command[PX4IO_OUTPUT_CHANNELS]; + uint16_t output_control[PX4IO_CONTROL_CHANNELS]; bool relay_state[PX4IO_RELAY_CHANNELS]; bool arm_ok; }; diff --git a/apps/px4io/px4io.h b/apps/px4io/px4io.h index ad10961b4..7257d6522 100644 --- a/apps/px4io/px4io.h +++ b/apps/px4io/px4io.h @@ -81,7 +81,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 -- cgit v1.2.3 From c740e9c6160b72fa11d252e5ef4efe6ae741abba Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 30 Dec 2012 01:16:54 -0800 Subject: Add a receive error counter for debugging purposes. --- apps/px4io/comms.c | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) (limited to 'apps/px4io') diff --git a/apps/px4io/comms.c b/apps/px4io/comms.c index b87f21dba..843d0978a 100644 --- a/apps/px4io/comms.c +++ b/apps/px4io/comms.c @@ -70,6 +70,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) { @@ -77,6 +79,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; @@ -175,15 +180,13 @@ comms_handle_command(const void *buffer, size_t length) system_state.fmu_channel_data[i] = cmd->output_control[i]; /* if the IO is armed and the FMU gets disarmed, the IO must also disarm */ - if (system_state.arm_ok && !cmd->arm_ok) { + if (system_state.arm_ok && !cmd->arm_ok) system_state.armed = false; - } system_state.arm_ok = cmd->arm_ok; system_state.mixer_use_fmu = true; system_state.fmu_data_received = true; - /* handle changes signalled by FMU */ // if (!system_state.arm_ok && system_state.armed) // system_state.armed = false; -- cgit v1.2.3 From fd016abd46954311f85dd4a9345fce6aef680c44 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 30 Dec 2012 01:17:19 -0800 Subject: Implement the remaining pieces of mixer upload to PX4IO. --- apps/drivers/px4io/px4io.cpp | 177 ++++++++++++++++++++++++---------------- apps/drivers/px4io/uploader.cpp | 1 + apps/px4io/mixer.cpp | 28 +++++-- apps/px4io/protocol.h | 2 +- 4 files changed, 129 insertions(+), 79 deletions(-) (limited to 'apps/px4io') diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index cb69856c8..e5a1edea1 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -91,7 +91,7 @@ public: bool dump_one; private: - static const unsigned _max_actuators = PX4IO_OUTPUT_CHANNELS; + static const unsigned _max_actuators = PX4IO_CONTROL_CHANNELS; int _serial_fd; ///< serial interface to PX4IO hx_stream_t _io_stream; ///< HX protocol stream @@ -112,7 +112,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 @@ -157,6 +158,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. @@ -186,7 +192,8 @@ PX4IO::PX4IO() : _t_actuators(-1), _t_armed(-1), _t_outputs(-1), - _mixers(nullptr), + _mix_buf(nullptr), + _mix_buf_len(0), _primary_pwm_device(false), _switch_armed(false), _send_needed(false), @@ -208,6 +215,7 @@ PX4IO::~PX4IO() /* give it another 100ms */ usleep(100000); } + /* well, kill it anyway, though this will probably crash */ if (_task != -1) task_delete(_task); @@ -238,6 +246,7 @@ PX4IO::init() /* start the IO interface task */ _task = task_create("px4io", SCHED_PRIORITY_DEFAULT, 4096, (main_t)&PX4IO::task_main_trampoline, nullptr); + if (_task < 0) { debug("task start failed: %d", errno); return -errno; @@ -249,13 +258,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; } @@ -268,7 +280,7 @@ PX4IO::task_main_trampoline(int argc, char *argv[]) void PX4IO::task_main() { - log("starting"); + debug("starting"); /* open the serial port */ _serial_fd = ::open("/dev/ttyS2", O_RDWR); @@ -290,10 +302,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"), @@ -330,13 +344,18 @@ PX4IO::task_main() fds[2].fd = _t_armed; fds[2].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) { @@ -353,26 +372,21 @@ 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(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls); - /* mix */ - if (_mixers != nullptr) { - /* XXX is this the right count? */ - _mixers->mix(&_outputs.output[0], _max_actuators); + /* scale controls to PWM (temporary measure) */ + for (unsigned i = 0; i < _max_actuators; i++) + _outputs.output[i] = 1500 + (600 * _controls.control[i]); - /* convert to PWM values */ - for (unsigned i = 0; i < _max_actuators; i++) - _outputs.output[i] = 1500 + (600 * _outputs.output[i]); - - /* and flag for update */ - _send_needed = true; - } + /* 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); @@ -391,14 +405,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 */ @@ -451,25 +477,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]; } @@ -486,13 +514,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 @@ -505,9 +536,10 @@ 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 */ + /* XXX needs to be based off post-mix values from the IO side */ orb_publish(ORB_ID_VEHICLE_CONTROLS, _t_outputs, &_outputs); // XXX relays @@ -516,6 +548,7 @@ PX4IO::io_send() cmd.arm_ok = (_armed.armed && !_armed.lockdown); ret = hx_stream_send(_io_stream, &cmd, sizeof(cmd)); + if (ret) debug("send error %d", ret); } @@ -529,10 +562,46 @@ 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) { @@ -556,7 +625,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; @@ -573,66 +642,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; @@ -691,6 +724,7 @@ monitor(void) if (fds[0].revents == POLLIN) { int c; read(0, &c, 1); + if (cancels-- == 0) exit(0); } @@ -777,6 +811,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/mixer.cpp b/apps/px4io/mixer.cpp index 52666ae39..d21b3a898 100644 --- a/apps/px4io/mixer.cpp +++ b/apps/px4io/mixer.cpp @@ -52,6 +52,7 @@ #include extern "C" { +//#define DEBUG #include "px4io.h" } @@ -175,14 +176,17 @@ mixer_callback(uintptr_t handle, 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; @@ -190,11 +194,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)) @@ -204,14 +210,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); + + /* 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 (mixer_text_length > 0) - memcpy(&mixer_text[0], end - mixer_text_length, mixer_text_length); + /* 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 18d49a6be..afbf09cd3 100644 --- a/apps/px4io/protocol.h +++ b/apps/px4io/protocol.h @@ -99,6 +99,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 -- cgit v1.2.3