From 5b92c517779500d79e6e5f5cff48336550ce5edb Mon Sep 17 00:00:00 2001 From: px4dev Date: Thu, 20 Dec 2012 21:31:02 -0800 Subject: [PATCH 01/32] 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/32] 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/32] 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/32] 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/32] 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/32] 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/32] 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/32] 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/32] 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/32] 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/32] 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/32] 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/32] 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/32] 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/32] 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/32] 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/32] 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/32] 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/32] 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 0cc723dbc3511b8b19dfe9f2d3918c95c3b3d12e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 31 Dec 2012 09:21:44 +0100 Subject: [PATCH 20/32] Fixed connection loss / failsafe detection, added decoding of two switch channels if IO supports 18 RC channels. Loss is tested, switch channels are not. --- apps/px4io/sbus.c | 25 ++++++++++++++++--------- 1 file changed, 16 insertions(+), 9 deletions(-) diff --git a/apps/px4io/sbus.c b/apps/px4io/sbus.c index a8f628a84e..f84e5df8ac 100644 --- a/apps/px4io/sbus.c +++ b/apps/px4io/sbus.c @@ -53,7 +53,7 @@ #include "debug.h" #define SBUS_FRAME_SIZE 25 -#define SBUS_INPUT_CHANNELS 16 +#define SBUS_INPUT_CHANNELS 18 static int sbus_fd = -1; @@ -87,9 +87,9 @@ sbus_init(const char *device) partial_frame_count = 0; last_rx_time = hrt_absolute_time(); - debug("Sbus: ready"); + debug("S.Bus: ready"); } else { - debug("Sbus: open failed"); + debug("S.Bus: open failed"); } return sbus_fd; @@ -205,9 +205,13 @@ sbus_decode(hrt_abstime frame_time) return; } - /* if the failsafe bit is set, we consider the frame invalid */ - if (frame[23] & (1 << 4)) { - return; + /* if the failsafe or connection lost bit is set, we consider the frame invalid */ + if ((frame[23] & (1 << 2)) && /* signal lost */ + (frame[23] & (1 << 3))) { /* failsafe */ + + /* actively announce signal loss */ + system_state.rc_channels = 0; + return 1; } /* we have received something we think is a frame */ @@ -236,9 +240,12 @@ sbus_decode(hrt_abstime frame_time) system_state.rc_channel_data[channel] = (value / 2) + 998; } - if (PX4IO_INPUT_CHANNELS >= 18) { - chancount = 18; - /* XXX decode the two switch channels */ + /* decode switch channels if data fields are wide enough */ + if (chancount > 17) { + /* channel 17 (index 16) */ + system_state.rc_channel_data[16] = (frame[23] & (1 << 0)) * 1000 + 998; + /* channel 18 (index 17) */ + system_state.rc_channel_data[17] = (frame[23] & (1 << 1)) * 1000 + 998; } /* note the number of channels decoded */ From 63464a8959d90bd92c4998a80992c416b85c2057 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 2 Jan 2013 17:37:55 +1100 Subject: [PATCH 21/32] mavlink: fixed build warning --- apps/mavlink/mavlink_log.c | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/apps/mavlink/mavlink_log.c b/apps/mavlink/mavlink_log.c index 660e282f86..73d59e76fc 100644 --- a/apps/mavlink/mavlink_log.c +++ b/apps/mavlink/mavlink_log.c @@ -40,6 +40,7 @@ */ #include +#include #include "mavlink_log.h" @@ -51,7 +52,7 @@ void mavlink_logbuffer_init(struct mavlink_logbuffer *lb, int size) { } int mavlink_logbuffer_is_full(struct mavlink_logbuffer *lb) { - return lb->count == lb->size; + return lb->count == (int)lb->size; } int mavlink_logbuffer_is_empty(struct mavlink_logbuffer *lb) { @@ -77,4 +78,4 @@ int mavlink_logbuffer_read(struct mavlink_logbuffer *lb, struct mavlink_logmessa } else { return 1; } -} \ No newline at end of file +} From 0fff8aa23b477bebda9c1a79f5cd0b5eceeb95d1 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 2 Jan 2013 17:38:28 +1100 Subject: [PATCH 22/32] GPS: added comment on units for variance --- apps/uORB/topics/vehicle_gps_position.h | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/apps/uORB/topics/vehicle_gps_position.h b/apps/uORB/topics/vehicle_gps_position.h index ebd83e1a9b..db529da06d 100644 --- a/apps/uORB/topics/vehicle_gps_position.h +++ b/apps/uORB/topics/vehicle_gps_position.h @@ -65,8 +65,8 @@ struct vehicle_gps_position_s uint16_t counter_pos_valid; /**< is only increased when new lat/lon/alt information was added */ uint16_t eph; /**< GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 //LOGME */ uint16_t epv; /**< GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 */ - float s_variance; // XXX testing - float p_variance; // XXX testing + float s_variance; /**< speed accuracy estimate cm/s */ + float p_variance; /**< position accuracy estimate cm */ uint16_t vel; /**< GPS ground speed (m/s * 100). If unknown, set to: 65535 */ float vel_n; /**< GPS ground speed in m/s */ float vel_e; /**< GPS ground speed in m/s */ @@ -84,7 +84,6 @@ struct vehicle_gps_position_s /* flags */ float vel_ned_valid; /**< Flag to indicate if NED speed is valid */ - }; /** From 890b1ac0689984cb6bc4638cc2aa8ec869d14f91 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 2 Jan 2013 17:38:52 +1100 Subject: [PATCH 23/32] build: cope better with absolute paths --- nuttx/tools/mkdeps.sh | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/nuttx/tools/mkdeps.sh b/nuttx/tools/mkdeps.sh index acb6001509..d8984e5536 100755 --- a/nuttx/tools/mkdeps.sh +++ b/nuttx/tools/mkdeps.sh @@ -86,8 +86,12 @@ dodep () fi done if [ -z "$fullpath" ]; then - echo "# ERROR: No readable file for $1 found at any location" - show_usage + if [ -r $1 ]; then + fullpath=$1 + else + echo "# ERROR: No readable file for $1 found at any location" + show_usage + fi fi fi From 8ffba227718f6f7897abb5a1fd77f770f1f3db1f Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 2 Jan 2013 17:39:12 +1100 Subject: [PATCH 24/32] build: allow for external apps via EXTERNAL_APPS this is used by the APM build --- apps/Makefile | 3 +++ 1 file changed, 3 insertions(+) diff --git a/apps/Makefile b/apps/Makefile index 19ad1c18bc..11d95bc190 100644 --- a/apps/Makefile +++ b/apps/Makefile @@ -112,6 +112,9 @@ endef $(foreach BUILTIN, $(CONFIGURED_APPS), $(eval $(call ADD_BUILTIN,$(BUILTIN)))) +# EXTERNAL_APPS is used to add out of tree apps to the build +INSTALLED_APPS += $(EXTERNAL_APPS) + # The external/ directory may also be added to the INSTALLED_APPS. But there # is no external/ directory in the repository. Rather, this directory may be # provided by the user (possibly as a symbolic link) to add libraries and From 3916230d8fc7185a7eefcc1640a68bacf3eac72b Mon Sep 17 00:00:00 2001 From: px4dev Date: Thu, 3 Jan 2013 00:33:22 -0800 Subject: [PATCH 25/32] 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 73b787a8ddbc0e4d3cb1a5b92b7c6604b5dd6db1 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 3 Jan 2013 20:42:36 +1100 Subject: [PATCH 26/32] serial: fixed up FIONREAD and FIONWRITE the device ioctl returns -ENOTTY when it hasn't handled the command --- nuttx/drivers/serial/serial.c | 16 ++++++++++++---- 1 file changed, 12 insertions(+), 4 deletions(-) diff --git a/nuttx/drivers/serial/serial.c b/nuttx/drivers/serial/serial.c index 24744524fa..71937da743 100644 --- a/nuttx/drivers/serial/serial.c +++ b/nuttx/drivers/serial/serial.c @@ -660,9 +660,11 @@ 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 (ret == OK) + /* + the device ioctl() handler returns -ENOTTY when it doesn't know + how to handle the command. Check if we can handle it here. + */ + if (ret == -ENOTTY) { switch (cmd) { @@ -686,7 +688,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 +699,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,7 +711,9 @@ static int uart_ioctl(FAR struct file *filep, int cmd, unsigned long arg) irqrestore(state); *(int *)arg = count; + ret = 0; } + break; #ifdef CONFIG_SERIAL_TERMIOS case TCGETS: @@ -725,6 +731,7 @@ static int uart_ioctl(FAR struct file *filep, int cmd, unsigned long arg) termiosp->c_iflag = dev->tc_iflag; termiosp->c_oflag = dev->tc_oflag; termiosp->c_lflag = dev->tc_lflag; + ret = 0; } break; @@ -744,6 +751,7 @@ static int uart_ioctl(FAR struct file *filep, int cmd, unsigned long arg) dev->tc_iflag = termiosp->c_iflag; dev->tc_oflag = termiosp->c_oflag; dev->tc_lflag = termiosp->c_lflag; + ret = 0; } break; From 920608bf3671662f94b4939ffdaf6fde2b08edff Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 5 Jan 2013 07:58:50 +1100 Subject: [PATCH 27/32] added vdprintf() library function --- nuttx/include/stdio.h | 1 + nuttx/lib/lib_internal.h | 1 + nuttx/lib/stdio/Make.defs | 2 +- nuttx/lib/stdio/lib_rawprintf.c | 15 ++++++ nuttx/lib/stdio/lib_vdprintf.c | 81 +++++++++++++++++++++++++++++++++ 5 files changed, 99 insertions(+), 1 deletion(-) create mode 100644 nuttx/lib/stdio/lib_vdprintf.c diff --git a/nuttx/include/stdio.h b/nuttx/include/stdio.h index e9218046c5..754cedbb40 100644 --- a/nuttx/include/stdio.h +++ b/nuttx/include/stdio.h @@ -133,6 +133,7 @@ EXTERN void perror(FAR const char *s); EXTERN int ungetc(int c, FAR FILE *stream); EXTERN int vprintf(FAR const char *format, va_list ap); EXTERN int vfprintf(FAR FILE *stream, const char *format, va_list ap); +EXTERN int vdprintf(FAR int fd, const char *format, va_list ap); EXTERN int vsprintf(FAR char *buf, const char *format, va_list ap); EXTERN int avsprintf(FAR char **ptr, const char *fmt, va_list ap); EXTERN int vsnprintf(FAR char *buf, size_t size, const char *format, va_list ap); diff --git a/nuttx/lib/lib_internal.h b/nuttx/lib/lib_internal.h index c3d9bfd18e..49cba9996f 100644 --- a/nuttx/lib/lib_internal.h +++ b/nuttx/lib/lib_internal.h @@ -140,6 +140,7 @@ extern int lib_vsprintf(FAR struct lib_outstream_s *obj, /* Defined lib_rawprintf.c */ extern int lib_rawvprintf(const char *src, va_list ap); +extern int lib_rawvdprintf(int fd, const char *fmt, va_list ap); /* Defined lib_lowprintf.c */ diff --git a/nuttx/lib/stdio/Make.defs b/nuttx/lib/stdio/Make.defs index f88a5edd9e..a4e9007052 100644 --- a/nuttx/lib/stdio/Make.defs +++ b/nuttx/lib/stdio/Make.defs @@ -50,7 +50,7 @@ CSRCS += lib_fopen.c lib_fclose.c lib_fread.c lib_libfread.c lib_fseek.c \ lib_gets.c lib_fwrite.c lib_libfwrite.c lib_fflush.c \ lib_libflushall.c lib_libfflush.c lib_rdflush.c lib_wrflush.c \ lib_fputc.c lib_puts.c lib_fputs.c lib_ungetc.c lib_vprintf.c \ - lib_fprintf.c lib_vfprintf.c lib_stdinstream.c lib_stdoutstream.c \ + lib_fprintf.c lib_vfprintf.c lib_vdprintf.c lib_stdinstream.c lib_stdoutstream.c \ lib_perror.c endif endif diff --git a/nuttx/lib/stdio/lib_rawprintf.c b/nuttx/lib/stdio/lib_rawprintf.c index 19dfa895e1..1a6eb16b7c 100644 --- a/nuttx/lib/stdio/lib_rawprintf.c +++ b/nuttx/lib/stdio/lib_rawprintf.c @@ -149,3 +149,18 @@ int lib_rawprintf(const char *fmt, ...) return ret; } + + +/**************************************************************************** + * Name: lib_rawvdprintf + ****************************************************************************/ + +int lib_rawvdprintf(int fd, const char *fmt, va_list ap) +{ + /* Wrap the stdout in a stream object and let lib_vsprintf + * do the work. + */ + struct lib_rawoutstream_s rawoutstream; + lib_rawoutstream(&rawoutstream, fd); + return lib_vsprintf(&rawoutstream.public, fmt, ap); +} diff --git a/nuttx/lib/stdio/lib_vdprintf.c b/nuttx/lib/stdio/lib_vdprintf.c new file mode 100644 index 0000000000..c2b5761107 --- /dev/null +++ b/nuttx/lib/stdio/lib_vdprintf.c @@ -0,0 +1,81 @@ +/**************************************************************************** + * lib/stdio/lib_vdprintf.c + * + * Copyright (C) 2007-2009, 2011 Andrew Tridgell. All rights reserved. + * Author: Andrew Tridgell + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include + +#include "lib_internal.h" + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Type Declarations + ****************************************************************************/ + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +/**************************************************************************** + * Global Constant Data + ****************************************************************************/ + +/**************************************************************************** + * Global Variables + ****************************************************************************/ + +/**************************************************************************** + * Private Constant Data + ****************************************************************************/ + +/**************************************************************************** + * Private Variables + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +int vdprintf(int fd, FAR const char *fmt, va_list ap) +{ + return lib_rawvdprintf(fd, fmt, ap); +} From 219ab05a707d8f2795dc91f5884a42f20dd6a640 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 6 Jan 2013 06:29:28 +1100 Subject: [PATCH 28/32] added PWM_SERVO_SET_UPDATE_RATE ioctl this allows apps to set the PWM update rate. APM needs this to honor the user configurable PWM rate setting for copters. --- apps/drivers/drv_pwm_output.h | 3 +++ apps/drivers/hil/hil.cpp | 4 ++++ apps/drivers/px4fmu/fmu.cpp | 4 ++++ 3 files changed, 11 insertions(+) diff --git a/apps/drivers/drv_pwm_output.h b/apps/drivers/drv_pwm_output.h index b2fee65aca..c110cd5cbc 100644 --- a/apps/drivers/drv_pwm_output.h +++ b/apps/drivers/drv_pwm_output.h @@ -103,6 +103,9 @@ ORB_DECLARE(output_pwm); /** disarm all servo outputs (stop generating pulses) */ #define PWM_SERVO_DISARM _IOC(_PWM_SERVO_BASE, 1) +/** set update rate in Hz */ +#define PWM_SERVO_SET_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 2) + /** set a single servo to a specific value */ #define PWM_SERVO_SET(_servo) _IOC(_PWM_SERVO_BASE, 0x20 + _servo) diff --git a/apps/drivers/hil/hil.cpp b/apps/drivers/hil/hil.cpp index 67b16aa42f..32bdb0776a 100644 --- a/apps/drivers/hil/hil.cpp +++ b/apps/drivers/hil/hil.cpp @@ -503,6 +503,10 @@ HIL::pwm_ioctl(file *filp, int cmd, unsigned long arg) // up_pwm_servo_arm(false); break; + case PWM_SERVO_SET_UPDATE_RATE: + g_hil->set_pwm_rate(arg); + break; + case PWM_SERVO_SET(2): case PWM_SERVO_SET(3): if (_mode != MODE_4PWM) { diff --git a/apps/drivers/px4fmu/fmu.cpp b/apps/drivers/px4fmu/fmu.cpp index a995f6214d..d5f4123c2e 100644 --- a/apps/drivers/px4fmu/fmu.cpp +++ b/apps/drivers/px4fmu/fmu.cpp @@ -470,6 +470,10 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) up_pwm_servo_arm(false); break; + case PWM_SERVO_SET_UPDATE_RATE: + set_pwm_rate(arg); + break; + case PWM_SERVO_SET(2): case PWM_SERVO_SET(3): if (_mode != MODE_4PWM) { From 0134186420273170c14b80b8114ccd2474da1792 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 6 Jan 2013 06:31:00 +1100 Subject: [PATCH 29/32] fixup rate --- apps/drivers/px4io/px4io.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index 9f3dba047f..d313659bcb 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -554,6 +554,11 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg) _send_needed = true; break; + case PWM_SERVO_SET_UPDATE_RATE: + // not supported yet + ret = -EINVAL; + break; + case PWM_SERVO_SET(0) ... PWM_SERVO_SET(_max_actuators - 1): /* fake an update to the selected servo channel */ From 318609bf59083e4247dfd179a3bb36dc2eb9fb45 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 6 Jan 2013 06:31:37 +1100 Subject: [PATCH 30/32] fixed PWM_SERVO_GET this was using the wrong channel --- 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 d5f4123c2e..7e78cb979d 100644 --- a/apps/drivers/px4fmu/fmu.cpp +++ b/apps/drivers/px4fmu/fmu.cpp @@ -504,7 +504,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 375d3c14d742248b434c080527886a95ea1d563f Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 6 Jan 2013 06:33:44 +1100 Subject: [PATCH 31/32] increase the UART buffer sizes to 256 The most critical one is the GPS serial port receive buffer size, which needs to be at least 128 to support the UBLOX protocol, but it seems a good idea for people running a FMU without a IO board to increase the UART buffer sizes generally --- nuttx/configs/px4fmu/nsh/defconfig | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/nuttx/configs/px4fmu/nsh/defconfig b/nuttx/configs/px4fmu/nsh/defconfig index bc724c0063..7ff9eaa8c8 100755 --- a/nuttx/configs/px4fmu/nsh/defconfig +++ b/nuttx/configs/px4fmu/nsh/defconfig @@ -245,18 +245,18 @@ CONFIG_USART6_SERIAL_CONSOLE=n #Mavlink messages can be bigger than 128 CONFIG_USART1_TXBUFSIZE=512 -CONFIG_USART2_TXBUFSIZE=128 -CONFIG_USART3_TXBUFSIZE=128 -CONFIG_UART4_TXBUFSIZE=128 -CONFIG_UART5_TXBUFSIZE=64 +CONFIG_USART2_TXBUFSIZE=256 +CONFIG_USART3_TXBUFSIZE=256 +CONFIG_UART4_TXBUFSIZE=256 +CONFIG_UART5_TXBUFSIZE=256 CONFIG_USART6_TXBUFSIZE=128 CONFIG_USART1_RXBUFSIZE=512 -CONFIG_USART2_RXBUFSIZE=128 -CONFIG_USART3_RXBUFSIZE=128 -CONFIG_UART4_RXBUFSIZE=128 -CONFIG_UART5_RXBUFSIZE=128 -CONFIG_USART6_RXBUFSIZE=128 +CONFIG_USART2_RXBUFSIZE=256 +CONFIG_USART3_RXBUFSIZE=256 +CONFIG_UART4_RXBUFSIZE=256 +CONFIG_UART5_RXBUFSIZE=256 +CONFIG_USART6_RXBUFSIZE=256 CONFIG_USART1_BAUD=57600 CONFIG_USART2_BAUD=115200 From 76277ec622742388aade35ef9d439d42ea6caad7 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 5 Jan 2013 22:37:26 -0800 Subject: [PATCH 32/32] 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