From 5b92c517779500d79e6e5f5cff48336550ce5edb Mon Sep 17 00:00:00 2001 From: px4dev Date: Thu, 20 Dec 2012 21:31:02 -0800 Subject: [PATCH 01/23] Initial implementation of application access to the PX4IO relays. --- apps/drivers/drv_pwm_output.h | 2 +- apps/drivers/px4io/px4io.cpp | 32 +++++++++++++++++++++++++++++++- apps/px4io/comms.c | 25 +++++++++++++++++++++---- 3 files changed, 53 insertions(+), 6 deletions(-) diff --git a/apps/drivers/drv_pwm_output.h b/apps/drivers/drv_pwm_output.h index 97033f2cd1..b2fee65aca 100644 --- a/apps/drivers/drv_pwm_output.h +++ b/apps/drivers/drv_pwm_output.h @@ -95,7 +95,7 @@ ORB_DECLARE(output_pwm); * Note that ioctls and ObjDev updates should not be mixed, as the * behaviour of the system in this case is not defined. */ -#define _PWM_SERVO_BASE 0x2700 +#define _PWM_SERVO_BASE 0x2a00 /** arm all servo outputs handle by this driver */ #define PWM_SERVO_ARM _IOC(_PWM_SERVO_BASE, 0) diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index 456564ba73..d662e634f2 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -61,6 +61,7 @@ #include #include #include +#include #include #include @@ -114,6 +115,8 @@ private: 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? @@ -185,6 +188,7 @@ PX4IO::PX4IO() : _t_outputs(-1), _mixers(nullptr), _primary_pwm_device(false), + _relays(0), _switch_armed(false), _send_needed(false), _config_needed(false) @@ -497,7 +501,9 @@ PX4IO::io_send() /* publish as we send */ 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); @@ -559,6 +565,30 @@ 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; break; diff --git a/apps/px4io/comms.c b/apps/px4io/comms.c index 40ea38cf74..ffb7fd6799 100644 --- a/apps/px4io/comms.c +++ b/apps/px4io/comms.c @@ -178,7 +178,7 @@ comms_handle_command(const void *buffer, size_t length) system_state.fmu_channel_data[i] = cmd->servo_command[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; } @@ -191,9 +191,26 @@ comms_handle_command(const void *buffer, size_t length) // 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++) - system_state.relays[i] = cmd->relay_state[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); } From 3c865c726110e74f22becb427e561799fa082250 Mon Sep 17 00:00:00 2001 From: px4dev Date: Thu, 27 Dec 2012 15:12:09 -0800 Subject: [PATCH 02/23] Build the mixer library for PX4IO as well --- nuttx/configs/px4io/io/appconfig | 3 +++ nuttx/configs/px4io/io/defconfig | 4 +++- 2 files changed, 6 insertions(+), 1 deletion(-) 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..794568a586 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,7 +344,7 @@ CONFIG_DEBUG_I2C=n CONFIG_DEBUG_INPUT=n CONFIG_MSEC_PER_TICK=1 -CONFIG_HAVE_CXX=n +CONFIG_HAVE_CXX=y CONFIG_HAVE_CXXINITIALIZE=n CONFIG_MM_REGIONS=1 CONFIG_MM_SMALL=y From f0da789626c32695e670b55dab29283eed4a05c6 Mon Sep 17 00:00:00 2001 From: px4dev Date: Fri, 28 Dec 2012 16:44:17 -0800 Subject: [PATCH 03/23] Remove the unused complex-multirotor setup ioctl, since it's not implemented anywhere. --- apps/drivers/drv_mixer.h | 18 ------------------ apps/drivers/hil/hil.cpp | 5 ----- apps/drivers/px4fmu/fmu.cpp | 5 ----- apps/drivers/px4io/px4io.cpp | 5 ----- 4 files changed, 33 deletions(-) diff --git a/apps/drivers/drv_mixer.h b/apps/drivers/drv_mixer.h index 793e86b324..498ca98360 100644 --- a/apps/drivers/drv_mixer.h +++ b/apps/drivers/drv_mixer.h @@ -100,24 +100,6 @@ 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 */ -}; - -/** - * Add a multirotor mixer in (struct mixer_multirotor_s *)arg - */ -#define MIXERIOCADDMULTIROTOR _MIXERIOC(3) - /** * Add mixers(s) from a the file in (const char *)arg */ diff --git a/apps/drivers/hil/hil.cpp b/apps/drivers/hil/hil.cpp index 67b16aa42f..0780f3bb25 100644 --- a/apps/drivers/hil/hil.cpp +++ b/apps/drivers/hil/hil.cpp @@ -577,11 +577,6 @@ HIL::pwm_ioctl(file *filp, int cmd, unsigned long arg) break; } - case MIXERIOCADDMULTIROTOR: - /* XXX not yet supported */ - ret = -ENOTTY; - break; - case MIXERIOCLOADFILE: { const char *path = (const char *)arg; diff --git a/apps/drivers/px4fmu/fmu.cpp b/apps/drivers/px4fmu/fmu.cpp index a995f6214d..e212507279 100644 --- a/apps/drivers/px4fmu/fmu.cpp +++ b/apps/drivers/px4fmu/fmu.cpp @@ -544,11 +544,6 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) break; } - case MIXERIOCADDMULTIROTOR: - /* XXX not yet supported */ - ret = -ENOTTY; - break; - case MIXERIOCLOADFILE: { const char *path = (const char *)arg; diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index 9f3dba047f..d185a1ac4d 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -609,11 +609,6 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg) } break; - case MIXERIOCADDMULTIROTOR: - /* XXX not yet supported */ - ret = -ENOTTY; - break; - case MIXERIOCLOADFILE: { MixerGroup *newmixers; const char *path = (const char *)arg; From 35c82ff2fc63ab823770f9776e6b6a0f81cd4452 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 29 Dec 2012 00:01:04 -0800 Subject: [PATCH 04/23] Make mixer ioctls load from a memory buffer rather than a file. This is prep for uploading the memory buffer to IO to be processed there. --- apps/drivers/drv_mixer.h | 7 +- apps/drivers/hil/hil.cpp | 18 +- apps/drivers/px4fmu/fmu.cpp | 17 +- apps/drivers/px4io/px4io.cpp | 36 +-- apps/systemcmds/mixer/mixer.c | 72 +++-- apps/systemlib/mixer/mixer.cpp | 88 +------ apps/systemlib/mixer/mixer.h | 140 +++++++--- apps/systemlib/mixer/mixer_group.cpp | 306 ++-------------------- apps/systemlib/mixer/mixer_multirotor.cpp | 56 ++++ apps/systemlib/mixer/mixer_simple.cpp | 260 ++++++++++++++++++ 10 files changed, 536 insertions(+), 464 deletions(-) create mode 100644 apps/systemlib/mixer/mixer_simple.cpp diff --git a/apps/drivers/drv_mixer.h b/apps/drivers/drv_mixer.h index 498ca98360..9f43015d91 100644 --- a/apps/drivers/drv_mixer.h +++ b/apps/drivers/drv_mixer.h @@ -100,10 +100,13 @@ struct mixer_simple_s { */ #define MIXERIOCADDSIMPLE _MIXERIOC(2) +/* _MIXERIOC(3) was deprecated */ +/* _MIXERIOC(4) was deprecated */ + /** - * Add mixers(s) from a the file in (const char *)arg + * Add mixer(s) from the buffer 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 0780f3bb25..f227db1f76 100644 --- a/apps/drivers/hil/hil.cpp +++ b/apps/drivers/hil/hil.cpp @@ -577,21 +577,19 @@ HIL::pwm_ioctl(file *filp, int cmd, unsigned long arg) break; } - case MIXERIOCLOADFILE: { - const char *path = (const char *)arg; + case MIXERIOCLOADBUF: { + const char *buf = (const char *)arg; + unsigned buflen = strnlen(buf, 1024); - if (_mixers != nullptr) { - delete _mixers; - _mixers = nullptr; - } + if (_mixers == nullptr) + _mixers = new MixerGroup(control_callback, (uintptr_t)&_controls); - _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); @@ -600,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 e212507279..a8d1601178 100644 --- a/apps/drivers/px4fmu/fmu.cpp +++ b/apps/drivers/px4fmu/fmu.cpp @@ -544,23 +544,19 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) break; } - case MIXERIOCLOADFILE: { - const char *path = (const char *)arg; + case MIXERIOCLOADBUF: { + const char *buf = (const char *)arg; + unsigned buflen = strnlen(buf, 1024); - 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); @@ -569,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 d185a1ac4d..cb69856c84 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -609,27 +609,29 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg) } break; - case MIXERIOCLOADFILE: { - MixerGroup *newmixers; - const char *path = (const char *)arg; + case MIXERIOCLOADBUF: { + const char *buf = (const char *)arg; + unsigned buflen = strnlen(buf, 1024); - /* allocate a new mixer group and load it from the file */ - newmixers = new MixerGroup(control_callback, (uintptr_t)&_controls); + if (_mixers == nullptr) + _mixers = new MixerGroup(control_callback, (uintptr_t)&_controls); - if (newmixers->load_from_file(path) != 0) { - delete newmixers; - ret = -EINVAL; + 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; + } } - - /* swap the new mixers in for the old */ - if (_mixers != nullptr) { - delete _mixers; - } - - _mixers = newmixers; - + break; } - break; default: /* not a recognised value */ diff --git a/apps/systemcmds/mixer/mixer.c b/apps/systemcmds/mixer/mixer.c index 3f52bdbf16..8d73bfcc4c 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,56 @@ 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; + + /* 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..0023dc3803 100644 --- a/apps/systemlib/mixer/mixer.h +++ b/apps/systemlib/mixer/mixer.h @@ -234,50 +234,45 @@ public: void add_mixer(Mixer *mixer); /** - * Reads a mixer definition from a file and configures a corresponding - * group. + * 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 + * .......... + * + * The null mixer definition has the form: + * + * Z: + * + * Simple Mixer + * ............ + * + * A simple mixer definition begins with: + * + * M: + * O: <-ve scale> <+ve scale> + * + * The definition continues with entries describing the control + * inputs and their scaling, in the form: + * + * S: <-ve scale> <+ve scale> + * + * Multirotor Mixer + * ................ + * + * The multirotor mixer definition is a single line of the form: + * + * R: * - * Null Mixer: - * - * Z: - * - * This mixer generates a constant zero output, and is normally used to - * skip over outputs that are not in use. - * - * Simple Mixer: - * - * M: - * S: - * S: ... - * - * 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 values for the output scaler are ignored by the mixer. - * - * - * - * Values marked * are integers representing floating point values; values are - * scaled by 10000 on load/save. - * - * 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. * @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 +289,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 +328,27 @@ public: mixer_simple_s *mixinfo); ~SimpleMixer(); + /** + * 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 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); + virtual unsigned mix(float *outputs, unsigned space); virtual void groups_required(uint32_t &groups); @@ -330,10 +361,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 +434,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..af55690012 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) @@ -358,44 +114,38 @@ 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 we have consumed the buffer */ + while (buflen > 0) { + Mixer *m = nullptr; + const char *p = end - buflen; - if (fd < 0) { - debug("failed to open %s", path); - return -1; - } - - for (unsigned count = 0;; count++) { - int result; - Mixer *mixer; - - result = mixer_load(_control_cb, - _cb_handle, - fd, - mixer); - - /* error? */ - if (result < 0) { - debug("error"); - 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, buflen); + break; + case 'M': + m = SimpleMixer::from_text(_control_cb, _cb_handle, p, buflen); + break; + case 'R': + m = MultirotorMixer::from_text(_control_cb, _cb_handle, p, buflen); + break; + default: + /* it's probably junk or whitespace */ + break; } - - /* EOF or error */ - if (result < 1) { - printf("[mixer] loaded %u mixers\n", count); - debug("EOF"); - break; + if (m != nullptr) { + add_mixer(m); + ret = 0; + } else { + /* skip whitespace or junk in the buffer */ + buflen--; } - - debug("loaded mixer %p", mixer); - add_mixer(mixer); } - - close(fd); - return 0; + return ret; } diff --git a/apps/systemlib/mixer/mixer_multirotor.cpp b/apps/systemlib/mixer/mixer_multirotor.cpp index b5b25e5320..233025b219 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,59 @@ 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) { diff --git a/apps/systemlib/mixer/mixer_simple.cpp b/apps/systemlib/mixer/mixer_simple.cpp new file mode 100644 index 0000000000..c0066ac8dc --- /dev/null +++ b/apps/systemlib/mixer/mixer_simple.cpp @@ -0,0 +1,260 @@ +/**************************************************************************** + * + * 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 * +skipspace(const char *p, unsigned &len) +{ + while (isspace(*p)) { + if (len == 0) + return nullptr; + len--; + p++; + } + return p; +} + +int +SimpleMixer::parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler_s &scaler) +{ + int ret; + int s[5]; + int used; + + buf = skipspace(buf, buflen); + if (buflen < 16) + return -1; + + if ((ret = sscanf(buf, "O: %d %d %d %d %d%n", + &s[0], &s[1], &s[2], &s[3], &s[4], &used)) != 5) { + debug("scaler parse failed on '%s' (got %d)", buf, ret); + return -1; + } + buflen -= used; + + 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]; + int used; + + buf = skipspace(buf, buflen); + if (buflen < 16) + return -1; + + if (sscanf(buf, "S: %u %u %d %d %d %d %d%n", + &u[0], &u[1], &s[0], &s[1], &s[2], &s[3], &s[4], &used) != 7) { + debug("control parse failed on '%s'", buf); + return -1; + } + buflen -= used; + + 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; +} + +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; +} From d5da457e292d5c4276774c3da85b3805d3d2bbb7 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 29 Dec 2012 12:57:29 -0800 Subject: [PATCH 05/23] Fix PX4IO to run C++ static ctors --- apps/px4io/px4io.c | 5 +++++ nuttx/configs/px4io/common/ld.script | 9 +++++++++ nuttx/configs/px4io/io/defconfig | 2 +- 3 files changed, 15 insertions(+), 1 deletion(-) diff --git a/apps/px4io/px4io.c b/apps/px4io/px4io.c index a3ac9e3e78..230906662f 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)); 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/defconfig b/nuttx/configs/px4io/io/defconfig index 794568a586..74433e86c4 100755 --- a/nuttx/configs/px4io/io/defconfig +++ b/nuttx/configs/px4io/io/defconfig @@ -345,7 +345,7 @@ CONFIG_DEBUG_INPUT=n CONFIG_MSEC_PER_TICK=1 CONFIG_HAVE_CXX=y -CONFIG_HAVE_CXXINITIALIZE=n +CONFIG_HAVE_CXXINITIALIZE=y CONFIG_MM_REGIONS=1 CONFIG_MM_SMALL=y CONFIG_ARCH_LOWPUTC=y From 6ede0e2f18f001afb390f3bbb0b989bdd2759c24 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 29 Dec 2012 12:58:10 -0800 Subject: [PATCH 06/23] Add the ability to reset a mixer group. Report the remaining buffer size from load_from_buf. --- apps/systemlib/mixer/mixer.h | 10 ++++++++-- apps/systemlib/mixer/mixer_group.cpp | 24 +++++++++++++++--------- 2 files changed, 23 insertions(+), 11 deletions(-) diff --git a/apps/systemlib/mixer/mixer.h b/apps/systemlib/mixer/mixer.h index 0023dc3803..95b576d41e 100644 --- a/apps/systemlib/mixer/mixer.h +++ b/apps/systemlib/mixer/mixer.h @@ -233,6 +233,11 @@ public: */ void add_mixer(Mixer *mixer); + /** + * Remove all the mixers from the group. + */ + void reset(); + /** * Adds mixers to the group based on a text description in a buffer. * @@ -269,10 +274,11 @@ public: * R: * * @param buf The mixer configuration buffer. - * @param buflen The length of the 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_buf(const char *buf, unsigned buflen); + int load_from_buf(const char *buf, unsigned &buflen); private: Mixer *_first; /**< linked list of mixers */ diff --git a/apps/systemlib/mixer/mixer_group.cpp b/apps/systemlib/mixer/mixer_group.cpp index af55690012..6dfe4fbefa 100644 --- a/apps/systemlib/mixer/mixer_group.cpp +++ b/apps/systemlib/mixer/mixer_group.cpp @@ -64,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 @@ -88,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) { @@ -114,7 +120,7 @@ MixerGroup::groups_required(uint32_t &groups) } int -MixerGroup::load_from_buf(const char *buf, unsigned buflen) +MixerGroup::load_from_buf(const char *buf, unsigned &buflen) { int ret = -1; const char *end = buf + buflen; From 6b9d5dac4dd866903ed28f062a3c998a85058e63 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 29 Dec 2012 12:58:41 -0800 Subject: [PATCH 07/23] Rough in the new mixer path for PX4IO. --- apps/px4io/Makefile | 4 +- apps/px4io/comms.c | 4 +- apps/px4io/{mixer.c => mixer.cpp} | 111 ++++++++++++++++++++++-------- apps/px4io/protocol.h | 48 ++++++++++--- apps/px4io/px4io.h | 6 ++ 5 files changed, 132 insertions(+), 41 deletions(-) rename apps/px4io/{mixer.c => mixer.cpp} (59%) 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..bc2142a4b8 100644 --- a/apps/px4io/comms.c +++ b/apps/px4io/comms.c @@ -192,7 +192,6 @@ comms_handle_command(const void *buffer, size_t length) irqrestore(flags); } - static void comms_handle_frame(void *arg, const void *buffer, size_t length) { @@ -208,6 +207,9 @@ comms_handle_frame(void *arg, const void *buffer, size_t length) case F2I_CONFIG_MAGIC: comms_handle_config(buffer, length); break; + case F2I_MIXER_MAGIC: + mixer_handle_text(buffer, length); + break; default: frame_bad++; break; diff --git a/apps/px4io/mixer.c b/apps/px4io/mixer.cpp similarity index 59% rename from apps/px4io/mixer.c rename to apps/px4io/mixer.cpp index f02e98ae45..6af1dd3dc6 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,11 @@ #include #include +#include +extern "C" { #include "px4io.h" +} /* * Count of periodic calls in which we have no FMU input. @@ -58,28 +61,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; /* @@ -115,17 +113,30 @@ 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] * 1000.0f) + 1000; + } 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]); } } @@ -145,13 +156,57 @@ 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 */ + /* 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] - 1000.0f) / 1000.0f; + + return 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; + if (length < sizeof(px4io_mixdata)) + return; + + unsigned text_length = length - sizeof(px4io_mixdata); + + switch (msg->action) { + case F2I_MIXER_ACTION_RESET: + mixer_group.reset(); + mixer_text_length = 0; + /* FALLTHROUGH */ + case F2I_MIXER_ACTION_APPEND: + /* 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'; + + /* 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); + + /* 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); + break; + } +} \ No newline at end of file diff --git a/apps/px4io/protocol.h b/apps/px4io/protocol.h index c704b12016..080d33001d 100644 --- a/apps/px4io/protocol.h +++ b/apps/px4io/protocol.h @@ -47,25 +47,21 @@ #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]; 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 (sizeof(struct px4io_mixdata) - HX_STREAM_MAX_FRAME) + #pragma pack(pop) \ No newline at end of file diff --git a/apps/px4io/px4io.h b/apps/px4io/px4io.h index 45b7cf847a..f127df4a3d 100644 --- a/apps/px4io/px4io.h +++ b/apps/px4io/px4io.h @@ -84,6 +84,11 @@ struct sys_state_s */ uint16_t fmu_channel_data[PX4IO_OUTPUT_CHANNELS]; + /* + * Mixed servo outputs + */ + uint16_t servos[IO_SERVO_COUNT]; + /* * Relay controls */ @@ -149,6 +154,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. From 0ae5997bd0f05ca15cca6a17e8b7894227788151 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 29 Dec 2012 13:28:32 -0800 Subject: [PATCH 08/23] Fix some scaling errors in the PWM <-> mixer-internal conversions. --- apps/px4io/mixer.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/apps/px4io/mixer.cpp b/apps/px4io/mixer.cpp index 6af1dd3dc6..9b3d65dbae 100644 --- a/apps/px4io/mixer.cpp +++ b/apps/px4io/mixer.cpp @@ -126,7 +126,7 @@ mixer_tick(void) for (unsigned i = 0; i < IO_SERVO_COUNT; i++) { if (i < mixed) { /* scale to servo output */ - system_state.servos[i] = (outputs[i] * 1000.0f) + 1000; + system_state.servos[i] = (outputs[i] * 500.0f) + 1500; } else { /* set to zero to inhibit PWM pulse output */ system_state.servos[i] = 0; @@ -168,7 +168,7 @@ mixer_callback(uintptr_t handle, /* 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] - 1000.0f) / 1000.0f; + control = ((float)control_values[control_index] - 1500.0f) / 500.0f; return 0; } From f9520ee39d0e14bc67cce809375fb69de9a7f977 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 29 Dec 2012 16:00:50 -0800 Subject: [PATCH 09/23] Factory method for a simple mixer that converts PWM/PPM values to the standard internal format. --- apps/systemlib/mixer/mixer.h | 64 +++++++++++++-------- apps/systemlib/mixer/mixer_simple.cpp | 80 +++++++++++++++++++++++++-- 2 files changed, 118 insertions(+), 26 deletions(-) diff --git a/apps/systemlib/mixer/mixer.h b/apps/systemlib/mixer/mixer.h index 95b576d41e..26ba5db7e7 100644 --- a/apps/systemlib/mixer/mixer.h +++ b/apps/systemlib/mixer/mixer.h @@ -243,34 +243,34 @@ public: * * 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 + * mixers; they are summarised here, but see ROMFS/mixers/README for * more details. * * Null Mixer * .......... - * + * * The null mixer definition has the form: - * + * * Z: - * + * * Simple Mixer * ............ - * + * * A simple mixer definition begins with: - * + * * M: * O: <-ve scale> <+ve scale> - * + * * The definition continues with entries describing the control * inputs and their scaling, in the form: - * + * * S: <-ve scale> <+ve scale> - * + * * Multirotor Mixer * ................ - * + * * The multirotor mixer definition is a single line of the form: - * + * * R: * * @param buf The mixer configuration buffer. @@ -335,7 +335,7 @@ public: ~SimpleMixer(); /** - * Factory method. + * 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. @@ -351,9 +351,29 @@ public: * if the text format is bad. */ static SimpleMixer *from_text(Mixer::ControlCallback control_cb, - uintptr_t cb_handle, - const char *buf, - unsigned &buflen); + 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); @@ -375,10 +395,10 @@ private: 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); + unsigned &buflen, + mixer_scaler_s &scaler, + uint8_t &control_group, + uint8_t &control_index); }; /** @@ -457,9 +477,9 @@ public: * if the text format is bad. */ static MultirotorMixer *from_text(Mixer::ControlCallback control_cb, - uintptr_t cb_handle, - const char *buf, - unsigned &buflen); + 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_simple.cpp b/apps/systemlib/mixer/mixer_simple.cpp index c0066ac8dc..05de1f6e04 100644 --- a/apps/systemlib/mixer/mixer_simple.cpp +++ b/apps/systemlib/mixer/mixer_simple.cpp @@ -77,9 +77,11 @@ skipspace(const char *p, unsigned &len) while (isspace(*p)) { if (len == 0) return nullptr; + len--; p++; } + return p; } @@ -91,14 +93,16 @@ SimpleMixer::parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler int used; buf = skipspace(buf, buflen); + if (buflen < 16) return -1; if ((ret = sscanf(buf, "O: %d %d %d %d %d%n", - &s[0], &s[1], &s[2], &s[3], &s[4], &used)) != 5) { + &s[0], &s[1], &s[2], &s[3], &s[4], &used)) != 5) { debug("scaler parse failed on '%s' (got %d)", buf, ret); return -1; } + buflen -= used; scaler.negative_scale = s[0] / 10000.0f; @@ -118,6 +122,7 @@ SimpleMixer::parse_control_scaler(const char *buf, unsigned &buflen, mixer_scale int used; buf = skipspace(buf, buflen); + if (buflen < 16) return -1; @@ -126,6 +131,7 @@ SimpleMixer::parse_control_scaler(const char *buf, unsigned &buflen, mixer_scale debug("control parse failed on '%s'", buf); return -1; } + buflen -= used; control_group = u[0]; @@ -153,13 +159,16 @@ SimpleMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, c 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)) @@ -167,21 +176,84 @@ SimpleMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, c 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)) + 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.lower_limit = -(mid - min); + mixinfo->controls[0].scaler.upper_limit = (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); From d81edb09cf29dd36d50ce7a7bcf55631fecc470f Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 29 Dec 2012 16:01:24 -0800 Subject: [PATCH 10/23] whitespace/formatting --- apps/px4io/comms.c | 14 +++++-- apps/px4io/controls.c | 4 +- apps/px4io/dsm.c | 40 +++++++++++------- apps/px4io/mixer.cpp | 13 ++++-- apps/px4io/protocol.h | 4 +- apps/px4io/px4io.c | 2 +- apps/px4io/px4io.h | 13 +++--- apps/px4io/safety.c | 22 +++++++--- apps/px4io/sbus.c | 50 ++++++++++++----------- apps/systemcmds/mixer/mixer.c | 2 + apps/systemlib/mixer/mixer_group.cpp | 32 +++++++++------ apps/systemlib/mixer/mixer_multirotor.cpp | 10 +++-- 12 files changed, 129 insertions(+), 77 deletions(-) diff --git a/apps/px4io/comms.c b/apps/px4io/comms.c index bc2142a4b8..b815eda3cc 100644 --- a/apps/px4io/comms.c +++ b/apps/px4io/comms.c @@ -110,6 +110,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 +124,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 +134,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; @@ -172,7 +175,7 @@ comms_handle_command(const void *buffer, size_t length) system_state.fmu_channel_data[i] = cmd->servo_command[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; } @@ -185,7 +188,7 @@ comms_handle_command(const void *buffer, size_t length) // if (!system_state.arm_ok && system_state.armed) // system_state.armed = false; - /* XXX do relay changes here */ + /* XXX do relay changes here */ for (unsigned i = 0; i < PX4IO_RELAY_CHANNELS; i++) system_state.relays[i] = cmd->relay_state[i]; @@ -204,14 +207,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.cpp b/apps/px4io/mixer.cpp index 9b3d65dbae..0fd4ac7172 100644 --- a/apps/px4io/mixer.cpp +++ b/apps/px4io/mixer.cpp @@ -69,9 +69,9 @@ 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); + uint8_t control_group, + uint8_t control_index, + float &control); static MixerGroup mixer_group(mixer_callback, 0); @@ -96,6 +96,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; @@ -127,6 +128,7 @@ mixer_tick(void) 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; @@ -144,6 +146,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); @@ -180,6 +183,7 @@ mixer_handle_text(const void *buffer, size_t length) static unsigned mixer_text_length = 0; px4io_mixdata *msg = (px4io_mixdata *)buffer; + if (length < sizeof(px4io_mixdata)) return; @@ -189,8 +193,10 @@ mixer_handle_text(const void *buffer, size_t length) case F2I_MIXER_ACTION_RESET: mixer_group.reset(); mixer_text_length = 0; + /* FALLTHROUGH */ case F2I_MIXER_ACTION_APPEND: + /* check for overflow - this is really fatal */ if ((mixer_text_length + text_length + 1) > sizeof(mixer_text)) return; @@ -207,6 +213,7 @@ mixer_handle_text(const void *buffer, size_t length) /* 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); + break; } } \ No newline at end of file diff --git a/apps/px4io/protocol.h b/apps/px4io/protocol.h index 080d33001d..979f2f8cb3 100644 --- a/apps/px4io/protocol.h +++ b/apps/px4io/protocol.h @@ -60,7 +60,7 @@ struct px4io_command { }; /** - * Periodic report from IO to FMU + * Periodic report from IO to FMU */ struct px4io_report { uint16_t i2f_magic; @@ -72,7 +72,7 @@ struct px4io_report { }; /** - * As-needed config message from FMU to IO + * As-needed config message from FMU to IO */ struct px4io_config { uint16_t f2i_config_magic; diff --git a/apps/px4io/px4io.c b/apps/px4io/px4io.c index 230906662f..74362617d8 100644 --- a/apps/px4io/px4io.c +++ b/apps/px4io/px4io.c @@ -88,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 f127df4a3d..ad10961b44 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 */ 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 a8f628a84e..9679f657be 100644 --- a/apps/px4io/sbus.c +++ b/apps/px4io/sbus.c @@ -88,6 +88,7 @@ sbus_init(const char *device) last_rx_time = hrt_absolute_time(); debug("Sbus: ready"); + } else { debug("Sbus: open failed"); } @@ -109,7 +110,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 +118,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 +135,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 +147,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 +158,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 +180,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 @@ -213,8 +216,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++) { @@ -232,6 +235,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; } @@ -245,7 +249,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 8d73bfcc4c..e2f7b5bd58 100644 --- a/apps/systemcmds/mixer/mixer.c +++ b/apps/systemcmds/mixer/mixer.c @@ -117,6 +117,8 @@ load(const char *devname, const char *fname) 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; diff --git a/apps/systemlib/mixer/mixer_group.cpp b/apps/systemlib/mixer/mixer_group.cpp index 6dfe4fbefa..b98531c4d7 100644 --- a/apps/systemlib/mixer/mixer_group.cpp +++ b/apps/systemlib/mixer/mixer_group.cpp @@ -91,7 +91,7 @@ MixerGroup::reset() mixer = _first; _first = mixer->_next; delete mixer; - } + } } unsigned @@ -132,26 +132,32 @@ MixerGroup::load_from_buf(const char *buf, unsigned &buflen) /* use the next character as a hint to decide which mixer class to construct */ switch (*p) { - case 'Z': - m = NullMixer::from_text(p, buflen); - break; - case 'M': - m = SimpleMixer::from_text(_control_cb, _cb_handle, p, buflen); - break; - case 'R': - m = MultirotorMixer::from_text(_control_cb, _cb_handle, p, buflen); - break; - default: - /* it's probably junk or whitespace */ - break; + case 'Z': + m = NullMixer::from_text(p, buflen); + break; + + case 'M': + m = SimpleMixer::from_text(_control_cb, _cb_handle, p, buflen); + break; + + case 'R': + m = MultirotorMixer::from_text(_control_cb, _cb_handle, p, buflen); + break; + + default: + /* it's probably junk or whitespace */ + break; } + if (m != nullptr) { add_mixer(m); ret = 0; + } else { /* skip whitespace or junk in the buffer */ buflen--; } } + return ret; } diff --git a/apps/systemlib/mixer/mixer_multirotor.cpp b/apps/systemlib/mixer/mixer_multirotor.cpp index 233025b219..3dd6dfcf75 100644 --- a/apps/systemlib/mixer/mixer_multirotor.cpp +++ b/apps/systemlib/mixer/mixer_multirotor.cpp @@ -166,10 +166,12 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl 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+")) { @@ -226,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; } From b8250de1e67f63f9ca3b990e016744584a328922 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 29 Dec 2012 16:22:30 -0800 Subject: [PATCH 11/23] Assorted compile fixes. --- apps/systemlib/mixer/mixer.h | 2 +- apps/systemlib/mixer/mixer_simple.cpp | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/apps/systemlib/mixer/mixer.h b/apps/systemlib/mixer/mixer.h index 26ba5db7e7..00ddf15817 100644 --- a/apps/systemlib/mixer/mixer.h +++ b/apps/systemlib/mixer/mixer.h @@ -368,7 +368,7 @@ public: * @return A new SimpleMixer instance, or nullptr if one could not be * allocated. */ - static SimpleMixer *pwm_input(Mixer::ControlCallback *control_cb, + static SimpleMixer *pwm_input(Mixer::ControlCallback control_cb, uintptr_t cb_handle, unsigned input, uint16_t min, diff --git a/apps/systemlib/mixer/mixer_simple.cpp b/apps/systemlib/mixer/mixer_simple.cpp index 05de1f6e04..048fba148e 100644 --- a/apps/systemlib/mixer/mixer_simple.cpp +++ b/apps/systemlib/mixer/mixer_simple.cpp @@ -201,7 +201,7 @@ out: } SimpleMixer * -SimpleMixer::pwm_input(Mixer::ControlCallback *control_cb, uintptr_t cb_handle, unsigned input, uint16_t min, uint16_t mid, uint16_t max) +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; @@ -233,8 +233,8 @@ SimpleMixer::pwm_input(Mixer::ControlCallback *control_cb, uintptr_t cb_handle, 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.lower_limit = -(mid - min); - mixinfo->controls[0].scaler.upper_limit = (max - 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); From 85375c2201e6b9fe4737cb85b462c0aef8d271ca Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 29 Dec 2012 17:15:48 -0800 Subject: [PATCH 12/23] 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(-) diff --git a/apps/px4io/comms.c b/apps/px4io/comms.c index b815eda3cc..b87f21dba8 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 0fd4ac7172..52666ae39c 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 979f2f8cb3..18d49a6be6 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 ad10961b44..7257d65227 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 From b14abad3a06f0207e051744d4f892b1840cb3291 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 30 Dec 2012 01:16:28 -0800 Subject: [PATCH 13/23] Fix logic for handling partial buffers. --- apps/systemlib/mixer/mixer_group.cpp | 37 +++++++++++++++++++++------- 1 file changed, 28 insertions(+), 9 deletions(-) diff --git a/apps/systemlib/mixer/mixer_group.cpp b/apps/systemlib/mixer/mixer_group.cpp index b98531c4d7..9aeab1dcc6 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; } From c740e9c6160b72fa11d252e5ef4efe6ae741abba Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 30 Dec 2012 01:16:54 -0800 Subject: [PATCH 14/23] Add a receive error counter for debugging purposes. --- apps/px4io/comms.c | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/apps/px4io/comms.c b/apps/px4io/comms.c index b87f21dba8..843d0978ac 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; From fd016abd46954311f85dd4a9345fce6aef680c44 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 30 Dec 2012 01:17:19 -0800 Subject: [PATCH 15/23] 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(-) diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index cb69856c84..e5a1edea10 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 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/mixer.cpp b/apps/px4io/mixer.cpp index 52666ae39c..d21b3a8988 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); - /* 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 18d49a6be6..afbf09cd39 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 From f6ea42ab5e886b3475350c5dab95b5985bda26bc Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 30 Dec 2012 01:28:07 -0800 Subject: [PATCH 16/23] Fix px4io signal test command to force FMU armed state. --- apps/drivers/px4io/px4io.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index e5a1edea10..8e0fde727a 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -705,6 +705,13 @@ test(void) close(fd); + actuator_armed_s aa; + + aa.armed = true; + aa.lockdown = false; + + orb_advertise(ORB_ID(actuator_armed), &aa); + exit(0); } From 668d1b330114fefc0ae7a6c476074f2c263c1476 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 30 Dec 2012 13:58:30 +0100 Subject: [PATCH 17/23] Removed text reuse, causing crash with stack trace --- apps/px4io/mixer.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/apps/px4io/mixer.cpp b/apps/px4io/mixer.cpp index d21b3a8988..6b37d83ab8 100644 --- a/apps/px4io/mixer.cpp +++ b/apps/px4io/mixer.cpp @@ -220,11 +220,11 @@ mixer_handle_text(const void *buffer, size_t length) 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); + // 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; + // mixer_text_length = resid; } break; From f2d4eb2887829efebef853c16223151e0a37f5bb Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 30 Dec 2012 15:08:25 -0800 Subject: [PATCH 18/23] Revert "Removed text reuse, causing crash with stack trace" This reverts commit 668d1b330114fefc0ae7a6c476074f2c263c1476. --- apps/px4io/mixer.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/apps/px4io/mixer.cpp b/apps/px4io/mixer.cpp index 6b37d83ab8..d21b3a8988 100644 --- a/apps/px4io/mixer.cpp +++ b/apps/px4io/mixer.cpp @@ -220,11 +220,11 @@ mixer_handle_text(const void *buffer, size_t length) 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); + /* 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; + mixer_text_length = resid; } break; From dbb841b0dcad55e36d221fc83ac7bab283438a94 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 30 Dec 2012 15:09:21 -0800 Subject: [PATCH 19/23] Rework the way we scan text for scaler definitions; something weird was going on with sscanf's handling of %n that wasn't obvious. This seems to work around the issue. --- apps/systemlib/mixer/mixer_simple.cpp | 52 ++++++++++++++------------- 1 file changed, 27 insertions(+), 25 deletions(-) diff --git a/apps/systemlib/mixer/mixer_simple.cpp b/apps/systemlib/mixer/mixer_simple.cpp index 048fba148e..07dc5f37fa 100644 --- a/apps/systemlib/mixer/mixer_simple.cpp +++ b/apps/systemlib/mixer/mixer_simple.cpp @@ -72,17 +72,25 @@ SimpleMixer::~SimpleMixer() } static const char * -skipspace(const char *p, unsigned &len) +findtag(const char *buf, unsigned &buflen, char tag) { - while (isspace(*p)) { - if (len == 0) - return nullptr; - - len--; - p++; + while (buflen >= 2) { + if ((buf[0] == tag) && (buf[1] == ':')) + return buf; + buf++; + buflen--; } + return nullptr; +} - return p; +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 @@ -90,20 +98,17 @@ SimpleMixer::parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler { int ret; int s[5]; - int used; - - buf = skipspace(buf, buflen); - - if (buflen < 16) + + buf = findtag(buf, buflen, 'O'); + if ((buf == nullptr) || (buflen < 12)) return -1; - if ((ret = sscanf(buf, "O: %d %d %d %d %d%n", - &s[0], &s[1], &s[2], &s[3], &s[4], &used)) != 5) { + 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; } - - buflen -= used; + skipline(buf, buflen); scaler.negative_scale = s[0] / 10000.0f; scaler.positive_scale = s[1] / 10000.0f; @@ -119,20 +124,17 @@ SimpleMixer::parse_control_scaler(const char *buf, unsigned &buflen, mixer_scale { unsigned u[2]; int s[5]; - int used; - buf = skipspace(buf, buflen); - - if (buflen < 16) + buf = findtag(buf, buflen, 'S'); + if ((buf == nullptr) || (buflen < 16)) return -1; - if (sscanf(buf, "S: %u %u %d %d %d %d %d%n", - &u[0], &u[1], &s[0], &s[1], &s[2], &s[3], &s[4], &used) != 7) { + 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; } - - buflen -= used; + skipline(buf, buflen); control_group = u[0]; control_index = u[1]; From e787fa5bce52e10179cae33df56caa765bfa75e2 Mon Sep 17 00:00:00 2001 From: px4dev Date: Thu, 3 Jan 2013 00:33:22 -0800 Subject: [PATCH 20/23] Add FIONWRITE to allow applications to sniff the amount of writable space on a descriptor. Implement this for serial devices only. --- nuttx/drivers/serial/serial.c | 21 +++++++++++++++++++++ nuttx/include/nuttx/fs/ioctl.h | 4 ++++ 2 files changed, 25 insertions(+) diff --git a/nuttx/drivers/serial/serial.c b/nuttx/drivers/serial/serial.c index c650da5db3..24744524fa 100644 --- a/nuttx/drivers/serial/serial.c +++ b/nuttx/drivers/serial/serial.c @@ -688,6 +688,27 @@ static int uart_ioctl(FAR struct file *filep, int cmd, unsigned long arg) *(int *)arg = count; } + 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; + } + #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) From 91ca80e6347feebb1e39b9d16df3f88fb8a68152 Mon Sep 17 00:00:00 2001 From: px4dev Date: Fri, 4 Jan 2013 21:28:26 -0800 Subject: [PATCH 21/23] Fix the handling of FIONREAD/FIONWRITE; thanks Tridge. --- nuttx/drivers/serial/serial.c | 22 ++++++++++++++++++---- 1 file changed, 18 insertions(+), 4 deletions(-) diff --git a/nuttx/drivers/serial/serial.c b/nuttx/drivers/serial/serial.c index 24744524fa..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,6 +686,9 @@ static int uart_ioctl(FAR struct file *filep, int cmd, unsigned long arg) irqrestore(state); *(int *)arg = count; + ret = 0; + + break; } case FIONWRITE: @@ -695,7 +698,7 @@ static int uart_ioctl(FAR struct file *filep, int cmd, unsigned long arg) /* determine the number of bytes free in the buffer */ - if (dev->xmit.head <= dev->xmit.tail) + if (dev->xmit.head < dev->xmit.tail) { count = dev->xmit.tail - dev->xmit.head - 1; } @@ -707,8 +710,19 @@ static int uart_ioctl(FAR struct file *filep, int cmd, unsigned long arg) 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: { From 69cdab9afc0215e26dc5084e21fd61725acb6c84 Mon Sep 17 00:00:00 2001 From: px4dev Date: Fri, 4 Jan 2013 23:41:21 -0800 Subject: [PATCH 22/23] Fix a typo that caused PWM_SERVO_GET ioctls to fail on the FMU PWM outputs. --- apps/drivers/px4fmu/fmu.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/apps/drivers/px4fmu/fmu.cpp b/apps/drivers/px4fmu/fmu.cpp index a995f6214d..2e3b130a9c 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; } From 76277ec622742388aade35ef9d439d42ea6caad7 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 5 Jan 2013 22:37:26 -0800 Subject: [PATCH 23/23] Ignore more. --- .gitignore | 2 ++ 1 file changed, 2 insertions(+) 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