diff --git a/.gitignore b/.gitignore index ea416fae48..d0d84b9854 100644 --- a/.gitignore +++ b/.gitignore @@ -11,6 +11,7 @@ Make.dep *.o *.a *~ +*.dSYM Images/*.bin Images/*.px4 nuttx/Make.defs @@ -40,3 +41,4 @@ nsh_romfsimg.h cscope.out .configX-e nuttx-export.zip +dot.gdbinit diff --git a/apps/drivers/boards/px4io/px4io_internal.h b/apps/drivers/boards/px4io/px4io_internal.h index a0342ac8ad..3ac8a5cfa4 100644 --- a/apps/drivers/boards/px4io/px4io_internal.h +++ b/apps/drivers/boards/px4io/px4io_internal.h @@ -61,28 +61,6 @@ #define GPIO_LED3 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10) -/* R/C in/out channels **************************************************************/ - -/* XXX just GPIOs for now - eventually timer pins */ - -#define GPIO_CH1_IN (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN0) -#define GPIO_CH2_IN (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN1) -#define GPIO_CH3_IN (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN8) -#define GPIO_CH4_IN (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN9) -#define GPIO_CH5_IN (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN6) -#define GPIO_CH6_IN (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN7) -#define GPIO_CH7_IN (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN0) -#define GPIO_CH8_IN (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN1) - -#define GPIO_CH1_OUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN0) -#define GPIO_CH2_OUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN1) -#define GPIO_CH3_OUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN8) -#define GPIO_CH4_OUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN9) -#define GPIO_CH5_OUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN6) -#define GPIO_CH6_OUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN7) -#define GPIO_CH7_OUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN0) -#define GPIO_CH8_OUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN1) - /* Safety switch button *************************************************************/ #define GPIO_BTN_SAFETY (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN5) diff --git a/apps/drivers/drv_mixer.h b/apps/drivers/drv_mixer.h index 793e86b324..9f43015d91 100644 --- a/apps/drivers/drv_mixer.h +++ b/apps/drivers/drv_mixer.h @@ -100,28 +100,13 @@ struct mixer_simple_s { */ #define MIXERIOCADDSIMPLE _MIXERIOC(2) -/** multirotor output definition */ -struct mixer_rotor_output_s { - float angle; /**< rotor angle clockwise from forward in radians */ - float distance; /**< motor distance from centre in arbitrary units */ -}; - -/** multirotor mixer */ -struct mixer_multirotor_s { - uint8_t rotor_count; - struct mixer_control_s controls[4]; /**< controls are roll, pitch, yaw, thrust */ - struct mixer_rotor_output_s rotors[0]; /**< actual size of the array is set by rotor_count */ -}; +/* _MIXERIOC(3) was deprecated */ +/* _MIXERIOC(4) was deprecated */ /** - * Add a multirotor mixer in (struct mixer_multirotor_s *)arg + * Add mixer(s) from the buffer in (const char *)arg */ -#define MIXERIOCADDMULTIROTOR _MIXERIOC(3) - -/** - * Add mixers(s) from a the file in (const char *)arg - */ -#define MIXERIOCLOADFILE _MIXERIOC(4) +#define MIXERIOCLOADBUF _MIXERIOC(5) /* * XXX Thoughts for additional operations: diff --git a/apps/drivers/hil/hil.cpp b/apps/drivers/hil/hil.cpp index 67b16aa42f..f227db1f76 100644 --- a/apps/drivers/hil/hil.cpp +++ b/apps/drivers/hil/hil.cpp @@ -577,26 +577,19 @@ HIL::pwm_ioctl(file *filp, int cmd, unsigned long arg) break; } - case MIXERIOCADDMULTIROTOR: - /* XXX not yet supported */ - ret = -ENOTTY; - break; + case MIXERIOCLOADBUF: { + const char *buf = (const char *)arg; + unsigned buflen = strnlen(buf, 1024); - case MIXERIOCLOADFILE: { - const char *path = (const char *)arg; + if (_mixers == nullptr) + _mixers = new MixerGroup(control_callback, (uintptr_t)&_controls); - if (_mixers != nullptr) { - delete _mixers; - _mixers = nullptr; - } - - _mixers = new MixerGroup(control_callback, (uintptr_t)&_controls); if (_mixers == nullptr) { ret = -ENOMEM; + } else { - debug("loading mixers from %s", path); - ret = _mixers->load_from_file(path); + ret = _mixers->load_from_buf(buf, buflen); if (ret != 0) { debug("mixer load failed with %d", ret); @@ -605,10 +598,10 @@ HIL::pwm_ioctl(file *filp, int cmd, unsigned long arg) ret = -EINVAL; } } - break; } + default: ret = -ENOTTY; break; diff --git a/apps/drivers/px4fmu/fmu.cpp b/apps/drivers/px4fmu/fmu.cpp index a995f6214d..819dbf2087 100644 --- a/apps/drivers/px4fmu/fmu.cpp +++ b/apps/drivers/px4fmu/fmu.cpp @@ -500,7 +500,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) /* FALLTHROUGH */ case PWM_SERVO_GET(0): case PWM_SERVO_GET(1): { - channel = cmd - PWM_SERVO_SET(0); + channel = cmd - PWM_SERVO_GET(0); *(servo_position_t *)arg = up_pwm_servo_get(channel); break; } @@ -544,28 +544,19 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) break; } - case MIXERIOCADDMULTIROTOR: - /* XXX not yet supported */ - ret = -ENOTTY; - break; + case MIXERIOCLOADBUF: { + const char *buf = (const char *)arg; + unsigned buflen = strnlen(buf, 1024); - case MIXERIOCLOADFILE: { - const char *path = (const char *)arg; - - if (_mixers != nullptr) { - delete _mixers; - _mixers = nullptr; - } - - _mixers = new MixerGroup(control_callback, (uintptr_t)&_controls); + if (_mixers == nullptr) + _mixers = new MixerGroup(control_callback, (uintptr_t)&_controls); if (_mixers == nullptr) { ret = -ENOMEM; } else { - debug("loading mixers from %s", path); - ret = _mixers->load_from_file(path); + ret = _mixers->load_from_buf(buf, buflen); if (ret != 0) { debug("mixer load failed with %d", ret); @@ -574,7 +565,6 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) ret = -EINVAL; } } - break; } diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index 9f3dba047f..c1b52b07b4 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -61,6 +61,7 @@ #include #include #include +#include #include #include @@ -91,7 +92,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,10 +113,13 @@ 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 + uint32_t _relays; ///< state of the PX4IO relays, one bit per relay + volatile bool _switch_armed; ///< PX4IO switch armed state // XXX how should this work? @@ -157,6 +161,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,8 +195,10 @@ PX4IO::PX4IO() : _t_actuators(-1), _t_armed(-1), _t_outputs(-1), - _mixers(nullptr), + _mix_buf(nullptr), + _mix_buf_len(0), _primary_pwm_device(false), + _relays(0), _switch_armed(false), _send_needed(false), _config_needed(false) @@ -208,6 +219,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 +250,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 +262,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 +284,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 +306,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 +348,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 +376,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 +409,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 +481,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 +518,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,17 +540,21 @@ 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 + /* update relays */ + for (unsigned i = 0; i < PX4IO_RELAY_CHANNELS; i++) + cmd.relay_state[i] = (_relays & (1<< i)) ? true : false; /* armed and not locked down */ 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 +568,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 +631,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; @@ -572,68 +647,53 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg) *(servo_position_t *)arg = _outputs.output[cmd - PWM_SERVO_GET(0)]; break; + case GPIO_RESET: + _relays = 0; + _send_needed = true; + break; + + case GPIO_SET: + case GPIO_CLEAR: + /* make sure only valid bits are being set */ + if ((arg & ((1UL << PX4IO_RELAY_CHANNELS) - 1)) != arg) { + ret = EINVAL; + break; + } + if (cmd == GPIO_SET) { + _relays |= arg; + } else { + _relays &= ~arg; + } + _send_needed = true; + break; + + case GPIO_GET: + *(uint32_t *)arg = _relays; + 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(); - } - break; - - case MIXERIOCADDMULTIROTOR: - /* XXX not yet supported */ - ret = -ENOTTY; - break; - - case MIXERIOCLOADFILE: { - MixerGroup *newmixers; - const char *path = (const char *)arg; - - /* 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; - - } + ret = 0; break; default: @@ -675,6 +735,13 @@ test(void) close(fd); + actuator_armed_s aa; + + aa.armed = true; + aa.lockdown = false; + + orb_advertise(ORB_ID(actuator_armed), &aa); + exit(0); } @@ -694,6 +761,7 @@ monitor(void) if (fds[0].revents == POLLIN) { int c; read(0, &c, 1); + if (cancels-- == 0) exit(0); } @@ -780,6 +848,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 8b354ff600..6442e947c9 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/Makefile b/apps/px4io/Makefile index 801695f842..d97f963ab4 100644 --- a/apps/px4io/Makefile +++ b/apps/px4io/Makefile @@ -41,7 +41,9 @@ # CSRCS = $(wildcard *.c) \ ../systemlib/hx_stream.c \ - ../systemlib/perf_counter.c + ../systemlib/perf_counter.c \ + ../systemlib/up_cxxinitialize.c +CXXSRCS = $(wildcard *.cpp) INCLUDES = $(TOPDIR)/arch/arm/src/stm32 $(TOPDIR)/arch/arm/src/common diff --git a/apps/px4io/comms.c b/apps/px4io/comms.c index 83a006d43b..1836347423 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; @@ -110,6 +115,7 @@ comms_main(void) if (fds.revents & POLLIN) { char buf[32]; ssize_t count = read(fmu_fd, buf, sizeof(buf)); + for (int i = 0; i < count; i++) hx_stream_rx(stream, buf[i]); } @@ -123,7 +129,8 @@ comms_main(void) /* should we send a report to the FMU? */ now = hrt_absolute_time(); delta = now - last_report_time; - if ((delta > FMU_MIN_REPORT_INTERVAL) && + + if ((delta > FMU_MIN_REPORT_INTERVAL) && (system_state.fmu_report_due || (delta > FMU_MAX_REPORT_INTERVAL))) { system_state.fmu_report_due = false; @@ -132,6 +139,7 @@ comms_main(void) /* populate the report */ for (unsigned i = 0; i < system_state.rc_channels; i++) report.rc_channel[i] = system_state.rc_channel_data[i]; + report.channel_count = system_state.rc_channels; report.armed = system_state.armed; @@ -168,31 +176,45 @@ 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) { + 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; - /* XXX do relay changes here */ - for (unsigned i = 0; i < PX4IO_RELAY_CHANNELS; i++) + /* handle relay state changes here */ + for (unsigned i = 0; i < PX4IO_RELAY_CHANNELS; i++) { + if (system_state.relays[i] != cmd->relay_state[i]) { + switch (i) { + case 0: + POWER_ACC1(cmd->relay_state[i]); + break; + case 1: + POWER_ACC2(cmd->relay_state[i]); + break; + case 2: + POWER_RELAY1(cmd->relay_state[i]); + break; + case 3: + POWER_RELAY2(cmd->relay_state[i]); + break; + } + } system_state.relays[i] = cmd->relay_state[i]; + } irqrestore(flags); } - static void comms_handle_frame(void *arg, const void *buffer, size_t length) { @@ -205,11 +227,17 @@ comms_handle_frame(void *arg, const void *buffer, size_t length) case F2I_MAGIC: comms_handle_command(buffer, length); break; + case F2I_CONFIG_MAGIC: comms_handle_config(buffer, length); break; + + case F2I_MIXER_MAGIC: + mixer_handle_text(buffer, length); + break; + default: - frame_bad++; + frame_bad++; break; } } diff --git a/apps/px4io/controls.c b/apps/px4io/controls.c index 3b37829188..43e811ab0d 100644 --- a/apps/px4io/controls.c +++ b/apps/px4io/controls.c @@ -90,6 +90,7 @@ controls_main(void) if (fds[0].revents & POLLIN) locked |= dsm_input(); + if (fds[1].revents & POLLIN) locked |= sbus_input(); @@ -139,6 +140,7 @@ ppm_input(void) /* PPM data exists, copy it */ system_state.rc_channels = ppm_decoded_channels; + for (unsigned i = 0; i < ppm_decoded_channels; i++) system_state.rc_channel_data[i] = ppm_buffer[i]; @@ -150,5 +152,5 @@ ppm_input(void) /* trigger an immediate report to the FMU */ system_state.fmu_report_due = true; - } + } } diff --git a/apps/px4io/dsm.c b/apps/px4io/dsm.c index 2611f3a034..560ef47d94 100644 --- a/apps/px4io/dsm.c +++ b/apps/px4io/dsm.c @@ -47,7 +47,7 @@ #include #include - + #include #define DEBUG @@ -97,6 +97,7 @@ dsm_init(const char *device) dsm_guess_format(true); debug("DSM: ready"); + } else { debug("DSM: open failed"); } @@ -118,7 +119,7 @@ dsm_input(void) * frame transmission time is ~1.4ms. * * We expect to only be called when bytes arrive for processing, - * and if an interval of more than 5ms passes between calls, + * and if an interval of more than 5ms passes between calls, * the first byte we read will be the first byte of a frame. * * In the case where byte(s) are dropped from a frame, this also @@ -126,6 +127,7 @@ dsm_input(void) * if we didn't drop bytes... */ now = hrt_absolute_time(); + if ((now - last_rx_time) > 5000) { if (partial_frame_count > 0) { dsm_frame_drops++; @@ -142,6 +144,7 @@ dsm_input(void) /* if the read failed for any reason, just give up here */ if (ret < 1) goto out; + last_rx_time = now; /* @@ -153,7 +156,7 @@ dsm_input(void) * If we don't have a full frame, return */ if (partial_frame_count < DSM_FRAME_SIZE) - goto out; + goto out; /* * Great, it looks like we might have a frame. Go ahead and @@ -164,7 +167,7 @@ dsm_input(void) out: /* - * If we have seen a frame in the last 200ms, we consider ourselves 'locked' + * If we have seen a frame in the last 200ms, we consider ourselves 'locked' */ return (now - last_frame_time) < 200000; } @@ -212,6 +215,7 @@ dsm_guess_format(bool reset) /* if the channel decodes, remember the assigned number */ if (dsm_decode_channel(raw, 10, &channel, &value) && (channel < 31)) cs10 |= (1 << channel); + if (dsm_decode_channel(raw, 11, &channel, &value) && (channel < 31)) cs11 |= (1 << channel); @@ -222,7 +226,7 @@ dsm_guess_format(bool reset) if (samples++ < 5) return; - /* + /* * Iterate the set of sensible sniffed channel sets and see whether * decoding in 10 or 11-bit mode has yielded anything we recognise. * @@ -233,7 +237,7 @@ dsm_guess_format(bool reset) * See e.g. http://git.openpilot.org/cru/OPReview-116 for a discussion * of this issue. */ - static uint32_t masks[] = { + static uint32_t masks[] = { 0x3f, /* 6 channels (DX6) */ 0x7f, /* 7 channels (DX7) */ 0xff, /* 8 channels (DX8) */ @@ -247,14 +251,17 @@ dsm_guess_format(bool reset) if (cs10 == masks[i]) votes10++; + if (cs11 == masks[i]) votes11++; } + if ((votes11 == 1) && (votes10 == 0)) { channel_shift = 11; debug("DSM: detected 11-bit format"); return; } + if ((votes10 == 1) && (votes11 == 0)) { channel_shift = 10; debug("DSM: detected 10-bit format"); @@ -270,13 +277,13 @@ static void dsm_decode(hrt_abstime frame_time) { -/* - debug("DSM frame %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x", - frame[0], frame[1], frame[2], frame[3], frame[4], frame[5], frame[6], frame[7], - frame[8], frame[9], frame[10], frame[11], frame[12], frame[13], frame[14], frame[15]); -*/ /* - * If we have lost signal for at least a second, reset the + debug("DSM frame %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x", + frame[0], frame[1], frame[2], frame[3], frame[4], frame[5], frame[6], frame[7], + frame[8], frame[9], frame[10], frame[11], frame[12], frame[13], frame[14], frame[15]); + */ + /* + * If we have lost signal for at least a second, reset the * format guessing heuristic. */ if (((frame_time - last_frame_time) > 1000000) && (channel_shift != 0)) @@ -292,7 +299,7 @@ dsm_decode(hrt_abstime frame_time) } /* - * The encoding of the first two bytes is uncertain, so we're + * The encoding of the first two bytes is uncertain, so we're * going to ignore them for now. * * Each channel is a 16-bit unsigned value containing either a 10- @@ -322,9 +329,10 @@ dsm_decode(hrt_abstime frame_time) /* convert 0-1024 / 0-2048 values to 1000-2000 ppm encoding in a very sloppy fashion */ if (channel_shift == 11) value /= 2; + value += 998; - /* + /* * Store the decoded channel into the R/C input buffer, taking into * account the different ideas about channel assignement that we have. * @@ -335,14 +343,18 @@ dsm_decode(hrt_abstime frame_time) case 0: channel = 2; break; + case 1: channel = 0; break; + case 2: channel = 1; + default: break; } + system_state.rc_channel_data[channel] = value; } diff --git a/apps/px4io/mixer.c b/apps/px4io/mixer.cpp similarity index 57% rename from apps/px4io/mixer.c rename to apps/px4io/mixer.cpp index f02e98ae45..d21b3a8988 100644 --- a/apps/px4io/mixer.c +++ b/apps/px4io/mixer.cpp @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file mixer.c + * @file mixer.cpp * * Control channel input/output mixer and failsafe. */ @@ -49,8 +49,12 @@ #include #include +#include +extern "C" { +//#define DEBUG #include "px4io.h" +} /* * Count of periodic calls in which we have no FMU input. @@ -58,28 +62,23 @@ static unsigned fmu_input_drops; #define FMU_INPUT_DROP_LIMIT 20 -/* - * Update a mixer based on the current control signals. - */ -static void mixer_update(int mixer, uint16_t *inputs, int input_count); - /* current servo arm/disarm state */ bool mixer_servos_armed = false; -/* - * Each mixer consumes a set of inputs and produces a single output. - */ -struct mixer { - uint16_t current_value; - /* XXX more config here */ -} mixers[IO_SERVO_COUNT]; +/* selected control values and count for mixing */ +static uint16_t *control_values; +static int control_count; + +static int mixer_callback(uintptr_t handle, + uint8_t control_group, + uint8_t control_index, + float &control); + +static MixerGroup mixer_group(mixer_callback, 0); void mixer_tick(void) { - uint16_t *control_values; - int control_count; - int i; bool should_arm; /* @@ -87,7 +86,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 */ @@ -98,6 +97,7 @@ mixer_tick(void) if (fmu_input_drops >= FMU_INPUT_DROP_LIMIT) { system_state.mixer_use_fmu = false; } + } else { fmu_input_drops = 0; system_state.fmu_data_received = false; @@ -115,17 +115,31 @@ mixer_tick(void) } /* - * Tickle each mixer, if we have control data. + * Run the mixers if we have any control data at all. */ if (control_count > 0) { - for (i = 0; i < IO_SERVO_COUNT; i++) { - mixer_update(i, control_values, control_count); + float outputs[IO_SERVO_COUNT]; + unsigned mixed; + + /* mix */ + mixed = mixer_group.mix(&outputs[0], IO_SERVO_COUNT); + + /* scale to PWM and update the servo outputs as required */ + for (unsigned i = 0; i < IO_SERVO_COUNT; i++) { + if (i < mixed) { + /* scale to servo output */ + system_state.servos[i] = (outputs[i] * 500.0f) + 1500; + + } else { + /* set to zero to inhibit PWM pulse output */ + system_state.servos[i] = 0; + } /* * If we are armed, update the servo output. */ if (system_state.armed && system_state.arm_ok) - up_pwm_servo_set(i, mixers[i].current_value); + up_pwm_servo_set(i, system_state.servos[i]); } } @@ -133,6 +147,7 @@ mixer_tick(void) * Decide whether the servos should be armed right now. */ should_arm = system_state.armed && system_state.arm_ok && (control_count > 0); + if (should_arm && !mixer_servos_armed) { /* need to arm, but not armed */ up_pwm_servo_arm(true); @@ -145,13 +160,73 @@ mixer_tick(void) } } -static void -mixer_update(int mixer, uint16_t *inputs, int input_count) +static int +mixer_callback(uintptr_t handle, + uint8_t control_group, + uint8_t control_index, + float &control) { - /* simple passthrough for now */ - if (mixer < input_count) { - mixers[mixer].current_value = inputs[mixer]; - } else { - mixers[mixer].current_value = 0; - } + /* if the control index refers to an input that's not valid, we can't return it */ + if (control_index >= control_count) + return -1; + + /* scale from current PWM units (1000-2000) to mixer input values */ + 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) +{ + + px4io_mixdata *msg = (px4io_mixdata *)buffer; + + debug("mixer text %u", length); + + if (length < sizeof(px4io_mixdata)) + return; + + unsigned text_length = length - sizeof(px4io_mixdata); + + 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)) + return; + + /* append mixer text and nul-terminate */ + 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 */ + 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 (resid > 0) + memcpy(&mixer_text[0], &mixer_text[mixer_text_length - resid], resid); + + mixer_text_length = resid; + } + + break; + } +} \ No newline at end of file diff --git a/apps/px4io/protocol.h b/apps/px4io/protocol.h index c704b12016..afbf09cd39 100644 --- a/apps/px4io/protocol.h +++ b/apps/px4io/protocol.h @@ -41,31 +41,27 @@ #pragma once -#define PX4IO_OUTPUT_CHANNELS 8 +#define PX4IO_CONTROL_CHANNELS 8 #define PX4IO_INPUT_CHANNELS 12 #define PX4IO_RELAY_CHANNELS 4 #pragma pack(push, 1) -/* command from FMU to IO */ +/** + * Periodic command from FMU to IO. + */ struct px4io_command { uint16_t f2i_magic; -#define F2I_MAGIC 0x636d +#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; }; -/* config message from FMU to IO */ -struct px4io_config { - uint16_t f2i_config_magic; -#define F2I_CONFIG_MAGIC 0x6366 - - /* XXX currently nothing here */ -}; - -/* report from IO to FMU */ +/** + * Periodic report from IO to FMU + */ struct px4io_report { uint16_t i2f_magic; #define I2F_MAGIC 0x7570 @@ -75,4 +71,34 @@ struct px4io_report { uint8_t channel_count; }; +/** + * As-needed config message from FMU to IO + */ +struct px4io_config { + uint16_t f2i_config_magic; +#define F2I_CONFIG_MAGIC 0x6366 + + /* XXX currently nothing here */ +}; + +/** + * As-needed mixer data upload. + * + * This message adds text to the mixer text buffer; the text + * buffer is drained as the definitions are consumed. + */ +struct px4io_mixdata { + uint16_t f2i_mixer_magic; +#define F2I_MIXER_MAGIC 0x6d74 + + uint8_t action; +#define F2I_MIXER_ACTION_RESET 0 +#define F2I_MIXER_ACTION_APPEND 1 + + char text[0]; /* actual text size may vary */ +}; + +/* maximum size is limited by the HX frame size */ +#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.c b/apps/px4io/px4io.c index a3ac9e3e78..74362617d8 100644 --- a/apps/px4io/px4io.c +++ b/apps/px4io/px4io.c @@ -55,10 +55,15 @@ __EXPORT int user_start(int argc, char *argv[]); +extern void up_cxxinitialize(void); + struct sys_state_s system_state; int user_start(int argc, char *argv[]) { + /* run C++ ctors before we go any further */ + up_cxxinitialize(); + /* reset all to zero */ memset(&system_state, 0, sizeof(system_state)); @@ -83,7 +88,7 @@ int user_start(int argc, char *argv[]) up_pwm_servo_init(0xff); /* start the flight control signal handler */ - task_create("FCon", + task_create("FCon", SCHED_PRIORITY_DEFAULT, 1024, (main_t)controls_main, diff --git a/apps/px4io/px4io.h b/apps/px4io/px4io.h index 45b7cf847a..bd91eba0e4 100644 --- a/apps/px4io/px4io.h +++ b/apps/px4io/px4io.h @@ -31,11 +31,11 @@ * ****************************************************************************/ - /** - * @file px4io.h - * - * General defines and structures for the PX4IO module firmware. - */ +/** + * @file px4io.h + * + * General defines and structures for the PX4IO module firmware. + */ #include @@ -66,8 +66,7 @@ /* * System state structure. */ -struct sys_state_s -{ +struct sys_state_s { bool armed; /* IO armed */ bool arm_ok; /* FMU says OK to arm */ @@ -82,7 +81,12 @@ 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 + */ + uint16_t servos[IO_SERVO_COUNT]; /* * Relay controls @@ -136,8 +140,8 @@ extern volatile int timers[TIMER_NUM_TIMERS]; #define LED_SAFETY(_s) stm32_gpiowrite(GPIO_LED3, !(_s)) #define POWER_SERVO(_s) stm32_gpiowrite(GPIO_SERVO_PWR_EN, (_s)) -#define POWER_ACC1(_s) stm32_gpiowrite(GPIO_SERVO_ACC1_EN, (_s)) -#define POWER_ACC2(_s) stm32_gpiowrite(GPIO_SERVO_ACC2_EN, (_s)) +#define POWER_ACC1(_s) stm32_gpiowrite(GPIO_ACC1_PWR_EN, (_s)) +#define POWER_ACC2(_s) stm32_gpiowrite(GPIO_ACC2_PWR_EN, (_s)) #define POWER_RELAY1(_s) stm32_gpiowrite(GPIO_RELAY1_EN, (_s)) #define POWER_RELAY2(_s) stm32_gpiowrite(GPIO_RELAY2_EN, (_s)) @@ -149,6 +153,7 @@ extern volatile int timers[TIMER_NUM_TIMERS]; * Mixer */ extern void mixer_tick(void); +extern void mixer_handle_text(const void *buffer, size_t length); /* * Safety switch/LED. diff --git a/apps/px4io/safety.c b/apps/px4io/safety.c index 60d20905ab..93596ca757 100644 --- a/apps/px4io/safety.c +++ b/apps/px4io/safety.c @@ -31,9 +31,9 @@ * ****************************************************************************/ - /** - * @file Safety button logic. - */ +/** + * @file Safety button logic. + */ #include #include @@ -95,13 +95,13 @@ safety_init(void) static void safety_check_button(void *arg) { - /* + /* * Debounce the safety button, change state if it has been held for long enough. * */ safety_button_pressed = BUTTON_SAFETY; - if(safety_button_pressed) { + if (safety_button_pressed) { //printf("Pressed, Arm counter: %d, Disarm counter: %d\n", arm_counter, disarm_counter); } @@ -109,34 +109,42 @@ safety_check_button(void *arg) if (safety_button_pressed && !system_state.armed) { if (counter < ARM_COUNTER_THRESHOLD) { counter++; + } else if (counter == ARM_COUNTER_THRESHOLD) { /* change to armed state and notify the FMU */ system_state.armed = true; counter++; system_state.fmu_report_due = true; } - /* Disarm quickly */ + + /* Disarm quickly */ + } else if (safety_button_pressed && system_state.armed) { if (counter < DISARM_COUNTER_THRESHOLD) { counter++; + } else if (counter == DISARM_COUNTER_THRESHOLD) { /* change to disarmed state and notify the FMU */ system_state.armed = false; counter++; system_state.fmu_report_due = true; } + } else { counter = 0; } /* Select the appropriate LED flash pattern depending on the current IO/FMU arm state */ uint16_t pattern = LED_PATTERN_SAFE; + if (system_state.armed) { if (system_state.arm_ok) { pattern = LED_PATTERN_IO_FMU_ARMED; + } else { pattern = LED_PATTERN_IO_ARMED; } + } else if (system_state.arm_ok) { pattern = LED_PATTERN_FMU_ARMED; } @@ -167,8 +175,10 @@ failsafe_blink(void *arg) /* blink the failsafe LED if we don't have FMU input */ if (!system_state.mixer_use_fmu) { failsafe = !failsafe; + } else { failsafe = false; } + LED_AMBER(failsafe); } \ No newline at end of file diff --git a/apps/px4io/sbus.c b/apps/px4io/sbus.c index f84e5df8ac..c89388be1e 100644 --- a/apps/px4io/sbus.c +++ b/apps/px4io/sbus.c @@ -109,7 +109,7 @@ sbus_input(void) * frame transmission time is ~2ms. * * We expect to only be called when bytes arrive for processing, - * and if an interval of more than 3ms passes between calls, + * and if an interval of more than 3ms passes between calls, * the first byte we read will be the first byte of a frame. * * In the case where byte(s) are dropped from a frame, this also @@ -117,6 +117,7 @@ sbus_input(void) * if we didn't drop bytes... */ now = hrt_absolute_time(); + if ((now - last_rx_time) > 3000) { if (partial_frame_count > 0) { sbus_frame_drops++; @@ -133,6 +134,7 @@ sbus_input(void) /* if the read failed for any reason, just give up here */ if (ret < 1) goto out; + last_rx_time = now; /* @@ -144,7 +146,7 @@ sbus_input(void) * If we don't have a full frame, return */ if (partial_frame_count < SBUS_FRAME_SIZE) - goto out; + goto out; /* * Great, it looks like we might have a frame. Go ahead and @@ -155,7 +157,7 @@ sbus_input(void) out: /* - * If we have seen a frame in the last 200ms, we consider ourselves 'locked' + * If we have seen a frame in the last 200ms, we consider ourselves 'locked' */ return (now - last_frame_time) < 200000; } @@ -177,23 +179,23 @@ struct sbus_bit_pick { uint8_t mask; uint8_t lshift; }; -static const struct sbus_bit_pick sbus_decoder[SBUS_INPUT_CHANNELS][3] = { -/* 0 */ { { 0, 0, 0xff, 0},{ 1, 0, 0x07, 8},{ 0, 0, 0x00, 0} }, -/* 1 */ { { 1, 3, 0x1f, 0},{ 2, 0, 0x3f, 5},{ 0, 0, 0x00, 0} }, -/* 2 */ { { 2, 6, 0x03, 0},{ 3, 0, 0xff, 2},{ 4, 0, 0x01, 10} }, -/* 3 */ { { 4, 1, 0x7f, 0},{ 5, 0, 0x0f, 7},{ 0, 0, 0x00, 0} }, -/* 4 */ { { 5, 4, 0x0f, 0},{ 6, 0, 0x7f, 4},{ 0, 0, 0x00, 0} }, -/* 5 */ { { 6, 7, 0x01, 0},{ 7, 0, 0xff, 1},{ 8, 0, 0x03, 9} }, -/* 6 */ { { 8, 2, 0x3f, 0},{ 9, 0, 0x1f, 6},{ 0, 0, 0x00, 0} }, -/* 7 */ { { 9, 5, 0x07, 0},{10, 0, 0xff, 3},{ 0, 0, 0x00, 0} }, -/* 8 */ { {11, 0, 0xff, 0},{12, 0, 0x07, 8},{ 0, 0, 0x00, 0} }, -/* 9 */ { {12, 3, 0x1f, 0},{13, 0, 0x3f, 5},{ 0, 0, 0x00, 0} }, -/* 10 */ { {13, 6, 0x03, 0},{14, 0, 0xff, 2},{15, 0, 0x01, 10} }, -/* 11 */ { {15, 1, 0x7f, 0},{16, 0, 0x0f, 7},{ 0, 0, 0x00, 0} }, -/* 12 */ { {16, 4, 0x0f, 0},{17, 0, 0x7f, 4},{ 0, 0, 0x00, 0} }, -/* 13 */ { {17, 7, 0x01, 0},{18, 0, 0xff, 1},{19, 0, 0x03, 9} }, -/* 14 */ { {19, 2, 0x3f, 0},{20, 0, 0x1f, 6},{ 0, 0, 0x00, 0} }, -/* 15 */ { {20, 5, 0x07, 0},{21, 0, 0xff, 3},{ 0, 0, 0x00, 0} } +static const struct sbus_bit_pick sbus_decoder[SBUS_INPUT_CHANNELS][3] = { + /* 0 */ { { 0, 0, 0xff, 0}, { 1, 0, 0x07, 8}, { 0, 0, 0x00, 0} }, + /* 1 */ { { 1, 3, 0x1f, 0}, { 2, 0, 0x3f, 5}, { 0, 0, 0x00, 0} }, + /* 2 */ { { 2, 6, 0x03, 0}, { 3, 0, 0xff, 2}, { 4, 0, 0x01, 10} }, + /* 3 */ { { 4, 1, 0x7f, 0}, { 5, 0, 0x0f, 7}, { 0, 0, 0x00, 0} }, + /* 4 */ { { 5, 4, 0x0f, 0}, { 6, 0, 0x7f, 4}, { 0, 0, 0x00, 0} }, + /* 5 */ { { 6, 7, 0x01, 0}, { 7, 0, 0xff, 1}, { 8, 0, 0x03, 9} }, + /* 6 */ { { 8, 2, 0x3f, 0}, { 9, 0, 0x1f, 6}, { 0, 0, 0x00, 0} }, + /* 7 */ { { 9, 5, 0x07, 0}, {10, 0, 0xff, 3}, { 0, 0, 0x00, 0} }, + /* 8 */ { {11, 0, 0xff, 0}, {12, 0, 0x07, 8}, { 0, 0, 0x00, 0} }, + /* 9 */ { {12, 3, 0x1f, 0}, {13, 0, 0x3f, 5}, { 0, 0, 0x00, 0} }, + /* 10 */ { {13, 6, 0x03, 0}, {14, 0, 0xff, 2}, {15, 0, 0x01, 10} }, + /* 11 */ { {15, 1, 0x7f, 0}, {16, 0, 0x0f, 7}, { 0, 0, 0x00, 0} }, + /* 12 */ { {16, 4, 0x0f, 0}, {17, 0, 0x7f, 4}, { 0, 0, 0x00, 0} }, + /* 13 */ { {17, 7, 0x01, 0}, {18, 0, 0xff, 1}, {19, 0, 0x03, 9} }, + /* 14 */ { {19, 2, 0x3f, 0}, {20, 0, 0x1f, 6}, { 0, 0, 0x00, 0} }, + /* 15 */ { {20, 5, 0x07, 0}, {21, 0, 0xff, 3}, { 0, 0, 0x00, 0} } }; static void @@ -217,8 +219,8 @@ sbus_decode(hrt_abstime frame_time) /* we have received something we think is a frame */ last_frame_time = frame_time; - unsigned chancount = (PX4IO_INPUT_CHANNELS > SBUS_INPUT_CHANNELS) ? - SBUS_INPUT_CHANNELS : PX4IO_INPUT_CHANNELS; + unsigned chancount = (PX4IO_INPUT_CHANNELS > SBUS_INPUT_CHANNELS) ? + SBUS_INPUT_CHANNELS : PX4IO_INPUT_CHANNELS; /* use the decoder matrix to extract channel data */ for (unsigned channel = 0; channel < chancount; channel++) { @@ -236,6 +238,7 @@ sbus_decode(hrt_abstime frame_time) value |= piece; } } + /* convert 0-2048 values to 1000-2000 ppm encoding in a very sloppy fashion */ system_state.rc_channel_data[channel] = (value / 2) + 998; } @@ -252,7 +255,7 @@ sbus_decode(hrt_abstime frame_time) system_state.rc_channels = chancount; /* and note that we have received data from the R/C controller */ - system_state.rc_channels_timestamp = frame_time; + system_state.rc_channels_timestamp = frame_time; /* trigger an immediate report to the FMU */ system_state.fmu_report_due = true; diff --git a/apps/systemcmds/mixer/mixer.c b/apps/systemcmds/mixer/mixer.c index 3f52bdbf16..e2f7b5bd58 100644 --- a/apps/systemcmds/mixer/mixer.c +++ b/apps/systemcmds/mixer/mixer.c @@ -43,14 +43,17 @@ #include #include #include +#include +#include +#include #include #include __EXPORT int mixer_main(int argc, char *argv[]); -static void usage(const char *reason); -static void load(const char *devname, const char *fname); +static void usage(const char *reason) noreturn_function; +static void load(const char *devname, const char *fname) noreturn_function; int mixer_main(int argc, char *argv[]) @@ -63,12 +66,9 @@ mixer_main(int argc, char *argv[]) usage("missing device or filename"); load(argv[2], argv[3]); - - } else { - usage("unrecognised command"); } - return 0; + usage("unrecognised command"); } static void @@ -79,34 +79,58 @@ usage(const char *reason) fprintf(stderr, "usage:\n"); fprintf(stderr, " mixer load \n"); - /* XXX automatic setups for quad, etc. */ + /* XXX other useful commands? */ exit(1); } static void load(const char *devname, const char *fname) { - int dev = -1; - int ret, result = 1; + int dev; + FILE *fp; + char line[80]; + char buf[512]; /* open the device */ - if ((dev = open(devname, 0)) < 0) { - fprintf(stderr, "can't open %s\n", devname); - goto out; + if ((dev = open(devname, 0)) < 0) + err(1, "can't open %s\n", devname); + + /* reset mixers on the device */ + if (ioctl(dev, MIXERIOCRESET, 0)) + err(1, "can't reset mixers on %s", devname); + + /* open the mixer definition file */ + fp = fopen(fname, "r"); + if (fp == NULL) + err(1, "can't open %s", fname); + + /* read valid lines from the file into a buffer */ + buf[0] = '\0'; + for (;;) { + + /* get a line, bail on error/EOF */ + line[0] = '\0'; + if (fgets(line, sizeof(line), fp) == NULL) + break; + + /* if the line doesn't look like a mixer definition line, skip it */ + if ((strlen(line) < 2) || !isupper(line[0]) || (line[1] != ':')) + continue; + + /* XXX an optimisation here would be to strip extra whitespace */ + + /* if the line is too long to fit in the buffer, bail */ + if ((strlen(line) + strlen(buf) + 1) >= sizeof(buf)) + break; + + /* add the line to the buffer */ + strcat(buf, line); } - /* tell it to load the file */ - ret = ioctl(dev, MIXERIOCLOADFILE, (unsigned long)fname); + /* XXX pass the buffer to the device */ + int ret = ioctl(dev, MIXERIOCLOADBUF, (unsigned long)buf); - if (ret != 0) { - fprintf(stderr, "failed loading %s\n", fname); - } - - result = 0; -out: - - if (dev != -1) - close(dev); - - exit(result); + if (ret < 0) + err(1, "error loading mixers from %s", fname); + exit(0); } diff --git a/apps/systemlib/mixer/mixer.cpp b/apps/systemlib/mixer/mixer.cpp index 6c1bbe469b..72f765d90c 100644 --- a/apps/systemlib/mixer/mixer.cpp +++ b/apps/systemlib/mixer/mixer.cpp @@ -137,89 +137,15 @@ NullMixer::groups_required(uint32_t &groups) } -/****************************************************************************/ - -SimpleMixer::SimpleMixer(ControlCallback control_cb, - uintptr_t cb_handle, - mixer_simple_s *mixinfo) : - Mixer(control_cb, cb_handle), - _info(mixinfo) +NullMixer * +NullMixer::from_text(const char *buf, unsigned &buflen) { -} + NullMixer *nm = nullptr; -SimpleMixer::~SimpleMixer() -{ - if (_info != nullptr) - free(_info); -} - -unsigned -SimpleMixer::mix(float *outputs, unsigned space) -{ - float sum = 0.0f; - - if (_info == nullptr) - return 0; - - if (space < 1) - return 0; - - for (unsigned i = 0; i < _info->control_count; i++) { - float input; - - _control_cb(_cb_handle, - _info->controls[i].control_group, - _info->controls[i].control_index, - input); - - sum += scale(_info->controls[i].scaler, input); + if ((buflen >= 2) && (buf[0] == 'Z') && (buf[1] == ':')) { + nm = new NullMixer; + buflen -= 2; } - *outputs = scale(_info->output_scaler, sum); - return 1; -} - -void -SimpleMixer::groups_required(uint32_t &groups) -{ - for (unsigned i = 0; i < _info->control_count; i++) - groups |= 1 << _info->controls[i].control_group; -} - -int -SimpleMixer::check() -{ - int ret; - float junk; - - /* sanity that presumes that a mixer includes a control no more than once */ - /* max of 32 groups due to groups_required API */ - if (_info->control_count > 32) - return -2; - - /* validate the output scaler */ - ret = scale_check(_info->output_scaler); - - if (ret != 0) - return ret; - - /* validate input scalers */ - for (unsigned i = 0; i < _info->control_count; i++) { - - /* verify that we can fetch the control */ - if (_control_cb(_cb_handle, - _info->controls[i].control_group, - _info->controls[i].control_index, - junk) != 0) { - return -3; - } - - /* validate the scaler */ - ret = scale_check(_info->controls[i].scaler); - - if (ret != 0) - return (10 * i + ret); - } - - return 0; + return nm; } diff --git a/apps/systemlib/mixer/mixer.h b/apps/systemlib/mixer/mixer.h index 94c179eba0..00ddf15817 100644 --- a/apps/systemlib/mixer/mixer.h +++ b/apps/systemlib/mixer/mixer.h @@ -234,50 +234,51 @@ public: void add_mixer(Mixer *mixer); /** - * Reads a mixer definition from a file and configures a corresponding - * group. + * Remove all the mixers from the group. + */ + void reset(); + + /** + * Adds mixers to the group based on a text description in a buffer. * - * The mixer group must be empty when this function is called. + * Mixer definitions begin with a single capital letter and a colon. + * The actual format of the mixer definition varies with the individual + * mixers; they are summarised here, but see ROMFS/mixers/README for + * more details. * - * A mixer definition is a text representation of the configuration of a - * mixer. Definition lines begin with a capital letter followed by a colon. + * Null Mixer + * .......... * - * Null Mixer: + * The null mixer definition has the form: * - * Z: + * Z: * - * This mixer generates a constant zero output, and is normally used to - * skip over outputs that are not in use. + * Simple Mixer + * ............ * - * Simple Mixer: + * A simple mixer definition begins with: * - * M: - * S: - * S: ... + * M: + * O: <-ve scale> <+ve scale> * - * The definition consists of a single-line header indicating the - * number of scalers and then one line defining each scaler. The first - * scaler in the file is always the output scaler, followed by the input - * scalers. + * The definition continues with entries describing the control + * inputs and their scaling, in the form: * - * The values for the output scaler are ignored by the mixer. + * S: <-ve scale> <+ve scale> * + * Multirotor Mixer + * ................ * + * The multirotor mixer definition is a single line of the form: * - * Values marked * are integers representing floating point values; values are - * scaled by 10000 on load/save. + * R: * - * Multiple mixer definitions may be stored in a single file; it is assumed that - * the reader will know how many to expect and read accordingly. - * - * A mixer entry with a scaler count of zero indicates a disabled mixer. This - * will return NULL for the mixer when processed by this function, and will be - * generated by passing NULL as the mixer to mixer_save. - * - * @param path The mixer configuration file to read. + * @param buf The mixer configuration buffer. + * @param buflen The length of the buffer, updated to reflect + * bytes as they are consumed. * @return Zero on successful load, nonzero otherwise. */ - int load_from_file(const char *path); + int load_from_buf(const char *buf, unsigned &buflen); private: Mixer *_first; /**< linked list of mixers */ @@ -294,6 +295,21 @@ public: NullMixer(); ~NullMixer() {}; + /** + * Factory method. + * + * Given a pointer to a buffer containing a text description of the mixer, + * returns a pointer to a new instance of the mixer. + * + * @param buf Buffer containing a text description of + * the mixer. + * @param buflen Length of the buffer in bytes, adjusted + * to reflect the bytes consumed. + * @return A new NullMixer instance, or nullptr + * if the text format is bad. + */ + static NullMixer *from_text(const char *buf, unsigned &buflen); + virtual unsigned mix(float *outputs, unsigned space); virtual void groups_required(uint32_t &groups); }; @@ -318,6 +334,47 @@ public: mixer_simple_s *mixinfo); ~SimpleMixer(); + /** + * Factory method with full external configuration. + * + * Given a pointer to a buffer containing a text description of the mixer, + * returns a pointer to a new instance of the mixer. + * + * @param control_cb The callback to invoke when fetching a + * control value. + * @param cb_handle Handle passed to the control callback. + * @param buf Buffer containing a text description of + * the mixer. + * @param buflen Length of the buffer in bytes, adjusted + * to reflect the bytes consumed. + * @return A new SimpleMixer instance, or nullptr + * if the text format is bad. + */ + static SimpleMixer *from_text(Mixer::ControlCallback control_cb, + uintptr_t cb_handle, + const char *buf, + unsigned &buflen); + + /** + * Factory method for PWM/PPM input to internal float representation. + * + * @param control_cb The callback to invoke when fetching a + * control value. + * @param cb_handle Handle passed to the control callback. + * @param input The control index used when fetching the input. + * @param min The PWM/PPM value considered to be "minimum" (gives -1.0 out) + * @param mid The PWM/PPM value considered to be the midpoint (gives 0.0 out) + * @param max The PWM/PPM value considered to be "maximum" (gives 1.0 out) + * @return A new SimpleMixer instance, or nullptr if one could not be + * allocated. + */ + static SimpleMixer *pwm_input(Mixer::ControlCallback control_cb, + uintptr_t cb_handle, + unsigned input, + uint16_t min, + uint16_t mid, + uint16_t max); + virtual unsigned mix(float *outputs, unsigned space); virtual void groups_required(uint32_t &groups); @@ -330,10 +387,18 @@ public: * @return Zero if the mixer makes sense, nonzero otherwise. */ int check(); + protected: private: - mixer_simple_s *_info; + mixer_simple_s *_info; + + static int parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler_s &scaler); + static int parse_control_scaler(const char *buf, + unsigned &buflen, + mixer_scaler_s &scaler, + uint8_t &control_group, + uint8_t &control_index); }; /** @@ -395,6 +460,27 @@ public: float deadband); ~MultirotorMixer(); + /** + * Factory method. + * + * Given a pointer to a buffer containing a text description of the mixer, + * returns a pointer to a new instance of the mixer. + * + * @param control_cb The callback to invoke when fetching a + * control value. + * @param cb_handle Handle passed to the control callback. + * @param buf Buffer containing a text description of + * the mixer. + * @param buflen Length of the buffer in bytes, adjusted + * to reflect the bytes consumed. + * @return A new MultirotorMixer instance, or nullptr + * if the text format is bad. + */ + static MultirotorMixer *from_text(Mixer::ControlCallback control_cb, + uintptr_t cb_handle, + const char *buf, + unsigned &buflen); + virtual unsigned mix(float *outputs, unsigned space); virtual void groups_required(uint32_t &groups); diff --git a/apps/systemlib/mixer/mixer_group.cpp b/apps/systemlib/mixer/mixer_group.cpp index 19ce25553a..9aeab1dcc6 100644 --- a/apps/systemlib/mixer/mixer_group.cpp +++ b/apps/systemlib/mixer/mixer_group.cpp @@ -56,250 +56,6 @@ #define debug(fmt, args...) do { } while(0) //#define debug(fmt, args...) do { printf("[mixer] " fmt "\n", ##args); } while(0) -namespace -{ - -/** - * Effectively fdgets() with some extra smarts. - */ -static int -mixer_getline(int fd, char *line, unsigned maxlen) -{ - /* reduce line budget by 1 to account for terminal NUL */ - maxlen--; - - /* loop looking for a non-comment line */ - for (;;) { - int ret; - char c; - char *p = line; - - /* loop reading characters for this line */ - for (;;) { - ret = read(fd, &c, 1); - - /* on error or EOF, return same */ - if (ret <= 0) { - debug("read: EOF"); - return ret; - } - - /* ignore carriage returns */ - if (c == '\r') - continue; - - /* line termination */ - if (c == '\n') { - /* ignore malformed lines */ - if ((p - line) < 4) - break; - - if (line[1] != ':') - break; - - /* terminate line as string and return */ - *p = '\0'; - debug("read: '%s'", line); - return 1; - } - - /* if we have space, accumulate the byte and go on */ - if ((p - line) < maxlen) - *p++ = c; - } - } -} - -/** - * Parse an output scaler from the buffer. - */ -static int -mixer_parse_output_scaler(const char *buf, mixer_scaler_s &scaler) -{ - int s[5]; - - if (sscanf(buf, "O: %d %d %d %d %d", - &s[0], &s[1], &s[2], &s[3], &s[4]) != 5) { - debug("scaler parse failed on '%s'", buf); - return -1; - } - - scaler.negative_scale = s[0] / 10000.0f; - scaler.positive_scale = s[1] / 10000.0f; - scaler.offset = s[2] / 10000.0f; - scaler.min_output = s[3] / 10000.0f; - scaler.max_output = s[4] / 10000.0f; - - return 0; -} - -/** - * Parse a control scaler from the buffer. - */ -static int -mixer_parse_control_scaler(const char *buf, mixer_scaler_s &scaler, uint8_t &control_group, uint8_t &control_index) -{ - unsigned u[2]; - int s[5]; - - if (sscanf(buf, "S: %u %u %d %d %d %d %d", - &u[0], &u[1], &s[0], &s[1], &s[2], &s[3], &s[4]) != 7) { - debug("scaler parse failed on '%s'", buf); - return -1; - } - - control_group = u[0]; - control_index = u[1]; - scaler.negative_scale = s[0] / 10000.0f; - scaler.positive_scale = s[1] / 10000.0f; - scaler.offset = s[2] / 10000.0f; - scaler.min_output = s[3] / 10000.0f; - scaler.max_output = s[4] / 10000.0f; - - return 0; -} - -SimpleMixer * -mixer_load_simple(Mixer::ControlCallback control_cb, uintptr_t cb_handle, int fd, unsigned inputs) -{ - mixer_simple_s *mixinfo = nullptr; - char buf[60]; - int ret; - - /* let's assume we're going to read a simple mixer */ - mixinfo = (mixer_simple_s *)malloc(MIXER_SIMPLE_SIZE(inputs)); - mixinfo->control_count = inputs; - - /* first, get the output scaler */ - ret = mixer_getline(fd, buf, sizeof(buf)); - - if (ret < 1) { - debug("failed reading for output scaler"); - goto fail; - } - - if (mixer_parse_output_scaler(buf, mixinfo->output_scaler)) { - debug("failed parsing output scaler"); - goto fail; - } - - /* now get any inputs */ - for (unsigned i = 0; i < inputs; i++) { - ret = mixer_getline(fd, buf, sizeof(buf)); - - if (ret < 1) { - debug("failed reading for control scaler"); - goto fail; - } - - if (mixer_parse_control_scaler(buf, - mixinfo->controls[i].scaler, - mixinfo->controls[i].control_group, - mixinfo->controls[i].control_index)) { - debug("failed parsing control scaler"); - goto fail; - } - - debug("got control %d", i); - } - - /* XXX should be a factory that validates the mixinfo ... */ - return new SimpleMixer(control_cb, cb_handle, mixinfo); - -fail: - free(mixinfo); - return nullptr; -} - -MultirotorMixer * -mixer_load_multirotor(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf) -{ - MultirotorMixer::Geometry geometry; - char geomname[8]; - int s[4]; - - if (sscanf(buf, "R: %s %d %d %d %d", geomname, &s[0], &s[1], &s[2], &s[3]) != 5) { - debug("multirotor parse failed on '%s'", buf); - return nullptr; - } - - if (!strcmp(geomname, "4+")) { - geometry = MultirotorMixer::QUAD_PLUS; - - } else if (!strcmp(geomname, "4x")) { - geometry = MultirotorMixer::QUAD_X; - - } else if (!strcmp(geomname, "6+")) { - geometry = MultirotorMixer::HEX_PLUS; - - } else if (!strcmp(geomname, "6x")) { - geometry = MultirotorMixer::HEX_X; - - } else if (!strcmp(geomname, "8+")) { - geometry = MultirotorMixer::OCTA_PLUS; - - } else if (!strcmp(geomname, "8x")) { - geometry = MultirotorMixer::OCTA_X; - - } else { - debug("unrecognised geometry '%s'", geomname); - return nullptr; - } - - return new MultirotorMixer( - control_cb, - cb_handle, - geometry, - s[0] / 10000.0f, - s[1] / 10000.0f, - s[2] / 10000.0f, - s[3] / 10000.0f); -} - -int -mixer_load(Mixer::ControlCallback control_cb, uintptr_t cb_handle, int fd, Mixer *&mixer) -{ - int ret; - char buf[60]; - unsigned inputs; - - ret = mixer_getline(fd, buf, sizeof(buf)); - - /* end of file or error ?*/ - if (ret < 1) { - debug("getline %d", ret); - return ret; - } - - /* slot is empty - allocate a null mixer */ - if (buf[0] == 'Z') { - debug("got null mixer"); - mixer = new NullMixer(); - return 1; - } - - /* is it a simple mixer? */ - if (sscanf(buf, "M: %u", &inputs) == 1) { - debug("got simple mixer with %d inputs", inputs); - mixer = mixer_load_simple(control_cb, cb_handle, fd, inputs); - return (mixer == nullptr) ? -1 : 1; - } - - /* is it a multirotor mixer? */ - if (buf[0] == 'R') { - debug("got a multirotor mixer"); - mixer = mixer_load_multirotor(control_cb, cb_handle, buf); - return (mixer == nullptr) ? -1 : 1; - } - - /* we don't recognise the mixer type */ - debug("unrecognized mixer type '%c'", buf[0]); - return -1; -} - - -} // namespace - MixerGroup::MixerGroup(ControlCallback control_cb, uintptr_t cb_handle) : Mixer(control_cb, cb_handle), _first(nullptr) @@ -308,14 +64,7 @@ MixerGroup::MixerGroup(ControlCallback control_cb, uintptr_t cb_handle) : MixerGroup::~MixerGroup() { - Mixer *mixer; - - /* discard sub-mixers */ - while (_first != nullptr) { - mixer = _first; - _first = mixer->_next; - delete mixer; - } + reset(); } void @@ -332,6 +81,19 @@ MixerGroup::add_mixer(Mixer *mixer) mixer->_next = nullptr; } +void +MixerGroup::reset() +{ + Mixer *mixer; + + /* discard sub-mixers */ + while (_first != nullptr) { + mixer = _first; + _first = mixer->_next; + delete mixer; + } +} + unsigned MixerGroup::mix(float *outputs, unsigned space) { @@ -358,44 +120,63 @@ MixerGroup::groups_required(uint32_t &groups) } int -MixerGroup::load_from_file(const char *path) +MixerGroup::load_from_buf(const char *buf, unsigned &buflen) { - if (_first != nullptr) - return -1; + int ret = -1; + const char *end = buf + buflen; - int fd = open(path, O_RDONLY); + /* + * 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; - if (fd < 0) { - debug("failed to open %s", path); - return -1; - } + /* + * Use the next character as a hint to decide which mixer class to construct. + */ + switch (*p) { + case 'Z': + m = NullMixer::from_text(p, resid); + break; - for (unsigned count = 0;; count++) { - int result; - Mixer *mixer; + case 'M': + m = SimpleMixer::from_text(_control_cb, _cb_handle, p, resid); + break; - result = mixer_load(_control_cb, - _cb_handle, - fd, - mixer); + case 'R': + m = MultirotorMixer::from_text(_control_cb, _cb_handle, p, resid); + break; - /* error? */ - if (result < 0) { - debug("error"); - return -1; + default: + /* it's probably junk or whitespace, skip a byte and retry */ + buflen--; + continue; } - /* EOF or error */ - if (result < 1) { - printf("[mixer] loaded %u mixers\n", count); - debug("EOF"); + /* + * 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 { + + /* + * There is data in the buffer that we expected to parse, but it didn't, + * so give up for now. + */ break; } - - debug("loaded mixer %p", mixer); - add_mixer(mixer); } - close(fd); - return 0; + /* nothing more in the buffer for us now */ + return ret; } diff --git a/apps/systemlib/mixer/mixer_multirotor.cpp b/apps/systemlib/mixer/mixer_multirotor.cpp index b5b25e5320..3dd6dfcf75 100644 --- a/apps/systemlib/mixer/mixer_multirotor.cpp +++ b/apps/systemlib/mixer/mixer_multirotor.cpp @@ -54,6 +54,9 @@ #include "mixer.h" +#define debug(fmt, args...) do { } while(0) +//#define debug(fmt, args...) do { printf("[mixer] " fmt "\n", ##args); } while(0) + #define CW (-1.0f) #define CCW (1.0f) @@ -151,6 +154,61 @@ MultirotorMixer::~MultirotorMixer() { } +MultirotorMixer * +MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf, unsigned &buflen) +{ + MultirotorMixer::Geometry geometry; + char geomname[8]; + int s[4]; + int used; + + if (sscanf(buf, "R: %s %d %d %d %d%n", geomname, &s[0], &s[1], &s[2], &s[3], &used) != 5) { + debug("multirotor parse failed on '%s'", buf); + return nullptr; + } + + if (used > (int)buflen) { + debug("multirotor spec used %d of %u", used, buflen); + return nullptr; + } + + buflen -= used; + + if (!strcmp(geomname, "4+")) { + geometry = MultirotorMixer::QUAD_PLUS; + + } else if (!strcmp(geomname, "4x")) { + geometry = MultirotorMixer::QUAD_X; + + } else if (!strcmp(geomname, "6+")) { + geometry = MultirotorMixer::HEX_PLUS; + + } else if (!strcmp(geomname, "6x")) { + geometry = MultirotorMixer::HEX_X; + + } else if (!strcmp(geomname, "8+")) { + geometry = MultirotorMixer::OCTA_PLUS; + + } else if (!strcmp(geomname, "8x")) { + geometry = MultirotorMixer::OCTA_X; + + } else { + debug("unrecognised geometry '%s'", geomname); + return nullptr; + } + + debug("adding multirotor mixer '%s'", geomname); + + return new MultirotorMixer( + control_cb, + cb_handle, + geometry, + s[0] / 10000.0f, + s[1] / 10000.0f, + s[2] / 10000.0f, + s[3] / 10000.0f); +} + unsigned MultirotorMixer::mix(float *outputs, unsigned space) { @@ -170,10 +228,12 @@ MultirotorMixer::mix(float *outputs, unsigned space) /* keep roll, pitch and yaw control to 0 below min thrust */ if (thrust <= min_thrust) { output_factor = 0.0f; - /* linearly increase the output factor from 0 to 1 between min_thrust and startpoint_full_control */ + /* linearly increase the output factor from 0 to 1 between min_thrust and startpoint_full_control */ + } else if (thrust < startpoint_full_control && thrust > min_thrust) { - output_factor = (thrust/max_thrust)/(startpoint_full_control-min_thrust); - /* and then stay at full control */ + output_factor = (thrust / max_thrust) / (startpoint_full_control - min_thrust); + /* and then stay at full control */ + } else { output_factor = max_thrust; } diff --git a/apps/systemlib/mixer/mixer_simple.cpp b/apps/systemlib/mixer/mixer_simple.cpp new file mode 100644 index 0000000000..07dc5f37fa --- /dev/null +++ b/apps/systemlib/mixer/mixer_simple.cpp @@ -0,0 +1,334 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mixer_simple.cpp + * + * Simple summing mixer. + */ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "mixer.h" + +#define debug(fmt, args...) do { } while(0) +//#define debug(fmt, args...) do { printf("[mixer] " fmt "\n", ##args); } while(0) + +SimpleMixer::SimpleMixer(ControlCallback control_cb, + uintptr_t cb_handle, + mixer_simple_s *mixinfo) : + Mixer(control_cb, cb_handle), + _info(mixinfo) +{ +} + +SimpleMixer::~SimpleMixer() +{ + if (_info != nullptr) + free(_info); +} + +static const char * +findtag(const char *buf, unsigned &buflen, char tag) +{ + while (buflen >= 2) { + if ((buf[0] == tag) && (buf[1] == ':')) + return buf; + buf++; + buflen--; + } + return nullptr; +} + +static void +skipline(const char *buf, unsigned &buflen) +{ + const char *p; + + /* if we can find a CR or NL in the buffer, skip up to it */ + if ((p = (const char *)memchr(buf, '\r', buflen)) || (p = (const char *)memchr(buf, '\n', buflen))) + buflen -= (p - buf); +} + +int +SimpleMixer::parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler_s &scaler) +{ + int ret; + int s[5]; + + buf = findtag(buf, buflen, 'O'); + if ((buf == nullptr) || (buflen < 12)) + return -1; + + if ((ret = sscanf(buf, "O: %d %d %d %d %d", + &s[0], &s[1], &s[2], &s[3], &s[4])) != 5) { + debug("scaler parse failed on '%s' (got %d)", buf, ret); + return -1; + } + skipline(buf, buflen); + + scaler.negative_scale = s[0] / 10000.0f; + scaler.positive_scale = s[1] / 10000.0f; + scaler.offset = s[2] / 10000.0f; + scaler.min_output = s[3] / 10000.0f; + scaler.max_output = s[4] / 10000.0f; + + return 0; +} + +int +SimpleMixer::parse_control_scaler(const char *buf, unsigned &buflen, mixer_scaler_s &scaler, uint8_t &control_group, uint8_t &control_index) +{ + unsigned u[2]; + int s[5]; + + buf = findtag(buf, buflen, 'S'); + if ((buf == nullptr) || (buflen < 16)) + return -1; + + if (sscanf(buf, "S: %u %u %d %d %d %d %d", + &u[0], &u[1], &s[0], &s[1], &s[2], &s[3], &s[4]) != 7) { + debug("control parse failed on '%s'", buf); + return -1; + } + skipline(buf, buflen); + + control_group = u[0]; + control_index = u[1]; + scaler.negative_scale = s[0] / 10000.0f; + scaler.positive_scale = s[1] / 10000.0f; + scaler.offset = s[2] / 10000.0f; + scaler.min_output = s[3] / 10000.0f; + scaler.max_output = s[4] / 10000.0f; + + return 0; +} + +SimpleMixer * +SimpleMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf, unsigned &buflen) +{ + SimpleMixer *sm = nullptr; + mixer_simple_s *mixinfo = nullptr; + unsigned inputs; + int used; + const char *end = buf + buflen; + + /* get the base info for the mixer */ + if (sscanf(buf, "M: %u%n", &inputs, &used) != 1) { + debug("simple parse failed on '%s'", buf); + goto out; + } + + buflen -= used; + + mixinfo = (mixer_simple_s *)malloc(MIXER_SIMPLE_SIZE(inputs)); + + if (mixinfo == nullptr) { + debug("could not allocate memory for mixer info"); + goto out; + } + + mixinfo->control_count = inputs; + + if (parse_output_scaler(end - buflen, buflen, mixinfo->output_scaler)) + goto out; + + for (unsigned i = 0; i < inputs; i++) { + if (parse_control_scaler(end - buflen, buflen, + mixinfo->controls[i].scaler, + mixinfo->controls[i].control_group, + mixinfo->controls[i].control_index)) + goto out; + } + + sm = new SimpleMixer(control_cb, cb_handle, mixinfo); + + if (sm != nullptr) { + mixinfo = nullptr; + debug("loaded mixer with %d inputs", inputs); + + } else { + debug("could not allocate memory for mixer"); + } + +out: + + if (mixinfo != nullptr) + free(mixinfo); + + return sm; +} + +SimpleMixer * +SimpleMixer::pwm_input(Mixer::ControlCallback control_cb, uintptr_t cb_handle, unsigned input, uint16_t min, uint16_t mid, uint16_t max) +{ + SimpleMixer *sm = nullptr; + mixer_simple_s *mixinfo = nullptr; + + mixinfo = (mixer_simple_s *)malloc(MIXER_SIMPLE_SIZE(1)); + + if (mixinfo == nullptr) { + debug("could not allocate memory for mixer info"); + goto out; + } + + mixinfo->control_count = 1; + + /* + * Always pull from group 0, with the input value giving the channel. + */ + mixinfo->controls[0].control_group = 0; + mixinfo->controls[0].control_index = input; + + /* + * Conversion uses both the input and output side of the mixer. + * + * The input side is used to slide the control value such that the min argument + * results in a value of zero. + * + * The output side is used to apply the scaling for the min/max values so that + * the resulting output is a -1.0 ... 1.0 value for the min...max range. + */ + mixinfo->controls[0].scaler.negative_scale = 1.0f; + mixinfo->controls[0].scaler.positive_scale = 1.0f; + mixinfo->controls[0].scaler.offset = -mid; + mixinfo->controls[0].scaler.min_output = -(mid - min); + mixinfo->controls[0].scaler.max_output = (max - mid); + + mixinfo->output_scaler.negative_scale = 500.0f / (mid - min); + mixinfo->output_scaler.positive_scale = 500.0f / (max - mid); + mixinfo->output_scaler.offset = 0.0f; + mixinfo->output_scaler.min_output = -1.0f; + mixinfo->output_scaler.max_output = 1.0f; + + sm = new SimpleMixer(control_cb, cb_handle, mixinfo); + + if (sm != nullptr) { + mixinfo = nullptr; + debug("PWM input mixer for %d", input); + + } else { + debug("could not allocate memory for PWM input mixer"); + } + +out: + + if (mixinfo != nullptr) + free(mixinfo); + + return sm; +} + +unsigned +SimpleMixer::mix(float *outputs, unsigned space) +{ + float sum = 0.0f; + + if (_info == nullptr) + return 0; + + if (space < 1) + return 0; + + for (unsigned i = 0; i < _info->control_count; i++) { + float input; + + _control_cb(_cb_handle, + _info->controls[i].control_group, + _info->controls[i].control_index, + input); + + sum += scale(_info->controls[i].scaler, input); + } + + *outputs = scale(_info->output_scaler, sum); + return 1; +} + +void +SimpleMixer::groups_required(uint32_t &groups) +{ + for (unsigned i = 0; i < _info->control_count; i++) + groups |= 1 << _info->controls[i].control_group; +} + +int +SimpleMixer::check() +{ + int ret; + float junk; + + /* sanity that presumes that a mixer includes a control no more than once */ + /* max of 32 groups due to groups_required API */ + if (_info->control_count > 32) + return -2; + + /* validate the output scaler */ + ret = scale_check(_info->output_scaler); + + if (ret != 0) + return ret; + + /* validate input scalers */ + for (unsigned i = 0; i < _info->control_count; i++) { + + /* verify that we can fetch the control */ + if (_control_cb(_cb_handle, + _info->controls[i].control_group, + _info->controls[i].control_index, + junk) != 0) { + return -3; + } + + /* validate the scaler */ + ret = scale_check(_info->controls[i].scaler); + + if (ret != 0) + return (10 * i + ret); + } + + return 0; +} diff --git a/nuttx/configs/px4io/common/ld.script b/nuttx/configs/px4io/common/ld.script index 17f816acfe..69c2f9cb2e 100755 --- a/nuttx/configs/px4io/common/ld.script +++ b/nuttx/configs/px4io/common/ld.script @@ -74,6 +74,15 @@ SECTIONS _etext = ABSOLUTE(.); } > flash + /* + * Init functions (static constructors and the like) + */ + .init_section : { + _sinit = ABSOLUTE(.); + KEEP(*(.init_array .init_array.*)) + _einit = ABSOLUTE(.); + } > flash + .ARM.extab : { *(.ARM.extab*) } > flash diff --git a/nuttx/configs/px4io/io/appconfig b/nuttx/configs/px4io/io/appconfig index 21a6fbacf4..628607a515 100644 --- a/nuttx/configs/px4io/io/appconfig +++ b/nuttx/configs/px4io/io/appconfig @@ -35,3 +35,6 @@ CONFIGURED_APPS += drivers/boards/px4io CONFIGURED_APPS += drivers/stm32 CONFIGURED_APPS += px4io + +# Mixer library from systemlib +CONFIGURED_APPS += systemlib/mixer diff --git a/nuttx/configs/px4io/io/defconfig b/nuttx/configs/px4io/io/defconfig index 1ac70f8abf..74433e86c4 100755 --- a/nuttx/configs/px4io/io/defconfig +++ b/nuttx/configs/px4io/io/defconfig @@ -87,6 +87,8 @@ CONFIG_ARCH_LEDS=n CONFIG_ARCH_BUTTONS=n CONFIG_ARCH_CALIBRATION=n CONFIG_ARCH_DMA=n +CONFIG_ARCH_MATH_H=y + CONFIG_ARMV7M_CMNVECTOR=y # @@ -342,8 +344,8 @@ CONFIG_DEBUG_I2C=n CONFIG_DEBUG_INPUT=n CONFIG_MSEC_PER_TICK=1 -CONFIG_HAVE_CXX=n -CONFIG_HAVE_CXXINITIALIZE=n +CONFIG_HAVE_CXX=y +CONFIG_HAVE_CXXINITIALIZE=y CONFIG_MM_REGIONS=1 CONFIG_MM_SMALL=y CONFIG_ARCH_LOWPUTC=y diff --git a/nuttx/drivers/serial/serial.c b/nuttx/drivers/serial/serial.c index c650da5db3..28c657af05 100644 --- a/nuttx/drivers/serial/serial.c +++ b/nuttx/drivers/serial/serial.c @@ -660,9 +660,9 @@ static int uart_ioctl(FAR struct file *filep, int cmd, unsigned long arg) int ret = dev->ops->ioctl(filep, cmd, arg); - /* Append any higher level TTY flags */ + /* If the low-level handler didn't handle the call, see if we can handle it here */ - if (ret == OK) + if (ret == -ENOTTY) { switch (cmd) { @@ -686,8 +686,43 @@ static int uart_ioctl(FAR struct file *filep, int cmd, unsigned long arg) irqrestore(state); *(int *)arg = count; + ret = 0; + + break; } + case FIONWRITE: + { + int count; + irqstate_t state = irqsave(); + + /* determine the number of bytes free in the buffer */ + + if (dev->xmit.head < dev->xmit.tail) + { + count = dev->xmit.tail - dev->xmit.head - 1; + } + else + { + count = dev->xmit.size - (dev->xmit.head - dev->xmit.tail) - 1; + } + + irqrestore(state); + + *(int *)arg = count; + ret = 0; + + break; + } + } + } + + /* Append any higher level TTY flags */ + + else if (ret == OK) + { + switch (cmd) + { #ifdef CONFIG_SERIAL_TERMIOS case TCGETS: { diff --git a/nuttx/include/nuttx/fs/ioctl.h b/nuttx/include/nuttx/fs/ioctl.h index 08f62e1648..6d60c2ee97 100644 --- a/nuttx/include/nuttx/fs/ioctl.h +++ b/nuttx/include/nuttx/fs/ioctl.h @@ -110,6 +110,10 @@ * OUT: Bytes readable from this fd */ +#define FIONWRITE _FIOC(0x0005) /* IN: Location to return value (int *) + * OUT: Bytes writable to this fd + */ + /* NuttX file system ioctl definitions **************************************/ #define _DIOCVALID(c) (_IOC_TYPE(c)==_DIOCBASE)