From c3038619a420359bd00e513a63fa15a135209872 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Thu, 6 Dec 2012 22:35:20 +0100 Subject: [PATCH 01/50] Updating the PX4IO rc script to the latest state. --- ROMFS/scripts/rc.PX4IO | 45 +++++++++++++++--------------------------- 1 file changed, 16 insertions(+), 29 deletions(-) diff --git a/ROMFS/scripts/rc.PX4IO b/ROMFS/scripts/rc.PX4IO index b5a0874335..3a5ccbc5c6 100644 --- a/ROMFS/scripts/rc.PX4IO +++ b/ROMFS/scripts/rc.PX4IO @@ -1,12 +1,9 @@ #!nsh -# -# Flight startup script for PX4FMU with PX4IO carrier board. -# -echo "[init] doing PX4IO startup..." +set USB no # -# Start the ORB +# Start the object request broker # uorb start @@ -20,15 +17,21 @@ then param load fi +# +# Enable / connect to PX4IO +# +px4io start + # # Start the sensors. # sh /etc/init.d/rc.sensors # -# Start MAVLink +# Start MAVLink on UART1 (dev/ttyS0) at 57600 baud (CLI is now unusable) # mavlink start -d /dev/ttyS0 -b 57600 +usleep 5000 # # Start the commander. @@ -41,35 +44,19 @@ commander start attitude_estimator_ekf start # -# Configure PX4FMU for operation with PX4IO +# Start the attitude and position controller # -# XXX arguments? -# -px4fmu start +fixedwing_att_control start +fixedwing_pos_control start # -# Start the fixed-wing controller +# Start GPS capture # -fixedwing_control start - -# -# Fire up the PX4IO interface. -# -px4io start - -# -# Start looking for a GPS. -# -gps start - -# -# Start logging to microSD if we can -# -sh /etc/init.d/rc.logging +#gps start # # startup is done; we don't want the shell because we -# use the same UART for telemetry (dumb). +# use the same UART for telemetry # -echo "[init] startup done, exiting." +echo "[init] startup done" exit From b7b79d825423dcdf84f684ebb4de879a36d86dc9 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Thu, 6 Dec 2012 22:37:28 +0100 Subject: [PATCH 02/50] Add logging back in. --- ROMFS/scripts/rc.PX4IO | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/ROMFS/scripts/rc.PX4IO b/ROMFS/scripts/rc.PX4IO index 3a5ccbc5c6..9a7509b1e6 100644 --- a/ROMFS/scripts/rc.PX4IO +++ b/ROMFS/scripts/rc.PX4IO @@ -54,6 +54,11 @@ fixedwing_pos_control start # #gps start +# +# Start logging to microSD if we can +# +sh /etc/init.d/rc.logging + # # startup is done; we don't want the shell because we # use the same UART for telemetry From 096305f6259abd612e571c713f0ef0bc24f2b38d Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Mon, 10 Dec 2012 09:05:26 +0100 Subject: [PATCH 03/50] A mixer is required. --- ROMFS/scripts/rc.PX4IO | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/ROMFS/scripts/rc.PX4IO b/ROMFS/scripts/rc.PX4IO index 9a7509b1e6..3e91f7ef6e 100644 --- a/ROMFS/scripts/rc.PX4IO +++ b/ROMFS/scripts/rc.PX4IO @@ -22,6 +22,12 @@ fi # px4io start +# +# Load an appropriate mixer. FMU_pass.mix is a passthru mixer. +# See ROMFS/mixers for a full list of mixers. +# +mixer load /dev/pwm_output /etc/mixers/FMU_pass.mix + # # Start the sensors. # From 06126d0d9e2a5d85363ebe3136d35b164f1fd966 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Thu, 13 Dec 2012 07:09:11 +0100 Subject: [PATCH 04/50] Starting GPS should probably be on by default. --- ROMFS/scripts/rc.PX4IO | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/ROMFS/scripts/rc.PX4IO b/ROMFS/scripts/rc.PX4IO index 3e91f7ef6e..84e181a5ae 100644 --- a/ROMFS/scripts/rc.PX4IO +++ b/ROMFS/scripts/rc.PX4IO @@ -23,8 +23,8 @@ fi px4io start # -# Load an appropriate mixer. FMU_pass.mix is a passthru mixer. -# See ROMFS/mixers for a full list of mixers. +# Load an appropriate mixer. FMU_pass.mix is a passthru mixer +# which is good for testing. See ROMFS/mixers for a full list of mixers. # mixer load /dev/pwm_output /etc/mixers/FMU_pass.mix @@ -56,9 +56,9 @@ fixedwing_att_control start fixedwing_pos_control start # -# Start GPS capture +# Start GPS capture. Comment this out if you do not have a GPS. # -#gps start +gps start # # Start logging to microSD if we can From 5b92c517779500d79e6e5f5cff48336550ce5edb Mon Sep 17 00:00:00 2001 From: px4dev Date: Thu, 20 Dec 2012 21:31:02 -0800 Subject: [PATCH 05/50] 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 06/50] 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 07/50] 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 08/50] 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 09/50] 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 10/50] 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 11/50] 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 12/50] 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 13/50] 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 14/50] 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 15/50] 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 16/50] 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 17/50] 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 18/50] 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 19/50] 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 20/50] 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 21/50] 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 22/50] 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 23/50] 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 24/50] 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 476db468697089895c15ab151e58b71c5b3a2c95 Mon Sep 17 00:00:00 2001 From: px4dev Date: Mon, 31 Dec 2012 01:26:58 -0800 Subject: [PATCH 25/50] kill off the NuttX ADC driver config; we're not going to use it --- nuttx/configs/px4fmu/nsh/appconfig | 1 + nuttx/configs/px4fmu/nsh/defconfig | 26 +++----------------------- nuttx/configs/px4io/io/defconfig | 1 + 3 files changed, 5 insertions(+), 23 deletions(-) diff --git a/nuttx/configs/px4fmu/nsh/appconfig b/nuttx/configs/px4fmu/nsh/appconfig index 0959687ded..7f1a93df92 100644 --- a/nuttx/configs/px4fmu/nsh/appconfig +++ b/nuttx/configs/px4fmu/nsh/appconfig @@ -100,6 +100,7 @@ CONFIGURED_APPS += drivers/stm32 CONFIGURED_APPS += drivers/led CONFIGURED_APPS += drivers/blinkm CONFIGURED_APPS += drivers/stm32/tone_alarm +CONFIGURED_APPS += drivers/stm32/adc CONFIGURED_APPS += drivers/px4fmu CONFIGURED_APPS += drivers/hil diff --git a/nuttx/configs/px4fmu/nsh/defconfig b/nuttx/configs/px4fmu/nsh/defconfig index bc724c0063..d9a48f2c86 100755 --- a/nuttx/configs/px4fmu/nsh/defconfig +++ b/nuttx/configs/px4fmu/nsh/defconfig @@ -189,9 +189,10 @@ CONFIG_STM32_TIM1=n CONFIG_STM32_TIM8=n CONFIG_STM32_USART1=y CONFIG_STM32_USART6=y -CONFIG_STM32_ADC1=n +# We use our own driver, but leave this on. +CONFIG_STM32_ADC1=y CONFIG_STM32_ADC2=n -CONFIG_STM32_ADC3=y +CONFIG_STM32_ADC3=n CONFIG_STM32_SDIO=n CONFIG_STM32_SPI1=y CONFIG_STM32_SYSCFG=y @@ -358,27 +359,6 @@ CONFIG_STM32_I2CTIMEOUS_START_STOP=700 # XXX this is bad and we want it gone CONFIG_I2C_WRITEREAD=y -# -# ADC configuration -# -# Enable ADC driver support. -# -# CONFIG_ADC=y : Enable the generic ADC infrastructure -# CONFIG_STM32_TIM1_ADC=y : Indicate that timer 1 will be used to trigger an ADC -# CONFIG_STM32_TIM1_ADC3=y : Assign timer 1 to drive ADC3 sampling -# CONFIG_STM32_ADC3_SAMPLE_FREQUENCY=100 : Select a sampling frequency -# -CONFIG_ADC=y -CONFIG_STM32_TIM4_ADC3=y -# select sample frequency 1^=1.5Msamples/second -# 65535^=10samples/second 16bit-timer runs at 84/128 MHz -CONFIG_STM32_ADC3_SAMPLE_FREQUENCY=6000 -# select timer channel 0=CC1,...,3=CC4 -CONFIG_STM32_ADC3_TIMTRIG=3 -CONFIG_ADC_DMA=y -# only 4 places usable! -CONFIG_ADC_FIFOSIZE=5 - # # General build options # diff --git a/nuttx/configs/px4io/io/defconfig b/nuttx/configs/px4io/io/defconfig index 1ac70f8abf..043754e29b 100755 --- a/nuttx/configs/px4io/io/defconfig +++ b/nuttx/configs/px4io/io/defconfig @@ -131,6 +131,7 @@ CONFIG_STM32_BKP=n CONFIG_STM32_PWR=n CONFIG_STM32_DAC=n # APB2: +# We use our own ADC driver, but leave this on for clocking purposes. CONFIG_STM32_ADC1=y CONFIG_STM32_ADC2=n # TIM1 is owned by the HRT From 22f5a1dc9423d04e70c109f74b1948536070598f Mon Sep 17 00:00:00 2001 From: px4dev Date: Mon, 31 Dec 2012 01:30:57 -0800 Subject: [PATCH 26/50] =?UTF-8?q?First=20cut=20at=20a=20simple=C2=AE=20ADC?= =?UTF-8?q?=20driver=20built=20on=20our=20driver=20framework.?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- apps/drivers/boards/px4fmu/px4fmu_adc.c | 139 ---------- apps/drivers/boards/px4fmu/px4fmu_init.c | 23 +- apps/drivers/drv_adc.h | 54 ++++ apps/drivers/stm32/adc/Makefile | 43 ++++ apps/drivers/stm32/adc/adc.cpp | 310 +++++++++++++++++++++++ 5 files changed, 412 insertions(+), 157 deletions(-) delete mode 100644 apps/drivers/boards/px4fmu/px4fmu_adc.c create mode 100644 apps/drivers/drv_adc.h create mode 100644 apps/drivers/stm32/adc/Makefile create mode 100644 apps/drivers/stm32/adc/adc.cpp diff --git a/apps/drivers/boards/px4fmu/px4fmu_adc.c b/apps/drivers/boards/px4fmu/px4fmu_adc.c deleted file mode 100644 index 9878941631..0000000000 --- a/apps/drivers/boards/px4fmu/px4fmu_adc.c +++ /dev/null @@ -1,139 +0,0 @@ -/**************************************************************************** - * - * 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 px4fmu_adc.c - * - * Board-specific ADC functions. - */ - -/************************************************************************************ - * Included Files - ************************************************************************************/ - -#include - -#include -#include -#include - -#include -#include - -#include "chip.h" -#include "up_arch.h" - -#include "stm32_adc.h" -#include "px4fmu_internal.h" - -#define ADC3_NCHANNELS 4 - -/************************************************************************************ - * Private Data - ************************************************************************************/ -/* The PX4FMU board has four ADC channels: ADC323 IN10-13 - */ - -/* Identifying number of each ADC channel: Variable Resistor. */ - -#ifdef CONFIG_STM32_ADC3 -static const uint8_t g_chanlist[ADC3_NCHANNELS] = {10, 11};// , 12, 13}; ADC12 and 13 are used by MPU on v1.5 boards - -/* Configurations of pins used byte each ADC channels */ -static const uint32_t g_pinlist[ADC3_NCHANNELS] = {GPIO_ADC3_IN10, GPIO_ADC3_IN11}; // ADC12 and 13 are used by MPU on v1.5 boards, GPIO_ADC3_IN12, GPIO_ADC3_IN13}; -#endif - -/************************************************************************************ - * Private Functions - ************************************************************************************/ - -/************************************************************************************ - * Public Functions - ************************************************************************************/ - -/************************************************************************************ - * Name: adc_devinit - * - * Description: - * All STM32 architectures must provide the following interface to work with - * examples/adc. - * - ************************************************************************************/ - -int adc_devinit(void) -{ - static bool initialized = false; - struct adc_dev_s *adc[ADC3_NCHANNELS]; - int ret; - int i; - - /* Check if we have already initialized */ - - if (!initialized) { - char name[11]; - - for (i = 0; i < ADC3_NCHANNELS; i++) { - stm32_configgpio(g_pinlist[i]); - } - - for (i = 0; i < 1; i++) { - /* Configure the pins as analog inputs for the selected channels */ - //stm32_configgpio(g_pinlist[i]); - - /* Call stm32_adcinitialize() to get an instance of the ADC interface */ - //multiple channels only supported with dma! - adc[i] = stm32_adcinitialize(3, (g_chanlist), 4); - - if (adc == NULL) { - adbg("ERROR: Failed to get ADC interface\n"); - return -ENODEV; - } - - - /* Register the ADC driver at "/dev/adc0" */ - sprintf(name, "/dev/adc%d", i); - ret = adc_register(name, adc[i]); - - if (ret < 0) { - adbg("adc_register failed for adc %s: %d\n", name, ret); - return ret; - } - } - - /* Now we are initialized */ - - initialized = true; - } - - return OK; -} diff --git a/apps/drivers/boards/px4fmu/px4fmu_init.c b/apps/drivers/boards/px4fmu/px4fmu_init.c index 57ffb77d34..4ebc5a439e 100644 --- a/apps/drivers/boards/px4fmu/px4fmu_init.c +++ b/apps/drivers/boards/px4fmu/px4fmu_init.c @@ -209,8 +209,7 @@ __EXPORT int nsh_archinitialize(void) message("[boot] Successfully initialized SPI port 1\r\n"); -#if defined(CONFIG_STM32_SPI3) - /* Get the SPI port */ + /* Get the SPI port for the microSD slot */ message("[boot] Initializing SPI port 3\n"); spi3 = up_spiinitialize(3); @@ -233,23 +232,11 @@ __EXPORT int nsh_archinitialize(void) } message("[boot] Successfully bound SPI port 3 to the MMCSD driver\n"); -#endif /* SPI3 */ -#ifdef CONFIG_ADC - int adc_state = adc_devinit(); - - if (adc_state != OK) { - /* Try again */ - adc_state = adc_devinit(); - - if (adc_state != OK) { - /* Give up */ - message("[boot] FAILED adc_devinit: %d\n", adc_state); - return -ENODEV; - } - } - -#endif + stm32_configgpio(GPIO_ADC1_IN10); + stm32_configgpio(GPIO_ADC1_IN11); + //stm32_configgpio(GPIO_ADC1_IN12); // XXX is this available? + //stm32_configgpio(GPIO_ADC1_IN13); // jumperable to MPU6000 DRDY on some boards return OK; } diff --git a/apps/drivers/drv_adc.h b/apps/drivers/drv_adc.h new file mode 100644 index 0000000000..f42350cbb5 --- /dev/null +++ b/apps/drivers/drv_adc.h @@ -0,0 +1,54 @@ +/**************************************************************************** + * + * 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 drv_adc.h + * + * ADC driver interface. + * + * This defines additional operations over and above the standard NuttX + * ADC API. + */ + +#pragma once + +#include +#include + +#include + +#define ADC_DEVICE_PATH "/dev/adc0" + +/* + * ioctl definitions + */ diff --git a/apps/drivers/stm32/adc/Makefile b/apps/drivers/stm32/adc/Makefile new file mode 100644 index 0000000000..443bc06239 --- /dev/null +++ b/apps/drivers/stm32/adc/Makefile @@ -0,0 +1,43 @@ +############################################################################ +# +# 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. +# +############################################################################ + +# +# STM32 ADC driver +# + +APPNAME = adc +PRIORITY = SCHED_PRIORITY_DEFAULT +STACKSIZE = 2048 +INCLUDES = $(TOPDIR)/arch/arm/src/stm32 $(TOPDIR)/arch/arm/src/common + +include $(APPDIR)/mk/app.mk diff --git a/apps/drivers/stm32/adc/adc.cpp b/apps/drivers/stm32/adc/adc.cpp new file mode 100644 index 0000000000..f77499706e --- /dev/null +++ b/apps/drivers/stm32/adc/adc.cpp @@ -0,0 +1,310 @@ +/**************************************************************************** + * + * 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 adc.cpp + * + * Driver for the STM32 ADC. + * + * This is a low-rate driver, designed for sampling things like voltages + * and so forth. It avoids the gross complexity of the NuttX ADC driver. + */ + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include + +#include + +#define ADC_BASE STM32_ADC1_BASE + +/* + * Register accessors. + */ +#define REG(_reg) (*(volatile uint32_t *)(_base + _reg)) + +#define rSR REG(STM32_ADC_SR_OFFSET) +#define rCR1 REG(STM32_ADC_CR1_OFFSET) +#define rCR2 REG(STM32_ADC_CR2_OFFSET) +#define rSMPR1 REG(STM32_ADC_SMPR1_OFFSET) +#define rSMPR2 REG(STM32_ADC_SMPR2_OFFSET) +#define rJOFR1 REG(STM32_ADC_JOFR1_OFFSET) +#define rJOFR2 REG(STM32_ADC_JOFR2_OFFSET) +#define rJOFR3 REG(STM32_ADC_JOFR3_OFFSET) +#define rJOFR4 REG(STM32_ADC_JOFR4_OFFSET) +#define rHTR REG(STM32_ADC_HTR_OFFSET) +#define rLTR REG(STM32_ADC_LTR_OFFSET) +#define rSQR1 REG(STM32_ADC_SQR1_OFFSET) +#define rSQR2 REG(STM32_ADC_SQR2_OFFSET) +#define rSQR3 REG(STM32_ADC_SQR3_OFFSET) +#define rJSQR REG(STM32_ADC_JSQR_OFFSET) +#define rJDR1 REG(STM32_ADC_JDR1_OFFSET) +#define rJDR2 REG(STM32_ADC_JDR2_OFFSET) +#define rJDR3 REG(STM32_ADC_JDR3_OFFSET) +#define rJDR4 REG(STM32_ADC_JDR4_OFFSET) +#define rDR REG(STM32_ADC_DR_OFFSET) + +class ADC : public device::CDev +{ +public: + ADC(); + ~ADC(); + + virtual int init(); + + virtual int ioctl(file *filp, int cmd, unsigned long arg); + virtual ssize_t read(file *filp, char *buffer, size_t len); + +protected: + virtual int open_first(struct file *filp); + virtual int close_last(struct file *filp); + +private: + static const hrt_abstime _tickrate = 10000; /* 100Hz base rate */ + static const uint32_t _base = ADC_BASE; + + hrt_call _call; + + static void _tick_trampoline(void *arg); + void _tick(); + + uint16_t _sample(unsigned channel); + + uint16_t _result; +}; + +ADC::ADC() : + CDev("adc", ADC_DEVICE_PATH) +{ + _debug_enabled = true; +} + +ADC::~ADC() +{ +} + +int +ADC::init() +{ + /* do calibration if supported */ +#ifdef ADC_CR2_CAL + rCR2 |= ADC_CR2_CAL; + usleep(100); + if (rCR2 & ADC_CR2_CAL) + return -1; +#endif + + /* arbitrarily configure all channels for 55 cycle sample time */ + rSMPR1 = 0b00000011011011011011011011011011; + rSMPR2 = 0b00011011011011011011011011011011; + + /* XXX for F2/4, might want to select 12-bit mode? */ + rCR1 = 0; + + /* enable the temperature sensor / Vrefint channel if supported*/ + rCR2 = +#ifdef ADC_CR2_TSVREFE + ADC_CR2_TSVREFE | +#endif + 0; + + /* configure for a single-channel sequence */ + rSQR1 = 0; + rSQR2 = 0; + rSQR3 = 11; /* will be updated with the channel each tick */ + + /* power-cycle the ADC and turn it on */ + rCR2 &= ~ADC_CR2_ADON; + usleep(10); + rCR2 |= ADC_CR2_ADON; + usleep(10); + rCR2 |= ADC_CR2_ADON; + usleep(10); + + /* kick off a sample and wait for it to complete */ + hrt_abstime now = hrt_absolute_time(); + rCR2 |= ADC_CR2_SWSTART; + while (!(rSR & ADC_SR_EOC)) { + + /* don't wait for more than 500us, since that means something broke - should reset here if we see this */ + if ((hrt_absolute_time() - now) > 500) { + log("sample timeout"); + return -1; + return 0xffff; + } + } + debug("init done"); + + /* create the device node */ + return CDev::init(); +} + +int +ADC::ioctl(file *filp, int cmd, unsigned long arg) +{ + return -ENOTTY; +} + +ssize_t +ADC::read(file *filp, char *buffer, size_t len) +{ + if (len > sizeof(_result)) + len = sizeof(_result); + +// _result = _sample(11); + + memcpy(buffer, &_result, len); + return len; +} + +int +ADC::open_first(struct file *filp) +{ + hrt_call_every(&_call, _tickrate, _tickrate, _tick_trampoline, this); + + return 0; +} + +int +ADC::close_last(struct file *filp) +{ + hrt_cancel(&_call); + return 0; +} + +void +ADC::_tick_trampoline(void *arg) +{ + ((ADC *)arg)->_tick(); +} + +void +ADC::_tick() +{ + _result = _sample(11); +} + +uint16_t +ADC::_sample(unsigned channel) +{ + /* run a single conversion right now - should take about 60 cycles (a few microseconds) max */ + rSQR3 = channel; + rCR2 |= ADC_CR2_SWSTART; + + /* wait for the conversion to complete */ + hrt_abstime now = hrt_absolute_time(); + while (!(rSR & ADC_SR_EOC)) { + + /* don't wait for more than 50us, since that means something broke - should reset here if we see this */ + if ((hrt_absolute_time() - now) > 50) { + log("sample timeout"); + return 0xffff; + } + } + + /* read the result and clear EOC */ + uint16_t result = rDR; + + return result; +} + +/* + * Driver 'main' command. + */ +extern "C" __EXPORT int adc_main(int argc, char *argv[]); + +namespace +{ +ADC *g_adc; + +void +test(void) +{ + int fd; + + fd = open(ADC_DEVICE_PATH, O_RDONLY); + if (fd < 0) + err(1, "can't open ADC device"); + + uint16_t value; + + for (unsigned i = 0; i < 50; i++) { + if (read(fd, &value, sizeof(value)) != sizeof(value)) + errx(1, "short read"); + printf(" %d\n", value); + usleep(500000); + } + + exit(0); +} +} + +int +adc_main(int argc, char *argv[]) +{ + if (g_adc == nullptr) { + g_adc = new ADC; + + if (g_adc == nullptr) + errx(1, "couldn't allocate the ADC driver"); + + if (g_adc->init() != OK) { + delete g_adc; + errx(1, "ADC init failed"); + } + } + + if (argc > 1) { + if (!strcmp(argv[1], "test")) + test(); + } + + exit(0); +} From bc432b1feb906e5c2895d35bf1430fa6bf004061 Mon Sep 17 00:00:00 2001 From: px4dev Date: Mon, 31 Dec 2012 17:06:30 -0800 Subject: [PATCH 27/50] Cleanup and add support for multiple channels. --- apps/drivers/boards/px4fmu/px4fmu_init.c | 2 - apps/drivers/drv_adc.h | 2 - apps/drivers/stm32/adc/adc.cpp | 112 ++++++++++++++++++----- 3 files changed, 88 insertions(+), 28 deletions(-) diff --git a/apps/drivers/boards/px4fmu/px4fmu_init.c b/apps/drivers/boards/px4fmu/px4fmu_init.c index 4ebc5a439e..c2aed9b546 100644 --- a/apps/drivers/boards/px4fmu/px4fmu_init.c +++ b/apps/drivers/boards/px4fmu/px4fmu_init.c @@ -95,8 +95,6 @@ * Protected Functions ****************************************************************************/ -extern int adc_devinit(void); - /**************************************************************************** * Public Functions ****************************************************************************/ diff --git a/apps/drivers/drv_adc.h b/apps/drivers/drv_adc.h index f42350cbb5..8ec6d1233f 100644 --- a/apps/drivers/drv_adc.h +++ b/apps/drivers/drv_adc.h @@ -45,8 +45,6 @@ #include #include -#include - #define ADC_DEVICE_PATH "/dev/adc0" /* diff --git a/apps/drivers/stm32/adc/adc.cpp b/apps/drivers/stm32/adc/adc.cpp index f77499706e..ed40b478a1 100644 --- a/apps/drivers/stm32/adc/adc.cpp +++ b/apps/drivers/stm32/adc/adc.cpp @@ -62,13 +62,13 @@ #include #include - -#define ADC_BASE STM32_ADC1_BASE +#include /* * Register accessors. + * For now, no reason not to just use ADC1. */ -#define REG(_reg) (*(volatile uint32_t *)(_base + _reg)) +#define REG(_reg) (*(volatile uint32_t *)(STM32_ADC1_BASE + _reg)) #define rSR REG(STM32_ADC_SR_OFFSET) #define rCR1 REG(STM32_ADC_CR1_OFFSET) @@ -94,7 +94,7 @@ class ADC : public device::CDev { public: - ADC(); + ADC(uint32_t channels); ~ADC(); virtual int init(); @@ -107,27 +107,64 @@ protected: virtual int close_last(struct file *filp); private: - static const hrt_abstime _tickrate = 10000; /* 100Hz base rate */ - static const uint32_t _base = ADC_BASE; - + static const hrt_abstime _tickrate = 10000; /**< 100Hz base rate */ + hrt_call _call; + perf_counter_t _sample_perf; + unsigned _channel_count; + adc_msg_s *_samples; /**< sample buffer */ + + /** work trampoline */ static void _tick_trampoline(void *arg); + + /** worker function */ void _tick(); + /** + * Sample a single channel and return the measured value. + * + * @param channel The channel to sample. + * @return The sampled value, or 0xffff if + * sampling failed. + */ uint16_t _sample(unsigned channel); - uint16_t _result; }; -ADC::ADC() : - CDev("adc", ADC_DEVICE_PATH) +ADC::ADC(uint32_t channels) : + CDev("adc", ADC_DEVICE_PATH), + _sample_perf(perf_alloc(PC_ELAPSED, "ADC samples")), + _channel_count(0), + _samples(nullptr) { _debug_enabled = true; + + /* allocate the sample array */ + for (unsigned i = 0; i < 32; i++) { + if (channels & (1 << i)) { + _channel_count++; + } + } + _samples = new adc_msg_s[_channel_count]; + + /* prefill the channel numbers in the sample array */ + if (_samples != nullptr) { + unsigned index = 0; + for (unsigned i = 0; i < 32; i++) { + if (channels & (1 << i)) { + _samples[index].am_channel = i; + _samples[index].am_data = 0; + index++; + } + } + } } ADC::~ADC() { + if (_samples != nullptr) + delete _samples; } int @@ -158,7 +195,7 @@ ADC::init() /* configure for a single-channel sequence */ rSQR1 = 0; rSQR2 = 0; - rSQR3 = 11; /* will be updated with the channel each tick */ + rSQR3 = 0; /* will be updated with the channel each tick */ /* power-cycle the ADC and turn it on */ rCR2 &= ~ADC_CR2_ADON; @@ -180,6 +217,8 @@ ADC::init() return 0xffff; } } + + debug("init done"); /* create the device node */ @@ -195,18 +234,26 @@ ADC::ioctl(file *filp, int cmd, unsigned long arg) ssize_t ADC::read(file *filp, char *buffer, size_t len) { - if (len > sizeof(_result)) - len = sizeof(_result); + const size_t maxsize = sizeof(adc_msg_s) * _channel_count; -// _result = _sample(11); + if (len > maxsize) + len = maxsize; + + /* block interrupts while copying samples to avoid racing with an update */ + irqstate_t flags = irqsave(); + memcpy(buffer, _samples, len); + irqrestore(flags); - memcpy(buffer, &_result, len); return len; } int ADC::open_first(struct file *filp) { + /* get fresh data */ + _tick(); + + /* and schedule regular updates */ hrt_call_every(&_call, _tickrate, _tickrate, _tick_trampoline, this); return 0; @@ -228,12 +275,20 @@ ADC::_tick_trampoline(void *arg) void ADC::_tick() { - _result = _sample(11); + /* scan the channel set and sample each */ + for (unsigned i = 0; i < _channel_count; i++) + _samples[i].am_data = _sample(_samples[i].am_channel); } uint16_t ADC::_sample(unsigned channel) { + perf_begin(_sample_perf); + + /* clear any previous EOC */ + if (rSR & ADC_SR_EOC) + rSR &= ~ADC_SR_EOC; + /* run a single conversion right now - should take about 60 cycles (a few microseconds) max */ rSQR3 = channel; rCR2 |= ADC_CR2_SWSTART; @@ -252,6 +307,7 @@ ADC::_sample(unsigned channel) /* read the result and clear EOC */ uint16_t result = rDR; + perf_end(_sample_perf); return result; } @@ -267,18 +323,25 @@ ADC *g_adc; void test(void) { - int fd; - fd = open(ADC_DEVICE_PATH, O_RDONLY); + int fd = open(ADC_DEVICE_PATH, O_RDONLY); if (fd < 0) err(1, "can't open ADC device"); - uint16_t value; - for (unsigned i = 0; i < 50; i++) { - if (read(fd, &value, sizeof(value)) != sizeof(value)) - errx(1, "short read"); - printf(" %d\n", value); + adc_msg_s data[8]; + ssize_t count = read(fd, data, sizeof(data)); + + if (count < 0) + errx(1, "read error"); + + unsigned channels = count / sizeof(data[0]); + + for (unsigned j = 0; j < channels; j++) { + printf ("%d: %u ", data[j].am_channel, data[j].am_data); + } + + printf("\n"); usleep(500000); } @@ -290,7 +353,8 @@ int adc_main(int argc, char *argv[]) { if (g_adc == nullptr) { - g_adc = new ADC; + /* XXX this hardcodes the minimum channel set for PX4FMU - should be configurable */ + g_adc = new ADC((1 << 10) | (1 << 11)); if (g_adc == nullptr) errx(1, "couldn't allocate the ADC driver"); From b167912b1b3137c494a5033fe0dea6bb373624d2 Mon Sep 17 00:00:00 2001 From: px4dev Date: Mon, 31 Dec 2012 18:31:37 -0800 Subject: [PATCH 28/50] Enable the temperature sensor channel for F2/F4 devices. --- apps/drivers/stm32/adc/adc.cpp | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/apps/drivers/stm32/adc/adc.cpp b/apps/drivers/stm32/adc/adc.cpp index ed40b478a1..87dac1ef91 100644 --- a/apps/drivers/stm32/adc/adc.cpp +++ b/apps/drivers/stm32/adc/adc.cpp @@ -91,6 +91,10 @@ #define rJDR4 REG(STM32_ADC_JDR4_OFFSET) #define rDR REG(STM32_ADC_DR_OFFSET) +#ifdef STM32_ADC_CCR +# define rCCR REG(STM32_ADC_CCR_OFFSET) +#endif + class ADC : public device::CDev { public: @@ -140,6 +144,9 @@ ADC::ADC(uint32_t channels) : { _debug_enabled = true; + /* always enable the temperature sensor */ + channels |= 1 << 16; + /* allocate the sample array */ for (unsigned i = 0; i < 32; i++) { if (channels & (1 << i)) { @@ -188,10 +195,16 @@ ADC::init() /* enable the temperature sensor / Vrefint channel if supported*/ rCR2 = #ifdef ADC_CR2_TSVREFE + /* enable the temperature sensor in CR2 */ ADC_CR2_TSVREFE | #endif 0; +#ifdef ADC_CCR_TSVREFE + /* enable temperature sensor in CCR */ + rCCR = ADC_CCR_TSVREFE; +#endif + /* configure for a single-channel sequence */ rSQR1 = 0; rSQR2 = 0; From bd2f6b58e67245b4fbe6da272b8c59be3511c53b Mon Sep 17 00:00:00 2001 From: px4dev Date: Mon, 31 Dec 2012 19:40:41 -0800 Subject: [PATCH 29/50] Configure ADC GPIOs on IO --- apps/drivers/boards/px4io/px4io_init.c | 3 +++ apps/drivers/boards/px4io/px4io_internal.h | 27 ++++------------------ 2 files changed, 8 insertions(+), 22 deletions(-) diff --git a/apps/drivers/boards/px4io/px4io_init.c b/apps/drivers/boards/px4io/px4io_init.c index 33a6707cec..14e8dc13a9 100644 --- a/apps/drivers/boards/px4io/px4io_init.c +++ b/apps/drivers/boards/px4io/px4io_init.c @@ -92,4 +92,7 @@ __EXPORT void stm32_boardinitialize(void) stm32_configgpio(GPIO_ACC_OC_DETECT); stm32_configgpio(GPIO_SERVO_OC_DETECT); stm32_configgpio(GPIO_BTN_SAFETY); + + stm32_configgpio(GPIO_ADC_VBATT); + stm32_configgpio(GPIO_ADC_IN5); } diff --git a/apps/drivers/boards/px4io/px4io_internal.h b/apps/drivers/boards/px4io/px4io_internal.h index a0342ac8ad..f77d237a77 100644 --- a/apps/drivers/boards/px4io/px4io_internal.h +++ b/apps/drivers/boards/px4io/px4io_internal.h @@ -61,28 +61,6 @@ #define GPIO_LED3 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10) -/* R/C in/out channels **************************************************************/ - -/* XXX just GPIOs for now - eventually timer pins */ - -#define GPIO_CH1_IN (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN0) -#define GPIO_CH2_IN (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN1) -#define GPIO_CH3_IN (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN8) -#define GPIO_CH4_IN (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN9) -#define GPIO_CH5_IN (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN6) -#define GPIO_CH6_IN (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN7) -#define GPIO_CH7_IN (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN0) -#define GPIO_CH8_IN (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN1) - -#define GPIO_CH1_OUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN0) -#define GPIO_CH2_OUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN1) -#define GPIO_CH3_OUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN8) -#define GPIO_CH4_OUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN9) -#define GPIO_CH5_OUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN6) -#define GPIO_CH6_OUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN7) -#define GPIO_CH7_OUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN0) -#define GPIO_CH8_OUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN1) - /* Safety switch button *************************************************************/ #define GPIO_BTN_SAFETY (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN5) @@ -98,3 +76,8 @@ #define GPIO_RELAY1_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN13) #define GPIO_RELAY2_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN12) + +/* Analog inputs ********************************************************************/ + +#define GPIO_ADC_VBATT (GPIO_INPUT|GPIO_CNF_ANALOGIN|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN4) +#define GPIO_ADC_IN5 (GPIO_INPUT|GPIO_CNF_ANALOGIN|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN5) From 9be1f999357edd37658c301d9dd5aaa1d8db26a7 Mon Sep 17 00:00:00 2001 From: px4dev Date: Mon, 31 Dec 2012 19:41:18 -0800 Subject: [PATCH 30/50] Add trivial ADC support to PX4IO. --- apps/px4io/adc.c | 163 +++++++++++++++++++++++++++++++++++++++++++++ apps/px4io/px4io.h | 6 ++ 2 files changed, 169 insertions(+) create mode 100644 apps/px4io/adc.c diff --git a/apps/px4io/adc.c b/apps/px4io/adc.c new file mode 100644 index 0000000000..62ff0b1f19 --- /dev/null +++ b/apps/px4io/adc.c @@ -0,0 +1,163 @@ +/**************************************************************************** + * + * 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 adc.c + * + * Simple ADC support for PX4IO on STM32. + */ +#include +#include + +#include +#include +#include + +#include +#include + +#define DEBUG +#include "px4io.h" + +/* + * Register accessors. + * For now, no reason not to just use ADC1. + */ +#define REG(_reg) (*(volatile uint32_t *)(STM32_ADC1_BASE + _reg)) + +#define rSR REG(STM32_ADC_SR_OFFSET) +#define rCR1 REG(STM32_ADC_CR1_OFFSET) +#define rCR2 REG(STM32_ADC_CR2_OFFSET) +#define rSMPR1 REG(STM32_ADC_SMPR1_OFFSET) +#define rSMPR2 REG(STM32_ADC_SMPR2_OFFSET) +#define rJOFR1 REG(STM32_ADC_JOFR1_OFFSET) +#define rJOFR2 REG(STM32_ADC_JOFR2_OFFSET) +#define rJOFR3 REG(STM32_ADC_JOFR3_OFFSET) +#define rJOFR4 REG(STM32_ADC_JOFR4_OFFSET) +#define rHTR REG(STM32_ADC_HTR_OFFSET) +#define rLTR REG(STM32_ADC_LTR_OFFSET) +#define rSQR1 REG(STM32_ADC_SQR1_OFFSET) +#define rSQR2 REG(STM32_ADC_SQR2_OFFSET) +#define rSQR3 REG(STM32_ADC_SQR3_OFFSET) +#define rJSQR REG(STM32_ADC_JSQR_OFFSET) +#define rJDR1 REG(STM32_ADC_JDR1_OFFSET) +#define rJDR2 REG(STM32_ADC_JDR2_OFFSET) +#define rJDR3 REG(STM32_ADC_JDR3_OFFSET) +#define rJDR4 REG(STM32_ADC_JDR4_OFFSET) +#define rDR REG(STM32_ADC_DR_OFFSET) + +perf_counter_t adc_perf; + +int +adc_init(void) +{ + adc_perf = perf_alloc(PC_ELAPSED, "adc"); + + /* do calibration if supported */ +#ifdef ADC_CR2_CAL + rCR2 |= ADC_CR2_RSTCAL; + up_udelay(1); + if (rCR2 & ADC_CR2_RSTCAL) + return -1; + rCR2 |= ADC_CR2_CAL; + up_udelay(100); + if (rCR2 & ADC_CR2_CAL) + return -1; +#endif + + /* arbitrarily configure all channels for 55 cycle sample time */ + rSMPR1 = 0b00000011011011011011011011011011; + rSMPR2 = 0b00011011011011011011011011011011; + + /* XXX for F2/4, might want to select 12-bit mode? */ + rCR1 = 0; + + /* enable the temperature sensor / Vrefint channel if supported*/ + rCR2 = +#ifdef ADC_CR2_TSVREFE + /* enable the temperature sensor in CR2 */ + ADC_CR2_TSVREFE | +#endif + 0; + +#ifdef ADC_CCR_TSVREFE + /* enable temperature sensor in CCR */ + rCCR = ADC_CCR_TSVREFE; +#endif + + /* configure for a single-channel sequence */ + rSQR1 = 0; + rSQR2 = 0; + rSQR3 = 0; /* will be updated with the channel each tick */ + + /* power-cycle the ADC and turn it on */ + rCR2 &= ~ADC_CR2_ADON; + up_udelay(10); + rCR2 |= ADC_CR2_ADON; + up_udelay(10); + rCR2 |= ADC_CR2_ADON; + up_udelay(10); + + return 0; +} + +uint16_t +adc_measure(unsigned channel) +{ + perf_begin(adc_perf); + + /* clear any previous EOC */ + if (rSR & ADC_SR_EOC) + rSR &= ~ADC_SR_EOC; + + /* run a single conversion right now - should take about 60 cycles (a few microseconds) max */ + rSQR3 = channel; + rCR2 |= ADC_CR2_ADON; + + /* wait for the conversion to complete */ + hrt_abstime now = hrt_absolute_time(); + while (!(rSR & ADC_SR_EOC)) { + + /* never spin forever - this will give a bogus result though */ + if ((hrt_absolute_time() - now) > 1000) { + debug("adc timeout"); + break; + } + } + + /* read the result and clear EOC */ + uint16_t result = rDR; + + perf_end(adc_perf); + return result; +} \ No newline at end of file diff --git a/apps/px4io/px4io.h b/apps/px4io/px4io.h index 45b7cf847a..6221b08f24 100644 --- a/apps/px4io/px4io.h +++ b/apps/px4io/px4io.h @@ -160,6 +160,12 @@ extern void safety_init(void); */ extern void comms_main(void) __attribute__((noreturn)); +/* + * Sensors/misc inputs + */ +extern int adc_init(void); +extern uint16_t adc_measure(unsigned channel); + /* * R/C receiver handling. */ From d93fda20fd55746923d607e717f254bc92741eab Mon Sep 17 00:00:00 2001 From: px4dev Date: Mon, 31 Dec 2012 21:06:26 -0800 Subject: [PATCH 31/50] Add ADC measurements and reporting to PX4IO, including calibration for the battery input. --- apps/px4io/comms.c | 66 +++++++++++++++++++++++++++++++++++-------- apps/px4io/protocol.h | 4 +++ apps/px4io/px4io.h | 28 ++++++++++++------ 3 files changed, 78 insertions(+), 20 deletions(-) diff --git a/apps/px4io/comms.c b/apps/px4io/comms.c index 83a006d43b..eaaea7817d 100644 --- a/apps/px4io/comms.c +++ b/apps/px4io/comms.c @@ -61,8 +61,7 @@ #define FMU_MIN_REPORT_INTERVAL 5000 /* 5ms */ #define FMU_MAX_REPORT_INTERVAL 100000 /* 100ms */ -int frame_rx; -int frame_bad; +#define FMU_STATUS_INTERVAL 1000000 /* 100ms */ static int fmu_fd; static hx_stream_t stream; @@ -87,6 +86,9 @@ comms_init(void) cfsetspeed(&t, 115200); t.c_cflag &= ~(CSTOPB | PARENB); tcsetattr(fmu_fd, TCSANOW, &t); + + /* init the ADC */ + adc_init(); } void @@ -135,9 +137,55 @@ comms_main(void) report.channel_count = system_state.rc_channels; report.armed = system_state.armed; + report.battery_mv = system_state.battery_mv; + report.adc_in = system_state.adc_in5; + report.overcurrent = system_state.overcurrent; + /* and send it */ hx_stream_send(stream, &report, sizeof(report)); } + + /* + * Fetch ADC values, check overcurrent flags, etc. + */ + static hrt_abstime last_status_time; + + if ((now - last_status_time) > FMU_STATUS_INTERVAL) { + + /* + * Coefficients here derived by measurement of the 5-16V + * range on one unit: + * + * V counts + * 5 1001 + * 6 1219 + * 7 1436 + * 8 1653 + * 9 1870 + * 10 2086 + * 11 2303 + * 12 2522 + * 13 2738 + * 14 2956 + * 15 3172 + * 16 3389 + * + * slope = 0.0046067 + * intercept = 0.3863 + * + * Intercept corrected for best results @ 12V. + */ + unsigned counts = adc_measure(ADC_VBATT); + system_state.battery_mv = (4150 + (counts * 46)) / 10; + + system_state.adc_in5 = adc_measure(ADC_IN5); + + system_state.overcurrent = + (OVERCURRENT_SERVO ? (1 << 0) : 0) | + (OVERCURRENT_ACC ? (1 << 1) : 0); + + last_status_time = now; + } } } @@ -146,12 +194,10 @@ comms_handle_config(const void *buffer, size_t length) { const struct px4io_config *cfg = (struct px4io_config *)buffer; - if (length != sizeof(*cfg)) { - frame_bad++; + if (length != sizeof(*cfg)) return; - } - frame_rx++; + /* XXX */ } static void @@ -159,12 +205,9 @@ comms_handle_command(const void *buffer, size_t length) { const struct px4io_command *cmd = (struct px4io_command *)buffer; - if (length != sizeof(*cmd)) { - frame_bad++; + if (length != sizeof(*cmd)) return; - } - - frame_rx++; + irqstate_t flags = irqsave(); /* fetch new PWM output values */ @@ -209,7 +252,6 @@ comms_handle_frame(void *arg, const void *buffer, size_t length) comms_handle_config(buffer, length); break; default: - frame_bad++; break; } } diff --git a/apps/px4io/protocol.h b/apps/px4io/protocol.h index c704b12016..a64e2f27de 100644 --- a/apps/px4io/protocol.h +++ b/apps/px4io/protocol.h @@ -73,6 +73,10 @@ struct px4io_report { uint16_t rc_channel[PX4IO_INPUT_CHANNELS]; bool armed; uint8_t channel_count; + + uint16_t battery_mv; + uint16_t adc_in; + uint8_t overcurrent; }; #pragma pack(pop) \ No newline at end of file diff --git a/apps/px4io/px4io.h b/apps/px4io/px4io.h index 6221b08f24..af06c6ec1e 100644 --- a/apps/px4io/px4io.h +++ b/apps/px4io/px4io.h @@ -105,17 +105,25 @@ struct sys_state_s bool fmu_data_received; /* - * Current serial interface mode, per the serial_rx_mode parameter - * in the config packet. + * Measured battery voltage in mV */ - uint8_t serial_rx_mode; + uint16_t battery_mv; + + /* + * ADC IN5 measurement + */ + uint16_t adc_in5; + + /* + * Power supply overcurrent status bits. + */ + uint8_t overcurrent; + }; extern struct sys_state_s system_state; -extern int frame_rx; -extern int frame_bad; - +#if 0 /* * Software countdown timers. * @@ -127,6 +135,7 @@ extern int frame_bad; #define TIMER_SANITY 7 #define TIMER_NUM_TIMERS 8 extern volatile int timers[TIMER_NUM_TIMERS]; +#endif /* * GPIO handling. @@ -141,10 +150,13 @@ extern volatile int timers[TIMER_NUM_TIMERS]; #define POWER_RELAY1(_s) stm32_gpiowrite(GPIO_RELAY1_EN, (_s)) #define POWER_RELAY2(_s) stm32_gpiowrite(GPIO_RELAY2_EN, (_s)) -#define OVERCURRENT_ACC stm32_gpioread(GPIO_ACC_OC_DETECT) -#define OVERCURRENT_SERVO stm32_gpioread(GPIO_SERVO_OC_DETECT +#define OVERCURRENT_ACC (!stm32_gpioread(GPIO_ACC_OC_DETECT)) +#define OVERCURRENT_SERVO (!stm32_gpioread(GPIO_SERVO_OC_DETECT)) #define BUTTON_SAFETY stm32_gpioread(GPIO_BTN_SAFETY) +#define ADC_VBATT 4 +#define ADC_IN5 5 + /* * Mixer */ From 4eb7df6ff5b0015a825ca07c1206dd545b4567b5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 1 Jan 2013 13:30:24 +0100 Subject: [PATCH 32/50] Introduced battery_status uORB topic, changed sensors app to publish to it, extended px4io driver to publish to it. Both do only so if the battery voltage is reasonably high, at 3.3V --- apps/commander/commander.c | 26 ++++++++---- apps/drivers/px4io/px4io.cpp | 25 +++++++++++ apps/sdlog/sdlog.c | 2 +- apps/sensors/sensors.cpp | 33 ++++++++++----- apps/uORB/objects_common.cpp | 3 ++ apps/uORB/topics/battery_status.h | 68 ++++++++++++++++++++++++++++++ apps/uORB/topics/sensor_combined.h | 4 +- 7 files changed, 140 insertions(+), 21 deletions(-) create mode 100644 apps/uORB/topics/battery_status.h diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 7277e9fa4c..17087ab8af 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -70,6 +70,7 @@ #include #include #include +#include #include #include #include @@ -1262,6 +1263,11 @@ int commander_thread_main(int argc, char *argv[]) struct vehicle_command_s cmd; memset(&cmd, 0, sizeof(cmd)); + /* subscribe to battery topic */ + int battery_sub = orb_subscribe(ORB_ID(battery_status)); + struct battery_status_s battery; + memset(&battery, 0, sizeof(battery)); + // uint8_t vehicle_state_previous = current_status.state_machine; float voltage_previous = 0.0f; @@ -1300,15 +1306,19 @@ int commander_thread_main(int argc, char *argv[]) handle_command(stat_pub, ¤t_status, &cmd); } - battery_voltage = sensors.battery_voltage_v; - battery_voltage_valid = sensors.battery_voltage_valid; + orb_check(battery_sub, &new_data); + if (new_data) { + orb_copy(ORB_ID(battery_status), battery_sub, &battery); + battery_voltage = battery.voltage_v; + battery_voltage_valid = true; - /* - * Only update battery voltage estimate if voltage is - * valid and system has been running for two and a half seconds - */ - if (battery_voltage_valid && (hrt_absolute_time() - start_time > 2500000)) { - bat_remain = battery_remaining_estimate_voltage(battery_voltage); + /* + * Only update battery voltage estimate if system has + * been running for two and a half seconds. + */ + if (hrt_absolute_time() - start_time > 2500000) { + bat_remain = battery_remaining_estimate_voltage(battery_voltage); + } } /* Slow but important 8 Hz checks */ diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index 9f3dba047f..90c0773630 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -73,6 +73,7 @@ #include #include #include +#include #include #include "uploader.h" @@ -109,6 +110,9 @@ private: orb_advert_t _to_input_rc; ///< rc inputs from io rc_input_values _input_rc; ///< rc input values + orb_advert_t _to_battery; ///< battery status / voltage + battery_status_s _battery_status;///< battery status data + orb_advert_t _t_outputs; ///< mixed outputs topic actuator_outputs_s _outputs; ///< mixed outputs @@ -321,6 +325,10 @@ PX4IO::task_main() memset(&_input_rc, 0, sizeof(_input_rc)); _to_input_rc = orb_advertise(ORB_ID(input_rc), &_input_rc); + /* do not advertise the battery status until its clear that a battery is connected */ + memset(&_input_rc, 0, sizeof(_input_rc)); + _to_battery = -1; + /* poll descriptor */ pollfd fds[3]; fds[0].fd = _serial_fd; @@ -479,6 +487,23 @@ PX4IO::rx_callback(const uint8_t *buffer, size_t bytes_received) /* remember the latched arming switch state */ _switch_armed = rep->armed; + /* publish battery information */ + + /* only publish if battery has a valid minimum voltage */ + if (rep->battery_mv > 3300) { + _battery_status.timestamp = hrt_absolute_time(); + _battery_status.voltage_v = rep->battery_mv / 1000.0f; + /* current and discharge are unknown */ + _battery_status.current_a = -1.0f; + _battery_status.discharged_mah = -1.0f; + /* announce the battery voltage if needed, just publish else */ + if (_to_battery > 0) { + orb_publish(ORB_ID(battery_status), _to_battery, &_battery_status); + } else { + _to_battery = orb_advertise(ORB_ID(battery_status), &_battery_status); + } + } + _send_needed = true; /* if monitoring, dump the received info */ diff --git a/apps/sdlog/sdlog.c b/apps/sdlog/sdlog.c index d38bf9122d..9b4cd16222 100644 --- a/apps/sdlog/sdlog.c +++ b/apps/sdlog/sdlog.c @@ -554,7 +554,7 @@ int sdlog_thread_main(int argc, char *argv[]) { .control = {buf.act_controls.control[0], buf.act_controls.control[1], buf.act_controls.control[2], buf.act_controls.control[3]}, .actuators = {buf.act_outputs.output[0], buf.act_outputs.output[1], buf.act_outputs.output[2], buf.act_outputs.output[3], buf.act_outputs.output[4], buf.act_outputs.output[5], buf.act_outputs.output[6], buf.act_outputs.output[7]}, - .vbat = buf.raw.battery_voltage_v, + .vbat = 0.0f, /* XXX use battery_status uORB topic */ .adc = {buf.raw.adc_voltage_v[0], buf.raw.adc_voltage_v[1], buf.raw.adc_voltage_v[2]}, .local_position = {buf.local_pos.x, buf.local_pos.y, buf.local_pos.z}, .gps_raw_position = {buf.gps_pos.lat, buf.gps_pos.lon, buf.gps_pos.alt}, diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp index 07a9015fed..55786333ff 100644 --- a/apps/sensors/sensors.cpp +++ b/apps/sensors/sensors.cpp @@ -73,6 +73,7 @@ #include #include #include +#include #define GYRO_HEALTH_COUNTER_LIMIT_ERROR 20 /* 40 ms downtime at 500 Hz update rate */ #define ACC_HEALTH_COUNTER_LIMIT_ERROR 20 /* 40 ms downtime at 500 Hz update rate */ @@ -156,10 +157,12 @@ private: orb_advert_t _sensor_pub; /**< combined sensor data topic */ orb_advert_t _manual_control_pub; /**< manual control signal topic */ orb_advert_t _rc_pub; /**< raw r/c control topic */ + orb_advert_t _battery_pub; /**< battery status */ perf_counter_t _loop_perf; /**< loop performance counter */ struct rc_channels_s _rc; /**< r/c channel data */ + struct battery_status_s _battery_status; /**< battery status */ struct { float min[_rc_max_chan_count]; @@ -348,6 +351,7 @@ Sensors::Sensors() : _sensor_pub(-1), _manual_control_pub(-1), _rc_pub(-1), + _battery_pub(-1), /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "sensor task update")) @@ -844,14 +848,22 @@ Sensors::adc_poll(struct sensor_combined_s &raw) if (ADC_BATTERY_VOLATGE_CHANNEL == buf_adc.am_channel1) { /* Voltage in volts */ - raw.battery_voltage_v = (BAT_VOL_LOWPASS_1 * (raw.battery_voltage_v + BAT_VOL_LOWPASS_2 * (buf_adc.am_data1 * _parameters.battery_voltage_scaling))); + float voltage = (buf_adc.am_data1 * _parameters.battery_voltage_scaling); - if ((raw.battery_voltage_v) < VOLTAGE_BATTERY_IGNORE_THRESHOLD_VOLTS) { - raw.battery_voltage_valid = false; - raw.battery_voltage_v = 0.f; + if (voltage > VOLTAGE_BATTERY_IGNORE_THRESHOLD_VOLTS) { - } else { - raw.battery_voltage_valid = true; + _battery_status.timestamp = hrt_absolute_time(); + _battery_status.voltage_v = (BAT_VOL_LOWPASS_1 * (_battery_status.voltage_v + BAT_VOL_LOWPASS_2 * voltage));; + /* current and discharge are unknown */ + _battery_status.current_a = -1.0f; + _battery_status.discharged_mah = -1.0f; + + /* announce the battery voltage if needed, just publish else */ + if (_battery_pub > 0) { + orb_publish(ORB_ID(battery_status), _battery_pub, &_battery_status); + } else { + _battery_pub = orb_advertise(ORB_ID(battery_status), &_battery_status); + } } raw.battery_voltage_counter++; @@ -879,7 +891,7 @@ Sensors::ppm_poll() */ if (ppm_decoded_channels > 4 && hrt_absolute_time() - _ppm_last_valid < PPM_INPUT_TIMEOUT_INTERVAL) { - for (int i = 0; i < ppm_decoded_channels; i++) { + for (unsigned i = 0; i < ppm_decoded_channels; i++) { raw.values[i] = ppm_buffer[i]; } @@ -1039,12 +1051,13 @@ Sensors::task_main() struct sensor_combined_s raw; memset(&raw, 0, sizeof(raw)); raw.timestamp = hrt_absolute_time(); - raw.battery_voltage_v = BAT_VOL_INITIAL; raw.adc_voltage_v[0] = 0.9f; raw.adc_voltage_v[1] = 0.0f; raw.adc_voltage_v[2] = 0.0f; - raw.battery_voltage_counter = 0; - raw.battery_voltage_valid = false; + raw.adc_voltage_v[3] = 0.0f; + + memset(&_battery_status, 0, sizeof(_battery_status)); + _battery_status.voltage_v = BAT_VOL_INITIAL; /* get a set of initial values */ accel_poll(raw); diff --git a/apps/uORB/objects_common.cpp b/apps/uORB/objects_common.cpp index dbee15050c..2d249a47fc 100644 --- a/apps/uORB/objects_common.cpp +++ b/apps/uORB/objects_common.cpp @@ -71,6 +71,9 @@ ORB_DEFINE(vehicle_gps_position, struct vehicle_gps_position_s); #include "topics/vehicle_status.h" ORB_DEFINE(vehicle_status, struct vehicle_status_s); +#include "topics/battery_status.h" +ORB_DEFINE(battery_status, struct battery_status_s); + #include "topics/vehicle_global_position.h" ORB_DEFINE(vehicle_global_position, struct vehicle_global_position_s); diff --git a/apps/uORB/topics/battery_status.h b/apps/uORB/topics/battery_status.h new file mode 100644 index 0000000000..c40d0d4e56 --- /dev/null +++ b/apps/uORB/topics/battery_status.h @@ -0,0 +1,68 @@ +/**************************************************************************** + * + * Copyright (C) 2012-2013 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 battery_status.h + * + * Definition of the battery status uORB topic. + */ + +#ifndef BATTERY_STATUS_H_ +#define BATTERY_STATUS_H_ + +#include "../uORB.h" +#include + +/** + * @addtogroup topics + * @{ + */ + +/** + * Battery voltages and status + */ +struct battery_status_s { + uint64_t timestamp; /**< microseconds since system boot, needed to integrate */ + float voltage_v; /**< Battery voltage in volts, filtered */ + float current_a; /**< Battery current in amperes, filtered, -1 if unknown */ + float discharged_mah; /**< Discharged amount in mAh, filtered, -1 if unknown */ +}; + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(battery_status); + +#endif \ No newline at end of file diff --git a/apps/uORB/topics/sensor_combined.h b/apps/uORB/topics/sensor_combined.h index 0324500acf..1d25af35a5 100644 --- a/apps/uORB/topics/sensor_combined.h +++ b/apps/uORB/topics/sensor_combined.h @@ -99,8 +99,8 @@ struct sensor_combined_s { float baro_pres_mbar; /**< Barometric pressure, already temp. comp. */ float baro_alt_meter; /**< Altitude, already temp. comp. */ float baro_temp_celcius; /**< Temperature in degrees celsius */ - float battery_voltage_v; /**< Battery voltage in volts, filtered */ - float adc_voltage_v[3]; /**< ADC voltages of ADC Chan 11/12/13 or -1 */ + float adc_voltage_v[4]; /**< ADC voltages of ADC Chan 10/11/12/13 or -1 */ + float mcu_temp_celcius; /**< Internal temperature measurement of MCU */ uint32_t baro_counter; /**< Number of raw baro measurements taken */ uint32_t battery_voltage_counter; /**< Number of voltage measurements taken */ bool battery_voltage_valid; /**< True if battery voltage can be measured */ From 803352e7225ace232ed4a419118956656f13b947 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 1 Jan 2013 13:39:00 +0100 Subject: [PATCH 33/50] Fixed stupid copy/paste typo --- apps/drivers/px4io/px4io.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index 90c0773630..f6b350cc9e 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -326,7 +326,7 @@ PX4IO::task_main() _to_input_rc = orb_advertise(ORB_ID(input_rc), &_input_rc); /* do not advertise the battery status until its clear that a battery is connected */ - memset(&_input_rc, 0, sizeof(_input_rc)); + memset(&_battery_status, 0, sizeof(_battery_status)); _to_battery = -1; /* poll descriptor */ From 63464a8959d90bd92c4998a80992c416b85c2057 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 2 Jan 2013 17:37:55 +1100 Subject: [PATCH 34/50] 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 35/50] 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 36/50] 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 37/50] 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 e787fa5bce52e10179cae33df56caa765bfa75e2 Mon Sep 17 00:00:00 2001 From: px4dev Date: Thu, 3 Jan 2013 00:33:22 -0800 Subject: [PATCH 38/50] 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 3916230d8fc7185a7eefcc1640a68bacf3eac72b Mon Sep 17 00:00:00 2001 From: px4dev Date: Thu, 3 Jan 2013 00:33:22 -0800 Subject: [PATCH 39/50] 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 40/50] 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 41/50] 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 91ca80e6347feebb1e39b9d16df3f88fb8a68152 Mon Sep 17 00:00:00 2001 From: px4dev Date: Fri, 4 Jan 2013 21:28:26 -0800 Subject: [PATCH 42/50] Fix the handling of FIONREAD/FIONWRITE; thanks Tridge. --- nuttx/drivers/serial/serial.c | 22 ++++++++++++++++++---- 1 file changed, 18 insertions(+), 4 deletions(-) diff --git a/nuttx/drivers/serial/serial.c b/nuttx/drivers/serial/serial.c index 24744524fa..28c657af05 100644 --- a/nuttx/drivers/serial/serial.c +++ b/nuttx/drivers/serial/serial.c @@ -660,9 +660,9 @@ static int uart_ioctl(FAR struct file *filep, int cmd, unsigned long arg) int ret = dev->ops->ioctl(filep, cmd, arg); - /* Append any higher level TTY flags */ + /* If the low-level handler didn't handle the call, see if we can handle it here */ - if (ret == OK) + if (ret == -ENOTTY) { switch (cmd) { @@ -686,6 +686,9 @@ static int uart_ioctl(FAR struct file *filep, int cmd, unsigned long arg) irqrestore(state); *(int *)arg = count; + ret = 0; + + break; } case FIONWRITE: @@ -695,7 +698,7 @@ static int uart_ioctl(FAR struct file *filep, int cmd, unsigned long arg) /* determine the number of bytes free in the buffer */ - if (dev->xmit.head <= dev->xmit.tail) + if (dev->xmit.head < dev->xmit.tail) { count = dev->xmit.tail - dev->xmit.head - 1; } @@ -707,8 +710,19 @@ static int uart_ioctl(FAR struct file *filep, int cmd, unsigned long arg) irqrestore(state); *(int *)arg = count; - } + ret = 0; + break; + } + } + } + + /* Append any higher level TTY flags */ + + else if (ret == OK) + { + switch (cmd) + { #ifdef CONFIG_SERIAL_TERMIOS case TCGETS: { From 69cdab9afc0215e26dc5084e21fd61725acb6c84 Mon Sep 17 00:00:00 2001 From: px4dev Date: Fri, 4 Jan 2013 23:41:21 -0800 Subject: [PATCH 43/50] Fix a typo that caused PWM_SERVO_GET ioctls to fail on the FMU PWM outputs. --- apps/drivers/px4fmu/fmu.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/apps/drivers/px4fmu/fmu.cpp b/apps/drivers/px4fmu/fmu.cpp index a995f6214d..2e3b130a9c 100644 --- a/apps/drivers/px4fmu/fmu.cpp +++ b/apps/drivers/px4fmu/fmu.cpp @@ -500,7 +500,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) /* FALLTHROUGH */ case PWM_SERVO_GET(0): case PWM_SERVO_GET(1): { - channel = cmd - PWM_SERVO_SET(0); + channel = cmd - PWM_SERVO_GET(0); *(servo_position_t *)arg = up_pwm_servo_get(channel); break; } From 219ab05a707d8f2795dc91f5884a42f20dd6a640 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 6 Jan 2013 06:29:28 +1100 Subject: [PATCH 44/50] 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 45/50] 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 46/50] 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 47/50] 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 48/50] 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 From c94076f6735290dc7aa8742428f46ef51521f9a6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 6 Jan 2013 12:36:46 +0100 Subject: [PATCH 49/50] Added missing ADC start command, fixed return value / error behavior of ADC init --- ROMFS/scripts/rc.sensors | 1 + apps/sensors/sensors.cpp | 4 ++-- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/ROMFS/scripts/rc.sensors b/ROMFS/scripts/rc.sensors index 536fcfd2c0..42c2f52e94 100644 --- a/ROMFS/scripts/rc.sensors +++ b/ROMFS/scripts/rc.sensors @@ -8,6 +8,7 @@ # ms5611 start +adc start if mpu6000 start then diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp index 55786333ff..c099509a78 100644 --- a/apps/sensors/sensors.cpp +++ b/apps/sensors/sensors.cpp @@ -640,8 +640,8 @@ Sensors::adc_init() _fd_adc = open("/dev/adc0", O_RDONLY | O_NONBLOCK); if (_fd_adc < 0) { - warn("/dev/adc0"); - errx(1, "FATAL: no ADC found"); + warnx("/dev/adc0"); + warnx(1, "FATAL: no ADC found"); } } From ec3d298720af06cd40184b3809555487e5df24d2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 6 Jan 2013 12:40:29 +0100 Subject: [PATCH 50/50] Fixed minor compile error --- apps/sensors/sensors.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp index c099509a78..6b62eb9f2e 100644 --- a/apps/sensors/sensors.cpp +++ b/apps/sensors/sensors.cpp @@ -641,7 +641,7 @@ Sensors::adc_init() _fd_adc = open("/dev/adc0", O_RDONLY | O_NONBLOCK); if (_fd_adc < 0) { warnx("/dev/adc0"); - warnx(1, "FATAL: no ADC found"); + warnx("FATAL: no ADC found"); } }